激光雷达与相机融合（六）-------目标检测与跟踪ros实时版

在本部分中，将之前的优达学城的整套目标检测与跟踪算法改写为ros实时处理，但改写完成后利用我现有的数据包实时检测跟踪，并计算TTC，发现效果不尽人意啊。。。算法的鲁棒性整体较差，环境稍微复杂一点计算的碰撞时间就有问题，甚至为负值。另外在实际运行时，还会时不时出现core dumped内存泄漏的问题，不知道是哪里写的有问题，有同学发现错误欢迎留言交流，万分感谢。
大概讲解一下整套算法结构。

1.整体框架

这一部分的代码被我改写为一个ros package的形式，包名为ros_detection_tracking,包下有两个节点-----ros_ttc和project，project节点在上一个博客中已经详细解释了。在这个包中Cmakelist.txt文件中project的部分被我注释了，需要运行的朋友可以自行找到下列几行取消注释，编译就可以运行project节点了。

#add_executable(projector src/project/project.cpp)
#target_link_libraries(projector ${catkin_LIBRARIES}
#                                ${
   
   OpenCV_LIBS})
#add_dependencies(projector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

主要的头文件存放在目录include下，源程序存放在src/ros_ttc目录下，配置文件放置在cfg目录下，YOLO的相关配置和权重文件放置在dat/yolo目录下。

2.主要函数

写成了一个节点实现，方法简单粗暴，该节点的主体加上构造函数就只有三个函数。
分别是主要的回调函数、参数加载函数、以及初始化构造函数。

 void detectandtrack_callback(const sensor_msgs::Image::ConstPtr &img, 
                                const sensor_msgs::PointCloud2::ConstPtr &pc);
 void initParams();
 DetectandTrack();

当然，绝大部分的调用到的函数都在其余的源文件中。
这里就大概贴一下主要的回调函数，这个函数包含了整个目标检测跟踪的整体流程。包括数据加载以及点云分割、利用YOLO进行目标检测、基于boundingbox对点云聚类、基于图像的特征点提取、描述、匹配并跟踪目标，最后基于点云计算碰撞时间。
由于博主水平有限，改写的部分有误的还望批评指正，欢迎交流。
void detectandtrack_callback(const sensor_msgs::Image::ConstPtr &img,
const sensor_msgs::PointCloud2::ConstPtr &pc);

void DetectandTrack::detectandtrack_callback(const sensor_msgs::Image::ConstPtr &img, 
                                const sensor_msgs::PointCloud2::ConstPtr &pc)
{
   
   
    /*1.construct data Frame and crop*/
    cv_bridge::CvImagePtr cv_ptr;
     try {
   
   
            cv_ptr = cv_bridge::toCvCopy(img, "bgr8");
        }
    catch (cv_bridge::Exception &e) {
   
   
        return;
    }
    cv::Mat raw_img = cv_ptr->image;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*pc, *cloud);
    
    cv::Mat visImg = raw_img.clone();
    cv::Mat overlay = visImg.clone();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
    cloudA = cloud;
    DataFrame frame;
    frame.cameraImg = raw_img;

    cropLidarPoints(cloudA, i_params.crop_minX, i_params.crop_maxX, i_params.crop_Y, i_params.crop_minZ, i_params.crop_maxZ);//crop pointcloud
    
    //frame.pointcloud = *cloudA;
  
    cout<<"/*1.construct data Frame and crop*/ done"<<endl;

   /*2.detection and classification*/
   detectObjects(frame.cameraImg, frame.boundingBoxes, confThreshold, nmsThreshold, i_params.yoloBasePath, i_params.yoloClassesFile,
                    i_params.yoloModelConfiguration, i_params.yoloModelWeights, bVis);

    cout<<"/*2.detection and classification*/ has done"<<endl;

    /*3.cluster the point with the bounding box*/

    clusterLidarWithROI(frame.boundingBoxes, cloudA, shrinkFactor, i_params.cameraIn, i_params.camtocam_mat, i_params.RT);
    cout<<" /*3.cluster the point with the bounding box*/ done" <<endl;

     /*4.detect keypoint*/
     cv::cvtColor(frame.cameraImg, imgGray