#include #include #include #include #include #include #include #include // TicToc typedef pcl::PointXYZ PointT; typedef pcl::PointCloud PointCloudT; bool next_iteration = false; void print4x4Matrix (const Eigen::Matrix4d & matrix) { printf ("Rotation matrix :\n"); printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2)); printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2)); printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2)); printf ("Translation vector :\n"); printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3)); } void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, void* nothing) { if (event.getKeySym () == "space" && event.keyDown ()) next_iteration = true; } int import_depth_map(std::string const& file_depthmap, PointCloudT::Ptr const& cloud){ std::ifstream depthmap(file_depthmap.c_str()); float fx(365.166), fy(365.166), cx(259.653), cy(206.166); if(depthmap.is_open()) { std::string line; int height(0), width(0), counter(0); while(getline(depthmap, line)) { std::istringstream is_before(line); width = 0; for(std::istream_iterator it = std::istream_iterator(is_before); it != std::istream_iterator(); ++it) { if((*it)*4500.< 1260. && width > 80 && width < 432){ PointT point3d; point3d.x = (width-cx)*(*it)*4500.f/fx; point3d.y = (cy-height)*(*it)*4500.f/fy; point3d.z = (*it)*4500.f; cloud->points.push_back(point3d);++counter;} width++; } height++; } cloud->header.frame_id = file_depthmap.substr(0,file_depthmap.find(".")); cloud->height = 1; cloud->width = counter; cloud->is_dense = true; cloud->points.resize(counter); std::cout << file_depthmap << " load" << std::endl; return 0; } else { return 1; } } int main (int argc, char* argv[]) { // The point clouds we will be using PointCloudT::Ptr cloud_in (new PointCloudT); // Original point cloud PointCloudT::Ptr cloud_tr (new PointCloudT); // Transformed point cloud PointCloudT::Ptr cloud_icp (new PointCloudT); // ICP output point cloud // Checking program arguments if (argc < 4) { printf ("Usage :\n"); printf ("\t\t%s point_cloud1.txt point_cloud2.txt number_of_ICP_iterations threshold\n", argv[0]); return (-1); } int iterations = atoi(argv[3]); // Default number of ICP iterations if (iterations < 1) { PCL_ERROR ("Number of initial iterations must be >= 1\n"); return (-1); } // Defining a rotation matrix and translation vector Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity (); pcl::console::TicToc time; time.tic (); if (import_depth_map(argv[1], cloud_in) != 0) { PCL_ERROR ("Error loading cloud %s.\n", argv[1]); return (-1); } std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl; time.tic (); if (import_depth_map(argv[2], cloud_icp) != 0) { PCL_ERROR ("Error loading cloud %s.\n", argv[2]); return (-1); } std::cout << "\nLoaded file " << argv[2] << " (" << cloud_icp->size () << " points) in " << time.toc () << " ms\n" << std::endl; *cloud_tr = *cloud_icp; // We backup cloud_icp into cloud_tr for later use // The Iterative Closest Point algorithm time.tic (); pcl::IterativeClosestPoint icp; icp.setMaximumIterations (iterations); //Avec des kdtrees a la main // boost::shared_ptr > kdtree_icp (new pcl::search::KdTree); // boost::shared_ptr > kdtree_in (new pcl::search::KdTree); // kdtree_icp->setInputCloud(cloud_icp); // kdtree_in->setInputCloud(cloud_in); // icp.setSearchMethodSource(kdtree_icp, false); // icp.setSearchMethodTarget(kdtree_in, false); std::cout << "RANSAC : " << icp.getRANSACIterations() << std::endl; //icp.setMaxCorrespondenceDistance (100); icp.setUseReciprocalCorrespondences(true); icp.setInputSource (cloud_icp); icp.setInputTarget (cloud_in); icp.setMaxCorrespondenceDistance (atof(argv[4])); icp.align (*cloud_icp); icp.setMaximumIterations (1); // We set this variable to 1 for the next time we will call .align () function std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl; if (icp.hasConverged ()) { std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl; std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl; transformation_matrix = icp.getFinalTransformation ().cast(); print4x4Matrix (transformation_matrix); } else { PCL_ERROR ("\nICP has not converged.\n"); return (-1); } // Visualization pcl::visualization::PCLVisualizer viewer ("ICP"); // Create two verticaly separated viewports int v1 (0); int v2 (1); viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1); viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2); // The color we will be using float bckgr_gray_level = 0.0; // Black float txt_gray_lvl = 1.0 - bckgr_gray_level; // Original point cloud is white pcl::visualization::PointCloudColorHandlerCustom cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl); viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1); viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2); // Transformed point cloud is green pcl::visualization::PointCloudColorHandlerCustom cloud_tr_color_h (cloud_tr, 20, 180, 20); viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1); // ICP aligned point cloud is red pcl::visualization::PointCloudColorHandlerCustom cloud_icp_color_h (cloud_icp, 180, 20, 20); viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2); // Adding text descriptions in each viewport viewer.addText ("White: Point cloud: " + std::string(argv[1]) + "\nGreen: Point cloud " + std::string(argv[2]), 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1); viewer.addText ("White: Point cloud: " + std::string(argv[1]) + "\nRed: ICP aligned point cloud of " + std::string(argv[2]), 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2); std::stringstream ss; ss << iterations; std::string iterations_cnt = "ICP iterations = " + ss.str (); viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2); // Set background color viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1); viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2); // Set camera position and orientation viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0); viewer.setSize (1280, 1024); // Visualiser window size // Register keyboard callback : viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL); // Display the visualiser while (!viewer.wasStopped ()) { viewer.spinOnce (); // The user pressed "space" : if (next_iteration) { // The Iterative Closest Point algorithm time.tic (); icp.align (*cloud_icp); std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl; if (icp.hasConverged ()) { printf ("\033[11A"); // Go up 11 lines in terminal output. printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ()); std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl; transformation_matrix *= icp.getFinalTransformation ().cast(); // WARNING /!\ This is not accurate! For "educational" purpose only! print4x4Matrix (transformation_matrix); // Print the transformation between original pose and current pose ss.str (""); ss << iterations; std::string iterations_cnt = "ICP iterations = " + ss.str (); viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt"); viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2"); } else { PCL_ERROR ("\nICP has not converged.\n"); return (-1); } } next_iteration = false; } return (0); }