视觉SLAM十四讲习题——第三讲习题7

最新推荐文章于 2024-10-10 22:59:02 发布

咕噜个咕噜23

最新推荐文章于 2024-10-10 22:59:02 发布

阅读量269

点赞数 5

文章标签：算法

本文链接：https://blog.csdn.net/weixin_71218145/article/details/142499774

版权

这是我阅读这本书动手编程完成的第一个习题，答案应该正确，记录一下。

#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

//Eigen 几何模块
#include <Eigen/Geometry>
#define MATRIX_SIZE 50

/****************************
* 本程序是SLAM十四讲中第三讲的习题7
****************************/

int main( int argc, char** argv )
{
    // 写出Tcw1
    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    
    // 定义q1 四元数
    Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);
    
    // 归一化四元数
    q1.normalize();
    
    // 将四元数转换为旋转矩阵
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix = q1.toRotationMatrix();
    T1.rotate ( rotation_matrix );                                     // 按照rotation_vector进行旋转
    T1.pretranslate ( Eigen::Vector3d ( 0.3, 0.1, 0.1 ) );                     // 把平移向量设成(1,3,4)
    cout << "normalize_q = \n" << q1.coeffs().transpose() <<endl;
    cout << "Transform matrix = \n" << T1.matrix() <<endl;
    
    // 1号视野中的点位置
    Eigen::Vector4d Point1(0.5,0,0.2,1);     // 齐次坐标
    
    // 点的世界坐标
    Eigen::Vector4d Point;
    
    Point = T1.inverse()*Point1;
    
    Eigen::Isometry3d T2=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    
    // 定义q1 四元数
    Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2);
    
    // 归一化四元数
    q2.normalize();
    
    // 将四元数转换为旋转矩阵
    Eigen::Matrix3d rotation_matrix2;
    rotation_matrix2 = q2.toRotationMatrix();
    T2.rotate ( rotation_matrix2 );                                     // 按照rotation_vector进行旋转
    T2.pretranslate ( Eigen::Vector3d ( -0.1, 0.5, 0.3 ) );                     // 把平移向量设成(1,3,4)
    cout << "normalize_q = \n" << q2.coeffs().transpose() <<endl;
    cout << "Transform matrix = \n" << T2.matrix() <<endl;
    
    // 2号视野点
    Eigen::Vector4d Point2;
    Point2 = T2*Point;
    cout << "Point2 = \n" << Point2 <<endl;
    
    return 0;
}