在乘以大量旋转矩阵后，由于舍入问题（去正交化），最终结果可能不再是有效的旋转矩阵
重新正交化的一种方法是按照以下步骤操作：
Eigen 库中是否有一些东西通过隐藏所有细节来做同样的事情？或者有没有更好的食谱？
由于特殊的奇点情况，这个过程必须小心处理，所以如果 Eigen 提供了一个更好的工具，那就太好了。
【问题讨论】：
标签： algorithm math rotation eigen rotational-matrices
我没有使用 Eigen，也没有费心去查找 API，但这里有一个简单、计算成本低且稳定的过程来重新正交化旋转矩阵。这个正交化过程取自Direction Cosine Matrix IMU: Theory William Premerlani 和 Paul Bizard；等式 19-21。
让x、y 和z 成为（稍微混乱的）旋转矩阵的行向量。设error=dot(x,y) 其中dot() 是点积。如果矩阵是正交的，x 和y 的点积，即error 将为零。
error 分布在 x 和 y 中：x_ort=x-(error/2)*y 和 y_ort=y-(error/2)*x。第三行z_ort=cross(x_ort, y_ort)，根据定义与x_ort和y_ort正交。
现在，您仍然需要规范化 x_ort、y_ort 和 z_ort，因为这些向量应该是单位向量。
x_new = 0.5*(3-dot(x_ort,x_ort))*x_ort
y_new = 0.5*(3-dot(y_ort,y_ort))*y_ort
z_new = 0.5*(3-dot(z_ort,z_ort))*z_ort
到此为止，已经完成了。
使用 Eigen 提供的 API 应该很容易实现这一点。您可以轻松地提出其他正交化程序，但我认为它不会在实践中产生显着差异。我在我的运动跟踪应用程序中使用了上述程序，它运行良好；它既稳定又快速。
【讨论】：
x_new = 0.5*(3-dot(x_ort,x_ort))*x_ort。您的代码正在做的是x_new = 1/ (2*(3-dot(x_ort,x_ort))*x_ort) 请注意，由于操作优先级，括号是如何重新排列的。我猜人们复制粘贴了您的代码，但对他们不起作用，他们也没有调试。
您可以使用 QR 分解系统地重新正交化，用 Q 因子替换原始矩阵。在库例程中，如有必要，您必须通过否定 Q 中的相应列来检查和纠正 R 的对角线条目是否为正（如果原始矩阵接近正交，则接近 1）。
最接近给定矩阵的旋转矩阵 Q 是从极坐标或 QP 分解中获得的，其中 P 是半正定对称矩阵。 QP 分解可以迭代计算或使用 SVD 计算。如果后者有分解USV'，那么Q=UV'。
【讨论】：
rotation.householderQr().householderQ()。但是，我不熟悉 houseHolder 方法。这够好吗？您能否在回答中详细说明“检查并更正”和“否定相应列”作为额外步骤？任何参考资料或代码示例也会很棒！
I 来测试它。结果可能是[Q, R] = [-I, -I]。这是因为 Householder 反射被选择为最稳定的，即避免除以零或接近零。
Singular Value Decomposition 应该非常健壮。引用参考：
令M=UΣV为M的奇异值分解，则R=UV。
对于您的矩阵，Σ 中的奇异值应该非常接近 1。矩阵 R 保证为orthogonal，这是旋转矩阵的定义属性。如果在计算原始旋转矩阵时没有任何舍入误差，那么 R 将与您的 M 在数值精度范围内完全相同。
【讨论】：
同时：
#include <Eigen/Geometry>
Eigen::Matrix3d mmm;
Eigen::Matrix3d rrr;
rrr << 0.882966, -0.321461, 0.342102,
0.431433, 0.842929, -0.321461,
-0.185031, 0.431433, 0.882966;
// replace this with any rotation matrix
mmm = rrr;
Eigen::AngleAxisd aa(rrr); // RotationMatrix to AxisAngle
rrr = aa.toRotationMatrix(); // AxisAngle to RotationMatrix
std::cout << mmm << std::endl << std::endl;
std::cout << rrr << std::endl << std::endl;
std::cout << rrr-mmm << std::endl << std::endl;
这是个好消息，因为我可以摆脱我的自定义方法并减少头痛（如何确定他会处理所有奇点？），
但我真的很想听听您对更好/替代方法的看法：)
【讨论】：
rrr 实际上是一个旋转矩阵。如果不是（这就是你的问题，你的旋转矩阵搞砸了），那么它很可能做错了什么。这种方法似乎更像是一种临时破解，而不是一种解决方案。
另一种方法是使用Eigen::Quaternion 来表示您的轮换。这更容易标准化，rotation*rotation 产品通常更快。如果您有很多 rotation*vector 产品（具有相同的矩阵），您应该将四元数本地转换为 3x3 矩阵。
【讨论】：