反光板導(dǎo)航SLAM：VEnus代碼淺析

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

1、IntensityExtraction::Extract

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

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

VEnus::IntensityRange2D

IntensityRange2D數(shù)據(jù)類型是定義在sensor/proto/sensor.proto文件內(nèi)

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

IntensityPoint2D的數(shù)據(jù)格式如下：

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

repeated 類似于std的vector，可以用來存放N個相同類型的內(nèi)容。所以這個函數(shù)的輸入cloud是一系列帶有強(qiáng)度信息的2D坐標(biāo)點。函數(shù)的candidate_cloud代表的是匹配到的可能的反光柱點。所以它們數(shù)據(jù)結(jié)構(gòu)是一樣的。

然后是函數(shù)實現(xiàn)，這個函數(shù)其實很簡單，它只是對整個點云進(jìn)行了一次遍歷，取連續(xù)的n個點，這個由一個參數(shù)intensity_median_filter_param決定，如果反光柱粗一點的或者激光分辨率高一點的話可以設(shè)置大一點，否則的話可以設(shè)置小一點，例如3。

然后對這些點進(jìn)行判斷，只要其中有一半的點云強(qiáng)度超過閾值intensity_threshold則認(rèn)為這里存在一個反光柱，將其中的點云強(qiáng)度處于中間值的那個點存入到容器candidate_cloud。這里其實有點隨機(jī)性在里面，因為沒有把所有的點都加入到容器中，所以是存在一定遺漏的。

2、DBscanAssociation::Association

DBscanAssociation::IntensityRange2D& candidate_cloud,
                  VEnus::Feature2DList& feature_list)

前面我們提取出了反光柱的中心點，但是這里的點云中可能會出現(xiàn)很多個點代表同一個反光柱的情況。所以這個函數(shù)即是對剛才的這些點進(jìn)行一次類似于聚類的操作。

candidate_cloud為第一步中選出來的高反點。feature_list為可能的反光柱中心。然后我們具體看一下函數(shù)實現(xiàn)：

Association的第一部分主要是遍歷了所有點，通過調(diào)用expand_cluster函數(shù)進(jìn)行一個分類：

 for (; iter < dataset.end(); ++iter) {
 ? ?if (iter->status == UNCLASSIFIED) {
   //聚類，將所有高反點根據(jù)相互之間距離分類，分類完成的點status為CLASSIFIED，同時處于同一類的點ID一致。
   if (expand_cluster(iter, cluster_id)) {
    cluster_id++;
   }
  }
 }

dataset里面存儲的就是當(dāng)前幀的高反點，來自于candidate_cloud，數(shù)據(jù)類型為：

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

可以看到對于每一個高反點，都有坐標(biāo)x/y、狀態(tài)status以及id等幾個屬性。初始狀態(tài)下狀態(tài)都為UNCLASSIFIED未區(qū)分，id都為0。然后函數(shù)對于每個點調(diào)用expand_cluster函數(shù)。

這個函數(shù)的作用是：對于每一個candidate_cloud，找到所有candidate_cloud中的點與其接近的點(兩者間距離小于一定閾值)這些點的status都會被設(shè)置為CLASSIFIED防止重復(fù)判斷。id都會被設(shè)置為相同代表同一個反光柱。

通過這種方式可以將所有點都劃分成一個個點的區(qū)域，每個區(qū)域代表了一個反光柱。

在劃分完成后，當(dāng)然是要對每個區(qū)域做處理：

//按照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());
  }
 }
 //找到每一組點的中心點的坐標(biāo)，存放到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)));
 }
 //判斷每個中心點之間的距離，將距離過近的中心點視為代表同一個反光柱，更新容器中反光柱中心點坐標(biāo)
 merge_cluster(tmp_res);

首先這里通過map維護(hù)了每一個待選的反光柱信息，所有的同一個反光柱的信息放到一起。

進(jìn)行一個劃分，然后對每一類點集調(diào)用merge_cluster進(jìn)行一個中心點的求取。求取的方式其實很簡單就是簡單相加求平均。這樣子就可以知道了所有反光柱中心點所在的位置。

3、CartoMapping::InsertFeatureList

這個函數(shù)的功能還是挺多的，包括匹配，位姿估計，位姿優(yōu)化等一系列其實都是在這個函數(shù)中實現(xiàn)的。詳細(xì)看一下這個函數(shù)具體情況:

3.1 初始化

InsertFeatureList函數(shù)的第一步判斷了feature_points容器是否為空，這里面存儲的是全局坐標(biāo)下的反光柱。這個容器為空說明目前沒有已經(jīng)確定的反光柱。則進(jìn)行初始化。

if (feature_points.empty())

初始化的方式比較簡單,將第一幀的反光柱坐標(biāo)存儲到feature_points中作為初始狀態(tài)下反光柱的中心坐標(biāo)，然后更新FeatureGraph。

注意到這里的函數(shù)：InsertToFeatureGraph，這個函數(shù)的意義是對于每一個待插入的點(反光柱)，計算它與周圍反光柱之間的距離，然后存儲到feature_graph中。注意到feature_graph的數(shù)據(jù)類型。

第一個int為對應(yīng)的feature_points的ID，第二個std::unordered_map中的int為與這個feature_points相互關(guān)聯(lián)的反光柱點的ID，后面的double類型數(shù)據(jù)為兩個點之間的距離。

3.2 特征點匹配

SiftNewFeaturePoints(input_fpts, hit_id_pair, wait_insert);

在確認(rèn)初始化完成的情況下，調(diào)用SiftNewFeaturePoints函數(shù)進(jìn)行反光柱的匹配。這個函數(shù)有三個參數(shù)：input_fpts為前面識別出來的反光柱坐標(biāo)，hit_id_pair為當(dāng)前幀反光柱與世界坐標(biāo)系下的反光柱匹配上的ID，wait_insert為當(dāng)前幀中沒能跟世界坐標(biāo)系下反光柱所匹配上的點的ID。函數(shù)主要執(zhí)行了以下工作：

首先，對于所有當(dāng)前點，建立一個局部的local_feature_graph，記錄當(dāng)前點與點之間的距離。

然后，對于每一個當(dāng)前幀的點，使用其局部local_feature_graph與全局點進(jìn)行匹配。

注意這里不是點與點的匹配而是類似于線與線的匹配，如果一個點到其他點的距離與全局點中某個點到其周圍點的距離基本一致，則認(rèn)為這兩個點是匹配上了的，這時候會把這一對ID存放到hit_id_pair中。這個方式其實應(yīng)該是有問題的，如果兩個點到周圍點的距離都很接近，就可能發(fā)生誤匹配。

最后，對于input_fpts中剩下沒有匹配到的點，則會將其存放到wait_insert中，這個數(shù)據(jù)代表這里檢測出一個反光柱但是在全局中沒有，后面需要另外處理。

3.3 初始位姿估計

ComputeCurrentPose(localization_hit, pose)

這個函數(shù)emmm其實我沒看，因為似乎它基本就沒有正確計算出來過。不過也不要緊，直接看下一步

3.4 位姿優(yōu)化

OptimizeCurrentPose(localization_hit, pose);

OptimizeCurrentPose中需要輸入兩個變量：localization_hit里面存放的是當(dāng)前幀下匹配點全局點之間的坐標(biāo)。

pose是機(jī)器人當(dāng)前初步估計下的位姿，按照邏輯它應(yīng)該會來自于步驟3，但是由于3總是出問題所以大部分情況下它來自于上一次估計出來的位姿。

然后根據(jù)輸入的約束關(guān)系localization_hit以及初始位姿pose，調(diào)用ceres優(yōu)化得到一個新的位姿：

 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);

最后這里輸出得到的是一個新的優(yōu)化后的位姿pose

3.5 更新反光柱信息

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

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

 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將這些反光柱點轉(zhuǎn)到世界坐標(biāo)系下，然后再根據(jù)初始化時候的方式一樣更新feature_points容器，存儲新的反光柱信息。當(dāng)然最后還要調(diào)用：

InsertToFeatureGraph(new_fpt_wait_insert, new_fpt_assigned_id);

來更新feature_graph，也就是反光柱之間的距離信息。

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

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

這里先介紹完代碼思路，具體的代碼實現(xiàn)以及優(yōu)化過程下一章單獨展開介紹。

審核編輯：湯梓紅