#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/range_image_visualizer.h>
void visualize_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd)
{
// Visualization of keypoints along with the original cloud
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (pcd, 255, 0, 0);
viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
viewer.addPointCloud(pcd, cloud_color_handler, "cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
while(!viewer.wasStopped ())
{
viewer.spinOnce ();
}
}
void visualize_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_src1,
pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_src2,
pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_final)
{
int vp_1, vp_2;
// Create a PCLVisualizer object
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src1_h (pcd_src1, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src2_h (pcd_src2, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h (pcd_final, 0, 255, 0);
viewer.addPointCloud (pcd_src1, src1_h, "vp1_src1", vp_1);
viewer.addPointCloud (pcd_src2, src2_h, "vp1_src2", vp_1);
viewer.addPointCloud (pcd_final, final_h, "vp1_final", vp_2);
viewer.spin();
}
int main ()
{
std::string fname_src = "D:/pcl_test/robot1.pcd";
std::string fname_tgt = "D:/pcl_test/robot2.pcd";
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt (new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ> (fname_src, *cloud_src) == -1)
{
PCL_ERROR("Couldn't read file");
return -1;
}
if(pcl::io::loadPCDFile<pcl::PointXYZ> (fname_tgt, *cloud_tgt) == -1)
{
PCL_ERROR("Couldn't read file");
return -1;
}
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_src);
icp.setInputTarget(cloud_tgt);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final (new pcl::PointCloud<pcl::PointXYZ>);;
icp.align(*cloud_final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
pcl::io::savePCDFileASCII("D://PCL//icp_result.pcd", *cloud_final);
//visualize_pcd(Final);
visualize_pcd(cloud_src, cloud_tgt, cloud_final);
return (0);
}
|