SVD求解ICP旋转矩阵不正确处理

背景:

手写SVD方式的ICP求解时，发现很多时候求解出的旋转矩阵R的数值不对。经过一番排查和查找后发现需要对SVD求解的R做如下特殊处理。

if (R.determinant() < 0) {LOG(INFO) << "R determinant < 0, change R to -R";Vt.block<1, 3>(2, 0) *= -1;R = (U * Vt);}

原因:

以下为AI解答，详细原理还需要研究

在选定的代码段中，进行的是对旋转矩阵 R 的调整，以确保其正确性。这段代码的目的是检查旋转矩阵的行列式，并根据行列式的值决定是否需要调整旋转矩阵的方向。

具体来说：

1. 检查行列式：

   if (R.determinant() < 0) {
   

   这里判断旋转矩阵 R 的行列式是否小于零。如果行列式为负，表示该矩阵表示了一个反向变换或反向旋转。

2. 记录日志：

   LOG(INFO) << "R determinant < 0, change R to -R";
   

   如果判断成立，这行代码输出一条日志信息，用于提示用户旋转矩阵的方向不正确。

3. 调整 Vt 矩阵：

   Vt.block<1, 3>(2, 0) *= -1;
   

   该行代码通过将 Vt 矩阵的最后一行（与行列式为负相关的行）乘以 -1 来反转 Vt 的方向。这是通过选择特定的矩阵区块来实现的。

4. 重新计算旋转矩阵：

   R = (U * Vt);
   

   最后，根据调整后的 Vt 重新计算旋转矩阵 R。

为什么需要这样做？

旋转矩阵的行列式应始终为 1（单位矩阵），任何小于 0 的行列式都表示矩阵的反向旋转。通过这种方式，可以确保计算出的旋转矩阵 R 是有效且一致的。

可能的影响

如果不处理行列式小于零的情况，可能导致刚体变换不正确，例如在后续的点云配准或者三维转化中产生错误的旋转效果。

这段代码的逻辑在计算点云刚体变换时是非常重要的步骤，可以帮助确保结果的正确性。