A brief analysis of why point cloud registration in CC is better than PCL?

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 on the input point cloud. The default is 8 levels.

unsigned char meshDistOctreeLevel = 8;
Find the octree level that best fits the model
meshDistOctreeLevel = dataOctree.findBestLevelForComparisonWithOctree( & amp;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);
  // Get the mean and variance
  N.getParameters(mu, sigma2);
  // Calculate the maximum distance
  ScalarType maxDistance = static_cast(mu + 2.5 * sqrt(sigma2));

(7) Then retain the point clouds whose distance is not too high, sort the distances of overlapping point clouds in parallel, and calculate the weight value of each point

(8) The point cloud that will be used for registration has now 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 of iterative point cloud registrationRegistrationProcedure,the conditions for stopping point cloud registration are as follows:

if ((params.convType == MAX_ERROR_CONVERGENCE & amp; & amp; deltaRMS < params.minRMSDecrease) || (params.convType == MAX_ITER_CONVERGENCE & amp; & amp; 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 of an iteration processRegistrationProcedureComments

// Registration process, used to calculate the transformation between the data point cloud P and the model point cloud X
bool RegistrationTools::RegistrationProcedure(GenericCloud* P, // data
                                              GenericCloud* X, // model
                                              ScaledTransformation & trans,
                                              bool adjustScale/*=false*/,
                                              ScalarField* coupleWeights/*=0*/,
                                              PointCoordinateType aPrioriScale/*=1.0f*/)
{
    //The transformation matrix of the result (R is invalid at initialization, T is (0,0,0), s is 1)
    trans.R.invalidate();
    trans.T = CCVector3(0, 0, 0);
    trans.s = PC_ONE;
    // Check whether the data point cloud and model point cloud are valid, equal in size, and the number of points is no less than 3
    if (P == nullptr || X == nullptr || P->size() != X->size() || P->size() < 3)
        return false;
    // Calculate center of mass
    CCVector3 Gp = coupleWeights ? GeometricalAnalysisTools::ComputeWeightedGravityCenter(P, coupleWeights) : GeometricalAnalysisTools::ComputeGravityCenter(P);
    CCVector3 Gx = coupleWeights ? GeometricalAnalysisTools::ComputeWeightedGravityCenter(X, coupleWeights) : GeometricalAnalysisTools::ComputeGravityCenter(X);
    // Special case: only 3 points
    if (P->size() == 3)
    {
        // Calculate the first set of normals
        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);
        }


        // Calculate the second set of normals
        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);
        }


        // Now the rotation is simply from Nx to Np, centered on 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 & amp; & amp; 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};
            //Normalized quaternion
            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);
        }
        // Derive the first translation
        trans.T = Gx - (trans.R * Gp) * (aPrioriScale * trans.s);
        // Now we need to find the rotation in the (X) plane
        {
            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 & amp; 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);
            // Column 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;


            //Column 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;
            //Column 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); // Update T
        }
    }
    else
    {
        CCVector3bbMin;
        CCVector3bbMax;
        X->getBoundingBox(bbMin, bbMax);
        // If the data point cloud is equivalent to a single point (e.g. two point clouds are far apart during ICP), we try to bring the two point clouds closer
        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;
        }


        // Cross-covariance matrix, see equation #24 in Besl92 (but includes weights if there are any)
        SquareMatrixd Sigma_px = (coupleWeights ? GeometricalAnalysisTools::ComputeWeightedCrossCovarianceMatrix(P, X, Gp, Gx, coupleWeights)
                                              : GeometricalAnalysisTools::ComputeCrossCovarianceMatrix(P, X, Gp, Gx));
        if (!Sigma_px.isValid())
            return false;
            
        // Transpose sigma_px
        SquareMatrixd Sigma_px_t = Sigma_px.transposed();
        SquareMatrixd Aij = Sigma_px - Sigma_px_t;
        double trace = Sigma_px.trace(); // That is the sum of the diagonal elements of sigma_px
        SquareMatrixd traceI3(3); // Create an I matrix with eigenvalues equal to trace
        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;
// Calculate the lower half of the cross-covariance matrix
SquareMatrixd bottomMat = Sigma_px + Sigma_px_t - traceI3;
// Construct registration matrix (see ICP algorithm)
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];
// Calculate the eigenvalues and eigenvectors of QSigma
CCLib::SquareMatrixd eigVectors;
std::vectoreigValues;
if (!Jacobi::ComputeEigenValuesAndVectors(QSigma, eigVectors, eigValues, false))
{
    // fail
    return false;
}


// As Besl said, the optimal rotation corresponds to the eigenvector associated with the largest eigenvalue
double qR[4];
double maxEigValue = 0;
Jacobi::GetMaxEigenValueAndVector(eigVectors, eigValues, maxEigValue, qR);
// These eigenvalues and eigenvectors correspond to a quaternion --> we get the corresponding matrix
trans.R.initFromQuaternion(qR);


if(adjustScale)
{
    // two accumulators
    double acc_num = 0.0;
    double acc_denom = 0.0;
    // Now derive the scale (see "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' refers to data 'A' (moved) = P
        // 'b' refers to the model 'B' (immobile) = 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 cannot be 0 because we have checked that the bounding box is not a single point!
    assert(acc_denom > 0.0);
    trans.s = static_cast(std::abs(acc_num / acc_denom));
}
// Derive translation
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