void Projection::project_models_xyzrgb() { CLOG(LTRACE) << "project_models_xyzrgb"; vector<Types::HomogMatrix> poses = in_poses.read(); pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene = in_cloud_xyzrgb_scene.read(); vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> models = in_model_clouds_xyzrgb.read(); if(models.size() != poses.size()){ CLOG(LERROR) << "Wrong models vector size"; return; } CLOG(LDEBUG) << "Scene points " << scene->size(); /** * Generates clouds for each instances found */ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> instances; std::vector<pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr> parts_of_scene; for (size_t i = 0; i < poses.size (); ++i) { //rotate model pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_model (new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::transformPointCloud (*(models[i]), *rotated_model, poses[i]); instances.push_back (rotated_model); //cut part of scene pcl::PointXYZRGB minPt, maxPt; pcl::getMinMax3D(*rotated_model, minPt, maxPt); minPt.x -= bounding_box_epsilon; minPt.y -= bounding_box_epsilon; minPt.z -= bounding_box_epsilon; maxPt.x += bounding_box_epsilon; maxPt.y += bounding_box_epsilon; maxPt.z += bounding_box_epsilon; pcl::PointCloud<pcl::PointXYZRGB>::Ptr part_of_scene (new pcl::PointCloud<pcl::PointXYZRGB> ()); *part_of_scene = *scene; pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud (part_of_scene); pass.setFilterFieldName ("x"); pass.setFilterLimits (minPt.x, maxPt.x); pass.setFilterLimitsNegative (false); pass.filter (*part_of_scene); pass.setFilterFieldName ("y"); pass.setFilterLimits (minPt.y, maxPt.y); pass.setFilterLimitsNegative (false); pass.filter (*part_of_scene); pass.setFilterFieldName ("z"); pass.setFilterLimits (minPt.z, maxPt.z); pass.setFilterLimitsNegative (false); pass.filter (*part_of_scene); parts_of_scene.push_back(part_of_scene); CLOG(LDEBUG) << "part of scene " << i << " points " << part_of_scene->size(); } /** * ICP */ if(use_icp){ pcl::VoxelGrid<pcl::PointXYZRGB> vg; pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene_filtered (new pcl::PointCloud<pcl::PointXYZRGB>); vg.setInputCloud (scene); vg.setLeafSize (voxel_grid_resolution, voxel_grid_resolution, voxel_grid_resolution); vg.filter (*scene_filtered); std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> registered_instances; CLOG(LINFO) << "ICP"; for (size_t i = 0; i < poses.size (); ++i) { pcl::VoxelGrid<pcl::PointXYZRGB> vg; pcl::PointCloud<pcl::PointXYZRGB>::Ptr instance (new pcl::PointCloud<pcl::PointXYZRGB>); vg.setInputCloud (instances[i]); vg.setLeafSize (voxel_grid_resolution, voxel_grid_resolution, voxel_grid_resolution); vg.filter (*instance); pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp; icp.setMaximumIterations (icp_max_iter); icp.setMaxCorrespondenceDistance (icp_corr_distance); icp.setInputTarget (scene_filtered); //parts_of_scene[i]); icp.setInputSource (instance); //instances[i]); pcl::PointCloud<pcl::PointXYZRGB>::Ptr registered (new pcl::PointCloud<pcl::PointXYZRGB>); icp.align (*registered); cout<< "Registered instance " << i << " size " << registered->size() <<endl; registered_instances.push_back (registered); CLOG(LINFO) << "Instance " << i << " "; if (icp.hasConverged ()) { CLOG(LINFO) << "Aligned!" << endl; } else { CLOG(LINFO) << "Not Aligned!" << endl; } } out_registered_instances_xyzrgb.write(registered_instances); } else out_registered_instances_xyzrgb.write(instances); //link parts to one cloud pcl::PointCloud<pcl::PointXYZRGB>::Ptr scene1(new pcl::PointCloud<pcl::PointXYZRGB>); for(int i = 0; i < parts_of_scene.size(); i++){ *scene1 += *(parts_of_scene[i]); } out_parts_of_scene_xyzrgb.write(scene1); }
// Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); FeatureCloudT::Ptr object_features (new FeatureCloudT); FeatureCloudT::Ptr scene_features (new FeatureCloudT); // Get input object and scene if (argc != 3) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 || pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0) { pcl::console::print_error ("Error loading object/scene file!\n"); return (1); } std::cout << "object density " << object->is_dense << std::endl; PointCloudT::Ptr object_filtered (new PointCloudT); std::vector<int> filter_index; pcl::removeNaNFromPointCloud (*object, *object_filtered, filter_index); object = object_filtered; std::cout << "scene density " << scene->is_dense << std::endl; PointCloudT::Ptr scene_filtered (new PointCloudT); filter_index.clear(); pcl::removeNaNFromPointCloud (*scene, *scene_filtered, filter_index); scene = scene_filtered; // Downsample // pcl::console::print_highlight ("Downsampling...\n"); // pcl::VoxelGrid<PointNT> grid; // const float leaf = 0.005f; // grid.setLeafSize (leaf, leaf, leaf); // grid.setInputCloud (object); // grid.filter (*object); // grid.setInputCloud (scene); // grid.filter (*scene); std::cout << "object size " << object->size() << std::endl; // Estimate normals for scene pcl::console::print_highlight ("Estimating scene normals...\n"); pcl::NormalEstimationOMP<PointNT,PointNT> nest; nest.setRadiusSearch (0.01); nest.setInputCloud (scene); nest.compute (*scene); // Estimate features pcl::console::print_highlight ("Estimating features...\n"); FeatureEstimationT fest; fest.setRadiusSearch (0.025); fest.setInputCloud (object); fest.setInputNormals (object); fest.compute (*object_features); fest.setInputCloud (scene); fest.setInputNormals (scene); fest.compute (*scene_features); // Perform alignment pcl::console::print_highlight ("Starting alignment...\n"); pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align; align.setInputSource (object); align.setSourceFeatures (object_features); align.setInputTarget (scene); align.setTargetFeatures (scene_features); align.setMaximumIterations (50000); // Number of RANSAC iterations align.setNumberOfSamples (100); // Number of points to sample for generating/prerejecting a pose align.setCorrespondenceRandomness (5); // Number of nearest features to use align.setSimilarityThreshold (0.8f); // Polygonal edge length similarity threshold align.setMaxCorrespondenceDistance (0.0025f); // Inlier threshold align.setInlierFraction (0.15f); // Required inlier fraction for accepting a pose hypothesis { pcl::ScopeTime t("Alignment"); align.align (*object_aligned); } if (align.hasConverged ()) { // Print results printf ("\n"); Eigen::Matrix4f transformation = align.getFinalTransformation (); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2)); pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2)); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2)); pcl::console::print_info ("\n"); pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3)); pcl::console::print_info ("\n"); pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ()); // Show alignment pcl::visualization::PCLVisualizer visu("Alignment"); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned"); visu.spin (); } else { pcl::console::print_error ("Alignment failed!\n"); return (1); } return (0); }