venus反光柱建图记录

首先把常用的数据结构记录一下

IntensityPoint2D	一个激光强度点
IntensityRange2D	激光强度点的集合
Feature2DWithID	一个反光柱的点
Feature2DLlist	反光柱点的集合
Feature2DAdjacency	相邻特征点
AdjacencyList	反光柱点的集合，相邻特征点的集合
RobotPose	机器人的位姿

首先第一个函数：使用（Intensity方法）

extract_handler->Extract(cld_,candiate_cld_)

这个函数提取的是是符合要求的强度激光点，所以candiate_cld_存放的是符合要求的强度激光点的集合。

接着第二个函数：（使用DSU方法）

associate_handler->Association(candiate_cld_,feature_list)

这个函数是提取的反光柱的位置，所以feature_list是存放反光柱位置的集合。

接下来第三个函数：（使用Dustin方法）

mapping_handler->InsertFeatureList(feature_list_,adjacency_list_,pose)

这个函数把上一步求到的feature_list_作为输入，构建反光柱地图，所以adjacency_list_就是反光柱地图，pose是机器人在反光柱地图的位姿。

我们仔细分析这个一个函数InsertFeatureList

1-InsertFeatureList->InsertToFeatureGraph

这一步将新的反光柱的位置存入feature_points中，并更新反光柱地图的结构图（就是反光柱之间的边）feature_graph。

2-InsertFeatureList->RelocalizationRobot

这一步首先用新的观测点构建一个局部的反光柱图，第二步用局部图与加载的地图进行匹配，从而找到观测点对应的地图点，实现重定位功能，但是这里作者使用的方法是：从一点出发对应不同反光柱的距离，找到已知地图上相似的点，就实现了局部点和全局点的一一对应。（这里表示？？？？，万一距离相同但是角度不同怎么办）

3-InsertFeatureList->InitialCurrentPose(localization_hit,pose)

这一步是使用上一步匹配的点定位机器人当前的位姿。初始位姿。

4-InsertFeatureList->OptimizeCurrentPose(localization_hit,pose)

这一步是使用ceres优化上一步的初始位姿。

5-InsertFeatureList->SiftNewFeaturePoints()

这一步是已知初始位姿的情况下扩展反光柱地图，就不需要每步都进行重定位。这也是为什么即使有高度一致的反光柱也不会影响。

6-InsertFeatureList->DropAdjacencyList

这一步是在定位丢失时，保留之前的feature_points和feature_graph。