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