在两组点上找到平移和缩放以获得距离的最小二乘误差？
Posted
技术标签:
【中文标题】在两组点上找到平移和缩放以获得距离的最小二乘误差？【英文标题】：Finding translation and scale on two sets of points to get least square error in their distance? 【发布时间】：2012-11-06 03:15:13 【问题描述】：我有两组 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]，这根本没有帮助。
【问题讨论】：
可能Kabsch algorithm 是您所需要的。两个质心的差异给出平移；在计算协方差矩阵的 SVD 后，奇异值给出缩放因子，酉矩阵给出最优旋转矩阵。 Evgeny Kluev：非常感谢，看起来就是这样。我会尝试发布结果（这需要一些时间；我还有其他一些事情要实施）。顺便说一句，对我来说幸运的是，OpenCV 包含 SVD 计算器，这大大简化了事情。 Evgeny Kluev：对于这么晚的回复，我深表歉意：我有更重要的项目。我想问一下;我应该如何解释比例因子？这些数字确实很大（200 - 2000）或很小（~0.5），但根据我的判断，规模应该在 1-4 左右。此外，不同轴的比例因子通常不同（例如 [2000, 200, 20]）。 实际上没有办法直接从奇异值中获取比例因子。我的错。对不起。基于 SVD 的算法可能适用于此，但我不知道如何。无论如何，你冷尝试更通用的迭代最近点算法。 你看过我下面的回答了吗？您也可以从 Eigen 获得比例 eigen.tuxfamily.org/dox/… 当然这假设您有对应关系 【参考方案1】：由于您已经在使用 Kabsch 算法，只需查看 Umeyama's paper 即可扩展它以获得规模。您需要做的就是获得点的标准偏差并计算比例为：
(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
【讨论】：
正确答案并引用论文。尚未测试代码，但该方法工作正常并且在论文中得到充分证明，应该会得到更多的支持。至于 OP，我认为您在 D 中获得了较大的值，因为您首先需要将要分解的矩阵缩放 1/N，其中 N 是您的点数。事实证明人们通常不会这样做，因为即使没有它，平移和旋转估计也能正常工作。 @Laky 好吧，老实说，我也不缩放协方差矩阵（正如您在代码中看到的那样）。该算法可能会使用它，但比例计算不会。 D 中的大数字不一定有问题，因为尺度还取决于标准偏差，它本身可能很大（或很小），与尺度无关。不过感谢您的称赞。 附带说明，有些人更喜欢Horn's algorithm。它与 Kabsch 非常相似，只是它分解了一个 4x4 矩阵并且输出是一个四元数。对我来说，计算成本似乎稍微高一些，所以我坚持使用 Kabsch，然后将 3x3 矩阵转换为四元数。我不太确定是否有一个版本的 Horn 算法也可以恢复比例。 感谢您的出色回答。我已经对完整的问题here 进行了 20 行 Python 实现，我尝试对其进行调整以使其易于理解。 @nh2 抱歉，但它给了我 NameError: global name 'a1' is not defined at line 34.. 应该是 P 吗？【参考方案2】：对于 3D 点，该问题称为绝对方向问题。可从 Eigen http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60 和论文 http://web.stanford.edu/class/cs273/refs/umeyama.pdf 获得 c++ 实现
您可以通过 opencv 使用它，通过调用 cv::cv2eigen() 将矩阵转换为特征值。
【讨论】：
【参考方案3】：从两组点的平移开始。使它们的质心与坐标系的原点重合。平移向量就是这些质心之间的区别。
现在我们有两组坐标表示为矩阵 P 和 Q。一组点可以通过应用一些线性算子（执行缩放和旋转）从其他点获得。该运算符由 3x3 矩阵 X 表示：
P * X = Q
要找到合适的缩放/旋转，我们只需要求解这个矩阵方程，找到 X，然后将其分解为多个矩阵，每个矩阵代表一些缩放或旋转。
解决它的一个简单（但可能在数值上不稳定）的方法是将方程的两个部分乘以转置矩阵 P（去除非方阵），然后将两者相乘等式的部分反转为 PT * P:
PT * P * X = PT * 问
X = (PT * P)-1 * PT * Q
将Singular value decomposition 应用于矩阵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 algorithm 即可获取平移/旋转值。然后执行这些平移和旋转（质心应该与坐标系的原点重合）。然后为每对点（和每个坐标）估计Linear regression。线性回归系数正是比例因子。
【讨论】：
你是如何确定 U 和 V 矩阵的旋转的？旋转是弧度吗？ @blueshift：在二维情况下，这很简单：矩阵分量只是角度的正弦和余弦。在 3D 情况下：旋转轴由特征向量确定，然后您会找到垂直于轴的任何向量与由旋转矩阵变换的同一向量之间的角度。请参阅wikipedia 中的详细信息。 谢谢，Evgeny，特别是***链接。在您的示例中，您如何确定是使用 -0.7317 还是 0.7317 作为余弦？ @blueshift：老实说，我不知道。问题是来自 SVD 的矩阵的某些行被否定了，我不知道是哪一个。使用这些数据的唯一方法（不深入研究问题）是尝试两种可能性，将旋转应用于实际点，看看哪一个是正确的。【参考方案4】：很好的解释Finding optimal rotation and translation between corresponding 3D points
代码在 matlab 中，但使用 cv::SVD 函数转换为 opengl 很简单
【讨论】：
我对这么晚的回复深表歉意；我有更重要的项目。非常感谢您的链接，它帮助了我。我用 cv::SVD 实现了它，它工作得很好。但我还有一个问题：规模呢？来自 SVD 的比例值太大或太小。 这种方法只适用于同等大小的云。通常你知道云是相同的比例，我没有尝试过但是有一个比例方法google.com/…【参考方案5】：您可能想尝试 ICP（迭代最近点）。 给定两组 3d 点，它将告诉您从第一组到第二组的变换（旋转 + 平移）。 如果您对 c++ 轻量级实现感兴趣，请尝试 libicp。
祝你好运！
【讨论】：
感谢您的回答（并对迟到的回复表示歉意）。我改用 Kabsch 算法。另外，我需要一个比例值。 如果点之间的对应关系已知，ICP 会不必要地过于复杂和缓慢。对于 Kabsch，您已经知道点之间的对应关系。 ICP 可以匹配任何两个点集（不同数量的点，仅部分重叠的形状，...），但当然价格要高得多，并且存在无法收敛到最佳解决方案的风险。【参考方案6】：可以通过Procrustes Analysis 检索一般转换以及比例尺。它通过将对象相互叠加并尝试从该设置估计转换来工作。它已在 ICP 的上下文中多次使用。事实上，你的偏好，Kabash 算法是这种情况的一个特例。
此外，Horn 的对齐算法（基于四元数）也找到了一个非常好的解决方案，同时非常高效。 Matlab implementation 也可用。
【讨论】：
注意：我正在监控这个，但是我真的很忙于其他工作，所以我需要时间才能有时间回到这个项目。无论如何，感谢您回答这么老的问题。【参考方案7】：如果您的点在所有方向上均匀缩放，则可以在没有 SVD 的情况下推断出比例（我也无法理解 SVD-s 比例矩阵）。这是我解决相同问题的方法：
测量每个点到点云中其他点的距离，以获得二维距离表，其中 (i,j) 处的条目是标准 (point_i-point_j)。对另一个点云做同样的事情，这样你就得到了两张表——一张用于原始点，另一张用于重建点。
将一个表中的所有值除以另一表中的对应值。因为这些点彼此对应，所以距离也是如此。理想情况下，结果表的所有值都彼此相等，这就是比例。
分区的中值应该非常接近您正在寻找的比例。平均值也很接近，但我选择中位数只是为了排除异常值。
现在您可以使用比例值来缩放所有重建的点，然后继续估计旋转。
提示：如果点云中的点太多而无法找到所有点之间的距离，那么距离的较小子集也可以工作，只要它是两个点云的相同子集。理想情况下，如果没有测量噪声，则只有一对距离可以工作，例如，当一个点云通过旋转它直接从另一个点云导出时。
【讨论】：
【参考方案8】：您也可以使用 BaoweiLin 提出的ScaleRatio ICP 代码可以在github上找到
【讨论】：
以上是关于在两组点上找到平移和缩放以获得距离的最小二乘误差？的主要内容，如果未能解决你的问题，请参考以下文章