21

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

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

解决方案：（从您的回答中）
对我来说，这是 Kabsch 算法；
基本信息：http
://en.wikipedia.org/wiki/Kabsch_algorithm 一般解决方案：http ://nghiaho.com/?page_id=671

仍然没有解决： 我还需要规模。SVD 的比例值对我来说是无法理解的；当我需要所有轴的比例约为 1-4 时（由我估计），SVD 比例约为 [2000, 200, 20]，这根本没有帮助。

4

8 回答 8

23

由于您已经在使用 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
}
于 2015-08-27T08:53:53.103 回答
7

对于 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() 将矩阵转换为特征值来使用它。

于 2014-08-09T23:02:46.317 回答
4

从两组点的平移开始。使它们的质心与坐标系的原点重合。平移向量就是这些质心之间的差异。

现在我们有两组坐标表示为矩阵P和Q。一组点可以通过应用一些线性算子（执行缩放和旋转）从其他点获得。该运算符由 3x3 矩阵X表示：

P * X = Q

为了找到合适的缩放/旋转，我们只需要求解这个矩阵方程，找到X，然后将其分解为几个矩阵，每个矩阵代表一些缩放或旋转。

解决它的一个简单（但可能在数值上不稳定）的方法是将方程的两个部分乘以转置矩阵P（以摆脱非方阵），然后将方程的两个部分乘以倒置的P T * : _

P T * P * X = P T * Q

X = ( P T * P ) -1 * P T * Q

对矩阵X应用奇异值分解给出两个旋转矩阵和一个具有比例因子的矩阵：

X = U * S * V

这里S是一个具有比例因子的对角矩阵（每个坐标一个比例），U和V是旋转矩阵，一个适当地旋转点以便它们可以沿坐标轴缩放，另一个旋转它们一次以对齐它们的方向到第二组点。

示例（为简单起见，使用二维点）：

P =  1     2     Q =   7.5391    4.3455
     2     3          12.9796    5.8897
    -2     1          -4.5847    5.3159
    -1    -6         -15.9340  -15.5511

解方程后：

X =  3.3417   -1.2573
     2.0987    2.8014

SVD分解后：

U = -0.7317   -0.6816
    -0.6816    0.7317

S =  4  0
     0  3

V = -0.9689   -0.2474
    -0.2474    0.9689

在这里，SVD 已经正确地重构了我对矩阵P执行的所有操作以获得矩阵Q：旋转 0.75 角度，将 X 轴缩放 4，将 Y 轴缩放 3，旋转角度 -0.25。

如果点集被统一缩放（每个轴的比例因子相等），则此过程可能会大大简化。

只需使用Kabsch 算法即可获得平移/旋转值。然后执行这些平移和旋转（质心应与坐标系的原点重合）。然后为每对点（和每个坐标）估计线性回归。线性回归系数正是比例因子。

于 2013-01-25T16:11:32.147 回答
1

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

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

于 2012-11-18T04:37:35.593 回答
1

您可能想尝试 ICP（迭代最近点）。给定两组 3d 点，它将告诉您从第一组到第二组的变换（旋转 + 平移）。如果您对 c++ 轻量级实现感兴趣，请尝试 libicp。

祝你好运！

于 2012-11-19T16:36:46.877 回答
0

一般的转换，以及规模可以通过Procrustes Analysis检索。它通过将对象相互叠加并尝试从该设置估计转换来工作。它已在 ICP 的上下文中多次使用。实际上，您的偏好，Kabash 算法是这种情况的一个特例。

此外，Horn 的对齐算法（基于四元数）也找到了一个非常好的解决方案，同时非常高效。Matlab 实现也可用。

于 2015-04-16T11:18:18.610 回答
0

如果您的点在所有方向上均匀缩放（我也无法理解 SVD-s 比例矩阵），则可以在没有 SVD 的情况下推断比例。这是我解决相同问题的方法：

  1. 测量每个点到点云中其他点的距离以获得二维距离表，其中 (i,j) 处的条目是 norm(point_i-point_j)。对另一个点云做同样的事情，这样你就得到了两个表——一个用于原始点，另一个用于重建点。

  2. 将一张表中的所有值除以另一张表中的对应值。因为这些点彼此对应，所以距离也是如此。理想情况下，结果表的所有值都彼此相等，这就是比例。

  3. 分区的中值应该非常接近您正在寻找的比例。平均值也很接近，但我选择中位数只是为了排除异常值。

现在您可以使用比例值来缩放所有重建点，然后继续估计旋转。

提示：如果点云中的点太多而无法找到所有点之间的距离，那么距离的较小子集也可以工作，只要它是两个点云的相同子集。理想情况下，如果没有测量噪声，则只有一对距离可以工作，例如，当一个点云通过旋转它直接从另一个点云导出时。

于 2015-07-28T07:37:26.993 回答
-2

也可以使用 BaoweiLin 提出的ScaleRatio ICP 代码可以在github找到

于 2014-05-20T03:12:27.347 回答