Eigen四元数初始化的坑！！！一天的教训！

四元数初始化有多种形式，容易出错的是：由于Eigen中四元数存储顺序（从打印可知）是xyzw，导致赋值出现的错误：

方式一：4个标量

Quaterniond q1(1, 2, 3, 4); // 第一种方式实部为1 ，虚部234

方式二：Vector4d

Quaterniond q2(Vector4d(1, 2, 3, 4)); // 第二种方式实部为4 ，虚部123

方式三：数组

Quaterniond q2(tmp_q); // 第三种方式，double tmp_q[4]; 实部为4 ，虚部123

大坑：方式三不能用！解决方案：我在使用的时候改成了Eigen::Quaternion<T> q_map_vinsWorld(tmp_q[0], tmp_q[1], tmp_q[2], tmp_q[3])因为ceres::RotationMatrixToQuaternion(tmp_R_transposed, tmp_q)得到了wxyz顺序的T tmp_q[4]

最终结论：推荐方式一。

1、通过向量初始化时，要注意Eigen四元数的存储顺序，从cout << q.coeffs() << endl; 打印可知，其内部存储是：xyzw。

2、第二种第三种怀疑是构造初始化时直接把q.coeffs() = Vector4d / double* ，而第一种方式，构建初始化时自动调整了顺序给coeffs。

3、本人出错记录：

（1）通过ceres::RotationMatrixToQuaternion(R_map_vinsWorld, tmp_q)得到了wxyz顺序的T tmp_q[4]

（2）Eigen::Quaternion<T> q_map_vinsWorld(tmp_q)直接赋值，结果出错！！！

T tmp_q[4];
T R_map_vinsWorld[9];
YawPitchRollToRotationMatrix(extri_yaw_baselink_IMU[0], T(0), T(0), R_map_vinsWorld);//建图程序中R_map_vinsWorld和R_baselink_IMU的yaw角相同
ceres::RotationMatrixToQuaternion(R_map_vinsWorld, tmp_q);
cout<<"extri_yaw_baselink_IMU[0]="<<extri_yaw_baselink_IMU[0]<<endl;
cout<<"tmp_q="<<tmp_q[0]<<" "<<tmp_q[1]<<" "<<tmp_q[2]<<" "<<tmp_q[3]<<endl;
Eigen::Quaternion<T> q_map_vinsWorld(tmp_q);
cout<<"q_map_vinsWorld="<<q_map_vinsWorld.w()<<" "<<q_map_vinsWorld.x()<<" "<<q_map_vinsWorld.y()<<" "<<q_map_vinsWorld.z()<<endl;

输出如下：

extri_yaw_baselink_IMU[0]=-90
tmp_q =0.707107 -0 -0 0.707107
q_map_vinsWorld=0.707107 0.707107 -0 -0

3、注意ceres中有专门的EigenQuaternionParameterization来针对解决 Eigen四元数中通过q.coeffs.data()得到double * 时xyzw的情况。如果通过 4个double标量赋值，就可以wxyz赋值，避免EigenQuaternionParameterization，而直接用QuaternionParameterization

好比6DOF优化时

                q_array[i][0] = tmp_q.w();q_array[i][1] = tmp_q.x();q_array[i][2] = tmp_q.y();q_array[i][3] = tmp_q.z();sequence_array[i] = (*it)->sequence;problem.AddParameterBlock(q_array[i], 4, local_parameterization);

但在CameraFatory中

        ceres::LocalParameterization* quaternionParameterization = new EigenQuaternionParameterization;problem.SetParameterization(transformVec.at(i).rotationData(),quaternionParameterization);
其原因在于他自己写的rotationData函数，用的Eigen::Quaternion.coeffs().data()中内部的存储形式：
double*
Transform::rotationData(void)
{return m_q.coeffs().data();
}

int main() {cout << "Quaternion from vector4d(1, 2, 3, 4) is:\n"        << Quaterniond(Vector4d(1, 2, 3, 4)).coeffs().transpose() << endl;   cout << "Quaternion from (1, 2, 3, 4) is:\n"        << Quaterniond(1, 2, 3, 4).coeffs().transpose() << endl;   return 0;
}// 输出：Quaternion from vector4d(1, 2, 3, 4) is:
1 2 3 4
Quaternion from (1, 2, 3, 4) is:
2 3 4 1

Eigen四元数初始化的坑！！！一天的教训！相关推荐

truffle unbox react 初始化踩坑
truffle unbox react 初始化踩坑 1.文件位置引起的错误一定要在package.json 所在文件夹下npm run start ,新版本一定要进入client文件夹再npm ru ...
python dtype=float 是32还是64_【Python】numpy数组用dtype=float16初始化的坑
[Python坑系列] 为了节约空间,使用numpy数组时候采用了float16,结果发现精度远远不达标默认的floar64是最接近原本浮点数的 a = 0.0001 * np.ones(10,np ...
萤石云初始化爬坑2——摄像头预览
下面两张是萤石云的官方开发文档截图最近几天博主一直在做摄像头的预览(根据萤石云的官方文档) 但是呢,预览的画面总是漆黑一片. 研究了几天,找到了问题的所在. 请注意下图中标记的方法根据开发文档的介 ...
FaceBook的一个一直报请先初始化的坑
在一个项目里引用了4.36.0版本的facebook(这个版本的这个接口FacebookSdk.sdkInitialize() 已经被废弃了),导出arr引用到eclipse项目中之后就一直报错:W ...
【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）
矩阵的使用可参考系列博客:点击此处原文链接:基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换. 也可以参考另一篇博客:eigen 中四元数.欧拉角.旋转矩阵.旋转向量. ...
车辆姿态表达：旋转矩阵、欧拉角、四元数的转换以及eigen、matlab、pathon方法实现
目录 1 概述 2 原理 2.1 旋转矩阵 2.1.1 绕x轴旋转 2.1.2 绕y轴旋转 2.1.3 绕z轴旋转 2.2 欧拉角 2.2.1 基本思想 2.2.2 欧拉角的缺点 2.3 四元数 2. ...
Eigen 部分功能介绍
vector3d vector3d 默认为列向量 Eigen::Vector3d a(1.0, 2.0, 3.0) std::cout << "a = \n" < ...
SLAM——之Eigen函数库
0. Eigen/四元数/欧拉角/旋转矩阵相关系列文章 SLAM--之Eigen入门(矩阵运算及几何模块) SLAM--之Eigen函数库,一个相对复杂的EIgen使用实例 SLAM--Eigen函 ...
Eigen库学习教程(全)
说明:本教程主要是对eigen官网文档做了一个简要的翻译,参考了eigen官网以及一些博主的技术贴,在此表示感谢. Eigen是一个高层次的C ++库,有效支持线性代数,矩阵和矢量运算,数值分析及其相 ...

Eigen四元数初始化的坑！！！一天的教训！

Eigen四元数初始化的坑！！！一天的教训！相关推荐

最新文章

热门文章

Eigen四元数 初始化的坑！！！一天的教训！

Eigen四元数 初始化的坑！！！一天的教训！相关推荐

最新文章

热门文章

Eigen四元数初始化的坑！！！一天的教训！

Eigen四元数初始化的坑！！！一天的教训！相关推荐