Sophus：：SO3SE3_3dso36dse3_trajectoryErr
Sophus：：SO3SE3_3dso36dse3_trajectoryErr
Sophus::SO3SE3_3dso36dse3_trajectoryErr
引⾔
ch4.task3.
0.常⽤windows7摄像头在哪
李：
SO(3);R(3*3)
SE(3);T(4*4)
李代数：
so(3);(3*1)=>三个旋转
se(3);(6*1)=>前三维平移，后三维旋转
定义：
Sophus::SO3 SO3_R(R ); //旋转矩阵定义李SO(3)
Sophus::SO3 SO3_q(Q );//四元数定义李SO(3)
Sophus::SE3 SE3_Rt(R, t);//R,t构造SE(3)
Sophus::SE3 SE3_qt(q,t); //q,t构造SE(3)
Sophus::Vector3d so3 = SO3_R.log();//李代数so3为李SO(3)的对数映射
Sophus::Vector6d se3 = SE3_Rt.log();//李代数se(3) 是⼀个6维向量为李SE3 的对数映射
SE3_Rt = p();//李SE3是李代数se3的指数映射
SO3_R = p();//李SO3是李代数so3的指数映射
注意：
vector<Sophus::SE3, Eigen::aligned_allocator< Sophus::SE3>> poses;//这句实际上表达的是vector<
Sophus::SE3> poses的意思。但是在Eigen管理内存和C++11中的⽅法是不⼀样的，所以需要单独强调元素的内存分配和管理。(特别注意Eigen库数据结构内存对齐问题)
1.Sophus库
Eigen库是⼀个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解⽅程等功能。但是Eigen库提供了集合模块，但没有提供李代数的⽀持。⼀个较好的李和李代数的库是Sophus库，它很好的⽀持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使⽤Eigen库中的类时，既可以使⽤Eigen命名空间，也可以使⽤Sophus 命名空间。但最好使⽤Sophus命名空间，便于阅读。
Eigen::Matrix3d和Sophus::Matrix3d
Eigen::Vector3d和Sophus::Vector3d
此外，为了⽅便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：
Sophus::Vector4d
Sophus::Matrix4d
Sophus::Vector6d
小学二年级语文期末试卷
Sophus::Matrix6d
类型的转换：
Type conversion
// Eigen                          // Matlab
A.cast<double>();// double(A)
A.cast<float>();// single(A)
A.cast<int>();// int32(A)
A.imag();// imag(A)
// if the original type equals destination type, no work is done
2.Sophus安装⽅式：
git clone github/strasdat/Sophus.git
cd Sophus
日本旅游签证git checkout a621ff
mkdir build
cd build
cmake ..
make
sudo make install
3.Sophus的使⽤教程：
⼏个需要注意的地⽅：
1. Sophus库的各种形式的表⽰如下：
李代数so(3)：Sophus：：Vector3d //因为so(3)仅仅只是⼀个普通的3维向量
李代数se(3)：Sophus：：Vector6d //因为se(3)仅仅只是⼀个普通的6维向量
2. SO3的构造函数为：
SO3 ();表带脏了如何清洗
SO3 (const SO3 & other);
explicit SO3 (const Matrix3d & _R);
explicit SO3 (const Quaterniond & unit_quaternion);
SO3 (double rot_x, double rot_y, double rot_z);
3. SE3的构造函数为：
SE3 ();
SE3 (const SO3 & so3,const Vector3d & translation);
店名大全SE3 (const Matrix3d & rotation_matrix,const Vector3d & translation);
SE3 (const Quaterniond & unit_quaternion,const Vector3d & translation_);
SE3 (const SE3 & other);
4. SO3,SE3和se3的输出说明：
尽管SO3对应于矩阵，但是SO3在使⽤cout时是以so3形式输出的，输出的是⼀个3维向量.SE3在使⽤cout输出时输出的是⼀个6维向量，其中前3维为对应的so3的值，后3维为实际的平移向量t.se3在使⽤cout输出时输出的也是⼀个6维向量，但是其前3维为平移值ρρ（注意此时的ρρ与SE3输出的t是不同的，t=Jρρ，其中J是雅克⽐矩阵），后3维为其对应的so3.
4.SO3，so3，SE3和se3初始化以及相互转换关系
1.转换关系图：
2.代码⽰意：
SO3，so3，SE3和se3初始化以及相互转换useSophusc:
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
// 李李代数库
#include "sophus/so3.h"
#include "sophus/se3.h"
/*********************************
* sophus 库安装
* 本库为来版本⾮模板的版本
* git clone github//strasdat/Sophus.git
* git checkout a621ff  版本
* 在cmake编译
* *
欧拉⾓定义：
旋转向量转旋转矩阵            Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
旋转矩阵转四元数              Eigen::Quaterniond q(R);            // 或者四元数(从旋转矩阵构造)
平移Eigen::Vector3d t(1,0,0);    // 沿X轴平移1
SO(n) 特殊正交对应 n*n 的旋转矩阵(集合)
SE(n+1) 特殊欧式对应 n*n 的旋转矩阵和 n*1的平移向量组合成的变换矩阵(集合)
so(n)  对应的李代数为so(n)  n×1 列向量，使得向量和代数⼀⼀对应可以使⽤代数的更新⽅法来更新矩阵
SO(3)  表⽰三维空间的旋转矩阵集合  3×3
SE(3)  表⽰三维空间的变换矩阵集合  4×4
李代数 so3的本质就是个三维向量，直接Eigen::Vector3d定义.3个旋转
李代数 se3的本质就是个六维向量，直接Eigen::Matrix<double,6,1>定义,3个旋转 + 3个平移
旋转向量定义的李SO(3)      Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造这⾥注意，不是旋转向量的三个坐标值，有点像欧拉⾓构造。旋转矩阵定义的李SO(3)      Sophus::SO3 SO3_R(R);        // Sophus::SO(3)可以直接从旋转矩阵构造
旋转矩阵定义的李SO(3)      Sophus::SO3 SO3_R(R);        // Sophus::SO(3)可以直接从旋转矩阵构造
四元素定义的李SO(3)        Sophus::SO3 SO3_q(q);
从旋转矩阵和平移t构造SE3      Sophus::SE3 SE3_Rt(R,t);    // 从R,t构造SE(3)
从四元素和平移t构造SE3        Sophus::SE3 SE3_qt(q,t);    // 从q,t构造SE(3)
李代数so3为李SO(3)的对数映射 Eigen::Vector3d so3 = SO3_R.log();
李代数se(3)是⼀个6维向量，为李SE3 的对数映射
typedef Eigen::Matrix<double,6,1> Vector6d;// Vector6d指代Eigen::Matrix<double,6,1>
Vector6d se3 = SE3_Rt.log();
李代数指数映射成旋转矩阵对应的李
Eigen::Vector3d so33 (1, 1, 1);
Sophus::SO3 SO3 =Sophus::SO3::exp(so33);
Sophus::SO3::hat(so3) //hat为向量到反对称矩阵，相当于^运算.
Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose() //vee为反对称矩阵到向量，相当于下尖尖运算 .
**********************************/
int main( int argc, char** argv )
{
/*******************************************/
// 旋转矩阵/特殊正交仅有旋转没有平移
// 沿Z轴转90度的旋转矩阵
// 旋转向量　⾓度　轴  罗德⾥格公式进⾏转换为旋转矩阵  　
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
cout<<"RotationMatrix R: \n"<<R<<endl;
/***李*****/
Sophus::SO3 SO3_R(R);              // Sophus::SO(3)可以直接从旋转矩阵构造
Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造这⾥注意，不是旋转向量的三个坐标值，有点像欧拉⾓构造。
/*
SO3  ::SO3(double rot_x, double rot_y, double rot_z)
{
unit_quaternion_
= (SO3::exp(Vector3d(rot_x, 0.f, 0.f))
*SO3::exp(Vector3d(0.f, rot_y, 0.f))
*SO3::exp(Vector3d(0.f, 0.f, rot_z))).unit_quaternion_;
}
显⽰的貌似是三个过程，先转Ｘ轴，再转Ｙ轴，再转Ｚ轴，完全跟旋转向量不搭边。瞅着过程有点像欧拉⾓的过程，三个轴分了三步。我就有⼀个(1, 1, 1)旋转向量，如何构造成SO3呢？也就是让它输出(1, 1, 1)。
Eigen::Vector3d so33 (1, 1, 1);
Sophus::SO3 SO3 =Sophus::SO3::exp(so33);  //李代数指数映射成旋转矩阵对于的李
cout<<"SO3=\n"<<SO3<<endl;
*/
Eigen::Quaterniond q(R);            // 或者四元数(从旋转矩阵构造)
Sophus::SO3 SO3_q( q );
// 上述表达⽅式都是等价的
// 输出SO(3)时，以so(3)形式输出
//从输出的形式可以看出，虽然SO3是李，是旋转矩阵，但是输出形式还是向量（被转化成李代数输出）。
// 重载了 << 运算符  out_str << so3.log().transpose() << std::endl;
cout<<"SO(3) from matrix: "<<SO3_R<<endl;  //SO(3) from matrix:      0      0 1.5708
cout<<"SO(3) from vector: "<<SO3_v<<endl;
cout<<"SO(3) from quaternion :"<<SO3_q<<endl;
/****李代数*****/
// 使⽤对数映射获得它的李代数
// 所以，李代数 so3的本质就是个三维向量，直接Eigen::Vector3d定义。
Eigen::Vector3d so3 = SO3_R.log();
cout<<"so3 = "<&anspose()<<endl;
// hat 为向量到反对称矩阵相当于　^　运算。
cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
提速最快的车// 相对的，vee为反对称矩阵到向量相当于下尖尖运算
cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观⼀些

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。