习题练习（视觉slam14讲课后习题）

设有⼩萝⼘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;
 ;
}

然后它对应的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