cloud_accumulator.cc
Go to the documentation of this file.
1 #define PCL_NO_PRECOMPILE
2 #include "cloud_accumulator.h"
3 #include <limits>
4 #include <pcl_ros/point_cloud.h>
5 #include <pcl/point_types.h>
7 #include <pcl/point_cloud.h>
8 #include <pcl/visualization/pcl_visualizer.h>
9 #include <pcl/common/transforms.h>
10 #include <pcl/filters/passthrough.h>
11 #include <pcl/filters/voxel_grid.h>
12 #include <pcl/io/pcd_io.h>
13 #include <boost/lexical_cast.hpp>
14 
15 namespace rc
16 {
17 
18 /****************************** Utility functions ********************************************/
19 
20 inline geometry_msgs::TransformStamped myPoseStampedMsgToTF(const geometry_msgs::PoseStamped & msg)
21 {
22  geometry_msgs::TransformStamped bt;
23  bt.transform.rotation = msg.pose.orientation;
24  bt.transform.translation.x = msg.pose.position.x;
25  bt.transform.translation.y = msg.pose.position.y;
26  bt.transform.translation.z = msg.pose.position.z;
27  bt.header.stamp = msg.header.stamp;
28  bt.header.frame_id = msg.header.frame_id;
29  bt.child_frame_id = "camera";
30  return bt;
31 }
32 
33 
34 inline Eigen::Affine3f toAffine(const geometry_msgs::TransformStamped& tf_stamped)
35 {
36  const geometry_msgs::Vector3& t = tf_stamped.transform.translation;//just abbreviate
37  const geometry_msgs::Quaternion& q = tf_stamped.transform.rotation;//just abbreviate
38 
39  Eigen::Affine3f result = Eigen::Affine3f::Identity();
40  result.translation() << t.x, t.y, t.z;
41  Eigen::Quaternionf rot(q.w, q.x, q.y, q.z);
42  result.rotate(rot);
43  return result;
44 }
45 
46 
48 void voxelGridFilter(double size, pointcloud_t::Ptr input, pointcloud_t::Ptr& output)
49 {
50  if(size <= 0.0){ output = input; }
51  else
52  {
53  pcl::VoxelGrid<point_t> vgf;
54  vgf.setInputCloud(input);
55  vgf.setLeafSize(size, size, size);
56  vgf.filter(*output);
57  }
58 }
59 
62 pointcloud_t::Ptr distanceFiltered(double min, double max, const pointcloud_t::ConstPtr input)
63 {
64  pointcloud_t::Ptr output(new pointcloud_t());
65  if(min >= max){ *output = *input; }
66  else
67  {
68  pcl::PassThrough<point_t> pass;
69  pass.setInputCloud(input);
70  pass.setFilterFieldName("z");
71  pass.setFilterLimits(min, max);
72  pass.filter(*output);
73  }
74  return output;
75 }
76 /****************************** CloudAccumulator ********************************************/
77 
78 CloudAccumulator::CloudAccumulator(double voxelgrid_size_display,
79  double voxelgrid_size_save,
80  double min_distance,
81  double max_distance,
82  std::string output_filename,
83  bool keep_high_res,
84  bool start_paused)
85  : tf_buffer_(ros::Duration(std::numeric_limits<int>::max(), 0)),
86  num_poses_(0),
87  display_cloud_(new pointcloud_t()),
88  voxelgrid_size_display_(voxelgrid_size_display),
89  voxelgrid_size_save_(voxelgrid_size_save),
90  min_distance_(min_distance),
91  max_distance_(max_distance),
92  output_filename_(output_filename),
93  keep_high_res_(keep_high_res),
94  pause_(start_paused),
95  viewer_("Roboception Cloud Viewer"),
96  viewer_thread_(boost::ref(viewer_))
97 {
98  viewer_.addCloudToViewer(display_cloud_);//so update can be called
99  ROS_INFO("Distance filter range: %.2fm - %.2fm", min_distance_, max_distance_);
100  ROS_INFO("Voxel grid filter for display: %.3fm", voxelgrid_size_display_);
101  ROS_INFO("Voxel grid filter for saving: %.3fm", voxelgrid_size_save_);
102 }
103 
104 
105 void CloudAccumulator::pointCloudCallback(const pointcloud_t::ConstPtr& pointcloud)
106 {
107  if(pause_){ return; }
108  static int counter = 0;
109  ++counter;
110  ROS_INFO_THROTTLE(10, "Received %d point cloud%s", counter, counter == 1 ? "" : "s");
111  double timestamp = pointcloud->header.stamp * 1e-6;//pcl stamps are in milliseconds
112  ROS_DEBUG("Storing cloud (%.3f)", timestamp);
113 
114  pointcloud_t::Ptr cropped_cloud = distanceFiltered(min_distance_, max_distance_, pointcloud);
115  if(keep_high_res_) { clouds_.push_back(cropped_cloud); }//store for later corrections & saving
116 
117  const Eigen::Affine3f transformation = lookupTransform(timestamp);
118  if(transformation.matrix()(0,0) == transformation.matrix()(0,0)) //not nan
119  {
120  pointcloud_t::Ptr transformed_cloud(new pointcloud_t());
121  pcl::transformPointCloud(*cropped_cloud, *transformed_cloud, transformation);
122 
123  boost::mutex::scoped_lock guard(display_cloud_mutex_);
124  *transformed_cloud += *display_cloud_;
125  //Voxelgridfilter to speedup display
128  ROS_INFO_ONCE("Change view to start displaying the clouds.");
129  }
130  else if(num_poses_ == 0)
131  {
132  ROS_INFO_THROTTLE(5,"Cannot transform current point cloud: No poses were received yet. "
133  "Is Stereo INS or SLAM running?");
134  }
135  else {
136  ROS_INFO_THROTTLE(5,"Cannot transform current point cloud: Pose not (yet) received\n"
137  "Current cloud stamp: %.3f\n"
138  "Latest pose stamp: %.3f", timestamp, last_pose_stamp_.toSec());
139  }
140 }
141 
142 
143 void CloudAccumulator::trajectoryCallback(const nav_msgs::PathConstPtr& path)
144 {
145  ROS_INFO("Received Trajectory with %zu poses", path->poses.size());
146  boost::mutex::scoped_lock guard(tf_buffer_mutex_);
147  tf_buffer_.clear();
148  num_poses_ = path->poses.size();
149  for(size_t i = 0; i < path->poses.size(); ++i)
150  //for(const geometry_msgs::PoseStamped& pose : path->poses)
151  {
152  tf_buffer_.setTransform(myPoseStampedMsgToTF(path->poses[i]), "trajectory");
153  }
154 }
155 
156 
157 void CloudAccumulator::poseCallback(const geometry_msgs::PoseStampedConstPtr& current_pose)
158 {
159  if(pause_){ return; }
160  ROS_INFO_ONCE("Received first live pose");
161  boost::mutex::scoped_lock guard(tf_buffer_mutex_);
162  tf_buffer_.setTransform(myPoseStampedMsgToTF(*current_pose), "current_pose");
163  last_pose_stamp_ = current_pose->header.stamp;
164  ++num_poses_;
165 }
166 
167 
168 Eigen::Affine3f CloudAccumulator::lookupTransform(double timestamp)
169 {
170  ros::Time rosstamp; rosstamp.fromSec(timestamp);//pcl timestamp is in microsec
171  //cannot use waitForTransform because of the concurrent use of the tf_buffer
172  try
173  {
174  ros::Time endtime = ros::Time::now() + ros::Duration(0.25);
175  do
176  {
177  {//lock-scope
178  boost::mutex::scoped_lock guard(tf_buffer_mutex_);
179  if(tf_buffer_.canTransform("world", "camera", rosstamp))
180  {
181  geometry_msgs::TransformStamped pose;
182  Eigen::Affine3f result = toAffine(tf_buffer_.lookupTransform("world", "camera", rosstamp)); //target, source
183  return result;
184  }
185  }
186  usleep(10000);
187  }
188  while(endtime > ros::Time::now());
189  }
190  catch(tf2::LookupException ex) { ROS_WARN("%s", ex.what()); }
191  catch(tf2::ExtrapolationException ex) { ROS_WARN("%s", ex.what()); }
192 
193  Eigen::Affine3f invalid_result;
194  invalid_result.matrix().setConstant(std::numeric_limits<double>::quiet_NaN());
195  return invalid_result;
196 }
197 
198 
200 {
201  ROS_INFO("(Re-)merging all stored clouds with latest poses.");
202  int counter = 0;
203  pointcloud_t::Ptr merged(new pointcloud_t());
204  for(size_t i = 0; i < clouds_.size(); ++i)
205  //for(std::map<double, pointcloud_t::ConstPtr>::iterator it = cloud_map_.begin(); it != cloud_map_.end(); ++it)
206  {
207  double timestamp = clouds_[i]->header.stamp * 1e-6;//pcl stamps are in milliseconds
208  const Eigen::Affine3f transformation = lookupTransform(timestamp);
209  if(transformation.matrix()(0,0) == transformation.matrix()(0,0)) //not nan
210  {
211  ++counter;
212  pointcloud_t transformed_cloud;
213  pcl::transformPointCloud(*clouds_[i], transformed_cloud, transformation);
214  *merged += transformed_cloud;
215  if(counter % 10 == 0){ ROS_INFO("Merged %d of %zu clouds.", counter, clouds_.size()); }
216  }
217  else { ROS_DEBUG("Skipped cloud at %.3f in merge, cannot get its position", timestamp); }
218  }
219  ROS_INFO("Merged %d/%zu clouds.", counter, clouds_.size());
220 
221  pointcloud_t::Ptr voxel_filtered(new pointcloud_t());
222  voxelGridFilter(voxelgrid_size_save_, merged, voxel_filtered);
223  return voxel_filtered;
224 }
225 
226 
227 bool CloudAccumulator::saveCloud(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp)
228 {
229  if(keep_high_res_) // consider slam corrections
230  {
231  pointcloud_t::Ptr merged = rebuildCloud();
232  pcl::io::savePCDFileASCII(output_filename_, *merged);
233 
234  //Now show the saved cloud
235  //(it will quickly get downfiltered in the point cloud callback though if not paused)
236  boost::mutex::scoped_lock guard(display_cloud_mutex_);
237  display_cloud_ = merged;
239  }
240  else { pcl::io::savePCDFileASCII(output_filename_, *display_cloud_); }//just save what we have
241 
242  ROS_INFO("Saved point cloud to %s", output_filename_.c_str());
243  return true;
244 }
245 
246 
247 bool CloudAccumulator::togglePause(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp)
248 {
249  pause_ = !pause_;
250  return true;
251 }
252 
253 }//namespace rc
CloudAccumulator(double voxelgrid_size_display, double voxelgrid_size_save, double min_distance, double max_distance, std::string output_filename, bool keep_high_res, bool start_paused)
void trajectoryCallback(const nav_msgs::PathConstPtr &path)
Replace all saved camera poses by those in path.
bool pause_
Whether to ignore input data.
void poseCallback(const geometry_msgs::PoseStampedConstPtr &current_pose)
Saves camera pose.
CloudVisualizer viewer_
tf2_ros::Buffer tf_buffer_
Stores the history of poses and allows to interpolate.
#define ROS_INFO_ONCE(...)
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
void pointCloudCallback(const pointcloud_t::ConstPtr &pointcloud)
For each cloud.
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
double voxelgrid_size_save_
Filter size for the saved cloud.
Eigen::Affine3f toAffine(const geometry_msgs::TransformStamped &tf_stamped)
pointcloud_t::Ptr display_cloud_
Cloud that is displayed.
#define ROS_WARN(...)
std::string output_filename_
Where to save the cloud.
std::deque< pointcloud_t::Ptr > clouds_
Storage for later corraection and saving.
double voxelgrid_size_display_
Filter size for the display cloud.
Time & fromSec(double t)
pointcloud_t::Ptr distanceFiltered(double min, double max, const pointcloud_t::ConstPtr input)
Filter cloud along the optical axis (Z) Filter if min < max, otherwise copy (deep, not only the pointer)
double min_distance_
Discard points closer than this.
boost::mutex display_cloud_mutex_
#define ROS_INFO(...)
boost::mutex tf_buffer_mutex_
Eigen::Affine3f lookupTransform(double timestamp)
void addCloudToViewer(const pointcloud_t::Ptr pointcloud)
Thread-safe addition/replacement of the cloud.
void updateCloudInViewer(const pointcloud_t::Ptr pointcloud)
Thread-safe update to the cloud.
double max_distance_
Discard points farther than this.
geometry_msgs::TransformStamped myPoseStampedMsgToTF(const geometry_msgs::PoseStamped &msg)
pointcloud_t::Ptr rebuildCloud()
For all saved clouds.
bool saveCloud(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp)
static Time now()
#define ROS_INFO_THROTTLE(rate,...)
void voxelGridFilter(double size, pointcloud_t::Ptr input, pointcloud_t::Ptr &output)
Filter if size >= 0, otherwise just copy the pointer.
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
size_t num_poses_
Keep track of pose count (to show a good error message)
bool togglePause(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp)
Toggle whether to ignore incoming data.
pcl::PointCloud< point_t > pointcloud_t
bool keep_high_res_
If false, do not store clouds. On save, save display cloud.
#define ROS_DEBUG(...)


rc_cloud_accumulator
Author(s): Felix Endres
autogenerated on Mon Jun 10 2019 14:34:34