视觉SLAM十四讲——第三讲习题

#include 
#include 
using namespace std;

#include 
// Eigen 几何模块
#include 

int main ( int argc, char** argv )
{    
Eigen::Quaterniond q1(0.2,0.3,0.1,0.35);
Eigen::Quaterniond q2(0.4,-0.1,0.2,-0.5);
Eigen::Vector3d p ( 0.5,0,0.2 );
Eigen::Vector3d t1 ( 0.3,0.1,0.1 );
Eigen::Vector3d t2 ( -0.1,0.5,0.3 );
Eigen::Matrix3d R1 = q1.toRotationMatrix();
Eigen::Matrix3d R2 = q2.toRotationMatrix();
Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();                                                
T1.pretranslate ( Eigen::Vector3d ( 0.3,0.1,0.1 ) );
Eigen::Isometry3d T2=Eigen::Isometry3d::Identity();                                                
T2.pretranslate ( Eigen::Vector3d ( -0.1,0.5,0.3 ) ); 
Eigen::Vector3d p1=T2*T1.inverse()*p;
cout<<"p1="<