LVI-SAM 视觉部分 feature_tracker.h 代码解析

现在开始LVI-SAM 视觉部分的代码阅读。

头文件、命名空间

#pragma once // 只编译一次

#include <cstdio>
#include <iostream>
#include <queue>
#include <execinfo.h>
#include <csignal>

#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>

#include "camera_models/CameraFactory.h"
#include "camera_models/CataCamera.h"
#include "camera_models/PinholeCamera.h"

#include "parameters.h"
#include "tic_toc.h"

using namespace std;
using namespace camodocal;
using namespace Eigen;

这上面没啥好说的，老一套了，所以不多讲。

一些函数

// 判断跟踪的特征点是否在图像边界内
bool inBorder(const cv::Point2f &pt);

// 去除无法跟踪的特征点
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status);
void reduceVector(vector<int> &v, vector<uchar> status);

class FeatureTracker
{
  public:
    FeatureTracker();

    void readImage(const cv::Mat &_img,double _cur_time); // 对图像使用光流法进行特征点跟踪

    void setMask(); // 对特征点进行排序并去除密集点

    void addPoints(); // 添加新检测到的特征点n_pts，ID初始化-1，跟踪次数1

    bool updateID(unsigned int i); // 更新特征点id

    void readIntrinsicParameter(const string &calib_file); // 读取相机内参

    void showUndistortion(const string &name); // 显示去畸变矫正后的特征点

    void rejectWithF(); // 通过F矩阵去除outliers

    void undistortedPoints(); // 对特征点的图像坐标去畸变矫正，并计算每个角点的速度

一些在头文件里面定义的函数，具体实现的话，还是要在cpp文件中进行详细的阅读。这些函数没啥好讲的，作用功能都写在注释里面了，自行阅读。

一些变量

    cv::Mat mask; // 图像掩码 好像也叫掩膜
    cv::Mat fisheye_mask; // 鱼眼相机mask，用来去除边缘噪点
    
    // prev_img是上一次发布的帧的图像数据
    // cur_img是光流跟踪的前一帧的图像数据
    // forw_img是光流跟踪的后一帧的图像数据
    cv::Mat prev_img, cur_img, forw_img;
    vector<cv::Point2f> n_pts; // 每一帧中新提取的特征点
    vector<cv::Point2f> prev_pts, cur_pts, forw_pts; // 对应的图像特征点
    vector<cv::Point2f> prev_un_pts, cur_un_pts; // 归一化相机坐标系下的坐标
    vector<cv::Point2f> pts_velocity; // 当前帧相对前一帧特征点沿x,y方向的像素移动速度
    vector<int> ids; // 能够被跟踪到的特征点的id
    vector<int> track_cnt; // 当前帧forw_img中每个特征点被跟踪的时间次数
    map<int, cv::Point2f> cur_un_pts_map;
    map<int, cv::Point2f> prev_un_pts_map;
    camodocal::CameraPtr m_camera; // 相机模型
    double cur_time;
    double prev_time;

    static int n_id; // 用来作为特征点的id，每检测到一个新的特征点，就将n_id作为该特征点的id，然后n_id的值加1

上面是一些变量，每个变量都有自己的意义。在这里的话，就是forw表示当前，cur是forw的前一帧，prev是cur的前一帧，这三个的时间关系务必搞清楚。

class DepthRegister
{
public:

    ros::NodeHandle n;
    // publisher for visualization
    ros::Publisher pub_depth_feature;
    ros::Publisher pub_depth_image;
    ros::Publisher pub_depth_cloud;

    tf::TransformListener listener;
    tf::StampedTransform transform;

    const int num_bins = 360;
    vector<vector<PointType>> pointsArray;

    DepthRegister(ros::NodeHandle n_in): // 构造函数
    n(n_in)
    {
        // messages for RVIZ visualization 发布出去的信息，用于RVIZ可视化用的
        pub_depth_feature = n.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/vins/depth/depth_feature", 5);
        pub_depth_image =   n.advertise<sensor_msgs::Image>      (PROJECT_NAME + "/vins/depth/depth_image",   5);
        pub_depth_cloud =   n.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/vins/depth/depth_cloud",   5);

        pointsArray.resize(num_bins);
        for (int i = 0; i < num_bins; ++i)
            pointsArray[i].resize(num_bins);
    }

    // 核心！
    sensor_msgs::ChannelFloat32 get_depth(const ros::Time& stamp_cur, const cv::Mat& imageCur, 
                                          const pcl::PointCloud<PointType>::Ptr& depthCloud,
                                          const camodocal::CameraPtr& camera_model ,
                                          const vector<geometry_msgs::Point32>& features_2d)
    {
        // 0.1 initialize depth for return
        // 初始化需要返回的深度信息
        sensor_msgs::ChannelFloat32 depth_of_point;
        depth_of_point.name = "depth";
        depth_of_point.values.resize(features_2d.size(), -1);

        // 0.2  check if depthCloud available
        // 检查点云图是否可用
        if (depthCloud->size() == 0)
            return depth_of_point;

        // 0.3 look up transform at current image time
        // 在当前图像时间查找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);

        // 0.4 transform cloud from global frame to camera frame
        // 将点云图从世界坐标系转换到当前相机坐标系
        pcl::PointCloud<PointType>::Ptr depth_cloud_local(new pcl::PointCloud<PointType>());
        pcl::transformPointCloud(*depthCloud, *depth_cloud_local, transNow.inverse());

        // 0.5 project undistorted normalized (z) 2d features onto a unit sphere
        // 将未失真的归一化2d特征投影到单位球体上
        pcl::PointCloud<PointType>::Ptr features_3d_sphere(new pcl::PointCloud<PointType>());
        for (int i = 0; i < (int)features_2d.size(); ++i)
        {
            // normalize 2d feature to a unit sphere
            // 归一化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(); 
            // convert to ROS standard
            // 转换为ROS的标准
            PointType p;
            p.x =  feature_cur(2);
            p.y = -feature_cur(0);
            p.z = -feature_cur(1);
            p.intensity = -1; // intensity will be used to save depth
            // intensity会被用来保存深度信息
            features_3d_sphere->push_back(p);
        }

        // 3. project depth cloud on a range image, filter points satcked in the same region
        // 投影深度点云在距离图像上并过滤位于同一区域的点
        float bin_res = 180.0 / (float)num_bins; // currently only cover the space in front of lidar (-90 ~ 90) 只能覆盖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];
            // filter points not in camera view
            // 过滤不在相机视野中的点
            if (p.x < 0 || abs(p.y / p.x) > 10 || abs(p.z / p.x) > 10)
                continue;
            // find row id in range image
            // 在图像的范围中找到行号
            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);
            // find column id in range image
            // 在图像的范围中找到列号
            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 may be out of boundary
            // id可能超出约束条件
            if (row_id < 0 || row_id >= num_bins || col_id < 0 || col_id >= num_bins)
                continue;
            // only keep points that's closer
            // 只保留较近的点
            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;
            }
        }

        // 4. extract downsampled depth cloud from range image
        // 从距离图像中提取下采样深度点云图
        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");

        // 5. project depth cloud onto a unit sphere
        // 将深度点云图投影到单位球体上
        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;

        // 6. create a kd-tree using the spherical depth cloud
        // 使用球形深度点云创建kd树
        pcl::KdTreeFLANN<PointType>::Ptr kdtree(new pcl::KdTreeFLANN<PointType>());
        kdtree->setInputCloud(depth_cloud_unit_sphere);

        // 7. find the feature depth using kd-tree
        // 使用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;
                }
                // convert feature into cartesian space if depth is available
                // 如果深度可用，则将特征转换为笛卡尔空间
                features_3d_sphere->points[i].x *= s;
                features_3d_sphere->points[i].y *= s;
                features_3d_sphere->points[i].z *= s;
                // the obtained depth here is for unit sphere, VINS estimator needs depth for normalized feature (by value z), (lidar x = camera z)
                // 这里获得的深度是针对单位球体的，VINS估计器需要归一化特征的深度（按z值）(激光雷达x = 相机z)
                features_3d_sphere->points[i].intensity = features_3d_sphere->points[i].x;
            }
        }

        // visualize features in cartesian 3d space (including the feature without depth (default 1))
        // 可视化笛卡尔3d空间中的特征（包括没有深度的特征（默认1））
        publishCloud(&pub_depth_feature, features_3d_sphere, stamp_cur, "vins_body_ros");
        
        // update depth value for return
        // 更新返回的深度值
        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;
        }

        // visualization project points on image for visualization (it's slow!)
        // 可视化项目点在图像上进行可视化（很慢）
        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)
            {
                // convert points from 3D to 2D
                // 将点从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;
    }

    void getColor(float p, float np, float&r, float&g, float&b) 
    {
        // 这里rgb表示三个通道的颜色，最后乘255，得到颜色的值
        float inc = 6.0 / np;
        float x = p * inc;
        r = 0.0f; g = 0.0f; b = 0.0f;
        if ((0 <= x && x <= 1) || (5 <= x && x <= 6)) r = 1.0f;
        else if (4 <= x && x <= 5) r = x - 4;
        else if (1 <= x && x <= 2) r = 1.0f - (x - 1);

        if (1 <= x && x <= 3) g = 1.0f;
        else if (0 <= x && x <= 1) g = x - 0;
        else if (3 <= x && x <= 4) g = 1.0f - (x - 3);

        if (3 <= x && x <= 5) b = 1.0f;
        else if (2 <= x && x <= 3) b = x - 2;
        else if (5 <= x && x <= 6) b = 1.0f - (x - 5);
        r *= 255.0;
        g *= 255.0;
        b *= 255.0;
    }
};

这个的话就是DepthRegister类的具体内容。一开始定义了public的变量，用来rviz的可视化。然后下面是一个构造函数，用来可视化用的，这个没涉及到其他多余的操作，也不多讲。下面一个get_depth()的函数，这个非常重要，首先整理一下这个函数的流程。

get_depth()函数流程：

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

2、检查点云图是否可用

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

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

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

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

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

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

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

10、使用kd-tree找到特征深度，并放回结果

11、可视化笛卡尔三维空间中的特征

12、更新返回深度值

13、可视化项目点在图像上进行可视化

以上便是这个函数的流程，总共分了13步进行操作，猜测每一步代码比较简洁，因此就直接if，for这种直接上代码了，没有进行包装。一系列转换没啥好说的，然后转换矩阵要看懂，中间还有一步降采样的过程，也是为了减少不必要的计算了。然后创建kd树，这个数据结构的技巧，使得搜索更快。然后找到特征深度，里面r1、r2、r3这些公式的话，没太看懂。然后下面就是发布点云。这里的话，感觉没啥特别重要的，就是get_depth()这个要好好理解一下。

void getColor(float p, float np, float&r, float&g, float&b) 
    {
        // 这里rgb表示三个通道的颜色，最后乘255，得到颜色的值
        float inc = 6.0 / np;
        float x = p * inc;
        r = 0.0f; g = 0.0f; b = 0.0f;
        if ((0 <= x && x <= 1) || (5 <= x && x <= 6)) r = 1.0f;
        else if (4 <= x && x <= 5) r = x - 4;
        else if (1 <= x && x <= 2) r = 1.0f - (x - 1);

        if (1 <= x && x <= 3) g = 1.0f;
        else if (0 <= x && x <= 1) g = x - 0;
        else if (3 <= x && x <= 4) g = 1.0f - (x - 3);

        if (3 <= x && x <= 5) b = 1.0f;
        else if (2 <= x && x <= 3) b = x - 2;
        else if (5 <= x && x <= 6) b = 1.0f - (x - 5);
        r *= 255.0;
        g *= 255.0;
        b *= 255.0;
    }

上面这个函数的话就是获取颜色，rgb代表三个通道，然后根据if语句来进行rgb三通道的赋值，最后乘255即可得到了最终值。

本篇结，感觉自己没学到位，所以感觉好像没啥疑问的地方。。。