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

最新推荐文章于 2024-05-23 18:59:34 发布

原创

最新推荐文章于 2024-05-23 18:59:34 发布 · 2.2k 阅读

11 ·

CC 4.0 BY-SA版权

这篇博客探讨了在ROS和Eigen中使用旋转量时需要注意的问题，如四元数的顺序差异。作者建议在工程中优先使用ROS的旋转量，并警告不要混合使用ROS和Eigen的转换方法。博客详细介绍了旋转向量、四元数、欧拉角和旋转矩阵之间的转换函数，并提供了测试代码示例。

坐标变换学习笔记—代码篇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$ 是通过 $a t a n 2$ 的方式求出来的，与理论篇中的式 $(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{aligned}\mathbf{R} \tag{1.1.1} =& cos(\theta) \mathbf{I} + (1-cos(\theta))\mathbf{n}\mathbf{n}^T + sin(\theta) \mathbf{n}^{\wedge} \\ =& cos(\theta) \mathbf{I} + (1-cos(\theta))\left[\begin{matrix}n_x^2 & n_xn_y & n_xn_z \\ n_yn_x & n_y^2 & n_yn_z \\ n_zn_x & n_zn_y & n_z^2 \end{matrix}\right] + sin(\theta) \left[\begin{matrix}0 & -n_z & n_y \\ n_z & 0 & -n_x \\ -n_y & n_x & 0 \end{matrix}\right] \\ =& cos(\theta) \mathbf{I} + (1-cos(\theta))\left[\begin{matrix}n_x^2 & 0& 0 \\ 0 & n_y^2 & 0 \\ 0 & 0 & n_z^2 \end{matrix}\right] + \left[\begin{matrix}0 & (1-cos(\theta))n_xn_y - sin(\theta)n_z & (1-cos(\theta))n_xn_z + sin(\theta)n_y \\ (1-cos(\theta))n_yn_x+sin(\theta)n_z & 0 & (1-cos(\theta))-sin(\theta)n_x \\ (1-cos(\theta))n_zn_x-sin(\theta)n_y & (1-cos(\theta))n_zn_y+sin(\theta)n_x & 0 \end{matrix}\right] \end{aligned}$