void ICP( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 )
{
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
///////////////////ICP - REGISTRATION ////////////////
LOGI("Start ICP...%d,%d,%d,%d",cloud1->width,cloud1->height,cloud2->width,cloud2->height);
MyReg* mr=new MyReg();
PointCloud::Ptr src (new PointCloud(mr->width_ds,mr->height_ds));
PointCloud::Ptr tgt (new PointCloud(mr->width_ds,mr->height_ds));
LOGI("Start Downsampling...");
mr->DownSampling(cloud1,cloud2,src,tgt);
pcl::PointCloud<PointNormalT>::Ptr points_with_normals_src (new pcl::PointCloud<PointNormalT>);
pcl::PointCloud<PointNormalT>::Ptr points_with_normals_tgt (new pcl::PointCloud<PointNormalT>);
LOGI("Start Normals estimation...");
mr->Normals(points_with_normals_src,points_with_normals_tgt,src,tgt);
LOGI("Start Matrix estimation...");
Eigen::Matrix4f GlobalTransf;
mr->MatrixEstimation(points_with_normals_src, points_with_normals_tgt, GlobalTransf);
string outputstr;
ostringstream out_message;
out_message << GlobalTransf;
outputstr=out_message.str();
LOGI("%s", outputstr.c_str());
PointCloud::Ptr transf (new PointCloud);
// pcl::transformPointCloud (*cloud1, *transf, GlobalTransf);
pcl::transformPointCloud (*src, *transf, GlobalTransf);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
LOGI("Time of registration: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
// LOGI("Transform");
// std::cout<< GlobalTransf <<endl;
// LOGI("point-size: %d", transf->points.size());
/*ShowPCLtoKiwi(cloud1,255,0,0,-1.5);
ShowPCLtoKiwi(src,0,255,0,-1.6);
//ShowPCLtoKiwi(cloud2,0,255,0,1.5);
*/
ShowPCLtoKiwi(src,255,0,0,-1.5);
ShowPCLtoKiwi(tgt,0,255,0,-1.5);
ShowPCLtoKiwi(transf,255,0,0,1.5);
ShowPCLtoKiwi(tgt,0,255,0,1.5);
}
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
//
// Downsample for consistency and speed
// \note enable this for large datasets
PointCloud::Ptr src (new PointCloud);
PointCloud::Ptr tgt (new PointCloud);
pcl::VoxelGrid<PointT> grid;
if (downsample)
{
grid.setLeafSize (0.05, 0.05, 0.05);
grid.setInputCloud (cloud_src);
grid.filter (*src);
grid.setInputCloud (cloud_tgt);
grid.filter (*tgt);
}
else
{
src = cloud_src;
tgt = cloud_tgt;
}
// Compute surface normals and curvature
PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);
pcl::NormalEstimation<PointT, PointNormalT> norm_est;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
norm_est.setSearchMethod (tree);
norm_est.setKSearch (30);
norm_est.setInputCloud (src);
norm_est.compute (*points_with_normals_src);
pcl::copyPointCloud (*src, *points_with_normals_src);
norm_est.setInputCloud (tgt);
norm_est.compute (*points_with_normals_tgt);
pcl::copyPointCloud (*tgt, *points_with_normals_tgt);
//
// Instantiate our custom point representation (defined above) ...
MyPointRepresentation point_representation;
// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues (alpha);
//
// Align
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
reg.setTransformationEpsilon (1e-6);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance (0.1);
// Set the point representation
reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputCloud (points_with_normals_src);
reg.setInputTarget (points_with_normals_tgt);
//
// Run the same optimization in a loop and visualize the results
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations (2);
for (int i = 0; i < 30; ++i)
{
PCL_INFO ("Iteration Nr. %d.\n", i);
// save cloud for visualization purpose
points_with_normals_src = reg_result;
// Estimate
reg.setInputCloud (points_with_normals_src);
reg.align (*reg_result);
//accumulate transformation between each Iteration
Ti = reg.getFinalTransformation () * Ti;
//if the difference between this transformation and the previous one
//is smaller than the threshold, refine the process by reducing
//the maximal correspondence distance
if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
prev = reg.getLastIncrementalTransformation ();
// visualize current state
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}
//
// Get the transformation from target to source
targetToSource = Ti.inverse();
//
// Transform target back in source frame
pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
p->removePointCloud ("source");
p->removePointCloud ("target");
PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);
p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
p->spin ();
p->removePointCloud ("source");
p->removePointCloud ("target");
//add the source to the transformed target
*output += *cloud_src;
final_transform = targetToSource;
}
/** registration two clouds using ICP with projective correspondence */
void ICP()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file1, *cloud1) == -1) //* load the file
{
LOGI("Couldn't read file1");
return;
}
LOGI("Loaded %d data points from file2",cloud1->width * cloud1->height);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file2, *cloud2) == -1) //* load the file
{
LOGI("Couldn't read file2");
return;
}
LOGI("Loaded %d data points from file2",cloud1->width * cloud1->height);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
///////////////////ICP - REGISTRATION ////////////////
MyReg* mr=new MyReg();
PointCloud::Ptr src (new PointCloud(mr->width_ds,mr->height_ds));
PointCloud::Ptr tgt (new PointCloud(mr->width_ds,mr->height_ds));
mr->DownSampling(cloud1,cloud2,src,tgt);
LOGI("Start Downsampling...");
pcl::PointCloud<PointNormalT>::Ptr points_with_normals_src (new pcl::PointCloud<PointNormalT>);
pcl::PointCloud<PointNormalT>::Ptr points_with_normals_tgt (new pcl::PointCloud<PointNormalT>);
LOGI("Start Normals estimation...");
mr->Normals(points_with_normals_src,points_with_normals_tgt,src,tgt);
LOGI("Start Matrix estimation...");
Eigen::Matrix4f GlobalTransf;
mr->MatrixEstimation(points_with_normals_src, points_with_normals_tgt, GlobalTransf);
string outputstr;
ostringstream out_message;
out_message << GlobalTransf;
outputstr=out_message.str();
LOGI("%s", outputstr.c_str());
PointCloud::Ptr transf (new PointCloud);
// pcl::transformPointCloud (*cloud1, *transf, GlobalTransf);
pcl::transformPointCloud (*src, *transf, GlobalTransf);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
LOGI("Time of registration: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
// LOGI("Transform");
// std::cout<< GlobalTransf <<endl;
// LOGI("point-size: %d", transf->points.size());
ShowPCLtoKiwi(src,255,0,0,-1.5);
ShowPCLtoKiwi(tgt,0,255,0,-1.5);
ShowPCLtoKiwi(transf,255,0,0,1.5);
ShowPCLtoKiwi(tgt,0,255,0,1.5);
}
void pairAlign( const PointCloud::Ptr cloud_src , const PointCloud::Ptr cloud_tgt , Eigen::Matrix4f &final_transform )
{
PointCloud::Ptr src (new PointCloud);
PointCloud::Ptr tgt (new PointCloud);
*src = *cloud_src;
*tgt = *cloud_tgt;
// Compute surface normals and curvature
PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);
pcl::NormalEstimation<PointT, PointNormalT> norm_est;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
norm_est.setSearchMethod (tree);
norm_est.setKSearch (30);
norm_est.setInputCloud (src);
norm_est.compute (*points_with_normals_src);
pcl::copyPointCloud (*src, *points_with_normals_src);
norm_est.setInputCloud (tgt);
norm_est.compute (*points_with_normals_tgt);
pcl::copyPointCloud (*tgt, *points_with_normals_tgt);
// Instantiate our custom point representation (defined above) ...
MyPointRepresentation point_representation;
// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_representation.setRescaleValues (alpha);
//
// Align
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
reg.setTransformationEpsilon (1e-3);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance (0.5);
// Set the point representation
reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
reg.setInputSource (points_with_normals_src);
reg.setInputTarget (points_with_normals_tgt);
//
// Run the same optimization in a loop and visualize the results
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations (2);
//originally iterates up to 30
for (int i = 0; i < 10; ++i)
{
// save cloud for visualization purpose
points_with_normals_src = reg_result;
// Estimate
reg.setInputSource (points_with_normals_src);
reg.align (*reg_result);
//accumulate transformation between each Iteration
Ti = reg.getFinalTransformation () * Ti;
//if the difference between this transformation and the previous one
//is smaller than the threshold, refine the process by reducing
//the maximal correspondence distance
if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
{
reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);
}
prev = reg.getLastIncrementalTransformation ();
}
//
// Get the transformation from target to source
targetToSource = Ti.inverse();
final_transform = targetToSource;
}
