C++ Eigen - 使用变换矩阵，实现两个传感器之间的坐标变换

原创于 2024-07-16 12:00:00 发布 · 1.4k 阅读

30 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #矩阵 #android

文章目录

1. 需求描述

实际项目中，有Lidar和 IMU两个传感器，上下背靠背安装。设Lidar所在坐标系为1系，IMU所在坐标系为2系，且均为右手系。

其中2系的x轴与1系的y轴方向相同，2系的y轴与1系z轴方向相反，2系的z轴与1系的x轴相反,两个坐标系原点重.

求1系中 (0.2, 0.5, 1) 在2系中的坐标。请自己编写一个c++程序实现它，并用Cmake编译，得到能输出答案的可执行文件

在这里插入图片描述

2. 需求分析

整个变化经历了两次旋转：

经分析，整个变化经历了两次旋转（动轴）：
（1）先绕Z轴旋转90度(逆时针)；
（2）再绕第一次旋转后的X轴旋转90度(顺时针)；

也可以看做（动轴）：
（1）先绕Y轴旋转90度(顺时针)；
（2）再绕第一次旋转后的Z轴旋转90度(顺时针)；

我们以第一种旋转为例进行介绍。建议读者按第二种方案自行联系验证。

通过手工分析草图不难发现：

（1）在1系中的原始坐标为： P(x, y, z)，即：P(0.2, 0.5, 1) ；

（2）经过第一次旋转后：在m系中的坐标应为：P’(x’, y’, z’)，即： P’(0.5, -0.2, 1)；

（3）经过第二次旋转后：在2系中的坐标应为：P"(x", y", z")，即： P"(0.5, -1, -0.2)；

注：m 为middle的意思。

也就是： P(0.2, 0.5, 1) ----> P"(0.5, -1, -0.2)

在这里插入图片描述

3. 理论基础(来源于本人:实践中测试)

（1）在使用旋转矩阵、旋转向量等进行坐标旋转时，使用公式： P_2 = T_2_1 * P_1, 可将1系下的P点，旋转至2系下；

（2）对于 T_2_1 的计算，需要注意：若为顺时针旋转，则计算过程中旋转角度值为正数；若为逆时针旋转，则计算过程中旋转角度值为负数；

（3）若T_2_1计算中，逆时针时也将旋转角度设置为了正数，则实际计算的旋转矩阵为： T_1_2, 需要将计算结果再求逆： T_1_2.inverse();

注意：这里的顺时针、逆时针的判定标准为：以旋转无关轴面向相关轴所构成平面的方向视角。

比如两次旋转是按照：

（1）先绕Z轴旋转90度：从Z轴面向X、Y轴所构成的平面的视角来看，为逆时针。

（2）再绕第一次旋转后的X轴旋转90度：从X轴面向Y、Z轴所构成的平面的视角来看，为顺时针；

在这里插入图片描述

若两次旋转是按照：

（1）先绕Y轴旋转90度(顺时针)：从Y轴面向X、Z轴所构成的平面的视角来看，为顺时针；

（2）再绕第一次旋转后的Z轴旋转90度：从Z轴面向X、Y轴所构成的平面的视角来看，为逆时针；

在这里插入图片描述

4. C++ Eigen代码 - 旋转方式1

两次旋转是按照：

（1）先绕Z轴旋转90度：从Z轴面向X、Y轴所构成的平面的视角来看，为逆时针。

（2）再绕第一次旋转后的X轴旋转90度：从X轴面向Y、Z轴所构成的平面的视角来看，为顺时针；



#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>



//变换矩阵: 旋转一个三维点
Eigen::Isometry3d GetIsometry(const Eigen::Vector3d& axis, double angle, const Eigen::Vector3d& translation)
{
  Eigen::Vector3d normalized_axis = axis.normalized();  // 归一化轴向量

  //转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  Eigen::AngleAxisd rotation_vector(angle, normalized_axis);

  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  T.rotate(rotation_vector);
  T.pretranslate(translation);

  return T;
}



int main(int argc, char **argv)
{
    //构造第一次旋转的旋转矩阵
    Eigen::Isometry3d T_w1 = Eigen::Isometry3d::Identity();
    //方案1: 实现与验证:
    {
      double angle = -M_PI / 2;                 //旋转角度（弧度），例如90度（PI / 2）
      Eigen::Vector3d axis(0, 0, 1);  //定义旋转轴: 绕z轴旋转
      Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
      T_w1 = GetIsometry(axis, angle, translation);

      //for test: 分析8个单位顶点的坐标变化:
      {
        std::cout << "方案1(首次旋转):  Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (T_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
      }
    }

    //方案2 实现与验证:
    Eigen::Isometry3d T_w1_temp = Eigen::Isometry3d::Identity();
    {
      double angle = M_PI / 2;                 //旋转角度（弧度），例如90度（PI / 2）
      Eigen::Vector3d axis(0, 0, 1);  //定义旋转轴: 绕z轴旋转
      Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
      T_w1_temp = GetIsometry(axis, angle, translation);

      //for test:  顶点的坐标变化:
      {
        std::cout << "方案2(首次旋转): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (T_w1_temp.inverse() * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
        std::cout << std::endl;
      }
    }
  }


  //构造第二次旋转的旋转矩阵
  Eigen::Isometry3d T_w2 = Eigen::Isometry3d::Identity();
  {
    double angle = M_PI / 2; // 旋转角度（弧度），例如90度（PI / 2）
    Eigen::Vector3d axis(1, 0, 0); //定义旋转轴: 绕X轴旋转
    Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
    T_w2 = GetIsometry(axis, angle, translation);
  }

  std::cout << "方案1(两次叠加):  Above Z plane: Eigen::Vector3d( 0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (T_w2 * (T_w1 * Eigen::Vector3d(0.2, 0.5, 1))).transpose() << std::endl;
  std::cout << "方案1(两次叠加, 矩阵满足结合律):  Above Z plane: Eigen::Vector3d( 0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (T_w2 * T_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
  std::cout << std::endl;

  return 0;
}

5. C++ Eigen代码 - 旋转方式2

两次旋转是按照：

（1）先绕Z轴旋转90度：从Z轴面向X、Y轴所构成的平面的视角来看，为逆时针。

（2）再绕第一次旋转后的X轴旋转90度：从X轴面向Y、Z轴所构成的平面的视角来看，为顺时针；

两次旋转是按照：

（1）先绕Y轴旋转90度(顺时针)：从Y轴面向X、Z轴所构成的平面的视角来看，为顺时针；

（2）再绕第一次旋转后的Z轴旋转90度：从Z轴面向X、Y轴所构成的平面的视角来看，为逆时针；



#include <iostream>
#include <cmath>


#include <Eigen/Core>
#include <Eigen/Geometry>




//变换矩阵: 旋转一个三维点
Eigen::Isometry3d GetIsometry(const Eigen::Vector3d& axis, double angle, const Eigen::Vector3d& translation)
{
  Eigen::Vector3d normalized_axis = axis.normalized();  // 归一化轴向量

  //转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  Eigen::AngleAxisd rotation_vector(angle, normalized_axis);

  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  T.rotate(rotation_vector);
  T.pretranslate(translation);

  return T;
}



int main(int argc, char **argv)
{
   //构造第一次旋转的旋转矩阵  
  Eigen::Isometry3d T_w1 = Eigen::Isometry3d::Identity();
  {
    //方案1: 实现与验证:
    {
      double angle = M_PI / 2;                 //旋转角度（弧度），例如90度（PI / 2）
      Eigen::Vector3d axis(0, 1, 0);  //定义旋转轴: 绕Y轴旋转
      Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
      T_w1 = GetIsometry(axis, angle, translation);

      //for test: 分析8个单位顶点的坐标变化:
      {
        //在Z平面(与Z轴垂直)以上，x,y 构成的二维平面中，逆时针寻找： (0.2, 0.5, 1), (-0.2, 0.5, 1), (-0.2,-0.5, 1), (0.2,-0.5, 1) 四个顶点
        std::cout << "方案1(首次旋转):  Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (T_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
      }
    }

    //方案2 实现与验证:    
     Eigen::Isometry3d T_w1_temp = Eigen::Isometry3d::Identity();
    {
      double angle = -M_PI / 2;                 //旋转角度（弧度），例如90度（PI / 2）
      Eigen::Vector3d axis(0, 1, 0);  //定义旋转轴: 绕Y轴旋转
      Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
      T_w1_temp = GetIsometry(axis, angle, translation);

      //for test: 分析8个单位顶点的坐标变化:
      {
        //在Z平面(与Z轴垂直)以上，x,y 构成的二维平面中，逆时针寻找： (1, 1), (-1, 1), (-1, -1), (1, -1) 四个顶点
        std::cout << "方案2(首次旋转): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (T_w1_temp.reverse() * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
      }
    }
  }


  //构造第二次旋转的旋转矩阵
   Eigen::Isometry3d T_w2 = Eigen::Isometry3d::Identity();
  {
    double angle = -M_PI / 2; // 旋转角度（弧度），例如90度（PI / 2）
    Eigen::Vector3d axis(0, 0, 1); //定义旋转轴: 绕Z轴旋转
    Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
    T_w2 = GetIsometry(axis, angle, translation);
  }


  std::cout << "方案1(两次叠加):  Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (T_w2 * (T_w1 * Eigen::Vector3d(0.2, 0.5, 1))).transpose() << std::endl;
  std::cout << "方案1(两次叠加, 矩阵满足结合律):  Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (T_w2 * T_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
  std::cout << std::endl;

  return 0;
}