Eine kurze Analyse, warum die Punktwolkenregistrierung in CC besser ist als in PCL?

Der öffentliche Account ist dem Austausch von Artikeln und Technologien im Zusammenhang mit Punktwolkenverarbeitung, SLAM, 3D-Vision und hochpräzisen Karten gewidmet. Sie sind herzlich eingeladen, sich uns anzuschließen, um zu kommunizieren und gemeinsam Fortschritte zu machen. Bei Interesse wenden Sie sich bitte an WeChat: cloudpoint9527 . Dieser Artikel wurde vom Blogger von Point Cloud PCL geteilt. Bitte nicht ohne die Erlaubnis des Autors nachdrucken. Alle Studierenden sind herzlich eingeladen, ihn aktiv zu teilen und zu kommunizieren.

Vorwort

Einige Freunde sagten: „Ich finde, dass die Punktwolkenregistrierung in CloudCompare besser ist als die Registrierung in PCL.“ Warum ist das so? Lassen Sie mich zunächst über mein allgemeines Verständnis sprechen. In Bezug auf die Algorithmusimplementierung verwendet CC zwar auch den ICP-Algorithmus, dieser wurde jedoch auf der Basis von ICP verbessert, um ihn vielseitiger zu machen. Wir werden uns den Code für die spezifische Implementierung ansehen Einzelheiten dazu gleich. Der verbesserte ICP-Algorithmus übernimmt einige spezielle Strategien oder Optimierungen, um sich an bestimmte Anwendungsszenarien anzupassen. Die PCL-Bibliothek bietet die Implementierung verschiedener Punktwolken-Registrierungsalgorithmen, einschließlich ICP (Iterative Closest Point), NDT (Normal Distributions Transform) usw. Diese Algorithmen können sich in Implementierung und Leistung von den Algorithmen von CloudCompare unterscheiden, da CloudCompare der ICP-Algorithmus ist wurde mit einigen Standardparametern optimiert, um den allgemeinen Registrierungsanforderungen gerecht zu werden. Im Vergleich dazu bietet PCL eine große Flexibilität und Benutzer können die Parameter des Registrierungsalgorithmus fein anpassen. Diese Freiheit kann für erfahrene Benutzer von Vorteil sein, erfordert aber auch ein tieferes Verständnis des Algorithmus.

Daher müssen alle Punktwolkenalgorithmen auf den Attributen der Punktwolke basieren, wie z. B. der Ordnung der Punktwolke, der Spärlichkeit der Punktwolke und der Menge an Rauschen. Beim Aufrufen des PCL-Algorithmus müssen Sie lernen, ihn anzupassen die Parameter für die Anpassung. Daher erfordert die Auswahl geeigneter Registrierungswerkzeuge und -parameter in praktischen Anwendungen normalerweise Experimente und Anpassungen auf der Grundlage spezifischer Anwendungsszenarien und Dateneigenschaften.

Registrierungsfunktion in CC

Definieren Sie die Register-Methode der ICPRegistrationTools-Klasse 

ICPRegistrationTools::RESULT_TYPE ICPRegistrationTools::Register( 
GenericIndexedCloudPersist* inputModelCloud, 
GenericIndexedMesh* inputModelMesh, 
GenericIndexedCloudPersist* inputDataCloud, 
const Parameters& params, 
ScaledTransformation& transform, 
double& finalRMS, 
unsigned& finalPointCount, 
GenericProgressCallback* progressCb /*=0*/)

Ich habe einige Schritt-für-Schritt-Anleitungen für diese Funktion erstellt:

(1) Überprüfen Sie zunächst, ob die Punktwolke leer ist

(2) Für eine bessere Registrierung möchten wir auf jeden Fall die gleiche Anzahl von Punkten verwenden, die ursprünglich vom Benutzer definiert wurden. Wenn die Anzahl jedoch relativ groß ist, wirkt sich dies definitiv auf die Effizienz aus. Wenn also die Punktwolke der Eingabedaten zu groß ist, Hier wird eine Zufallsstichprobe durchgeführt, um die Geschwindigkeit zu verbessern. Hier stellen wir fest, dass dataSamplingLimit standardmäßig 50000 beträgt. Wenn es größer als diese maximale Anzahl von Punkten ist, wird diese Funktion aufgerufen, um eine Zufallsstichprobe durchzuführen, und das Gewicht der Punktwolke muss erneut abgetastet werden .

data.cloud = CloudSamplingTools::subsampleCloudRandomly(inputDataCloud, dataSamplingLimit);

(3) Erstellen Sie einen Octree für die Eingabepunktwolke. Der Standardwert beträgt 8 Ebenen.

unsigned char meshDistOctreeLevel = 8; 
找到最适合与模型八叉树级别
meshDistOctreeLevel = dataOctree.findBestLevelForComparisonWithOctree(&modelOctree); }

(4) Das Modell erfordert auch die gleiche Verarbeitung, die gleiche Anzahl von Punktwolken, Gewichten und die gleiche Octree-Konstruktion

(5) Berechnen Sie den anfänglichen Abstand zwischen den beiden Punktwolken (berechnen Sie auch den CPSet).

(6) Bestimmen Sie, ob der am weitesten entfernte Punkt entfernt werden soll. Hier müssen Sie die Abstandsverteilungsparameter der Datenpunktwolke berechnen. 

NormalDistribution N; 
  N.computeParameters(data.cloud);
  // 获取均值和方差 
  N.getParameters(mu, sigma2); 
  // 计算最大距离 
  ScalarType maxDistance = static_cast(mu + 2.5 * sqrt(sigma2));

(7) Behalten Sie dann die Punktwolken bei, deren Abstand nicht zu groß ist, sortieren Sie die Abstände überlappender Punktwolken parallel und berechnen Sie den Gewichtungswert jedes Punkts.

(8) Nachdem nun die Punktwolke ausgewählt wurde, die für die Registrierung verwendet wird, muss bei Verwendung von Gewichten der gewichtete RMS-Root-Mean-Square-Fehler berechnet werden. Wenn die Gewichte ungültig sind, überspringen Sie sie direkt.

(9) Das Ziel von ICP besteht darin, die Reduzierung der Summe der Distanzquadrate sicherzustellen (die Reduzierung der Summe der Distanzen ist nicht garantiert).

(10) Die Prozessfunktion RegistrationProcedure der iterativen Punktwolkenregistrierung  . Die Bedingungen zum Stoppen der Punktwolkenregistrierung sind wie folgt:

if ((params.convType == MAX_ERROR_CONVERGENCE && deltaRMS < params.minRMSDecrease) || (params.convType == MAX_ITER_CONVERGENCE && iteration >= params.nbMaxIterations))
 { result = ICP_APPLY_TRANSFO; 
    break; 
}

(11) Übersetzungsmatrix filtern

FilterTransformation(currentTrans, params.transformationFilters, currentTrans);

(12) Wenden Sie die Transformationsmatrix nach der Berechnung an, um sie in eine neue Punktwolke umzuwandeln, und berechnen Sie deren Abstand zum Modell

Funktion RegistrationProcedure -Annotation für einen iterativen Prozess

// 配准过程，用于计算数据点云 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;
}

Spezifische Formeln finden Sie im Artikel: https://graphics.stanford.edu/courses/cs164-09-spring/Handouts/paper_icp.pdf

Weitere Modul-Links

Punktmengenregistrierung mit integrierter Skalenschätzung, Znisser et al, PRIP 2005 für die Skalenschätzung.

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

Robuste Punktsatzregistrierung mithilfe von Gaußschen Mischungsmodellen“, B. Jian und BC Vemuri, PAMI 2011

Sollten im obigen Inhalt Fehler auftreten, hinterlassen Sie bitte einen Kommentar. Korrekturen und Umtausch sind willkommen. Wenn ein Verstoß vorliegt, kontaktieren Sie uns bitte, um ihn zu löschen.

QR-Code scannen

                   Folgen Sie uns

Lasst uns gemeinsam teilen und lernen! Wir freuen uns auf Freunde, die Ideen haben und bereit sind zu teilen, sich Knowledge Planet anzuschließen und dem Teilen neue Dynamik zu verleihen. Zu den geteilten Themen gehören unter anderem dreidimensionales Sehen, Punktwolken, hochpräzise Karten, autonomes Fahren, Roboter und andere verwandte Bereiche.

Teilen und Zusammenarbeit: WeChat „cloudpoint9527“ (Anmerkungen erforderlich) Kontakt-E-Mail: [email protected]. Unternehmen können sich gerne an das öffentliche Konto wenden, um eine Zusammenarbeit zu beginnen.

Klicken Sie auf „Suchen“ und Sie werden besser aussehen.