vision_beyond_track代码解析

文件结构目录

# bokaichen @ bokaideMacBook-Pro in ~/Desktop/vision_beyond_track [11:14:36]
$ tree
.
├── CHANGELOG.rst
├── CMakeLists.txt                              # cmake文件
├── README.md
├── include
│   ├── detection.h                             # .h文件
│   └── vision_beyond_track.h
├── launch
│   └── vision_beyond_track.launch              # 启动文件
├── lib
│   ├── clipper.cpp                             # 两个工具类
│   ├── clipper.hpp
│   ├── hungarian.cpp
│   └── hungarian.h
├── package.xml
└── src
    ├── gencolors.cpp                           # 色彩生成文件
    ├── vision_beyond_track.cpp                 # track类实现
    └── vision_beyond_track_node.cpp            # 节点main函数

4 directories, 14 files

CMakeLists中

find_package中需要"

OpenCV的依赖
autoware_build_flags的依赖
ros组件中包括了std_msgs,tf,cv_bridge

add_library 生成beyond_track_lib STATIC 依赖于cliper和hu

STATIC、SHARED和MODULE STATIC、SHARED和MODULE的作用是指定生成的库文件的类型。

STATIC库是目标文件的归档文件，在链接其它目标的时候使用。
SHARED库会被动态链接（动态链接库），在运行时会被加载。
MODULE库是一种不会被链接到其它目标中的插件，但是可能会在运行时使用dlopen-系列的函数。

add_library(<name> [STATIC | SHARED | MODULE]
            [EXCLUDE_FROM_ALL]
            [source1] [source2] [...])

安装路径CATKIN_PACKAGE_LIB_DESTINATION

install(TARGETS MyLib
        EXPORT MyLibTargets 
        LIBRARY DESTINATION lib  # 动态库安装路径
        ARCHIVE DESTINATION lib  # 静态库安装路径
        RUNTIME DESTINATION bin  # 可执行文件安装路径
        PUBLIC_HEADER DESTINATION include  # 头文件安装路径
        )

代码流程

main函数在vision_beyond_track_node.cpp中:

实例化 BeyondTrackerNode 然后运行Run方法

BeyondTrackerNode的定义在vision_beyond_track.h中: 构建类时,定义了四个订阅者: ros::Subscriber rect_image_subscriber_;
ros::Subscriber intrinsics_subscriber_; ros::Subscriber detections_vision_subscriber_; ros::Subscriber ego_motion_subscriber_;

一个发布者: objects_publisher_ 一个句柄: ros::NodeHandle node_handle_;

构建一个tracker_实例定义也在vision_beyond_track.h中 beyondtrack::BeyondTracker tracker_;(后面写)

image_top_bottom_border_和image_left_right_border_的作用: 在输入图像中添加黑条以保持宽高比，同时调整其大小以适合网络输入大小

接下来定义了相机相关的信息:

图像的尺寸
相机的内参及相机的状态
是否运动
姿态矩阵
地面夹角
安装高度

四个函数声明:

parse_detected_object
vision_detection_callback //检测回调函数
intrinsics_callback //内参回调函数
detection_to_objects //目标回调函数其中parse_detected_object是获取Detection的信息返回值是Detection的vector

public: 一个Run的空方法,具体的实现在vision_beyond_track.cpp中

BeyondTracker的定义也在vision_beyond_track.h中:

全局ID 初始化标志位当前检测结果:std::vector cur_detections_; 上次检测结果:std::vector prev_detections_; 当前位姿之前位姿

四个相机参数

四个函数声明:

initialize
create_cuboid
propagate_detections
generate_score_matrices

public: 五个函数成员:

process
get_results
set_intrinsic
get_3d2d_score
get_3d3d_score

vision_beyond_track.cpp内容解析:

BeyondTrackerNode::Run

  ros::NodeHandle private_node_handle("~");//to receive args 
  //获取句柄
  std::string image_topic_src, camera_info_src, objects_topic_src;
  //节点名
  private_node_handle.param<std::string>("camera_info_src", camera_info_src, "/camera_info");
  private_node_handle.param<std::string>("objects_topic_src", objects_topic_src,"/detection/image_detector/objects");
  //节点名赋值
  ROS_INFO("[%s] Subscribing to... %s", __APP_NAME__, camera_info_src.c_str());
  //打印
  intrinsics_subscriber_ = private_node_handle.subscribe(
    camera_info_src, 1, &BeyondTrackerNode::intrinsics_callback, this);
  //设置订阅节点
  ROS_INFO("[%s] Subscribing to... %s", __APP_NAME__, 
  objects_topic_src.c_str());
  detections_vision_subscriber_ = private_node_handle.subscribe(objects_topic_src,1, &BeyondTrackerNode::vision_detection_callback,this);
  //视觉检测回调函数
  private_node_handle.param<double>("camera_height", camera_height_, 1.2);
  ROS_INFO("[%s] camera height: %f", __APP_NAME__, camera_height_);
  //获取相机高度
  objects_publisher_ = node_handle_.advertise<autoware_msgs::DetectedObjectArray>("/detection/image_tracker/objects",1);
  //发送publisher消息

#if (CV_MAJOR_VERSION <= 2)
  cv::generateColors(colors_, 20);
#else
  generateColors(colors_, 20);
#endif
  //生成颜色
  ground_angle_ = cv::Mat::zeros(1, 3, CV_64FC1);
  ground_angle_.at<double>(0, 1) = 1;

  tracker_ = beyondtrack::BeyondTracker();
  //通过重新初始化一个tracker,来完成tracker的清零
  ros::spin();

  ROS_INFO("END beyond_tracker");

Run方法中产生了两个订阅回调函数 intrinsics_callback 用于获取参数参数的获取来自CameraInfo的信息

std_msgs/Header header
uint32 height
uint32 width
string distortion_model
float64[] D
float64[9] K
float64[9] R
float64[12] P
uint32 binning_x
uint32 binning_y
sensor_msgs/RegionOfInterest roi

http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/CameraInfo.html

将订阅的相机参数传递给tracker_后关闭

vision_detection_callback 用于处理检测迭代回调函数中的流程:

确保相机状态正常,是否使用运动补偿(pose_)
创建一个detections利用parse_detected_object接收消息DetectedObjectArray
detections信息有x,y,width,height,label
最关键的一步是process tracker_.process的实现在void BeyondTracker::process(std::vector<Detection> in_detections, cv::Mat in_pose, cv::Mat in_angle, double in_height):

读取detections是否为空,决定是否初始化initialized_ 如果不为空,执行初始化void BeyondTracker::initialize(cv::Mat in_angle, double in_height): 执行后直接返回主要作用:

初始化中主要完成global_id的发放
prev_detections_和prev_pose状态的传递这样的好处是,再次进来后prev_*不会为空

然后修正运动

传播迭代生成权重矩阵建立匈牙利匹配

https://github.com/streetdrone-home/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/DetectedObject.msg

vision_segment_enet_detect代码解析

.
├── CHANGELOG.rst
├── CMakeLists.txt
├── README.md
├── include
│   └── vision_segment_enet_detect.h
├── launch
│   └── vision_segment_enet_detect.launch
├── nodes
│   └── vision_segment_enet_detect
│       ├── vision_segment_enet_detect.cpp
│       └── vision_segment_enet_detect_node.cpp
└── package.xml

4 directories, 8 files

CMakeLists.txt

设置里caffe-enet的地址 set(ENET_CAFFE_PATH "$ENV{HOME}/ENet/caffe-enet/distribute")

AW_CHECK_CUDA() 是对cuda和cudann的设置

其余无特殊之处

vision_segment_enet_detect_node.cpp

实例化 ROSENetSegmenterApp 执行Run方法

ROSENetSegmenterApp中的成员变量:

订阅者 subscriber_image_raw_ ros句柄 node_handle_ 发布者1 publisher_image_segmented_ 发布者2 publisher_image_segmented_blended_ ENetSegmenter* enet_segmenter_; 创建ENetSegmenter的类对象指针 enet_segmenter_

成员函数: 1.订阅的回调函数 2.Run函数

Run方法

  void Run()
  {
    ros::NodeHandle private_node_handle("~");//to receive args
    //读取句柄参数的topic,默认为image_raw
    std::string image_raw_topic_str;
    if (private_node_handle.getParam("image_src", image_raw_topic_str))
    {
      ROS_INFO("Setting image node to %s", image_raw_topic_str.c_str());
    }
    else
    {
      ROS_INFO("No image node received, defaulting to /image_raw, you can use _image_src:=YOUR_TOPIC");
      image_raw_topic_str = "/image_raw";
    }
    //获取prototxt文件/权重文件weights/lookuptable_file

    std::string network_definition_file;
    std::string pretrained_model_file;
    std::string lookuptable_file;
    if (private_node_handle.getParam("network_definition_file", network_definition_file))
    {
      ROS_INFO("Network Definition File: %s", network_definition_file.c_str());
    }
    else
    {
      ROS_INFO("No Network Definition File was received. Finishing execution.");
      return;
    }
    if (private_node_handle.getParam("pretrained_model_file", pretrained_model_file))
    {
      ROS_INFO("Pretrained Model File: %s", pretrained_model_file.c_str());
    }
    else
    {
      ROS_INFO("No Pretrained Model File was received. Finishing execution.");
      return;
    }
    if (private_node_handle.getParam("lookuptable_file", lookuptable_file))
    {
      ROS_INFO("lookuptable File: %s", lookuptable_file.c_str());
    }
    else
    {
      ROS_INFO("No lookuptable File was received. Finishing execution.");
      return;
    }
    //ENetSegmenter建立检测器
    enet_segmenter_ = new ENetSegmenter(network_definition_file,
                        pretrained_model_file,
                        lookuptable_file);

    if (NULL == enet_segmenter_)
    {
      ROS_INFO("Error while creating ENetSegmenter Object");
      return;
    }
    ROS_INFO("ENetSegmenter initialized.");
    //订阅器回调
    ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str());
    subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &ROSENetSegmenterApp::image_callback, this);
    //发送器 发送Image
    publisher_image_segmented_ = node_handle_.advertise<sensor_msgs::Image>("/image_segmented", 1);
    ROS_INFO("Publishing /image_segmented");
    publisher_image_segmented_blended_ = node_handle_.advertise<sensor_msgs::Image>("/image_segmented_blended", 1);
    ROS_INFO("Publishing /image_segmented_blended");

    ROS_INFO("Waiting for data...");
    ros::spin();
    ROS_INFO("END ENetSegmenter");
  }

in_lookuptable_file文件的作用用来分配不同label对应的颜色

image_callback回调函数

创建cv_image 创建两个Mat segmented_mat和blended_mat 利用enet_segmenter_的Predict方法传递segmented_mat

ENetSegmenter的实现: 函数成员:

ENetSegmenter 初始化caffe参数 load
Predict 识别过程
SetMean 图像均值化
WrapInputLayer 读取图片,保存在input_data中
Preprocess 调换通道和尺寸
Visualization 对图片进行渲染变量成员:
std::shared_ptr<caffe::Net > net_;
cv::Size input_geometry_;
int num_channels_;
std::string lookuptable_file_;
cv::Scalar pixel_mean_;

Autoware_vision