激光slam基础入门笔记2——位姿表示与变换矩阵

矩阵"/>

激光slam基础入门笔记2——位姿表示与变换矩阵

前言：

初步了解位置与位姿：

参考链接：机器人学——1.0-位置与姿态概述_「已注销」的博客-CSDN博客_位置和姿态

 二维空间位姿表示与变换矩阵推导：

参考链接：机器人学——1.1-二维空间位姿描述_「已注销」的博客-CSDN博客

深蓝学院参考：

C++简单实现相对位姿转换

#include <iostream>
#include<Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char** argv)
{// 机器人B在坐标系O中的坐标：Vector3d B(3, 4, M_PI);// 坐标系B到坐标O的转换矩阵：Matrix3d TOB;TOB << cos(B(2)), -sin(B(2)), B(0),sin(B(2)),  cos(B(2)), B(1),0,          0,        1;// 坐标系O到坐标B的转换矩阵:Matrix3d TBO = TOB.inverse();// 机器人A在坐标系O中的坐标：Vector3d A(1, 3, -M_PI / 2);// 求机器人A在机器人B中的坐标：Vector3d BA;Matrix3d TOA;TOA << cos(A(2)), -sin(A(2)), A(0),sin(A(2)),  cos(A(2)), A(1),0,          0,        1;Matrix3d TBA = TBO * TOA;cout << TBA << endl;BA << TBA(0, 2),TBA(1, 2),atan2(TBA(1, 0), TBA(0, 0));cout << "Your answer is BA: " << BA.transpose() << endl;return 0;
}

运行结果：

[Running] cd "/home/lxy/project/Eigen/" && g++ demo01.cpp -o demo01 && "/home/lxy/project/Eigen/"demo01
-1.83697e-16           -1            21 -1.83697e-16            10            0            1
Your answer is BA:      2      1 1.5708[Done] exited with code=0 in 7.399 seconds