icp算法步骤_A*算法

SVD分解求解

SVD分解法网上原理有很多文章，例如：ICP算法公式推导和PCL源码解析

#include <iostream>
#include <ctime>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 
#include <pcl/correspondence.h>
#include <pcl/visualization/pcl_visualizer.h>


int main(int argc, char** argv)
{ 
   
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);

	//加载点云
	pcl::io::loadPCDFile("bunny1.pcd", *source_cloud);
	pcl::io::loadPCDFile("bunny3.pcd", *target_cloud);

	//建立kd树
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(target_cloud);

	//初始化变换矩阵等参数
	int iters = 0;
	double error = std::numeric_limits<double>::infinity();
	Eigen::Matrix3f R = Eigen::Matrix3f::Identity();
	Eigen::Vector3f T = Eigen::Vector3f::Zero();
	Eigen::Matrix4f H = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f H_final = H;
	Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
	pcl::copyPointCloud(*source_cloud, *source_cloud_mid);

	//开始迭代，直到满足条件
	clock_t start = clock();
	while (error > 0.0001 && iters < 100)
	{ 
   
		iters++;
		double last_error = error;
		double err = 0.0;
		pcl::transformPointCloud(*source_cloud_mid, *source_cloud_mid, H);
		std::vector<int>indexs(source_cloud_mid->size());

		for (int i = 0; i < source_cloud_mid->size(); ++i)
		{ 
   
			std::vector<int>index(1);
			std::vector<float>distance(1);
			kdtree->nearestKSearch(source_cloud_mid->points[i], 1, index, distance);
			err = err + sqrt(distance[0]);
			indexs[i] = index[0];
		}
		pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid);
		error = err / source_cloud->size();
		std::cout << "iters:" << iters << " " << "error:" << error << std::endl;

		if (fabs(last_error - error) < 1e-6)
			break;

		//计算点云中心坐标
		Eigen::Vector4f source_centroid, target_centroid_mid;
		pcl::compute3DCentroid(*source_cloud_mid, source_centroid);
		pcl::compute3DCentroid(*target_cloud_mid, target_centroid_mid);

		//去中心化
		Eigen::MatrixXf souce_cloud_demean, target_cloud_demean;
		pcl::demeanPointCloud(*source_cloud_mid, source_centroid, souce_cloud_demean);
		pcl::demeanPointCloud(*target_cloud_mid, target_centroid_mid, target_cloud_demean);

		//计算W=q1*q2^T
		Eigen::MatrixXf W = (souce_cloud_demean * target_cloud_demean.transpose()).topLeftCorner(3, 3);

		//SVD分解得到新的旋转矩阵和平移矩阵
		Eigen::JacobiSVD<Eigen::MatrixXf> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
		Eigen::MatrixXf U = svd.matrixU();
		Eigen::MatrixXf V = svd.matrixV();

		if (U.determinant() * V.determinant() < 0)
		{ 
   
			for (int x = 0; x < 3; ++x)
				V(x, 2) *= -1;
		}

		R = V * U.transpose();
		T = target_centroid_mid.head(3) - R * source_centroid.head(3);
		H << R, T, 0, 0, 0, 1;
		H_final = H * H_final; //更新变换矩阵 
	}
	transformation_matrix << H_final;

	clock_t end = clock();
	std::cout << end - start << "ms" << std::endl;
	std::cout << transformation_matrix << std::endl;

	//配准结果
	pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix);
	pcl::io::savePCDFileBinary("icp_cloud.pcd", *icp_cloud);

	//可视化
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); 	//原始点云绿色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); 	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); 	//匹配好的点云蓝色

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(source_cloud, src_h, "source cloud");
	viewer.addPointCloud(target_cloud, tgt_h, "target cloud");
	viewer.addPointCloud(icp_cloud, final_h, "result cloud");
	while (!viewer.wasStopped())
	{ 
   
		viewer.spinOnce(100);
	}

	return 0;
}

GN法求解

原理参考：ICP的不同解法：SVD分解，高斯牛顿法。代码实现

#include <iostream>
#include <ctime>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 
#include <pcl/correspondence.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "sophus/se3.hpp"

int main(int argc, char** argv)
{ 
   
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);

	//加载点云
	pcl::io::loadPCDFile("bunny1.pcd", *source_cloud);
	pcl::io::loadPCDFile("bunny3.pcd", *target_cloud);

	//建立kd树
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(target_cloud);

	//初始化变换矩阵等参数
	int iters = 0;
	Sophus::SE3f pose_gn;
	double error = std::numeric_limits<double>::infinity();
	Eigen::Matrix3f R = Eigen::Matrix3f::Identity();
	Eigen::Vector3f T = Eigen::Vector3f::Zero();
	Eigen::Matrix4f H = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f H_final = H;
	Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
	pcl::copyPointCloud(*source_cloud, *source_cloud_mid);

	//开始迭代，直到满足条件
	clock_t start = clock();

	while (error > 0.0001 && iters < 100)
	{ 
   
		iters++;
		double err = 0.0;
		pcl::transformPointCloud(*source_cloud_mid, *source_cloud_mid, H);
		std::vector<int>indexs(source_cloud_mid->size());
		for (int i = 0; i < source_cloud_mid->size(); ++i)
		{ 
   
			std::vector<int>index(1);
			std::vector<float>distance(1);
			kdtree->nearestKSearch(source_cloud_mid->points[i], 1, index, distance);
			err = err + sqrt(distance[0]);
			indexs[i] = index[0];
		}
		pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid);
		error = err / source_cloud->size();
		std::cout << "iters:" << iters << " " << "error:" << error << std::endl;

		Eigen::Matrix<float, 6, 6> h = Eigen::Matrix<float, 6, 6>::Zero();
		Eigen::Matrix<float, 6, 1> g = Eigen::Matrix<float, 6, 1>::Zero();
		for (int i = 0; i < target_cloud->size(); ++i)
		{ 
   
			Eigen::Vector3f p1_i = target_cloud_mid->points[i].getVector3fMap();
			Eigen::Vector3f p2_i = source_cloud_mid->points[i].getVector3fMap();
			Eigen::Vector3f p_n = pose_gn * p2_i;								// 经过pose转化后的点
			Eigen::Matrix<float, 3, 6> J = Eigen::Matrix<float, 3, 6>::Zero(); 	// 计算J
			J.block(0, 0, 3, 3) = Eigen::Matrix3f::Identity();					// 推导的jacobin中左边单位阵
			J.rightCols<3>() = -Sophus::SO3f::hat(p_n);							// 右边的李群
			J = -J;
			Eigen::Vector3f e = p1_i - p_n;										// 计算e
			h += J.transpose() * J;
			g += -J.transpose() * e;
		}

		Eigen::Matrix<float, 6, 1> dx = h.ldlt().solve(g);						// 求解dx
		pose_gn = Sophus::SE3f::exp(dx) * pose_gn;								// 进行更新，这里的dx是李代数
		H = pose_gn.matrix();
		H_final = H * H_final; //更新变换矩阵 
		//std::cout << H << std::endl;
	}
	transformation_matrix << H_final;

	clock_t end = clock();
	std::cout << end - start << "ms" << std::endl;
	std::cout << transformation_matrix << std::endl;

	//配准结果
	pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix);
	pcl::io::savePCDFileBinary("icp.pcd", *icp_cloud);

	//可视化
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); 	//原始点云绿色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); 	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); 	//匹配好的点云蓝色

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(source_cloud, src_h, "source cloud");
	viewer.addPointCloud(target_cloud, tgt_h, "target cloud");
	viewer.addPointCloud(icp_cloud, final_h, "result cloud");
	while (!viewer.wasStopped())
	{ 
   
		viewer.spinOnce(100);
	}

	return 0;
}

四元数求解

参考文献：
【1】改进的ICP算法在点云配准中的应用
【2】基于KDTree改进的ICP算法在点云配准中的应用研究

#include <iostream>
#include <ctime>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 
#include <pcl/correspondence.h>
#include <pcl/visualization/pcl_visualizer.h>


int main(int argc, char** argv)
{ 
   
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);

	//加载点云
	pcl::io::loadPCDFile("bunny1.pcd", *source_cloud);
	pcl::io::loadPCDFile("bunny3.pcd", *target_cloud);

	//建立kd树
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(target_cloud);

	//初始化变换矩阵等参数
	int iters = 0;
	double error = std::numeric_limits<double>::infinity();
	Eigen::Matrix3f R = Eigen::Matrix3f::Identity();
	Eigen::Vector3f T = Eigen::Vector3f::Zero();
	Eigen::Matrix4f H = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f H_final = H;
	Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
	pcl::copyPointCloud(*source_cloud, *source_cloud_mid);

	//开始迭代，直到满足条件
	clock_t start = clock();
	while (error > 0.0001 && iters < 100)
	{ 
   
		iters++;
		double last_error = error;
		double err = 0.0;
		pcl::transformPointCloud(*source_cloud_mid, *source_cloud_mid, H);
		std::vector<int>indexs(source_cloud_mid->size());

		for (int i = 0; i < source_cloud_mid->size(); ++i)
		{ 
   
			std::vector<int>index(1);
			std::vector<float>distance(1);
			kdtree->nearestKSearch(source_cloud_mid->points[i], 1, index, distance);
			err = err + sqrt(distance[0]);
			indexs[i] = index[0];
		}
		pcl::copyPointCloud(*target_cloud, indexs, *target_cloud_mid);
		error = err / source_cloud->size();
		std::cout << "iters:" << iters << " " << "error:" << error << std::endl;

		if (fabs(last_error - error) < 1e-6)
			break;

		//计算点云中心坐标
		Eigen::Vector4f source_centroid, target_centroid_mid;
		pcl::compute3DCentroid(*source_cloud_mid, source_centroid);
		pcl::compute3DCentroid(*target_cloud_mid, target_centroid_mid);

		//去中心化
		Eigen::MatrixXf souce_cloud_demean, target_cloud_demean;
		pcl::demeanPointCloud(*source_cloud_mid, source_centroid, souce_cloud_demean);
		pcl::demeanPointCloud(*target_cloud_mid, target_centroid_mid, target_cloud_demean);

		Eigen::Matrix3f W = (souce_cloud_demean * target_cloud_demean.transpose()).topLeftCorner(3, 3);
		Eigen::Matrix3f WT = (target_cloud_demean * souce_cloud_demean.transpose()).topLeftCorner(3, 3);
		Eigen::Matrix3f delta = W - WT;

		Eigen::Matrix4f Q = Eigen::Matrix4f::Zero();
		Q.topLeftCorner(1, 1) << W.trace();
		Q.topRightCorner(1, 3) << delta(1, 2), delta(2, 0), delta(0, 1);
		Q.bottomLeftCorner(3, 1) << delta(1, 2), delta(2, 0), delta(0, 1);
		Q.bottomRightCorner(3, 3) << W + WT - W.trace() * Eigen::Matrix3f::Identity();

		//for (int i = 0; i < source_cloud_mid->size(); ++i)
		//{ 
   
		// float x_mi = source_cloud_mid->points[i].x;
		// float x_ni = target_cloud_mid->points[i].x;
		// float y_mi = source_cloud_mid->points[i].y;
		// float y_ni = target_cloud_mid->points[i].y;
		// float z_mi = source_cloud_mid->points[i].z;
		// float z_ni = target_cloud_mid->points[i].z;

		// Q(0, 0) += x_mi * x_ni + y_mi * y_ni + z_mi * z_ni;
		// Q(1, 0) += y_mi * z_ni - z_mi * y_ni;
		// Q(2, 0) += z_mi * x_ni - x_mi * z_ni;
		// Q(3, 0) += x_mi * y_ni - y_mi * x_ni;
		// Q(0, 1) += y_mi * z_ni - z_mi * y_ni;
		// Q(1, 1) += x_mi * x_ni - y_mi * y_ni - z_mi * z_ni;
		// Q(2, 1) += x_mi * y_ni + y_mi * x_ni;
		// Q(3, 1) += x_mi * z_ni + z_mi * x_ni;
		// Q(0, 2) += z_mi * x_ni - x_mi * z_ni;
		// Q(1, 2) += x_mi * y_ni + y_mi * x_ni;
		// Q(2, 2) += - x_mi * x_ni + y_mi * y_ni - z_mi * z_ni;
		// Q(3, 2) += y_mi * z_ni + z_mi * y_ni;
		// Q(0, 3) += x_mi * y_ni - y_mi * x_ni;
		// Q(1, 3) += x_mi * z_ni + z_mi * x_ni;
		// Q(2, 3) += y_mi * z_ni + z_mi * y_ni;
		// Q(3, 3) += - x_mi * x_ni - y_mi * y_ni + z_mi * z_ni;
		//}

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix4f> eigen_solver(Q);
		Eigen::Vector4f eigenvalues = eigen_solver.eigenvalues(); //特征值一般按从小到大排列
		Eigen::Matrix4f eigenvectors = eigen_solver.eigenvectors();
		Eigen::Vector4f eigenvector = eigenvectors.col(3);

		float w = eigenvector(0), m = eigenvector(1), n = eigenvector(2), p = eigenvector(3);
		Eigen::Matrix3f R;
		R << 1 - 2 * (p * p + n * n), 2 * (m * n - w * p), 2 * (w * n + m * p),
			2 * (m * n + w * p), 1 - 2 * (m * m + p * p), 2 * (n * p - w * m),
			2 * (m * p - w * n), 2 * (n * p + w * m), 1 - 2 * (m * m + n * n);

		T = target_centroid_mid.head(3) - R * source_centroid.head(3);
		H << R, T, 0, 0, 0, 1;
		H_final = H * H_final; //更新变换矩阵 
	}
	transformation_matrix << H_final;

	clock_t end = clock();
	std::cout << end - start << "ms" << std::endl;
	std::cout << transformation_matrix << std::endl;

	//配准结果
	pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix);
	pcl::io::savePCDFileBinary("icp_cloud.pcd", *icp_cloud);

	//可视化
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); 	//原始点云绿色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); 	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); 	//匹配好的点云蓝色

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(source_cloud, src_h, "source cloud");
	viewer.addPointCloud(target_cloud, tgt_h, "target cloud");
	viewer.addPointCloud(icp_cloud, final_h, "result cloud");
	while (!viewer.wasStopped())
	{ 
   
		viewer.spinOnce(100);
	}

	return 0;
}

BA求解

原理参考：基于高斯牛顿的点-点，点-线，点到面ICP过程推导
视觉SLAM笔记（39） 求解 ICP
代码参考：slam14讲高博的代码https://github.com/gaoxiang12/slambook2/blob/master/ch7/pose_estimation_3d3d.cpp
在此基础上修改，直接读入pcd点云，加入了迭代求解。

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 
#include <pcl/visualization/pcl_visualizer.h>


// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{ 
   
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d& point) : _point(point) { 
   }

	virtual void computeError()
	{ 
   
		const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> (_vertices[0]);
		// measurement is p, point is p'
		_error = _measurement - pose->estimate().map(_point);
	}

	virtual void linearizeOplus()
	{ 
   
		g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
		g2o::SE3Quat T(pose->estimate());
		Eigen::Vector3d xyz_trans = T.map(_point);
		double x = xyz_trans[0];
		double y = xyz_trans[1];
		double z = xyz_trans[2];

		_jacobianOplusXi(0, 0) = 0;
		_jacobianOplusXi(0, 1) = -z;
		_jacobianOplusXi(0, 2) = y;
		_jacobianOplusXi(0, 3) = -1;
		_jacobianOplusXi(0, 4) = 0;
		_jacobianOplusXi(0, 5) = 0;

		_jacobianOplusXi(1, 0) = z;
		_jacobianOplusXi(1, 1) = 0;
		_jacobianOplusXi(1, 2) = -x;
		_jacobianOplusXi(1, 3) = 0;
		_jacobianOplusXi(1, 4) = -1;
		_jacobianOplusXi(1, 5) = 0;

		_jacobianOplusXi(2, 0) = -y;
		_jacobianOplusXi(2, 1) = x;
		_jacobianOplusXi(2, 2) = 0;
		_jacobianOplusXi(2, 3) = 0;
		_jacobianOplusXi(2, 4) = 0;
		_jacobianOplusXi(2, 5) = -1;
	}

	bool read(std::istream& in) { 
    return true; }
	bool write(std::ostream& out) const { 
    return true; }

protected:
	Eigen::Vector3d _point;
};


void bundleAdjustment(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pts1, const pcl::PointCloud<pcl::PointXYZ>::Ptr& pts2, Eigen::Matrix4d& T)
{ 
   
	// 初始化g2o
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Block;  // pose维度为 6, landmark 维度为 3
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>(); // 线性方程求解器
	Block* solver_ptr = new Block(linearSolver);      // 矩阵块求解器
	g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);

	// vertex
	g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
	pose->setId(0);
	pose->setEstimate(g2o::SE3Quat(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0, 0)));
	optimizer.addVertex(pose);

	// edges
	int index = 1;
	std::vector<EdgeProjectXYZRGBDPoseOnly*> edges;
	for (size_t i = 0; i < pts1->size(); i++)
	{ 
   
		EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2->points[i].x, pts2->points[i].y, pts2->points[i].z));
		edge->setId(index);
		edge->setVertex(0, dynamic_cast<g2o::VertexSE3Expmap*> (pose));
		edge->setMeasurement(Eigen::Vector3d(pts1->points[i].x, pts1->points[i].y, pts1->points[i].z));
		edge->setInformation(Eigen::Matrix3d::Identity() * 1e4);
		optimizer.addEdge(edge);
		index++;
		edges.push_back(edge);
	}

	optimizer.setVerbose(true);
	optimizer.initializeOptimization();
	optimizer.optimize(10);

	T = Eigen::Isometry3d(pose->estimate()).matrix();
}


int main(int argc, char** argv)
{ 
   
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_copy(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::io::loadPCDFile("bunny1.pcd", *source_cloud);
	pcl::io::loadPCDFile("bunny3.pcd", *target_cloud);
	pcl::copyPointCloud(*source_cloud, *icp_cloud);
	pcl::copyPointCloud(*target_cloud, *target_copy);

	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(target_cloud);

	std::vector<int>indexs(source_cloud->size());
	int iters = 0;
	double error = std::numeric_limits<double>::infinity();

	while (error > 0.0001 && iters < 100)		
	{ 
   
		iters++;
		double last_error = error;
		double err = 0.0;

		Eigen::Matrix4d transformation_matrix;
		bundleAdjustment(target_copy, icp_cloud, transformation_matrix);
		pcl::transformPointCloud(*icp_cloud, *icp_cloud, transformation_matrix);

		for (int i = 0; i < icp_cloud->size(); ++i)
		{ 
   
			std::vector<int>index(1);
			std::vector<float>distance(1);
			kdtree->nearestKSearch(icp_cloud->points[i], 1, index, distance);
			err = err + sqrt(distance[0]);
			indexs[i] = index[0];
		}
		pcl::copyPointCloud(*target_cloud, indexs, *target_copy);

		error = err / source_cloud->size();
		std::cout << "iters:" << iters << " " << "error:" << error << std::endl << std::endl;
		if (fabs(last_error - error) < 1e-6)
			break;
	}
	pcl::io::savePCDFileBinary("icp_cloud.pcd", *icp_cloud);

	//可视化
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); 	//原始点云绿色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); 	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); 	//匹配好的点云蓝色

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(source_cloud, src_h, "source cloud");
	viewer.addPointCloud(target_cloud, tgt_h, "target cloud");
	viewer.addPointCloud(icp_cloud, final_h, "result cloud");
	while (!viewer.wasStopped())
	{ 
   
		viewer.spinOnce(100);
	}

	return 0;
}

今天的文章icp算法步骤_A*算法分享到此就结束了，感谢您的阅读。