视觉SLAM理论与实践3
视觉SLAM理论与实践3-李群李代数-附代码
-
- 一、群的性质
- 二、验证向量叉乘的李代数性质
- 三、推导SE(3)的指数映射
- 四、伴随
- 五、轨迹的描绘
- 六、轨迹误差
一、群的性质
1）{Z，+}是整数集上的加法运算，根据以上群的四条性质，随便带入两个整数显然满足封闭性、结合律、幺元、逆的条件。所以{Z，+}是群,且是阿贝尔群。
2）{N，+}是非负整数(或自然数）组成集合上的加法运算，这里如果a=0,那么第四个“逆”的条件a的逆为 “1/0 ”，显然没有意义，不满足“逆”的条件，所以{N，+}不是群。
二、验证向量叉乘的李代数性质
证明过程如下：
三、推导SE(3)的指数映射
证明过程如下：
四、伴随
表1：
表2：
证明过程如下：
五、轨迹的描绘
我们通常会记录机器人的运动轨迹,来观察它的运动是否符合预期。大部分数据集都会提供标准轨迹以供参考,如 kitti、TUM-RGBD 等。这些文件会有各自的格式,但首先你要理解它的内容。记世界坐标系为 W ,机器人坐标系为 C,那么机器人的运动可以用 T W C 或 T CW 来描述。现在,我们希望画出机器人在世界当中的运动轨迹,请回答以下问题:
1.事实上,T W C 的平移部分即构成了机器人的轨迹。它的物理意义是什么?为何画出 T W C 的平移部分就得到了机器人的轨迹?
答：坐标的变换就是Twc的平移部分，即机器人移动的距离，因为解算位姿的时候，是解算两帧之间的位姿，包括平移和旋转，平移即移动距离，所以画轨迹就是将Twc描绘出来。
2.根据轨迹文件(code/trajectory.txt)绘制出运动轨迹，代码详见code/draw_trajectory.cpp和CMakeLists.txt ,运行环境为ubuntu16.04。
[code/draw_trajectory.cpp]:
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string trajectory_file = "../trajectory.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>>);
int main(int argc, char **argv) {
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;//读出的位姿存入该容器类vector中
/// implement pose reading code
// start your code here (5~10 lines)
//*c++中getline()函数/
ifstream infile(trajectory_file);
double t1,tx,ty,tz,qx,qy,qz,qw;
string line;
if(infile)
{
while(getline(infile,line))
{
stringstream record(line); //从string读取数据
record>>t1>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Eigen::Vector3d t(tx,ty,tz);
Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized(); //四元数的顺序要注意
Sophus::SE3 SE3_qt(q,t);
poses.push_back(SE3_qt);
}
}
else
{
cout<<"没找到这个文件"<<endl;
}
infile.close();
DrawTrajectory(poses);
return 0;
}
/*******************************************************************************************/
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());
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[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); // sleep 5 ms
}
}
CMakeLists.txt :
cmake_minimum_required(VERSION 2.8)
project(draw_trajectory)
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
set( CMAKE_BUILD_TYPE "Debug" )
find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)
include_directories("/usr/include/eigen3")
include_directories(
${
Pangolin_INCLUDE_DIRS}
${
Sophus_INCLUDE_DIR}
)
add_executable(trajectory draw_trajectory.cpp)
target_link_libraries(trajectory
${
Pangolin_LIBRARIES}
${
Sophus_LIBRARIES}
)
运行结果如下：
六、轨迹误差
除了画出真实轨迹以外,我们经常需要把 SLAM 估计的轨迹与真实轨迹相比较。误差定义为：
即两个位姿之差的李代数二范数。于是，可以定义两条轨迹的均方根（Root-Mean-Square-Error, RMSE）误差为：
代码详见code1/compare_tra.cpp和CMakeLists.txt ，环境ubun16.04,详细描述如下：
compare_tra.cpp:
//
// Created by chengjun on 2019/11/20.
//
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string gt_file = "../groundtruth.txt";
string est_file = "../estimated.txt";
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> gt_poses,est_poses;
// 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>> gt_poses,
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> est_poses);
void readData(string filepath);
void ErrorTrajectory();
int main(int argc, char **argv) {
readData(gt_file);
readData(est_file);
ErrorTrajectory();
// draw trajectory in pangolin
DrawTrajectory(gt_poses,est_poses);//打印两条轨迹
return 0;
/// implement pose reading code
// start your code here (5~10 lines)
}
/*******************************************************************************************/
void readData(string filepath){
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
ifstream infile(filepath);
double t1,tx,ty,tz,qx,qy,qz,qw;
string line;
if(infile)
{
while(getline(infile,line))
{
stringstream record(line); //从string读取数据
record>>t1>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Eigen::Vector3d t(tx,ty,tz);
Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized(); //四元数的顺序要注意
Sophus::SE3 SE3_qt(q,t);
poses.push_back(SE3_qt);
}
}
else
{
cout<<"没找到这个文件"<<endl;
}
if(filepath==gt_file){
gt_poses = poses;
}else if( filepath==est_file ){
est_poses = poses;
}else{
cout<<"读文件出错"<<endl;}
infile.close();
}
/*******************************************************************************************/
void ErrorTrajectory()
{
double RMSE = 0;
Eigen::Matrix<double ,6,1> se3;
vector<double> error;
for(int i=0;i<gt_poses.size();i++){
se3=(gt_poses[i].inverse()*est_poses[i]).log(); //这里的se3为向量形式，求log之后是向量形式
//cout<
error.push_back( se3.squaredNorm() ); //二范数
// cout<
}
for(int i=0; i<gt_poses.size();i++){
RMSE += error[i];
}
RMSE /= double(error.size());
RMSE = sqrt(RMSE);
cout<<RMSE<<endl;
}
/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> gt_poses,
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> est_poses) {
if (gt_poses.empty()) {
cerr << "groundtruth is empty!" << endl;
return;
}
if (est_poses.empty()) {
cerr << "estimated 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);//窗口,rgba
glLineWidth(2);
for (size_t i = 0; i < est_poses.size() - 1; i++) {
glColor3f(1 - (float) i / est_poses.size(), 0.0f, (float) i / est_poses.size());
glBegin(GL_LINES);
auto p1 = est_poses[i], p2 = est_poses[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 < gt_poses.size() - 2; i++) {
glColor3f(0.f, 0.8f, 0.f);//绿色
glBegin(GL_LINES);
auto p3 = gt_poses[i], p4 = gt_poses[i + 1];//只显示tx,ty,tz
glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
}
CmakeLists.txt:
cmake_minimum_required(VERSION 2.8)
project(draw_trajectory)
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
set( CMAKE_BUILD_TYPE "Debug" )
find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)
include_directories("/usr/include/eigen3")
include_directories(
${
Pangolin_INCLUDE_DIRS}
${
Sophus_INCLUDE_DIR}
)
add_executable(tra campare_tra.cpp)
target_link_libraries(tra
${
Pangolin_LIBRARIES}
${
Sophus_LIBRARIES}
)
运行结果如下：
最后计算出来的RMSE值为2.20728.
友情提示：代码下载需要C币，请事先判断是否对您有帮助，谨慎下载哦！！！