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

题目如下

下面我们来练习如何使用 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

代码如下

//writed by zhang ning 2018/3/5 20:11
//本程序用来求解位姿变换问题
#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Dense>
// Eigen 几何模块
#include <Eigen/Geometry>

int main( int argc, char** argv)
{
  Eigen::Vector3d p1,t1,t2;
  p1 << 0.5,-0.1,0.2;
  t1 << 0.7,1.1,0.2;
  t2 << -0.1,0.4,0.8;
// 四元数Eigen::Quaterniond 的正确初始化顺序为Eigen::Quaterniond(w,x,y,z)
// 而 coeffs的顺序是（x,y,z,w）,w 为实部，前三者为虚部 
// 因为要表示相反的旋转，故输入为q1的共轭，即实部不变，虚部变为相反数
  Eigen::Quaterniond q1 = Eigen::Quaterniond(0.55,-0.3,-0.2,-0.2).normalized();
  cout << q1.coeffs().transpose() << endl;


  Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.1,0.3,-0.7,0.2).normalized();
  cout << q2.coeffs().transpose() << endl;
  
  Eigen::Vector3d pw = q1*(p1-t1);  //数学上是qpq-1
  Eigen::Vector3d p2 = q2*pw + t2;

  cout << p2 << endl;
  
  return 0;
}

2）用变换矩阵的方法

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

p2 = T2 * T1.inverse() * p1

代码如下

//writed by zhang ning 2018/3/5 20:11
//本程序用来求解位姿变换问题
#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Dense>
// Eigen 几何模块
#include <Eigen/Geometry>
//下面这段程序是用变换矩阵来计算的，可以得到相同的计算结果

int main( int argc, char** argv)
{
  Eigen::Vector3d p1,t1,t2;
  p1 << 0.5,-0.1,0.2;
  t1 << 0.7,1.1,0.2;
  t2 << -0.1,0.4,0.8;
  Eigen::Quaterniond q1 = Eigen::Quaterniond(0.55,0.3,0.2,0.2).normalized();
  
  Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
  T1.rotate ( q1 );
  T1.pretranslate ( t1 );
  cout << "Transform matrix = \n" << T1.matrix() << endl;

  Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.1,0.3,-0.7,0.2).normalized();

  Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
  T2.rotate ( q2 );
  T2.pretranslate ( t2 );
  cout << "Transform matrix = \n" << T2.matrix() << endl;
  
  Eigen::Vector3d p2 = T2*T1.inverse()*p1;
  cout << p2 << endl;
  
  return 0;
}

CMakeLists.txt文件内容如下

cmake_minimum_required( VERSION 2.8 )

project( geometry )

# 设置编译模式
set( CMAKE_BUILD_TYPE "Debug" )

# 添加头文件
include_directories( "/usr/include/eigen3")
 
add_executable( useGeometry useGeometry.cpp )

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

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