2021SC@SDUSC

visual_feature_img_callback浅析

上一blog，我们分析了LVI-SAM中的其中一个回调函数lidar_callback。通过上一blog，我们已知了lidar_callback通过订阅了lidar的消息，并通过处理，获得了升读信息，然后会把带有深度信息的点图，放入到共享内存中去，供visual_feature中的另外一个回调函数使用。而这也是我们这一个blog需要解决的问题。

由第9篇blog，我们可以知道img_callback的作用：对新来的图像进行特征点的追踪。而这也是这个节点的主要任务。下面开始对这个回调函数进行浅析，之后的blog会对这个函数的每个细节进行研究。

文章目录

visual_feature_img_callback浅析
- 0. 初始化及正确性校验
- 1. 数据准备
- - a. 控制发布频率
  - b. 图像的格式调整和图像读取
- 2. 对最新帧特征点的提取和光流追踪(核心)
- 3. 对新加入的特征点更新全局id
- 4. 矫正、封装并发布特征点到pub_feature
- 5. 封装并发布到pub_match
- 附录：代码
- 附录：引用

0. 初始化及正确性校验

一开始，首先判断是不是第一帧，如果是，则把数据初始化，记录第一个图像帧的时间。

之后，需要判断相机的数据流是否稳定，如果不稳定，则需要发布restart的消息，让系统重启。这里判断数据流是否稳定的方法，是看每个图像帧到来的时间与上一帧图像的时间相差过大，或者是比上一帧图像的时间更老。并且，在发出restart之前，需要重置数据。

double cur_img_time = img_msg->header.stamp.toSec();

// 处理第一帧图像
if(first_image_flag)
{
    first_image_flag = false;
    first_image_time = cur_img_time;
    last_image_time = cur_img_time;
    return;
}
// 处理不稳定数据流
if (cur_img_time - last_image_time > 1.0 || cur_img_time < last_image_time)
{
    ROS_WARN("image discontinue! reset the feature tracker!");
    // 重置
    first_image_flag = true; 
    last_image_time = 0;
    pub_count = 1;
    std_msgs::Bool restart_flag;
    restart_flag.data = true;
    pub_restart.publish(restart_flag);
    return;
}

1. 数据准备

在准备中，我们需要控制发布频率，以及转换图像编码。

a. 控制发布频率

我们需要控制发布特征点的频率，并不是每读入一帧图像，就要发布特征点，因为读入图像帧的频率是很快的，为了避免ROS的通信机制负载过大，因此需要控制发布的频率。

if (round(1.0 * pub_count / (cur_img_time - first_image_time)) <= FREQ)
{
    PUB_THIS_FRAME = true;
    // reset the frequency control
    if (abs(1.0 * pub_count / (cur_img_time - first_image_time) - FREQ) < 0.01 * FREQ)
    {
        first_image_time = cur_img_time;
        pub_count = 0;
    }
}
else
{
    PUB_THIS_FRAME = false;
}

这其中有几个参数需要说明。

pub_count: # 发布图像的个数
FREQ: 20 # 控制图像光流跟踪的频率，这里作者在参数配置文件中设为20HZ
PUB_THIS_FRAME: # 是否需要发布特征点的标志

因此，整个逻辑控制块的意思是：累计发布数量 / 当前收到图片的时间距离设定的首张图片的时间为小于20HZ（可以调整），就允许发布。同时，如果收到的图片的时间间隔比较长，发布的频率低于2HZ（可以调整），则重置流量控制，将当前收到的世家设置为第一张图片的时间，并重置pub_count。

这样做，我猜测为了避免出现波动。比如，前面有较长的一段时间没有收到图片（可能是因为网络原因），然后，突然收到了大量的图片，这样后面发布图片会超出20HZ的频率，但平均整个频率却又没有超出20HZ。因此，作者这样的做法比较巧妙，可以避免较大的波动。

b. 图像的格式调整和图像读取

读取sensor_msgs::Image img的数据，并转为MONO8格式，用cv::Mat show_img接收。这里的8uc1是8bit的单色灰度图。而mono8就是一个8uc1的格式，我想这里应该是为了把图片从ROS的标准转换成OpenCV的标准，这样方便后续处理。

这里简单说下cv_bridge。cv_bridge 的toCVCopy函数将ROS图像消息转化为OpenCV图像。ROS以自己的sensor_msgs / Image消息格式传递图像，但许多用户希望将图像与OpenCV结合使用。 CvBridge是一个ROS库，提供ROS和OpenCV之间的接口。可以在vision_opencv stack的cv_bridge包中找到CvBridge。

cv_bridge::CvImageConstPtr ptr;
if (img_msg->encoding == "8UC1")
{
    sensor_msgs::Image img;
    img.header = img_msg->header;
    img.height = img_msg->height;
    img.width = img_msg->width;
    img.is_bigendian = img_msg->is_bigendian;
    img.step = img_msg->step;
    img.data = img_msg->data;
    img.encoding = "mono8";
    ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
    ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);

2. 对最新帧特征点的提取和光流追踪(核心)

img_callback()最最核心的语句出现在这里，也就是readImage()，这个函数实现了特征的处理和光流的追踪，里面基本上调用了feature_tracker.cpp里面的全部函数。在这里首先进行了单目和双目的判断。

如果是单目摄像头，就调用readImage()，这是核心。但我们后面blog再分析。如果是双目摄像头，需要自适应直方图均衡化处理。这里的EQUALIZE是如果光太亮或太暗则为1，用来进行直方图均衡化。

还有开头的两行，也是一些小细节，见下面的代码注释。

cv::Mat show_img = ptr->image;  //img_msg或img都是sensor_msg格式的，我们需要一个桥梁，转换为CV::Mat格式的数据，以供后续图像处理。
TicToc t_r; // 计算时间的类
for (int i = 0; i < NUM_OF_CAM; i++)
{
    ROS_DEBUG("processing camera %d", i);
    if (i != 1 || !STEREO_TRACK)
        trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), cur_img_time);
    else
    {
        if (EQUALIZE)
        {
            cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
            clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
        }
        else
            trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
    }

    #if SHOW_UNDISTORTION
    trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
    #endif
}

3. 对新加入的特征点更新全局id

completed(或者是update())如果是true，说明没有更新完id，则持续循环，如果是false，说明更新完了则跳出循环。注意n_id是static类型的数据，具有累加的功能。

for (unsigned int i = 0;; i++)
{
    bool completed = false;
    for (int j = 0; j < NUM_OF_CAM; j++)
        if (j != 1 || !STEREO_TRACK)
            completed |= trackerData[j].updateID(i);
    if (!completed)
        break;
}

4. 矫正、封装并发布特征点到pub_feature

这里的详细步骤介绍见代码注释。

将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img。

// 这里省略了最外层的if语句，详细见下面的附录代码。
pub_count++; //发布数量+1
// 用于封装发布的信息
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point; // 像素坐标x
sensor_msgs::ChannelFloat32 v_of_point; // 像素坐标y
sensor_msgs::ChannelFloat32 velocity_x_of_point; // 速度x
sensor_msgs::ChannelFloat32 velocity_y_of_point; // 速度y

feature_points->header.stamp = img_msg->header.stamp;
feature_points->header.frame_id = "vins_body";

vector<set<int>> hash_ids(NUM_OF_CAM); // 哈希表id
for (int i = 0; i < NUM_OF_CAM; i++) // 循环相机数量
{
    auto &un_pts = trackerData[i].cur_un_pts; 
    auto &cur_pts = trackerData[i].cur_pts;
    auto &ids = trackerData[i].ids;
    auto &pts_velocity = trackerData[i].pts_velocity;
    for (unsigned int j = 0; j < ids.size(); j++) // 特征点的数量
    {
        if (trackerData[i].track_cnt[j] > 1) // 只发布追踪次数大于1的特征点
        {
            int p_id = ids[j];
            hash_ids[i].insert(p_id);
            geometry_msgs::Point32 p;
            p.x = un_pts[j].x;
            p.y = un_pts[j].y;
            p.z = 1;

            feature_points->points.push_back(p);
            id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
            u_of_point.values.push_back(cur_pts[j].x);
            v_of_point.values.push_back(cur_pts[j].y);
            velocity_x_of_point.values.push_back(pts_velocity[j].x);
            velocity_y_of_point.values.push_back(pts_velocity[j].y);
        }
    }
}

// 封装信息，准备发布
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);

// 从共享内存中获得深度信息
pcl::PointCloud<PointType>::Ptr depth_cloud_temp(new pcl::PointCloud<PointType>());
mtx_lidar.lock();
*depth_cloud_temp = *depthCloud;
mtx_lidar.unlock();

// 获取深度
sensor_msgs::ChannelFloat32 depth_of_points = depthRegister->get_depth(img_msg->header.stamp, show_img, depth_cloud_temp, trackerData[0].m_camera, feature_points->points);
feature_points->channels.push_back(depth_of_points);

// 第一帧不发布，因为没有光流速度。
if (!init_pub)
{
    init_pub = 1;
}
else
    pub_feature.publish(feature_points);

5. 封装并发布到pub_match

将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match。

// 在图像中发布特征
if (pub_match.getNumSubscribers() != 0)
{
    ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::RGB8);
    //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
    cv::Mat stereo_img = ptr->image;

    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
        cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB); // show_img灰度图转RGB（tmp_img）

        //显示追踪状态，越红越好，越蓝越不行---cv::Scalar决定的
        for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
        {
            if (SHOW_TRACK)
            {
                // 计算跟踪的特征点数量
                double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
                cv::circle(tmp_img, trackerData[i].cur_pts[j], 4, cv::Scalar(255 * (1 - len), 255 * len, 0), 4);
            } else {
                // 结合深度进行计算
                if(j < depth_of_points.values.size())
                {
                    if (depth_of_points.values[j] > 0)
                        cv::circle(tmp_img, trackerData[i].cur_pts[j], 4, cv::Scalar(0, 255, 0), 4);
                    else
                        cv::circle(tmp_img, trackerData[i].cur_pts[j], 4, cv::Scalar(0, 0, 255), 4);
                }
            }
        }
    }

    pub_match.publish(ptr->toImageMsg());
}

附录：代码

void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    double cur_img_time = img_msg->header.stamp.toSec();

    if(first_image_flag)
    {
        first_image_flag = false;
        first_image_time = cur_img_time;
        last_image_time = cur_img_time;
        return;
    }
    // detect unstable camera stream
    if (cur_img_time - last_image_time > 1.0 || cur_img_time < last_image_time)
    {
        ROS_WARN("image discontinue! reset the feature tracker!");
        first_image_flag = true; 
        last_image_time = 0;
        pub_count = 1;
        std_msgs::Bool restart_flag;
        restart_flag.data = true;
        pub_restart.publish(restart_flag);
        return;
    }
    last_image_time = cur_img_time;
    // frequency control
    if (round(1.0 * pub_count / (cur_img_time - first_image_time)) <= FREQ)
    {
        PUB_THIS_FRAME = true;
        // reset the frequency control
        if (abs(1.0 * pub_count / (cur_img_time - first_image_time) - FREQ) < 0.01 * FREQ)
        {
            first_image_time = cur_img_time;
            pub_count = 0;
        }
    }
    else
    {
        PUB_THIS_FRAME = false;
    }

    cv_bridge::CvImageConstPtr ptr;
    if (img_msg->encoding == "8UC1")
    {
        sensor_msgs::Image img;
        img.header = img_msg->header;
        img.height = img_msg->height;
        img.width = img_msg->width;
        img.is_bigendian = img_msg->is_bigendian;
        img.step = img_msg->step;
        img.data = img_msg->data;
        img.encoding = "mono8";
        ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
    }
    else
        ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);

    cv::Mat show_img = ptr->image;
    TicToc t_r;
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ROS_DEBUG("processing camera %d", i);
        if (i != 1 || !STEREO_TRACK)
            trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), cur_img_time);
        else
        {
            if (EQUALIZE)
            {
                cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
                clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
            }
            else
                trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
        }

        #if SHOW_UNDISTORTION
            trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
        #endif
    }

    for (unsigned int i = 0;; i++)
    {
        bool completed = false;
        for (int j = 0; j < NUM_OF_CAM; j++)
            if (j != 1 || !STEREO_TRACK)
                completed |= trackerData[j].updateID(i);
        if (!completed)
            break;
    }

   if (PUB_THIS_FRAME)
   {
        pub_count++;
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
        sensor_msgs::ChannelFloat32 id_of_point;
        sensor_msgs::ChannelFloat32 u_of_point;
        sensor_msgs::ChannelFloat32 v_of_point;
        sensor_msgs::ChannelFloat32 velocity_x_of_point;
        sensor_msgs::ChannelFloat32 velocity_y_of_point;

        feature_points->header.stamp = img_msg->header.stamp;
        feature_points->header.frame_id = "vins_body";

        vector<set<int>> hash_ids(NUM_OF_CAM);
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            auto &un_pts = trackerData[i].cur_un_pts;
            auto &cur_pts = trackerData[i].cur_pts;
            auto &ids = trackerData[i].ids;
            auto &pts_velocity = trackerData[i].pts_velocity;
            for (unsigned int j = 0; j < ids.size(); j++)
            {
                if (trackerData[i].track_cnt[j] > 1)
                {
                    int p_id = ids[j];
                    hash_ids[i].insert(p_id);
                    geometry_msgs::Point32 p;
                    p.x = un_pts[j].x;
                    p.y = un_pts[j].y;
                    p.z = 1;

                    feature_points->points.push_back(p);
                    id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
                    u_of_point.values.push_back(cur_pts[j].x);
                    v_of_point.values.push_back(cur_pts[j].y);
                    velocity_x_of_point.values.push_back(pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(pts_velocity[j].y);
                }
            }
        }

        feature_points->channels.push_back(id_of_point);
        feature_points->channels.push_back(u_of_point);
        feature_points->channels.push_back(v_of_point);
        feature_points->channels.push_back(velocity_x_of_point);
        feature_points->channels.push_back(velocity_y_of_point);

        // get feature depth from lidar point cloud
        pcl::PointCloud<PointType>::Ptr depth_cloud_temp(new pcl::PointCloud<PointType>());
        mtx_lidar.lock();
        *depth_cloud_temp = *depthCloud;
        mtx_lidar.unlock();

        sensor_msgs::ChannelFloat32 depth_of_points = depthRegister->get_depth(img_msg->header.stamp, show_img, depth_cloud_temp, trackerData[0].m_camera, feature_points->points);
        feature_points->channels.push_back(depth_of_points);
        
        // skip the first image; since no optical speed on frist image
        if (!init_pub)
        {
            init_pub = 1;
        }
        else
            pub_feature.publish(feature_points);

        // publish features in image
        if (pub_match.getNumSubscribers() != 0)
        {
            ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::RGB8);
            //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
            cv::Mat stereo_img = ptr->image;

            for (int i = 0; i < NUM_OF_CAM; i++)
            {
                cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
                cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);

                for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
                {
                    if (SHOW_TRACK)
                    {
                        // track count
                        double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
                        cv::circle(tmp_img, trackerData[i].cur_pts[j], 4, cv::Scalar(255 * (1 - len), 255 * len, 0), 4);
                    } else {
                        // depth 
                        if(j < depth_of_points.values.size())
                        {
                            if (depth_of_points.values[j] > 0)
                                cv::circle(tmp_img, trackerData[i].cur_pts[j], 4, cv::Scalar(0, 255, 0), 4);
                            else
                                cv::circle(tmp_img, trackerData[i].cur_pts[j], 4, cv::Scalar(0, 0, 255), 4);
                        }
                    }
                }
            }

            pub_match.publish(ptr->toImageMsg());
        }
    }
}

附录：引用

ManiiXu/VINS-Mono-Learning: VINS-Mono

VINS-Mono 代码解析一、前端_努力努力努力-程序员资料_mono8 - 程序员资料

11-[LVI-SAM]visual_feature_img_callback浅析