《视觉SLAM十四讲》第三讲习题7 编程练习

第三讲练习7

解题思路

已知一号位姿对世界坐标系的旋转四元数矩阵为：
q1=[0.35,0.2,0.3,0.1]
平移矩阵为：t2=[0.3,0.1,0.1]^T
二号位姿对世界坐标系的旋转四元数矩阵为：
q2=[-0.5,0.4,-0.1,0.2]
平移矩阵为：t=[-0.1,0.5,0.3]^T

某点在一号自身坐标系下的坐标为：p1=[0.5,0,0.2]^T求该点在二号自身坐标系下的坐标。编程实现。

设T1为世界坐标系到一号自身坐标系的欧氏变换矩阵，T2为世界坐标系到二号自身坐标系的欧氏变换矩阵；p为该点在世界坐标系中的坐标，p2为该点在二号自身坐标系中的坐标。则：
p=T1^-1 p1 （为了书写方便，实为齐次坐标变换）
p2=T2 p

故欲求p2只需求得T1 T2
而已知旋转四元数和平移矩阵求欧氏变换矩阵只需将四元数转换成旋转向量，然后由旋转向量和平移矩阵求得欧氏变换矩阵即可。

//将四元数转换成旋转矩阵
Eigen::AngleAxisd rotation_vector1 = Eigen::AngleAxisd ( q1 );
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
T1.rotate ( rotation_vector1 );		//按照rotation_vector1进行旋转
T1.pretranslate ( Eigen::Vector3d (0.3, 0.1, 0.1));	//设平移向量为t

代码实现

具体代码如下，用到了书中教的Eigen库：
task_7.cpp:

#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>

int main( int argc, char** argv )
{       
    //定义两个四元数
    Eigen::Quaterniond q1 (0.35, 0.2, 0.3, 0.1);
    Eigen::Quaterniond q2 (-0.5, 0.4, -0.1, 0.2);
    
    //将四元数转换成旋转矩阵
    Eigen::AngleAxisd rotation_vector1 = Eigen::AngleAxisd ( q1 );
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    T1.rotate ( rotation_vector1 );		//按照rotation_vector1进行旋转
    T1.pretranslate ( Eigen::Vector3d (0.3, 0.1, 0.1));	//设平移向量为t2
    cout << "Transform matrix1 = 
" << T1.matrix() << endl;
    
    //同上求出欧氏变换矩阵T2
    Eigen::AngleAxisd rotation_vector2 = Eigen::AngleAxisd ( q2 );
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T2.rotate ( rotation_vector2 );
    T2.pretranslate ( Eigen::Vector3d (-0.1, 0.5, 0.3));
    cout << "Transform matrix1 = 
" << T2.matrix() << endl;

    Eigen::Vector4d p1 (0.5, 0, 0.2, 1);	//定义该点在一号自身坐标系中的坐标
    Eigen::Vector4d p =T1.inverse() * p1;	//计算p
    cout << "p = 
" << p.transpose() << endl;

    Eigen::Vector4d p2 = T2 * p;		//计算p2
    cout << "p2 = 
" << p2.transpose() << endl;

    return 0;
}

CMakeLists.cpp:

cmake_minimum_required( VERSION 2.8 )

project( Task_7 )

//添加头文件，因为Eigen库只有头文件 故不用将程序链接到库上
include_directories( "/usr/include/eigen3")	

add_executable( task_7 task_7.cpp)

编译运行结果如下：

以后每周我会把自己跟着书做的例程和部分习题的代码提交到GitHub上。