一、Sophus的基本使用
注:在编译前,应该装好Sophus库,该库要用到一些依赖,可自行去csdn查找
在github的slambook2(by_gaoxiang)上给出了相关的第三方库
该程序主要包含简单的李群李代数的的定义以及转换
源码如下:
//
//该程序包含Sophus库的基本使用
//包括 李群(SO3、SE3) 、 李代数(so3、 se3) 以及相应的转换
#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include"sophus/se3.hpp"
using namespace std;
using namespace Eigen;
int main(int argc, char** argv)
{
cout<<"**********************************下面是SO3、 so3(旋转) ***********************************"<<endl;
Matrix3d R;
AngleAxisd rotation_vector(M_PI/2,Vector3d(0,0,1));//旋转向量,绕z轴旋转90°
R=rotation_vector.toRotationMatrix();//得到旋转矩阵R
cout<< "旋转矩阵 R = " <<endl<<R<<endl<<endl;
Quaterniond q(R);//得到四元数
cout<< "四元数 q = " <<endl<<q.coeffs()<<endl<<endl;
cout<<" *********************************李群*************************** "<<endl;
Sophus::SO3d SO3_R(R);//李群是矩阵
Sophus::SO3d SO3_q(q);
cout<<"SO3 from rotation matrix : \n"<< SO3_R.matrix() <<endl;
cout<<"SO3 from Quaternion : \n"<< SO3_q.matrix() <<endl;
cout<<" *********************************利用对数映射得到李代数*************************** "<<endl;
Vector3d so3=SO3_R.log();//李代数是三维向量
cout<<"so3 = "<< so3.transpose()<<endl;
cout<<" *****利用hat()函数,可以将向量变到反对称矩阵,利用vee()函数,可以将反对称矩阵变回向量****** "<<endl;
cout<<"so3 hat= \n"<< Sophus::SO3d::hat(so3)<<endl;
cout<<"so3 hat vee = "<< Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;
cout<<" *********************************增量扰动模型的更新*************************** "<<endl;
Vector3d so3_update(1e-4,0,0);//用李代数表示更新,李代数可以实现加减
Sophus::SO3d SO3_update =Sophus::SO3d::exp(so3_update)*SO3_R;//先用李代数得到更新量,再把更新量变为李群,左乘原来的李群就得到更新了
cout<<"SO3 update = \n"<< SO3_update.matrix() <<endl;
cout<<"**********************************下面是SE3、 se3(将旋转和平移放在一起,也就是整个的变换) ***********************************"<<endl;
//和上面SO3 类似
Vector3d t(1,0,0);//translation
Sophus::SE3d SE3_Rt(R,t);//李群SE3_Rt由旋转矩阵R和平移t构成
Sophus::SE3d SE3_qt(q,t);//李群SE3_Rt由四元数q和平移t构成
cout<<"SE3 from rotation matrix and t : \n"<< SE3_Rt.matrix() <<endl;
cout<<"SO3 from Quaternion and t : \n"<< SE3_qt.matrix()<<endl;
cout<<" *********************************利用对数映射得到李代数*************************** "<<endl;
//李代数se3是六维向量,前三维表示平移,后三维表示旋转
//利用typedef定义一个Vector6d
typedef Eigen::Matrix<double,6,1> Vector6d;
Vector6d se3=SE3_Rt.log();
cout<<" se3 = "<<se3.transpose()<<endl;
cout<<" *****利用hat()函数,可以将向量变到反对称矩阵,利用vee()函数,可以将反对称矩阵变回向量****** "<<endl;
cout<<"se3 hat= \n"<< Sophus::SE3d::hat(se3)<<endl;
cout<<"so3 hat vee = "<< Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;
Vector6d se3_update;
se3_update.setZero();
se3_update(0,0)=1e-4;//得到更新量(1e-4d,0,0,0,0,0)
Sophus::SE3d SE3_update= Sophus::SE3d::exp(se3_update)*SE3_Rt;//先将更新量变为李群,再右乘原来的SE3,得到更新量
cout<<"SE3 update = \n"<< SE3_update.matrix() <<endl;//得到更新后的SE3
return 0;
}
运行结果:
**********************************下面是SO3、 so3(旋转) ***********************************
旋转矩阵 R =
6.12323e-17 -1 0
1 6.12323e-17 0
0 0 1
四元数 q =
0
0
0.707107
0.707107
*********************************李群***************************
SO3 from rotation matrix :
2.22045e-16 -1 0
1 2.22045e-16 0
0 0 1
SO3 from Quaternion :
2.22045e-16 -1 0
1 2.22045e-16 0
0 0 1
*********************************利用对数映射得到李代数***************************
so3 = 0 0 1.5708
*****利用hat()函数,可以将向量变到反对称矩阵,利用vee()函数,可以将反对称矩阵变回向量******
so3 hat=
0 -1.5708 0
1.5708 0 -0
-0 0 0
so3 hat vee = 0 0 1.5708
*********************************增量扰动模型的更新***************************
SO3 update =
0 -1 0
1 0 -0.0001
0.0001 2.03288e-20 1
**********************************下面是SE3、 se3(将旋转和平移放在一起,也就是整个的变换) ***********************************
SE3 from rotation matrix and t :
2.22045e-16 -1 0 1
1 2.22045e-16 0 0
0 0 1 0
0 0 0 1
SO3 from Quaternion and t :
2.22045e-16 -1 0 1
1 2.22045e-16 0 0
0 0 1 0
0 0 0 1
*********************************利用对数映射得到李代数***************************
se3 = 0.785398 -0.785398 0 0 0 1.5708
*****利用hat()函数,可以将向量变到反对称矩阵,利用vee()函数,可以将反对称矩阵变回向量******
se3 hat=
0 -1.5708 0 0.785398
1.5708 0 -0 -0.785398
-0 0 0 0
0 0 0 0
so3 hat vee = 0.785398 -0.785398 0 0 0 1.5708
SE3 update =
2.22045e-16 -1 0 1.0001
1 2.22045e-16 0 0
0 0 1 0
0 0 0 1
二、真实轨迹和估计轨迹的绝对误差(包含绘制估计轨迹和真实轨迹)
该程序实现的是计算真实轨迹和估计轨迹的绝对误差,同时利用pangolin/OpenGL画出真实和估计轨迹
注:要用到Eigen、pangolin、Sophus,请提前装好
源码如下:
//
//
//该程序演示了绝对轨迹误差
#include<iostream>
#include<unistd.h>
#include <fstream>
#include "pangolin/pangolin.h"
#include "sophus/se3.hpp"
using namespace Sophus;
using namespace std;
//这个地方我放的是绝对路径
string groundtruth_file="/home/nnz/data/slam_pratice/pratice_sophus/example/groundtruth.txt";//真是的位姿轨迹文件
string edtimated_file="/home/nnz/data/slam_pratice/pratice_sophus/example/estimated.txt";//估计的位姿轨迹文件
//利用typedef定义一个容器类型,里面装的是位姿(time 平移,四元数【旋转】)
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
void DrawTrajectory(const TrajectoryType >,const TrajectoryType &esti);
TrajectoryType ReadTrajectory(const string &path);
int main(int argc,char **argv)
{
TrajectoryType groundtruth=ReadTrajectory(groundtruth_file);
TrajectoryType estimated=ReadTrajectory(edtimated_file);
//assert宏的原型定义在<assert.h>中,其作用是如果它的条件返回错误,则终止程序执行,原型定义:
//#include <assert.h>
//void assert( int expression );
// assert的作用是现计算表达式 expression ,如果其值为假(即为0)
// 那么它先向stderr打印一条出错信息,然后通过调用 abort 来终止程序运行
assert(!groundtruth.empty()&&!estimated.empty());
assert(groundtruth.size()==estimated.size());
//计算 绝对轨迹误差 rmse
//size_t它是一种“整型”类型,里面保存的是一个整数,就像int, long那样。
// 这种整数用来记录一个大小(size)。size_t的全称应该是size type,就是说“一种用来记录大小的数据类型”
//通常我们用sizeof(XXX)操作,这个操作所得到的结果就是size_t类型。
// 因为size_t类型的数据其实是保存了一个整数,所以它也可以做加减乘除,也可以转化为int并赋值给int类型的变量
//示例代码:
//int i; // 定义一个int类型的变量i
//size_t size = sizeof(i); // 用sizeof操作得到变量i的大小,这是一个size_t类型的值
// // 可以用来对一个size_t类型的变量做初始化
//i = (int)size; // size_t类型的值可以转化为int类型的值
double rmse=0;
for(std::size_t i=0;i<estimated.size();i++)
{
Sophus::SE3d p1=estimated[i],p2=groundtruth[i];//estimated and groundtruth : vector
double error =(p2.inverse()*p1).log().norm();//模长,见slam十四讲89页 公式4.44
rmse+=error*error;//得到模长的平方
}
rmse=rmse/double(estimated.size());
rmse=sqrt(rmse);
cout<<"绝对误差 rmse= "<< rmse<<endl;
DrawTrajectory(groundtruth,estimated);
return 0;
}
TrajectoryType ReadTrajectory(const string &path)
{
ifstream fin(path);//流入
TrajectoryType trajectory;
if(!fin)
{
cerr<<"could not find a trajectory file"<<endl;//cerr
return trajectory;
}
while(!fin.eof())//读文件
{
double time,tx,ty,tz,qx,qy,qz,qw;
fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Sophus::SE3d p1(Eigen::Quaterniond(qx,qy,qz,qw),Eigen::Vector3d(tx,ty,tz));//得到位姿
trajectory.push_back(p1);
}
return trajectory;
}
void DrawTrajectory(const TrajectoryType >,const TrajectoryType &esti)
{
// 创建 pangolin 窗口,画轨迹
//creat and set titles 、 width 、height of windows
pangolin::CreateWindowAndBind(" Trajectory form truth and estimated ", 1024, 768);
//glEnable(GL_DEPTH_TEST)启用了之后,OpenGL在绘制的时候就会检查,当前像素前面是否有别的像素,如果别的像素挡道了它,那它就不会绘制,也就是说,OpenGL就只绘制最前面的一层。
//当我们需要绘制透明图片时,就需要关闭它glDisable(GL_DEPTH_TEST);
//并且打开混合 glEnable(GL_BLEND);
glEnable(GL_DEPTH_TEST);//??
glEnable(GL_BLEND);//??
//glBlendFunc(GLenum sfactor,GLenum dfactor);
//源因子和目标因子是可以通过glBlendFunc函数来进行设置的。
// glBlendFunc有两个参数,前者sfactor表示源因子,后者dfactor表示目标因子。
//前者sfactor表示源颜色,后者dfactor表示目标颜色
//GL_SRC_ALPHA:表示使用源颜色的alpha值来作为因子
//GL_ONE_MINUS_SRC_ALPHA:表示用1.0减去源颜色的alpha值来作为因子(1-alpha)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
//OpenGlRenderState 创建一个相机的观察视图,模拟相机
pangolin::OpenGlRenderState s_cam(//定义投影和初始模型视图矩阵
//ProjectionMatrix 前两个参数是相机的宽高,紧接着四个参数是相机的内参,最后两个是最近和最远视距
pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
//ModelViewLookAt 前三个参数是相机的位置,紧接着三个是相机所看的视点的位置,最后三个参数是一个向量,表示相机的的朝向
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(1.0),1.0,-1024.0f/768.0f).
//SetBounds() 前四个参数是视图在视窗中的范围(下 上 左 右),建议动手改改参数就明白了
SetHandler(new pangolin::Handler3D(s_cam));
// SetHandle设置相机的视图句柄,需要用它来显示前面设置的 “相机” 所 “拍摄” 的内容
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< gt.size();i++)
{
glColor3f(0.0f, 0.0f, 1.0f);//blue for 真实轨迹
glBegin(GL_LINES);
auto p1=gt[i],p2=gt[i+1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
for(size_t i=0;i<esti.size();i++)
{
glColor3f(1.0f, 0.0f, 0.0f); // red for 估计轨迹
glBegin(GL_LINES);
auto p1=esti[i],p2=esti[i+1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000);//休眠5ms
}
}
运行结果如下:
绝对误差 rmse= 2.20728