OpenCV(opencv_apps)在ROS中的视频图像的应用（重点讲解哈里斯角点的检测）

1、引言

通过opencv_apps，你可以在ROS中以最简单的方式运行OpenCV提供的许多功能，也就是说，运行一个与功能相对应的launch启动文件，就可以跳过为OpenCV的许多功能编写OpenCV应用程序代码，非常的方便。
对于想熟悉每个细节的伙伴们，可以去看源码，对于熟悉视觉操作很有帮助。
官方说明：http://wiki.ros.org/opencv_apps
github源码：https://github.com/ros-perception/opencv_apps

2、启动操作

2.1、opencv_apps.launch

先来启动摄像头等相关操作，需要启动opencv_apps.launch文件，我们先来了解下：

gedit /home/jetson/workspace/catkin_ws/src/jetbot_ros/launch/opencv_apps.launch

<launch><arg name="img_flip" default="False"/><arg name="img_transform" default="True"/><group if="$(arg img_transform)"><arg name="img_topic" default="/csi_cam_0/image_raw"/><include file="$(find jetson_nano_csi_cam)/launch/jetson_csi_cam.launch"/><node name="img_transform" pkg="jetbot_ros" type="img_transform.py" output="screen"><param name="img_flip" type="bool" value="$(arg img_flip)"/><param name="img_topic" type="string" value="$(arg img_topic)"/></node></group>
</launch>

这里<include>节点包含一个jetson_csi_cam.launch启动文件以及一个名为img_transform的节点
其中jetson_csi_cam.launch我们查看下里面的内容：

cat /home/jetson/workspace/catkin_ws/src/jetson_nano_csi_cam_ros/launch/jetson_csi_cam.launch

里面是一些摄像头的参数设置和启动摄像头，通过GSCAM开源包将GStreamer图像流引入到ROS中，转换成sensor_msgs/Image类型的Image话题，发布到ROS中，供其他节点使用。

2.2、img_transform.py

然后就是做这些操作的节点，我们来看下它的源码：

gedit /home/jetson/workspace/catkin_ws/src/jetbot_ros/scripts/img_transform.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2 as cv
from cv_bridge import CvBridge
from sensor_msgs.msg import Imagedef topic(msg):if not isinstance(msg, Image):returnbridge = CvBridge()frame = bridge.imgmsg_to_cv2(msg, "bgr8")# Canonical input image sizeframe = cv.resize(frame, (640, 480))if img_flip == True: frame = cv.flip(frame, 1)# opencv mat ->  ros msgmsg = bridge.cv2_to_imgmsg(frame, "bgr8")pub_img.publish(msg)if __name__ == '__main__':rospy.init_node("pub_img", anonymous=False)img_topic = rospy.get_param("~img_topic", "/csi_cam_0/image_raw")img_flip = rospy.get_param("~img_flip", False)sub_img = rospy.Subscriber(img_topic, Image, topic)pub_img = rospy.Publisher("/image", Image, queue_size=10)rate = rospy.Rate(2)rospy.spin()

这里就是定义一个pub_img节点，订阅CSI摄像头相关的话题，然后通过Image话题进行发布，供其余节点使用。其中获取参数：rospy.get_param("~img_topic", "/csi_cam_0/image_raw")，如果没有在~img_topic获取到，就使用/csi_cam_0/image_raw默认值。同样的rospy.get_param("~img_flip", False)如果没有获取到~img_flip的值，就默认为False

2.3、打开相机

熟悉摄像头的相关操作之后，我们就来做一些准备工作，开启相机：
roslaunch jetbot_ros opencv_apps.launch
查看方式：rqt_image_view，如下图：

或者网页查看：rosrun web_video_server web_video_server
然后使用IP+端口就可以查看了，如下图：

然后点击里面的话题，就可以看到摄像视频了，如下图：

我们来看下开启了哪些话题：rostopic list

/csi_cam_0/camera_info
/csi_cam_0/image_raw
/csi_cam_0/image_raw/compressed
/csi_cam_0/image_raw/compressed/parameter_descriptions
/csi_cam_0/image_raw/compressed/parameter_updates
/csi_cam_0/image_raw/compressedDepth
/csi_cam_0/image_raw/compressedDepth/parameter_descriptions
/csi_cam_0/image_raw/compressedDepth/parameter_updates
/csi_cam_0/image_raw/theora
/csi_cam_0/image_raw/theora/parameter_descriptions
/csi_cam_0/image_raw/theora/parameter_updates
/image
/rosout
/rosout_agg

是一些关于csi摄像头相关的话题，接下来我们对里面大量的示例做一个演示

3、哈里斯角点

因为我们都是在opencv_apps包里面，所以先来这个所在工作区间的这个包的launch目录里面。
查看下有哪些启动文件：ls /home/jetson/workspace/catkin_ws/src/opencv_apps/launch/

adding_images.launch hough_circles.launch
camshift.launch hough_lines.launch
contour_moments.launch hsv_color_filter.launch
convex_hull.launch lk_flow.launch
corner_harris.launch people_detect.launch
discrete_fourier_transform.launch phase_corr.launch
edge_detection.launch pyramids.launch
face_detection.launch rgb_color_filter.launch
face_recognition.launch segment_objects.launch
fback_flow.launch simple_flow.launch
find_contours.launch smoothing.launch
general_contours.launch threshold.launch
goodfeature_track.launch watershed_segmentation.launch
hls_color_filter.launch

可以看到对图像操作的功能还是挺多的，有霍夫圆、霍夫直线、轮廓矩、LK光流、哈里斯角点、边缘、人物、面部识别等等

3.1、corner_harris.launch

判断某个点是图像中的角点，这里的对角点的检测，我们花多点时间重点来说明下，后面的节点基本就是展示为主。
roslaunch opencv_apps corner_harris.launch
启动如下，使用一张棋盘格式的图片让它识别：

可以看到图片中的角点，有很多的小圆圈给标注着，这里的角点检测的多少和准确度，取决于上面的threshold阈值大小，可以自行调节，调小了，角点数量就会多，相对而言准确度也在下降，调大阈值，角点数量就相应减少，准确度要高。

我们来看下这个启动文件里面的内容：

cat  /home/jetson/workspace/catkin_ws/src/opencv_apps/launch/corner_harris.launch

<launch><arg name="node_name" default="corner_harris" /><arg name="image" default="image" doc="The image topic. Should be remapped to the name of the real image topic." /><arg name="use_camera_info" default="false" doc="Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used." /><arg name="debug_view" default="true" doc="Specify whether the node displays a window to show image" /><arg name="queue_size" default="3" doc="Specigy queue_size of input image subscribers" /><arg name="threshold" default="200" doc="Threshold value of a circle around corners's norm" /><!-- corner_harris.cpp  --><node name="$(arg node_name)" pkg="opencv_apps" type="corner_harris" ><remap from="image" to="$(arg image)" /><param name="use_camera_info" value="$(arg use_camera_info)" /><param name="debug_view" value="$(arg debug_view)" /><param name="queue_size" value="$(arg queue_size)" /></node>
</launch>

3.2、corner_harris.cpp

前面是一些参数设置，后面是一个node节点部分，其中type="corner_harris"可以知道使用的是C++写的，因为如果是Python写的，就是type="corner_harris.py"这样的形式，当然那里也注释说明了是corner_harris.cpp
其中C++代码文件的地址：ls /home/jetson/workspace/catkin_ws/build/opencv_apps
我们来看下这个哈里斯角点的实际代码：
gedit /home/jetson/workspace/catkin_ws/build/opencv_apps/corner_harris.cpp

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2016, Kentaro Wada.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Kentaro Wada nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/#include <ros/ros.h>
#include <nodelet/loader.h>int main(int argc, char **argv)
{ros::init(argc, argv, "corner_harris", ros::init_options::AnonymousName);if (ros::names::remap("image") == "image") {ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n""\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");}// need to use 1 worker thread to prevent freezing in imshow, calling imshow from mutiple threads//nodelet::Loader manager(false);ros::param::set("~num_worker_threads", 1); // need to call   Loader(bool provide_ros_api = true);nodelet::Loader manager(true);nodelet::M_string remappings;nodelet::V_string my_argv(argv + 1, argv + argc);my_argv.push_back("--shutdown-on-close"); // Internalmanager.load(ros::this_node::getName(), "opencv_apps/corner_harris", remappings, my_argv);ros::spin();return 0;
}

这段代码就是初始化corner_harris节点，image话题的重映射等，然后用圆圈来标注角点，这个操作会用到接下来要讲的一个插件。

3.3、corner_harris_nodelet.cpp

上面的核心代码，会用到Nodelet，关于这个Nodelet插件的解释：
允许用户在ROS节点中添加自定义功能，Nodelet使得开发人员能够将复杂的代码封装到可重用的插件中，这些插件可以像其他ROS节点一样进行部署和通信，开发人员可以编写更加模块化和可维护的代码，提高ROS系统的可扩展性和可重用性。
更关键的是效率问题，Nodelet提供一种方法，可以让多个算法程序在一个进程中，使用共享指针shared_ptr来实现零拷贝通信，以降低因为拷贝传输大数据（比如图像流，点云等）而延迟的问题，换句话说就是将多个node捆绑在一起管理，使得同一个manager里面的话题的数据传输更快，因为不会在进程内传递消息时进行复制而产生的效率下降。

查看插件文件：ls /home/jetson/workspace/catkin_ws/src/opencv_apps/src/nodelet
我们打开哈里斯角点的插件代码来看下：

gedit /home/jetson/workspace/catkin_ws/src/opencv_apps/src/nodelet/corner_harris_nodelet.cpp

// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2016, JSK Lab.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Kei Okada nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <ros/ros.h>
#include "opencv_apps/nodelet.h"
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>#include <dynamic_reconfigure/server.h>
#include "opencv_apps/CornerHarrisConfig.h"// https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/TrackingMotion/cornerHarris_Demo.cpp
/*** @function cornerHarris_Demo.cpp* @brief Demo code for detecting corners using Harris-Stephens method* @author OpenCV team*/namespace opencv_apps
{
class CornerHarrisNodelet : public opencv_apps::Nodelet
{image_transport::Publisher img_pub_;image_transport::Subscriber img_sub_;image_transport::CameraSubscriber cam_sub_;ros::Publisher msg_pub_;boost::shared_ptr<image_transport::ImageTransport> it_;typedef opencv_apps::CornerHarrisConfig Config;typedef dynamic_reconfigure::Server<Config> ReconfigureServer;Config config_;boost::shared_ptr<ReconfigureServer> reconfigure_server_;int queue_size_;bool debug_view_;std::string window_name_;static bool need_config_update_;int threshold_;void reconfigureCallback(Config& new_config, uint32_t level){config_ = new_config;threshold_ = config_.threshold;}const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame){if (frame.empty())return image_frame;return frame;}void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info){doWork(msg, cam_info->header.frame_id);}void imageCallback(const sensor_msgs::ImageConstPtr& msg){doWork(msg, msg->header.frame_id);}static void trackbarCallback(int /*unused*/, void* /*unused*/){need_config_update_ = true;}void doWork(const sensor_msgs::ImageConstPtr& image_msg, const std::string& input_frame_from_msg){// Work on the image.try{// Convert the image into something opencv can handle.cv::Mat frame = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image;// Do the workcv::Mat dst, dst_norm, dst_norm_scaled;dst = cv::Mat::zeros(frame.size(), CV_32FC1);cv::Mat src_gray;if (frame.channels() > 1){cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY);}else{src_gray = frame;cv::cvtColor(src_gray, frame, cv::COLOR_GRAY2BGR);}/// Detector parametersint block_size = 2;int aperture_size = 3;double k = 0.04;/// Detecting cornerscv::cornerHarris(src_gray, dst, block_size, aperture_size, k, cv::BORDER_DEFAULT);/// Normalizingcv::normalize(dst, dst_norm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());cv::convertScaleAbs(dst_norm, dst_norm_scaled);/// Drawing a circle around cornersfor (int j = 0; j < dst_norm.rows; j++){for (int i = 0; i < dst_norm.cols; i++){if ((int)dst_norm.at<float>(j, i) > threshold_){cv::circle(frame, cv::Point(i, j), 5, cv::Scalar(0), 2, 8, 0);}}}/// Create windowif (debug_view_){cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);const int max_threshold = 255;if (need_config_update_){config_.threshold = threshold_;reconfigure_server_->updateConfig(config_);need_config_update_ = false;}cv::createTrackbar("Threshold:", window_name_, &threshold_, max_threshold, trackbarCallback);}if (debug_view_){cv::imshow(window_name_, frame);int c = cv::waitKey(1);}// Publish the image.sensor_msgs::Image::Ptr out_img =cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg();img_pub_.publish(out_img);}catch (cv::Exception& e){NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);}}void subscribe()  // NOLINT(modernize-use-override){NODELET_DEBUG("Subscribing to image topic.");if (config_.use_camera_info)cam_sub_ = it_->subscribeCamera("image", queue_size_, &CornerHarrisNodelet::imageCallbackWithInfo, this);elseimg_sub_ = it_->subscribe("image", queue_size_, &CornerHarrisNodelet::imageCallback, this);}void unsubscribe()  // NOLINT(modernize-use-override){NODELET_DEBUG("Unsubscribing from image topic.");img_sub_.shutdown();cam_sub_.shutdown();}public:virtual void onInit()  // NOLINT(modernize-use-override){Nodelet::onInit();it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));pnh_->param("queue_size", queue_size_, 3);pnh_->param("debug_view", debug_view_, false);if (debug_view_){always_subscribe_ = true;}window_name_ = "CornerHarris Demo";reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);dynamic_reconfigure::Server<Config>::CallbackType f =boost::bind(&CornerHarrisNodelet::reconfigureCallback, this, _1, _2);reconfigure_server_->setCallback(f);img_pub_ = advertiseImage(*pnh_, "image", 1);onInitPostProcess();}
};
bool CornerHarrisNodelet::need_config_update_ = false;
}  // namespace opencv_appsnamespace corner_harris
{
class CornerHarrisNodelet : public opencv_apps::CornerHarrisNodelet
{
public:virtual void onInit()  // NOLINT(modernize-use-override){ROS_WARN("DeprecationWarning: Nodelet corner_harris/corner_harris is deprecated, ""and renamed to opencv_apps/corner_harris.");opencv_apps::CornerHarrisNodelet::onInit();}
};
}  // namespace corner_harris#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(opencv_apps::CornerHarrisNodelet, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(corner_harris::CornerHarrisNodelet, nodelet::Nodelet);

主要关注其中检测角点的方法：

cv::cornerHarris(src_gray, dst, block_size, aperture_size, k, cv::BORDER_DEFAULT);

参数说明如下：

src_gray：输入的灰度Mat矩阵或浮点图像
dst：存储着哈里斯角点检测的结果，跟源图的尺寸和类型一样
block_size：邻域的大小
aperture_size：Sobel边缘检测滤波器大小
k：Harris中间参数，经验值0.04~0.06
cv::BORDER_DEFAULT：插值类型

我们也可以看到在发布和订阅时，用到的就是指针。
发布时：

sensor_msgs::Image::Ptr out_img =cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg();
img_pub_.publish(out_img);

订阅时：

Nodelet::onInit();
it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));if (config_.use_camera_info)cam_sub_ = it_->subscribeCamera("image", queue_size_, &CornerHarrisNodelet::imageCallbackWithInfo, this);elseimg_sub_ = it_->subscribe("image", queue_size_, &CornerHarrisNodelet::imageCallback, this);

这样我们在消息传递时，就只需要传指针了，当然这里是针对同一台设备的进程间通信，如果是不同设备那还是需要解引用传输实际内容，因为我们知道ROS的分布节点进行通信的协议是XML-RPC，本质也是HTTP协议，只不过编码格式是XML类型，它们之间的传输还得是拷贝内容进行通信。

查看节点关系：rqt_graph，我这里将调试节点隐藏，显得更清晰点，如下图：

可以看到CSI摄像头图像获取需要经过转换之后最终通过/Image话题发布给哈里斯角点算法处理。

识别角点的原理，简单来说就是在特征窗口里面，如果灰度发生了较大的变化，就认为这里是一个角点，有兴趣的可以查阅：harris.cpp

4、霍夫直线检测

接下来的部分，就没有上述那么去分析了，只是熟悉下常用的几个功能。
霍夫直线的检测在计算机视觉和图像处理中用途广泛，可以用于边缘检测、直线检测等。
实际场景中，可以通过从图像中检测出边缘，然后通过识别直线或曲线，将这些边缘连接起来，形成完整的物体。
另外无人驾驶的发展，对于自动化检测道路、车道线等应用也有着广泛的应用。
启动launch文件：roslaunch opencv_apps hough_lines.launch
很好的检测到了我身上的衣服以及上面的“中国”文字，如下图：

5、图像轮廓矩

contour_moments.launch是启动识别图像中轮廓的矩函数，这里的轮廓矩也可以理解成轮廓的特征，它也有着很广泛的应用：
目标识别：提取图像中物体的轮廓特征，可以对目标进行识别和分类。
目标检测：通过检测物体的形状和轮廓，来确定目标的位置。
图像分割：因为可以对不同的区域进行目标的识别，所以也可以帮助其进行图像的分割。
医学领域：可以用来识别图像中的组织器官和患病部位，从而提取特征，进行医学的诊断。
启动：roslaunch opencv_apps contour_moments.launch，如下图：

6、LK光流

LK光流是描述目标运动的方法，利用LK光流可以实现对目标的追踪，从而知道目标的位姿。
LK光流法的前提条件如下：
亮度恒定：一个像素点随着时间的变化，其亮度值（像素灰度值）是不能变化的。
小运动：时间的变化不会引起位置的剧烈变化。这样才能利用相邻帧之间的位置变化引起的灰度值变化，去求取灰度对位置的偏导数。
空间一致：前一帧的相邻像素点在后一帧也是相邻的，因为为了求解x，y方向的速度，需要建立方程组，而空间一致假设就可以利用邻域n个像素点来建立n个方程。
启动：roslaunch opencv_apps lk_flow.launch，如下图：