PCL点云匹配 3 之 Point-to-Plane ICP

Σίσυφος1900

已于 2023-12-26 09:44:24 修改

阅读量962

点赞数 12

分类专栏： 3D PCL 文章标签：算法

于 2023-12-20 17:18:00 首次发布

本文链接：https://blog.csdn.net/weixin_39354845/article/details/135109096

版权

PCL 同时被 2 个专栏收录

56 篇文章 16 订阅

订阅专栏

34 篇文章 26 订阅

订阅专栏

一、概述

3D【23】ICP配准：point-to-plane-CSDN博客

已经证明 Point -to-Plane 算法已经比Point-to-Point 的误差都要快更准确一些，在 ICP 算法的每次选代中，产生最小点到平面误差的相对位姿变化通常使用标准的非线性最小二乘法来解决。例如 Levenberg-Marquardt方法。当使用点到平面误差度量时，最小化的对象是每个源点与其对应目标点的切平面之间的平方距离之和。

二、原理介绍

那么我们可以计算间距最小函数，即loss 函数

$J=min\sum((R*Pi+T-Qi)*Ni) ^{2}$

参数注释：

R：表示的是旋转矩阵

T：表示的是平移矩阵

Pi：表示的是soruce 点云

Qi：表示的是target 点云

Ni：表示的是target点云该点对于的法向量

推导：

点云配准: 刚性ICP中 Point-to-Point 和 Point-to-Plane 公式推导 ==> 帮助你代码实现 - 知乎

准备1

$J=min\sum((R*Pi+T-Qi)*Ni) ^{2}$

点云刚性配准：point2point 和 point2plane 代码 - 知乎

void ICP::point2plane(float weight, Eigen::MatrixXf& A, Eigen::VectorXf& b)
{

	A.resize(m_srcOfPair.size(), 6);
	b.resize(m_srcOfPair.size());

	for (int j = 0; j < m_srcOfPair.size(); j++)
	{
		Eigen::Vector3f sourceCoordinates;
		sourceCoordinates = m_srcOfPair[j];

		Eigen::Vector3f targetCoordinates;
		targetCoordinates = m_targetOfPair[j];

		Eigen::Vector3f targetNormal;
		targetNormal = m_targetNormalsOfPair[j];

		//两个向量的叉乘结果是一个向量（三个数）
		Eigen::Vector3f sourceMultiplyNormal;
		sourceMultiplyNormal = sourceCoordinates.cross(targetNormal);

		A(j, 0) = sourceMultiplyNormal[0];
		A(j, 1) = sourceMultiplyNormal[1];
		A(j, 2) = sourceMultiplyNormal[2];
		A(j, 3) = targetNormal[0];
		A(j, 4) = targetNormal[1];
		A(j, 5) = targetNormal[2];

		b[j] = -(((sourceCoordinates - targetCoordinates).transpose()).dot(targetNormal));
	}

	A *= weight;
	b *= weight;

}