ROS系统下调用opencv：FAST特征检测算子

在我上一篇文章中说到，要在无人机上跑视觉算法。而团队师兄的方案是程序运行在ROS系统下，这样控制和视觉分离，比较好分工。ROS是什么？机器人操作系统(Robot Operating System, ROS)是一个应用于机器人上的操作系统,它操作方便、功能强大,特别适用于机器人这种多节点多任务的复杂场景。 因此自ROS诞生以来,受到了学术界和工业界的欢迎,如今已经广泛应用于机械臂、移动底盘、无人机、无人车等许多种类的机器人上。本文以一个简单的在ROS下调用opencv的demo供读者学习～～

ROS Kinetic

1.运行环境

• Ubuntu16.04 LTS
• ROS Kinetic
• opencv3.3 & contribe
• Roboware studio （ROS的一个IDE，strongly recommended）

2.创建工作空间

使用Roboware创建以下层次结构，教程可参照网上的其他教程。

工作空间

• vision_pac文件夹表示一个包（package），package是ROS源代码存放的地方,任何ROS的代码无论是C++还是Python都要放到package中,这样才能正常的编译和运行。

• src中存放的是C++和Python源代码，这里每一个.cpp文件都是一个节点（node）。从功能角度来说,通常一个node负责者机器人的某一个单独的功能。由于机器人的功能模块非常复杂,我们往往不会把所有功能都集中到一个node上,而会采用分布式的方式,把鸡蛋放到不同的篮子里。自然，节点之间会通过各种方法来进行通信，ROS中有两种，一种是topic通信，另一种是service通信。本文中使用的是topic通信。

• ROS平台始终的是cmake工具进行编译，并且ROS对cmake进行扩展，于是便有了Catkin编译系统。CMakeLists.txt原本是Cmake编译系统的规则文件,而Catkin编译系统基本沿用了CMake的编译风格,只是针对ROS工程添加了一些宏定义。所以在写法上,catkin的 CMakeLists.txt 与CMake的基本一致。这个文件直接规定了这个package要依赖哪些package,要编译生成哪些目标,如何编译等等流程。Roboware帮我们完成了大部分工作，不需要关心这个文件的具体编写。

• pacakge.xml 包含了package的名称、版本号、内容描述、维护人员、软件许可、编译构建工具、编译依赖、运行依赖等信息。 rospack find、rosdep 等命令能快速定位和分析出package的依赖项信息，原因就是直接读取了每一个pacakge中的 package.xml文件。

3.算法原理

这次的demo中，使用的算法是FAST特征检测。FAST特征检测算子基于Harris角点检测，它简化了Harris检测算子的过程。Harris检测算子本质上是对像素点创建一个窗口，并对周围各个方向求导。如果垂直的两个方向上变化很大，就判定为角点。其中用到协方差矩阵，特征值分解之类的理论，数学不好就不误人子弟了。。。但可以确定的是，求导是很消耗计算资源的，而且寻找特征点往往只是复杂检测流程中的一步。于是，就有了FAST算子。
FAST特征检测算法来源于corner的定义，这个定义基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个特征点。

FAST，图中为FAST-16

上图中选择了周围的16个像素点，称为FAST-16角点检测器。opencv中的FAST默认为FAST-9，当然你可以通过在构建检测器实例时指定FAST检测器的类型。

4.代码编写

1）先定义location.msg中的数据结构。通信的数据是一个二位坐标。

uint32 x
uint32 y

2）vision.cpp文件调用FAST特征检测算子，并发送给controller

/*******************************************************************
 * Created by 杨帮杰 on 9/21/18
 * Right to use this code in any way you want without warranty,
 * support or any guarantee of it working
 * E-mail: yangbangjie1998@qq.com
 * Association: SCAU 华南农大空机团
 ******************************************************************/

#include <ros/ros.h>
#include <vision_pac/location.h>        //自定义msg产生的头文件
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

#define PATH "/home/jacob/下载/bench1.jpg"

void ProcessImage(Mat& input, Mat& output, vector<KeyPoint>& points)
{
    points.clear();
    //创建FAST特征检测器，阈值为70
    Ptr<FastFeatureDetector> ptrFAST = FastFeatureDetector::create(70);
    ptrFAST->detect(input,points);
    drawKeypoints(input,points,output,Scalar::all(-1)); //在output上画出角点位置
    imshow("output",output);
    waitKey(10);
}

void PublishMsg(vision_pac::location &loc,ros::Publisher &pub,vector<KeyPoint>& points)
{
    for(vector<KeyPoint>::iterator it = points.begin(); it != points.end(); it++)
    {
        loc.x = it->pt.x;
        loc.y = it->pt.y;
        pub.publish(loc);//发布
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vision");        //用于解析ROS参数,第三个参数为本节点名
    ros::NodeHandle nh;             //实例化句柄,初始化node
    vision_pac::location msg;       //自定义location消息并初始化 
    //创建publisher,往"location_info"话题上发布消息
    ros::Publisher  pub = nh.advertise<vision_pac::location>("location_info",100);  
    ros::Rate loop_rate(1.0);           //定义发布的频率,1HZ
    Mat inputImage = imread(PATH);
    Mat outputImage = Mat::zeros(inputImage.size(),inputImage.type());
    vector<KeyPoint> keypoints; 
    while(ros::ok)          //循环发布msg
    {
        //计算一帧使用的时间
        double time1 = static_cast<double>(getTickCount());
        ProcessImage(inputImage,outputImage,keypoints); //对图像进行处理
        time1=((double)getTickCount()-time1)/getTickFrequency();//计算程序运行时间
        cout<<"一帧处理时间为"<<time1<<"s"<<endl;//输出运行时间
        PublishMsg(msg,pub,keypoints); //发布消息
        loop_rate.sleep();//根据前面的定义的loop_rate,设置1s的暂停           
    }
    return 0;
}

由于定义了location类型的消息，可以理解为定义了一个结构体，如

struct location
{
    int x;
    int y;
}

所以，可以通过这样的方式定义用于通信的数据结构

vision_pac::location msg; 

ROS节点的初始化十分简洁易懂，在官方wiki上说明详细。值得一提的是有时会发现通信丢包的情况（节点之间的通信是基于TCP/IP协议栈的）。一开始以为是ROS的bug，后来发现是因为缓冲区太小的原因。。。。所以程序出问题多找找自己的原因，尤其像我这种菜鸡- -！

ros::Publisher  pub = nh.advertise<vision_pac::location>("location_info",100); 

上面这条语句中，定义了一个Publisher。定义好的Publisher对象通过publish方法就可以把上面定义的msg发送出去。其中句柄nh的advertise方法中的泛型接口定义了发送出去的数据结构，第一个参数定义了topic的名字，第二个则是缓冲区的大小。缓冲区大小要特别注意，当一次发送多个msg时如果太小就会丢包。

3）controller.cpp中接受消息

/*******************************************************************
 * Created by 杨帮杰 on 9/21/18
 * Right to use this code in any way you want without warranty,
 * support or any guarantee of it working
 * E-mail: yangbangjie1998@qq.com
 * Association: SCAU 华南农大空机团
 ******************************************************************/
 
#include <ros/ros.h>
#include <vision_pac/location.h>

void msgCallback(const vision_pac::location::ConstPtr &msg)
{       
    int pointX,pointY;
    pointX = msg->x;
    pointY = msg->y;
    ROS_INFO("Controller:x = %d, y = %d",pointX,pointY);    //输出
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "controller");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("location_info", 100, msgCallback);//设置回调函数msgCallback
    //ros::spin()用于调用所有可触发的回调函数,将进入循环,不会返回,类似于在循环里反复调用spinOnce()    
    //而ros::spinOnce()只会去触发一次
    ros::spin();    
    return 0;
}

相应地，定义Subcriber对象去订阅主题，并指定缓冲区大小和回调函数。回调函数会接收到msg并作为一个引用参数去读写。

ROS_INFO("Controller:x = %d, y = %d",pointX,pointY);

上面这条语句相当于c语言中的格式输出printf，会在终端中输出相应消息。当然STL中的iostream也是可以使用的。

ros::spin(); 

而这条语句会阻塞线程，并不断检查是否有msg接收，有则调用回调函数。如果不执行这条语句，程序就直接return了。

5.结果验证

检测出来的角点
处理时间
角点坐标点

这篇文就至此为止了～～ROS还是很具有进步性的，库和IDE都让人有不错的编程体验。值得一提的是，ROS由于实时性欠佳，工业上很少使用。最近出了ROS-Industrial ，就是用于工业机器人的ROS。有时间去了解一下～

Reference:
Harris角点检测原理详解
OpenCV学习笔记（四十六）——FAST特征点检测features2D
学习opencv3（中文版） —— Adrian Kaehler & Gary Bradski
opencv计算机视觉编程攻略（第三版） —— Robert
中国大学MOOC---《机器人操作系统入门》课程讲义