公众号致力于分享点云处理，SLAM，三维视觉，高精地图相关的文章与技术，欢迎各位加入我们，一起交流一起进步。本文来自点云PCL博主的分享，未经作者允许请勿转载，欢迎各位同学积极分享和交流。
前言
一些小伙伴说“感觉CloudCompare中的点云配准要比PCL中的配准效果要好”，这是为什么呢？这里先说一下我的大致的理解，从算法实现上，虽然CC也是使用了ICP算法，但是在ICP基础上进行了改进，让其更具有通用性，具体实现细节咱们一会一起看看代码，改进的ICP算法采用了一些特殊的策略或优化来适应一些特定的应用场景。而PCL库提供了多种点云配准算法的实现，包括ICP（Iterative Closest Point）、NDT（Normal Distributions Transform）等，这些算法在实现和性能上可能与CloudCompare的算法有所不同，因为CloudCompare对ICP算法进行了一些默认参数的调优，以适应一般情况下的配准需求。相比较而言PCL提供了很大的灵活性，用户可以对配准算法的参数进行精细的调整。这种自由度对于专业用户可能是一项优势，但也需要用户对算法有更深入的理解。
所以说所有的点云的算法一定是根据点云的属性，比如点云的有序性，以及点云的稀疏程度，噪声大小，在调用PCL的算法的时候一定要学会调整参数进行适配，所以在实际应用中，选择合适的配准工具和参数通常需要根据具体的应用场景和数据特点进行实验和调整。
CC中的配准函数
定义 ICPRegistrationTools 类的 Register 方法
代码语言：javascript复制ICPRegistrationTools::RESULT_TYPE ICPRegistrationTools::Register(
GenericIndexedCloudPersist* inputModelCloud,
GenericIndexedMesh* inputModelMesh,
GenericIndexedCloudPersist* inputDataCloud,
const Parameters& params,
ScaledTransformation& transform,
double& finalRMS,
unsigned& finalPointCount,
GenericProgressCallback* progressCb /*=0*/)该函数的步骤我做了一些的步骤说明：
（1）首先检查点云是否为空
（2）为了更好的配准，我们肯定希望使用与用户最初定义的点数相同的数量，但是如果数量比较大，肯定影响效率，所以如果输入数据点云太大，这里进行随机采样以提高速度，这里我们注意到dataSamplingLimit默认为50000，当大于这个最大点数，调用该函数进行随机采样，同时需要对点云的权重进行重采样
代码语言：javascript复制data.cloud = CloudSamplingTools::subsampleCloudRandomly(inputDataCloud, dataSamplingLimit);（3）对输入点云进行八叉树构建 ,默认是8个层级
代码语言：javascript复制unsigned char meshDistOctreeLevel = 8;
找到最适合与模型八叉树级别
meshDistOctreeLevel = dataOctree.findBestLevelForComparisonWithOctree(&modelOctree); }（4）同样对模型也需要同样的处理，点云数量，权重，以及八叉树构建
（5）计算两个点云之间的初始距离（同时也计算 CPSet）
（6）判断是否应该移除最远的点，这里需要计算数据点云的距离分布参数
代码语言：javascript复制 NormalDistribution N;
N.computeParameters(data.cloud);
// 获取均值和方差
N.getParameters(mu, sigma2);
// 计算最大距离
ScalarType maxDistance = static_cast(mu 2.5 * sqrt(sigma2));（7）然后保留距离不太高的点云，并对重叠点云的距离进行并行排序，计算每个 point 的权重值
（8）现在已经选择了将用于配准的点云，如果使用权重，必须计算加权 RMS均方根误差，如果权重无效直接跳过。
（9）ICP 的目标就是保证平方距离和的减小（不保证距离和的减小）
(10) 迭代点云配准的过程函数 RegistrationProcedure，点云配准停止的条件如下：
代码语言：javascript复制 if ((params.convType == MAX_ERROR_CONVERGENCE && deltaRMS < params.minRMSDecrease) || (params.convType == MAX_ITER_CONVERGENCE && iteration >= params.nbMaxIterations))
{ result = ICP_APPLY_TRANSFO;
break;
}（11）过滤平移矩阵
代码语言：javascript复制FilterTransformation(currentTrans, params.transformationFilters, currentTrans);（12）计算得到转化矩阵后应用到转化成新的点云，并计算其到模型的距离
一次迭代过程的函数RegistrationProcedure注释
代码语言：javascript复制// 配准过程，用于计算数据点云 P 和模型点云 X 之间的变换
bool RegistrationTools::RegistrationProcedure(GenericCloud* P, // data
GenericCloud* X, // model
ScaledTransformation& trans,
bool adjustScale/*=false*/,
ScalarField* coupleWeights/*=0*/,
PointCoordinateType aPrioriScale/*=1.0f*/)
{
// 结果的变换矩阵（R 在初始化时无效，T 为 (0,0,0)，s 为 1）
trans.R.invalidate();
trans.T = CCVector3(0, 0, 0);
trans.s = PC_ONE;
// 检查数据点云和模型点云是否有效，大小是否相等，且点数不少于3
if (P == nullptr || X == nullptr || P->size() != X->size() || P->size() < 3)
return false;
// 计算质心
CCVector3 Gp = coupleWeights ? GeometricalAnalysisTools::ComputeWeightedGravityCenter(P, coupleWeights) : GeometricalAnalysisTools::ComputeGravityCenter(P);
CCVector3 Gx = coupleWeights ? GeometricalAnalysisTools::ComputeWeightedGravityCenter(X, coupleWeights) : GeometricalAnalysisTools::ComputeGravityCenter(X);
// 特殊情况：只有3个点
if (P->size() == 3)
{
// 计算第一组法线
P->placeIteratorAtBeginning();
const CCVector3* Ap = P->getNextPoint();
const CCVector3* Bp = P->getNextPoint();
const CCVector3* Cp = P->getNextPoint();
CCVector3 Np(0, 0, 1);
{
Np = (*Bp - *Ap).cross(*Cp - *Ap);
double norm = Np.normd();
if (norm < ZERO_TOLERANCE)
return false;
Np /= static_cast(norm);
}
// 计算第二组法线
X->placeIteratorAtBeginning();
const CCVector3* Ax = X->getNextPoint();
const CCVector3* Bx = X->getNextPoint();
const CCVector3* Cx = X->getNextPoint();
CCVector3 Nx(0, 0, 1);
{
Nx = (*Bx - *Ax).cross(*Cx - *Ax);
double norm = Nx.normd();
if (norm < ZERO_TOLERANCE)
return false;
Nx /= static_cast(norm);
}
// 现在旋转简单地从 Nx 到 Np，以 Gx 为中心
CCVector3 a = Np.cross(Nx);
if (a.norm() < ZERO_TOLERANCE)
{
trans.R = CCLib::SquareMatrix(3);
trans.R.toIdentity();
if (Np.dot(Nx) < 0)
{
trans.R.scale(-PC_ONE);
}
}
else
{
double cos_t = Np.dot(Nx);
assert(cos_t > -1.0 && cos_t < 1.0); //
double cos_half_t = sqrt((1 cos_t) / 2);
double sin_half_t = sqrt((1 - cos_t) / 2);
double q[4] = {cos_half_t, a.x * sin_half_t, a.y * sin_half_t, a.z * sin_half_t};
// 归一化四元数
double qnorm = q[0] * q[0] q[1] * q[1] q[2] * q[2] q[3] * q[3];
assert(qnorm >= ZERO_TOLERANCE);
qnorm = sqrt(qnorm);
q[0] /= qnorm;
q[1] /= qnorm;
q[2] /= qnorm;
q[3] /= qnorm;
trans.R.initFromQuaternion(q);
}
if (adjustScale)
{
double sumNormP = (*Bp - *Ap).norm() (*Cp - *Bp).norm() (*Ap - *Cp).norm();
sumNormP *= aPrioriScale;
if (sumNormP < ZERO_TOLERANCE)
return false;
double sumNormX = (*Bx - *Ax).norm() (*Cx - *Bx).norm() (*Ax - *Cx).norm();
trans.s = static_cast(sumNormX / sumNormP);
}
// 推导第一个平移
trans.T = Gx - (trans.R * Gp) * (aPrioriScale * trans.s);
// 现在需要在（X）平面上找到旋转
{
CCVector3 App = trans.apply(*Ap);
CCVector3 Bpp = trans.apply(*Bp);
CCVector3 Cpp = trans.apply(*Cp);
double C = 0;
double S = 0;
CCVector3 Ssum(0, 0, 0);
CCVector3 rx;
CCVector3 rp;
rx = *Ax - Gx;
rp = App - Gx;
C = rx.dot(rp);
Ssum = rx.cross(rp);
rx = *Bx - Gx;
rp = Bpp - Gx;
C = rx.dot(rp);
Ssum = rx.cross(rp);
rx = *Cx - Gx;
rp = Cpp - Gx;
C = rx.dot(rp);
Ssum = rx.cross(rp);
S = Ssum.dot(Nx);
double Q = sqrt(S * S C * C);
if (Q < ZERO_TOLERANCE)
return false;
PointCoordinateType sin_t = static_cast(S / Q);
PointCoordinateType cos_t = static_cast(C / Q);
PointCoordinateType inv_cos_t = 1 - cos_t;
const PointCoordinateType& l1 = Nx.x;
const PointCoordinateType& l2 = Nx.y;
const PointCoordinateType& l3 = Nx.z;
PointCoordinateType l1_inv_cos_t = l1 * inv_cos_t;
PointCoordinateType l3_inv_cos_t = l3 * inv_cos_t;
SquareMatrix R(3);
// 第1列
R.m_values[0][0] = cos_t l1 * l1_inv_cos_t;
R.m_values[0][1] = l2 * l1_inv_cos_t l3 * sin_t;
R.m_values[0][2] = l3 * l1_inv_cos_t - l2 * sin_t;
// 第2列
R.m_values[1][0] = l2 * l1_inv_cos_t - l3 * sin_t;
R.m_values[1][1] = cos_t l2 * l2 * inv_cos_t;
R.m_values[1][2] = l2 * l3_inv_cos_t l1 * sin_t;
// 第3列
R.m_values[2][0] = l3 * l1_inv_cos_t l2 * sin_t;
R.m_values[2][1] = l2 * l3_inv_cos_t - l1 * sin_t;
R.m_values[2][2] = cos_t l3 * l3_inv_cos_t;
trans.R = R * trans.R;
trans.T = Gx - (trans.R * Gp) * (aPrioriScale * trans.s); // 更新 T
}
}
else
{
CCVector3 bbMin;
CCVector3 bbMax;
X->getBoundingBox(bbMin, bbMax);
// 如果数据点云等效于单个点（例如，在ICP过程中两个点云相距很远），我们尝试让两个点云靠近
CCVector3 diag = bbMax - bbMin;
if (std::abs(diag.x) std::abs(diag.y) std::abs(diag.z) < ZERO_TOLERANCE)
{
trans.T = Gx - Gp * aPrioriScale;
return true;
}
// 交叉协方差矩阵，参见Besl92中的方程＃24（但如果有权重，则包含权重）
SquareMatrixd Sigma_px = (coupleWeights ? GeometricalAnalysisTools::ComputeWeightedCrossCovarianceMatrix(P, X, Gp, Gx, coupleWeights)
: GeometricalAnalysisTools::ComputeCrossCovarianceMatrix(P, X, Gp, Gx));
if (!Sigma_px.isValid())
return false;
// 转置 sigma_px
SquareMatrixd Sigma_px_t = Sigma_px.transposed();
SquareMatrixd Aij = Sigma_px - Sigma_px_t;
double trace = Sigma_px.trace(); // 即 sigma_px 的对角元素之和
SquareMatrixd traceI3(3); // 创建带有特征值等于 trace 的 I 矩阵
traceI3.m_values[0][0] = trace;
traceI3.m_values[1][1] = trace;
traceI3.m_values[2][2] = trace;
SquareMatrixd bottomMat = Sigma_px Sigma_px_t - traceI3;
// 计算交叉协方差矩阵的下半部分
SquareMatrixd bottomMat = Sigma_px Sigma_px_t - traceI3;
// 构建配准矩阵（参见 ICP 算法）
SquareMatrixd QSigma(4); // #25 in the paper (besl)
QSigma.m_values[0][0] = trace;
QSigma.m_values[0][1] = QSigma.m_values[1][0] = Aij.m_values[1][2];
QSigma.m_values[0][2] = QSigma.m_values[2][0] = Aij.m_values[2][0];
QSigma.m_values[0][3] = QSigma.m_values[3][0] = Aij.m_values[0][1];
QSigma.m_values[1][1] = bottomMat.m_values[0][0];
QSigma.m_values[1][2] = bottomMat.m_values[0][1];
QSigma.m_values[1][3] = bottomMat.m_values[0][2];
QSigma.m_values[2][1] = bottomMat.m_values[1][0];
QSigma.m_values[2][2] = bottomMat.m_values[1][1];
QSigma.m_values[2][3] = bottomMat.m_values[1][2];
QSigma.m_values[3][1] = bottomMat.m_values[2][0];
QSigma.m_values[3][2] = bottomMat.m_values[2][1];
QSigma.m_values[3][3] = bottomMat.m_values[2][2];
// 计算 QSigma 的特征值和特征向量
CCLib::SquareMatrixd eigVectors;
std::vectoreigValues;
if (!Jacobi::ComputeEigenValuesAndVectors(QSigma, eigVectors, eigValues, false))
{
// 失败
return false;
}
// 如Besl所说，最佳旋转对应于与最大特征值相关联的特征向量
double qR[4];
double maxEigValue = 0;
Jacobi::GetMaxEigenValueAndVector(eigVectors, eigValues, maxEigValue, qR);
// 这些特征值和特征向量对应于一个四元数 --> 我们获取相应的矩阵
trans.R.initFromQuaternion(qR);
if (adjustScale)
{
// 两个累加器
double acc_num = 0.0;
double acc_denom = 0.0;
// 现在推导尺度（参见 "Point Set Registration with Integrated Scale Estimation", Zinsser et. al, PRIP 2005）
X->placeIteratorAtBeginning();
P->placeIteratorAtBeginning();
unsigned count = X->size();
assert(P->size() == count);
for (unsigned i = 0; i < count; i)
{
// 'a' 指的是数据 'A'（移动的）= P
// 'b' 指的是模型 'B'（不动的）= X
CCVector3 a_tilde = trans.R * (*(P->getNextPoint()) - Gp); // a_tilde_i = R * (a_i - a_mean)
CCVector3 b_tilde = (*(X->getNextPoint()) - Gx); // b_tilde_j = (b_j - b_mean)
acc_num = b_tilde.dot(a_tilde);
acc_denom = a_tilde.dot(a_tilde);
}
// DGM: acc_denom 不能为0，因为我们已经检查过边界框不是单个点！
assert(acc_denom > 0.0);
trans.s = static_cast(std::abs(acc_num / acc_denom));
}
// 推导平移
trans.T = Gx - (trans.R * Gp) * (aPrioriScale * trans.s); // #26 in besl paper, modified with the scale as in jschmidt
}
return true;
}具体公式可以参考文章：https://graphics.stanford.edu/courses/cs164-09-spring/Handouts/paper_icp.pdf
其他模块链接
Point Set Registration with Integrated Scale Estimation，Znisser et al, PRIP 2005 for the scale estimation.
https://robotik.informatik.uni-wuerzburg.de/telematics/download/3dim2007/node2.html
https://en.wikipedia.org/wiki/Iterative_closest_point
https://graphics.stanford.edu/courses/cs164-09-spring/Handouts/paper_icp.pdf
Robust Point Set Registration Using Gaussian Mixture Models", B. Jian and B.C. Vemuri, PAMI 2011
以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除
