EGO_Plannerv2代码学习（一）：栅格建图之V1 V2的区别

最近在尝试将高飞老师的集群规划器 ego_plannerV2部署到真实无人机上跑一下，开这个新帖记录下学习过程中的问题和想法。

EGO-Plannerv2

首先，V2是在EGO-planner基础上的进阶版，主要的改进是将轨迹参数化方式由均匀b样条改成了汪博的MINCO，提升了规划器的时空规划能力，代码整体结构上跟V1类似。

前天已经试着把v2的规划器移植到原来v1的硬件平台上，飞行时遇到的问题是由于地图膨胀，地面被抬高了三十厘米，导致轨迹规划失败。下面就读一下v1 v2中建图部分的代码，看下差别在哪。

建图部分的代码存放在src/planner/plan_env文件夹中，其中v2有两份建图源码，grid_map.cpp和grid_map_bigmap.cpp改cmakelist可以选择编译哪份代码。
首先我们看一下V1中的建图代码流程：

grid_map.cpp
->	建图模块函数入口initMap( nh )
	1->	传入ros节点管理器node_ = nh;	
	2->	从参数服务器载入建图模块必要的参数
		//这里列出值得关注的变量，这些参数都是非常重要的，要在读代码的过程中不断理解参数的意义
		-----------------------------------------------------------------
		参数				变量名
		-----------------------------------------------------------------
		地图分辨率			mp_.resolution_
		地图大小				mp_.map_size_
		地图原点				mp_.map_origin_
		地图边界				mp_.map_min_boundary_
		地图边界				mp_.map_max_boundary_
		地图栅格个数			mp_.map_voxel_num_
		局部更新队列			mp_.local_update_range_
		障碍物膨胀系数		mp_.obstacles_inflation_
		深度命中值			mp_.p_hit_
		深度未命中值			mp_.p_miss_
		深度最小阈值			mp_.p_min_
		深度最大阈值			mp_.p_max_
		深度占据值			mp_.p_occ_
		最大投射长度			mp_.max_ray_length_
		局部地图边界			mp_.local_map_margin_
		地面高度				mp_.ground_height_

		缓存区大小			buffer_size			
		被占据缓存区			md_.occupancy_buffer_
		膨胀的被占据缓存区	md_.occupancy_buffer_inflate_
		被击中或未击中计数器	md_.count_hit_and_miss_
		被击中计数器			md_.count_hit_		
		-----------------------------------------------------------------
	3->	将深度阈值信息通过logit()函数映射一下//logit()函数将深度值从0→1映射到-∞ → ∞，映射后关于原点轴对称
	//后面都是参数赋值的部分，重要的参数我们都列出来了，就直接跳过参数处理的部分
	4->	接着调用同步函数sync_image_odom_()，同步深度图和里程计数据，并在回调函数里做数据处理，数据处理没什么好说的直接看代码
	5->	然后创建两个定时器occ_timer_、vis_timer_分别用于计算占据栅格信息和可视化
	6-> 设置需要的发布者，标志位初始化清零


//接下来细说两个定时器的回调函数，也是建图部分的主逻辑
updateOccupancyCallback()//概括下这个函数就是将深度信息和位姿结合，计算出地图中哪些点是被占据的
1-> 超时判断
2-> projectDepthImage()//这个函数简单来说就是从深度图中梯度深度信息，记录哪些被视为障碍物的点在全局坐标系中的位置
	->	如果不使用深度滤波器
		->	以mp_.k_depth_scaling_factor_跳像素扫描深度图
		->	根据相机成像原理，有深度点的三维相机系坐标信息//不明白的请看slam十四讲第2版 第97页到第99页的内容
		->	根据相机位姿计算该点在世界系中的位置坐标，将其存放在md_.proj_points_中
	-> 如果使用深度滤波
		->	第一步还是跳点扫面深度图，并且限制了边界
		->	对边界内的深度数值进行限幅(在图像边界、太近、太远)
		->	根据相机成像原理，有深度点的三维相机系坐标信息
		->	根据相机位姿计算该点在世界系中的位置坐标
	-> 如果使用图像连续检验
		->	还原这个点在上张图中的像素坐标以及在上个相机系中的三维位置
			->	如果这个像素在有效区间内
				->	上张深度图中该坐标点的深度值跟三维位置的z轴信息差值小于阈值mp_.depth_filter_tolerance_
					->	将其存放在md_.proj_points_中
			->	这个像素无法从上张图估计		
				->	将其存放在md_.proj_points_中	
3->	raycastProcess()//投射，就是从被占据点向相机原点进行投射，记录中间穿过的点是被占据还是未被占据的状态	
	->	md_.raycast_num_记录投射的进程次数
	->	  	// 记录更新区域的边值 bounding box of updated region
  			double min_x = mp_.map_max_boundary_(0);
  			double min_y = mp_.map_max_boundary_(1);
  			double min_z = mp_.map_max_boundary_(2);

  			double max_x = mp_.map_min_boundary_(0);
  			double max_y = mp_.map_min_boundary_(1);
  			double max_z = mp_.map_min_boundary_(2);
  	->	for 逐个处理proj_points_中的点pt_w
  		->	pt_w如果不在地图内，将其缩放到地图内
  			->	pt_w如果在视野外，将其缩放到视野范围内
  				setCacheOccupancy(pt_w, 0)
  		->	否则
  			setCacheOccupancy(pt_w, 1)
  			//setCacheOccupancy(pt_w，occ)将pt_w转化为缓存区编号idx_ctns
  			//函数每被调用一次 count_hit_and_miss_[idx_ctns]+=1
  			//如果count_hit_and_miss_[idx_ctns]==1则md_.cache_voxel_加入pt_w
  			//如果occ==1则md_.count_hit_[idx_ctns]+=1
  		->	缩紧局部边界范围//跳出所有循环后局部边界会更新为这次投射中所有用到的点的最大边界	
  		->	如果这个点已经在rayend序列里啦
  			->	跳过这个点
  		->	初始化raycaster，设置起点和终点		
  		->	从pt_w / mp_.resolution_向md_.camera_pos_ / mp_.resolution_进行投射，将投射中的点设置为setCacheOccupancy(tmp, 0)
  			->	如果遇到已经投射到的点，跳出本次循环
 	->	再次更新局部边界框大小，考虑了相机位置//如果相机在框内则大小不变，如果在框外就扩大一下框的大小
 	->	对边框进行变换、限幅 md_.local_bound_min_  md_.local_bound_max_//这个边框在障碍物膨胀时会用到
 	->	md_.local_updated_ = true		
 	->	根据相机位置设置局部占据队列 min_id	max_id//这个边框在计算占据信息时会用到，在参数服务器中写死的范围，框外的点在局部更新时都会理解为未占据的		
 	->	while() 逐个处理md_.cache_voxel_的点，(在上面for循环中将被投射到的点都加入到了cache_voxel_中)
 		->	三目运算：判断当前坐标被击中的概率是否大于0.5 如果是log_odds_update=mp_.prob_hit_log_否则log_odds_update=mp_.prob_miss_log_
 		->	清除这个坐标的计数值，为下次投射做准备
 		->	对这个坐标的占据信息进行判断
 			->	若本次预测为占据，且md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_
 				->	跳过这个点处理下一个
 			->	若本次预测为未占据，且md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_
 				->	md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_
 		->	判断这个坐标在不在局部占据队列范围内
 			->	对局部占据队列外的坐标：md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_
 		->	更新md_.occupancy_buffer_[idx_ctns]+=log_odds_update，并限幅			
4-> if md_.local_updated_ == true
	->	clearAndInflateLocalMap()//障碍物膨胀
		->	计算局部边框max_cut min_cut ,膨胀的局部边框max_cut_m min_cut_m
		->	将局部边框和膨胀的局部边框之间的区域设置为未知
		->	inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_)
		->	vector<Eigen::Vector3i> inf_pts(pow(2 * inf_step + 1, 3))
		->	清除md_.local_bound_min_  md_.local_bound_max_内的膨胀占据信息
		->	对md_.local_bound_min_  md_.local_bound_max_内点依次处理
			->	如果这个点在未膨胀的缓存区中是被占据的状态
				->	inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts)//膨胀
				->	对膨胀栅格内的所有点
					->	有效性校验
					->	md_.occupancy_buffer_inflate_[idx_inf] = 1
//	updateOccupancyCallback()函数结束	

visCallback()//可视化
1->	publishMapInflate(true)
	->	提取md_.occupancy_buffer_inflate_[idx_inf]中有占据信息的点，转化为点云数据，发布在map_inf_pub_上
2->	publishMap()			
	->	提取md_.occupancy_buffer_[idx_inf]>md.min_occupancy_log_点，转化为点云数据，发布在map_pub_上


//ego_planner v1部分的建图代码流程到此结束

v2的grid_map加入了滚动更新局部地图原点的机制，其他部分改动不大，感兴趣的朋友可以去读下源码