通過研究具體的代碼我們可以簡單了解VEnus中對于反光柱定位的具體流程。

1、IntensityExtraction::Extract

IntensityExtraction::IntensityRange2D &cloud,
                 VEnus::IntensityRange2D &candidate_cloud)

Extract函數的主要作用是從激光點云中提取出高反點，然后存儲到對應的容器中。輸入的數據類型為

VEnus::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::IntensityRange2D& candidate_cloud,
                  VEnus::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 > 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 > 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(
                 new FeaturePairCost(fpt_pair.second, fpt_pair.first)),
               new ceres::CauchyLoss(0.2), pose);
 }


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


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

最后這里輸出得到的是一個新的優化后的位姿pose

3.5 更新反光柱信息

注意到前面3.2中還有一個東西沒有處理，就是返回的wait_insert容器。

這個里面存放的是當時檢測到的反光柱但是這個反光柱沒能與地圖上其他地方的反光柱相匹配上，說明它可能是一個新的反光柱，對于這些信息我們要將其更新到最新的反光柱信息中：

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


 vector > new_fpt_wait_insert;
 vector 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，也就是反光柱之間的距離信息。

通過以上一系列步驟我們就可以得到一個連續的基于反光柱匹配的位姿估計算法。當然這個算法還有一點小問題，比如線匹配意味著每一個反光柱到周圍反光柱之間的距離都要獨一無二，否則就可能出現誤匹配的問題。

匹配閾值也不能設計的太大，否則也會發生匹配錯誤，但是設計的太小就可能導致本來是一個地方的反光柱沒有成功匹配上，最后該位置會生成出很多個反光柱，類似于這樣：

這里先介紹完代碼思路，具體的代碼實現以及優化過程下一章單獨展開介紹。

審核編輯：湯梓紅