VEnus算法對(duì)于反光柱導(dǎo)航的基本思路，其主要分為了高反點(diǎn)提取、高反點(diǎn)聚類查找中心、高反點(diǎn)與已知反光柱位姿匹配以及調(diào)用ceres庫(kù)進(jìn)行位姿優(yōu)化等步驟。

然后在這個(gè)算法的基礎(chǔ)上，再進(jìn)行一定的開發(fā)達(dá)到一個(gè)比較穩(wěn)定且可視化的版本。

使用：

ffmpeg -f x11grab -framerate 25 -video_size 1920*1080 -i :0.0 out.mp4

這里使用的是自己錄制的一個(gè)包，其中包含了激光數(shù)據(jù)以及里程計(jì)信息odom。為了使結(jié)果看起來更舒適，主要進(jìn)行了以下幾個(gè)部分的改進(jìn)：

1、發(fā)布map到odom變換關(guān)系

為了在rviz中進(jìn)行點(diǎn)云以及反光柱的顯示，需要綁定一個(gè)固定TF，一開始使用的是以odom作為固定坐標(biāo)系，但是由于odom本身存在比較大的累積誤差，所以后面發(fā)現(xiàn)效果并不好。

所以這里需要重新計(jì)算一下從map到odom的累計(jì)漂移發(fā)布到TF樹中，以形成一個(gè)較為穩(wěn)定的TF關(guān)系。

tf::Quaternion quat1;
  tf::quaternionMsgToTF(new_odom.pose.pose.orientation, quat1);
  double roll1, pitch1, yaw1;//定義存儲(chǔ)rpy的容器
  tf::Matrix3x3(quat1).getRPY(roll1, pitch1, yaw1);//進(jìn)行轉(zhuǎn)換
  //上一時(shí)刻機(jī)器人的角度
  double angel2 = last_pose.theta();
  //yaw1為機(jī)器人在odom下的角度，相減得到兩個(gè)坐標(biāo)系之間的角度差
  double angel = yaw1-angel2;
  odom_trans.header.frame_id = "map";
 odom_trans.child_frame_id = "odom";
  odom_trans.header.stamp = ros::now();
 odom_trans.transform.translation.x = (last_pose.x() - new_odom.pose.pose.position.x * cos(-angel) + new_odom.pose.pose.position.y * sin(-angel));
 odom_trans.transform.translation.y = (last_pose.y() - new_odom.pose.pose.position.x * sin(-angel) - new_odom.pose.pose.position.y * cos(-angel));
 odom_trans.transform.translation.z = 0;
 odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(-angel);
  odom_trans.header.stamp = ros::now();
  ROS_INFO("tf map_to_odom: x = %f,y = %f,theta = %f",new_odom.pose.pose.position.x - last_pose.x(),new_odom.pose.pose.position.y - last_pose.y(),angel);
  broadcaster.sendTransform(odom_trans);

通過已知機(jī)器人在map坐標(biāo)系下的pose以及odom下的位姿，反求odom到map的TF。先計(jì)算兩個(gè)坐標(biāo)系下的位姿的角度差，這個(gè)代表了兩個(gè)TF之間的旋轉(zhuǎn)分量。

在已知旋轉(zhuǎn)分量的情況下再計(jì)算兩個(gè)TF之間的平移分量。最后再將具體的TF關(guān)系通過broadcaster發(fā)布出來。

2、添加前端可視化插件

為了能夠在rviz中更加方便的查看機(jī)器人的運(yùn)動(dòng)以及匹配情況，需要在rviz中添加一下反光柱與機(jī)器人的一些可視化消息。

對(duì)于反光柱的消息，分為兩類：已經(jīng)確定的以及當(dāng)前的。已經(jīng)確定的是指那些被放到feature中我們認(rèn)為其已經(jīng)代表一個(gè)正常的反光柱的信息的那些點(diǎn)。

當(dāng)前的則是指當(dāng)前時(shí)間下我通過TF關(guān)系將當(dāng)前坐標(biāo)系下的點(diǎn)變換到map坐標(biāo)系下后的點(diǎn)。對(duì)于這兩種點(diǎn)。我們使用markarray來進(jìn)行可視化操作，同時(shí)以不同的顏色(紅色/綠色)來區(qū)分它們，類似于這樣：

std::unordered_map>::iterator ite;
  for(ite=feature_points.begin();ite!=feature_points.end();ite++)
  {
    cylinder_arrow.ns = "cylinder";  //命名空間namespace                                     
    cylinder_arrow.type = visualization_msgs::CYLINDER;   //類型
    cylinder_arrow.action = visualization_msgs::ADD;
    cylinder_arrow.lifetime = ros::Duration(); //(sec,nsec),0 forever
    cylinder_arrow.header.frame_id = "map";
    cylinder_arrow.header.stamp = ros::now();
    cylinder_arrow.id = ite->first;
    cylinder_arrow.pose.position.x = ite->second.first;
    cylinder_arrow.pose.position.y = ite->second.second;
    cylinder_arrow.pose.position.z = 0;
    cylinder_arrow.pose.orientation.w = 1.0;
    cylinder_arrow.pose.orientation.x = 0;
    cylinder_arrow.pose.orientation.y = 0;
    cylinder_arrow.pose.orientation.z = 0;
    //設(shè)置標(biāo)記尺寸
    cylinder_arrow.scale.x = 0.09; //m
    cylinder_arrow.scale.y = 0.09;
    cylinder_arrow.scale.z = 0.50;


     ///設(shè)置標(biāo)記顏色
    cylinder_arrow.color.a = 1.0; // Don't forget to set the alpha!
    cylinder_arrow.color.r = 1.0;
    cylinder_arrow.color.g = 0.0;
    cylinder_arrow.color.b = 0.0;
    cylinder_Array.markers.push_back(cylinder_arrow);
  }
  mark_info_pub.publish(cylinder_Array);
  cylinder_Array.markers.clear();

這里只是feature_point的可視化發(fā)布，另外一個(gè)類似就不摘抄了。最后得到的就是類似于下圖這樣子的對(duì)應(yīng)關(guān)系：

3、優(yōu)化反光柱位姿匹配問題

在上一章中分析源代碼的過程我們大致了解了算法的原理主要是基于對(duì)不同反光柱點(diǎn)的距離匹配的。例如對(duì)于我現(xiàn)有的反光柱ABCD。

A到B的距離為5，A到C的距離為8，A到D的距離為6，B到C的距離為7，B到D的距離為4，C到D的距離為3。然后當(dāng)前幀檢測(cè)到幾個(gè)反光柱點(diǎn)A1，B1，C1。A1到B1的距離為5，A1到C1的距離為4。

則其分別與BA、BD兩條邊對(duì)應(yīng)，所以A1與B點(diǎn)對(duì)應(yīng)。這種情況適合大部分反光柱，但是會(huì)存在一種不適合的情況：當(dāng)全局反光柱中存在A、D兩個(gè)反光柱其到周圍其他反光柱之間的距離都相等的時(shí)候。

那么新檢測(cè)到的反光柱可能會(huì)匹配到A也可能會(huì)匹配到D。這種情況下就會(huì)存在比較大的誤匹配問題。

對(duì)于這個(gè)問題，我們?cè)谒惴ㄖ械奈蛔似ヅ洳糠中绿砑恿艘粋€(gè)新的約束關(guān)系：在檢測(cè)與匹配當(dāng)前時(shí)刻的反光柱與全局反光柱的時(shí)候，我們不僅對(duì)其存在的邊進(jìn)行約束關(guān)系的判斷。

還對(duì)這兩個(gè)匹配上的點(diǎn)之間進(jìn)行一定距離上的約束。如果當(dāng)前幀下的投影點(diǎn)在全局坐標(biāo)系下的位姿與對(duì)應(yīng)匹配上的點(diǎn)的全局坐標(biāo)差別較大的話，我們認(rèn)為這是可能存在問題的。

舍棄這一個(gè)匹配結(jié)果繼續(xù)尋找其他可能存在的匹配點(diǎn)。這樣子可以保證基本每次匹配上的點(diǎn)都是比較正確的。類似于這樣子：

測(cè)試時(shí)在終端打印出了對(duì)應(yīng)匹配點(diǎn)的坐標(biāo)關(guān)系，point1是map坐標(biāo)系下的反光柱的位姿，而point2是當(dāng)前時(shí)刻下檢測(cè)到的高反點(diǎn)對(duì)應(yīng)在map坐標(biāo)系下的坐標(biāo)。整個(gè)匹配結(jié)果基本上是正確的。

4、優(yōu)化位姿匹配錯(cuò)誤的跳點(diǎn)問題

跳點(diǎn)問題是指當(dāng)反光柱匹配錯(cuò)誤是，導(dǎo)致后面優(yōu)化后的位姿突然一下子出現(xiàn)在離上一時(shí)刻很遠(yuǎn)的地方。

這個(gè)在沒有優(yōu)化匹配之前還是較容易出現(xiàn)的，但是匹配結(jié)果正確的話基本上不會(huì)出現(xiàn)。只是作為一個(gè)保險(xiǎn)的手段還是在這里添加一下。

根據(jù)運(yùn)動(dòng)邏輯我們知道機(jī)器人運(yùn)動(dòng)過程不可能一下子出現(xiàn)在離上一時(shí)刻很遠(yuǎn)的地方。所以在優(yōu)化完位姿后，算法會(huì)在更新位姿之前對(duì)新的位姿進(jìn)行一次判斷。

如果位姿與前一時(shí)刻想差較大，例如一下子跳到2-3米遠(yuǎn)。時(shí)間間隔才0.1S，那肯定是有問題的。這個(gè)位姿不應(yīng)該給到其他地方進(jìn)行運(yùn)動(dòng)計(jì)算。所以這時(shí)候需要舍棄這個(gè)結(jié)果重新計(jì)算。

一般場(chǎng)景中反光柱數(shù)量夠多的話是不會(huì)出現(xiàn)這個(gè)問題的，測(cè)試時(shí)基本位姿也都是正確的。

5、優(yōu)化同一位置出現(xiàn)多個(gè)反光柱的問題

在上面關(guān)于rviz中的圖片中存在一個(gè)BUG，就是在同一個(gè)位置出現(xiàn)了兩個(gè)不同的紅色反光柱。

這個(gè)反光柱其實(shí)是代表的同一個(gè)反光柱，出現(xiàn)這個(gè)問題的原因是再于：在進(jìn)行位姿匹配的過程中，當(dāng)前幀的反光柱點(diǎn)與現(xiàn)有的反光柱點(diǎn)之間的距離超過了閾值，所以這兩個(gè)點(diǎn)沒有被匹配為一個(gè)點(diǎn)。

于是在求完新的位姿后，這個(gè)點(diǎn)作為一個(gè)新的反光柱點(diǎn)被添加到了feature_point里面。所以在這個(gè)位置就出現(xiàn)了一個(gè)新的反光柱。這個(gè)問題有兩種解決方案：第一種比較簡(jiǎn)單就是增大范圍判斷的閾值。

這樣子同一個(gè)位置的反光柱就不會(huì)出現(xiàn)第二個(gè)了。另外一種是在新增反光柱的時(shí)候判斷一下這個(gè)點(diǎn)周圍多少范圍內(nèi)是否存在反光柱，如果有一個(gè)的話就不用新增了。

審核編輯：湯梓紅