int main (int argc, char* argv[]) { boost::program_options::options_description desc("Options"); desc.add_options() ("help", "Print help message") ("pcd_filename", boost::program_options::value<std::string>()->required(), "pcl pcd filename"); boost::program_options::variables_map vm; try { boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); if( vm.count("help") ){ std::cout << "Resize Point cloud using voxel grid filter" << std::endl; std::cerr << desc << std::endl; return 0; } boost::program_options::notify(vm); } catch (boost::program_options::error& e) { std::cerr << "ERROR: " << e.what() << std::endl << std::endl; std::cerr << desc << std::endl; return -1; } std::string pcd_fname = vm["pcd_filename"].as<std::string>(); pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::io::loadPCDFile (pcd_fname, *source_cloud); // Rotate pointcloud 0.6[rad] around z axis Eigen::Matrix4f rotate_around_zaxis = Eigen::Matrix4f::Identity(); float theta = 0.6; rotate_around_zaxis(0,0) = cos(theta); rotate_around_zaxis(0,1) = -sin(theta); rotate_around_zaxis(1,0) = sin(theta); rotate_around_zaxis(1,1) = cos(theta); pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::transformPointCloud(*source_cloud, *rotated_cloud, rotate_around_zaxis); // Transform point cloud using homogeneous transformation matrix // which is got by icp mathcing between 3D map reference point and manual map reference point Eigen::Matrix4f icp_transform_matrix = Eigen::Matrix4f::Identity(); icp_transform_matrix(0, 0) = 0.950235; icp_transform_matrix(0, 1) = -0.307961; icp_transform_matrix(0, 2) = 0.0470266; icp_transform_matrix(0, 3) = 0.147511; icp_transform_matrix(1, 0) = 0.30832; icp_transform_matrix(1, 1) = 0.951281; icp_transform_matrix(1, 2) = -0.000411891; icp_transform_matrix(1, 3) = -0.0305933; icp_transform_matrix(2, 0) = -0.0446088; icp_transform_matrix(2, 1) = 0.0148907; icp_transform_matrix(2, 2) = 0.998893; icp_transform_matrix(2, 3) = -0.0312964; pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::transformPointCloud(*rotated_cloud, *transformed_cloud, icp_transform_matrix); pcl::io::savePCDFileASCII("transformed_cloud.pcd", *transformed_cloud); std::cout << "Write to transformed_cloud.pcd" << std::endl; return 0; }
void FloorFilter::CloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud) { pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud( new pcl::PointCloud<pcl::PointXYZ>); if (!WaitForTransformToReferenceFrame(cloud->header.frame_id, cloud->header.stamp)) { ROS_WARN("Cannot transform pointcloud to reference frame (%s -> %s).", cloud->header.frame_id.c_str(), reference_frame_.c_str()); return; } try { pcl_ros::transformPointCloud(reference_frame_, *cloud, *transformed_cloud, tf_listener_); } catch (tf::TransformException e) { // Transformation fails in particular at start up because tilting // laser transforms might not be coming in yet. This is logged by // TF already, so we don't add another logging here. return; } if(!transformed_cloud->points.size()) { ROS_WARN("The input cloud is empty. No obstacles in range?"); return; } std::vector<int> floor_candidate_indices; FilterFloorCandidates(floor_z_distance_, tan(max_floor_y_rotation_), *transformed_cloud, &floor_candidate_indices); Eigen::ParametrizedLine<float, 3> floor_line; std::vector<int> line_inlier_indices; std::vector<int> indices_without_floor; pcl::PointCloud<pcl::PointXYZ>::Ptr cliff_cloud(new pcl::PointCloud<pcl::PointXYZ>); std::vector<int> cliff_indices; if (GetFloorLine(transformed_cloud, floor_candidate_indices, &floor_line, &line_inlier_indices)) { GetIndicesDifference(transformed_cloud->size(), line_inlier_indices, &indices_without_floor); GenerateCliffCloud(floor_line, *transformed_cloud, indices_without_floor, cliff_cloud.get(), &cliff_indices); } else { cliff_cloud->header = transformed_cloud->header; } // Always publish clouds even if they are empty to signal that // perception is still alive. PublishCloudFromIndices(*transformed_cloud, line_inlier_indices, floor_cloud_publisher_); PublishCloudFromIndices(*transformed_cloud, indices_without_floor, filtered_cloud_publisher_); PublishCloudFromIndices(*transformed_cloud, cliff_indices, cliff_generating_cloud_publisher_); cliff_cloud_publisher_.publish(cliff_cloud); ros::Duration message_age = ros::Time::now() - cloud->header.stamp; // This is just a hint. Throw a warning to make the user know // about something being fishy with the current configuration // because input data is pretty old. if (message_age > ros::Duration(1.0)) { ROS_WARN("Filtered cloud already %lf seconds old.", message_age.toSec()); } }
void addToGlobalCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in) { /* // Initialize global cloud if not already done so if (!globalCloudPtr){ globalCloudPtr = cloud_in; return; } */ pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ()); transformCam2Robot (*cloud_in, *transformed_cloud); // Initialize global cloud if not already done so if (!globalCloudPtr) { globalCloudPtr = transformed_cloud; return; } /* // Preform ICP pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp; icp.setInputCloud(transformed_cloud); icp.setInputTarget(globalCloudPtr); pcl::PointCloud<pcl::PointXYZRGB> Final; icp.align(Final); std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; std::cout << icp.getFinalTransformation() << std::endl; *globalCloudPtr += Final; */ //globalCloudPtr = transformed_cloud; //return; *globalCloudPtr += *transformed_cloud; // Perform voxelgrid filtering pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::VoxelGrid<pcl::PointXYZRGB> sor; sor.setInputCloud (globalCloudPtr); sor.setLeafSize (res, res, res); sor.filter (*cloud_filtered); globalCloudPtr = cloud_filtered; }
// generate colorized point cloud from point cloud and image data void LaserCamHyperopt::update() { ROS_DEBUG("Fusing image and point cloud!!"); // set up transformation and use it to transform the point cloud Eigen::Affine3d transform_plus_deltas; Eigen::Vector3d translation(m_translate_laser_to_cam.x() + m_delta_x, m_translate_laser_to_cam.y() + m_delta_y, m_translate_laser_to_cam.z() + m_delta_z); Eigen::Matrix3d rotation(m_rotate_laser_to_cam * Eigen::AngleAxisd(m_delta_yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(m_delta_pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(m_delta_roll, Eigen::Vector3d::UnitX())); transform_plus_deltas = Eigen::Translation3d(translation) * rotation; PointCloudIntensity::Ptr transformed_cloud(new PointCloudIntensity); ROS_DEBUG_STREAM("applying transformation to cloud"); pcl::transformPointCloud(*m_cloud, *transformed_cloud, transform_plus_deltas); // save points that are visible for the camera PointCloudIntensity::Ptr visible_cloud(new PointCloudIntensity); PointCloudRGB::Ptr colorized_cloud(new PointCloudRGB); ROS_DEBUG_STREAM("colorizing cloud"); if(m_camera_model == "pinhole") { colorizeCloudPinhole(transformed_cloud, colorized_cloud, visible_cloud); } else if(m_camera_model == "omni") { colorizeCloudOmni(transformed_cloud, colorized_cloud, visible_cloud); } ROS_DEBUG_STREAM("computing mutual information"); //float mutual_information = mutual_information::getMutualInformation(visible_cloud, colorized_cloud); m_mutual_information += mutual_information::getExtendedMutualInformation(visible_cloud, colorized_cloud); }
void VoxelGridDownsampleManager::pointCB(const sensor_msgs::PointCloud2ConstPtr &input) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>); if (grid_.size() == 0) { ROS_INFO("the number of registered grids is 0, skipping"); return; } fromROSMsg(*input, *cloud); for (size_t i = 0; i < grid_.size(); i++) { visualization_msgs::Marker::ConstPtr target_grid = grid_[i]; // not yet tf is supported int id = target_grid->id; // solve tf with ros::Time 0.0 // transform pointcloud to the frame_id of target_grid pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl_ros::transformPointCloud(target_grid->header.frame_id, *cloud, *transformed_cloud, tf_listener); double center_x = target_grid->pose.position.x; double center_y = target_grid->pose.position.y; double center_z = target_grid->pose.position.z; double range_x = target_grid->scale.x * 1.0; // for debug double range_y = target_grid->scale.y * 1.0; double range_z = target_grid->scale.z * 1.0; double min_x = center_x - range_x / 2.0; double max_x = center_x + range_x / 2.0; double min_y = center_y - range_y / 2.0; double max_y = center_y + range_y / 2.0; double min_z = center_z - range_z / 2.0; double max_z = center_z + range_z / 2.0; double resolution = target_grid->color.r; // filter order: x -> y -> z -> downsample pcl::PassThrough<pcl::PointXYZ> pass_x; pass_x.setFilterFieldName("x"); pass_x.setFilterLimits(min_x, max_x); pcl::PassThrough<pcl::PointXYZ> pass_y; pass_y.setFilterFieldName("y"); pass_y.setFilterLimits(min_y, max_y); pcl::PassThrough<pcl::PointXYZ> pass_z; pass_z.setFilterFieldName("z"); pass_z.setFilterLimits(min_z, max_z); ROS_INFO_STREAM(id << " filter x: " << min_x << " - " << max_x); ROS_INFO_STREAM(id << " filter y: " << min_y << " - " << max_y); ROS_INFO_STREAM(id << " filter z: " << min_z << " - " << max_z); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_x (new pcl::PointCloud<pcl::PointXYZ>); pass_x.setInputCloud (transformed_cloud); pass_x.filter(*cloud_after_x); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_y (new pcl::PointCloud<pcl::PointXYZ>); pass_y.setInputCloud (cloud_after_x); pass_y.filter(*cloud_after_y); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_z (new pcl::PointCloud<pcl::PointXYZ>); pass_z.setInputCloud (cloud_after_y); pass_z.filter(*cloud_after_z); // downsample pcl::VoxelGrid<pcl::PointXYZ> sor; sor.setInputCloud (cloud_after_z); sor.setLeafSize (resolution, resolution, resolution); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); sor.filter (*cloud_filtered); // reverse transform pcl::PointCloud<pcl::PointXYZ>::Ptr reverse_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl_ros::transformPointCloud(input->header.frame_id, *cloud_filtered, *reverse_transformed_cloud, tf_listener); // adding the output into *output_cloud // tmp <- cloud_filtered + output_cloud // output_cloud <- tmp //pcl::PointCloud<pcl::PointXYZ>::Ptr tmp (new pcl::PointCloud<pcl::PointXYZ>); //pcl::concatenatePointCloud (*cloud_filtered, *output_cloud, tmp); //output_cloud = tmp; ROS_INFO_STREAM(id << " includes " << reverse_transformed_cloud->points.size() << " points"); for (size_t i = 0; i < reverse_transformed_cloud->points.size(); i++) { output_cloud->points.push_back(reverse_transformed_cloud->points[i]); } } sensor_msgs::PointCloud2 out; toROSMsg(*output_cloud, out); out.header = input->header; pub_.publish(out); // for debugging // for concatenater size_t cluster_num = output_cloud->points.size() / max_points_ + 1; ROS_INFO_STREAM("encoding into " << cluster_num << " clusters"); for (size_t i = 0; i < cluster_num; i++) { size_t start_index = max_points_ * i; size_t end_index = max_points_ * (i + 1) > output_cloud->points.size() ? output_cloud->points.size(): max_points_ * (i + 1); sensor_msgs::PointCloud2 cluster_out_ros; pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_out_pcl(new pcl::PointCloud<pcl::PointXYZ>); cluster_out_pcl->points.resize(end_index - start_index); // build cluster_out_pcl ROS_INFO_STREAM("make cluster from " << start_index << " to " << end_index); for (size_t j = start_index; j < end_index; j++) { cluster_out_pcl->points[j - start_index] = output_cloud->points[j]; } // conevrt cluster_out_pcl into ros msg toROSMsg(*cluster_out_pcl, cluster_out_ros); cluster_out_ros.header = input->header; cluster_out_ros.header.frame_id = (boost::format("%s %d %d") % (cluster_out_ros.header.frame_id) % (i) % (sequence_id_)).str(); pub_encoded_.publish(cluster_out_ros); ros::Duration(1.0 / rate_).sleep(); } }
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; }
template<typename PointInT> int pcl::TextureMapping<PointInT>::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh, const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size, PointCloud &visible_pts) { if (tex_mesh.tex_polygons.size () != 1) { PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n"); return (-1); } if (cameras.size () == 0) { PCL_ERROR ("Must provide at least one camera info!\n"); return (-1); } // copy mesh sorted_mesh = tex_mesh; // clear polygons from cleaned_mesh sorted_mesh.tex_polygons.clear (); pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>); // load points into a PCL format pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud); // for each camera for (size_t cam = 0; cam < cameras.size (); ++cam) { // get camera pose as transform Eigen::Affine3f cam_trans = cameras[cam].pose; // transform original cloud in camera coordinates pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ()); // find occlusions on transformed cloud std::vector<int> visible, occluded; removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded); visible_pts = *filtered_cloud; // find visible faces => add them to polygon N for camera N // add polygon group for current camera in clean std::vector<pcl::Vertices> visibleFaces_currentCam; // iterate over the faces of the current mesh for (size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces) { // check if all the face's points are visible bool faceIsVisible = true; std::vector<int>::iterator it; // iterate over face's vertex for (size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice) { // TODO this is far too long! Better create an helper function that raycasts here. it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]); if (it == occluded.end ()) { // point is not occluded // does it land on the camera's image plane? pcl::PointXYZ pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]]; Eigen::Vector2f dummy_UV; if (!getPointUVCoordinates (pt, cameras[cam], dummy_UV)) { // point is not visible by the camera faceIsVisible = false; } } else { faceIsVisible = false; } } if (faceIsVisible) { // push current visible face into the sorted mesh visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]); // remove it from the unsorted mesh tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces); faces--; } } sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam); } // we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0] // we need to add them as an extra polygon in the sorted mesh sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]); return (0); }
// 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 callback(const sensor_msgs::PointCloud2ConstPtr& cloud) { ros::Time whole_start = ros::Time::now(); ros::Time declare_types_start = ros::Time::now(); // filter pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid; pcl::PassThrough<sensor_msgs::PointCloud2> pass; pcl::ExtractIndices<pcl::PointXYZ> extract_indices; pcl::ExtractIndices<pcl::Normal> extract_normals; // Normal estimation pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation; pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals; // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ()); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ()); // The plane and sphere coefficients pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ()); pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ()); pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ()); // The plane and sphere inliers pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ()); pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ()); pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ()); // The point clouds sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2); // The PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>); // The cloud normals pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); // for sphere ros::Time declare_types_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Voxel grid Filtering */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // // Create VoxelGrid filtering // voxel_grid.setInputCloud (cloud); // voxel_grid.setLeafSize (0.01, 0.01, 0.01); // voxel_grid.filter (*voxelgrid_filtered); // // // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Passthrough Filtering */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // pass through filter // pass.setInputCloud (cloud); // pass.setFilterFieldName ("z"); // pass.setFilterLimits (0, 1.5); // pass.filter (*cloud_filtered); // // // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*cloud_filtered, *transformed_cloud); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Estimate point normals */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ros::Time estimate_start = ros::Time::now(); // // // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*cloud, *transformed_cloud); // // // Estimate point normals // normal_estimation.setSearchMethod (tree); // normal_estimation.setInputCloud (transformed_cloud); // normal_estimation.setKSearch (50); // normal_estimation.compute (*cloud_normals); // // ros::Time estimate_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Create and processing the plane extraction */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ros::Time plane_start = ros::Time::now(); // // // Create the segmentation object for the planar model and set all the parameters // segmentation_from_normals.setOptimizeCoefficients (true); // segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE); // segmentation_from_normals.setNormalDistanceWeight (0.1); // segmentation_from_normals.setMethodType (pcl::SAC_RANSAC); // segmentation_from_normals.setMaxIterations (100); // segmentation_from_normals.setDistanceThreshold (0.03); // segmentation_from_normals.setInputCloud (transformed_cloud); // segmentation_from_normals.setInputNormals (cloud_normals); // // // Obtain the plane inliers and coefficients // segmentation_from_normals.segment (*inliers_plane, *coefficients_plane); // //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl; // // // Extract the planar inliers from the input cloud // extract_indices.setInputCloud (transformed_cloud); // extract_indices.setIndices (inliers_plane); // extract_indices.setNegative (false); // extract_indices.filter (*cloud_plane); // // pcl::toROSMsg (*cloud_plane, *plane_output_cloud); // plane_pub.publish(plane_output_cloud); // // ros::Time plane_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time plane_start = ros::Time::now(); pcl::fromROSMsg (*cloud, *transformed_cloud); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); seg.setInputCloud (transformed_cloud); seg.segment (*inliers_plane, *coefficients_plane); extract_indices.setInputCloud(transformed_cloud); extract_indices.setIndices(inliers_plane); extract_indices.setNegative(false); extract_indices.filter(*cloud_plane); pcl::toROSMsg (*cloud_plane, *plane_output_cloud); plane_pub.publish(plane_output_cloud); ros::Time plane_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Extract rest plane and passthrough filtering */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time rest_pass_start = ros::Time::now(); // Create the filtering object // Remove the planar inliers, extract the rest extract_indices.setNegative (true); extract_indices.filter (*remove_transformed_cloud); transformed_cloud.swap (remove_transformed_cloud); // publish result of Removal the planar inliers, extract the rest pcl::toROSMsg (*transformed_cloud, *rest_output_cloud); rest_pub.publish(rest_output_cloud); // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud // pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud); // pass through filter pass.setInputCloud (rest_output_cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (0, 2.5); pass.filter (*rest_cloud_filtered); ros::Time rest_pass_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * for cylinder features */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud); // Estimate point normals normal_estimation.setSearchMethod (tree2); normal_estimation.setInputCloud (cylinder_cloud); normal_estimation.setKSearch (50); normal_estimation.compute (*cloud_normals2); // Create the segmentation object for sphere segmentation and set all the paopennirameters segmentation_from_normals.setOptimizeCoefficients (true); segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER); segmentation_from_normals.setMethodType (pcl::SAC_RANSAC); segmentation_from_normals.setNormalDistanceWeight (0.1); segmentation_from_normals.setMaxIterations (10000); segmentation_from_normals.setDistanceThreshold (0.05); segmentation_from_normals.setRadiusLimits (0, 0.5); segmentation_from_normals.setInputCloud (cylinder_cloud); segmentation_from_normals.setInputNormals (cloud_normals2); // Obtain the sphere inliers and coefficients segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder); //std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl; // Publish the sphere cloud extract_indices.setInputCloud (cylinder_cloud); extract_indices.setIndices (inliers_cylinder); extract_indices.setNegative (false); extract_indices.filter (*cylinder_output); if (cylinder_output->points.empty ()) std::cerr << "Can't find the cylindrical component." << std::endl; pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud); cylinder_pub.publish(cylinder_output_cloud); */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * for sphere features */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time sphere_start = ros::Time::now(); // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud); // Estimate point normals normal_estimation.setSearchMethod (tree3); normal_estimation.setInputCloud (sphere_cloud); normal_estimation.setKSearch (50); normal_estimation.compute (*cloud_normals3); // Create the segmentation object for sphere segmentation and set all the paopennirameters segmentation_from_normals.setOptimizeCoefficients (true); //segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE); segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE); segmentation_from_normals.setMethodType (pcl::SAC_RANSAC); segmentation_from_normals.setNormalDistanceWeight (0.1); segmentation_from_normals.setMaxIterations (10000); segmentation_from_normals.setDistanceThreshold (0.05); segmentation_from_normals.setRadiusLimits (0, 0.2); segmentation_from_normals.setInputCloud (sphere_cloud); segmentation_from_normals.setInputNormals (cloud_normals3); // Obtain the sphere inliers and coefficients segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere); //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl; // Publish the sphere cloud extract_indices.setInputCloud (sphere_cloud); extract_indices.setIndices (inliers_sphere); extract_indices.setNegative (false); extract_indices.filter (*sphere_output); if (sphere_output->points.empty ()) std::cerr << "Can't find the sphere component." << std::endl; pcl::toROSMsg (*sphere_output, *sphere_output_cloud); sphere_pub.publish(sphere_output_cloud); ros::Time sphere_end = ros::Time::now(); std::cout << "cloud size : " << cloud->width * cloud->height << std::endl; std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl; //std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl; //std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl; std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl; ros::Time whole_now = ros::Time::now(); printf("\n"); std::cout << "whole time : " << whole_now - whole_start << " sec" << std::endl; std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl; //std::cout << "estimate time : " << estimate_end - estimate_start << " sec" << std::endl; std::cout << "plane time : " << plane_end - plane_start << " sec" << std::endl; std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl; std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl; printf("\n"); }
// I need to change the parameters to account for the // transformation matrices. I need to save the best TM // so I can combine it with the icp TM. // // Does kdtree search on each rotation of 45 degrees around // the central point of the moved_cloud Eigen::Matrix4f kdtree_search(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr moved_cloud, float radius) { pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; // Total number of nearest neighbors int num_matches = 0; // Return the cloud with the most number of nearest neighbors // within a given radius of the searchPoint int max_neighbors = 0; pcl::PointCloud<pcl::PointXYZ>::Ptr best_fit_cloud ; Eigen::Matrix4f best_fit_transform = Eigen::Matrix4f::Identity(); Eigen::Vector4f centroid1 = compute_centroid(*moved_cloud); cout << "centroid of source cloud - " << centroid1(0) << ", " << centroid1(1) << ", " << centroid1(2) << endl; // Executing the transformation pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); Eigen::Matrix4f transform_2 = Eigen::Matrix4f::Identity(); for (int i = 0; i < 8; i++) { float theta = 45 * i + 45; transform_1 (0,0) = cos (theta); transform_1 (0,2) = -sin (theta); transform_1 (2,0) = sin (theta); transform_1 (2,2) = cos (theta); cout << "cloud with " << theta << " degrees of rotation" << endl; pcl::transformPointCloud (*moved_cloud, *rotated_cloud, transform_1); // Probably need to compute centroid of the new transformed cloud // because the transformation seems to translate it Eigen::Vector4f centroid2 = compute_centroid(*rotated_cloud); cout << "centroid of rotated cloud - " << centroid2(0) << ", " << centroid2(1) << ", " << centroid2(2) << endl; float distance_x = centroid1(0) - centroid2(0); float distance_y = centroid1(1) - centroid2(1); float distance_z = centroid1(2) - centroid2(2); cout << "distance between centroids: (" << distance_x << ", " << distance_y << ", " << distance_z << ")" << endl; transform_2 (0,3) = (distance_x); transform_2 (1,3) = (distance_y); transform_2 (2,3) = (distance_z); pcl::transformPointCloud (*rotated_cloud, *transformed_cloud, transform_2); // Rotate the cloud by 45 degrees each time // May want to add some random translation as well. // This would correspond to doing kdtree search on a number of // clouds that are presumably close to the target point cloud. kdtree.setInputCloud(transformed_cloud); // Run the kdtree search on every 10th point of the moved_cloud // Increase this number to speed up the search // Test with different increments of i to see effect on speed for (int j = 0; j < (*moved_cloud).points.size(); j += 10) { pcl::PointXYZ searchPoint = moved_cloud->points[j]; int num_neighbors = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance); num_matches += num_neighbors; cout << "performed kdtree nearest neighbor search, found " << num_neighbors << " within " << radius << " radius" << endl; } cout << "num_matches = " << num_matches << endl; cout << "max_neighbors = " << max_neighbors << endl; if (num_matches > max_neighbors) { max_neighbors = num_matches; best_fit_cloud = transformed_cloud; // are these transforms relative? or absolute? // this currently calculates relative // should be transform_2 * transform_1 if absolute best_fit_transform = transform_2 * transform_1; } num_matches = 0; 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 (); } } // check whether the translations are relative or absolute pcl::PointCloud<pcl::PointXYZ>::Ptr best_transform_cloud (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::transformPointCloud (*moved_cloud, *best_transform_cloud, best_fit_transform); pcl::visualization::PCLVisualizer viewer ("Matrix transformation example"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_transform_cloud_color_handler (best_transform_cloud, 245, 20, 20); // red viewer.addPointCloud (best_transform_cloud, best_transform_cloud_color_handler, "best_transform_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> best_fit_cloud_color_handler (best_fit_cloud, 20, 245, 20); // green viewer.addPointCloud (best_transform_cloud, best_fit_cloud_color_handler, "best_fit_cloud"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_cloud_color_handler (input_cloud, 255, 255, 255); // white viewer.addPointCloud (input_cloud, input_cloud_color_handler, "input_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, "best_transform_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "best_fit_cloud"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input_cloud"); while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed viewer.spinOnce (); } return best_fit_transform; }
// 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; }
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, const Eigen::Matrix4f& transform) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::transformPointCloud(*cloud, *transformed_cloud, transform); return transformed_cloud; }