我不使用Eigen，也没费心查找其API，但这里有一个简单、计算便宜且稳定的程序来重新正交化旋转矩阵。这个正交化过程取自William Premerlani和Paul Bizard的Direction Cosine Matrix IMU: Theory，方程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实现这个应该很容易。你可以轻松地想出其他正交化过程，但我认为在实践中不会有明显的差异。我在我的运动跟踪应用程序中使用了上述过程，效果非常好；它既稳定又快速。
4
给定矩阵的最接近的旋转矩阵Q是从极分解或QP分解获得的，其中P是正半定对称矩阵。 QP分解可以通过迭代或使用SVD计算。如果后者具有因子分解USV'，则Q = UV'。
5
rotation.householderQr().householderQ()。我不太熟悉houseHolder方法。这样做足够好吗？此外，您能否在回答中详细说明“检查和纠正”以及“否定相应列”作为额外步骤的含义？任何参考资料或代码示例也将非常有帮助！ - Shital ShahI来测试它。结果可能是[Q，R] = [-I，-I]。这是因为选择Householder反射以使其最稳定，即避免除以零或接近零。 - Lutz LehmannM是我们想要进行正交化的矩阵，R是最接近M的旋转矩阵。
解析解决方案
Matrix R = M*inverse(sqrt(transpose(M)*M));
迭代解法
// To re-orthogonalize matrix M, repeat:
M = 0.5f*(inverse(transpose(M)) + M);
// until M converges
M收敛于最近的旋转矩阵R。每次迭代，精度位数将大致增加一倍。
检查(M - M^-T)/2元素平方和是否小于您的误差目标的平方，以确定(M + M^-T)/2何时达到精度阈值。其中M^-T是M的逆转置。
为什么它有效
我们想找到最接近M的旋转矩阵R。我们将误差定义为元素差的平方和。即最小化trace((M - R)^T (M - R))。
解析解是R = M (M^T M)^-(1/2)，在此处概述。
问题在于这需要找到M^T M的平方根。然而，如果我们注意到，有许多矩阵其最接近的旋转矩阵是R。其中之一是M (M^T M)^-1，它简化为M^-T，即逆转置。好处是M和M^-T在R的两侧（直观地类似于a和1/a在1的两侧）。
我们认识到平均值(M + M^-T)/2将更接近R，并且因为它是线性组合，也将保持R作为最接近的旋转矩阵。递归执行此操作，我们收敛于R。
最坏情况下的收敛（推测性的）
因为它与巴比伦平方根算法相关，所以它的收敛速度是二次的。
一个矩阵M和误差e进行一次迭代后的最坏情况精度误差是nextE：
nextE = e^2/(2 e + 2)
e = sqrt(trace((M - R)^T (M - R)))
R = M (M^T M)^-(1/2)
#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;
这是个好消息，因为我可以摆脱自己的定制方法并减少一个头痛问题 (如何确保处理所有的奇点？)，
但我真的想听听你有关更好/替代方法的意见 :)
3
rrr 实际上 是一个旋转矩阵。如果不是（这就是你的问题，你的旋转矩阵出了问题），那么它很可能会出错。这种方法似乎更像是一种临时的hack而不是解决方案。 - Ali另一种选择是使用 Eigen::Quaternion 来表示旋转。这样做更容易进行归一化，rotation*rotation 乘积通常更快。如果您有很多相同矩阵的 rotation*vector 乘积，您应该将四元数局部转换为一个 3x3 矩阵。
这里是使用Eigen的实现。 它将通过调整任意两个不正交的向量相互靠近/远离，最小化旋转矩阵所表示的旋转。
void orthonormalize(Eigen::Matrix3d &rot) {
Vector3d x = rot.col(0);
Vector3d y = rot.col(1);
Vector3d z = rot.col(2);
// normalize
x.normalize();
y.normalize();
z.normalize();
// orthogonalize
double errorXY = 0.5 * x.dot(y);
double errorYZ = 0.5 * y.dot(z);
double errorZX = 0.5 * z.dot(x);
rot.col(0) = x - errorXY * y - errorZX * z;
rot.col(1) = y - errorXY * x - errorYZ * z;
rot.col(2) = z - errorYZ * y - errorZX * x;
}
我们可以使用测试来验证它是否真正起作用。
double orthonormalityError(const Matrix3d &rot) {
Matrix3d prod = rot * rot.transpose();
return (prod - Matrix3d::Identity()).norm();
}
Matrix3d randomRotationMatrix() {
Matrix3d rand_mat = Matrix3d::Random();
HouseholderQR<Matrix3d> qr(rand_mat);
return rot = qr.householderQ();
}
void testOrthonormalize() {
Eigen::Matrix3d rot = randomRotationMatrix();
rot += Eigen::Matrix3d::Random() * 0.1;
std::cout << orthonormalityError(rot) << std::endl;
for (int j = 0; j < 4; j++) {
orthonormalize(rot);
std::cout << orthonormalityError(rot) << std::endl;
}
}
int main(int argc, char** argv) {
testOrthonormalize();
return 0;
}
输出结果为：
0.246795
0.00948343
1.45409e-05
3.59979e-11
3.4066e-16
原文链接
x_new = 0.5*(3-dot(x_ort,x_ort))*x_ort。而你的代码实际上是x_new = 1/ (2*(3-dot(x_ort,x_ort))*x_ort)。请注意括号的重新排列，因为操作优先级不同。我猜测人们复制粘贴了你的代码，但它对他们不起作用，他们没有进行调试。 - varagrawal