SLAM要点

偏导数

优化方程稀疏性的解释

SLAM优化函数中，H矩阵和B矩阵是稀疏的，因此可以针对性的进行一些算法设计来加速求解。

H和B为什么是稀疏的，直观解释如下所示：

基于SVD分解计算旋转和平移向量

点云配准任务详见pcl中的点云配准

如果可以得到两个点云之间的同名点对，那么就可以计算出旋转和平移向量的闭式解，以二维点集之间的相对变换为例进行解释：

1. 首先对原始的点云进行归一化操作，即减去对应质心点的坐标，得到\(\tilde{\mathbf{X}}, \tilde{\mathbf{Y}}\)；

\[ \tilde{x}=x_i-\bar{x},\tilde{y}=y_i-\bar{y},i=1,...,N \]

2. 计算加权协方差矩阵；

\[ \mathbf{H}=\tilde{\mathbf{X}}^{T} \mathbf{E} \tilde{\mathbf{Y}} \]

其中\(\mathbf{E}=\mbox{diag}(e_1,e_2,...,e_N)\).

3. 对\(\mathbf{H}\)做SVD分解即可计算从\(\mathbf{X}\)到\(\mathbf{Y}\)的旋转矩阵;

\[ \begin{align} [\mathbf{U},\mathbf{S},\mathbf{V}] &= \mbox{SVD}(\mathbf{H}) \\ \hat{\mathbf{R}} &= \mathbf{V} \left[\begin{array}{ccc} 1 &0 &0 \\ 0 &1 &0 \\ 0 &0 &\mbox{det}(\mathbf{V}\mathbf{U}^{T}) \end{array} \right] \mathbf{U}^{T} \end{align} \]

其中det()是行列式。

4. 最后再计算平移向量；

\[ \hat{\mathbf{t}}=\bar{y}-\hat{\mathbf{R}\bar{x}} \]

至于背后的数学推导，见这里。

PCL中的代码解释，详见另外一篇博客，或者文章《Least-Squares Rigid Motion Using SVD》。

坐标变换

常用地理坐标系

1. WGS84：World Geodetic System，世界大地测量系统，国外图商采用，例如：谷歌地图、Open Street Map（OSM）；
2. GCJ02：国测局坐标系，又称火星坐标系，对于WGS84坐标系加密而成，国内图商采用，例如：高德地图、腾讯地图；
3. BD09：百度地图坐标系，对于GCJ02坐标系加密而成；
4. CGCS2000：China Geodetic Coordinate System 2000，又称EPSG4490国家大地测量系统，国家2000坐标系；

WGS84和GCJ02和BD09坐标系转换，可以通过GeoConvert，coordtransform，gcoord实现。

一个疑问，既然是加密之后才有了GCJ02和BD09，那么为什么代码还可以直接使用开源代码实现转换呢？那不是相当于没加密？其实是近似的转换，转换多了之后就不准确了。对于没有测绘资质的人来说，拿到的坐标都是偏离过的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

class GeoConvert
{
public:
	GeoConvert(bool test);
	GeoConvert();
public:
	bool WGS84ToGCJ02(double wLon, double wLat, double& gLon, double& gLat);
	bool GCJ02ToWGS84(double gLon, double gLat, double& wLon, double& wLat);
	bool BD09ToGCJ02(double bLon, double bLat, double& gLon, double& gLat);
	bool GCJ02ToBD09(double gLon, double gLat, double& bLon, double& bLat);
	bool WGS84ToBD09(double wLon, double wLat, double& bLon, double& bLat);
	bool BD09ToWGS84(double& bLon, double& bLat, double& wLon, double& wLat);

private:
	bool outOfChina(double lon, double lat);
	double transformLat(double x, double y);
	double transformLon(double x, double y);

private:
	bool m_test;
};

地图投影

通用横轴墨卡托（Universal Transverse Mercator，UTM）投影是一种地图投影方法，将经纬度转换为二维笛卡尔坐标表示。UTM坐标是一种水平位置表示方式，忽略了海拔高度，将地球建模为完美椭球体。

• 开源代码

mgrs-Converting to and from MGRS and Decimal Degrees。

• 在线可视化工具

UTM-WGS84 坐标转换-支持在 UTM 坐标系统和 WGS84 经纬度坐标系统之间进行快速转换。

ESPG.io。