VEnus算法對于反光柱導航的基本思路，其主要分為了高反點提取、高反點聚類查找中心、高反點與已知反光柱位姿匹配以及調用ceres庫進行位姿優化等步驟。

然后在這個算法的基礎上，再進行一定的開發達到一個比較穩定且可視化的版本。

使用：

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

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

1、發布map到odom變換關系

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

所以這里需要重新計算一下從map到odom的累計漂移發布到TF樹中，以形成一個較為穩定的TF關系。

tf::Quaternion quat1;
  tf::quaternionMsgToTF(new_odom.pose.pose.orientation, quat1);
  double roll1, pitch1, yaw1;//定義存儲rpy的容器
  tf::Matrix3x3(quat1).getRPY(roll1, pitch1, yaw1);//進行轉換
  //上一時刻機器人的角度
  double angel2 = last_pose.theta();
  //yaw1為機器人在odom下的角度，相減得到兩個坐標系之間的角度差
  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);

通過已知機器人在map坐標系下的pose以及odom下的位姿，反求odom到map的TF。先計算兩個坐標系下的位姿的角度差，這個代表了兩個TF之間的旋轉分量。

在已知旋轉分量的情況下再計算兩個TF之間的平移分量。最后再將具體的TF關系通過broadcaster發布出來。

2、添加前端可視化插件

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

對于反光柱的消息，分為兩類：已經確定的以及當前的。已經確定的是指那些被放到feature中我們認為其已經代表一個正常的反光柱的信息的那些點。

當前的則是指當前時間下我通過TF關系將當前坐標系下的點變換到map坐標系下后的點。對于這兩種點。我們使用markarray來進行可視化操作，同時以不同的顏色(紅色/綠色)來區分它們，類似于這樣：

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;
    //設置標記尺寸
    cylinder_arrow.scale.x = 0.09; //m
    cylinder_arrow.scale.y = 0.09;
    cylinder_arrow.scale.z = 0.50;


     ///設置標記顏色
    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的可視化發布，另外一個類似就不摘抄了。最后得到的就是類似于下圖這樣子的對應關系：

3、優化反光柱位姿匹配問題

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

A到B的距離為5，A到C的距離為8，A到D的距離為6，B到C的距離為7，B到D的距離為4，C到D的距離為3。然后當前幀檢測到幾個反光柱點A1，B1，C1。A1到B1的距離為5，A1到C1的距離為4。

則其分別與BA、BD兩條邊對應，所以A1與B點對應。這種情況適合大部分反光柱，但是會存在一種不適合的情況：當全局反光柱中存在A、D兩個反光柱其到周圍其他反光柱之間的距離都相等的時候。

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

對于這個問題，我們在算法中的位姿匹配部分新添加了一個新的約束關系：在檢測與匹配當前時刻的反光柱與全局反光柱的時候，我們不僅對其存在的邊進行約束關系的判斷。

還對這兩個匹配上的點之間進行一定距離上的約束。如果當前幀下的投影點在全局坐標系下的位姿與對應匹配上的點的全局坐標差別較大的話，我們認為這是可能存在問題的。

舍棄這一個匹配結果繼續尋找其他可能存在的匹配點。這樣子可以保證基本每次匹配上的點都是比較正確的。類似于這樣子：

測試時在終端打印出了對應匹配點的坐標關系，point1是map坐標系下的反光柱的位姿，而point2是當前時刻下檢測到的高反點對應在map坐標系下的坐標。整個匹配結果基本上是正確的。

4、優化位姿匹配錯誤的跳點問題

跳點問題是指當反光柱匹配錯誤是，導致后面優化后的位姿突然一下子出現在離上一時刻很遠的地方。

這個在沒有優化匹配之前還是較容易出現的，但是匹配結果正確的話基本上不會出現。只是作為一個保險的手段還是在這里添加一下。

根據運動邏輯我們知道機器人運動過程不可能一下子出現在離上一時刻很遠的地方。所以在優化完位姿后，算法會在更新位姿之前對新的位姿進行一次判斷。

如果位姿與前一時刻想差較大，例如一下子跳到2-3米遠。時間間隔才0.1S，那肯定是有問題的。這個位姿不應該給到其他地方進行運動計算。所以這時候需要舍棄這個結果重新計算。

一般場景中反光柱數量夠多的話是不會出現這個問題的，測試時基本位姿也都是正確的。

5、優化同一位置出現多個反光柱的問題

在上面關于rviz中的圖片中存在一個BUG，就是在同一個位置出現了兩個不同的紅色反光柱。

這個反光柱其實是代表的同一個反光柱，出現這個問題的原因是再于：在進行位姿匹配的過程中，當前幀的反光柱點與現有的反光柱點之間的距離超過了閾值，所以這兩個點沒有被匹配為一個點。

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

這樣子同一個位置的反光柱就不會出現第二個了。另外一種是在新增反光柱的時候判斷一下這個點周圍多少范圍內是否存在反光柱，如果有一個的話就不用新增了。

審核編輯：湯梓紅