ICP算法的三种实现方法（SVD分解、四元数、BA）

企业开发 2023-05-18 01:38:02 阅读次数: 0

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;
}

四元数求解

参考文献：
【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;
}

猜你喜欢

转载自blog.csdn.net/taifyang/article/details/129766050
今日推荐
NetBSD 禁止提交由 AI 生成的代码
Apache Doris 2.0.10 版本正式发布！
开源日报 | 大模型开战；大模型独角兽被曝卖身；周鸿祎建议谷歌开源所有产品；最大开源AI社区提供1000万美元共享GPU
开源日报 | Chrome内置Gemini的意义不在于Gemini；中国AI追随之路的五大误区；ECharts创始人“下海”养鱼；谷歌I/O开发者大会什么都有，只是没有惊喜
微软回应中国区AI团队“打包赴美”传闻
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
周排行
static方法和非static方法的区别（java）
如何查找计算机专业paper
java.lang.ClassFormatError: Incompatible magic value 0 in class file com/sitecha
跳跃游戏II
stm32_之【建立工程】
TeaWeb v0.0.9 发布，统计底层优化、主机监控功能改进
事件分发 -----控制字体大小
JavaScript DOM练习（动态表格添加） December 25，2019
JSF Scope & CDI
实现从零搭建一个登录注册页面（附源代码）
每日归档
更多
2024-05-19(0)
2024-05-18(4)
2024-05-17(34)
2024-05-16(6)
2024-05-15(24)
2024-05-14(0)
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022