高博課程編程作業之計算小蘿卜的坐標

題目如下

下面我們來練習如何使用 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