// Cpp-pcl.cpp : This file contains the 'main' function. Program execution begins and ends there.
|
//
|
|
#include <iostream>
|
#include <vector>
|
#include <pcl/point_types.h>
|
#include <pcl/io/pcd_io.h>
|
#include <pcl/io/vtk_io.h>
|
#include <pcl/io/ply_io.h>
|
#include <pcl/registration/icp.h>
|
#include <pcl/visualization/cloud_viewer.h>
|
#include <pcl/visualization/pcl_visualizer.h>
|
#include <pcl/search/search.h>
|
#include <pcl/search/kdtree.h>
|
#include <pcl/features/normal_3d.h>
|
#include <pcl/visualization/cloud_viewer.h>
|
#include <pcl/filters/passthrough.h>
|
#include <pcl/filters/extract_indices.h>
|
#include <pcl/segmentation/region_growing.h>
|
#include <pcl/surface/gp3.h>
|
#include <pcl/console/time.h> // TicToc
|
#include <pcl/filters/random_sample.h>
|
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
|
#include <pcl/gpu/containers/initialization.h>
|
#include <pcl/gpu/octree/octree.hpp>
|
#include <pcl/gpu/features/features.hpp>
|
#include <pcl/gpu/segmentation/gpu_extract_clusters.h>
|
#include <pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp>
|
|
typedef pcl::PointXYZ PointT;
|
typedef pcl::PointCloud<PointT> PointCloudT;
|
|
|
|
// µãÔÆÊý¾Ý
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_trans(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);
|
|
bool next_iteration = false;
|
|
void print4x4Matrix(const Eigen::Matrix4d & matrix)
|
{
|
printf("Rotation matrix :\n");
|
printf(" | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
|
printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
|
printf(" | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
|
printf("Translation vector :\n");
|
printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
|
}
|
|
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,void* nothing)
|
{
|
if (event.getKeySym() == "space" && event.keyDown())
|
next_iteration = true;
|
}
|
|
|
|
// 1.¶ÁÈ¡µãÔÆ
|
// 2.È¥³ýÔؾ߲¿·ÖµãÔÆ
|
// 3.±£´æµãÔÆ
|
void step1(const std::string &fileName,bool isSave = false)
|
{
|
pcl::console::TicToc time;
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>); // ´´½¨µãÔÆ£¨Ö¸Õ룩
|
|
time.tic();
|
|
if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName, *cloud_in) < 0) //* ¶ÁÈëPCD¸ñʽµÄÎļþ£¬Èç¹ûÎļþ²»´æÔÚ£¬·µ»Ø-1
|
{
|
PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //Îļþ²»´æÔÚʱ£¬·µ»Ø´íÎó£¬ÖÕÖ¹³ÌÐò¡£
|
return;
|
}
|
|
cout << "Loaded file (" << cloud_in->size()<< " points) in " << time.toc() << " ms" << endl;
|
|
time.tic();
|
|
pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
|
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
|
|
|
//======================CPU version===========================//
|
/*pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
|
normal_estimator.setSearchMethod(tree);
|
normal_estimator.setInputCloud(cloud_in);
|
normal_estimator.setKSearch(50);
|
normal_estimator.compute(*normals);*/
|
//===========================================================//
|
|
//=====================GPU version===========================//
|
pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud_in));
|
pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
|
pcl::gpu::Octree::PointCloud cloud_device;
|
cloud_device.upload(cloud_in->points);
|
normals_device.upload(normals_p->points);
|
|
pcl::gpu::NormalEstimation normal_estimator;
|
normal_estimator.setRadiusSearch(2.0,50);
|
normal_estimator.setInputCloud(cloud_device);
|
normal_estimator.compute(normals_device);
|
|
normals_device.download(normals_p->points);
|
|
for (int i = 0; i < normals_p->size(); i++)
|
{
|
pcl::Normal normal;
|
|
normal.normal_x = normals_p->points[i].x;
|
normal.normal_y = normals_p->points[i].y;
|
normal.normal_z = normals_p->points[i].z;
|
|
normals->push_back(normal);
|
}
|
//=========================================================//
|
|
|
std::cout << "Estimation Normal in:" << time.toc() << "ms" << std::endl;
|
time.tic();
|
|
//boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Viewer"));
|
//viewer->setBackgroundColor(0, 0, 0);
|
//viewer->addPointCloud<pcl::PointXYZ>(cloud_in, "cloud");
|
//viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_in, normals, 1, 0.05, "normals");//ʵÏÖ¶ÔµãÔÆ·¨ÏßµÄÏÔʾ
|
//
|
//while (!viewer->wasStopped())
|
//{
|
// viewer->spinOnce();
|
//}
|
//return;
|
|
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
|
reg.setMinClusterSize(50000);
|
reg.setMaxClusterSize(1000000);
|
reg.setSearchMethod(tree);
|
reg.setNumberOfNeighbours(30);
|
reg.setInputCloud(cloud_in);
|
reg.setInputNormals(normals);
|
reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
|
reg.setCurvatureThreshold(1.0);
|
|
std::vector <pcl::PointIndices> clusters;
|
reg.extract(clusters);
|
|
std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
|
std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;
|
|
boost::shared_ptr<std::vector<int>> index_ptr;
|
pcl::ExtractIndices<pcl::PointXYZ> extract(new pcl::ExtractIndices<pcl::PointXYZ>);
|
|
index_ptr = boost::make_shared<std::vector<int>>(clusters[0].indices);
|
extract.setInputCloud(cloud_in);
|
extract.setIndices(index_ptr);
|
extract.setNegative(false);
|
extract.filter(*cloud);
|
|
std::cout << "Segmentation cloud in:" << time.toc() << "ms" << std::endl;
|
|
if (isSave)
|
{
|
pcl::io::savePCDFile<pcl::PointXYZ>("D:\\test\\Plane.pcd", *cloud);
|
}
|
|
|
/*index_ptr = boost::make_shared<std::vector<int>>(clusters[1].indices);
|
extract.setInputCloud(cloud);
|
extract.setIndices(index_ptr);
|
extract.setNegative(false);
|
extract.filter(*cloud_p);
|
pcl::io::savePLYFileBinary<pcl::PointXYZ>("D:\\test\\stand2.ply", *cloud_p);
|
|
index_ptr = boost::make_shared<std::vector<int>>(clusters[2].indices);
|
extract.setInputCloud(cloud);
|
extract.setIndices(index_ptr);
|
extract.setNegative(false);
|
extract.filter(*cloud_p);
|
pcl::io::savePLYFileBinary<pcl::PointXYZ>("D:\\test\\stand3.ply", *cloud_p);*/
|
|
/*int counter = 0;
|
while (counter < clusters[0].indices.size())
|
{
|
std::cout << clusters[0].indices[counter] << ", ";
|
counter++;
|
if (counter % 10 == 0)
|
std::cout << std::endl;
|
}*/
|
|
|
|
//չʾµãÔÆ
|
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
|
pcl::visualization::CloudViewer viewer("Cluster viewer");
|
viewer.showCloud(colored_cloud);
|
while (!viewer.wasStopped())
|
{
|
|
}
|
return;
|
}
|
|
void step1_gpu(const std::string &fileName, bool isSave = false)
|
{
|
pcl::console::TicToc time;
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>); // ´´½¨µãÔÆ£¨Ö¸Õ룩
|
|
time.tic();
|
|
if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName, *cloud_in) < 0) //* ¶ÁÈëPCD¸ñʽµÄÎļþ£¬Èç¹ûÎļþ²»´æÔÚ£¬·µ»Ø-1
|
{
|
PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //Îļþ²»´æÔÚʱ£¬·µ»Ø´íÎó£¬ÖÕÖ¹³ÌÐò¡£
|
return;
|
}
|
|
cout << "Loaded file (" << cloud_in->size() << " points) in " << time.toc() << " ms" << endl;
|
|
time.tic();
|
|
pcl::gpu::Octree::PointCloud cloud_device;
|
cloud_device.upload(cloud_in->points);
|
|
pcl::gpu::Octree::Ptr octree_device(new pcl::gpu::Octree);
|
octree_device->setCloud(cloud_device);
|
octree_device->build();
|
|
std::vector<pcl::PointIndices> cluster_indices_gpu;
|
pcl::gpu::EuclideanClusterExtraction gec;
|
gec.setClusterTolerance(0.6); // 2mm
|
gec.setMinClusterSize(100000);
|
gec.setMaxClusterSize(250000);
|
gec.setSearchMethod(octree_device);
|
gec.setHostCloud(cloud_in);
|
gec.extract(cluster_indices_gpu);
|
|
|
std::cout << "Number of clusters is equal to " << cluster_indices_gpu.size() << std::endl;
|
std::cout << "First cluster has " << cluster_indices_gpu[0].indices.size() << " points." << endl;
|
|
boost::shared_ptr<std::vector<int>> index_ptr;
|
pcl::ExtractIndices<pcl::PointXYZ> extract(new pcl::ExtractIndices<pcl::PointXYZ>);
|
|
index_ptr = boost::make_shared<std::vector<int>>(cluster_indices_gpu[0].indices);
|
extract.setInputCloud(cloud_in);
|
extract.setIndices(index_ptr);
|
extract.setNegative(false);
|
extract.filter(*cloud);
|
|
std::cout << "segmentation cloud in:" << time.toc() << "ms" << std::endl;
|
|
if (isSave)
|
{
|
pcl::io::savePCDFile<pcl::PointXYZ>("D:\\test\\Plane.pcd", *cloud);
|
}
|
}
|
|
// 1.¶ÁÈ¡µãÔÆ
|
// 2.Èý½Ç²âÁ¿Éú³Émesh
|
// 3.±£´æmesh
|
void step2(const std::string &fileName = "")
|
{
|
pcl::console::TicToc time;
|
|
if (fileName != "")
|
{
|
time.tic();
|
|
// Load input file into a PointCloud<T> with an appropriate type
|
|
pcl::PCLPointCloud2 cloud_blob;
|
if (pcl::io::loadPCDFile(fileName, cloud_blob) < 0)
|
{
|
PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //Îļþ²»´æÔÚʱ£¬·µ»Ø´íÎó£¬ÖÕÖ¹³ÌÐò¡£
|
return;
|
}
|
pcl::fromPCLPointCloud2(cloud_blob, *cloud);
|
//* the data should be available in cloud
|
|
std::cout << "cloud size: " << cloud->size() << "," << time.toc() << "ms" << endl;
|
}
|
|
|
time.tic();
|
|
// Normal estimation*
|
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
|
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
|
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
|
tree->setInputCloud(cloud);
|
n.setInputCloud(cloud);
|
n.setSearchMethod(tree);
|
n.setKSearch(20);
|
n.compute(*normals);
|
//* normals should not contain the point normals + surface curvatures
|
|
// Concatenate the XYZ and normal fields*
|
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
|
pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
|
//* cloud_with_normals = cloud + normals
|
|
// Create search tree*
|
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
|
tree2->setInputCloud(cloud_with_normals);
|
|
// Initialize objects
|
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
|
pcl::PolygonMesh triangles;
|
|
// Set the maximum distance between connected points (maximum edge length)
|
gp3.setSearchRadius(25);
|
|
// Set typical values for the parameters
|
gp3.setMu(25);
|
gp3.setMaximumNearestNeighbors(200);
|
gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees
|
gp3.setMinimumAngle(M_PI / 18); // 10 degrees
|
gp3.setMaximumAngle(2 * M_PI / 3); // 120 degrees
|
gp3.setNormalConsistency(false);
|
|
// Get result
|
gp3.setInputCloud(cloud_with_normals);
|
gp3.setSearchMethod(tree2);
|
gp3.reconstruct(triangles);
|
|
std::cout << "create mesh in " << time.toc() << "ms" << endl;
|
|
pcl::io::saveVTKFile("D:\\test\\Mesh.vtk", triangles);
|
|
// Finish
|
return;
|
|
}
|
|
// 1.¶ÁÈ¡mesh
|
// 2.icp
|
// 3.±£´æ¾¹ýת»»ºóµÄµãÔÆ
|
void step3(const std::string &fileName1,const std::string &fileName2,bool isShow,bool isSave)
|
{
|
pcl::console::TicToc time;
|
|
// The point clouds we will be using
|
PointCloudT::Ptr cloud_source(new PointCloudT);
|
|
PointCloudT::Ptr cloud_in(new PointCloudT); // Original point cloud
|
PointCloudT::Ptr cloud_tr(new PointCloudT); // Transformed point cloud
|
PointCloudT::Ptr cloud_standard(new PointCloudT); // ICP output point cloud
|
|
pcl::RandomSample<PointT> rs;
|
rs.setSample(10000);
|
|
int iterations = 200; // Default number of ICP iterations
|
|
|
time.tic();
|
|
if (fileName1 == "")
|
{
|
cloud_source = cloud;
|
}
|
else
|
{
|
if (pcl::io::loadPLYFile(fileName1, *cloud_source) < 0)
|
{
|
PCL_ERROR("Error loading cloud %s.\n", fileName1);
|
return;
|
}
|
}
|
|
rs.setInputCloud(cloud_source);
|
rs.filter(*cloud_in);
|
std::cout << "\nLoaded file " << fileName1 << " (" << cloud_in->size() << " points) in " << time.toc() << " ms\n" << std::endl;
|
|
if (pcl::io::loadPLYFile(fileName2, *cloud_target) < 0)
|
{
|
PCL_ERROR("Error loading cloud %s.\n", fileName2);
|
return;
|
}
|
rs.setInputCloud(cloud_target);
|
rs.filter(*cloud_standard);
|
std::cout << "\nLoaded file " << fileName2 << " (" << cloud_standard->size() << " points) in " << time.toc() << " ms\n" << std::endl;
|
|
*cloud_tr = *cloud_in; // We backup cloud_icp into cloud_tr for later use
|
|
// Defining a rotation matrix and translation vector
|
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
|
|
// The Iterative Closest Point algorithm
|
time.tic();
|
pcl::IterativeClosestPoint<PointT, PointT> icp;
|
icp.setMaximumIterations(iterations);
|
icp.setInputSource(cloud_in);
|
icp.setInputTarget(cloud_standard);
|
icp.align(*cloud_in);
|
|
std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;
|
|
if (icp.hasConverged())
|
{
|
transformation_matrix = icp.getFinalTransformation().cast<double>();
|
print4x4Matrix(transformation_matrix);
|
icp.setMaximumIterations(1);
|
icp.align(*cloud_in);
|
std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
|
std::cout << "\nICP transformation " << iterations << " : cloud_in -> cloud_standard" << std::endl;
|
|
// ±£´æת»»ºóµÄµãÔÆ
|
pcl::transformPointCloud(*cloud_source, *cloud_trans, transformation_matrix);
|
if (isSave)
|
{
|
pcl::io::savePLYFile("D:\\test\\mesh_trans.ply", *cloud_trans);
|
}
|
}
|
else
|
{
|
PCL_ERROR("\nICP has not converged.\n");
|
return;
|
}
|
|
if (isShow)
|
{
|
// Visualization
|
pcl::visualization::PCLVisualizer viewer("ICP demo");
|
// Create two vertically separated viewports
|
int v1(0);
|
int v2(1);
|
viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
|
viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
|
|
// The color we will be using
|
float bckgr_gray_level = 0.0; // Black
|
float txt_gray_lvl = 1.0 - bckgr_gray_level;
|
|
// Original point cloud is white
|
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_standard, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
|
(int)255 * txt_gray_lvl);
|
viewer.addPointCloud(cloud_standard, cloud_in_color_h, "cloud_in_v1", v1);
|
viewer.addPointCloud(cloud_standard, cloud_in_color_h, "cloud_in_v2", v2);
|
|
// Transformed point cloud is green
|
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 20, 180, 20);
|
viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
|
|
// ICP aligned point cloud is red
|
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_in, 180, 20, 20);
|
viewer.addPointCloud(cloud_in, cloud_icp_color_h, "cloud_icp_v2", v2);
|
|
// Adding text descriptions in each viewport
|
viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
|
viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);
|
|
std::stringstream ss;
|
ss << iterations;
|
std::string iterations_cnt = "ICP iterations = " + ss.str();
|
viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
|
|
// Set background color
|
viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
|
viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
|
|
// Set camera position and orientation
|
viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
|
viewer.setSize(1280, 1024); // Visualiser window size
|
|
// Register keyboard callback :
|
viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);
|
|
// Display the visualiser
|
while (!viewer.wasStopped())
|
{
|
viewer.spinOnce();
|
|
// The user pressed "space" :
|
if (next_iteration)
|
{
|
// The Iterative Closest Point algorithm
|
time.tic();
|
icp.align(*cloud_in);
|
|
std::cout << "Applied 1 ICP iteration in " << time.toc() << " ms" << std::endl;
|
|
if (icp.hasConverged())
|
{
|
printf("\033[11A"); // Go up 11 lines in terminal output.
|
printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());
|
std::cout << "\nICP transformation " << ++iterations << " : cloud_in -> cloud_standard" << std::endl;
|
transformation_matrix *= icp.getFinalTransformation().cast<double>(); // WARNING /!\ This is not accurate! For "educational" purpose only!
|
print4x4Matrix(transformation_matrix); // Print the transformation between original pose and current pose
|
|
ss.str("");
|
ss << iterations;
|
std::string iterations_cnt = "ICP iterations = " + ss.str();
|
viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
|
viewer.updatePointCloud(cloud_in, cloud_icp_color_h, "cloud_icp_v2");
|
}
|
else
|
{
|
PCL_ERROR("\nICP has not converged.\n");
|
return;
|
}
|
}
|
next_iteration = false;
|
}
|
}
|
}
|
|
// 1.¶ÁÈ¡Ô´µãÔƺÍÄ¿±êµãÔÆ
|
// 2.¼ÆËã·¨Ïß
|
// 3.¼ÆËãÔ´µãÔƵ½Ä¿±êµãÔƵÄ×îС¾àÀë
|
// 4.±£´æÊý¾ÝÒÔ¼°Êý¾Ýչʾ
|
void step4(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target)
|
{
|
pcl::console::TicToc time;
|
|
pcl::registration::CorrespondenceEstimationNormalShooting<PointT, PointT, pcl::Normal> cens;
|
pcl::Correspondences correspondence;
|
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_c(new pcl::PointCloud<pcl::PointXYZI>);
|
|
|
time.tic();
|
|
pcl::PointCloud <pcl::Normal>::Ptr source_normals(new pcl::PointCloud <pcl::Normal>);
|
|
//======================CPU version===========================//
|
//pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
|
//pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
|
//normal_estimator.setInputCloud(cloud_source);
|
//normal_estimator.setSearchMethod(tree);
|
//normal_estimator.setKSearch(10);
|
//normal_estimator.compute(*source_normals);
|
//===========================================================//
|
|
//=====================GPU version===========================//
|
pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud_source));
|
pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
|
pcl::gpu::Octree::PointCloud cloud_device;
|
cloud_device.upload(cloud_source->points);
|
normals_device.upload(normals_p->points);
|
|
pcl::gpu::NormalEstimation normal_estimator;
|
normal_estimator.setRadiusSearch(2.0, 50);
|
normal_estimator.setInputCloud(cloud_device);
|
normal_estimator.compute(normals_device);
|
normals_device.download(normals_p->points);
|
|
for (int i = 0; i < normals_p->size(); i++)
|
{
|
pcl::Normal normal;
|
|
normal.normal_x = normals_p->points[i].x;
|
normal.normal_y = normals_p->points[i].y;
|
normal.normal_z = normals_p->points[i].z;
|
|
source_normals->push_back(normal);
|
}
|
//=========================================================//
|
|
|
std::cout << "Calculate normals in " << time.toc() << " ms" << std::endl;
|
time.tic();
|
|
cens.setInputCloud(cloud_source);
|
cens.setInputTarget(cloud_target);
|
cens.setSourceNormals(source_normals);
|
cens.determineCorrespondences(correspondence);
|
|
|
|
//ces.setInputCloud(cloud);
|
//ces.setInputTarget(cloud_target);
|
//ces.determineCorrespondences(correspondence);
|
|
ofstream outFile;
|
outFile.open("D:\\data.csv", ios::out); // ´ò¿ªÄ£Ê½¿ÉÊ¡ÂÔ
|
for (int i = 0; i < cloud_source->size(); i++)
|
{
|
pcl::PointXYZI p;
|
|
p.x = cloud_source->points[i].x;
|
p.y = cloud_source->points[i].y;
|
p.z = cloud_source->points[i].z;
|
|
float delta_x = cloud_target->points[correspondence[i].index_match].x - p.x;
|
float delta_y = cloud_target->points[correspondence[i].index_match].y - p.y;
|
float delta_z = cloud_target->points[correspondence[i].index_match].z - p.z;
|
|
float distance =
|
source_normals->points[i].normal_x * delta_x +
|
source_normals->points[i].normal_y * delta_y +
|
source_normals->points[i].normal_z * delta_z;
|
|
p.intensity = (distance<0.15 && distance > -0.15) ? distance : 0;
|
|
p.z = p.intensity * 100;
|
|
//outFile << p.x << ',' << p.y << ',' << p.z << "," << p.intensity << endl;
|
|
cloud_c->push_back(p);
|
}
|
outFile.close();
|
|
std::cout << "Calculate distance in " << time.toc() << " ms" << std::endl;
|
|
//չʾµãÔÆ
|
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor(cloud_c, "intensity");//°´ÕÕintensity×ֶνøÐÐäÖȾ
|
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Normals"));
|
viewer->setBackgroundColor(0, 0, 0);
|
viewer->addPointCloud<pcl::PointXYZI>(cloud_c, fildColor, "sample");//ÏÔʾµãÔÆ£¬ÆäÖÐfildColorΪÑÕÉ«ÏÔʾ
|
//viewer->addPointCloudNormals<pcl::PointXYZI, pcl::Normal>(cloud_c, source_normals, 10, 0.05, "no");//ʵÏÖ¶ÔµãÔÆ·¨ÏßµÄÏÔʾ
|
while (!viewer->wasStopped())
|
{
|
viewer->spinOnce();
|
}
|
return ;
|
}
|
|
int main()
|
{
|
|
//step1("D:\\test\\Sample.pcd");
|
|
//step1_gpu("D:\\test\\Sample.pcd");
|
|
//step2("D:\\test\\Plane.pcd");
|
|
step3("D:\\test\\Mesh.ply", "D:\\test\\stand2.ply", false, false);
|
|
/*if (pcl::io::loadPLYFile("D:\\test\\mesh_trans.ply", *cloud_trans) < 0)
|
{
|
PCL_ERROR("Error loading cloud %s.\n", "D:\\test\\mesh_trans.ply");
|
return 0;
|
}*/
|
|
/*pcl::RandomSample<pcl::PointXYZ> rs;
|
rs.setSample(1000);
|
rs.setInputCloud(cloud_trans);
|
rs.filter(*cloud_trans);*/
|
|
/*if (pcl::io::loadPLYFile("D:\\test\\stand2.ply", *cloud_target) < 0)
|
{
|
PCL_ERROR("Error loading cloud %s.\n", "D:\\test\\stand2.ply");
|
return 0;
|
}
|
|
step4(cloud_trans, cloud_target);*/
|
|
return 0;
|
|
}
|
|
// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
|
// Debug program: F5 or Debug > Start Debugging menu
|
|
// Tips for Getting Started:
|
// 1. Use the Solution Explorer window to add/manage files
|
// 2. Use the Team Explorer window to connect to source control
|
// 3. Use the Output window to see build output and other messages
|
// 4. Use the Error List window to view errors
|
// 5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
|
// 6. In the future, to open this project again, go to File > Open > Project and select the .sln file
|
