// M063.3D.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
//
|
|
#include "pch.h"
|
#include <io.h>
|
#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/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/crop_box.h>
|
#include <pcl/filters/extract_indices.h>
|
#include <pcl/filters/statistical_outlier_removal.h>
|
#include <pcl/segmentation/region_growing.h>
|
#include <pcl/filters/approximate_voxel_grid.h>
|
#include <pcl/surface/gp3.h>
|
#include <pcl/console/time.h> // TicToc
|
#include <pcl/filters/random_sample.h>
|
#include <pcl/registration/ndt.h>
|
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
|
|
|
#include <opencv2/core/core.hpp>
|
#include <opencv2/highgui/highgui.hpp>
|
|
// 相机内参
|
const double camera_factor = 0.0002;
|
const double camera_cx = 0;
|
const double camera_cy = 0;
|
const double camera_fx = 0.015;
|
const double camera_fy = 0.015;
|
|
bool next_iteration;
|
|
#define PointT pcl::PointXYZ;
|
|
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;
|
}
|
|
void getFiles1(const std::string &path, std::vector<std::string>& files)
|
{
|
//文件句柄
|
//long hFile = 0; //win7
|
intptr_t hFile = 0; //win10
|
//文件信息
|
struct _finddata_t fileinfo;
|
std::string p;
|
if ((hFile = _findfirst(p.assign(path).append("\\*").c_str(), &fileinfo)) != -1)
|
// "\\*"是指读取文件夹下的所有类型的文件，若想读取特定类型的文件，以png为例，则用“\\*.png”
|
{
|
do
|
{
|
//如果是目录,迭代之
|
//如果不是,加入列表
|
if ((fileinfo.attrib & _A_SUBDIR))
|
{
|
if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)
|
getFiles1(p.assign(path).append("\\").append(fileinfo.name), files);
|
}
|
else
|
{
|
files.push_back(path + "\\" + fileinfo.name);
|
}
|
} while (_findnext(hFile, &fileinfo) == 0);
|
_findclose(hFile);
|
}
|
}
|
|
void string_replace(std::string &strBig, const std::string &strsrc, const std::string &strdst)
|
{
|
std::string::size_type pos = 0;
|
std::string::size_type srclen = strsrc.size();
|
std::string::size_type dstlen = strdst.size();
|
|
while ((pos = strBig.find(strsrc, pos)) != std::string::npos)
|
{
|
strBig.replace(pos, srclen, strdst);
|
pos += dstlen;
|
}
|
}
|
|
void step1(std::string &fileName)
|
{
|
cv::Mat depth;
|
|
depth = cv::imread(fileName, cv::ImreadModes::IMREAD_ANYDEPTH);
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // 遍历深度图
|
for (int m = 0; m < depth.rows; m += 4)
|
for (int n = 0; n < depth.cols; n += 4)
|
{
|
// 获取深度图中(m,n)处的值
|
ushort d = depth.ptr<ushort>(m)[n]; // d 可能没有值，若如此，跳过此点
|
if (d == 0)
|
continue; // d 存在值，则向点云增加一个点
|
pcl::PointXYZ p; // 计算这个点的空间坐标
|
p.z = double(d) * camera_factor;
|
p.x = n * camera_fx;
|
p.y = m * camera_fy;
|
|
// 把p加入到点云中
|
cloud->points.push_back(p);
|
}
|
// 设置并保存点云
|
cloud->height = 1;
|
cloud->width = cloud->points.size();
|
cout << "point cloud size = " << cloud->points.size() << endl;
|
cloud->is_dense = false;
|
|
string_replace(fileName, ".tif", ".pcd");
|
|
pcl::io::savePCDFile(fileName, *cloud); // 清除数据并退出
|
cloud->points.clear();
|
cout << "Point cloud saved." << endl;
|
}
|
|
void step2()
|
{
|
std::string fileName1 = "D:\\calibration_3d_0331_2\\5_T.pcd";
|
std::string fileName2 = "D:\\calibration_3d_0331_2\\6.pcd";
|
|
std::string outFileName = "D:\\calibration_3d_0331_2\\6_T.pcd";
|
|
// Set initial alignment estimate found using robot odometry.
|
Eigen::AngleAxisf init_rotation(0, Eigen::Vector3f::UnitZ());
|
Eigen::Translation3f init_translation(-200, 0, 0);
|
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName1, *target_cloud) == -1)
|
{
|
PCL_ERROR("Couldn't read file 1 \n");
|
return;
|
}
|
std::cout << "Loaded " << target_cloud->size() << " data points from "<< fileName1 << std::endl;
|
|
// Loading second scan of room from new perspective.
|
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
if (pcl::io::loadPCDFile<pcl::PointXYZ>(fileName2, *input_cloud) == -1)
|
{
|
PCL_ERROR("Couldn't read file 2.pcd \n");
|
return;
|
}
|
std::cout << "Loaded " << input_cloud->size() << " data points from" << fileName2 << std::endl;
|
|
|
// ===================================删除离群点=====================================//
|
//// Create the filtering object
|
//pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
|
//sor.setMeanK(50);//50个临近点
|
//sor.setStddevMulThresh(1.0);//距离大于1倍标准方差
|
//sor.setInputCloud(target_cloud);
|
//sor.filter(*target_cloud);
|
//sor.setInputCloud(input_cloud);
|
//sor.filter(*input_cloud);
|
//=================================================================================//
|
|
|
// Filtering input scan to roughly 10% of original size to increase speed of registration.
|
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
|
approximate_voxel_filter.setLeafSize(0.8, 0.8, 0.8);
|
approximate_voxel_filter.setInputCloud(input_cloud);
|
approximate_voxel_filter.filter(*filtered_cloud);
|
std::cout << "Filtered cloud contains " << filtered_cloud->size()
|
<< " data points from "<< fileName2 << std::endl;
|
|
//pcl::CropBox<pcl::PointXYZ> cropBox;
|
//cropBox.setMin(Eigen::Vector4f(20, 0, 0, 1.0));
|
//cropBox.setMax(Eigen::Vector4f(50, 250, 30, 1.0));
|
//cropBox.setInputCloud(filtered_cloud);
|
//cropBox.filter(*filtered_cloud);
|
|
|
// Initializing Normal Distributions Transform (NDT).
|
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
|
|
// Setting scale dependent NDT parameters
|
// Setting minimum transformation difference for termination condition.
|
ndt.setTransformationEpsilon(0.01);
|
// Setting maximum step size for More-Thuente line search.
|
ndt.setStepSize(1);
|
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
|
ndt.setResolution(1);
|
|
// Setting max number of registration iterations.
|
ndt.setMaximumIterations(100);
|
|
// Setting point cloud to be aligned.
|
ndt.setInputSource(filtered_cloud);
|
// Setting point cloud to be aligned to.
|
ndt.setInputTarget(target_cloud);
|
|
// Calculating required rigid transform to align the input cloud to the target cloud.
|
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
ndt.align(*output_cloud, init_guess);
|
|
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
|
<< " score: " << ndt.getFitnessScore() << std::endl;
|
print4x4Matrix(ndt.getFinalTransformation().cast<double>());
|
|
// Transforming unfiltered, input cloud using found transform.
|
pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
|
|
// Saving transformed input cloud.
|
pcl::io::savePCDFile(outFileName, *output_cloud);
|
|
// Initializing point cloud visualizer
|
pcl::visualization::PCLVisualizer::Ptr
|
viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
|
viewer_final->setBackgroundColor(0, 0, 0);
|
|
// Coloring and visualizing target cloud (red).
|
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
|
target_color(target_cloud, 255, 0, 0);
|
viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
|
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
|
1, "target cloud");
|
|
// Coloring and visualizing transformed input cloud (green).
|
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
|
output_color(output_cloud, 0, 255, 0);
|
viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
|
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
|
1, "output cloud");
|
|
// Starting visualizer
|
viewer_final->addCoordinateSystem(1.0, "global");
|
viewer_final->initCameraParameters();
|
|
// Wait until visualizer window is closed.
|
while (!viewer_final->wasStopped())
|
{
|
viewer_final->spinOnce(100);
|
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
|
}
|
}
|
|
void step3()
|
{
|
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Images\\1.pcd", *target_cloud) == -1)
|
{
|
PCL_ERROR("Couldn't read file 1.pcd \n");
|
return;
|
}
|
std::cout << "Loaded " << target_cloud->size() << " data points from 1.pcd" << std::endl;
|
|
// Loading second scan of room from new perspective.
|
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Images\\2.pcd", *input_cloud) == -1)
|
{
|
PCL_ERROR("Couldn't read file 2.pcd \n");
|
return;
|
}
|
std::cout << "Loaded " << input_cloud->size() << " data points from 2.pcd" << std::endl;
|
|
|
// ===================================删除离群点=====================================//
|
// Create the filtering object
|
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
|
sor.setMeanK(50);//50个临近点
|
sor.setStddevMulThresh(1.0);//距离大于1倍标准方差
|
|
sor.setInputCloud(target_cloud);
|
sor.filter(*target_cloud);
|
|
sor.setInputCloud(input_cloud);
|
sor.filter(*input_cloud);
|
//=================================================================================//
|
|
|
// Filtering input scan to roughly 10% of original size to increase speed of registration.
|
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
|
approximate_voxel_filter.setLeafSize(0.8, 0.8, 0.8);
|
approximate_voxel_filter.setInputCloud(input_cloud);
|
approximate_voxel_filter.filter(*filtered_cloud);
|
std::cout << "Filtered cloud contains " << filtered_cloud->size()
|
<< " data points from 2.pcd" << std::endl;
|
|
pcl::CropBox<pcl::PointXYZ> cropBox;
|
cropBox.setMin(Eigen::Vector4f(15, 0, 0, 1.0));
|
cropBox.setMax(Eigen::Vector4f(40, 250, 30, 1.0));
|
cropBox.setInputCloud(filtered_cloud);
|
cropBox.filter(*filtered_cloud);
|
|
int iterations = 1;
|
|
// Set initial alignment estimate found using robot odometry.
|
Eigen::AngleAxisd init_rotation(0, Eigen::Vector3d::UnitZ());
|
Eigen::Translation3d init_translation(41, 0, 2);
|
Eigen::Matrix4d init_guess = (init_translation * init_rotation).matrix();
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr testCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::transformPointCloud(*filtered_cloud, *testCloud, init_guess);
|
|
// The Iterative Closest Point algorithm
|
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
|
|
pcl::console::TicToc time;
|
time.tic();
|
icp.setMaximumIterations(1);
|
icp.setInputSource(testCloud);
|
icp.setInputTarget(target_cloud);
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
icp.align(*output_cloud);
|
|
|
|
std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;
|
|
Eigen::Matrix4d finalMat;
|
|
if (icp.hasConverged())
|
{
|
std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
|
std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
|
|
finalMat = init_guess * icp.getFinalTransformation().cast<double>();
|
pcl::transformPointCloud(*filtered_cloud, *output_cloud, finalMat);
|
}
|
else
|
{
|
PCL_ERROR("\nICP has not converged.\n");
|
return;
|
}
|
|
// 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<pcl::PointXYZ> cloud_target_color_h(target_cloud, 255 ,255 ,255);
|
viewer.addPointCloud(target_cloud, cloud_target_color_h, "cloud_in_v1", v1);
|
viewer.addPointCloud(target_cloud, cloud_target_color_h, "cloud_in_v2", v2);
|
|
// Transformed point cloud is green
|
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_input_color_h(input_cloud, 20, 180, 20);
|
viewer.addPointCloud(input_cloud, cloud_input_color_h, "cloud_tr_v1", v1);
|
|
// ICP aligned point cloud is red
|
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_output_color_h(output_cloud, 180, 20, 20);
|
viewer.addPointCloud(output_cloud, cloud_output_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.setInputSource(output_cloud);
|
icp.setInputTarget(target_cloud);
|
icp.align(*output_cloud);
|
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_icp -> cloud_in" << std::endl;
|
finalMat *= icp.getFinalTransformation().cast<double>(); // WARNING /!\ This is not accurate! For "educational" purpose only!
|
print4x4Matrix(finalMat); // 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(output_cloud, cloud_output_color_h, "cloud_icp_v2");
|
}
|
else
|
{
|
PCL_ERROR("\nICP has not converged.\n");
|
return;
|
}
|
}
|
next_iteration = false;
|
}
|
|
}
|
|
void step4()
|
{
|
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inliner(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_outliner(new pcl::PointCloud<pcl::PointXYZ>);
|
|
if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Images\\1.pcd", *target_cloud) == -1)
|
{
|
PCL_ERROR("Couldn't read file 1.pcd \n");
|
return;
|
}
|
std::cout << "Loaded " << target_cloud->size() << " data points from 1.pcd" << std::endl;
|
|
// Create the filtering object
|
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
|
sor.setInputCloud(target_cloud);
|
sor.setMeanK(50);//50个临近点
|
sor.setStddevMulThresh(1.0);//距离大于1倍标准方差
|
|
sor.filter(*cloud_inliner);
|
|
// Initializing point cloud visualizer
|
pcl::visualization::PCLVisualizer::Ptr
|
viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
|
viewer_final->setBackgroundColor(0, 0, 0);
|
|
// Coloring and visualizing target cloud (red).
|
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
|
target_color(target_cloud, 255, 0, 0);
|
viewer_final->addPointCloud<pcl::PointXYZ>(cloud_inliner, target_color, "target cloud");
|
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
|
1, "target cloud");
|
|
// Coloring and visualizing transformed input cloud (green).
|
/*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
|
output_color(output_cloud, 0, 255, 0);
|
viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
|
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
|
1, "output cloud");*/
|
|
// Starting visualizer
|
viewer_final->addCoordinateSystem(1.0, "global");
|
viewer_final->initCameraParameters();
|
|
// Wait until visualizer window is closed.
|
while (!viewer_final->wasStopped())
|
{
|
viewer_final->spinOnce(100);
|
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
|
}
|
|
}
|
|
int main()
|
{
|
std::vector<std::string> files;
|
|
getFiles1("D:\\1", files);
|
|
for (int i = 0; i < files.size(); i++)
|
{
|
step1(files[i]);
|
}
|
|
return (0);
|
|
}
|
|
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
// 调试程序: F5 或调试 >“开始调试”菜单
|
|
// 入门提示:
|
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
// 2. 使用团队资源管理器窗口连接到源代码管理
|
// 3. 使用输出窗口查看生成输出和其他消息
|
// 4. 使用错误列表窗口查看错误
|
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
