一、代码下载以及修改
下载以及建立项目：
链接：palanglois/icpSparse: Implementation of the sparse icp algorithm (github.com)
如果github进不去，我这里下载好了：Sparseicp源码资源-CSDN文库
下载好了之后，会有这些文件
首先visual studio项目，配置好PCL环境；ext文件夹包含了源码的依赖项Eigen和NanoFlann以及OptionParser，因为PCL自带Eigen所以ext文件夹的Eigen直接不要了，只要把NanoFlann和OptionParser的所有以cpp、h、hpp结尾的文件放入你的项目；lib文件夹则包含IcpOptimizer和ObjLoader，把只要把IcpOptimizer和ObjLoader的所有以cpp、h、hpp结尾的文件放入你的项目；接着把media放到你的项目路径下，media里面都是实验要用到的数据；最后把icpSparseDemo.h和main.cpp也放入项目即可；CMakeLists.txt不用放入项目，毕竟我是在windows平台，又不是在ubuntu。
修改 ：
由于源码没有可视化代码，我这里修改了下，使得可以进行可视化，只要修改下面四个文件就行
ObjLoader.h
#ifndef OBJECT_LOADER_H
#define OBJECT_LOADER_H
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <sstream>
#include <algorithm>
#include <iterator>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
class ObjectLoader
{
public:
ObjectLoader();
Eigen::Matrix<double, Eigen::Dynamic, 3> operator()(std::string filePath);
void dumpToFile(Eigen::Matrix<double, Eigen::Dynamic, 3> vertice, Eigen::Matrix<double, Eigen::Dynamic, 3> normals, std::string filePath, string name);
private:
};
#endif
ObjLoader.cpp
#include "ObjLoader.h"
ObjectLoader::ObjectLoader()
{
}
/*This function loads the vertice contained in the 3d .obj file located in filePath
*/
Matrix<double, Dynamic, 3> ObjectLoader::operator()(string filePath)
{
//Counting number of vertice in file
ifstream fileCount(filePath.c_str());
int nbVertice = 0;
for (std::string line; std::getline(fileCount, line); )
{
if (line[0] == 'v' && line[1] == ' ')
nbVertice++;
}
fileCount.close();
//Filling the point cloud
Eigen::Matrix<double, Eigen::Dynamic, 3> pointCloud;
pointCloud.resize(nbVertice, 3);
ifstream filein(filePath.c_str());
int iterator = 0;
for (std::string line; std::getline(filein, line); )
{
if (line[0] != 'v' || line[1] != ' ')
continue;
istringstream iss(line);
vector<string> itemsInString{ istream_iterator<string>{iss},
istream_iterator<string>{} };
if (itemsInString.size() != 4)
{
cout << "Problem when parsing vertex in file : " << filePath << endl;
continue;
}
pointCloud(iterator, 0) = stod(itemsInString[1]);
pointCloud(iterator, 1) = stod(itemsInString[2]);
pointCloud(iterator, 2) = stod(itemsInString[3]);
iterator++;
}
cout << "Point Cloud loaded with " << pointCloud.rows() << " vertice." << endl;
filein.close();
return pointCloud;
}
/* This function writes a .ply file at filePath that contains the vertice and normals as specified in the input variables
*/
void ObjectLoader::dumpToFile(Eigen::Matrix<double, Eigen::Dynamic, 3> vertice, Eigen::Matrix<double, Eigen::Dynamic, 3> normals, std::string filePath, string name)
{
ofstream fileout(filePath.c_str());
fileout << "ply" << endl
<< "format ascii 1.0" << endl
<< "element vertex " << vertice.rows() << endl
<< "property float x" << endl
<< "property float y" << endl
<< "property float z" << endl;
//Writing header
if (name == "result")
{
fileout << "property float nx" << endl
<< "property float ny" << endl
<< "property float nz" << endl;
}
fileout << "end_header" << endl;
//Writing the vertice and normals
for (int i = 0; i < vertice.rows(); i++)
{
if (name == "result")
{
fileout << vertice(i, 0) << " " << vertice(i, 1) << " " << vertice(i, 2) << " " << normals(i, 0) << " " << normals(i, 1) << " " << normals(i, 2) << endl;
}
else if (name == "source"|| name == "target")
{
fileout << vertice(i, 0) << " " << vertice(i, 1) << " " << vertice(i, 2) << endl;
}
}
//fileout << vertice(i, 0) << " " << vertice(i, 1) << " " << vertice(i, 2) << " " << normals(i, 0) << " " << normals(i, 1) << " " << normals(i, 2) << endl;
}
icpSparseDemo.h
#ifndef ICPSPARSEDEMO_H
#define ICPSPARSEDEMO_H
#include <iostream>
#include "ObjLoader.h"
#include "IcpOptimizer.h"
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using namespace std;
using namespace Eigen;
class IcpSparseDemo
{
public:
IcpSparseDemo()
{
}
void view_show(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));
}
}
/*
Run the demo
*/
void run()
{
/// First test with bunny /
cout << "Running demo for the bunny example" << endl;
//Parameters
size_t kNormals = 10; //Number of nearest neighbours in order to estimate the normals
int nbIterations = 25; //Number of iterations for the algorithm
int nbIterationsIn = 2; //Number of iterations for the step 2 of the algorithm
double mu = 10.; //Parameter for step 2.1
int nbIterShrink = 3; //Number of iterations for shrink step (2.1 also)
double p = 0.5; //We use the norm L_p
bool verbose = false; //Verbosity trigger
IcpMethod method = pointToPlane; //Underlying ICP method
//Finding the media directory
string mediaDir = string("media/");
mediaDir = mediaDir.substr(1, mediaDir.length() - 2);
//Loading the point clouds
ObjectLoader myLoader;
Matrix<double, Dynamic, 3> pointCloudOne = myLoader(mediaDir + "bunny_side1.obj");
Matrix<double, Dynamic, 3> pointCloudTwo = myLoader(mediaDir + "bunny_side2.obj");
//Creating an IcpOptimizer in order to perform the sparse icp
IcpOptimizer* myIcpOptimizer = new IcpOptimizer(pointCloudOne, pointCloudTwo, kNormals, nbIterations, nbIterationsIn, mu, nbIterShrink, p, method, verbose);
//Perform ICP
myIcpOptimizer->performSparceICP();
PointCloud resultingCloud = myIcpOptimizer->getMovedPointCloud();
myLoader.dumpToFile(resultingCloud, myIcpOptimizer->getMovedNormals(), mediaDir + "bunny_ICP_test.ply", "result");
myIcpOptimizer->saveIter(mediaDir + "bunny_ICP_test.txt");
cout << "Resulting point cloud is in media/bunny_ICP_test.ply" << endl;
delete myIcpOptimizer;
/// Second test with bunny + 1000 noisy points per file
cout << "Running demo for the bunny + 1000 noisy points per files example" << endl;
//Parameters
kNormals = 10; //Number of nearest neighbours in order to estimate the normals
nbIterations = 25; //Number of iterations for the algorithm
nbIterationsIn = 2; //Number of iterations for the step 2 of the algorithm
mu = 10.; //Parameter for step 2.1
nbIterShrink = 3; //Number of iterations for shrink step (2.1 also)
p = 0.5; //We use the norm L_p
verbose = false; //Verbosity trigger
method = pointToPlane; //Underlying ICP method
//Loading the point clouds
pointCloudOne = myLoader(mediaDir + "bunny_noised1.obj");
pointCloudTwo = myLoader(mediaDir + "bunny_noised2.obj");
//Creating an IcpOptimizer in order to perform the sparse icp
IcpOptimizer* myIcpOptimizer2 = new IcpOptimizer(pointCloudOne, pointCloudTwo, kNormals, nbIterations, nbIterationsIn, mu, nbIterShrink, p, method, verbose);
//Perform ICP
myIcpOptimizer2->performSparceICP();
resultingCloud = myIcpOptimizer2->getMovedPointCloud();
myLoader.dumpToFile(resultingCloud, myIcpOptimizer2->getMovedNormals(), mediaDir + "bunny_noised_ICP.ply", "result");
myIcpOptimizer2->saveIter(mediaDir + "bunny_noised_ICP.txt");
cout << "Resulting point cloud is in media/bunny_noised_ICP.ply" << endl;
delete myIcpOptimizer2;
}
private:
};
#endif
main.cpp
#ifdef _HAS_STD_BYTE
#undef _HAS_STD_BYTE
#endif
#define _HAS_STD_BYTE 0
#include <iostream>
#include "ObjLoader.h"
#include "IcpOptimizer.h"
#include "option_parser.h"
#include "icpSparseDemo.h"
#include <io.h>
#include <direct.h>
int main(int argc, char* argv[])
{
//Create parsing options
op::OptionParser opt;
opt.add_option("-h", "--help", "show option help");
opt.add_option("-d", "--demo", "demo mode (uses integrated media)");
opt.add_option("-i1", "--input_1", "Path to first input obj file (REQUIRED)", "media/bunny_side1.obj");
opt.add_option("-i2", "--input_2", "Path to second input obj file (REQUIRED)", "media/bunny_side2.obj");
opt.add_option("-o", "--output", "Path to the output directory (REQUIRED)", "media/myout_bunny");
opt.add_option("-n", "--name", "Name of the output file", "output");
opt.add_option("-k", "--k_normals", "knn parameter for normals computation", "10");//10
opt.add_option("-n1", "--n_iterations_1", "Nb of iterations for the algorithm", "25");//25
opt.add_option("-n2", "--n_iterations_2", "Nb of iterations for the algorithm's step 2", "2");//2
opt.add_option("-mu", "--mu", "Parameter for step 2.1", "10");//10
opt.add_option("-ns", "--n_iterations_shrink", "Number of iterations for shrink step (2.1)", "3");//3
//opt.add_option("-p", "--p_norm", "Use of norm L_p", "0.6");//0.6是适合兔子的参数
opt.add_option("-p", "--p_norm", "Use of norm L_p", "2");//2是适合牛和猪的参数
opt.add_option("-po", "--point_to_point", "Use point to point variant", "true");
opt.add_option("-pl", "--point_to_plane", "Use point to plane variant");
opt.add_option("-v", "--verbose", "Verbosity trigger");
//Parsing options
bool correctParsing = opt.parse_options(argc, argv);
if (!correctParsing)
return EXIT_FAILURE;
//Parameters
const string first_path = opt["-i1"];
const string second_path = opt["-i2"];
string output_path = opt["-o"];
size_t kNormals = op::str2int(opt["-k"]);
const int nbIterations = op::str2int(opt["-n1"]);
const int nbIterationsIn = op::str2int(opt["-n2"]);
const double mu = op::str2double(opt["-mu"]);
const int nbIterShrink = op::str2int(opt["-ns"]);
const double p = op::str2double(opt["-p"]);
const bool verbose = op::str2bool(opt["-v"]);
const bool demoMode = op::str2bool(opt["-d"]);
const bool hasHelp = op::str2bool(opt["-h"]);
const bool isPointToPoint = op::str2bool(opt["-po"]);
const bool isPointToPlane = op::str2bool(opt["-pl"]);
//Making checks
IcpSparseDemo demo;
if (demoMode)
{
//IcpSparseDemo demo;
demo.run();
return 0;
}
if (hasHelp)
{
opt.show_help();
return 0;
}
if (first_path == "")
{
cerr << "Please specify the path of the first object file." << endl;
opt.show_help();
return EXIT_FAILURE;
}
if (second_path == "")
{
cerr << "Please specify the path of the second object file." << endl;
opt.show_help();
return EXIT_FAILURE;
}
if (output_path == "")
{
cerr << "Please specify the path of the output directory." << endl;
opt.show_help();
return EXIT_FAILURE;
}
if (_access(output_path.c_str(), 0) == -1) //如果文件夹不存在
_mkdir(output_path.c_str());
if (output_path[output_path.size() - 1] != '/')
output_path.append("/");
string output_pc_path = output_path + opt["-n"] + ".ply";
string output_iter_path = output_path + opt["-n"] + ".txt";
string target_pc_path = output_path + opt["-n"] + "Target" + ".ply";
string source_pc_path = output_path + opt["-n"] + "Source" + ".ply";
if (isPointToPlane && isPointToPoint)
{
cerr << "Please choose only one ICP method !" << endl;
opt.show_help();
return EXIT_FAILURE;
}
IcpMethod method = pointToPoint;
if (isPointToPlane)
method = pointToPlane;
else if (isPointToPoint)
method = pointToPoint;
else
{
cerr << "Please choose at least one ICP method (point to point or point to plane)." << endl;
opt.show_help();
return EXIT_FAILURE;
}
//Loading the point clouds
ObjectLoader myLoader;
Matrix<double, Dynamic, 3> pointCloudOne = myLoader(first_path);
Matrix<double, Dynamic, 3> pointCloudTwo = myLoader(second_path);
//Creating an IcpOptimizer in order to perform the sparse icp
IcpOptimizer myIcpOptimizer(pointCloudOne, pointCloudTwo, kNormals, nbIterations, nbIterationsIn, mu, nbIterShrink, p, method, verbose);
//Perform ICP
int hasIcpFailed = myIcpOptimizer.performSparceICP();
if (hasIcpFailed)
{
cerr << "Failed to load the point clouds. Check the paths." << endl;
return EXIT_FAILURE;
}
PointCloud resultingCloud = myIcpOptimizer.getMovedPointCloud();
//Save the resulting point cloud and iterations report
myLoader.dumpToFile(resultingCloud, myIcpOptimizer.getMovedNormals(), output_pc_path, "result");
myIcpOptimizer.saveIter(output_iter_path);
myLoader.dumpToFile(pointCloudTwo, myIcpOptimizer.getMovedNormals(), target_pc_path, "target");
myLoader.dumpToFile(pointCloudOne, myIcpOptimizer.getMovedNormals(), source_pc_path, "source");
RigidTransfo resultingTransfo = myIcpOptimizer.getComputedTransfo();
cout << "Computed Rotation : " << endl << resultingTransfo.first << endl << "Computed Translation : " << endl << resultingTransfo.second << endl;
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>);
if (pcl::io::loadPLYFile<pcl::PointXYZ>(output_pc_path, *result) == -1)
{
PCL_ERROR("加载点云失败\n");
}
if (pcl::io::loadPLYFile<pcl::PointXYZ>(source_pc_path, *cloud) == -1)
{
PCL_ERROR("加载点云失败\n");
}
if (pcl::io::loadPLYFile<pcl::PointXYZ>(target_pc_path, *cloud_target) == -1)
{
PCL_ERROR("加载点云失败\n");
}
//配准前
demo.view_show(cloud_target, cloud);
//配准后
demo.view_show(cloud_target, result);
return 0;
}
使用：
main.cpp前面两行分别是用来设置输入点云和目标点云；第三行表示配准后的结果点云放置的文件夹位置，如果你用的是源码这个文件夹要提前建好，如果用的是我修改过的代码就不需要了,如果你想换一组点云配准，你最好把文件夹名字换个别的，不然会把之前点云配准结果覆盖掉；
第四行则是用来给点云命名的，就比如这行填的是output，下面的文件都是以output开头；这里我解释下为什么有四个文件，因为本人修改了源码，如果按源码来的话，只有两个文件，即output.ply和output.txt
main.cpp这两行决定配准是用点对点还是点对面的模式进行配准，选哪一个，就在哪个后面加上“true”，源码这里是一个“true”都没有，编译可以通过，但是根本不会帮你配准，所以这里一定要改
icpSparseDemo.h这里要修改文件路径，改成你自己的media存放的路径，我是放在我的项目文件夹里了，直接用相对路径
这几步改完之后，无论是源码或者我修改后的代码都可以运行
结果：
配准前
配准后，配准效果不错
配准前，这里我特意用鼠标转了个角度，方便看清
配准后，配准效果还行
配准前
配准后，感觉效果不太好，可能还得调整参数
