一、代码下载以、修改以及使用
链接：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::MatrixXf
points.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;
// scaling
Eigen::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-mean
VectorN 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 parameters
ICP::Parameters pars;
// set Sparse-ICP parameters
SICP::Parameters spars;
spars.p = 0.4;
spars.print_icpn = false;
/// Initial transformation
if (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 registration
std::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文件夹里面会有这些文件
由于生成的是ply文件，我的可视化代码并不会对其做出改变
结果 ：
这是一组obj点云文件
配准前，为了方便观察，我用鼠标转了个方向
配准后，为了方便观察，我用鼠标转了个方向
这是一组ply点云文件
配准前
配准后
感觉效果不错，你可以尝试选用其他ICP方法进行配准
