void pairAlign3(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;
// ICP object.
PointCloud::Ptr finalCloud(new PointCloud);
pcl::IterativeClosestPointNonLinear<PointT , PointT> registration;
registration.setInputSource( src );
registration.setInputTarget( tgt );
registration.setTransformationEpsilon( 0.001 );
registration.align(*finalCloud);
if (registration.hasConverged())
{
//std::cout << "ICP converged." << std::endl
// << "The score is " << registration.getFitnessScore() << std::endl;
//std::cout << "Transformation matrix:" << std::endl;
//std::cout << registration.getFinalTransformation() << std::endl;
}
else std::cout << "ICP did not converge." << std::endl;
final_transform = registration.getFinalTransformation();
}
int
main(int argc, char** argv)
{
// Objects for storing the point clouds.
pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr finalCloud(new pcl::PointCloud<pcl::PointXYZ>);
// Read two PCD files from disk.
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *sourceCloud) != 0)
{
return -1;
}
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[2], *targetCloud) != 0)
{
return -1;
}
// ICP object.
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration;
registration.setInputSource(sourceCloud);
registration.setInputTarget(targetCloud);
registration.align(*finalCloud);
if (registration.hasConverged())
{
std::cout << "ICP converged." << std::endl
<< "The score is " << registration.getFitnessScore() << std::endl;
std::cout << "Transformation matrix:" << std::endl;
std::cout << registration.getFinalTransformation() << std::endl;
}
else std::cout << "ICP did not converge." << std::endl;
}
int main(int argc, char* argv[])
{
std::cout<<"Merging maps...\n";
// *** Check input
if(argc < 4)
{
std::cout<<"Usage: ./mergeMaps <map1.pcd> <map2.pcd> <mergedMap.pcd> [transform-2-to-1.txt]";
return -1;
}
// *** Read data
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>());
if (pcl::io::loadPCDFile (argv[1], *cloud1) < 0)
{
std::cout << "Error loading point cloud " << argv[1] << std::endl;
return -1;
}
if (pcl::io::loadPCDFile (argv[2], *cloud2) < 0)
{
std::cout << "Error loading point cloud " << argv[2] << std::endl;
return -1;
}
// *** Transformation (optional)
Eigen::Matrix4f transform;
if(argc > 4)
readTransform(argv[4], transform);
else
transform.setIdentity();
pcl::transformPointCloud(*cloud1, *cloud1, transform);
// *** Merging
pcl::PointCloud<pcl::PointXYZRGB>::Ptr finalCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
*finalCloud = *cloud1 + *cloud2;
// *** Save to file
if (pcl::io::savePCDFile(argv[3], *finalCloud) < 0)
{
std::cout << "Error saving the point cloud in "<<argv[4] << std::endl;
return -1;
}
std::cout << "Successfully merged maps" << std::endl;
return 0;
}