SLAM十四讲第三讲实践：useGeometry------小白强行读代码

代码全文及双杠注释来自于《视觉SLAM十四讲–从理论到实践》
大部分带*注释是自己参考Eigen网站及其他博客的内容拼凑的。
由于本人编程基础薄弱，新手上路以下仅作为个人学习的一个总结。

#include<cmath>
using namespace std;#include <Eigen/Core>
//eigen几何模块
#include<Eigen/Geometry>/*****************************本程序演示了Eigen 几何模块的使用方法****************************/int main(int argc,char** argv)
{Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();// Eigen/Geometry模块提供了各种旋转和平移的表示// 3D旋转矩阵直接使用Matrix3d或Matrix3f/* 一个典型的3阶矩阵的表达式为：Eigen::Matrix3d* 常用矩阵如下：* 单位矩阵：Matrix3d::Identity();* 全1矩阵：Matrix3d::Ones();* 零矩阵：Matrix3d::Zero();* 随机矩阵：Matrix3d::Random();* 三维向量：Eigen::Vector3d,本质是Matrix<double,3,1>  */Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));//旋转向量使用AngleAxis,它底层不直接是Matrix,但运算可以当做矩阵（因为重载了运算符）/* 旋转向量使用AngleAxis (角度，轴)*/                                                                      //沿z轴旋转45度cout.precision(3);/* cout.precision()是输出流cout的一个格式控制函数，是iostream的一个成员函数* precision()返回当前的浮点数的精度值，而cout.precision(val)是在输出的时候设定输出值以新的浮点精度值显示，即小数点后保留val倍*/cout <<"rotation  =\n"<<rotation_vector.matrix()<<endl;//用Matrix()转换成矩阵/* 对旋转向量rotation_vector,使用方法.matrix()，将旋转向量转化为旋转矩阵  */rotation_matrix = rotation_vector.toRotationMatrix(); //也可以直接赋值/* 对旋转向量rotation_vector,使用方法.toRotationMatris()，将旋转向量转化为旋转矩阵（另一种方法）*/Eigen::Vector3d v(1,0,0);//用AngleAxis可以进行坐标变换/* 设置三维单位向量v  *//* cout<<v<<endl; v实质上是三维列向量 */Eigen::Vector3d v_rotated = rotation_vector*v;/* 对v进行旋转：需要旋转的向量左乘旋转向量 */cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;/* 使用类方法.transpose()将列向量转置成行向量 */v_rotated=rotation_matrix*v;//或者用旋转矩阵/* 对v进行旋转：需要旋转的向量左乘旋转矩阵 */cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;Eigen::Vector3d euler_angles=rotation_matrix.eulerAngles(2,1,0);//欧拉角：可以将旋转矩阵直接转换成欧拉角//ZYX顺序，即yaw,pitch,roll顺序/* eularAngles( Index a0,*              Index a1,*              Index a2)* 三个参数a0、a1、a2代表了旋转轴，由{0,1,2}中的整数取值，*  其中 “0”代表X ， ”1“代表Y ， ”2“代表Z*  返回角度为弧度，取值范围为[0:pi]×[-pi:pi]×[-pi:pi] */cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//欧氏变换矩阵使用：Eigen::Isometry//虽然称为3d,实质上是4*4的矩阵/* Isometry3d 是一种声明的定义，用于把三维矩阵转化为齐次坐标矩阵存储 */T.rotate(rotation_vector);//按照rotation_vector进行旋转/* 对Isometry3d类型的T的旋转矩阵部分通过rotate()类方法进行赋值* rotate()支持的旋转类型包括：*         1、任何标量（2D）*         2、Dim×Dim矩阵表达式*         3、一个四元数（3D）  Quaternion*         4、一个旋转向量（3D） AngleAxis   */T.pretranslate(Eigen::Vector3d(1,3,4));//把平移向量设置成(1,3,4）/* 使用类方法pretranslate()把一个平移向量传递给 Isometry3d 类型的T */cout<<"Transform matrix = \n"<<T.matrix()<<endl;/* Isometry3d类的T 不能直接打印（cout）出来，需要使用类方法matrix()才能以矩阵的形式将其打印出来  */Eigen::Vector3d v_transformed = T*v;//用变换矩阵进行坐标变换//相当于R*v+tcout<<"v tranformed = "<<v_transformed.transpose()<<endl;/* 因为v本身是个列向量，为了便于打印表示，一直把它作为行向量打印，所以最后结果需要转置*/Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);//对于四元数//可以直接把AngleAxis赋值给四元数，反之亦然/* 定义并初始化四元数q* 常用的几种四元数的构造：* 1、Eigen::Quaternion<-Scalar,_Options>::Quaternion(const Scalar& w,*                                                    const Scalar& x,*                                                    const Scalar& y,*                                                    const Scalar& z)*                                              注意参数的输入顺序，实部w是第一个，而四元数在内部的存储顺序则是(x,y,z,w)* 2、Eigen::Quaternion<_Scalar,_Options>::Quaternion(const AngleAixType&& aa)*     使用旋转向量（角度/轴）aa来构造并初始化四元数* 3、Eigen::Quaternion<_Scalar,_Options>::Quaternion(const MatrixBase<Derived>& 其他)*     可以用一下任一构造和初始化四元数：*     <1>旋转矩阵表达式*     <2>表示四元数系数的4D向量表达式 */cout<<"quaternion = \n"<<q.coeffs()<<endl;//注意coeffs的顺序是（x,y,z,w),w为实部,,前三者为虚部/* Quaternion类的q 不能直接打印，需要用类方法coeffs()把系数(x,y,z,w)用向量表达  */q=Eigen::Quaterniond(rotation_matrix);//也可以把旋转矩阵赋值给它cout<<"quaternion = \n"<<q.coeffs()<<endl;v_rotated = q*v;//使用四元数旋转一个向量,使用重载的乘法即可//注意数学上是qvq-{-1}cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;return 0;
}

SLAM十四讲第三讲实践：useGeometry------小白强行读代码相关推荐

视觉SLAM十四讲--第13讲实践：设计SLAM系统（最详细的代码调试运行步骤）
文章目录一. 代码目录介绍及运行步骤代码目录: 如何运行二. 代码调试问题 1. 准备工作 2. 遇到的问题及解决办法 3. 成功运行三. 代码理解核心算法结构数据结构: 算法代码理解及 ...
视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
【《视觉SLAM十四讲》前ch2-ch6实践全过程和遇到的问题及解决办法】
文章目录前言一.运行环境配置 1.在虚拟机上安装Ubuntu14.04 2.方便Ubuntu使用二.<十四讲>的实践部分过程与问题 1.Ubuntu下安装包的两种方法 2.编译高翔的 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-PnP和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记- ...

SLAM十四讲第三讲实践：useGeometry------小白强行读代码

SLAM十四讲第三讲实践：useGeometry------小白强行读代码

SLAM十四讲第三讲实践：useGeometry------小白强行读代码相关推荐

最新文章

热门文章