#include
#include
#include
//Geometry模块提供了各种旋转和平移的表示
#include
using namespace std;
int main()
{
//一号的姿态
Eigen::Quaterniond q1(0.35, 0.2, 0.3,...0.1);
Eigen::Vector3d t1(0.3, 0.1, 0.1);
Eigen::Isometry3d Tcw_1 = Eigen::Isometry3d::Identity()...::Vector3d Pc1(0.5, 0, 0.2);
Eigen::Vector3d Pw = Tcw_1.inverse()*Pc1;
//二号的姿态
Eigen::Quaterniond...q2(-0.5, 0.4, -0.1, 0.2);
Eigen::Vector3d t2(-0.1, 0.5, 0.3);
Eigen::Isometry3d Tcw_2 = Eigen::Isometry3d