Finden, translation und Skalierung auf zwei Sätze von Punkte zu bekommen, die kleinste quadratische Fehler in der Entfernung?

Da Sie bereits Kabsch Algorithmus, werfen Sie einen Blick auf Umeyama Papier die sich es zu Holen Skala. Alles, was Sie tun müssen, ist, um die Standardabweichung Ihrer Punkte und berechnen Maßstab:

(1/sigma^2)*trace(D*S)

wo D ist die diagonal-matrix mit SVD-ZERLEGUNG in der rotation estimation and S ist entweder die Einheitsmatrix oder [1 1 -1] Diagonale matrix, je nach dem Vorzeichen der Determinante von UV (Kabsch verwendet, um die korrekte Reflexionen in der richtigen Rotationen). Also, wenn Sie [2000, 200, 20] multiplizieren Sie das Letzte element von +-1 (je nach dem Vorzeichen der Determinante von UV), Summe Sie und dividieren durch die Standardabweichung Ihrer Punkte zu Holen Skala.

Können Sie recyceln, der folgende code, der ist mit dem Eigen Bibliothek:

typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3d_U; //microsoft's 32-bit compiler can't put Eigen::Vector3d inside a std::vector. for other compilers or for 64-bit, feel free to replace this by Eigen::Vector3d

/**
 *  @brief rigidly aligns two sets of poses
 *
 *  This calculates such a relative pose <tt>R, t</tt>, such that:
 *
 *  @code
 *  _TyVector v_pose = R * r_vertices[i] + t;
 *  double f_error = (r_tar_vertices[i] - v_pose).squaredNorm();
 *  @endcode
 *
 *  The sum of squared errors in <tt>f_error</tt> for each <tt>i</tt> is minimized.
 *
 *  @param[in] r_vertices is a set of vertices to be aligned
 *  @param[in] r_tar_vertices is a set of vertices to align to
 *
 *  @return Returns a relative pose that rigidly aligns the two given sets of poses.
 *
 *  @note This requires the two sets of poses to have the corresponding vertices stored under the same index.
 */
static std::pair<Eigen::Matrix3d, Eigen::Vector3d> t_Align_Points(
    const std::vector<Vector3d_U> &r_vertices, const std::vector<Vector3d_U> &r_tar_vertices)
{
    _ASSERTE(r_tar_vertices.size() == r_vertices.size());
    const size_t n = r_vertices.size();

    Eigen::Vector3d v_center_tar3 = Eigen::Vector3d::Zero(), v_center3 = Eigen::Vector3d::Zero();
    for(size_t i = 0; i < n; ++ i) {
        v_center_tar3 += r_tar_vertices[i];
        v_center3 += r_vertices[i];
    }
    v_center_tar3 /= double(n);
    v_center3 /= double(n);
    //calculate centers of positions, potentially extend to 3D

    double f_sd2_tar = 0, f_sd2 = 0; //only one of those is really needed
    Eigen::Matrix3d t_cov = Eigen::Matrix3d::Zero();
    for(size_t i = 0; i < n; ++ i) {
        Eigen::Vector3d v_vert_i_tar = r_tar_vertices[i] - v_center_tar3;
        Eigen::Vector3d v_vert_i = r_vertices[i] - v_center3;
        //get both vertices

        f_sd2 += v_vert_i.squaredNorm();
        f_sd2_tar += v_vert_i_tar.squaredNorm();
        //accumulate squared standard deviation (only one of those is really needed)

        t_cov.noalias() += v_vert_i * v_vert_i_tar.transpose();
        //accumulate covariance
    }
    //calculate the covariance matrix

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(t_cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
    //calculate the SVD

    Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();
    //compute the rotation

    double f_det = R.determinant();
    Eigen::Vector3d e(1, 1, (f_det < 0)? -1 : 1);
    //calculate determinant of V*U^T to disambiguate rotation sign

    if(f_det < 0)
        R.noalias() = svd.matrixV() * e.asDiagonal() * svd.matrixU().transpose();
    //recompute the rotation part if the determinant was negative

    R = Eigen::Quaterniond(R).normalized().toRotationMatrix();
    //renormalize the rotation (not needed but gives slightly more orthogonal transformations)

    double f_scale = svd.singularValues().dot(e) / f_sd2_tar;
    double f_inv_scale = svd.singularValues().dot(e) / f_sd2; //only one of those is needed
    //calculate the scale

    R *= f_inv_scale;
    //apply scale

    Eigen::Vector3d t = v_center_tar3 - (R * v_center3); //R needs to contain scale here, otherwise the translation is wrong
    //want to align center with ground truth

    return std::make_pair(R, t); //or put it in a single 4x4 matrix if you like
}

InformationsquelleAutor the swine

Möchten Sie vielleicht versuchen, ICP - (Iterative closest point).
Zwei Sätze von 3d-Punkten, es wird Ihnen sagen, die transformation (rotation + translation) vom ersten auf den zweiten.
Wenn Sie interessiert sind, in einer c++ - einfache Implementierung, versuchen libicp.

Glück!

InformationsquelleAutor Nacho

Eine gute Erklärung Die Suche nach optimalen rotation und translation zwischen den entsprechenden 3D-Punkte

Sich der code in matlab, aber es ist trivial zu konvertieren, um opengl mit der cv::SVD-Funktion

InformationsquelleAutor Martin Beckett

Maßstab abgeleitet werden kann, ohne SVD, wenn Sie Ihre Punkte sind gleichmäßig in alle Richtungen skaliert (ich konnte nicht Sinn der SVD-s-Skala-matrix). Ist hier, wie ich gelöst habe das gleiche problem:

1. Messen von Entfernungen eines jeden Punktes auf andere Punkte in der Punktwolke um eine 2d-Tabelle der Strecken, wo der Eintrag in (i,j) ist die norm(point_i-point_j). Machen Sie dasselbe für die andere Punktwolke, so erhalten Sie zwei Tabellen-eine für die original-und die andere für die rekonstruierten Punkte.

2. Teilt alle Werte in einer Tabelle durch die entsprechenden Werte in der anderen Tabelle. Da die Punkte einander entsprechen, die Entfernungen auch. Im Idealfall ist die resultierende Tabelle hat alle Werte einander gleich, und das ist der Maßstab.

3. Wird der Mittelwert der Divisionen sollte ziemlich in der Nähe der Skala, die Sie suchen. Der Mittelwert ist auch in der Nähe, aber ich wählte median nur um Ausreißer auszuschließen.

Nun können Sie den scale-Wert zu skalieren alle rekonstruierten Punkte und dann gehen Sie zu der Schätzung der rotation.

Tipp: Wenn es zu viele Punkte in den Punktwolken zu finden, Entfernungen zwischen allen von Ihnen, dann eine kleinere Teilmenge von Entfernungen funktioniert auch, solange es die gleiche Teilmenge für beide Punktwolken. Im Idealfall nur eine Distanz-pair-Mädchen arbeiten würde, wenn es keine Messung von Lärm, e.g wenn eine Punktwolke ist direkt abgeleitet von der anderen nur durch drehen.

InformationsquelleAutor Rasmus