参考网址:

peitianyu/lite_slam (github.com)

环境

1
2
3
- Ubuntu18
- Eigen
- opencv(仅用于显示)

框架

建图框架

1
2
3
4
5
6
7
8
9
10
11
- grid_map作为概率地图
	- map_manage用于显示,保存,读取grid_map
	- down_sample_map用于降采样概率图
- scantomap作为匹配
	- 为加快速度,采用多层地图,即down_sample_map
	- 为加快速度,采用scan_points体素滤波
	- 为加快速度,当迭代到一定程度时会提前退出迭代
- scan_context作为回环检测模块
	- 通过开源nanoflann库进行kdtree搜索得到先验pose
	- 然后通过scantomap进行精确定位得到回环pose
- graph_optimize图优化模块,图优化完后,更新关键帧与地图

代码实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void Run(const Pose2d& prior_pose = Pose2d()) override
{
    static uint is_first = 0;
    if(is_first++ < 20){
        Init(prior_pose); // 建图初始化
        return;
    }
	
    Location(); // 定位
    
    UpdateKeyFrame(); // 更新keyframe与更新地图

    // 显示pose与显示grid_map
    static uint id = 0;
    std::cout<<id++<<" m_estimate_pose: "<<m_estimate_pose.transpose()<<std::endl;
    m_map_manager->DisplayGridMap(m_scan_matcher->GetGridMap(), 1);
}

纯定位框架

1
2
3
4
5
- 保存scan_context相关的KeyFrame到txt文本中
- 保存原始grid_map用于纯定位
- scan_context用于重定位,由于场景时agv,所以路线一般固定,可以直接进行kdtree搜索得到先验pose,后scantomap得到精确pose
- scantomap用于纯定位
- 根据不同需求更新地图

代码实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
void Run(const Pose2d& prior_pose = Pose2d()) override
{
	static uint is_first = 0;
	if(is_first++ == 20) {Init(prior_pose);} // 纯定位初始化
		
	static bool is_loop_closure = false;
	if(!is_loop_closure){
		if(LoopClosure() != -1) {is_loop_closure = true;} // 重定位,若重定位成功
	}else{   
		Location(); // 定位   
		UpdateKeyFrame(); // 更新keyframe与更新地图

        // 显示
		static uint id = 0;
		std::cout<<id++<<" m_estimate_pose: "<<m_estimate_pose.transpose()<<std::endl;
		m_map_manager->DisplayGridMap(m_scan_matcher->GetGridMap(), 1);
	}
}