计算机视觉life从零开始一起学习SLAM 学习笔记
推导李代数小se(3)的指数映射
(题目来自计算机视觉life)
题目:推导李代数小se(3)的指数映射。
我们知道对于大SE(3),其对应的李代数为小se(3)。其定义如下:
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{s e}(3)=\left\{\boldsymbol{\xi}=\left[\begin{array}{l} \rho \\ \phi \end{array}\right] \in \mathbb{R}^{6}, \boldsymbol{\rho} \in \mathbb{R}^{3}, \boldsymbol{\phi} \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc} \phi^{\wedge} & \rho \\ 0^{T} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\} se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
证明1:
exp ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] \exp \left(\boldsymbol{\xi}^{\wedge}\right)=\left[\begin{array}{cc} \sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} & \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] exp(ξ∧)=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]
证明2:令 ϕ \phi ϕ=θa,那么
∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ ≜ J \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\phi^{\wedge}\right)^{n}=\frac{\sin \theta}{\theta} I+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \triangleq \boldsymbol{J} n=0∑∞(n+1)!1(ϕ∧)n=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧≜J
知识点:李代数
(下面答案来自星球问答)
答案:
Sophus库练习
**编程练习:**SLAM问题的目标之一就是精确的估计相机运动的轨迹(姿态),如果我们将相机运动的轨迹绘制出来,就可以直观的观察它的运动是否符合预期。给定一个轨迹文件trajectory.txt,该文件的每一行由若干个数据组成,格式为 [time, tx, ty, tz, qx, qy, qz, qw],其中 time 为时间,tx,ty,tz 为平移部分,qx,qy,qz,qw 是四元数表示的旋转部分,请完成数据读取部分的代码,绘制部分代码已经给出。
知识点: 熟悉李代数库Sophus安装及基本操作
熟悉数据流读取基本操作需要安装pangolin,注意异常数据处理
参考答案:代码框架和轨迹数据见: 链接:https://pan.baidu.com/s/1Nm15-bQcFp1g-Pvck_NVkA 密码:n53r
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <pangolin/pangolin.h>
#include <iomanip>
#include <cfloat>
using namespace std;
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
string timestamp;
long double tx;
long double ty;
long double tz;
long double qx;
long double qy;
long double qz;
long double qw;
// path to trajectory file
string trajectory_file = "./trajectory.txt";
int main(int argc, char **argv) {
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
/// implement pose reading code
// 开始你的代码
string str;
ifstream fin(trajectory_file.c_str());
if (!fin)
{
cout<<"Fail to open"<<trajectory_file<<endl;
exit(-1);;
}
while (!fin.eof())
{
getline(fin,str);
cout<<"str="<<str<<endl;
if (str.at(0) == '#')
{
continue;
}
istringstream is(str); // 字符串流
is>>timestamp>>setprecision(9)>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
cout<<timestamp<<" "<<tx<<" "<<ty<<" "<<tz<<" "
<<qx<<" "<<qy<<" "<<qz<<" "<<qw<<" ";
if(timestamp == "-1.#INF")
{
cout<<"WARN: INF ERROR"<<endl;
continue;
}
// SE3:T R/Q
Eigen::Vector3d t(tx,ty,tz);
Eigen::Quaterniond q(qw,qx,qy,qz);
Sophus::SE3 se3(q,t);
poses.push_back(se3);
}
// 结束你的代码
// draw trajectory in pangolin
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);
}
}
CMakeLists.txt:
cmake_minimum_required(VERSION 2.8)
project(08_practice)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
# 添加eigen
include_directories("/usr/include/eigen3/")
# 添加sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
# 添加pangolin
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
# 添加可执行程序
add_executable(draw_trajectory draw_trajectory.cpp)
target_link_libraries(draw_trajectory
${Sophus_LIBRARIES}
${Pangolin_LIBRARIES})
问题:
- 在编译make的时候,出现了
draw_trajectory.cpp:(.text+0x289):对‘glClearColor’未定义的引用
draw_trajectory.cpp:(.text+0x296):对‘glLineWidth’未定义的引用……,
检查了所有的头文件也没找到错误的原因,后来发现是CMakeLists.txt写错了链接命令,将${Pangolin_LIBRARIES} 错误拼写成了${Pangolin_LIBSRARIES}。这类错误真的应该细心细心再细心。
- 对于C++中的数据类型:
浮点数采用:基准值+缩放因子来表示
float类型只能表示数字中的前6位或前7位,double类型只能表示数字中的前12或前13位。
- cout的一些用法
cout输出的时候,通常会删除结尾的零,比如将3.23000输出为3.23,cout.self()将覆盖这种行为。Debug的时候要注意这点。