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);
}
