在两组点上找到平移和缩放以获得距离最小的平方误差？

Question

我有两组3D点(原始和重建)和关于对的对应信息 - 从一组代表第二组.我需要找到转换重建集的3D平移和缩放因子,因此平方距离的总和最小(旋转也会很好,但点也是相似的,因此这不是主要优先级,为简单起见可能省略速度).所以我的问题是 - 这是解决并在互联网上的某个地方可用吗？就个人而言,我会使用最小二乘法,但我没有太多时间(虽然我有点擅长数学,但我不经常使用它,所以我最好避免使用它),所以我如果它存在,我想使用其他的解决方案.我更喜欢C++中的解决方案,例如使用OpenCV,但仅凭算法就足够了.

如果没有这样的解决方案,我会自己计算,我不想打扰你.

解决方案:(从你的答案)
对我而言,它是Kabsch alhorithm;
基本信息:http ://en.wikipedia.org/wiki/Kabsch_algorithm
一般解决方案:http://nghiaho.com/？page_id = 671

仍然没有解决: 我也需要规模.来自SVD的比例值对我来说是不可理解的; 当我需要所有轴的比例约为1-4(由我估计)时,SVD比例约为[2000,200,20],这根本没有帮助.

Answer 1

由于你已经在使用Kabsch算法,只需看一下Umeyama的论文,扩展它以获得规模.您需要做的就是获得积分的标准偏差并计算尺度:

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

其中D是旋转估计中SVD分解中的对角矩阵,并且S是单位矩阵或[1 1 -1]对角矩阵,这取决于UV(Kabsch用于将反射校正为正确旋转)的行列式的符号.因此,如果你有[2000, 200, 20],将最后一个元素乘以+ -1(取决于行列式的符号UV),将它们相加并除以你的点的标准偏差以得到比例.

您可以回收以下使用Eigen库的代码:

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
}

Answer 2

对于 3D 点，该问题称为绝对方向问题。C++ 实现可从 Eigen 获得http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60和论文http://web.stanford.edu/class/cs273/refs/umeyama.pdf

您可以通过 opencv 使用它，方法是使用 cv::cv2eigen() 调用将矩阵转换为特征值。

Answer 3

一个很好的解释寻找相应 3D 点之间的最佳旋转和平移

代码位于 matlab 中，但使用 cv::SVD 函数转换为 opengl 很简单