$search
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 }