int main(int argc, char* argv[]) { // std::cout << "hello" << std::endl; // pcl::TextureMesh mesh1; // pcl::io::loadPolygonFileOBJ (argv[1], mesh1); // pcl::TextureMesh::Ptr mesh2(new pcl::TextureMesh); // std::cout << "hello" << std::endl; // pcl::io::loadOBJFile (argv[1], *mesh2); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr model(new pcl::PointCloud<pcl::PointXYZRGBA>); std::string filename = argv[1]; vtkSmartPointer<vtkPLYReader> reader = vtkSmartPointer<vtkPLYReader>::New(); vtkSmartPointer<vtkPolyData> data = vtkSmartPointer<vtkPolyData>::New(); reader->SetFileName (filename.c_str()); reader->Update(); data=reader->GetOutput(); std::cerr << "model loaded" << std::endl; pcl::io::vtkPolyDataToPointCloud(data, *model); pcl::visualization::PCLVisualizer visu("Test"); visu.addPointCloud (model, "texture"); visu.spin (); }
void compute_normals(PointCloudT::Ptr cloud) { // Estimate normals for scene pcl::console::print_highlight ("Estimating scene normals...\n"); pcl::NormalEstimationOMP<PointNT,PointNT> nest; nest.setRadiusSearch (normal_radius); nest.setInputCloud (cloud); nest.compute (*cloud); pcl::visualization::PCLVisualizer visu("Alignment"); visu.addPointCloudNormals<PointNT>(cloud, 1); visu.spin(); }
void CDataList::Show(double minV, double maxV) { CImg<unsigned char> visu(1000,600, 1,3,0); Draw(visu, minV, maxV); CImgDisplay draw_disp(visu, "DTB Scope"); while (!draw_disp.is_closed() && !draw_disp.is_keyESC()) { draw_disp.wait(); if (draw_disp.is_resized()) { draw_disp.resize(false); visu.resize(draw_disp); Draw(visu, minV, maxV); visu.display(draw_disp); } } }
void shot_detector::ransac(std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >& transforms, pcl::PointCloud<PointType>::Ptr model, pcl::PointCloud<PointType>::Ptr scene) { // Note: here you would compute or load the descriptors for both // the scene and the model. It has been omitted here for simplicity. std::cerr << "STart ransac" << std::endl; pcl::PointCloud<PointType>::Ptr alignedModel(new pcl::PointCloud<PointType>); // Object for pose estimation. pcl::SampleConsensusPrerejective<PointType, PointType, DescriptorType> pose; pose.setInputSource(model); pose.setInputTarget(scene); pose.setSourceFeatures(model_descriptors); pose.setTargetFeatures(scene_descriptors); // Instead of matching a descriptor with its nearest neighbor, choose randomly between // the N closest ones, making it more robust to outliers, but increasing time. pose.setCorrespondenceRandomness(ran_corr_random_); // Set the fraction (0-1) of inlier points required for accepting a transformation. // At least this number of points will need to be aligned to accept a pose. pose.setInlierFraction(ran_inlier_dist_); // Set the number of samples to use during each iteration (minimum for 6 DoF is 3). pose.setNumberOfSamples(ran_sample_num_); // Set the similarity threshold (0-1) between edge lengths of the polygons. The // closer to 1, the more strict the rejector will be, probably discarding acceptable poses. pose.setSimilarityThreshold(ran_sim_thresh_); // Set the maximum distance threshold between two correspondent points in source and target. // If the distance is larger, the points will be ignored in the alignment process. pose.setMaxCorrespondenceDistance(ran_max_corr_dist_); //pose.setMaximumIterations (ran_max_iter_); pose.align(*alignedModel); std::cerr << "ransac ran" << std::endl; if (pose.hasConverged()) { Eigen::Matrix4f transformation = pose.getFinalTransformation(); transforms.push_back(transformation); Eigen::Matrix3f rotation = transformation.block<3, 3>(0, 0); Eigen::Vector3f translation = transformation.block<3, 1>(0, 3); std::cerr << "Transformation matrix:" << transformation << std::endl << std::endl; pcl::visualization::PCLVisualizer visu("Alignment"); visu.addPointCloud (scene, "scene"); visu.addPointCloud (alignedModel, "object_aligned"); visu.spin (); } else std::cerr << "Did not converge." << std::endl; }
void drawCities() { fillConstants(); const unsigned char black[] = {0,0,0}; const unsigned char red[] = { 255,0,0 }; //const unsigned char green[] = { 0,255,0 }; //const unsigned char blue[] = { 0,0,255 }; CImg<unsigned char> visu(COLUMNS,ROWS,1,3,255); CImgDisplay draw_disp(visu,"Cities"); //draw cities for(int i=0;i<nodes;++i) { visu.draw_circle(xtoc(x[i]),ytor(y[i]),CITYRADIUS,black,1,1); } //draw optimal tour int currNode,nextNode; for(int i=0;i<nodes;++i) { currNode = optTour[i]; nextNode = optTour[(i+1)%nodes]; visu.draw_line( xtoc(x[currNode]),ytor(y[currNode]), xtoc(x[nextNode]),ytor(y[nextNode]), red,1,~0U); } visu.display(draw_disp); while ( !draw_disp.is_closed() ) { draw_disp.wait(); } //visu.draw_graph(image.get_crop(0,y,0,2,image.width()-1,y,0,2),blue,1,0,256,0).display(draw_disp); }
int main (int argc, char **argv) { PointCloudxyz::Ptr object (new PointCloudxyz); PointCloudxyz::Ptr scene (new PointCloudxyz); PointCloudxyz::Ptr object_aligned (new PointCloudxyz); // Get input object and scene if (argc != 2) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); pcl_tools::cloud_from_ply(argv[1], *object); std::vector<int> indices; pcl::removeNaNFromPointCloud(*object, *object, indices); int index = closest_point_line(*object, Eigen::Vector3f(0.57735054, 0.57735054, 0.57735054), Eigen::Vector3f(0.0, 0.0, 0.0)); // int index = closest_point_line(*object, Eigen::Vector3f::UnitY(), Eigen::Vector3f(0.0, 0.0, 0.0)); pcl::PointXYZ centerpt = object->points[index]; pcl::visualization::PCLVisualizer visu("LineFind"); visu.addPointCloud(object, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(object, 0.0, 0.0, 255.0), "object"); visu.addSphere(centerpt, 0.005, "location"); // visu.addLine<Eigen::Vector4f, Eigen::Vector4f>(Eigen::Vector4f(0.0, 0.0, 0.0, 1.0), Eigen::Vector4f(0.0, 0.0, 0.0, 1.0) + Eigen::Vector4f::UnitX(), "line"); visu.addCoordinateSystem (0.04, "View_1"); // visu.addPointCloudNormals<PointNT>(object); visu.spin (); }
// Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); FeatureCloudT::Ptr object_features (new FeatureCloudT); FeatureCloudT::Ptr scene_features (new FeatureCloudT); // Get input object and scene if (argc != 3) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 || pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0) { pcl::console::print_error ("Error loading object/scene file!\n"); return (1); } // Downsample pcl::console::print_highlight ("Downsampling...\n"); pcl::VoxelGrid<PointNT> grid; const float leaf = 0.005f; grid.setLeafSize (leaf, leaf, leaf); grid.setInputCloud (object); grid.filter (*object); grid.setInputCloud (scene); grid.filter (*scene); // Estimate normals for scene pcl::console::print_highlight ("Estimating scene normals...\n"); pcl::NormalEstimationOMP<PointNT,PointNT> nest; nest.setRadiusSearch (0.01); nest.setInputCloud (scene); nest.compute (*scene); // Estimate features pcl::console::print_highlight ("Estimating features...\n"); FeatureEstimationT fest; fest.setRadiusSearch (0.025); fest.setInputCloud (object); fest.setInputNormals (object); fest.compute (*object_features); fest.setInputCloud (scene); fest.setInputNormals (scene); fest.compute (*scene_features); // Perform alignment pcl::console::print_highlight ("Starting alignment...\n"); pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align; align.setInputSource (object); align.setSourceFeatures (object_features); align.setInputTarget (scene); align.setTargetFeatures (scene_features); align.setMaximumIterations (100000); // Number of RANSAC iterations align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose align.setCorrespondenceRandomness (5); // Number of nearest features to use align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis { pcl::ScopeTime t("Alignment"); align.align (*object_aligned); } if (align.hasConverged ()) { // Print results printf ("\n"); Eigen::Matrix4f transformation = align.getFinalTransformation (); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2)); pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2)); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2)); pcl::console::print_info ("\n"); pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3)); pcl::console::print_info ("\n"); pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ()); // Show alignment pcl::visualization::PCLVisualizer visu("Alignment"); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned"); visu.spin (); } else { pcl::console::print_error ("Alignment failed!\n"); return (1); } return (0); }
void drawConvexHull() { fillConstants(); const unsigned char black[] = {0,0,0}; const unsigned char red[] = { 255,0,0 }; const unsigned char green[] = { 0,255,0 }; //const unsigned char blue[] = { 0,0,255 }; CImg<unsigned char> visu(COLUMNS,ROWS,1,3,255); CImgDisplay draw_disp(visu,"Cities"); //draw cities for(int i=0;i<nodes;++i) { visu.draw_circle(xtoc(x[i]),ytor(y[i]),CITYRADIUS,black,1,1); } drawCity(45,&visu,green); drawCity(35,&visu,green); drawCity(48,&visu,green); drawCity(63,&visu,green); //draw convex hull cout << "Checkpoint 1" << "\n"; convexHullAlgorithm(); cout << "Checkpoint 2" << "\n"; displayCurrentHull(&visu,red); /* int i=0; while(mask[i]<0) { ++i; } cout << "Checkpoint 3: " << i << "\n"; int firstNode = i; int currNode = i; int nextNode = mask[currNode]; cout << "Current Node: " << currNode << "\n"; cout << "Next Node: " << nextNode << "\n"; while(nextNode!=firstNode && nextNode>=0) { visu.draw_line( xtoc(x[currNode]),ytor(y[currNode]), xtoc(x[nextNode]),ytor(y[nextNode]), red,1,~0U); currNode = nextNode; nextNode = mask[nextNode]; } */ visu.display(draw_disp); while ( !draw_disp.is_closed() ) { draw_disp.wait(); } }
/** \brief Returns closest poses of of objects in training data to the query object \param -q the path to the input point cloud \param -k the number of nearest neighbors to return */ int main (int argc, char **argv) { //parse data directory std::string queryCloudName; pcl::console::parse_argument (argc, argv, "-q", queryCloudName); boost::filesystem::path queryCloudPath(queryCloudName); //parse number of nearest neighbors k int k = 1; pcl::console::parse_argument (argc, argv, "-k", k); pcl::console::print_highlight ("using %d nearest neighbors.\n", k); //read in point cloud PointCloud::Ptr cloud (new PointCloud); pcl::PCDReader reader; //read ply file pcl::PolygonMesh triangles; if(queryCloudPath.extension().native().compare(".ply") == 0) { if( pcl::io::loadPolygonFilePLY(queryCloudPath.native(), triangles) == -1) { PCL_ERROR("Could not read .ply file\n"); return -1; } #if PCL17 pcl::fromPCLPointCloud2(triangles.cloud, *cloud); #endif #if PCL16 pcl::fromROSMsg(triangles.cloud, *cloud); #endif } //read pcd file else if(queryCloudPath.extension().native().compare(".pcd") == 0) { if( reader.read(queryCloudPath.native(), *cloud) == -1) { PCL_ERROR("Could not read .pcd file\n"); return -1; } } else { PCL_ERROR("File must have extension .ply or .pcd\n"); return -1; } //Move point cloud so it is is centered at the origin Eigen::Matrix<float,4,1> centroid; pcl::compute3DCentroid(*cloud, centroid); pcl::demeanPointCloud(*cloud, centroid, *cloud); //Estimate normals Normals::Ptr normals (new Normals); pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst; normEst.setInputCloud(cloud); pcl::search::KdTree<pcl::PointXYZ>::Ptr normTree (new pcl::search::KdTree<pcl::PointXYZ>); normEst.setSearchMethod(normTree); normEst.setRadiusSearch(0.005); normEst.compute(*normals); //Create VFH estimation class pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh; vfh.setInputCloud(cloud); vfh.setInputNormals(normals); pcl::search::KdTree<pcl::PointXYZ>::Ptr vfhsTree (new pcl::search::KdTree<pcl::PointXYZ>); vfh.setSearchMethod(vfhsTree); //calculate VFHS features pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308>); vfh.setViewPoint(1, 0, 0); vfh.compute(*vfhs); //filenames std::string featuresFileName = "training_features.h5"; std::string anglesFileName = "training_angles.list"; std::string kdtreeIdxFileName = "training_kdtree.idx"; //allocate flann matrices std::vector<CloudInfo> cloudInfoList; std::vector<PointCloud::Ptr> cloudList; cloudList.resize(k); flann::Matrix<int> k_indices; flann::Matrix<float> k_distances; flann::Matrix<float> data; //load training data angles list loadAngleData(cloudInfoList, anglesFileName); flann::load_from_file (data, featuresFileName, "training_data"); flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("training_kdtree.idx")); //perform knn search index.buildIndex (); nearestKSearch (index, vfhs, k, k_indices, k_distances); // Output the results on screen pcl::console::print_highlight ("The closest %d neighbors are:\n", k); for (int i = 0; i < k; ++i) { //print nearest neighbor info to screen pcl::console::print_info (" %d - theta = %f, phi = %f (%s) with a distance of: %f\n", i, cloudInfoList.at(k_indices[0][i]).theta*180.0/M_PI, cloudInfoList.at(k_indices[0][i]).phi*180.0/M_PI, cloudInfoList.at(k_indices[0][i]).filePath.c_str(), k_distances[0][i]); //retrieve pointcloud and store in list PointCloud::Ptr cloudMatch (new PointCloud); reader.read(cloudInfoList.at(k_indices[0][i]).filePath.native(), *cloudMatch); //Move point cloud so it is is centered at the origin pcl::compute3DCentroid(*cloudMatch, centroid); pcl::demeanPointCloud(*cloudMatch, centroid, *cloudMatch); cloudList[i] = cloudMatch; } //visualize histogram /* pcl::visualization::PCLHistogramVisualizer histvis; histvis.addFeatureHistogram<pcl::VFHSignature308> (*vfhs, histLength); */ //Visualize point cloud and matches //viewpoint cals int y_s = (int)std::floor (sqrt ((double)k)); int x_s = y_s + (int)std::ceil ((k / (double)y_s) - y_s); double x_step = (double)(1 / (double)x_s); double y_step = (double)(1 / (double)y_s); int viewport = 0, l = 0, m = 0; std::string viewName = "match number "; //setup visualizer and add query cloud pcl::visualization::PCLVisualizer visu("KNN search"); visu.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport); visu.addPointCloud<pcl::PointXYZ> (cloud, ColorHandler(cloud, 0.0 , 255.0, 0.0), "Query Cloud Cloud", viewport); visu.addText ("Query Cloud", 20, 30, 136.0/255.0, 58.0/255.0, 1, "Query Cloud", viewport); visu.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, "Query Cloud", viewport); visu.addCoordinateSystem (0.05, 0); //add matches to plot for(int i = 1; i < (k+1); ++i) { //shift viewpoint ++l; if (l >= x_s) { l = 0; m++; } //names and text labels std::string textString = viewName; std::string cloudname = viewName; textString.append(boost::lexical_cast<std::string>(i)); cloudname.append(boost::lexical_cast<std::string>(i)).append("cloud"); //color proportional to distance //add cloud visu.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport); visu.addPointCloud<pcl::PointXYZ> (cloudList[i-1], ColorHandler(cloudList[i-1], 0.0 , 255.0, 0.0), cloudname, viewport); visu.addText (textString, 20, 30, 136.0/255.0, 58.0/255.0, 1, textString, viewport); visu.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, textString, viewport); } visu.spin(); return 0; }
// Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); // Get input object and scene if (argc < 3) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } pcl::console::parse_argument (argc, argv, "--max_iterations", in_max_iterations); pcl::console::parse_argument (argc, argv, "--num_samples", in_num_samples); pcl::console::parse_argument (argc, argv, "--s_thresh", in_similarity_thresh); pcl::console::parse_argument (argc, argv, "--max_cdist", in_max_corresp_dist); pcl::console::parse_argument (argc, argv, "--inlier_frac", in_inlier_frac); pcl::console::parse_argument (argc, argv, "--leaf", in_leaf); pcl::console::parse_argument (argc, argv, "--normal_radius", normal_radius); pcl::console::parse_argument (argc, argv, "--feature_radius", feature_radius); pcl::console::parse_argument (argc, argv, "--icp", in_icp); pcl::console::parse_argument (argc, argv, "--max_corr_icp", max_corr_icp); pcl::console::parse_argument (argc, argv, "--icp_eps", max_eps_icp); // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); pcl_tools::cloud_from_stl(argv[2], *object); if (pcl::io::loadPCDFile<PointNT> (argv[1], *scene) < 0) { pcl::console::print_error ("Error loading object/scene file!\n"); return (1); } std::vector<int> indices; pcl::removeNaNFromPointCloud(*scene, *scene, indices); pcl::removeNaNFromPointCloud(*object, *object, indices); // /*pcl_tools::icp_result align = */alp_align(object, scene, object_aligned, 50000, 3, 0.9f, 5.5f * leaf, 0.7f); // /*pcl_tools::icp_result align = */alp_align(object_aligned, scene, object_aligned, 50000, 3, 0.9f, 7.5f * leaf, 0.4f); std::cout << "Inlier frac " << in_inlier_frac << std::endl; pcl_tools::icp_result align = alp_align(object, scene, object_aligned, in_max_iterations, in_num_samples, in_similarity_thresh, in_max_corresp_dist, in_inlier_frac, in_leaf); pcl::visualization::PCLVisualizer visu("Alignment"); if (align.converged) { pcl::console::print_info ("Inliers: %i/%i, scene: %i\n", align.inliers, object->size (), scene->size ()); // Show alignment visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object"); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned"); // visu.addPointCloudNormals<PointNT>(object); visu.spin (); } else { pcl::console::print_error ("Alignment failed!\n"); return (1); } if (in_icp) { pcl::console::print_highlight ("Applying ICP now\n"); pcl::IterativeClosestPointNonLinear<PointNT, PointNT> icp; // pcl::IterativeClosestPoint<PointNT, PointNT> icp; pcl_tools::affine_cloud(Eigen::Vector3f::UnitZ(), 0.0, Eigen::Vector3f(0.0, 0.0, 0.02), *object_aligned, *object_aligned); icp.setMaximumIterations (100); icp.setMaxCorrespondenceDistance(max_corr_icp); icp.setTransformationEpsilon (max_eps_icp); icp.setInputSource (object_aligned); icp.setInputTarget (scene); icp.align (*object_aligned); if (icp.hasConverged()) { pcl::console::print_highlight ("ICP: Converged with fitness %f\n", icp.getFitnessScore()); } // pcl::visualization::PCLVisualizer visu("Alignment"); // visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object"); // visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.updatePointCloud (object_aligned, ColorHandlerT (object_aligned, 100.0, 50.0, 200.0), "object_aligned"); // visu.addPointCloudNormals<PointNT>(object); visu.spin (); } return (0); }
// Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); FeatureCloudT::Ptr object_features (new FeatureCloudT); FeatureCloudT::Ptr scene_features (new FeatureCloudT); // parameter reader readParameters param_reader; param_reader.setConfigureFile("param.cfg"); // Get input object and scene if (argc != 3) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 || pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0) { pcl::console::print_error ("Error loading object/scene file!\n"); return (1); } //------------------------------------------------------------ // pre translation the object model bool pre_transform_p = false; param_reader.get<bool>("pre_transform_model_p", pre_transform_p); if(pre_transform_p) { Eigen::Vector4f scene_center(0.0f, 0.0f, 0.0f, 0.0f); pcl::compute3DCentroid(*scene, scene_center); // generate the rotation matrix: // define the rotation angle and rotation axis float rotation_angle = (float)20.0/180.0*M_PI; Eigen::Vector3f rotation_axis(0.2f, 1.0f, 1.0f); Eigen::AngleAxisf rotation_mg (rotation_angle, rotation_axis); // generate the whole tranform: // translate the model to the origin first, and then do the rotation. Eigen::Vector3f tmpVec3f(scene_center(0), scene_center(1), scene_center(2)); // tmpVec3f = tmpVec3f*0.09; // translate first, then rotation Eigen::Affine3f transform_mg ( rotation_mg*Eigen::Translation3f((-1) * tmpVec3f)); // transform the model cloud pcl::transformPointCloud(*object, *object, transform_mg); } //------------------------------------------------------------ // Downsample pcl::console::print_highlight ("Downsampling...\n"); pcl::VoxelGrid<PointNT> grid; float leaf = 0.003f; param_reader.get<float>("leaf_size", leaf); grid.setLeafSize (leaf, leaf, leaf); grid.setInputCloud (object); grid.filter (*object); grid.setInputCloud (scene); grid.filter (*scene); // Estimate normals for scene pcl::console::print_highlight ("Estimating scene normals...\n"); pcl::NormalEstimationOMP<PointNT,PointNT> nest; float normal_search_radius = 0.015; param_reader.get<float>("normal_search_radius", normal_search_radius); nest.setRadiusSearch (normal_search_radius); nest.setInputCloud (scene); nest.compute (*scene); // Estimate features pcl::console::print_highlight ("Estimating features...\n"); FeatureEstimationT fest; float feature_search_radius=0.01; param_reader.get<float>("feature_search_radius", feature_search_radius); fest.setRadiusSearch (feature_search_radius); fest.setInputCloud (object); fest.setInputNormals (object); fest.compute (*object_features); fest.setInputCloud (scene); fest.setInputNormals (scene); fest.compute (*scene_features); // Perform alignment pcl::console::print_highlight ("Starting alignment...\n"); // Initialize Sample Consensus Initial Alignment (SAC-IA) pcl::SampleConsensusInitialAlignment<PointNT, PointNT, FeatureT> reg; reg.setMinSampleDistance (0.01f); reg.setMaxCorrespondenceDistance (0.01); reg.setMaximumIterations (1000); reg.setInputSource (object); reg.setInputTarget (scene); reg.setSourceFeatures (object_features); reg.setTargetFeatures (scene_features); { pcl::ScopeTime t("Alignment"); reg.align (*object_aligned); } if (reg.hasConverged ()) { // Print results printf ("\n"); Eigen::Matrix4f transformation = reg.getFinalTransformation (); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2)); pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2)); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2)); pcl::console::print_info ("\n"); pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3)); pcl::console::print_info ("\n"); // pcl::console::print_info ("Inliers: %i/%i\n", reg.getInliers ().size (), object->size ()); // Show alignment pcl::visualization::PCLVisualizer visu("Alignment"); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned"); visu.spin (); } else { pcl::console::print_error ("Alignment failed!\n"); return (1); } return (0); }