Skip to content

Commit

Permalink
Merge pull request ros-industrial-attic#13 from mpowelson/feature/ima…
Browse files Browse the repository at this point in the history
…ge_queue

ROS 1: Add image queue to deal with camera/TF timing mismatch
  • Loading branch information
schornakj authored Dec 7, 2019
2 parents 3e7cf8a + 4644238 commit 28a7f0e
Show file tree
Hide file tree
Showing 2 changed files with 45 additions and 30 deletions.
5 changes: 5 additions & 0 deletions yak_ros/include/yak_ros/online_fusion_server_ros1.h
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@
#include <yak_ros_msgs/GenerateMesh.h>
#include <yak_ros_msgs/UpdateKinFuParams.h>

#include <queue>

namespace yak_ros
{
/**
Expand Down Expand Up @@ -116,6 +118,9 @@ class OnlineFusionServer
Eigen::Affine3d tsdf_frame_to_camera_prev_;
/** @brief TF frame associated with the TSDF volume. */
std::string tsdf_frame_;
/** @brief Queue that stores images until their transforms are available. This allows for the mismatch in timing
* between images being published and TFs */
std::queue<sensor_msgs::ImageConstPtr> image_queue_;
};
} // namespace yak_ros

Expand Down
70 changes: 40 additions & 30 deletions yak_ros/src/yak_ros1_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,39 +50,49 @@ void OnlineFusionServer::onReceivedDepthImg(const sensor_msgs::ImageConstPtr& im
{
// Get the camera pose in the world frame at the time when the depth image was generated.
ROS_DEBUG_STREAM("Got depth image");
image_queue_.push(image_in);
geometry_msgs::TransformStamped transform_tsdf_frame_to_camera;
try
if (image_queue_.size() > 0)
{
transform_tsdf_frame_to_camera =
tf_buffer_.lookupTransform(tsdf_frame_, image_in->header.frame_id, image_in->header.stamp);
try
{
sensor_msgs::ImageConstPtr next_image = image_queue_.front();
transform_tsdf_frame_to_camera =
tf_buffer_.lookupTransform(tsdf_frame_, next_image->header.frame_id, next_image->header.stamp);
image_queue_.pop();

Eigen::Affine3d tsdf_frame_to_camera = tf2::transformToEigen(transform_tsdf_frame_to_camera);

// Find how much the camera moved since the last depth image. If the magnitude of motion was below some threshold,
// abort integration. This is to prevent noise from accumulating in the isosurface due to numerous observations
// from the same pose.
std::double_t motion_mag = (tsdf_frame_to_camera.inverse() * tsdf_frame_to_camera_prev_).translation().norm();
ROS_DEBUG_STREAM(motion_mag);
if (motion_mag < DEFAULT_MINIMUM_TRANSLATION)
{
ROS_DEBUG_STREAM("Camera motion below threshold");
return;
}

cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(next_image, sensor_msgs::image_encodings::TYPE_16UC1);

// Integrate the depth image into the TSDF volume
if (!fusion_.fuse(cv_ptr->image, tsdf_frame_to_camera.cast<float>()))
ROS_WARN_STREAM("Failed to fuse image");

// If integration was successful, update the previous camera pose with the new camera pose
tsdf_frame_to_camera_prev_ = tsdf_frame_to_camera;
}
catch (tf2::TransformException& ex)
{
// Abort integration if tf lookup failed
ROS_WARN("%s", ex.what());
// Remove image if TF lookup failed because it is in the unknown past
if (std::string(ex.what()).find(std::string("past")) != std::string::npos)
image_queue_.pop();
return;
}
}
catch (tf2::TransformException& ex)
{
// Abort integration if tf lookup failed
ROS_WARN("%s", ex.what());
return;
}
Eigen::Affine3d tsdf_frame_to_camera = tf2::transformToEigen(transform_tsdf_frame_to_camera);

// Find how much the camera moved since the last depth image. If the magnitude of motion was below some threshold,
// abort integration. This is to prevent noise from accumulating in the isosurface due to numerous observations from
// the same pose.
std::double_t motion_mag = (tsdf_frame_to_camera.inverse() * tsdf_frame_to_camera_prev_).translation().norm();
ROS_DEBUG_STREAM(motion_mag);
if (motion_mag < DEFAULT_MINIMUM_TRANSLATION)
{
ROS_DEBUG_STREAM("Camera motion below threshold");
return;
}

cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_in, sensor_msgs::image_encodings::TYPE_16UC1);

// Integrate the depth image into the TSDF volume
if (!fusion_.fuse(cv_ptr->image, tsdf_frame_to_camera.cast<float>()))
ROS_WARN_STREAM("Failed to fuse image");

// If integration was successful, update the previous camera pose with the new camera pose
tsdf_frame_to_camera_prev_ = tsdf_frame_to_camera;
return;
}

Expand Down

0 comments on commit 28a7f0e

Please sign in to comment.