使用eigen库将四元数转换为旋转矩阵时,发现有一个地方的转换是错误的,而且有时出错有时没错,代码如下
double x0 = 7203.5308;
double y0 = 2389.2666;
double z0 = 2505.2303;
double a0 = 0.71953;
double b0 = -0.0154782;
double c0 = 0.0109039;
double d0 = 0.00181833;
Quaterniond r0(-a0,b0,c0,d0);
Vector3d p0(x0,y0,z0);
Isometry3d t0 = Isometry3d::Identity();
Eigen::Matrix3d R;
R = Matrix3d(r0);
t0.rotate(r0);
t0.pretranslate(p0);
对应的结果为
使用matlab做验证,代码如下
p0 = [7203.53082389.26662505.2303]';
r0 = [0.71953-0.01547820.01090390.00181833];
R0 = quat2dcm(r0)
T0 = [R0 p0];
T0 = [T0; [0 0 0 1]]
结果如下
R0 =
0.9995 0.0044 -0.0304
-0.0057 0.9991 -0.0429
0.0302 0.0431 0.9986
可以发现,二者结果并不相等,eigen求出的结果中,非对角元素约为matlab结果中对应元素的一半,且eigen求出的结果并不归一。
问题在于:输入的四元数没有归一化,在matlab中,可能将四元数的虚数部分自动进行了归一化处理,但是eigen库没有进行,直接进行了相关求解,使得结果不一致。
解决方案:在使用四元数前,先校核四元数的正确性