Sophus::SO3SE3_3dso36dse3_trajectoryErr
引言
ch4.task3.
0.常用
李群：
- 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 = se3.exp();//李群SE3是李代数se3的指数映射
- SO3_R = so3.exp();//李群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.real(); // real(A)
A.imag(); // imag(A)
// if the original type equals destination type, no work is done
2.Sophus安装方式：
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build
cd build
cmake ..
make
sudo make install
3.Sophus的使用教程：
几个需要注意的地方：
-
Sophus库的各种形式的表示如下：
李代数so(3)：Sophus：：Vector3d //因为so(3)仅仅只是一个普通的3维向量
李代数se(3)：Sophus：：Vector6d //因为se(3)仅仅只是一个普通的6维向量 -
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); -
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); -
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 https://github.com//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_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 = "<<so3.transpose()<<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纯粹是为了输出美观一些
/****李代数求导 更新*****/
// 增量扰动模型的更新
Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;// 在李群中连乘更新
cout<<"SO3 updated = "<<SO3_updated<<endl;
/********************萌萌的分割线*********************************/
/************特殊欧式群 变换矩阵群 有旋转有平移*********************/
cout<<"************我是分割线*************"<<endl;
// 李群 对SE(3)操作大同小异
Eigen::Vector3d t(1,0,0); // 沿X轴平移1
Sophus::SE3 SE3_Rt(R, t); // 从R,t构造SE(3)
Sophus::SE3 SE3_qt(q,t); // 从q,t构造SE(3)
cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
// 李代数se(3) 是一个六维向量，方便起见先typedef一下
typedef Eigen::Matrix<double,6,1> Vector6d;// Vector6d指代 Eigen::Matrix<double,6,1>
Vector6d se3 = SE3_Rt.log();
cout<<"se3 = "<<se3.transpose()<<endl;
// 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
// 同样的，有hat和vee两个算符
cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;
// 最后，演示一下更新
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0,0) = 1e-4d;
Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;
return 0;
}
5.求轨迹误差
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
//xiao的方法更直接易懂
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string trajectory_file = "./../trajectory.txt"; //"./trajectory.txt";
string groundtruth_file = "./../groundtruth.txt"; //"./groundtruth.txt";
string estimated_file = "./../estimated.txt"; //"./estimated.txt";
// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
void Draw3Trajectory();
void computeErr();
int main(int argc, char **argv) {
//Draw3Trajectory();
computeErr();
return 0;
}
void computeErr()
{
ifstream g_file(groundtruth_file);
ifstream e_file(estimated_file);
if (!g_file||!e_file) {
cout<<"unable to read file!"<<endl;
exit(1);
}
int count = 0;
double rmse_square = 0;
double data1[8] = {0};
double data2[8] = {0};
while(!g_file.eof() && !e_file.eof()) {
for (auto &p:data1)
g_file >> p;
Eigen::Quaterniond q1(data1[7], data1[4], data1[5], data1[6]);
Eigen::Vector3d t1(data1[1], data1[2], data1[3]);
Sophus::SE3 SE3_g(q1,t1);
for (auto &p:data2)
e_file >> p;
Eigen::Quaterniond q2(data2[7], data2[4], data2[5], data2[6]);
Eigen::Vector3d t2(data2[1], data2[2], data2[3]);
Sophus::SE3 SE3_e(q2,t2);
Eigen::Matrix<double,4,4> m = (SE3_g.matrix().inverse())*SE3_e.matrix();//公式！
Eigen::Matrix<double,3,3> R = m.topLeftCorner<3,3>();//函数名很直观
Eigen::Matrix<double,3,1> t = m.topRightCorner<3,1>();
Eigen::Quaterniond q(R);
Eigen::Vector3d tt(t);
Sophus::SE3 SE3_dot(q,tt); //q,t构建李群
Sophus::Vector6d se3_dot = SE3_dot.log();//转为李代数
double err = se3_dot.norm();//二范数
rmse_square = (rmse_square + err*err); //公式
count ++ ;
}
g_file.close();
e_file.close();
double rmse = sqrt(rmse_square/count);
cout<< "the RMSE is:\n"<<rmse<<endl;
}
//画三段轨迹
void Draw3Trajectory()
{
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
/// implement pose reading code
// start your code here (5~10 lines)
///**
// Read1
ifstream t_file(trajectory_file);
ifstream g_file(groundtruth_file);
ifstream e_file(estimated_file);
if (!t_file) {
cout<<"unable to read file!"<<endl;
exit(1);
}
while(!t_file.eof()) {
double data[8] = {0};
for (auto &p:data)
t_file >> p;
Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
Eigen::Vector3d t(data[1], data[2], data[3]);
Sophus::SE3 pose(q,t);
poses.push_back(pose);
}
t_file.close();
// end your code here
if (!g_file) {
cout<<"unable to read file!"<<endl;
exit(1);
}
while(!g_file.eof()) {
double data[8] = {0};
for (auto &p:data)
g_file >> p;
Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
Eigen::Vector3d t(data[1], data[2], data[3]);
Sophus::SE3 pose(q,t);
poses.push_back(pose);
}
g_file.close();
if (!e_file) {
cout<<"unable to read file!"<<endl;
exit(1);
}
while(!e_file.eof()) {
double data[8] = {0};
for (auto &p:data)
e_file >> p;
Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
Eigen::Vector3d t(data[1], data[2], data[3]);
Sophus::SE3 pose(q,t);
poses.push_back(pose);
}
e_file.close();
//*/
//把三个轨迹都导入poses一起画出来就是，不用定义三个poses;
//这种思路比另一个大神的要强一点！！
// draw trajectory in pangolin
DrawTrajectory(poses);
}
/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
if (poses.empty()) {
cerr << "Trajectory is empty!" << endl;
return;
}
// create pangolin window and plot the trajectory
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
);
pangolin::View &d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
.SetHandler(new pangolin::Handler3D(s_cam));
while (pangolin::ShouldQuit() == false) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glLineWidth(2);
for (size_t i = 0; i < poses.size() - 1; i++) {
glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size()); // start from red and end with blue color
// start the drawing
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[i + 1];
// define two vertex, 1st is starting point, 2nd is ending point.
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
// finish the drawing
}
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
}
扫一扫
1094
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
