If you wish for peace, prepare for war

标定雷达和相机的外参 2022-12-12|激光SLAM其他

粗标定

realsense和雷达外参的粗标定，也就是laser坐标系和camera_color_optical_frame之间的相对位姿，但如果realsense是低着头，竖直和横向的相对位姿的测量误差就大了。

使用realsense检测贴在墙上的二维码，获得距离，同时使用雷达检测到墙的距离，让雷达尽量正对二维码位置。

雷达到前方距离: 1.288，对应realsense的z值: 1.119，二者差 0.169
用直尺测量高度的差: 0.125
横向: realsense在雷达左侧 0.035

如果laser为父坐标系，二者的变换矩阵为

$\begin{gathered} \begin{bmatrix} 0 & 0 & 1 & 0.169 \\ -1 & 0& 1& 0.035 \\ 0& -1& 0& -0.125 \\ 0& 0& 0& 1 \end{bmatrix} \end{gathered}$

但realsense的坐标系体系是 camera_link —-> camera_color_frame —-> camera_color_optical_frame，其中

camera_link camera_color_optical_frame
- Translation: [0.000, 0.015, 0.000]
- Rotation: in Quaternion [-0.498, 0.502, -0.498, 0.502]
            in RPY (radian) [-1.571, 0.008, -1.570]
            in RPY (degree) [-89.992, 0.476, -89.975]

所以不能直接建立laser坐标系和camera_color_optical_frame的父子关系，要建立laser和camera_link的。

使用变换矩阵的计算，已知 $T_{laser-color} = T_A$ ， $T_{camera-color} = T_B$ 。计算 $T_{laser-camera} = T_A * T_{B}^{-1}$

结果转为tf格式，应当为 <node pkg="tf" type="static_transform_publisher" name="laser_camera_broadcaster" args="0.169 0.02 -0.125 0 0 0 laser camera_link 100"/>

向量，平面和坐标系变换 2022-12-03|数学基础

向量

标准化后的向量点乘得到的值为夹角的余弦。如果得到 -1, 0, -1，便可知道两个向量方向的关系是相反, 垂直, 相同
向量叉乘后得到的向量和原先两个向量垂直，也就是法向量
向量叉乘后得到的向量的模，其值为两个向量构成的三角形的面积的二倍。

叉乘的一些性质

用行列式的表示方法观察，不用计算就能看出前三条。

平面的一般式

高中学过的表示方法为 $ax+by+cz=d$ ，法向量为 $(a,b,c)^T$ 。这里涉及到了归一化问题，两边可以乘以系数，还是同一个平面，也可以说法向量是不唯一的。但如果给出了一般式 $ax+by+cz=D$ ，就说法向量为 $(a,b,c)^T$

空间内一点 $(x_0, y_0, z_0)$ 到平面的距离是 $\frac{| ax_0 + b y_0 + cz_0|}{ \sqrt{a^2 + b^2 + c^2}}$ ，如果已经归一化，那么分母就是 1

法向量可以看做平面上两个向量的叉乘。

法向量 * 距离形式

N为法向量(1x3的向量)，那么[N, d]或者(N | d)为 1x4 的向量，其中d为平面到原点的距离。

旋转问题

现在平面的表示形式(无论哪种)都是基于坐标系A，另有一个坐标系B，从B到A的变换矩阵是M(4x4)，那么平面在坐标系B下的表示形式是什么？

向量 $P=(a,b,c,d)$ ，注意不是法向量，平面上一点为 $v=(x,y,z,1)$ ，那么 $P^Tv =0$ ，经过变换后，点v变成了 $v'$ ，那么有 $P'Mv=0$ ，也就有 $P'Mv= P^Tv$ ，因此可得 $P'= (M^{-1})^{T}p$ ，M矩阵的逆根据《SLAM十四讲》的公式3.13可以直接写出，最后根据 $P'$ 写成一般式

感觉这样推导并不严谨，但可以这样理解。

参考：OpenGL Normal Vector Transformation

Ceres(三) 解析求导 2022-12-03|SLAM工具

最常见的一个例子是 $f(x) = e^{mx+c}$ 链接，不再复制粘贴

还是之前的曲线拟合问题： $e^{(ax^2 + bx + c)}$ ，如果改用解析求导，需要构建一个继承CostFunction的类。核心函数是bool CostFunction: :Evaluate(double const *const *parameters, double *residuals, double **jacobians)，计算残差向量和雅可比矩阵

parameters: parameters是一个大小为CostFunction::parameter_block_sizes_.size()(输入参数块的数量和大小)的数组，parameters[i]是一个大小为parameter_block_sizes_[i]的数组，其中包含CostFunction所依赖的第i个参数块。parameters不会为nullptr
residuals: 是一个大小为num_residuals_(输出残差的个数)的数组。residuals也不会为nullptr
jacobians: 是一个大小为CostFunction::parameter_block_sizes_.size()的数组。如果jacobians是nullptr，用户只需要计算残差
jacobians[i]: 是一个大小为 num_residuals x parameter_block_sizes_[i] 的行数组，如果jacobians[i]不为nullptr，用户需要计算关于parameters[i]的残差向量的雅可比矩阵，并将其存储在这个数组中，即

如果jacobians为nullptr，通常我们只需要在Evaluate函数中实现residuals的计算，jacobians后面可以通过Ceres提供的自动求导（数值求导）替代，否则，还需要在Evaluate函数中实现jacobians的计算

如果parameters大小和residuals大小在编译时是已知的，就可以使用SizeCostFunction，该函数可以将paramters大小和residuals大小设置为模板参数，用户只需要在使用的时候给模板参数赋值就可以

// 模板参数依次为： number of residuals,  first parameter的维度
class MyCostFun : public SizedCostFunction<1, 1, 1, 1>
{
    public:
		MyCostFun(double x, double y):
			m_x(x), m_y(y){}
   virtual bool Evaluate(double const* const* parameters,
		      	 double* residuals,
			 double** jacobians) const
   {
       double a = parameters[0][0];
       double b = parameters[0][1];
       double c = parameters[0][2];
       residuals[0] = m_y - exp(a*m_x*m_x + b*m_x + c);

		if (jacobians != NULL && jacobians[0] != NULL)
		{
		    jacobians[0][0] = -exp(a*m_x*m_x + b*m_x + c)* m_x * m_x;
		    jacobians[0][1] = -exp(a*m_x*m_x + b*m_x + c)* m_x;
		    jacobians[0][2] = -exp(a*m_x*m_x + b*m_x + c);

		}
		return true;
   }
   protected:
	double m_x;
    double m_y;
};

拟合 $f(x) = e^{mx+c}$ 时，使用SizedCostFunction<1, 2>，只计算雅格比jacobians[0][0] 和 jacobians[0][1]

int main(int argv, char** argc)
{
    ros::init(argv, argc, "test_ceres");
    ros::NodeHandle nh;
    // 真实值
	double a = 1.0, b = 2.0, c = 1.0;

	// 生成带有噪声的模拟数据
	vector<double> x_data, y_data;
	int N = 240;  // 数据数量
	double w_sigma = 1.0;  // 高斯标准差
	cv::RNG  rng;
	for(unsigned int i =0; i<N; i++)
	{
		double x = i/100.0;
		x_data.push_back(x);
		y_data.push_back(exp(a*x*x + b*x + c) + rng.gaussian(w_sigma)  );
	}
	// 作为初值
	a = 0.0;
	b = 0.0;
	c = 0.0;
	Problem problem;
	for(unsigned int i=0; i<N; i++)
	{
		CostFunction* cost_func = new MyCostFun(x_data[i], y_data[i]);
		problem.AddResidualBlock(cost_func, nullptr, &a, &b, &c);
	}
	Solver::Options option;
	option.minimizer_progress_to_stdout = true;

	Solver::Summary summary;
	Solve(option, &problem, &summary);
	cout << summary.BriefReport() << endl <<endl;
	cout << "a: "<< a <<"  b: "<< b << "  c: "<< c <<endl;

    return 0;
}

一开始我设置代价函数的模板为SizedCostFunction<1, 3>，结果运行报错： [problem_impl.cc:286] Check failed: num_parameter_blocks == cost_function->parameter_block_sizes().size() (3 vs. 1)

问题在于parameter_block_sizes只有1，而我们需要3，也就是对应abc三个参数。读SizedCostFunction源码发现，模板定义为template <int kNumResiduals, int... Ns>，构造函数只有两行

1 2	set_num_residuals(kNumResiduals); *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};

因此从第2个模板参数开始，有几个参数就对应parameter_block_sizes，改为SizedCostFunction<1, 1, 1, 1>

cmake教程（七） add_definitions设置编译参数 2022-11-22|cmake/qmake|编译调试

添加编译参数，例如：

add_definitions(-DDEBUG)将在gcc命令行添加 DEBUG 宏定义；
add_definitions(“-Wall -ansi –pedantic –g”)

编译时有时出现报警warning: extra ‘;’ [-Wpedantic]，很影响看编译信息，可使用下面方法禁止pedantic

//save compiler switches
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"

//Bad headers with problem goes here
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

//restore compiler switches
#pragma GCC diagnostic pop

类似的报警还有warning: ISO C++ forbids variable length array ‘angle_compensate_nodes’ [-Wvla]，把上面的"-Wpedantic"换成"-Wvla"即可

Docker的使用 2022-11-13|常用工具

安装

# 添加docker的GPG公匙
curl -fsSL https://mirrors.ustc.edu.cn/docker-ce/linux/ubuntu/gpg | sudo apt-key add -    
# 添加中科大源
sudo add-apt-repository "deb [arch=amd64] https://mirrors.ustc.edu.cn/docker-ce/linux/ubuntu $(lsb_release -cs) stable"
sudo apt-get update 
apt-cache policy docker-ce
sudo apt-get install -y docker-ce
docker version   #查看版本

镜像

docker load -i 本地文件名 把本地文件做成image
执行docker images，出现下面结果

REPOSITORY   TAG          IMAGE ID       CREATED        SIZE
ros2         jazzy-full   15b1a367ae86   3 weeks ago    4.45GB
hello        gazebo       7ca40590c2f8   7 months ago   5.12GB
<none>       <none>       a4c6cd090ec3   2 years ago    4.34GB

对第一个镜像设置tag: docker tag 15b1a367ae86 ros2:jazzy-full

docker rmi IMAGE_ID 删除镜像

容器

docker ps -a 查看所有容器，包括已经退出的，对不需要的可以删除
删除容器: docker rm CONTAINER_ID
docker start ed86d97c09bd 启动已经停止的容器
docker stop ed86d97c09bd 停止容器
docker run --name ubuntu -p 3306:3306 -p 22000:22 -it django_lesson:1.0 -it 表示以交互模式运行容器，通常用于进入容器内部进行操作
进入容器: docker run -it a4c6cd090ec3 bash，但是如果在另一个终端再次运行这个命令，进入的不是同一个容器
如果要在另一个终端进同一个容器，使用 docker exec -it CONTAINER_ID /bin/bash

docker build

docker build 命令从 Dockerfile 和上下文构建镜像。Dockerfile往往是镜像需要安装的一些库和依赖项

docker build .，指定 PATH ，因此本地目录中的所有文件都会被延迟并发送到 Docker 守护程序

docker build -f /path/to/a/Dockerfile . 指定dockerfile，不指定的话，默认会读取上下文路径下的 dockerfile

问题

解决Docker运行命令时提示 `Got permission denied while trying to connect to the Docker daemon socket`

sudo groupadd docker     #添加docker用户组
sudo gpasswd -a $USER docker     #将登陆用户加入到docker用户组中
newgrp docker     #更新用户组
docker ps    #测试docker命令是否可以使用sudo正常使用

从Docker镜像中导出文件

从运行的容器中复制

先把镜像跑起来，然后从运行起来的容器中复制文件出来，复制命令如下：

1 2	# 从容器复制文件或目录到宿主机器 docker cp 6619ff360cce:/opt/h2-data/pkslow ./

解压镜像tar文件

首先，第一种方法并不是万能的，因为有些镜像过于简单，少了许多基础命令，以至于无法复制文件，也无法进入shell环境。其次，要运行起来再操作，也有点占用资源，比较麻烦。

以ghcr.io/kedacore/keda:2.2.0为例演示如下从该镜像中提取文件：

（1）将镜像保存为tar文件

1	docker save -o keda.tar ghcr.io/kedacore/keda:2.2.0

（2）解压tar文件

tar xvf keda.tar

x 42b88f0429143256463a478dda36b5e6d63f6dc43e033c3415414149c8c3257b.json
x 82a2e23fb9f1f5ac86b6c60196bff58e163601e5f37f1bc2bb1bd1781e8f6906/
x 82a2e23fb9f1f5ac86b6c60196bff58e163601e5f37f1bc2bb1bd1781e8f6906/VERSION
x 82a2e23fb9f1f5ac86b6c60196bff58e163601e5f37f1bc2bb1bd1781e8f6906/json
x 82a2e23fb9f1f5ac86b6c60196bff58e163601e5f37f1bc2bb1bd1781e8f6906/layer.tar
x ec12616cdd736751f41ba8d32cb9e9553ec33fc9d0bd1df92d4d8995b3dbc8ea/
x ec12616cdd736751f41ba8d32cb9e9553ec33fc9d0bd1df92d4d8995b3dbc8ea/VERSION
x ec12616cdd736751f41ba8d32cb9e9553ec33fc9d0bd1df92d4d8995b3dbc8ea/json
x ec12616cdd736751f41ba8d32cb9e9553ec33fc9d0bd1df92d4d8995b3dbc8ea/layer.tar
x manifest.json
x repositories

可以看到每个分层的信息，我们查看manifest.json可以看到具体哪个layer是最新的。

（3）找其中一个layer再解压

1 2	tar xvf ec12616cdd736751f41ba8d32cb9e9553ec33fc9d0bd1df92d4d8995b3dbc8ea/layer.tar x keda

这样，我们就获取到了keda这个可执行文件。每层的打包内容不一样，需要看所需的文件在哪个Layer。

No title 2022-09-20

—-title: SLAM中的图优化公式推导
date: 2022-09-20 22:17:51
categories:

- 算法推导

预测值

这里可以联系cartographer的前端理解。

g2o的基础使用 2022-09-19|SLAM工具

以下为ubuntu18版本的g2o对应的代码，和16版不兼容。TEB的常见安装问题就是16版到18版不兼容

// 求解器一般有两种, TEB用的是 Levenberg
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 3>> Block; // 每个误差项优化变量维度为3，误差值维度为1

Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver) );

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );

g2o::SparseOptimizer optimizer;
// setAlgorithm()需要传递一个优化算法
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
optimizer->initMultiThreading();

// ......
optimizer.optimize(maxIterations); 	//开始优化，设置迭代次数

static bool initMultiThreading();

Eigen3.1开始，要求we call an initialize函数，如果从不同线程调用Eigen
如果g2o基于OpenMP编译，而且Eigen版本高于3.1，则这个函数调用 Eigen::initParallel

LinearSolver

它用来求解线性方程 $H\Delta X = -b$ ，可以从PCG、CSparse、Choldmod、Dense、Eigen选择求解方法。

LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen：依赖项只有eigen，使用eigen中的sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver

选哪个求解器很重要，有一次使用LinearSolverDense，结果260秒才迭代一次，换成LinearSolverCSparse，0.4秒就迭代一次

TEB是涉及到CSparse和Choldmod，默认使用前者

解析器solver的另一种创建方式

// 加载几组 solver
G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)
G2O_USE_OPTIMIZATION_LIBRARY(dense)
G2O_USE_OPTIMIZATION_LIBRARY(eigen)


g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();
// 记录所有可用的 solver
std::ofstream fileout("/home/user/test_nodes/solvers.txt");
solver_factory->listSolvers(fileout);
// or lm_var_cparse
g2o::OptimizationAlgorithmProperty solver_property;
g2o::OptimizationAlgorithm *solver = solver_factory->construct("lm_var", solver_property);
std::unique_ptr<g2o::SparseOptimizer> optimizer(new g2o::SparseOptimizer());
optimizer->setAlgorithm(solver);

construct函数比上面的LinearSolverDense系列更灵活，设置求解器更精准。

使用listSolvers输出所有可用solver到文件，这个结果是和使用了多少G2O_USE_OPTIMIZATION_LIBRARY有关的，上面的5个应该涵盖了所有求解器，结果非常多。比如不加载G2O_USE_OPTIMIZATION_LIBRARY(eigen)，那么lm_var不可用，会报错 SOLVER FACTORY WARNING: Unable to create solver lm_var

listSolvers会输出所有求解器名称和解释，比如

1	lm_var Eigen Levenberg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)

优化结果

optimizer.setVerbose(true)后才能看到优化过程

或者g2o::SparseOptimizer::chi2()，优化前后各调用一次，可得到优化前后误差变化

iteration= 0	 chi2= 6973711.783675	 time= 0.228985	 cumTime= 0.228985	 edges= 3995	 schur= 0	 lambda= 42.450053	 levenbergIter= 1
iteration= 1	 chi2= 477444.019837	 time= 0.219176	 cumTime= 0.448161	 edges= 3995	 schur= 0	 lambda= 14.150018	 levenbergIter= 1
								......
iteration= 20	 chi2= 10344.665262	 time= 0.200148	 cumTime= 4.63974	 edges= 3995	 schur= 0	 lambda= 1.634041	 levenbergIter= 1
iteration= 21	 chi2= 10344.665262	 time= 0.461369	 cumTime= 5.10111	 edges= 3995	 schur= 0	 lambda= 112290463347.367386	 levenbergIter= 8

time是每次的优化时间，cumTime是累计用时。可以看到chi2最后几次迭代不怎么变化了，说明iteration设置大了

非线性最小二乘 2022-09-17|算法推导

照搬其他人的讲解。

方程个数 > 未知数个数，也就是”超定”线性方程组。方程个数比未知数个数多，可以想象不同方程之间会出现”冲突”的现象，也就是说导致整个方程组没有精确解。但我们只要设定一个误差标准，找到一个让这个误差标准值最小的近似解也是完全可以接收的。即针对超定不相容线性方程组，利用最小二乘误差标准求解近似值。

1.1 首先构建位姿之间的关系，即图的边

下面和开头的非线性最小二乘的推导一样了

问题：用最小二乘拟合直线，什么时候会失败？

答: 矩阵 $A^TA$ 不可逆的情况。我的理解是把最小二乘按正规方程解法，也就是先获得 $A^TAx = A^Tb$ ，这是一个 n×n
的线性方程组，称为正规方程组。显然解为 $x = (A^TA)^{-1}A^Tb$

图优化 2022-09-04|算法推导

图优化SLAM通过减去原始传感器的测量，构建了简化的估计问题。这些原始的测量由图中的边取代，边可以看做“虚拟测量”(virtual measurements)。图中的两个节点之间的边是用两个位姿之间的相对位置的概率分布表示的，这一概率分布的条件是它们的相互测量。观测模型 $p(z_t | \ x_t, m_t)$ 是多模态的，因此高斯分布的假设不成立。这意味着一次观测 $z_t$ 可能影响图中连接不同位姿的多条边。 graph connectivity 应当用概率分布来描述。如果直接处理这个多模态问题，将出现极大的复杂度。我们需要将地图变成node和edge的拓扑结构。因此，需要确定不同node之间的edge约束。

我们应当判断一次观测导致的最可能约束，这取决于机器人位姿的概率分布。这一问题称作data association，常由SLAM后端解决。前端需要对机器人轨迹 $p(X_{1:T} \ | \ Z_{1:T}, \ u_{1:T})$ 上的条件先验进行一致估计，这就要求机器人运动过程中对前端和后端穿插地执行。因此后端的准确性和效率对SLAM系统非常重要。

如果观测值受局部高斯噪声影响，而且data association已知，基于图的建图算法的目标就是机器人轨迹的高斯估计后验。包括计算这个高斯分布的均值，然后以均值作为节点的configuration，这样最大化了观测的似然。

图优化中，顶点是优化项，而边是约束项。优化过程就是通过全局调整优化项，使约束项的和最小。
Pose(x)与Landmark(m)构成了图的顶点集
边(Edge)存在两种。第一种是Pose到Pose，另一种是Pose到 Landmark
图优化中边表示约束，Pose到Pose的边与机器人的运动模型相关，机器人从 $x_0$ 运动到 $x_1$ ，形成了这条边。同时在运动模型的作用下，可以获得机器人在 $x_0$ ，控制量为 $u_1$ 时的状态 g。该值与“真实”的Pose 的差，构成了这条边的约束。由于边的约束是标量，因此取这个差的平方。R表示为运动模型的协方差。公式可以写作 $(x_1 - g)^T R^{-1}(x_1 - g)$
Pose到Landmark的边与机器人的观测模型相关，机器人从 $x_{1}$ 观测到 $m_{1}$ ，就会形成一条边。同时在观测模型 $h_{(measure, Pose)}$ 的作用下，可以获得机器人在 $x_{1}$ 时对 $m_1$ 的预测 $h_{(m_{1}, x_{1})}$ 。该值与此时真正观测到的值 $z_{1}$ 的差，构成了这条边的约束。同样的，取这个差的平方作为约束值。 $Q$ 表示为观测模型的协方差。公式 $[z_1 - h(m_1,x_1)]^TQ^{-1}[z_1 - h(m_1,x_1)]$
完整的约束为两种约束的和 $J_{GraghSLAM}$

SLAM的图优化问题就是寻找合适的 Pose(x), Landmark(m)，使 $J_{GraghSLAM}$ 最小。

看到预测值和观测值的差，很容易想到Cartographer，PoseExtrapolator提供预测值，Scan Matcher提供观测值。

或者可以这样理解：
Pose Graph

我们用 $e(x_i,x_j,z_{ij})$ 表示预测值和观测值之间的差，假设其满足高斯分布 $N(0, \Sigma)$ ，或者说观测值的分布是以真值为中心的多元高斯分布。我们的目标是让预测值和观测值尽可能趋近于0（均值），也就是让 $e$ 出现在概率最大的地方，这就是个最大似然的问题。我们让概率密度函数取最大值

把图中的x换成 $e(x_i,x_j,z_{ij})$ ，均值取0，得到下面的最小二乘形式

最小二乘问题的目标函数的变量维度很高，但每个误差项都是简单的，仅与少量变量有关。因此为了直观的展示变量与变量之间的关系，可以把优化问题用图的方式来展现。其中顶点表示优化变量，边表示误差项。

图优化的流程

选择你想要的图里的顶点与边的类型，确定它们的参数化形式
往图里加入实际的顶点和边
选择初值，开始迭代
每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵
求解稀疏线性方程 $H_k \Delta x = -b_k$ ，得到梯度方向
继续用高斯牛顿法(GN)或列文列文伯格-马夸尔特法(LM)进行迭代。如果迭代结束，返回优化值。

SLAM中的图优化

滤波类算法最大的问题，是无法处理大尺度场景的建图。由于滤波类算法是基于递归计算的，下一时刻的估计值依赖于上一时刻的估计，因而在大尺度场景下，由于系统参数和传感器观测数据的不准确性，会造成误差慢慢累积，一旦当前时刻的估计出现偏差，无法修正该误差，最终无法获得一致性的地图。

另外滤波方法对计算量和内存消耗都很大，使用图优化方法则效率较高，这种方法位姿不是实时纠正，而是将数据记录下来，最后进行计算优化。

机器人的位姿是一个顶点(vertex)，位姿之间的关系构成边(edge)，边表示两个位姿的空间约束(相对位姿和对应方差)。比如t+1时刻和t时刻之间的里程计关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。图构建完成后，就要调整机器人的位姿去尽量满足这些边构成的约束。图优化就是构建图并调整各顶点的位姿，使预测和观测的误差最小。每一条边的信息矩阵就是协方差矩阵的逆。协方差越小，说明这次测量越准，信息权重就越大。

前端：从里程计和激光雷达数据中求得位姿，构建图。机器人位姿当做顶点，位姿间关系当做边，常常是传感器信息的堆积
后端：在图构建好以后，采用回环检测，出现了回环边说明有误差。构造闭环约束，调整机器人位姿，通过最小化观测和估计残差求得优化后的位姿。

pose graph的概念
帧间边
回环边