高博课程编程作业之计算小萝卜的坐标

题目如下

下面我们来练习如何使用 Eigen/Geometry 计算一个具体的例子。
设有小萝卜 1 一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为: q 1 = [0.55, 0.3, 0.2, 0.2], t 1 =
[0.7, 1.1, 0.2] T (q 的第一项为实部)
。这里的 q 和 t 表达的是 T cw ,也就是世界到相机的变换关系。小萝卜
二号的位姿为 q 2 = [−0.1, 0.3, −0.7, 0.2], t 2 = [−0.1, 0.4, 0.8] T 。现在,小萝卜一号看到某个点在自身的坐
标系下,坐标为 p 1 = [0.5, −0.1, 0.2] T ,求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交
你的程序。
提示:
1. 四元数在使用前需要归一化。
2. 请注意 Eigen 在使用四元数时的虚部和实部顺序。
3. 参考答案为 p 2 = [1.08228, 0.663509, 0.686957] T 。你可以用它验证程序是否正确。

1）用四元数的方法计算

整体思路是由  p1，q1, t1 计算点在世界坐标下的坐标pw

先把四元数归一化，题目给出的表达是Tcw,是世界到相机的变换关系，所以要先把q1转化为相机到世界的变换，也就是q1的逆可以表达相反的变换

对于单位四元数，其逆和共轭就是同一个量，而四元数的共轭是把虚部取成相反数，所以在归一化的时候把q1的虚部取为相反数就行了。

pw = q1wc * (p1 - t1)

然后计算pw在萝卜二号坐标系的坐标

p2 = pw * q2  + t2

代码如下

 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 }

2）用变换矩阵的方法

同样的思路，先求出两个变换矩阵T1和T2  ，p1和T1 的逆矩阵相乘得到 pw, 然后再和T2相乘得到 p2

p2 = T2 * T1.inverse() * p1

代码如下

 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 }

CMakeLists.txt文件内容如下

 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 )

相关源码可以在我的github仓库中下载，

项目地址：https://github.com/feifanrensheng/useGeometry

转载于:https://www.cnblogs.com/feifanrensheng/p/8516302.html