C++ Eigen - 使用旋转矩阵，实现两个传感器之间的坐标变换（仅平移）

1. 需求描述

设有三个坐标系：1系，m系，2系； 其中在1系下有坐标P’(1, 1, 0). 求该坐标在2系下的坐标值 P"？ 手工分析肯定是： P"(3, 3, 0).

2. 需求分析

此过程由于仅涉及平移，不涉及旋转，在实现过程中，可以：

（1）先平移至m系，再平移至 2系。

（2）也可以直接由1系平移至2系。

由于本案例比较简单，按照直接平移至2系的方式进行说明。

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

由需求描述不难看出，X、Y的平移量均是3。 那么再构造旋转向量时，是取值-3 还是 3呢？

经实际测试验证发现如下规律：

（1）变换公式为：P_2 = T_2_1 * P_1, 可将1系下的P点，旋转至2系下。

（2）公式中，T_2_1 的旋转部分，是指：由坐标系1 旋转至2的过程中，相对于1产生的角度。该角度的取值依据顺、逆时针进行判定（详见上节内容中的示例2中有详细说明）；

（3）公式中，T_2_1 的平移部分，也应该是指由坐标系1 平移至2的过程中，相对于1系产生的平移量。正、负的取值也应以原始坐标系1为基础，分析对1系坐标产生的后果： 向负轴移动，取正数；向正轴移动，取负值。 所以本示例中，应该取值：2，2.

（4）若在实际构造旋转向量过程中，向负轴移动也取负数，则需要对旋转矩阵或变换矩阵 .invert()

4. 代码实现

#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;


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)
{

  Quaterniond q1(1, 0, 0, 0), q2(1, 0, 0, 0);
  q1.normalize();
  q2.normalize();

  Vector3d p1(1, 1, 0);


  double angle = 0;                 //旋转角度（弧度），例如90度（PI / 2）
  Eigen::Vector3d axis(0, 0, 1);  //定义旋转轴: 绕z轴旋转
  Eigen::AngleAxisd rotation_vector(angle, axis.normalized());

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

    std::cout << "Use Isometry:  P_w: " << (T_w1 * p1).transpose() << std::endl;

  }
  
  Eigen::Isometry3d T_w2 = Eigen::Isometry3d::Identity();
  {
    double angle = 0;                 //旋转角度（弧度），例如90度（PI / 2）
    Eigen::Vector3d axis(0, 0, 1);  //定义旋转轴: 绕z轴旋转
    Eigen::Vector3d translation = Eigen::Vector3d(-2, -2, 0); //定义平移量
    T_w2 = GetIsometry(axis, angle, translation);

    std::cout << "Use Isometry:  P_w: " << (T_w2 * p1).transpose() << std::endl;
    std::cout << "Use Isometry:  P_w: " << (T_w2.inverse() * p1).transpose() << std::endl;

  }

运行结果：

Use Isometry: P_w: 3 3 0
Use Isometry: P_w: 3 3 0

原文地址：https://blog.csdn.net/limeigui/article/details/140475856

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！