eigen和ceres中的四元数

Cc1924

已于 2022-04-21 18:45:15 修改

阅读量2.3k

点赞数 2

分类专栏： SLAM 文章标签： C++

于 2022-04-19 11:59:37 首次发布

本文链接：https://blog.csdn.net/qq_42731705/article/details/124269886

版权

SLAM 专栏收录该内容

81 篇文章

订阅专栏

文章目录

1.ceres中的四元数乘法函数
2.eigen中的四元数初始化
3.VINS-Mono中使用四元数优化
- 3.1.VIO初始化中视觉sfm最后做global BA
- 3.2.后端优化中自定义的7维位姿变量

1.ceres中的四元数乘法函数

ceres::QuaternionRotatePoint(quantion, p, result )函数

ceres中定义了ceres::QuaternionRotatePoint(quantion, p, result )函数，就是实现四元数对点的乘法，其内部调用的是UnitQuaternionRotatePoint函数进行计算。

四元数乘法：
$\quad q^{-1} = (w, -x, -y, -z), \quad p = (0, a, b, c) \\ p' = q \otimes p \otimes q^{-1}$

$\begin{aligned} qp &= (w, x, y, z) \otimes (0, a, b, c) \\ &=(0-xa-yb-zc, \quad wa+0+yc-zb, wb+0-xc+za,\quad wc+0+xb-ya) \end{aligned}$

$\begin{aligned} \{q \otimes p \otimes q^{-1}\}_w =& -w(xa+yb+zc)+ x(wa+yc-zb) +y(wb-xc+za)+z(wc+xb-ya) \\ =& (-wx+xw + yz-yz)a + (-wy-xz+yw+xz)b +(-wz+xy-xy+zw)c \\ =& 0 \end{aligned}$

$\begin{aligned} \{q \otimes p \otimes q^{-1}\}_x =& x(xa+yb+zc)+w(wa+yc-zb) - z(wb-xc+za)+y(wc+xb-ya) \\ =& (x^2+w^2-y^2-z^2)a + 2(xy-wz)b + 2(xz+wy)c \\ =&(1+2(-y^2-z^2))a + 2(xy-wz)b + 2(xz+wy)c \\ =& 2(-y^2-z^2)a + 2(xy-wz)b + 2(xz+wy)c + a \end{aligned}$

$\begin{aligned} \{q \otimes p \otimes q^{-1}\}_y =& y(xa+yb+zc) + w(wb-xc+za) -x(wc+xb-ya) + z(wa+yc-zb) \\ =&2(xy+wz)a+(y^2+w^2-x^2-z^2)b + 2(yz-wx)c \\ =&2(xy+wz)a + (1-2x^2-2z^2)b + 2(yz-wx)c \\ =&2(xy+wz)a + 2(-x^2-z^2)b + 2(yz-wx)c + b \end{aligned}$

$\begin{aligned} \{q \otimes p \otimes q^{-1}\}_z =& z(xa+yb+zc) + w(wc+xb-ya) -y(wa+yc-zb) + x(wb-xc+za) \\ =&2(xz-yw)a + 2(yz+xw)b + (z^2 + w^2 - x^2 - y^2)c \\ =& 2(xz-yw)a + 2(yz+xw)b + (1-2x^2-2y^2)c \\ =& 2(xz-yw)a + 2(yz+xw)b + 2(-x^2-y^2)c + c \end{aligned}$

从上面推导的计算公式可以看出两点：

使用虚四元数表示三维坐标点，即坐标作为四元数的虚部，实部为0。对这个坐标点构成的虚四元数执行四元数乘法，最后的结果确实是实部为0，即计算结果仍然是一个虚四元数，其虚部就代表旋转后的点的坐标。
表示旋转的四元数必须是单位四元数，否则从计算结果的xyz坐标可以看出来，假设不是单位四元数，那么xyzw每个系数相对单位四元数都有一个放大倍数，那么旋转后的点的坐标自然也就不正确了。

代码：

template <typename T> inline
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
  const T t2 =  q[0] * q[1];  //; wx
  const T t3 =  q[0] * q[2];  //; wy
  const T t4 =  q[0] * q[3];  //; wz
  const T t5 = -q[1] * q[1];  //; -x^2
  const T t6 =  q[1] * q[2];  //; xy
  const T t7 =  q[1] * q[3];  //; xz
  const T t8 = -q[2] * q[2];  //; -y^2
  const T t9 =  q[2] * q[3];  //; yz
  const T t1 = -q[3] * q[3];  //; -z^2

  //; x部分，和上面公式推导相符
  result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0];  // NOLINT
  //; y部分，和上面公式推导相符
  result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1];  // NOLINT
  //; z部分，和上面公式推导相符
  result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2];  // NOLINT
}

另外，另一篇博客中对这个四元数乘法计算进行了简化加速。
参考：ceres UnitQuaternionRotatePoint 利用单位四元数旋转向量的函数算法原理推导

2.eigen中的四元数初始化

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

方式1：4个标量

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

方式2：数组

double tmp_q = {1, 2, 3, 4}
Quaterniond q2(tmp_q);              // 第2种方式，double tmp_q[4];   实部为4 ，虚部123

这个属实是坑，看eigen中源码的定义：

/** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
 * its four coefficients \a w, \a x, \a y and \a z.
 *
 * \warning Note the order of the arguments: the real \a w coefficient first,
 * while internally the coefficients are stored in the following order:
 * [\c x, \c y, \c z, \c w]
 */
Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}

/** Constructs and initialize a quaternion from the array data */
Quaternion(const Scalar* data) : m_coeffs(data) {}

m_coeffs：eigen中四元数存储的系数，在内存中存储的顺序是(x, y, z, w)。
上面第1种方式就是把四元数的4个数全给出来赋值，注意注释说的，顺序是(w, x, y, z)，但是内部存储的时候系数存储顺序仍然是(x, y, z, w)。也就是这种方式只是提供了一个用户方便使用的接口。
上面第2中方式则是直接给系数m_coeffs赋值，所以传入的时候顺序应该是x, y, z, w。

总结：

尽量不要使用第二种数组赋值的方式，很容易出错。还是尽量使用显式赋值的方式，比如第1种方式。
还有在vins中看到使用四元数如下，可见已经使用了double数组存储四元数，但是最后还是显式的赋值，这样不容易出错。

for (int i = 0; i < frame_num; i++)
	{
		q[i].w() = c_rotation[i][0]; 
		q[i].x() = c_rotation[i][1]; 
		q[i].y() = c_rotation[i][2]; 
		q[i].z() = c_rotation[i][3]; 
		q[i] = q[i].inverse();
		//cout << "final  q" << " i " << i <<"  " <<q[i].w() << "  " << q[i].vec().transpose() << endl;
	}

如果赋值顺序错误会如何？比如上面用数组，正常数组中应该存储(x, y, z, w)，但是实际存的是(w, x, y, z)，那么最后看起来转成轴角的话，两个旋转轴会交叉。比如(1, 0, 0)变成(0, 1, 0)；旋转角度互补，如一个60度，一个120度。
ceres中的四元数和eigen存储顺序不同：ceres中优化四元数的话，其存储顺序是(w, x, y, z)，即实部在前；而eigen中的四元数存储顺序是(x, y, z, w)，即实部在后。

3.VINS-Mono中使用四元数优化

在VINS-Mono中，VIO初始化和后端优化中都使用了ceres进行了优化，并且二者使用的四元数存储顺序还不同。

3.1.VIO初始化中视觉sfm最后做global BA

initial_sfm.cpp文件中相关部分如下，可以看到这里使用了ceres中定义好的local_parameterization，即QuaternionParameterization，也就是说这里使用的是ceres中默认定义的四元数，即实部在前的四元数，所以内存中存储的待优化的四元数也应该是实部在前的四元数。

//full BA
ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//cout << " begin full BA " << endl;
for (int i = 0; i < frame_num; i++)
{
	//double array for ceres
	c_translation[i][0] = c_Translation[i].x();
	c_translation[i][1] = c_Translation[i].y();
	c_translation[i][2] = c_Translation[i].z();
	//; 实部在前的四元数，因为用的是ceres默认的四元数localparameterization
	c_rotation[i][0] = c_Quat[i].w();  //; 给四元数的数组赋值，是wxyz的顺序
	c_rotation[i][1] = c_Quat[i].x();
	c_rotation[i][2] = c_Quat[i].y();
	c_rotation[i][3] = c_Quat[i].z();
	problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
	problem.AddParameterBlock(c_translation[i], 3);
	if (i == l)
	{
		problem.SetParameterBlockConstant(c_rotation[i]);
	}
	if (i == l || i == frame_num - 1)
	{
		problem.SetParameterBlockConstant(c_translation[i]);
	}
}

3.2.后端优化中自定义的7维位姿变量

estimator.cpp文件中相关部分如下，其中7维的位姿变量分别是3维的位置和4维的四元数。

for (int i = 0; i < WINDOW_SIZE + 1; i++)
 {
     ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
     problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
     problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
 }
 for (int i = 0; i < NUM_OF_CAM; i++)
 {
     ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
     problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
     if (!ESTIMATE_EXTRINSIC)
     {
         ROS_DEBUG("fix extinsic param");
         problem.SetParameterBlockConstant(para_Ex_Pose[i]);
     }
     else
         ROS_DEBUG("estimate extinsic param");
 }

其中的PoseLocalParameterization是作者自己定义的和para_Pose对应的局部参数，其定义如下。可以看到在定义加法Plus的时候，使用前3维作为平移，然后使用数组赋值的方式把内存中后4维定义为四元数，也就是说此时内存中的四元数应该是实部在后的，即xyzw的形式。和ceres默认的方式不一样！

class PoseLocalParameterization : public ceres::LocalParameterization
{
    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
    virtual bool ComputeJacobian(const double *x, double *jacobian) const;
    virtual int GlobalSize() const { return 7; };
    virtual int LocalSize() const { return 6; };
};

//; 注意这个位姿的排列：优化变量(x)的前3个是位置xyz, 后4个是实部在后的四元数(x, y, z, w)
bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> _p(x);

    //; 看这里四元数赋值就知道了，用的是数组赋值的方法，因此对应的x+3到最后的内存中存储的四元数顺序应该是x, y, z, w,即实部在后
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);
    
    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();

    return true;
}
bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();

    return true;
}