快速鲁棒的ICP(一)-编程知识

一、代码下载以、修改以及使用

链接：OpenGP/sparseicp: Automatically exported from code.google.com/p/sparseicp (github.com)

解压之后：

快速鲁棒的ICP是其他人在这份Sparse ICP代码基础上改写出来的：

我这里已经下载好了：快速鲁棒ICP.zip资源-CSDN文库

首先visual studio项目，配置好PCL环境；首先把上述图片中看得到的以cpp、h、hpp结尾的文件全都放入你的项目中；再把include文件夹的nanoflann.hpp放入你的项目就行了。

其中view_show.hpp是我为了可视化新建的文件，源文件里面并没有

修改：

由于该代码有几处头文件使用的是绝对路径，你需要修改成自己的路径，而且源码没有可视化，我这里修改了下，使得可以进行可视化

1、头文件修改

Types.h、median.h、io_pc.h

把这行改成#include <Eigen/Dense>

FRICP.h

把这行改成#include <unsupported/Eigen/MatrixFunctions>

这样头文件的问题就解决了

2、可视化的修改

我增加了一个文件view_show.hpp

#ifndef VIEW_SHOW_H
#define VIEW_SHOW_H
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/print.h>
class ViewShow
{
public:ViewShow(){}int readOBJ(const std::string objFilePath, Eigen::MatrixXf& points){cout << objFilePath << endl;std::ifstream objFile(objFilePath);std::string line;std::vector<Eigen::Vector3f> vertexList; // 用于临时存储顶点数据的列表if (!objFile.is_open()) {std::cerr << "Unable to open OBJ file." << std::endl;return -1;}while (std::getline(objFile, line)) {std::stringstream ss(line);std::string lineType;ss >> lineType;// 只处理顶点数据行if (lineType == "v") {float x, y, z;//cout << x << y << z << endl;ss >> x >> y >> z;vertexList.push_back(Eigen::Vector3f(x, y, z));}}//cout << vertexList.size() << endl;objFile.close();// 将顶点数据从vector转移到Eigen::MatrixXfpoints.resize(vertexList.size(), 3); // 初始化points矩阵for (size_t i = 0; i < vertexList.size(); ++i) {points(i, 0) = vertexList[i][0];points(i, 1) = vertexList[i][1];points(i, 2) = vertexList[i][2];//cout << points.row(i) << endl;}// 此时points矩阵已填充完成，可以继续使用它进行其他操作std::cout << "Loaded " << points.rows() << " vertices into the points matrix." << std::endl;return 1;}void transOBJToPLY(const std::string objFilePath,const std::string plyFilePath,Eigen::MatrixXf& points){// 创建一个PointCloud对象pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);// 填充点云数据cloud->width = points.rows();cloud->height = 1;cloud->is_dense = false;cloud->points.resize(cloud->width * cloud->height);for (size_t i = 0; i < cloud->points.size(); ++i) {cloud->points[i].x = points(i, 0);cloud->points[i].y = points(i, 1);cloud->points[i].z = points(i, 2);}// 保存PLY文件pcl::PLYWriter writer;writer.write(plyFilePath, *cloud);std::cout << "Successfully converted OBJ to PLY." << std::endl;}void saveOBJToPLY(const std::string file_source_reg, std::string& resultF, const std::string out_path, const std::string name){if (strcmp(file_source_reg.substr(file_source_reg.size() - 4, 4).c_str(), ".obj") == 0){Eigen::MatrixXf points0;int res = readOBJ(file_source_reg, points0);if (res){resultF = out_path + name + "reg_pc.ply";transOBJToPLY(file_source_reg, resultF, points0);}else{std::cout << "读取" << file_source_reg << "失败" << std::endl;}}}void view_display(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target, pcl::PointCloud<pcl::PointXYZ>::Ptr result){boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("PCL Viewer"));viewer->setBackgroundColor(0, 0, 0);对目标点云着色可视化 (red).pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(cloud_target, 255, 0, 0);//红色viewer->addPointCloud<pcl::PointXYZ>(cloud_target, target_color, "target cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");// 对配准点云着色可视化 (green).pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>output_color(result, 0, 255, 0);//绿色viewer->addPointCloud<pcl::PointXYZ>(result, output_color, "output_color");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "output_color");while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(1000));}}~ViewShow(){}
};#endif

在main.cpp增加了调用，同时取消了从命令行读取参数，改成固定的参数，方便测试，当然你想用也可以，直接把if(argc == 5)这段代码解开注释就行

#ifdef _HAS_STD_BYTE
#undef _HAS_STD_BYTE
#endif
#define _HAS_STD_BYTE 0
#include <iostream>
#include "ICP.h"
#include "io_pc.h"
#include "FRICP.h"
#include <time.h>
#include "view_show.hpp"
#include <io.h>
#include <direct.h>
int main(int argc, char const** argv)
{clock_t start, end;double totaltime;start = clock();typedef double Scalar;typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Vertices;typedef Eigen::Matrix<Scalar, 3, 1> VectorN;std::string file_source;std::string file_target;std::string file_init = "./data/";std::string res_trans_path;std::string out_path;bool use_init = false;MatrixXX res_trans;enum Method { ICP, AA_ICP, FICP, RICP, PPL, RPPL, SparseICP, SICPPPL } method = RICP;/*file_target = "./data/bunny_side2.obj";file_source = "./data/bunny_side1.obj";out_path = "./data/res/";method = Method::ICP;*/file_target = "./data/target.ply";file_source = "./data/source.ply";out_path = "./data/res2/";method = Method::ICP;if (_access(out_path.c_str(), 0) == -1)	//如果文件夹不存在_mkdir(out_path.c_str());/*if (argc == 5){file_target = argv[1];file_source = argv[2];out_path = argv[3];method = Method(std::stoi(argv[4]));}else if (argc == 4){file_target = argv[1];file_source = argv[2];out_path = argv[3];}else{std::cout << "Usage: target.ply source.ply out_path <Method>" << std::endl;std::cout << "Method :\n"<< "0: ICP\n1: AA-ICP\n2: Our Fast ICP\n3: Our Robust ICP\n4: ICP Point-to-plane\n"<< "5: Our Robust ICP point to plane\n6: Sparse ICP\n7: Sparse ICP point to plane" << std::endl;exit(0);}*/int dim = 3;//--- Model that will be rigidly transformed 将被严格转换的模型Vertices vertices_source, normal_source, src_vert_colors;read_file(vertices_source, normal_source, src_vert_colors, file_source);std::cout << "source: " << vertices_source.rows() << "x" << vertices_source.cols() << std::endl;//--- Model that source will be aligned to 源点云将与之对齐的模型Vertices vertices_target, normal_target, tar_vert_colors;read_file(vertices_target, normal_target, tar_vert_colors, file_target);std::cout << "target: " << vertices_target.rows() << "x" << vertices_target.cols() << std::endl;// scalingEigen::Vector3d source_scale, target_scale;source_scale = vertices_source.rowwise().maxCoeff() - vertices_source.rowwise().minCoeff();target_scale = vertices_target.rowwise().maxCoeff() - vertices_target.rowwise().minCoeff();double scale = std::max(source_scale.norm(), target_scale.norm());std::cout << "scale = " << scale << std::endl;vertices_source /= scale;vertices_target /= scale;/// De-meanVectorN source_mean, target_mean;source_mean = vertices_source.rowwise().sum() / double(vertices_source.cols());target_mean = vertices_target.rowwise().sum() / double(vertices_target.cols());vertices_source.colwise() -= source_mean;vertices_target.colwise() -= target_mean;double time;// set ICP parametersICP::Parameters pars;// set Sparse-ICP parametersSICP::Parameters spars;spars.p = 0.4;spars.print_icpn = false;/// Initial transformationif (use_init){MatrixXX init_trans;read_transMat(init_trans, file_init);init_trans.block(0, dim, dim, 1) /= scale;init_trans.block(0, 3, 3, 1) += init_trans.block(0, 0, 3, 3) * source_mean - target_mean;pars.use_init = true;pars.init_trans = init_trans;spars.init_trans = init_trans;}///--- Execute registrationstd::cout << "begin registration..." << std::endl;FRICP<3> fricp;double begin_reg = omp_get_wtime();double converge_rmse = 0;switch (method){case ICP:{pars.f = ICP::NONE;pars.use_AA = false;fricp.point_to_point(vertices_source, vertices_target, source_mean, target_mean, pars);res_trans = pars.res_trans;break;}case AA_ICP:{AAICP::point_to_point_aaicp(vertices_source, vertices_target, source_mean, target_mean, pars);res_trans = pars.res_trans;break;}case FICP:{pars.f = ICP::NONE;pars.use_AA = true;fricp.point_to_point(vertices_source, vertices_target, source_mean, target_mean, pars);res_trans = pars.res_trans;break;}case RICP:{pars.f = ICP::WELSCH;pars.use_AA = true;fricp.point_to_point(vertices_source, vertices_target, source_mean, target_mean, pars);res_trans = pars.res_trans;break;}case PPL:{pars.f = ICP::NONE;pars.use_AA = false;fricp.point_to_plane(vertices_source, vertices_target, normal_source, normal_target, source_mean, target_mean, pars);res_trans = pars.res_trans;break;}case RPPL:{pars.nu_end_k = 1.0 / 6;pars.f = ICP::WELSCH;pars.use_AA = true;fricp.point_to_plane_GN(vertices_source, vertices_target, normal_source, normal_target, source_mean, target_mean, pars);res_trans = pars.res_trans;break;}case SparseICP:{SICP::point_to_point(vertices_source, vertices_target, source_mean, target_mean, spars);res_trans = spars.res_trans;break;}case SICPPPL:{SICP::point_to_plane(vertices_source, vertices_target, normal_target, source_mean, target_mean, spars);res_trans = spars.res_trans;break;}}std::cout << "Registration done!" << std::endl;double end_reg = omp_get_wtime();time = end_reg - begin_reg;vertices_source = scale * vertices_source;out_path = out_path + "m" + std::to_string(method);Eigen::Affine3d res_T;res_T.linear() = res_trans.block(0, 0, 3, 3);res_T.translation() = res_trans.block(0, 3, 3, 1);res_trans_path = out_path + "trans.txt";std::ofstream out_trans(res_trans_path);res_trans.block(0, 3, 3, 1) *= scale;out_trans << res_trans << std::endl;out_trans.close();///--- Write result to file//std::string file_source_reg = out_path + "reg_pc.obj";std::string file_source_reg = out_path + "reg_pc.ply";write_file(file_source, vertices_source, normal_source, src_vert_colors, file_source_reg);end = clock();totaltime = (double)(end - start) / CLOCKS_PER_SEC;std::cout << "Time：" << totaltime << "s" << std::endl;//不管是obj还是ply文件，统一转换成ply进行可视化ViewShow v;std::string resultF = file_source_reg;std::string sourceF = file_source;std::string targetF = file_target;pcl::console::setVerbosityLevel(pcl::console::L_ERROR);pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);v.saveOBJToPLY(file_source_reg, resultF, out_path, "Result");v.saveOBJToPLY(file_target, targetF, out_path, "Target");v.saveOBJToPLY(file_source, sourceF, out_path, "Source");if (pcl::io::loadPLYFile<pcl::PointXYZ>(resultF, *result) == -1){PCL_ERROR("加载点云失败\n");}if (pcl::io::loadPLYFile<pcl::PointXYZ>(sourceF, *cloud) == -1){PCL_ERROR("加载点云失败\n");}if (pcl::io::loadPLYFile<pcl::PointXYZ>(targetF, *cloud_target) == -1){PCL_ERROR("加载点云失败\n");}v.view_display(cloud_target, cloud);v.view_display(cloud_target, result);return 0;
}

使用：

如果不从命令行读取数据，那就在这份代码的main.cpp里面修改，首先第一行定义了一个枚举类型，里面有很多配准方法FICP和RICP就是快速鲁棒ICP，并创建了method这个枚举类型的变量；第二行就是目标点云的路径，第三行就是输入点云的路径；第四行是你的输出文件夹路径，你要配准不同组的点云，最好换个名字，不然上一组点云的实验结果会被覆盖；第五行是我自己用来修改method的，方便测试不同的ICP。

这份代码可支持obj和ply点云文件

当你目标点云和输入点云都是ply点云文件时，这行代码的结尾也是ply，当你目标点云和输入点云都是obj点云文件时，这行代码的结尾也得是obj ，否则会出现报错

当你目标点云和输入点云都是obj点云文件时，data/res文件夹里面会有这些文件

如果不加入我的可视化代码，只是修改了头文件以及参数，那么只会产生配准完成的文件m0reg_pc.obj和m0trans.txt，但是由于我的可视化代码，是把文件都转换成ply再进行可视化，所以会多出三个ply文件，m0Resultreg_pc.ply对应的就是 m0reg_pc.obj，m0Sourcereg_pc.ply对应的就是输入点云，m0Targetreg_pc.ply对应的就是目标点云。

当你目标点云和输入点云都是ply点云文件时，data/res2文件夹里面会有这些文件