原本想使用

ceres::RotationMatrixToQuaternion把VINS中YawPitchRollToRotationMatrix()得到的R[9] RowMajor向量 转成四元数,结果出现BUG!!!

原因R[9]是RowMajor,但ceres::RotationMatrixToQuaternion的输入是ColumnMajor,故得到的四元数出错了(由于RowMajor和ColumnMajor矩阵为 逆,故四元数也变成逆了)

template <typename T>
void RotationMatrixToQuaternion(const T* R, T* angle_axis) {
  RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), angle_axis);
}

解决方案:在ceres::RotationMatrixToQuaternion输入之前用RotationMatrixTranspose对旋转向量矩阵向量求一个矩阵转置,类似于求逆!

YawPitchRollToRotationMatrix(extri_yaw_baselink_IMU[0], T(0), T(0), R_map_vinsWorld);//建图程序中R_map_vinsWorld和R_baselink_IMU的yaw角相同
RotationMatrixTranspose(R_map_vinsWorld, tmp_R_transposed);//将RowMajor的旋转矩阵向量转置成ColumnMajor
ceres::RotationMatrixToQuaternion(tmp_R_transposed, tmp_q); //注意:该函数的输入向量为ColumnMajor!如果输入的是RowMajor的矩阵向量,必须实现做转置!
Eigen::Quaternion<T> q_map_vinsWorld(tmp_q[0], tmp_q[1], tmp_q[2], tmp_q[3]);//此处tmp_q为double数组【double tmp_q[4]】,通过ceres::RotationMatrixToQuaternion(R_map_vinsWorld, tmp_q)得到的

注意:!!!一天的教训

一定要用Eigen::Quaternion<T> q_map_vinsWorld(tmp_q[0], tmp_q[1], tmp_q[2], tmp_q[3])

而不能用Eigen::Quaternion<T> q_map_vinsWorld(tmp_q),后者是个BUG!!!!

 

 


版权声明:本文为delovsam原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
原文链接:https://blog.csdn.net/delovsam/article/details/104469738