1 //writed by zhang ning 2018/3/5 20:11
 2 //本程序用来求解位姿变换问题
 3 #include <iostream>
 4 #include <cmath>
 5 using namespace std;
 6
 7 #include <Eigen/Core>
 8 #include <Eigen/Dense>
 9 // Eigen 几何模块
10 #include <Eigen/Geometry>
11
12 int main( int argc, char** argv)
13 {
14   Eigen::Vector3d p1,t1,t2;
15   p1 << 0.5,-0.1,0.2;
16   t1 << 0.7,1.1,0.2;
17   t2 << -0.1,0.4,0.8;
18 // 四元数Eigen::Quaterniond 的正确初始化顺序为Eigen::Quaterniond(w,x,y,z)
19 // 而 coeffs的顺序是（x,y,z,w）,w 为实部，前三者为虚部
20 // 因为要表示相反的旋转，故输入为q1的共轭，即实部不变，虚部变为相反数
21   Eigen::Quaterniond q1 = Eigen::Quaterniond(0.55,-0.3,-0.2,-0.2).normalized();
22   cout << q1.coeffs().transpose() << endl;
23
24
25   Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.1,0.3,-0.7,0.2).normalized();
26   cout << q2.coeffs().transpose() << endl;
27
28   Eigen::Vector3d pw = q1*(p1-t1);  //数学上是qpq-1
29   Eigen::Vector3d p2 = q2*pw + t2;
30
31   cout << p2 << endl;
32
33   return 0;
34 }

 1 //writed by zhang ning 2018/3/5 20:11
 2 //本程序用来求解位姿变换问题
 3 #include <iostream>
 4 #include <cmath>
 5 using namespace std;
 6
 7 #include <Eigen/Core>
 8 #include <Eigen/Dense>
 9 // Eigen 几何模块
10 #include <Eigen/Geometry>
11 //下面这段程序是用变换矩阵来计算的，可以得到相同的计算结果
12
13 int main( int argc, char** argv)
14 {
15   Eigen::Vector3d p1,t1,t2;
16   p1 << 0.5,-0.1,0.2;
17   t1 << 0.7,1.1,0.2;
18   t2 << -0.1,0.4,0.8;
19   Eigen::Quaterniond q1 = Eigen::Quaterniond(0.55,0.3,0.2,0.2).normalized();
20
21   Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
22   T1.rotate ( q1 );
23   T1.pretranslate ( t1 );
24   cout << "Transform matrix = \n" << T1.matrix() << endl;
25
26   Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.1,0.3,-0.7,0.2).normalized();
27
28   Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
29   T2.rotate ( q2 );
30   T2.pretranslate ( t2 );
31   cout << "Transform matrix = \n" << T2.matrix() << endl;
32
33   Eigen::Vector3d p2 = T2*T1.inverse()*p1;
34   cout << p2 << endl;
35
36   return 0;
37 }

 1 cmake_minimum_required( VERSION 2.8 )
 2
 3 project( geometry )
 4
 5 # 设置编译模式
 6 set( CMAKE_BUILD_TYPE "Debug" )
 7
 8 # 添加头文件
 9 include_directories( "/usr/include/eigen3")
10
11 add_executable( useGeometry useGeometry.cpp )