register_box.cpp
Go to the documentation of this file.
00001 #include <simple_object_capture/common.h>
00002 
00003 #include <pcl/registration/icp.h>
00004 #include <pcl/registration/registration.h>
00005 #include <pcl/registration/transformation_estimation.h>
00006 #include <pcl/registration/transformation_estimation_point_to_plane.h>
00007 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
00008 #include <pcl/point_types.h>
00009 #include <pcl/io/pcd_io.h>
00010 #include <pcl/features/normal_3d.h>
00011 #include <pcl/features/integral_image_normal.h>
00012 #include <pcl/sample_consensus/method_types.h>
00013 #include <pcl/sample_consensus/model_types.h>
00014 #include <pcl/segmentation/sac_segmentation.h>
00015 #include <pcl/surface/mls.h>
00016 #include <pcl/filters/radius_outlier_removal.h>
00017 
00018 
00019 #include <rosbag/bag.h>
00020 #include <rosbag/query.h>
00021 #include <rosbag/view.h>
00022 #include <string>
00023 #include <sstream>
00024 #include <fstream>
00025 #include <sensor_msgs/CameraInfo.h>
00026 #include <sensor_msgs/Image.h>
00027 #include <geometry_msgs/PoseStamped.h>
00028 #include <image_geometry/pinhole_camera_model.h>
00029 #include <sensor_msgs/image_encodings.h>
00030 #include <boost/filesystem.hpp>
00031 
00032 typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> PointToPlane;
00033 
00034 namespace enc = sensor_msgs::image_encodings;
00035 
00036 template <class T>
00037 std::vector<typename T::ConstPtr> getAllMsgs(const rosbag::Bag& bag, const std::string& topic)
00038 {
00039   std::vector<typename T::ConstPtr> res;
00040 
00041   rosbag::View view(bag, rosbag::TopicQuery(topic));
00042   BOOST_FOREACH(rosbag::MessageInstance const m, view)
00043   {
00044     typename T::ConstPtr i = m.instantiate<T>();
00045     res.push_back(i);
00046   }
00047   return res;
00048 }
00049 
00050 
00051 template <class P>
00052 void pcdWrite(typename pcl::PointCloud<P>::Ptr cloud, const std::string& name, int count, const boost::filesystem::path& folder = "")
00053 {
00054   std::stringstream s;
00055 
00056   // create folder if needed
00057   if (folder != "")
00058   {
00059     s << folder.string() << "/";
00060     if (!boost::filesystem::is_directory(folder))
00061       boost::filesystem::create_directory(folder);
00062   }
00063 
00064   pcl::PCDWriter write;
00065   s << name;
00066   if (count >= 0)
00067   {
00068     s << "_";
00069     if (count < 10)
00070       s << "00";
00071     else if (count < 100)
00072       s << "0";
00073     s << count;
00074   }
00075   s << ".pcd";
00076   write.write(s.str(), *cloud);
00077 }
00078 
00079 
00080 
00081 int main(int argc, char** argv)
00082 {
00083   // usage
00084   if (argc != 3)
00085   {
00086     printf("Usage: ./register_box bagfile output_folder\n");
00087     return -1;
00088   }
00089 
00090   // open bag file
00091   rosbag::Bag bag;
00092   std::string bagfile(argv[1]);
00093   std::string folder(argv[2]);
00094   printf("Opening bag file %s\n", bagfile.c_str());
00095   bag.open(bagfile, rosbag::bagmode::Read);
00096 
00097   // get messages from bag file
00098   std::vector<sensor_msgs::CameraInfo::ConstPtr> depth_info = getAllMsgs<sensor_msgs::CameraInfo>(bag, "camera/depth/camera_info");
00099   std::vector<sensor_msgs::Image::ConstPtr> depth_image = getAllMsgs<sensor_msgs::Image>(bag, "camera/depth/image");
00100   std::vector<sensor_msgs::Image::ConstPtr> rgb_image = getAllMsgs<sensor_msgs::Image>(bag, "camera/rgb/image_color");
00101   std::vector<sensor_msgs::Image::ConstPtr> mask = getAllMsgs<sensor_msgs::Image>(bag, "camera/mask");
00102   std::vector<geometry_msgs::PoseStamped::ConstPtr> pose = getAllMsgs<geometry_msgs::PoseStamped>(bag, "camera/pose");
00103   assert(depth_info.size() == depth_image.size());
00104   assert(depth_info.size() == rgb_image.size());
00105   assert(depth_info.size() == mask.size());
00106   assert(depth_info.size() == pose.size());
00107   printf("Read %zu pointcloud snapshots from bag file\n", depth_info.size());
00108   bag.close();
00109 
00110   // create pointclouds from messages
00111   std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> full_clouds;
00112   std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> normal_clouds;
00113   
00114   printf("Importing all pointclouds\n");
00115   for (unsigned i=0; i<depth_info.size(); i++)
00116   {
00117     printf("  - Importing pointcloud %d / %d\n", (int)i+1, (int)depth_info.size());
00118 
00119     if (depth_image[i]->encoding != enc::TYPE_32FC1)
00120     {
00121       printf("Expected data of type [%s] for depth image, got [%s]", enc::TYPE_32FC1.c_str(), depth_image[i]->encoding.c_str());
00122       return -1;
00123     }
00124 
00125     // Update the camera model 
00126     image_geometry::PinholeCameraModel model;
00127     model.fromCameraInfo(depth_info[i]);
00128     float center_x = model.cx();
00129     float center_y = model.cy();
00130     float constant_x = 1.0 / model.fx();
00131     float constant_y = 1.0 / model.fy();
00132     float bad_point = std::numeric_limits<float>::quiet_NaN ();
00133 
00134     // create initial pointlcoud  
00135     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00136     cloud->header = depth_image[i]->header;
00137     cloud->height = depth_image[i]->height;
00138     cloud->width  = depth_image[i]->width;
00139     cloud->is_dense = false;
00140     cloud->points.resize (cloud->height * cloud->width);
00141 
00142     // create the pointcloud while applying the mask
00143     int idx = 0;
00144     pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter = cloud->begin ();
00145     const float* depth_data = reinterpret_cast<const float*>(&depth_image[i]->data[0]);
00146     const uint8_t* mask_data  = reinterpret_cast<const uint8_t*>(&mask[i]->data[0]);
00147     const uint8_t* image_data  = reinterpret_cast<const uint8_t*>(&rgb_image[i]->data[0]);
00148 
00149     for (int v = 0; v < (int)cloud->height; ++v)
00150     {
00151       for (int u = 0; u < (int)cloud->width; ++u, ++idx, ++pt_iter)
00152       {
00153         pcl::PointXYZRGB& pt = *pt_iter;
00154 
00155         // Check for invalid measurements
00156         if (mask_data[idx] == 0 || depth_data[idx] == 0)
00157         {
00158           // not valid
00159           pt.x = pt.y = pt.z = bad_point;
00160           continue;
00161         }
00162 
00163         // Fill in XYZ
00164         pt.x = (u - center_x) * depth_data[idx] * constant_x;
00165         pt.y = (v - center_y) * depth_data[idx] * constant_y;
00166         pt.z = depth_data[idx];
00167 
00168         pt.r = image_data[3*idx+0];
00169         pt.g = image_data[3*idx+1];
00170         pt.b = image_data[3*idx+2];
00171       }
00172     }
00173 
00174     // transform pointcloud with prior
00175     transform_inverse<pcl::PointXYZRGB>(cloud, cloud, pose[i]->pose);
00176 
00177     // downsample cloud
00178     pcl::PointCloud<pcl::PointXYZRGB>::Ptr down_cloud = downsample<pcl::PointXYZRGB>(cloud, 0.005);
00179 
00180     //compute normals for the cloud
00181     pcl::PointCloud<pcl::Normal>::Ptr normals = compute_surface_normals<pcl::PointXYZRGB, pcl::Normal>(down_cloud, 0.03);
00182     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr normal_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00183     pcl::concatenateFields(*normals, *down_cloud, *normal_cloud);
00184 
00185     // write pointcloud to file for debugging
00186     pcdWrite<pcl::PointXYZRGBNormal>(normal_cloud, "normal", i, folder + "_debug");
00187     pcdWrite<pcl::PointXYZRGB>(cloud, "full", i, folder + "_debug");
00188 
00189     // store result
00190     normal_clouds.push_back(normal_cloud);
00191     full_clouds.push_back(cloud);
00192   }
00193 
00194 
00195   // create point-to-plane icp object
00196   printf("Registring all pointclouds\n");
00197   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGBNormal>(*(normal_clouds[0])));
00198   pcl::PointCloud<pcl::PointXYZRGB>::Ptr full(new pcl::PointCloud<pcl::PointXYZRGB>(*(full_clouds[0])));
00199   pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icp;
00200   icp.setTransformationEstimation(pcl::registration::TransformationEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal>::Ptr(new PointToPlane));
00201 
00202   for (unsigned i=1; i<normal_clouds.size(); i++)
00203   {
00204     printf("  - Registring pointcloud %d to clouds 1-%d\n", (int)i+1, (int)i);
00205     // smooth cloud and recompute normals
00206     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr target_smooth(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00207     pcl::MovingLeastSquares<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> mls;
00208     mls.setInputCloud(target);
00209     mls.setSearchRadius(0.03);
00210     mls.setSqrGaussParam(0.03*0.03);
00211     mls.setPolynomialOrder(2);
00212     mls.setPolynomialFit(true);
00213     mls.setOutputNormals(target_smooth);
00214     mls.reconstruct(*target_smooth);
00215 
00216     icp.setInputTarget(target_smooth);
00217     icp.setInputCloud(normal_clouds[i]);
00218     pcl::PointCloud<pcl::PointXYZRGBNormal> Final;
00219     icp.align(Final);
00220     
00221     // build up cloud to register against
00222     pcl::transformPointCloudWithNormals(*(normal_clouds[i]), *(normal_clouds[i]), icp.getFinalTransformation());
00223     *target += *(normal_clouds[i]);
00224 
00225     // build up full cloud
00226     pcl::transformPointCloud(*(full_clouds[i]), *(full_clouds[i]), icp.getFinalTransformation());
00227     *full += *(full_clouds[i]);
00228 
00229     // write result to disk
00230     pcdWrite<pcl::PointXYZRGBNormal>(target, "target", i, folder + "_debug");
00231     pcdWrite<pcl::PointXYZRGBNormal>(target_smooth, "target_smooth", i, folder + "_debug");
00232     pcdWrite<pcl::PointXYZRGB>(full, "target_full", i, folder + "_debug");
00233   } 
00234 
00235   printf("Downsampling and smoothing result\n");
00236   // downsample cloud
00237   pcl::PointCloud<pcl::PointXYZRGB>::Ptr full_down = downsample<pcl::PointXYZRGB>(full, 0.002);
00238   pcdWrite<pcl::PointXYZRGB>(full_down, "final_down", -1, folder + "_debug");
00239   
00240   //compute normals for the cloud
00241   pcl::PointCloud<pcl::Normal>::Ptr tmp_normals = compute_surface_normals<pcl::PointXYZRGB, pcl::Normal>(full_down, 0.03);
00242   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr full_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00243   pcl::concatenateFields(*tmp_normals, *full_down, *full_normals);
00244 
00245   // smooth cloud and recompute normals
00246   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr full_smooth(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00247   pcl::MovingLeastSquares<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> mls;
00248   mls.setInputCloud(full_normals);
00249   mls.setSearchRadius(0.03);
00250   mls.setSqrGaussParam(0.03*0.03);
00251   mls.setPolynomialOrder(2);
00252   mls.setPolynomialFit(true);
00253   mls.setOutputNormals(full_smooth);
00254   mls.reconstruct(*full_smooth);
00255   pcdWrite<pcl::PointXYZRGBNormal>(full_smooth, "final_smooth", -1, folder + "_debug");
00256 
00257   // remove outliers
00258   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr full_outliers(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00259   pcl::RadiusOutlierRemoval<pcl::PointXYZRGBNormal> ror;
00260   ror.setRadiusSearch(0.01);
00261   ror.setInputCloud(full_smooth);
00262   ror.setMinNeighborsInRadius(60);
00263   ror.filter(*full_outliers);  
00264   pcdWrite<pcl::PointXYZRGBNormal>(full_outliers, "final_smooth_outliers", -1, folder + "_debug");
00265 
00266 
00267 
00268   // write output for RoboEarth
00269   // --------------------------
00270 
00271   // write dense pointclouds of measurements
00272   for (unsigned i=0; i<full_clouds.size(); i++)
00273     pcdWrite<pcl::PointXYZRGB>(full_clouds[i], "dense_face", i, folder);
00274   pcdWrite<pcl::PointXYZRGBNormal>(full_outliers, "merged", -1, folder);
00275 
00276   std::ofstream f((folder + "/meta.xml").c_str());
00277   f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl 
00278     << "<model>" << std::endl
00279     << "  <!-- name of the model, as it is stored in the database. i.e. 'object class'.'object name' -->" << std::endl
00280     << "  <name>" << folder << "</name>" << std::endl
00281     << std::endl
00282     << "  <!-- object model type, for pointcloud models 'kinect_pcl' -->" << std::endl
00283     << "  <type>kinect_pcl</type>" << std::endl
00284     << std::endl
00285     << "  <!-- number of dense_face_xxx.pcd files -->" << std::endl
00286     << "  <faces>" << full_clouds.size() << "</faces>" << std::endl
00287     << std::endl
00288     << "  <!-- this is only for compatibility with re_vision's models -->" << std::endl
00289     << "  <dimensions> <scale>1</scale> </dimensions>" << std::endl
00290     << std::endl
00291     << "</model>" << std::endl;
00292   f.close();
00293 
00294   std::stringstream cmd;
00295   cmd << "zip -r " << folder << ".zip " << folder;
00296   int res = system(cmd.str().c_str());
00297   if (res != 0)
00298     printf("Failed to create zip file called %s.zip\n", folder.c_str());
00299 
00300   printf("Finished!\nThe result is stored in %s.zip\n", folder.c_str());
00301   return 0;
00302 }


simple_object_capture
Author(s): Stuart Glaser
autogenerated on Mon Dec 2 2013 12:05:59