The public account is dedicated to sharing articles and technologies related to point cloud processing, SLAM, 3D vision, and high-precision maps. You are welcome to join us to communicate and make progress together. If you are interested, please contact WeChat: cloudpoint9527. This article is shared by the blogger of Point Cloud PCL. Please do not reprint without the permission of the author. All students are welcome to actively share and communicate.
Preface
Some friends said, "I feel that the point cloud registration in CloudCompare is better than the registration in PCL." Why is this? Let me first talk about my general understanding. In terms of algorithm implementation, although CC also uses the ICP algorithm, it has been improved on the basis of ICP to make it more versatile. We will take a look at the code for the specific implementation details in a moment. The improved ICP algorithm adopts some special strategies or optimizations to adapt to some specific application scenarios. The PCL library provides the implementation of a variety of point cloud registration algorithms, including ICP (Iterative Closest Point), NDT (Normal Distributions Transform), etc. These algorithms may be different from CloudCompare’s algorithms in implementation and performance, because CloudCompare The ICP algorithm has been tuned with some default parameters to adapt to general registration requirements. In comparison, PCL provides great flexibility, and users can finely adjust the parameters of the registration algorithm. This freedom can be an advantage for expert users, but it also requires a deeper understanding of the algorithm.
Therefore, all point cloud algorithms must be based on the attributes of the point cloud, such as the orderliness of the point cloud, the sparseness of the point cloud, and the amount of noise. When calling the PCL algorithm, you must learn to adjust the parameters for adaptation. Therefore, in practical applications, selecting appropriate registration tools and parameters usually requires experimentation and adjustment based on specific application scenarios and data characteristics.
Registration function in CC
Define the Register method of the ICPRegistrationTools class
ICPRegistrationTools::RESULT_TYPE ICPRegistrationTools::Register(
GenericIndexedCloudPersist* inputModelCloud,
GenericIndexedMesh* inputModelMesh,
GenericIndexedCloudPersist* inputDataCloud,
const Parameters& params,
ScaledTransformation& transform,
double& finalRMS,
unsigned& finalPointCount,
GenericProgressCallback* progressCb /*=0*/)I have made some step-by-step instructions for this function:
(1) First check whether the point cloud is empty
(2) For better registration, we definitely want to use the same number of points as originally defined by the user, but if the number is relatively large, it will definitely affect the efficiency, so if the input data point cloud is too large, random sampling is performed here to improve speed. , here we notice that dataSamplingLimit defaults to 50000. When it is greater than this maximum number of points, this function is called to perform random sampling, and the weight of the point cloud needs to be resampled.
data.cloud = CloudSamplingTools::subsampleCloudRandomly(inputDataCloud, dataSamplingLimit);(3) Construct an octree for the input point cloud. The default is 8 levels.
unsigned char meshDistOctreeLevel = 8;
找到最适合与模型八叉树级别
meshDistOctreeLevel = dataOctree.findBestLevelForComparisonWithOctree(&modelOctree); }(4) The model also requires the same processing, the number of point clouds, weights, and octree construction
(5) Calculate the initial distance between the two point clouds (also calculate the CPSet)
(6) Determine whether the farthest point should be removed. Here you need to calculate the distance distribution parameters of the data point cloud.
NormalDistribution N;
N.computeParameters(data.cloud);
// 获取均值和方差
N.getParameters(mu, sigma2);
// 计算最大距离
ScalarType maxDistance = static_cast(mu + 2.5 * sqrt(sigma2));(7) Then retain the point clouds that are not too high in distance, sort the distances of overlapping point clouds in parallel, and calculate the weight value of each point.
(8) Now that the point cloud that will be used for registration has been selected, if weights are used, the weighted RMS root mean square error must be calculated. If the weights are invalid, skip them directly.
(9) The goal of ICP is to ensure the reduction of the sum of squared distances (the reduction of the sum of distances is not guaranteed)
(10) The process function RegistrationProcedure of iterative point cloud registration . The conditions for stopping point cloud registration are as follows:
if ((params.convType == MAX_ERROR_CONVERGENCE && deltaRMS < params.minRMSDecrease) || (params.convType == MAX_ITER_CONVERGENCE && iteration >= params.nbMaxIterations))
{ result = ICP_APPLY_TRANSFO;
break;
}(11) Filter translation matrix
FilterTransformation(currentTrans, params.transformationFilters, currentTrans);(12) After calculating the transformation matrix, apply it to convert it into a new point cloud, and calculate its distance to the model
Function RegistrationProcedure annotation for an iterative process
// 配准过程，用于计算数据点云 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;
}For specific formulas, please refer to the article: https://graphics.stanford.edu/courses/cs164-09-spring/Handouts/paper_icp.pdf
Other module links
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
If there are any errors in the above content, please leave a comment. Corrections and exchanges are welcome. If there is any infringement, please contact us to delete it.
Scan QR code
Follow us
Let’s share and learn together! We look forward to friends who have ideas and are willing to share to join Knowledge Planet and inject fresh vitality into sharing. Topics shared include but are not limited to three-dimensional vision, point clouds, high-precision maps, autonomous driving, robots and other related fields.
Sharing and cooperation: WeChat "cloudpoint9527" (remarks required) Contact email: [email protected]. Companies are welcome to contact the public account to start cooperation.
Click "Looking" and you will look better.
