通過(guò)研究具體的代碼我們可以簡(jiǎn)單了解VEnus中對(duì)于反光柱定位的具體流程。

1、IntensityExtraction::Extract

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

Extract函數(shù)的主要作用是從激光點(diǎn)云中提取出高反點(diǎn)，然后存儲(chǔ)到對(duì)應(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，可以用來(lái)存放N個(gè)相同類型的內(nèi)容。所以這個(gè)函數(shù)的輸入cloud是一系列帶有強(qiáng)度信息的2D坐標(biāo)點(diǎn)。函數(shù)的candidate_cloud代表的是匹配到的可能的反光柱點(diǎn)。所以它們數(shù)據(jù)結(jié)構(gòu)是一樣的。

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

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

2、DBscanAssociation::Association

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3、CartoMapping::InsertFeatureList

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

3.1 初始化

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

if (feature_points.empty())

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

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

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

3.2 特征點(diǎn)匹配

SiftNewFeaturePoints(input_fpts, hit_id_pair, wait_insert);

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

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

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

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

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

3.3 初始位姿估計(jì)

ComputeCurrentPose(localization_hit, pose)

這個(gè)函數(shù)emmm其實(shí)我沒看，因?yàn)樗坪跛揪蜎]有正確計(jì)算出來(lái)過(guò)。不過(guò)也不要緊，直接看下一步

3.4 位姿優(yōu)化

OptimizeCurrentPose(localization_hit, pose);

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

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

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

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

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

3.5 更新反光柱信息

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

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

 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++;
 }

首先是通過(guò)矩陣matrixT將這些反光柱點(diǎn)轉(zhuǎn)到世界坐標(biāo)系下，然后再根據(jù)初始化時(shí)候的方式一樣更新feature_points容器，存儲(chǔ)新的反光柱信息。當(dāng)然最后還要調(diào)用：

InsertToFeatureGraph(new_fpt_wait_insert, new_fpt_assigned_id);

來(lái)更新feature_graph，也就是反光柱之間的距離信息。

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

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

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

審核編輯：湯梓紅