設有?蘿?1?號和?蘿??號位於世界坐標系中。

?蘿??號的位姿為：q1 = [0.55, 0.3, 0.2, 0.2], t1 = [0.7, 1.1, 0.2]T（q 的第?項為實部）。這?的 q 和 t 表達的是 Tcw，也就是世界到相機的變換關系。

?蘿? ?號的位姿為 q2 = [?0.1, 0.3, ?0.7, 0.2], t2 = [?0.1, 0.4, 0.8]T。

現在，?蘿??號看到某個點在??的坐 標系下，坐標為 p1 = [0.5, ?0.1, 0.2]T，求該向量在?蘿??號坐標系下的坐標。

請編程實現此事，並提交 你的程序。

下面是我的程序：hw3_3.cpp

#include <iostream>
#include 
 
<Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
using namespace std;
int main()
{
 /*1、先求出p點在世界坐標系下的坐標pw=Twc*pc1=Tcw`*pc1
  *先根據q1和t1，得到Twc(四元數使用之前要歸一化)
  *再求出Tcw，再代入公式得到Pw
  */
   Eigen::Quaterniond q1(0.55,0.3,0.2,0.2);
   Eigen::Vector3d t1;
   t1<<0.7,1.1,0.2;
   Eigen::Vector3d pc1(
 
0.5,-0.1,0.2);

    // 變換矩陣T用 Eigen::Isometry
    Eigen::Isometry3d Twc1=Eigen::Isometry3d::Identity();      // 雖然稱為3d，實質上是4＊4的矩陣
    Twc1.rotate ( q1.normalized() );                                     // 按照q1進行旋轉
    Twc1.pretranslate ( t1 );                                 // 按照t1進行平移
    // 用變換矩陣進行坐標變換
    Eigen::Vector3d pw = Twc1.inverse()*pc1;                              //
 
 相當於R*v+t
 //2、再求出pw在小蘿蔔二號坐標系下的坐標pc2=Tcw*pw
    Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2);
    Eigen::Vector3d t2;
    t2<<-0.1,0.4,0.8;
    // 變換矩陣T用 Eigen::Isometry
    Eigen::Isometry3d Tcw2= Eigen::Isometry3d::Identity();      // 雖然稱為3d，實質上是4＊4的矩陣
    Tcw2.rotate ( q2.normalized() );                                     // 按照q2進行旋轉
    Tcw2.pretranslate (t2 );                                 // 按照t2進行平移
    Eigen::Vector3d pc2=Tcw2*pw;
    cout << "pc2= " <<pc2.transpose()<<endl;
 return 0;
}

然後它對應的CMakeLists.txt文件對應為：

cmake_minimum_required( VERSION 2.8 )
project( hw3 )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )

# 添加Eigen頭文件
include_directories( "/usr/include/eigen3" )

add_executable( hw3_3 hw3_3.cpp )

然後cmake make得到答案是：

pc2=  1.08228 0.663509 0.686957

習題練習（視覺slam14講課後習題）