在两组点上找到翻译和比例，以获得它们之间距离的最小平方误差？

Question

我有两组3D点（原始和重建的），并且有关于它们之间对应信息 - 哪个点从一组代表第二组。我需要找到3D平移和缩放因子，将重建集合转换为使平方距离总和最小（旋转可能也可以，但是点已经类似旋转，所以这不是主要任务，为了简单和速度可以忽略它）。所以我的问题是 - 这个问题是否已经解决并在互联网上可用？对我来说，我会使用最小二乘法，但是我没有太多时间（虽然我在数学方面有一些经验，但我并不经常使用它，所以使用其他人的解决方案会更好），所以如果有现成的解决方案，我更愿意使用。我更喜欢C++解决方案，例如使用OpenCV，但仅算法本身就足够了。

如果没有这样的解决方案，我会自己计算，不想给你带来太多麻烦。 解决方案：（来自您的答案）
对我来说，这是Kabsch算法；
基本信息：http://en.wikipedia.org/wiki/Kabsch_algorithm
通用解决方案：http://nghiaho.com/?page_id=671 仍未解决： 我还需要比例尺。SVD中的比例值对我来说不可理解；当我需要所有轴的比例约为1-4（由我估计），SVD比例大约是[2000, 200, 20]，完全没有帮助。

- user1476710

---

可能Kabsch算法是你需要的。两个质心的差值代表平移；在计算协方差矩阵的SVD后，奇异值表示缩放因子，而酉矩阵则提供了最优旋转矩阵。 - Evgeny Kluev

Evgeny Kluev：非常感谢，看起来就是这样了。我会尝试并发布结果（需要一些时间；我还有其他事情要实现）。顺便说一句，幸运的是，OpenCV包含SVD计算器，这大大简化了事情。 - user1476710

Evgeny Kluev：非常抱歉回复晚了，我有更重要的项目需要处理。我想问一下，如何解释缩放因子？这些数字非常大（200-2000）或者很小（~0.5），但是根据我的判断，缩放比例应该在1-4之间。此外，不同轴的缩放因子通常也不同（例如[2000，200，20]）。 - user1476710

实际上，没有直接从奇异值中获取缩放因子的方法。我犯了个错误，抱歉。基于SVD的算法可能适用于这里，但我不知道具体如何操作。无论如何，你可以尝试更通用的迭代最近点算法。 - Evgeny Kluev

你看过我下面的答案了吗？你也可以从Eigen中获得比例 https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60 当然，这假设你有对应关系。 - bendervader

@bendervader 对不起，还没有。我没能完成这个项目，虽然我很想回头继续做，但是目前还没有时间。生活有些复杂嘛 ;)。 - user1476710

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
}

- the swine

---

对于OP来说，我认为你在D中得到了大的值，因为你首先需要将要分解的矩阵按1/N进行缩放，其中N是你的点数。结果发现，即使没有这样做，翻译和旋转估计也可以正常工作，所以人们经常不这样做。正确答案需要引用论文。 - Laky

@Laky 嗯，说实话我也没有缩放协方差矩阵（正如你在代码中所看到的）。算法可能能够处理它，但是缩放计算就不行了。D 中的大数值并不一定会有问题，因为缩放还取决于标准差，它本身可能很大（或很小），与缩放无关。不过还是谢谢你的赞美。 - the swine

顺便提一下，有些人更喜欢使用Horn算法。它与Kabsch非常相似，唯一的区别是它分解了一个4x4矩阵，输出是四元数。对我来说，它只是稍微计算成本更高，所以我坚持使用Kabsch，然后将3x3矩阵转换为四元数。我不确定是否有Horn算法的版本可以恢复比例。 - the swine

感谢您的出色回答。我已经在这里实现了一个20行的Python代码，涵盖了整个问题（https://gist.github.com/nh2/bc4e2981b0e213fefd4aaa33edfb3893#file-rigid-transform-with-scale-py-L20-L39）。我尽力调整了代码以提高可读性。 - nh2

@nh2 抱歉，但它给了我一个 NameError: 全局变量 'a1' 在第34行未定义。应该是 P 吗？ - sunrise

Answer 2

对于3D点，这个问题被称为绝对定位问题。Eigen提供了一个C++实现，您可以通过以下链接进行访问：http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60，并且有相关的论文http://web.stanford.edu/class/cs273/refs/umeyama.pdf。您可以使用cv::cv2eigen()调用将矩阵转换为Eigen格式，然后在OpenCV中使用它。

- bendervader

Answer 3

先将两组点进行平移，使它们的质心与坐标系原点重合。平移向量就是这些质心之间的差。

现在我们有两组表示为矩阵P和Q的坐标。一组点可以通过应用某些线性操作（旋转和缩放）从另一组点中获得。该操作由3x3矩阵X表示：

P * X = Q

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

解决它的一个简单（但可能不稳定）方法是将方程的两个部分乘以转置矩阵P（以摆脱非方形矩阵），然后将方程的两个部分乘以反转置矩阵P^T * P：

P^T * P * X = P^T * Q

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

将奇异值分解应用于矩阵X，得到两个旋转矩阵和一个带有比例因子的矩阵：

X = U * S * V

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

---

示例（为简单起见使用2D点）：

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算法获取平移/旋转值。然后执行这些平移和旋转（质心应与坐标系原点重合）。然后对于每对点（和每个坐标），估计线性回归。线性回归系数正是比例因子。

- Evgeny Kluev

---

你是如何从U和V矩阵中确定旋转的？这些旋转是以弧度为单位的吗？ - blueshift

@blueshift：在二维情况下，这很简单：矩阵分量只是角度的正弦和余弦。在三维情况下：旋转轴由特征向量确定，然后您可以找到轴垂直的任何向量与通过旋转矩阵变换的相同向量之间的角度。详见wikipedia。 - Evgeny Kluev

谢谢，Evgeny，特别是提供维基百科链接。在你的例子中，你是如何确定使用-0.7317还是0.7317作为余弦值的？ - blueshift

@blueshift：老实说，我不知道。问题在于SVD矩阵的某些行被取反了，而我不知道哪些行被取反了。唯一的方法是尝试两种可能性，将旋转应用于实际点并查看哪一个是正确的，以使用这些数据（而不深入挖掘问题）。 - Evgeny Kluev

Answer 4

一个好的解释 在对应的3D点之间找到最优旋转和平移 这段代码是用matlab编写的，但使用cv::SVD函数将其转换为opengl非常简单。

- Martin Beckett

---

非常抱歉回复晚了，我有更重要的项目需要处理。非常感谢您提供的链接，它对我很有帮助。我使用cv::SVD实现了它，效果很好。但我仍有一个问题：关于比例尺度怎么办？SVD中的比例值太大或太小了。 - user1476710

这种方法只适用于大小相等的云。通常你知道云是相同比例的，我没有尝试过，但有一个比例方法http://www.google.com/url?sa=t&rct=j&q=optimal%20rotation%20point%20cluds%20with%20scale&source=web&cd=4&cad=rja&ved=0CEsQFjAD&url=http%3A%2F%2Fmi.eng.cam.ac.uk%2F~cipolla%2Fpublications%2FcontributionToEditedBook%2F2012-ICVSS-Toshiba.pdf&ei=yO7_UIT1LajyigLcp4CoCQ&usg=AFQjCNE-bHBZgquyyLbYVo-xavM4sWcTRA&bvm=bv.41248874,d.cGE - Martin Beckett

Answer 5

你可能想尝试ICP（迭代最近点）。 给定两组3D点，它会告诉你如何从第一组到第二组进行变换（旋转+平移）。 如果你对C++轻量级实现感兴趣，可以尝试libicp。 祝你好运！

- Nacho

---

感谢您的回答（并为晚回复道歉）。我使用了Kabsch算法。此外，我需要一个比例值。 - user1476710

如果点之间的对应关系已知，ICP过程就会变得过于复杂和缓慢。对于Kabsch算法而言，点之间的对应关系已经被确定。ICP可以匹配任意两组点（包括不同数量的点、只有部分重叠的形状等），但是这样做的代价要高得多，并且存在无法收敛到最佳解决方案的风险。 - the swine

Answer 6

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

1. 测量每个点到点云中其他点的距离，以获得距离的2D表格，其中(i，j)处的条目为norm(point_i-point_j)。对于另一个点云，做同样的事情，这样您就会得到两个表格--一个用于原始点，另一个用于重建点。

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

3. 除法的中位数值应该非常接近您要查找的比例。平均值也很接近，但我选择中位数来排除异常值。

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

提示：如果点云中的点太多，无法计算它们之间的距离，那么选择一个较小的子集来计算距离也是可行的，只要这个子集对于两个点云都是相同的即可。理想情况下，如果没有测量误差，例如一个点云直接通过旋转得到另一个点云，那么只需要一个距离对就足够了。

- Rasmus

Answer 7

一般的变换和比例可以通过普洛克鲁斯特分析来获取。它通过将对象叠加在彼此上并尝试从该设置中估计变换来工作。它已经在ICP的背景下多次使用。实际上，您偏好的Kabash算法是这种情况的一个特例。

此外，Horn的对准算法（基于四元数）也能找到非常好的解决方案，同时效率也相当高。Matlab实现也可用。

- Tolga Birdal

---

请注意：我正在监控此项目，但是由于其他工作非常繁忙，所以在有时间回到这个项目之前可能需要一些时间。不管怎样，感谢您对这个旧问题的回答。 - user1476710

Answer 8

您也可以使用由BaoweiLin提出的ScaleRatio ICP。

代码可以在Github上找到。

- user3654844