// This is the main function int main (int argc, char** argv) { // Show help if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) { showHelp (argv[0]); return 0; } // Fetch point cloud filename in arguments | Works with PCD and PLY files std::vector<int> filenames; bool file_is_pcd = false; filenames = pcl::console::parse_file_extension_argument (argc, argv, ".ply"); if (filenames.size () != 1) { filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); if (filenames.size () != 1) { showHelp (argv[0]); return -1; } else { file_is_pcd = true; } } // Load file | Works with PCD and PLY files pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); if (file_is_pcd) { if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) { std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl; showHelp (argv[0]); return -1; } } else { if (pcl::io::loadPLYFile (argv[filenames[0]], *source_cloud) < 0) { std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl; showHelp (argv[0]); return -1; } } /* Reminder: how transformation matrices work : |-------> This column is the translation | 1 0 0 x | \ | 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left | 0 0 1 z | / | 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1) METHOD #1: Using a Matrix4f This is the "manual" method, perfect to understand but error prone ! */ Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix) float theta = M_PI/4; // The angle of rotation in radians transform_1 (0,0) = cos (theta); transform_1 (0,1) = -sin(theta); transform_1 (1,0) = sin (theta); transform_1 (1,1) = cos (theta); // (row, column) // Define a translation of 2.5 meters on the x axis. transform_1 (0,3) = 2.5; // Print the transformation printf ("Method #1: using a Matrix4f\n"); std::cout << transform_1 << std::endl; /* METHOD #2: Using a Affine3f This method is easier and less error prone */ Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity(); // Define a translation of 2.5 meters on the x axis. transform_2.translation() << 2.5, 0.0, 0.0; // The same rotation matrix as before; theta radians arround Z axis transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ())); // Print the transformation printf ("\nMethod #2: using an Affine3f\n"); std::cout << transform_2.matrix() << std::endl; // Executing the transformation pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); // You can either apply transform_1 or transform_2; they are the same pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2); // Visualization printf( "\nPoint cloud colors : white = original point cloud\n" " red = transformed point cloud\n"); pcl::visualization::PCLVisualizer viewer ("Matrix transformation example"); // Define R,G,B colors for the point cloud pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // We add the point cloud to the viewer and pass the color handler viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud"); viewer.addCoordinateSystem (1.0, "cloud", 0); viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud"); //viewer.setPosition(800, 400); // Setting visualiser window position while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed viewer.spinOnce (); } return 0; }
void processCloud(const sensor_msgs::PointCloud2 msg) { //********* Retirive and process raw pointcloud************ std::cout<<"Recieved cloud"<<std::endl; //std::cout<<"Create Octomap"<<std::endl; //octomap::OcTree tree(res); //std::cout<<"Load points "<<std::endl; pcl::PCLPointCloud2 cloud; pcl_conversions::toPCL(msg,cloud); pcl::PointCloud<pcl::PointXYZ> pcl_pc; pcl::fromPCLPointCloud2(cloud,pcl_pc); //std::cout<<"Filter point clouds for NAN"<<std::endl; std::vector<int> nan_indices; pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices); //octomap::Pointcloud oct_pc; //octomap::point3d origin(0.0f,0.0f,0.0f); //std::cout<<"Adding point cloud to octomap"<<std::endl; //octomap::point3d origin(0.0f,0.0f,0.0f); //for(int i = 0;i<pcl_pc.points.size();i++){ //oct_pc.push_back((float) pcl_pc.points[i].x,(float) pcl_pc.points[i].y,(float) pcl_pc.points[i].z); //} //tree.insertPointCloud(oct_pc,origin,-1,false,false); //*********** Remove the oldest data, update the data*************** cloud_seq_loaded.push_back(pcl_pc); std::cout<<cloud_seq_loaded.size()<<std::endl; if(cloud_seq_loaded.size()>2){ cloud_seq_loaded.pop_front(); } //*********** Process currently observerd and buffered data********* if(cloud_seq_loaded.size()==2){ //std::cout<< "Generating octomap"<<std::endl; std::cout<<"Ransac"<<std::endl; pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>); *prev_pc = cloud_seq_loaded.front(); pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>); *curr_pc =pcl_pc; pcl::RandomSample<pcl::PointXYZ> rs(true); rs.setSample(20000); rs.setInputCloud(curr_pc); std::vector<int> indices; rs.filter (indices); pcl::PointCloud<pcl::PointXYZ> curr_cld_out; rs.filter(curr_cld_out); *curr_pc = curr_cld_out; std::cout<<curr_pc->size()<<std::endl; rs.setInputCloud(prev_pc); rs.filter(indices); pcl::PointCloud<pcl::PointXYZ> prev_cld_out; rs.filter(prev_cld_out); *prev_pc = prev_cld_out; /* pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> esTrSVD; pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat; esTrSVD.estimateRigidTransformation(*prev_pc,*curr_pc,trMat); std::cout<<trMat<<std::endl; */ pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>::Ptr trans_lls (new pcl::registration::TransformationEstimationPointToPlaneLLS < pcl::PointXYZ,pcl::PointXYZ,float>); pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >::Ptr trans_LM (new pcl::registration::TransformationEstimationLM< pcl::PointXYZ,pcl::PointXYZ,float >); icp.setInputSource(prev_pc); icp.setInputTarget(curr_pc); icp.setTransformationEstimation (trans_LM); //icp.setMaximumIterations (2); //icp.setRANSACIterations(5); icp.setTransformationEpsilon (1e-8); icp.setMaxCorrespondenceDistance (0.5); pcl::PointCloud<pcl::PointXYZ> Final; icp.align(Final); pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 trMat; trMat = icp.getFinalTransformation(); //*curr_pc = pcl_pc; pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::transformPointCloud (*prev_pc, *transformed_cloud, trMat); pcl::visualization::PCLVisualizer viewer ("Matrix transformation example"); // Define R,G,B colors for the point cloud pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (prev_pc, 255, 255, 255); // We add the point cloud to the viewer and pass the color handler viewer.addPointCloud (prev_pc, source_cloud_color_handler, "original_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud"); viewer.addCoordinateSystem (1.0, 0); viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud"); //viewer.setPosition(800, 400); // Setting visualiser window position while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed viewer.spinOnce (); } //std::cout << "Node center: " << it.getCoordinate() << std::endl; //std::cout << "Node size: " << it.getSize() << std::endl; //std::cout << "Node value: " << it->getValue() << std::endl; } //********** print out the statistics ****************** //**************Process Point cloud in octree data structure ***************** /* //******************Traverse the tree ******************** for(octomap::OcTree::tree_iterator it =tree.begin_tree(), end = tree.end_tree();it!= end;it++){ //manipulate node, e.g.: std::cout << "_____________________________________"<<std::endl; std::cout << "Node center: " << it.getCoordinate() << std::endl; std::cout << "Node size: " << it.getSize() << std::endl; std::cout << "Node depth: "<<it.getDepth() << std::endl; std::cout << "Is Leaf : "<< it.isLeaf()<< std::endl; std::cout << "_____________________________________"<<std::endl; } //********************************************************** */ std::cout<<"finished"<<std::endl; std::cout<<std::endl; }
// Main function int main (int argc, char** argv) { // Show help if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) { showHelp (argv[0]); return 0; } // Fetch point cloud filename in arguments | Works with PCD files std::vector<int> filenames; bool file_is_pcd = false; if (filenames.size () != 2) { filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); if (filenames.size () != 2) { showHelp (argv[0]); return -1; } else { file_is_pcd = true; } } // Load file | Works with PCD and PLY files pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); // The source cloud is the original cloud if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0) { std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl; showHelp (argv[0]); return -1; } // The moved cloud is the original cloud after a transformation pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::io::loadPCDFile (argv[filenames[1]], *moved_cloud); // compute the distance between the source_cloud's centroid and // the moved_cloud's centroid /* Eigen::Vector4f zero(4); zero << 0,0,0,0; unsigned int centroid1 = pcl::compute3DCentroid(*source_cloud, zero); unsigned int centroid2 = pcl::compute3DCentroid(*moved_cloud, zero); float distance = 0; float centroid1d = (float)centroid1; float centroid2d = (float)centroid2; // ints or floats won't do. I need a 3D vector or something // so i can translate the x,y,z points over to the new location. // unfortunately, the only other compute centroid function I can // find returns void. distance = (centroid1d - centroid2d) / 1000.0; cout << centroid1 << std::endl; cout << centroid2 << std::endl; cout << distance << std::endl; */ /* If I'm going to translate the new cloud to the correct centroid position later, I don't need this initial translation. Eigen::Vector4f centroid2 = compute_centroid(*moved_cloud); float distance_x = centroid2(0) - centroid1(0); float distance_y = centroid2(1) - centroid1(1); float distance_z = centroid2(2) - centroid1(2); Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix) // float theta = 45; // The angle of rotation in radians // transform_1 (2,0) = sin (theta); // transform_1 (2,1) = cos (theta); transform_1 (0,3) = abs(distance_x); transform_1 (1,3) = abs(distance_y); transform_1 (2,3) = abs(distance_z); */ // Executing the transformation // pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); // pcl::transformPointCloud (*source_cloud, *transform_cloud, transform_1); // transformed_cloud = kdtree_search(moved_cloud, transform_cloud); /* for (int i = 0; i < 8; i++) { // this transformation isn't giving me an overlay of the moved // cloud at different angle rotations // it's accompanied by a translation that puts the resulting // cloud too far from the moved_cloud, so the kdtree estimate // won't be too good // However, if icp can correct for this, it might be ok // ideally, I would want this to work as well as possible // to speed up icp. // I also am not passing the original centroid-translated // point cloud to this kdtree search. I need to find a way to do that // // The issue here is that I'm not translating the cloud correctly // based on the distance between the centroids. It's more complicated // than just translating the cloud by the distance. // Look at the picture Robbie drew. There's lin alg involved. // passing in 0 for theta doesn't work because cos(0) = 1 // Add 45 degrees so that you don't do cos 0 */ pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); Eigen::Matrix4f best_fit_transform = kdtree_search(source_cloud, moved_cloud, 0.04); pcl::transformPointCloud (*moved_cloud, *transformed_cloud, best_fit_transform); // Visualization printf( "\nPoint cloud colors : white = original point cloud\n" " red = transformed point cloud\n" " blue = moved point cloud\n"); // " green = rotated point cloud\n"); pcl::visualization::PCLVisualizer viewer ("Matrix transformation example"); // Define R,G,B colors for the point cloud pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255); // white // We add the point cloud to the viewer and pass the color handler viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud"); /* pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rotated_cloud_color_handler (rotated_cloud, 20, 245, 20); // green viewer.addPointCloud (rotated_cloud, rotated_cloud_color_handler, "rotated_cloud"); */ pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> moved_cloud_color_handler (moved_cloud, 20, 230, 230); // blue viewer.addPointCloud (moved_cloud, moved_cloud_color_handler, "moved_cloud"); viewer.addCoordinateSystem (1.0, 0); viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud"); // viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "rotated_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "moved_cloud"); //viewer.setPosition(800, 400); // Setting visualiser window position while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed viewer.spinOnce (); } return 0; }