视觉SLAM理论与实践学习笔记（三）
二 群的性质
三 验证向量叉乘的李代数性质
四 推导 SE(3) 的指数映射
五 伴随
六 轨迹的描绘
七 轨迹的误差
二 群的性质
课上我们讲解了什么是群。请根据群定义，求解以下问题：
{Z; +}是否为群？若是，验证其满⾜群定义；若不是，说明理由。
{N; +}是否为群？若是，验证其满⾜群定义；若不是，说明理由。
其中 Z 为整数集， N 为⾃然数集。
三 验证向量叉乘的李代数性质
我们说向量和叉乘运算构成了李代数，现在请你验证它。书中对李代数的定义为：李代数由⼀个集合V，⼀个数域 F 和⼀个⼆元运算 [; ] 组成。如果它们满⾜以下⼏条性质，称 (V; F; [; ]) 为⼀个李代数，记作g。
其中⼆元运算被称为李括号。现取集合 V = R3，数域 F = R，李括号为：[a; b] = a × b，请验证 g = (R3; R; ×) 构成李代数。
四 推导 SE(3) 的指数映射
课上给出了 SO(3) 的指数映射推导，但对于 SE(3)，仅介绍了结论，没有给出详细推导。请你完成SE(3) 指数映射部分，有关左雅可⽐的详细推导。
这也正是课件⾥提到的左雅可⽐。提⽰：类⽐于 SO(3) 的泰勒展开，然后合并奇偶数项级数即得。
五 伴随
在 SO(3) 和 SE(3) 上，有⼀个东西称为伴随（Adjoint）。下⾯请你证明 SO(3) 伴随的性质。对于 SO(3)，有：
此时称 Ad® = R。
提⽰：⾸先你需要证明:
页面https://math.stackexchange.com/questions/2190603/derivation-of-adjoint-for-so3提示了⼀种简洁的途径。
对于 SE(3)，有：
其中 Ad(T ) 定义为：
这个性质将在后⽂的 Pose Graph 优化中⽤到。但是 SE(3) 的证明较为复杂，不作要求。
完整的 SO(3) 和 SE(3) 性质见表1和表2：
六 轨迹的描绘
我们通常会记录机器⼈的运动轨迹，来观察它的运动是否符合预期。⼤部分数据集都会提供标准轨迹以供参考，如 kitti、 TUM-RGBD 等。这些⽂件会有各⾃的格式，但⾸先你要理解它的内容。记世界坐标系为 W，机器⼈坐标系为 C，那么机器⼈的运动可以⽤ Twc或 Tcw来描述。现在，我们希望画出机器⼈在世界当中的运动轨迹，请回答以下问题：
1.事实上， Twc的平移部分即构成了机器⼈的轨迹。它的物理意义是什么？为何画出 Twc的平移部分就得到了机器⼈的轨迹？
答：Twc的平移部分指的是从世界坐标系原点到相机中心的平移向量，世界坐标系不随相机运动变化，因此可以认为Twc是机器人相对于原点坐标在移动, 移动可视化在观察者眼中就是机器人的运动轨迹。
2.我为你准备了⼀个轨迹⽂件（code/trajectory.txt）。该⽂件的每⼀⾏由若⼲个数据组成，格式为[t; tx; ty; tz; qx; qy; qz; qw];其中 t 为时间， tx; ty; tz 为 TW C 的平移部分， qx; qy; qz; qw 是四元数表⽰的 Twc的旋转部分， qw为四元数实部。同时，我为你提供了画图程序 draw_trajectory.cpp ⽂件。该⽂件提供了画图部分的代码，请你完成数据读取部分的代码，然后书写 CMakeLists.txt 以让此程序运⾏起来。注意我们需要⽤到 Pangolin 库来画图，所以你需要事先安装 Pangolin（如果你做了第⼀次作业，那么现在已经安装了）。 CMakeLists.txt 可以参照 ORB-SLAM2 部分。
答：把draw_trajectory.cpp程序中的#include <sophus/se3.h>改成#include <sophus/se3.hpp>，再把所有的Sophus::SE3改成Sophus::SE3d。增加两个头件#include <unistd.h>和#include <Eigen/Core>。
#include <sophus/se3.hpp>
#include <unistd.h>
#include <Eigen/Core>
// start your code here (5~10 lines)
ifstream fin(trajectory_file);
if(!fin){
cout << "cannot find trajectory file at " << trajectory_file << endl;
return 1;
}
while(!fin.eof()){
double time, tx, ty, tz, qx, qy, qz, qw;
fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
Sophus::SE3d Twr(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
poses.push_back(Twr);
}
cout << "read total " << poses.size() << " pose entries" << endl;
// end your code here
CmakeLists.txt：
cmake_minimum_required(VERSION 2.6)
project(test)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
include_directories("/usr/include/eigen3")
add_executable(draw_trajectory draw_trajectory.cpp)
target_link_libraries(draw_trajectory Sophus::Sophus)
target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES})
运行结果：
七 轨迹的误差
本题为附加题。
除了画出真实轨迹以外，我们经常需要把 SLAM 估计的轨迹与真实轨迹相⽐较。下⾯说明⽐较的原理，请你完成⽐较部分的代码实现。设真实轨迹（ground-truth）为 Tg，估计轨迹 Te。它们都以 Twc的形式存储，格式同上题。现在，你需要计算估计轨迹的误差。我们假设每⼀个 Tg 都与给定的 Te 对应。那么，对于任意第 i 个位姿，它的误差可定义为：
即两个位姿之差的李代数⼆范数。于是，可以定义两条轨迹的均⽅根（Root-Mean-Square-Error, RMSE）误差为：
我为你准备了 code/ground-truth.txt 和 code/estimate.txt 两条轨迹。请你根据上⾯公式，实现 RMSE的计算代码，给出最后的 RMSE 结果。作为验算，参考答案为： 2.207。
注：
实际当中的轨迹⽐较还要更复杂⼀些。通常 ground-truth 由其他传感器记录（如 vicon），它的采样频率通常⾼于相机的频率，所以在处理之前还需要按照时间戳对齐。另外，由于传感器坐标系不⼀致，还需要计算两个坐标系之间的差异。这件事也可以⽤ ICP 解得，我们将在后⾯的课程中讲到。
你可以⽤上题的画图程序将两条轨迹画在同⼀个图⾥，看看它们相差多少。
trajectoryError.cpp：
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>
using namespace Sophus;
using namespace std;
string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";
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(estimated_file);
assert(!groundtruth.empty() && !estimated.empty());
assert(groundtruth.size() == estimated.size());
// compute rmse
double rmse = 0;
for (size_t i = 0; i < estimated.size(); i++) {
Sophus::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 = " << rmse << endl;
DrawTrajectory(groundtruth, estimated);
return 0;
}
TrajectoryType ReadTrajectory(const string &path) {
ifstream fin(path);
TrajectoryType trajectory;
if (!fin) {
cerr << "trajectory " << path << " not found." << endl;
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(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
trajectory.push_back(p1);
}
return trajectory;
}
void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti) {
// 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 < gt.size() - 1; i++) {
glColor3f(0.0f, 0.0f, 1.0f); // blue for ground truth
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() - 1; i++) {
glColor3f(1.0f, 0.0f, 0.0f); // red for estimated
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); // sleep 5 ms
}
}
运行结果：
评论（0）
您还未登录，请登录后发表或查看评论