11-[LVI-SAM]visual_feature_img_callback浅析

最新推荐文章于 2023-12-06 17:19:57 发布
最新推荐文章于 2023-12-06 17:19:57 发布
阅读量2.9k 收藏 4
点赞数 2
分类专栏： LVI-SAM分析 文章标签： c++ 自动驾驶 人工智能
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/loyer_kong/article/details/121594053
版权

2021SC@SDUSC

visual_feature_img_callback浅析

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

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

文章目录

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);

get_depth函数（核心）

  1. 初始化需要返回的深度信息

    sensor_msgs::ChannelFloat32 depth_of_point;
depth_of_point.name = "depth";
depth_of_point.values.resize(features_2d.size(), -1);

  2. 检查点云图是否可用

    if (depthCloud->size() == 0)
    return depth_of_point;

  3. 在当前图像时间查找tf变换

    try{
    listener.waitForTransform("vins_world", "vins_body_ros", stamp_cur, ros::Duration(0.01));
    listener.lookupTransform("vins_world", "vins_body_ros", stamp_cur, transform);
} 
catch (tf::TransformException ex){
    // ROS_ERROR("image no tf");
    return depth_of_point;
}

double xCur, yCur, zCur, rollCur, pitchCur, yawCur;
xCur = transform.getOrigin().x();
yCur = transform.getOrigin().y();
zCur = transform.getOrigin().z();
tf::Matrix3x3 m(transform.getRotation());
m.getRPY(rollCur, pitchCur, yawCur);
Eigen::Affine3f transNow = pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur);

  4. 将点云图从世界坐标系转换到当前相机坐标系

    pcl::PointCloud<PointType>::Ptr depth_cloud_local(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*depthCloud, *depth_cloud_local, transNow.inverse());

  5. 将未失真的归一化 (z) 2d 特征投影到单位球体上

    首先，归一化2d特征到单位球体上，然后转换到ROS的标准中。

    pcl::PointCloud<PointType>::Ptr features_3d_sphere(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)features_2d.size(); ++i)
{
    // 归一化2d特征到单位球体上
    Eigen::Vector3f feature_cur(features_2d[i].x, features_2d[i].y, features_2d[i].z); // z always equal to 1
    feature_cur.normalize(); 
    // 转换为ROS的标准
    PointType p;
    p.x =  feature_cur(2);
    p.y = -feature_cur(0);
    p.z = -feature_cur(1);
    p.intensity = -1; // intensity 会被用来保存深度信息。
    features_3d_sphere->push_back(p);
}

  6. 投影深度点云图在距离图像上并过滤位于同一区域的点

    首先，过滤不在相机视野中的点，然后，在图像的范围中找到行号，在图像的范围中找到列号。但是找到的序列号（包括行和列）可能超过限制范围，需要过滤。最后，只保留较近的点。

    float bin_res = 180.0 / (float)num_bins; // 只能覆盖lidar前面的 (-90 ~ 90) 区域
cv::Mat rangeImage = cv::Mat(num_bins, num_bins, CV_32F, cv::Scalar::all(FLT_MAX));

for (int i = 0; i < (int)depth_cloud_local->size(); ++i)
{
    PointType p = depth_cloud_local->points[i];
    // 过滤不在相机视野中的点
    if (p.x < 0 || abs(p.y / p.x) > 10 || abs(p.z / p.x) > 10)
        continue;
    // 在图像的范围中找到行号
    float row_angle = atan2(p.z, sqrt(p.x * p.x + p.y * p.y)) * 180.0 / M_PI + 90.0; // degrees, bottom -> up, 0 -> 360
    int row_id = round(row_angle / bin_res);
    // 在图像的范围中找到列号
    float col_angle = atan2(p.x, p.y) * 180.0 / M_PI; // degrees, left -> right, 0 -> 360
    int col_id = round(col_angle / bin_res);
    // id 可能超出约束条件
    if (row_id < 0 || row_id >= num_bins || col_id < 0 || col_id >= num_bins)
        continue;
    // 只保留较近的点
    float dist = pointDistance(p);
    if (dist < rangeImage.at<float>(row_id, col_id))
    {
        rangeImage.at<float>(row_id, col_id) = dist;
        pointsArray[row_id][col_id] = p;
    }
}

  7. 从距离图像中提取下采样深度点云图

    pcl::PointCloud<PointType>::Ptr depth_cloud_local_filter2(new pcl::PointCloud<PointType>());
for (int i = 0; i < num_bins; ++i)
{
    for (int j = 0; j < num_bins; ++j)
    {
        if (rangeImage.at<float>(i, j) != FLT_MAX)
            depth_cloud_local_filter2->push_back(pointsArray[i][j]);
    }
}
*depth_cloud_local = *depth_cloud_local_filter2;
publishCloud(&pub_depth_cloud, depth_cloud_local, stamp_cur, "vins_body_ros");

  8. 将深度点云图投影到单位球体上

    pcl::PointCloud<PointType>::Ptr depth_cloud_unit_sphere(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)depth_cloud_local->size(); ++i)
{
    PointType p = depth_cloud_local->points[i];
    float range = pointDistance(p);
    p.x /= range;
    p.y /= range;
    p.z /= range;
    p.intensity = range;
    depth_cloud_unit_sphere->push_back(p);
}
if (depth_cloud_unit_sphere->size() < 10)
    return depth_of_point;

  9. 使用球形深度点云创建 kd 树

    pcl::KdTreeFLANN<PointType>::Ptr kdtree(new pcl::KdTreeFLANN<PointType>());
kdtree->setInputCloud(depth_cloud_unit_sphere);

    k-d树是每个叶子节点都为k维点的二叉树。所有非叶子节点可以视作用一个超平面把空间分割成两个半空间。节点左边的子树代表在超平面左边的点，节点右边的子树代表在超平面右边的点。选择超平面的方法如下：每个节点都与k维中垂直于超平面的那一维有关。因此，如果选择按照x轴划分，所有x值小于指定值的节点都会出现在左子树，所有x值大于指定值的节点都会出现在右子树。这样，超平面可以用该x值来确定，其法线为x轴的单位向量。

  10. 使用 kd-tree 找到特征深度，并返回结果
    详细分析见下面代码注释。

    vector<int> pointSearchInd;
vector<float> pointSearchSqDis;
float dist_sq_threshold = pow(sin(bin_res / 180.0 * M_PI) * 5.0, 2);
for (int i = 0; i < (int)features_3d_sphere->size(); ++i)
{
    kdtree->nearestKSearch(features_3d_sphere->points[i], 3, pointSearchInd, pointSearchSqDis);
    if (pointSearchInd.size() == 3 && pointSearchSqDis[2] < dist_sq_threshold)
    {
        float r1 = depth_cloud_unit_sphere->points[pointSearchInd[0]].intensity;
        Eigen::Vector3f A(depth_cloud_unit_sphere->points[pointSearchInd[0]].x * r1,
                          depth_cloud_unit_sphere->points[pointSearchInd[0]].y * r1,
                          depth_cloud_unit_sphere->points[pointSearchInd[0]].z * r1);

        float r2 = depth_cloud_unit_sphere->points[pointSearchInd[1]].intensity;
        Eigen::Vector3f B(depth_cloud_unit_sphere->points[pointSearchInd[1]].x * r2,
                          depth_cloud_unit_sphere->points[pointSearchInd[1]].y * r2,
                          depth_cloud_unit_sphere->points[pointSearchInd[1]].z * r2);

        float r3 = depth_cloud_unit_sphere->points[pointSearchInd[2]].intensity;
        Eigen::Vector3f C(depth_cloud_unit_sphere->points[pointSearchInd[2]].x * r3,
                          depth_cloud_unit_sphere->points[pointSearchInd[2]].y * r3,
                          depth_cloud_unit_sphere->points[pointSearchInd[2]].z * r3);

        // https://math.stackexchange.com/questions/100439/determine-where-a-vector-will-intersect-a-plane
        Eigen::Vector3f V(features_3d_sphere->points[i].x,
                          features_3d_sphere->points[i].y,
                          features_3d_sphere->points[i].z);

        Eigen::Vector3f N = (A - B).cross(B - C);
        float s = (N(0) * A(0) + N(1) * A(1) + N(2) * A(2)) 
            / (N(0) * V(0) + N(1) * V(1) + N(2) * V(2));

        float min_depth = min(r1, min(r2, r3));
        float max_depth = max(r1, max(r2, r3));
        if (max_depth - min_depth > 2 || s <= 0.5)
        {
            continue;
        } else if (s - max_depth > 0) {
            s = max_depth;
        } else if (s - min_depth < 0) {
            s = min_depth;
        }
        // 如果深度可用，则将特征转换为笛卡尔空间
        features_3d_sphere->points[i].x *= s;
        features_3d_sphere->points[i].y *= s;
        features_3d_sphere->points[i].z *= s;
        // 这里获得的深度是针对单位球体的，VINS 估计器需要归一化特征的深度（按 z 值），（激光雷达 x = 相机 z）
        features_3d_sphere->points[i].intensity = features_3d_sphere->points[i].x;
    }
}

// 可视化笛卡尔 3d 空间中的特征（包括没有深度的特征（默认 1））
publishCloud(&pub_depth_feature, features_3d_sphere, stamp_cur, "vins_body_ros");

// 更新返回的深度值
for (int i = 0; i < (int)features_3d_sphere->size(); ++i)
{
    if (features_3d_sphere->points[i].intensity > 3.0)
        depth_of_point.values[i] = features_3d_sphere->points[i].intensity;
}

// 可视化项目点在图像上进行可视化（这里处理较慢！）
if (pub_depth_image.getNumSubscribers() != 0)
{
    vector<cv::Point2f> points_2d;
    vector<float> points_distance;

    for (int i = 0; i < (int)depth_cloud_local->size(); ++i)
    {
        // 将点从 3D 转换为 2D
        Eigen::Vector3d p_3d(-depth_cloud_local->points[i].y,
                             -depth_cloud_local->points[i].z,
                             depth_cloud_local->points[i].x);
        Eigen::Vector2d p_2d;
        camera_model->spaceToPlane(p_3d, p_2d);

        points_2d.push_back(cv::Point2f(p_2d(0), p_2d(1)));
        points_distance.push_back(pointDistance(depth_cloud_local->points[i]));
    }

    cv::Mat showImage, circleImage;
    cv::cvtColor(imageCur, showImage, cv::COLOR_GRAY2RGB);
    circleImage = showImage.clone();
    for (int i = 0; i < (int)points_2d.size(); ++i)
    {
        float r, g, b;
        getColor(points_distance[i], 50.0, r, g, b);
        cv::circle(circleImage, points_2d[i], 0, cv::Scalar(r, g, b), 5);
    }
    cv::addWeighted(showImage, 1.0, circleImage, 0.7, 0, showImage); // blend camera image and circle image

    cv_bridge::CvImage bridge;
    bridge.image = showImage;
    bridge.encoding = "rgb8";
    sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
    imageShowPointer->header.stamp = stamp_cur;
    pub_depth_image.publish(imageShowPointer);
}

return depth_of_point;

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 - 程序员资料

loyer_kong
关注
  • 2
    点赞
  • 踩
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
matlab终止以下代码-QP_solver-LVI_PDNN-FPGA_Implementation:解决二次规划问题的基于LVI的双对偶神
05-21
何昊天10月、11月工作汇报 论文题目：求解二次规划问题的基于LVI的原-对偶神经网络FPGA设计和实现 论文作者：袁银娟 论文链接： 基于LVI（Linear Variational Inequalities）的原-对偶神经网络（Primal-Dual Neural ...
LVI-SAM论文！！！！！！！！！
04-13
LVI-SAM（Lidar-Visual-Inertial Odometry via Smoothing and Mapping）是一种紧耦合的激光雷达-视觉-惯性定位与建图算法，该算法能够实时估计状态和构建高精度的环境图。LVI-SAM 系统由两个子系统组成：视觉惯性...
从零学习VINS-Mono/Fusion源代码（二）：前端图像跟踪
slender_1031的博客
10-11 1830
本节学习VINS-Mono前端对图像数据的处理过程，解读回调函数img_callback()中的操作。
vins_mono的feature_tracker线程
m0_46345373的博客
12-06 197
//新的一帧图像传入ros的回调函数中 void img_callback(const sensor_msgs::ImageConstPtr &img_msg) { 1、判断是否是第一帧 if(first_image_flag) 2、通过时间间隔判断相机数据流是否稳定，有问题则restart if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() &
【Vins-mono一】vins-mono的前端特征提取
a_happy_bird的博客
06-02 1375
vins-mono的前端特征提取
vins-mono--视觉跟踪--feature_trackers
qq_42739167的博客
05-11 607
VINS的视觉处理前端的视觉跟踪模块（feature_trackers）。 论文第四章A节（IV. MEASUREMENT PREPROCESSING——A. Vision Processing Front-end） 可以看到其算法主要包括以下内容： 1、对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪； 2、检测新的角点特征以保证每个图像特征的最小数目(100-300)； 3、通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布； 4、利用基本矩阵模型的RANSAC算法进行外点剔除；(跟踪失败
10-visual_feature_lidar_callback
loyer_kong的博客
11-20 893
​ 经过上一篇blog——VINS-Mono的分析，我们现如今对LVI-SAM的visual_feature有了初步的了解。现在，开始对visual_feature的两个重要的回调函数开始分析。 ​ 由上一篇blog，我们已知LVI-SAM利用lidar的相关信息获取到了图像中现实的深度信息，并且把信息进行处理作为图像信息的初始化数据。因此，这一篇blog我们先来研究lidar的回调函数。 ​ 整个过程分为了9个步骤。
LVI-SAM-master.zip
03-16
4. LVI-SAM：Local Visual-Inertial Sensor Fusion with Smoothness Analysis and Marginalization（局部视觉惯性传感器融合与平滑分析和消元）是结合了视觉、IMU和激光雷达的SLAM算法。它通过联合优化视觉、IMU和...
apollo 定位localization代码NDT算法模块解析(一)
li812732767的博客
01-06 3981
apollo 定位localization 代码NDT算法模块解析（一）前言一、Init()二、Proc( const std::shared_ptr& gps_msg)总结 ndt_localization_component.cc前言一、Init()二、Proc( const std::shared_ptr& gps_msg)总结 前言 入行已经5年多，一直想分享点东西，希望能帮助到大家。 apollo 中对定位模块的代码及算法介绍实在是太少了，不知道是因为用的人少
【ROS学习】使用C++获取激光雷达数据
最新发布
祖国花朵的博文
12-06 507
执行roslaunch启动测距观测示例程序。
LVI-SAM代码复现、调试与运行
weixin_50578602的博客
10-24 5244
LVI-SAM是Tixiao Shan的最新力作，Tixiao Shan是Lego-loam和Lio-sam的作者，LVI-SAM是Tixiao Shan最新开源的基于视觉-激光-惯导里程计SLAM框架，结合了Lio-sam和Vins-Mono的视觉-激光-惯导融合的SLAM框架。
12-[LVI-SAM]visual_feature_光流追踪算法
loyer_kong的博客
12-05 2218
​ 上一个blog，我们简要分析了img_callback这个回调函数，但是没有分析里面使用的函数。特别是我们之前已经提到过的光流追踪算法。 ​ 而本次blog主要就是分析上一个blog中提到的`readImage`函数。
LVI-SAM：配置环境、安装测试、适配自己采集数据集
热门推荐
有待成长的小学生
07-26 1万+
LVI-SAM调试详细记录，包括环境配置、源码编译安装、运行示例程序及如何适配自己传感器数据
C++修改lvi-sam代码遇到的错误
cyx610481953的博客
04-25 936
分享一下修改别人代码的时候遇到的错误。 错误一：‘A’ has no member named ‘b’ 例如下图所示： 这种错误指的就是对应的.h头文件中没有b成员。解决办法：在对应的.h文件中的A函数中定义b成员。 错误二：对‘…'未定义的引用 例如下图所示： 这种错误有两种可能，一种是CmakeLists.txt文件中没有添加并链接对应的库，另一种是未定义引用对应的函数没有实现。解决办法：首先查看CmakeLists.txt文件中有没有添加并链接对应的库，然后再检查对应的函数是否有具体的实现，这种实
15-[LVI-SAM]分析总结
loyer_kong的博客
12-26 2805
这是我的LVI-SAM代码分析的最后一篇。在这一篇，我会做一个总结，对我前面的Blog做一个综述，分享在分析的过程中，我的收获。
14-[LVI-SAM]visual_odometry_callbackAndprocess
loyer_kong的博客
12-19 607
在上一章，我们大致把visual_odometry的main函数过了一遍，把节点在生成回调函数之前的准备工作介绍了一遍。我们已经大致了解了节点的输入和输出。其实，通过上一章的分析，可以发现，这个节点和我之前分析的visual_feature的关联还是比较紧密，需要用到visual_feature发布的视觉特征点。下面，我们开始分析这个节点最重要的回调函数部分，以及主线程process。其中，imu_callback，主线程process是主要部分。
[lvi_sam_visual_odometry-9] process has died [pid 2367, exit code -6,
04-02
This error message indicates that the process called "lvi_sam_visual_odometry-9" has unexpectedly terminated or crashed with an exit code of -6. The exact cause of the crash is not clear from this message alone, but it could be due to a variety of factors such as memory issues, segmentation faults, or bugs in the code. To troubleshoot this issue, you may need to gather more information from the system logs or by running the process in a debugger.

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值