1 Sophus的基本使用
cpp文件内容如下,
#include <iostream>
#include <cmath>
#include <ctime>
using namespace std;
#include <Eigen/Core> //Eigen核心模块
#include <Eigen/Geometry> //Eigen几何模块
using namespace Eigen;
#include "sophus/se3.hpp"
using namespace Sophus;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
clock_t start, last;
//本程序演示sophus的基本用法
int main()
{
start = clock();
cout << "对SO3和so3的操作如下:" << endl;
//沿Z轴旋转90度的旋转矩阵
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
//用旋转矩阵初始化的四元数
Quaterniond q(R);
//从旋转矩阵构造正交群SO3
SO3d SO3_R(R);
//从四元数构造正交群SO3
SO3d SO3_q(q);
cout << "SO3_R和SO3_q是相等的!" << endl;
cout << "SO3_R = \n" << SO3_R.matrix() << endl;
cout << "SO3_q = \n" << SO3_q.matrix() << endl;
//使用对数映射获得正交群SO3的李代数so3
Vector3d so3 = SO3_R.log();
cout << "so3 = \n" << so3 << endl;
//hat表示从三维向量到反对称矩阵
cout << "so3的反对称矩阵为:\n" << SO3d::hat(so3) << endl;
//vee表示从反对称矩阵到三维向量
cout << "so3 = \n" << SO3d::vee(SO3d::hat(so3)) << endl;
//增量扰动模型的更新
Vector3d update_so3(1e-4, 0, 0); //更新量
SO3d SO3_updated = SO3d::exp(update_so3) * SO3_R;
cout << "经扰动增量更新之后的正交群SO3为:\n" << SO3_updated.matrix() << endl;
cout << "****************************************************************************************************************" << endl;
cout << "对SE3和se3的操作如下:" << endl;
Vector3d t(1, 0, 0); //沿X轴平移1个单位
SE3d SE3_Rt(R, t); //用R和t初始化SE3
SE3d SE3_qt(q, t); //用q和t初始化SE3
cout << "SE3_Rt和SE3_qt是相等的!" << endl;
cout << "SE3_Rt = \n" << SE3_Rt.matrix() << endl;
cout << "SE3_qt = \n" << SE3_qt.matrix() << endl;
Vector6d se3 = SE3_Rt.log();
cout << "se3 = \n" << se3 << endl;
cout << "观察se3输出可知,在Sophus库中se(3)的平移在前,旋转在后!" << endl;
//hat表示从四维向量到反对称矩阵
cout << "se3的反对称矩阵为:\n" << SE3d::hat(se3) << endl;
//vee表示从反对称矩阵到四维向量
cout << "se3 = \n" << SE3d::vee(SE3d::hat(se3)) << endl;
//增量扰动模型的更新
Vector6d update_se3;
update_se3.setZero();
update_se3(0, 0) = 1e-4d;
SE3d SE3_updated = SE3d::exp(update_se3) * SE3_Rt;
cout << "经扰动增量更新之后的欧式群SE3为:\n" << SE3_updated.matrix() << endl;
last = clock();
cout << "总共花费的时间为:" << 1000 * double(last - start) / CLOCKS_PER_SEC << "毫秒!" << endl;
return 0;
}
CMakeLists.txt文件内容如下,
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRECTORIES})
include_directories("/usr/include/eigen3")
add_executable(main main.cpp)
结果如下,
对SO3和so3的操作如下:
SO3_R和SO3_q是相等的!
SO3_R =
2.22045e-16 -1 0
1 2.22045e-16 0
0 0 1
SO3_q =
2.22045e-16 -1 0
1 2.22045e-16 0
0 0 1
so3 =
0
0
1.5708
so3的反对称矩阵为:
0 -1.5708 0
1.5708 0 -0
-0 0 0
so3 =
0
0
1.5708
经扰动增量更新之后的正交群SO3为:
0 -1 0
1 0 -0.0001
0.0001 2.03288e-20 1
****************************************************************************************************************
对SE3和se3的操作如下:
SE3_Rt和SE3_qt是相等的!
SE3_Rt =
2.22045e-16 -1 0 1
1 2.22045e-16 0 0
0 0 1 0
0 0 0 1
SE3_qt =
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
观察se3输出可知,在Sophus库中se(3)的平移在前,旋转在后!
se3的反对称矩阵为:
0 -1.5708 0 0.785398
1.5708 0 -0 -0.785398
-0 0 0 0
0 0 0 0
se3 =
0.785398
-0.785398
0
0
0
1.5708
经扰动增量更新之后的欧式群SE3为:
2.22045e-16 -1 0 1.0001
1 2.22045e-16 0 0
0 0 1 0
0 0 0 1
总共花费的时间为:0.838毫秒!
2 评估轨迹的误差
cpp文件内容如下,
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <ctime>
using namespace std;
#include <sophus/se3.hpp>
using namespace Sophus;
#include <pangolin/pangolin.h>
string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";
typedef vector<SE3d, Eigen::aligned_allocator<SE3d>> TrajectoryType;
TrajectoryType ReadTrajectory(const string& path);
void DrawTrajectory(const TrajectoryType& gt, const TrajectoryType& esti);
clock_t start, last;
int main()
{
start = clock();
TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
TrajectoryType estimated = ReadTrajectory(estimated_file);
//assert()为断言函数。如果它的条件返回错误,则终止程序执行!
assert(!groundtruth.empty() && !estimated.empty());
assert(groundtruth.size() == estimated.size());
//计算绝对轨迹误差,即为李代数的均方根误差
double rmse = 0;
for(int i = 0; i < estimated.size(); i++)
{
SE3d p1 = estimated[i], p2 = groundtruth[i];
double error = (p2.inverse() * p1).log().norm();
rmse += error * error;
}
rmse = rmse / double(estimated.size());
rmse = sqrt(rmse);
cout << "绝对轨迹误差为:" << rmse << endl;
DrawTrajectory(groundtruth, estimated);
last = clock();
cout << "花费的时间为" << (double)(last - start) * 1000 / CLOCKS_PER_SEC << "毫秒!" << endl;
return 0;
}
TrajectoryType ReadTrajectory(const string& path)
{
ifstream fin(path);
TrajectoryType trajectory;
if(!fin) //如果文件打开失败,则执行
{
//cerr是标准错误流。该流中的信息只能输出到显示器上,而不能输出到文件中。该流中信息不经过缓冲区,直接输出至显示器。
cerr << "没有找到文件" << path << "!" << endl;
return trajectory;
}
while(!fin.eof()) //当fin并未指向EOF时,则执行
{
double time, tx, ty, tz, qx, qy, qz, qw;
fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
//wrong: SE3d p1(Eigen::Quaterniond(qx, qy, qz, qw), Eigen::Vector3d(tx, ty, tz));
SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
trajectory.push_back(p1);
}
return trajectory;
}
void DrawTrajectory(const TrajectoryType& gt, const TrajectoryType& esti)
{
//创建pangolin窗口
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST); //启用深度渲染,根据目标的远近自动隐藏被遮挡的模型
glEnable(GL_BLEND); //窗口使用颜色混合模式,让物体显示半透明效果
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//GL_SRC_ALPHA表示使用源颜色的alpha值作为权重因子,GL_ONE_MINUS_SRC_ALPHA表示用1.0-源颜色的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)
);
//ProjectionMatrix()中的参数依次为相机视图宽度、相机视图高度、fx、fy、cx、cy、深度最小值和深度最大值
//ModelViewLookAt()中的参数依次为相机的位置,被观察点位置,右上前为相机坐标系中XYZ轴的正方向
//创建交互视图,用于显示上一步相机所观测到的内容
pangolin::Handler3D handler(s_cam); //交互相机视图句柄
pangolin::View& d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
.SetHandler(&handler);
//SetBounds()参数依次为交互视图的bottom、top、left、right和其横纵比
while(pangolin::ShouldQuit() == false) //如果pangolin窗口没有关闭,则执行
{
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); //设置线条宽度为2
//绘制真实轨迹,即循环画线段
for(int i = 0; i < gt.size() - 1; i++)
{
glColor3f(0.0f, 0.0f, 1.0f); //颜色设置为蓝色
glBegin(GL_LINES);
SE3d 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(int i = 0; i < esti.size(); i++)
{
glColor3f(1.0f, 0.0f, 0.0f); //颜色设置为红色
glBegin(GL_LINES);
SE3d 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); //停止执行5毫秒
}
}
CMakeLists.txt文件内容如下,
find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_DIRECTORIES})
add_executable(main main.cpp)
target_link_libraries(main ${Pangolin_LIBRARIES})
结果如下,
绝对轨迹误差为:2.20728
花费的时间为2826.6毫秒!