深蓝学院-视觉SLAM理论与实践-第十二期-第3章作业

群的性质

问题1：$ {Z, +} $ 是否为群？若是，验证其满足群定义；若不是，说明理由

是群，满足如下性质

• 封闭性：任何两个整数相加结果依然是整数
• 结合律：$a+b+b=a+(b+c)$
• 幺元：0为幺元
• 逆：任何一个整数与其相反数相加结果都为0

问题2：$ {N, +} $ 是否为群？若是，验证其满足群定义；若不是，说明理由

是群，满足如下性质

• 封闭性：任何两个整数相加结果依然是整数
• 结合律：$a+b+b=a+(b+c)$
• 幺元：0为幺元
• 逆：任何一个自然数与其相反数相加结果都为0

问题3：解释什么是阿贝尔群。并说明矩阵及乘法构成的群是否为阿贝尔群

阿贝尔群：除满足群的公理外，还满足交换律公理。矩阵及乘法不能构成阿贝尔群，因为矩阵乘法不满足交换律。

验证向量叉乘的李代数性质

问题1：验证三维向量及其叉乘运算构成李代数

• 封闭性：任何两个向量叉乘其结果仍然为一个向量
• 双线性：

• 自反性：$a\times b=-b\times a$
• 雅可比等价：

推导SE(3)的指数映射

主要思想

• 展开后，通过$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }$,将展开后的无限项，通过同类项合并变成两项
• 对上述两项再通过简单的增补合并成$sin\theta$和$cos\theta$形式

推导过程

伴随

关于$(Rp{)}^{\wedge }=R{p}^{\wedge }{R}^T$的证明

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-gXQ9SSOQ-1645373988174)(https://pic.imgdb.cn/item/620896bd2ab3f51d915ee54d.jpg)]

关于$exp((Rp{)}^{\wedge })=Rexp(p^{\wedge })R^T$的证明

常见函数的求导应用

问题1：$Rp$对$R$求导

左扰动求导

主要思想

应用泰勒展开以及叉乘的交换律使得分子分母能够同时消去$\varphi$

求解过程

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-kmnHjmwd-1645373988176)(https://pic.imgdb.cn/item/62089e3c2ab3f51d9165d996.jpg)]

右扰动求导

主要思想

应用泰勒展开以及叉乘的交换律使得分子分母能够同时消去$\varphi$

求解过程

问题2：$R_1{R}_2$对$R_1$求导

左扰动求导

主要思想

通过BCH近似公式将扰动项和旋转项进行合并

求解过程

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-YL15fX2P-1645373988178)(https://pic.imgdb.cn/item/6208b5b52ab3f51d917be3af.jpg)]

右扰动求导

主要思想

通过添加$R_1{R}_1^T$再利用伴随的性质，凑成和做扰动相似的形式

求解过程

问题3：$R_1{R}_2$对$R_2$求导

左扰动求导

主要思想

和用右扰动对$R_1$求导类似

求解过程

右扰动求导

主要思想

和利用左扰动求解$R_1$的偏导数类似

求解过程

轨迹的描绘

问题1：事实上， $T_{WC}$ 的平移部分即构成了机器人的轨迹。它的物理意义是什么？为何画出 $T_{WC}$ 的平移部分就得到了机器人的轨迹？

$T_{WC}$ 的平移部分代表在世界坐标系下机器人的三维空间坐标，因此绘制出他的平移矩阵，就能绘制出机器人的轨迹。

问题2：绘制轨迹

源代码补充部分

	ifstream f("trajectory.txt");
	if(!f)
	{
		std::cout << "打开失败" << std::endl;
		return 0;		
	}

	else
		std::cout << "打开成功" << std::endl;
	
	while(!f.eof())
	{
		float time, px, py, pz, qx, qy, qz, qw;
		f >> time >> px >> py >> pz >> qx >> qy >> qz >> qw;
		Eigen::Quaterniond q(qw, qx, qy, qz);	//这种方式初始化，实部在前
		q.normalize();
		
		Eigen::Vector3d t(px, py, pz);
		Eigen::Matrix3d R = q.matrix();
		Sophus::SE3d se3(R, t);
		poses.push_back(se3);
		std::cout << px << ", " << py << ", " << pz << std::endl;
	}

cmake部分

cmake_minimum_required(VERSION 2.8)

project(draw_trajectory)

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

# sophus
include_directories("/usr/local/include/")

add_executable(main2 draw_trajectory.cpp)

target_link_libraries(main2 ${Pangolin_LIBRARIES})

运行结果

计算轨迹误差

源代码

#include <iostream>
#include <sophus/se3.hpp>
#include <fstream>
#include <Eigen/Core>

int main()
{
	std::ifstream fGT("groundtruth.txt"); 
	std::ifstream fEst("estimated.txt");
	
	if(!fGT || !fEst)
	{
		std::cout << "file open failed" << std::endl;
		return 1;
	}
	
	int posNum = 0;
	double rmse = 0;
	while(!fGT.eof() && !fEst.eof())
	{
		float time, px, py, pz, qx, qy, qz, qw;
		fGT >> time >> px >> py >> pz >> qx >> qy >> qz >> qw;
		Eigen::Quaterniond qGT(qw, qx, qy, qz);
		qGT.normalize();
		
		Eigen::Vector3d tGT(px, py, pz);
		Sophus::SE3d se3GT(qGT, tGT);
		
		fEst >> time >> px >> py >> pz >> qx >> qy >> qz >> qw;
		Eigen::Quaterniond qEst(qw, qx, qy, qz);
		qEst.normalize();
		
		Eigen::Vector3d tEst(px, py, pz);
		Sophus::SE3d se3Est(qEst, tEst);
		
		double err = (se3Est.inverse()*se3GT).log().norm();
		rmse += err;
		posNum++;
	}
	rmse /= posNum;
	std::cout << "rmse = " << rmse << std::endl;
	return 0;
}

CMake

cmake_minimum_required(VERSION 2.8)

project(draw_trajectory)

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})


find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

# sophus
include_directories("/usr/local/include/")

# 轨迹显示
add_executable(main2 draw_trajectory.cpp)

target_link_libraries(main2 ${Pangolin_LIBRARIES})

# 轨迹误差计算
add_executable(main test.cpp)

运行结果