反光板导航SLAM（三）反光柱导航开发与实验

在上一章中简单了解了VEnus算法对于反光柱导航的基本思路。其主要分为了高反点提取、高反点聚类查找中心、高反点与已知反光柱位姿匹配以及调用ceres库进行位姿优化等步骤。然后在这个算法的基础上，再进行一定的开发达到一个比较稳定且可视化的版本。
使用：

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

简单录制一下运行效果，类似于这样：

out

这里使用的是自己录制的一个包，其中包含了激光数据以及里程计信息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;//定义存储r\p\y的容器
    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::Time::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::Time::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<int, std::pair<double, double>>::iterator ite;
    for(ite=feature_points.begin();ite!=feature_points.end();ite++)
    {
        cylinder_arrow.ns = "cylinder";    //命名空间namespace                                                                          
	      cylinder_arrow.type = visualization_msgs::Marker::CYLINDER;     //类型
        cylinder_arrow.action = visualization_msgs::Marker::ADD;
        cylinder_arrow.lifetime = ros::Duration(); //(sec,nsec),0 forever
        cylinder_arrow.header.frame_id = "map";
        cylinder_arrow.header.stamp = ros::Time::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里面。所以在这个位置就出现了一个新的反光柱。这个问题有两种解决方案：第一种比较简单就是增大范围判断的阈值。这样子同一个位置的反光柱就不会出现第二个了。另外一种是在新增反光柱的时候判断一下这个点周围多少范围内是否存在反光柱，如果有一个的话就不用新增了。