坐标变换学习笔记—代码篇Eigen

先说结论，关于ROS和Eigen中使用旋转量，个人认为有一下问题需要注意下：

• ROS中使用的四元数顺序是x,y,z,w; 而Eigen中使用的四元数顺序是w,x,y,z; 这里需要注意下。

• 在工程中，千万不要用Eigen中的方法转换由ROS中的方法得到的旋转量，也不要用ROS中的旋转方法转换Eigen得到的旋转量。

• 在工程中如果用到ROS，则建议优先使用ROS中的旋转量来表达旋转，因为它们事先确定了旋转顺序按照ZYX来，而不像Eigen中，可通过参数选择旋转顺序，得到不同的顺序的旋转量，容易给自己挖坑。

• 在工程中，尽量少用欧拉角。

旋转向量

旋转向量与其它旋转量间的转换

Eigen/src/Geometry/AngleAxis.h

四元数 $\to$ 旋转向量：

Eigen中，通过重载=操作符，实现旋转矩阵，四元数对转旋转向量的直接赋值，但是从下面的函数实现看，它的旋转角$\theta$是通过$atan2$的方式求出来的，与理论篇中的式$(3.1)$稍微有点差别。
Eigen/src/Geometry/AngleAxis.h，operator=(const QuaternionBase

/** Set \c *this from a \b unit quaternion.
  *
  * The resulting axis is normalized, and the computed angle is in the [0,pi] range.
  * 
  * This function implicitly normalizes the quaternion \a q.
  */
template<typename Scalar>
template<typename QuatDerived>
EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
{
   
  EIGEN_USING_STD_MATH(atan2)
  EIGEN_USING_STD_MATH(abs)
  Scalar n = q.vec().norm();
  if(n<NumTraits<Scalar>::epsilon())
    n = q.vec().stableNorm();

  if (n != Scalar(0))
  {
   
    m_angle = Scalar(2)*atan2(n, abs(q.w()));
    if(q.w() < Scalar(0))
      n = -n;
    m_axis  = q.vec() / n;
  }
  else
  {
   
    m_angle = Scalar(0);
    m_axis << Scalar(1), Scalar(0), Scalar(0);
  }
  return *this;
}

旋转向量 $\to$ 旋转矩阵：

将$\mathbf{n}=[n_x{n}_y{n}_z{]}^T$代入展开可得：
$$\begin{array}{rl}\mathbf{R}=& cos(\theta )\mathbf{I}+(1-cos(\theta ))\mathbf{n}{\mathbf{n}}^T+sin(\theta ){\mathbf{n}}^{\wedge }\\ =& cos(\theta )\mathbf{I}+(1-cos(\theta ))[\begin{array}{c}{n}_x^2\\ n_y{n}_x\\ n_z\end{array}\\ =& \end{array}$$