激光SLAM学习笔记

我的梦境 提交于 2019-12-10 12:28:23

位姿表示与旋转矩阵

位姿表示旋转矩阵推导旋转矩阵推导位姿表示

作业

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

using namespace std;

int main(int argc, char** argv)
{
    // 机器人B在坐标系O中的坐标:
    Eigen::Vector3d B(3, 4, M_PI);

    // 坐标系B到坐标O的转换矩阵:
    Eigen::Matrix3d TOB;
    TOB << cos(B(2)), -sin(B(2)), B(0),
           sin(B(2)),  cos(B(2)), B(1),
              0,          0,        1;//向量、矩阵赋值使用 “<<”

    // 坐标系O到坐标B的转换矩阵:
    Eigen::Matrix3d TBO = TOB.inverse();

    // 机器人A在坐标系O中的坐标:
    Eigen::Vector3d A(1, 3, -M_PI / 2);

    // 求机器人A在机器人B中的坐标:
    Eigen::Vector3d BA;
    // TODO 参照PPT
    // start your code here (5~10 lines)
    Eigen::Matrix3d TOA;
	TOA << cos(A(2)), -sin(A(2)), A(0),
			sin(A(2)), cos(A(2)), A(1),
			0,			0,			1;

	Eigen::Matrix3d TBA;
	TBA = TBO*TOA;
	//cout << TBA(2) << " " << TBA(5) << endl;//表示方式错误,应为TBA(0,2),TBA(1,2)
	BA << TBA(0,2),
		  TBA(1,2),
		  acos(TBA(0,0));

    // end your code here

    cout << "The right answer is BA: 2 1 1.5708" << endl;
    cout << "Your answer is BA: " << BA.transpose() << endl;

    return 0;
}

总结

1、记住转换矩阵公式,方向(顺时针、逆时针);
2、ROS坐标系(右手坐标系)
2、Eigen库的使用:
1)向量、矩阵赋值符号 “<<";
2)矩阵元素位置表示a(row, col);
3)反三角函数asin(), acos();

参考:深蓝学院激光SLAM第三期 主讲人:曾书格

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!