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);
}
