# lego-loam-gps

**Repository Path**: luckdsfy_admin/lego-loam-gps

## Basic Information

- **Project Name**: lego-loam-gps
- **Description**: 融合GPS的LeGO-LOAM
- **Primary Language**: C++
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 18
- **Created**: 2020-12-10
- **Last Updated**: 2024-03-03

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

融合GPS的LeGO-LOAM

GPS话题：/chattergps
消息类型：sensor_msgs::JointState
内容：
//gps_msg->effort[0] 经度 longitude
//gps_msg->effort[1] 纬度 latitude
//gps_msg->effort[2] 高度 altitude
//gps_msg->effort[3] 水平速度 speed_hor
//gps_msg->effort[4] 天向速度 speed_ver
//gps_msg->effort[5] 航迹角 speed_sog
//gps_msg->effort[6] 标志位 status
//gps_msg->effort[7] 航向角 heading

改动：修改了mapOptmization.cpp文件，其他三个文件不变
具体思路：将GPS的位置约束，作为先验边添加到后端的因子图中
首先通过ROS的geodesy功能包将GPS经纬高信息转换为UTM坐标系并将起点坐标设为0(局部东北天系)，然后通过插值进行时间戳对准，最后将位置约束作为先验边添加至因子图中，对于第0帧，固定第0帧的位置(0, 0, 0)，只估计其三轴角度，这样就能直接将GPS量测约束给第0帧。

坐标系定义：
GPS初始系：GPS中心为原点，局部东北天
t时刻GPS系：将初始系与车体固连，随车体一起运动
LiDAR初始系：第0帧点云所在LiDAR系
t时刻LiDAR系：t时刻点云所在LiDAR系
世界系设定为GPS初始系，所需估计的状态为t时刻GPS系在世界系下的位姿，记为(R_t, t_t)，

设空间内任意一点p，在不同坐标系下的坐标分别为：
     GPS初始系(0_GPS)：p_0g
    t时刻GPS系(t_GPS)：p_tg
 LiDAR初始系(0_LiDAR)：p_0l
t时刻LiDAR系(t_LiDAR)：p_tl

坐标系转换关系：
设LiDAR系到GPS系的变换(可以理解为LiDAR与GPS之间的外参)为(R_0, t_0)，则有
0_LiDAR-->0_GPS : R_0 * p_0l + t_0 = p_0g;
t_LiDAR-->t_GPS : R_0 * p_tl + t_0 = p_tg;
t时刻LiDAR的量测(R_tl, t_tl)，表示t_LiDAR系到0_LiDAR系的变换，即
t_LiDAR-->0_LiDAR : R_tl * p_tl + t_tl = p_0l;

残差：需要构造关于(R_t, t_t)的残差
GPS量测残差：
t时刻GPS的量测t_tg，t_GPS-->0_GPS，因此残差表示为delta_gps = t_tg - t_t;
LiDAR量测残差：
t时刻LiDAR的量测(R_tl, t_tl)，t_LiDAR-->0_LiDAR，
真值：p_0g = R_t * p_tg + t_t
量测：p_0g = R_0 * p_0l + t_0
          = R_0 * (R_tl * p_tl + t_tl) + t_0
          = R_0 * (R_tl * R_0^(-1) * (p_tg - t_0) + t_tl) + t_0
          = R_0 * R_tl * R_0^(-1) * p_tg - R_0 * R_tl * R_0^(-1) * t_0 + R_0 * t_tl + t_0;
(上面把LiDAR量测处理成了绝对位姿，实际应该处理成前后两帧的相对位姿，即构造相对量测(R_t,t-1,l, t_t,t-1,l)和状态量(R_t, t_t),(R_t-1, t_t-1)之间的联系)
因此delta_LiDAR = delta(R_t, R_0 * R_tl * R_0^(-1)) + delta(t_t, - R_0 * R_tl * R_0^(-1) * t_0 + R_0 * t_tl + t_0);
如果令t_0 = 0，即不估计平移量，则
delta_LiDAR = delta(R_t, R_0 * R_tl * R_0^(-1)) + delta(t_t, R_0 * t_tl);
此时，(R_0, t_0 = 0)可以看作是第0帧的位姿，(R_tl, t_tl)是相对于第0帧的位姿,(R_0 * R_tl * R_0^(-1)), R_0 * t_tl)是在世界系下的位姿，
因此，可以放松对第0帧位姿的先验约束，而其他关键帧位姿用t_tg约束，这样由于delta_gps和delta(t_t, R_0 * t_tl)的存在，可以对R_0进行估计。

此外还加入了保存地图的功能，具体改动是在visualizeGlobalMapThread()方法中
使用：
在计算机对应路径下建立对应文件夹，路径设置在utility.h文件的fileDirectory变量，修改完成后需要编译。
程序结束后会自动将地图保存至对应文件夹下：
finalCloud.pcd 全地图
cornerMap.pcd 角点地图
surfaceMap.pcd 平面点地图
trajectory.pcd 关键帧轨迹
cornerCloudKeyFramesMapCloud_i.pcd 各关键帧角点点云(已变换至世界系)
surfCloudKeyFramesMapCloud_i.pcd 各关键帧平面点点云(已变换至世界系)