位姿表示與旋轉矩陣
作業
#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第三期 主講人:曾書格