反光板导航SLAM（二）VEnus代码浅析

上一章简单介绍了VEnus中几个主要函数的作用，这里详细展开看一下每个函数的具体思路，通过研究具体的代码我们可以简单了解VEnus中对于反光柱定位的具体流程。

1、IntensityExtraction::Extract

IntensityExtraction::Extract(VEnus::Sensor::IntensityRange2D &cloud,
                                  VEnus::Sensor::IntensityRange2D &candidate_cloud)

Extract函数的主要作用是从激光点云中提取出高反点，然后存储到对应的容器中。输入的数据类型为

VEnus::Sensor::IntensityRange2D 

IntensityRange2D数据类型是定义在sensor/proto/sensor.proto文件内

message IntensityRange2D
{
    int64 timestamp = 1;
    string frame_id = 2;
    repeated IntensityPoint2D points = 3;
}

IntensityPoint2D的数据格式如下：

message IntensityPoint2D
{
    float x = 1;
    float y = 2;
    float intensity = 3;
}

repeated 类似于std的vector，可以用来存放N个相同类型的内容。所以这个函数的输入cloud是一系列带有强度信息的2D坐标点。函数的candidate_cloud代表的是匹配到的可能的反光柱点。所以它们数据结构是一样的。

然后是函数实现，这个函数其实很简单，它只是对整个点云进行了一次遍历，取连续的n个点，这个由一个参数intensity_median_filter_param决定，如果反光柱粗一点的或者激光分辨率高一点的话可以设置大一点，否则的话可以设置小一点，例如3。然后对这些点进行判断，只要其中有一半的点云强度超过阈值intensity_threshold则认为这里存在一个反光柱，将其中的点云强度处于中间值的那个点存入到容器candidate_cloud。

2、DBscanAssociation::Association

DBscanAssociation::Association(VEnus::Sensor::IntensityRange2D& candidate_cloud,
                                    VEnus::Sensor::Feature2DList& feature_list)

前面我们提取出了反光柱的中心点，但是这里的点云中可能会出现很多个点代表同一个反光柱的情况。所以这个函数即是对刚才的这些点进行一次类似于聚类的操作。candidate_cloud为第一步中选出来的高反点。feature_list为可能的反光柱中心。然后我们具体看一下函数实现：

Association的第一部分主要是遍历了所有点，通过调用expand_cluster函数进行一个分类：

  for (; iter < dataset.end(); ++iter) {
    if (iter->status == UNCLASSIFIED) {
      //聚类，将所有高反点根据相互之间距离分类，分类完成的点status为CLASSIFIED，同时处于同一类的点ID一致。
      if (expand_cluster(iter, cluster_id)) {
        cluster_id++;
      }
    }
  }

dataset里面存储的就是当前帧的高反点，来自于candidate_cloud，数据类型为：

PointDBSCAN(double px, double py) {
      x = px;
      y = py;
      status = UNCLASSIFIED;
      id = 0;
    }

可以看到对于每一个高反点，都有坐标x/y、状态status以及id等几个属性。初始状态下状态都为UNCLASSIFIED未区分，id都为0。然后函数对于每个点调用expand_cluster函数。这个函数的作用是：对于每一个candidate_cloud，找到所有candidate_cloud中的点与其接近的点(两者间距离小于一定阈值)这些点的status都会被设置为CLASSIFIED防止重复判断。id都会被设置为相同代表同一个反光柱。通过这种方式可以将所有点都划分成一个个点的区域，每个区域代表了一个反光柱。

在划分完成后，当然是要对每个区域做处理：

//按照ID将所有点放到map容器中
  unordered_map<int, vector<int> > feature_dict;
  iter = dataset.begin();
  for (; iter < dataset.end(); ++iter) {
    if (iter->status == CLASSIFIED) {
      feature_dict[iter->id].push_back(iter - dataset.begin());
    }
  }
  //找到每一组点的中心点的坐标，存放到tmp_res里面
  vector<pair<double, double> > tmp_res;
  for (auto pr : feature_dict) {
    double center_x = 0;
    double center_y = 0;
    for (int idx : pr.second) {
      center_x += dataset.at(idx).x;
      center_y += dataset.at(idx).y;
    }
    int sz = pr.second.size();
    //ROS_INFO("center point,x = %f,y = %f",center_x / double(sz),center_y / double(sz));
    tmp_res.push_back(std::make_pair(center_x / double(sz), center_y / double(sz)));
  }
  //判断每个中心点之间的距离，将距离过近的中心点视为代表同一个反光柱，更新容器中反光柱中心点坐标
  merge_cluster(tmp_res);

首先这里通过map维护了每一个待选的反光柱信息，所有的同一个反光柱的信息放到一起。进行一个划分，然后对每一类点集调用merge_cluster进行一个中心点的求取。求取的方式其实很简单就是简单相加求平均。这样子就可以知道了所有反光柱中心点所在的位置。

3、CartoMapping::InsertFeatureList

这个函数的功能还是挺多的，包括匹配，位姿估计，位姿优化等一系列其实都是在这个函数中实现的。详细看一下这个函数具体情况:

3.1、初始化

InsertFeatureList函数的第一步判断了feature_points容器是否为空，这里面存储的是全局坐标下的反光柱。这个容器为空说明目前没有已经确定的反光柱。则进行初始化。

if (feature_points.empty())

初始化的方式比较简单,将第一帧的反光柱坐标存储到feature_points中作为初始状态下反光柱的中心坐标，然后更新FeatureGraph。注意到这里的函数：InsertToFeatureGraph，这个函数的意义是对于每一个待插入的点(反光柱)，计算它与周围反光柱之间的距离，然后存储到feature_graph中。注意到feature_graph的数据类型>。第一个int为对应的feature_points的ID，第二个std::unordered_map中的int为与这个feature_points相互关联的反光柱点的ID，后面的double类型数据为两个点之间的距离。

3.2、特征点匹配

SiftNewFeaturePoints(input_fpts, hit_id_pair, wait_insert);

在确认初始化完成的情况下，调用SiftNewFeaturePoints函数进行反光柱的匹配。这个函数有三个参数：input_fpts为前面识别出来的反光柱坐标，hit_id_pair为当前帧反光柱与世界坐标系下的反光柱匹配上的ID，wait_insert为当前帧中没能跟世界坐标系下反光柱所匹配上的点的ID。函数主要执行了以下工作：

首先，对于所有当前点，建立一个局部的local_feature_graph，记录当前点与点之间的距离。

然后，对于每一个当前帧的点，使用其局部local_feature_graph与全局点进行匹配。注意这里不是点与点的匹配而是类似于线与线的匹配，如果一个点到其他点的距离与全局点中某个点到其周围点的距离基本一致，则认为这两个点是匹配上了的，这时候会把这一对ID存放到hit_id_pair中。这个方式其实应该是有问题的，如果两个点到周围点的距离都很接近，就可能发生误匹配。

最后，对于input_fpts中剩下没有匹配到的点，则会将其存放到wait_insert中，这个数据代表这里检测出一个反光柱但是在全局中没有，后面需要另外处理。

3.3、初始位姿估计

ComputeCurrentPose(localization_hit, pose)

这个函数emmm其实我没看，因为似乎它基本就没有正确计算出来过。不过也不要紧，直接看下一步

3.4、位姿优化

OptimizeCurrentPose(localization_hit, pose);

OptimizeCurrentPose中需要输入两个变量：localization_hit里面存放的是当前帧下匹配点全局点之间的坐标。pose是机器人当前初步估计下的位姿，按照逻辑它应该会来自于步骤3，但是由于3总是出问题所以大部分情况下它来自于上一次估计出来的位姿。

然后根据输入的约束关系localization_hit以及初始位姿pose，调用ceres优化得到一个新的位姿：

  ceres::Problem problem;
  double pose[3] = {robot_pose.x(), robot_pose.y(), robot_pose.theta()};

  for (auto fpt_pair : match_result) {
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<FeaturePairCost, 2, 3>(
                                 new FeaturePairCost(fpt_pair.second, fpt_pair.first)),
                             new ceres::CauchyLoss(0.2), pose);
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

最后这里输出得到的是一个新的优化后的位姿pose

3.5、更新反光柱信息

注意到前面3.2中还有一个东西没有处理，就是返回的wait_insert容器。这个里面存放的是当时检测到的反光柱但是这个反光柱没能与地图上其他地方的反光柱相匹配上，说明它可能是一个新的反光柱，对于这些信息我们要将其更新到最新的反光柱信息中：

  Eigen::Isometry2d matrixT = Eigen::Isometry2d::Identity();
  matrixT.pretranslate(Eigen::Vector2d(pose.x(), pose.y()));
  matrixT.rotate(pose.theta());
  //   Eigen::Isometry2d matrixTinv = matrixT.inverse();

  vector<pair<double, double> > new_fpt_wait_insert;
  vector<int> new_fpt_assigned_id;
  for (auto unhit : wait_insert) {
    Eigen::Vector3d local_fpt(unhit.first, unhit.second, 1.0);
    auto global_fpt = matrixT * local_fpt;

    auto new_fpt = make_pair(global_fpt[0], global_fpt[1]);
    feature_points[feature_point_cnt] = new_fpt;
    new_fpt_wait_insert.push_back(new_fpt);
    new_fpt_assigned_id.push_back(feature_point_cnt);
    feature_point_cnt++;
  }

首先是通过矩阵matrixT将这些反光柱点转到世界坐标系下，然后再根据初始化时候的方式一样更新feature_points容器，存储新的反光柱信息。当然最后还要调用：

InsertToFeatureGraph(new_fpt_wait_insert, new_fpt_assigned_id);

来更新feature_graph，也就是反光柱之间的距离信息。

通过以上一系列步骤我们就可以得到一个连续的基于反光柱匹配的位姿估计算法。当然这个算法还有一点小问题，比如线匹配意味着每一个反光柱到周围反光柱之间的距离都要独一无二，否则就可能出现误匹配的问题。匹配阈值也不能设计的太大，否则也会发生匹配错误，但是设计的太小就可能导致本来是一个地方的反光柱没有成功匹配上，最后该位置会生成出很多个反光柱，类似于这样：

这里先介绍完代码思路，具体的代码实现以及优化过程下一章单独展开介绍。