9-visual_feature_VINS-Mono

2021SC@SDUSC

visual_feature_VINS-Mono

​ 经过上一篇的分析，我们大致了解了这个节点总体情况，以及节点初始化的调用函数的过程。但是，在初步分析的时候，我遇到了很多没见过的名词，对很多概念没有足够的只是理解。在分析完上一篇后，我参考了网上的资料和队友的分析，决定结合视觉模块的参考论文——VINS-Mono来分析visual_feature部分。在本篇代码分析中，我们对作者参考的VINS-Mono和LVI-SAM的visual_feature来分析。

1. VINS-Mono简介

​ 由一个相机和一个低成本惯性测量单元(IMU)组成的单目视觉惯性系统(VINS)，构成了用于度量六自由度状态估计的最小传感器套件。然而，由于缺乏直接距离测量，在IMU处理、估计器初始化、外部标定和非线性优化等方面提出了重大挑战。VINS-Mono：一种具有鲁棒性和通用性的单目视觉惯性状态估计器。该方法从用于估计器初始化和故障恢复的鲁棒的程序开始。采用一种基于紧耦合、非线性优化的方法，通过融合预积分后的IMU测量值和特征观测值，获得高精度的视觉惯性里程计。结合紧耦合方法，回环检测模块能够以最小的计算代价实现重定位。此外，我们还进行四自由度位姿图优化，以加强全局一致性。

2. VINS-Mono - visual_feature

​ VINS-Mono的visual_feature在本篇论文中的第四部分的A部分。

​ 对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪。同时，检测新的角点特征以保证每个图像特征的最小数目(100-300)。该检测器通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布。二维特征首先是不失真的，然后在通过外点剔除后投影到一个单位球面上。利用基本矩阵模型的RANSAC算法进行外点剔除。

角点可以简单的认为是两条边的交点，比较严格的定义则是在邻域内具有两个主方向的特征点，也就是说在两个方向上灰度变化剧烈。如下图所示，在各个方向上移动小窗口，如果在所有方向上移动，窗口内灰度都发生变化，则认为是角点；如果任何方向都不变化，则是均匀区域；如果灰度只在一个方向上变化，则可能是图像边缘。

​ 在此步骤中还选择了关键帧。我们有两个关键帧选择标准。第一是与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。请注意，不仅平移，旋转也会产生视差。然而，特征点无法在纯旋转运动中三角化。为了避免这种情况，在计算视差时我们使用陀螺仪测量值的短时积分来补偿旋转。请注意，此旋转补偿仅用于关键帧选择，而不涉及VINS公式中的旋转计算。为此，即使陀螺仪含有较大的噪声或存在偏置，也只会导致次优的关键帧选择结果，不会直接影响估计质量。另一个标准是跟踪质量。如果跟踪的特征数量低于某一阈值，我们将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。

a. KLT稀疏光流算法

​ 在解释这个算法的时候，我们需要解释一个名词——光流（Optical flow）。

光流(Optical flow or optic flow)是关于视域中的物体运动检测中的概念。用来描述相对于观察者的运动所造成的观测目标、表面或边缘的运动。 ——维基百科

​ 它是空间运动物体在观察成像平面上的像素运动的瞬时速度，是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息的一种方法。一般而言，光流是由于场景中前景目标本身的移动、相机的运动，或者两者的共同运动所产生的。

​ 光流是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息的一种方法。

光流法假设：

1. 亮度恒定，图像中物体的像素亮度在连续帧之间不会发生变化；
2. 短距离(短时)运动，相邻帧之间的时间足够短，物体运动较小；
3. 空间一致性，相邻像素具有相似的运动；

​ 记 $I(x,y,t)$ 为 $t$ 时刻像素点 $(x,y)$ 的像素值，那么根据前两个假设，可得到：
$$I(x,y,t)=I(x+dx,y+dy,t+dt)$$
一阶泰勒展开：
$$I(x+dx,y+dy,t+dt)=I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt$$
由此可得：
$$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0⟺\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy=-\frac{\partial I}{\partial t}dt$$
记$(\frac{dx}{dt},\frac{dy}{dt})=(u,v)$, 即为所要求解的像素光流；$(\frac{\partial I}{\partial x},\frac{\partial I}{\partial y})=(I_x,I_y)$ 为像素灰度空间微分；$\frac{\partial I}{\partial t}=I_x$ , 为像素坐标点的时间灰度微分。整理成矩阵形式：
$$\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-I_t$$
该式表示相同坐标位置的时间灰度微分是空间灰度微分与这个位置上相对于观察者的速度的乘积。由空间一致性假设，对于周围多个点，有：
$$\left[\begin{array}{cc}{I}_{x1}& I_{y1}\\ I_{x2}& I_{y2}\\ I_{x3}& I_{y3}\\ ⋮& ⋮\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-\left[\begin{array}{c}{I}_{t1}\\ I_{t2}\\ I_{t3}\\ ⋮\end{array}\right]⟺A\vec{u}=b$$
这是标准的线性方程组，可用最小二乘法求解 $\vec{u}=(A^{T}A{)}^{-1}{A}^{T}b$ ，也可以迭代求解。这种方式得到的光流，称为 Lucas-Kanade 算法。

KLT

KLT 算法本质上也基于光流的三个假设，不同于前述直接比较像素点灰度值的作法，KLT 比较像素点周围的窗口像素，来寻找最相似的像素点。由光流假设，在很短时间 $\tau$ 内，前后两帧图像满足：
$$J(Ax+d)=I(x),其中A=1+D=1+\left[\begin{array}{cc}{d}_{xx}& d_{xy}\\ d_{yx}& d_{yy}\end{array}\right]$$
像素位移(displacement)向量满足仿射运动模型(Affine Motion)$=Dx+d$ ，其中 $D$ 称为变形矩阵(Deformation Matrix)，$d$ 称为位移向量(Displacement Vector)。 $D$ 表示两个像素窗口块运动后的变形量，所以当窗口较小时，会比较难估计。通常 $D$ 可以用来衡量两个像素窗口的相似度，即衡量特征点有没有漂移。而对于光流跟踪量，一般只考虑平移模型(Translation Model)：
$$J(x+d)=I(x)$$

b. RANSAC算法

​ Random sample consensus随机抽样一致算法 (RANSAC)。它采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。 RANSAC是一个非确定性算法，在某种意义上说，它会产生一个在一定概率下合理的结果，而更多次的迭代会使这一概率增加。此RANSAC算法在1981年由Fischler和Bolles首次提出。

基本假设

1. “内群”(inlier, 似乎译为内点群更加妥当，即正常数据，正确数据)数据可以通过几组模型的参数来叙述其分布，而“离群”(outlier,似乎译为外点群更加妥当，异常数据)数据则是不适合模型化的数据。
2. 数据会受噪声影响，噪声指的是离群，例如从极端的噪声或错误解释有关数据的测量或不正确的假设。
3. RANSAC假定，给定一组（通常很小）的内点群，存在一个程序，这个程序可以估算最佳解释或最适用于这一数据模型的参数。

算法步骤

1. 在数据中随机选择若干个点设定为内点群；(对于直线拟合来说就是两个点，对于计算Homography矩阵就是4个点)；
2. 使用这个数据集来计算出数据模型；
3. 把其它刚才没选到的点带入刚才建立的模型中，计算是否属于内点群；(累加在一定误差范围内的适合当前迭代推出模型的数据)；
4. 比较当前模型和之前推出的最好的模型的“内点“的数量，记录最大“内点”数的模型参数和“内点”数；
5. 重复1-4步，比较哪次计算中内点群数量最多，内点群最多的那次所建的模型就是我们所要求的解(“内点数目大于一定数量”)。

参数

1. 一开始的时候我们要随机选择多少点（n）
2. 以及要重复做多少次（k）

算法说明

​ 这里用一个简单的例子来说明，在一组数据点中找到一条最适合的线。假设，此有一组集合包含了内点群以及外点群，其中内点群包含可以被拟合到线段上的点，而外点群则是无法被拟合的点。如果我们用简单的最小二乘法来找此线，我们将无法得到一条适合于内点群的直线，因为最小二乘法会受外点群影响而影响其结果。而用RANSAC，可以只由内点群来计算出模型，而且概率还够高。然而，RANSAC无法保证结果一定最好，所以必须小心选择参数，使其能有足够的概率。

包含许多离群的一组数据，要找一条最适合的线。

RANSAC找到的线，离群值对结果没影响（蓝色点为内群，红色点为离群）

c. parallax（视差）

​ parallax（视差）是指从两个不同位置观察同一个物体时，此物体在视野中的位置变化与差异。从两个观察点看目标，两条视线之间的夹角叫做这两个点的视差角，两点之间的距离称作视差基线。

​ 从同样的两个观察点看，目标物体距离越近就有越大的视差，因此视差可以被用来反向估算物体的距离。比如天文学家使用视差的原理测量天体距离地球的距离，包括月球、太阳和在太阳系之外的恒星。

​ 一个简单的，日常都能见到的视差例子是，汽车仪表板上"指针"显示的速度计。当从正前方观看时，显示的正确数值可能是60；但从乘客的位置观看，由于视角的不同，指针显示的速度可能会略有不同。

3. LVI-SAM的改进

​ 我通过比较分析LVI-SAM和VINS-Mono的feature_tracker_node.cpp的源代码，分析LVI-SAM的改进。

​ 首先一个最大的不同是，LVI-SAM订阅了lidar的相关话题——imageProjection节点变换到世界坐标系的当前点云信息，这个在上一个blog中有提到。而这也是作者的论文的贡献之一。那么，如何将获取到的lidar的相关消息融入原本是纯视觉的VINS-Mono中呢？

​ VINS-Mono通常会因在初始化时解决非线性问题产生比较大的误差,另外初始化的结果比较依赖于初始的运动和IMU的精确度,VINS-MONO在慢速或者匀速运动时的初始化结果会比较差,原因是因为在这个速度下的IMU获得的加速度不足以为尺度提供比较好的约束。

​ 因此为了提升VIS的初始化效果，作者采用LIS辅助初始化的方式(因为可以从LIS中直接获得深度).然后把这些从LIS获得的状态和IMU偏差进行插值并且和图像帧进行关联，关联后的状态和IMU偏差作为VIS初始化的结果。

​ 除了一些细小的改动，在代码中呈现的主要改进如下：

a. 改进一：

​ 增加了获取lidar点云信息的操作。

VINS-Mono

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);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());

LVI-SAM

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

// 从lidar点云数据中获取深度信息
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);

b. 改进二：

在图中标出跟踪到的特征点和没有跟踪的。

VINS-Mono：

// 跳过第一帧图像，因为没有光流信息
if (!init_pub)
{
    init_pub = 1;
}
else
    pub_img.publish(feature_points);

if (SHOW_TRACK)
{
    ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
    //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++)
        {
            double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
            cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
            //画速度线，这里应该是作者自己用来调试程序使用的
            /*
                    Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y);
                    Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y);
                    Vector3d tmp_prev_un_pts;
                    tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity;
                    tmp_prev_un_pts.z() = 1;
                    Vector2d tmp_prev_uv;
                    trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv);
                    cv::line(tmp_img, trackerData[i].cur_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0);
                    */
            //char name[10];
            //sprintf(name, "%d", trackerData[i].ids[j]);
            //cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
        }
    }
    //cv::imshow("vis", stereo_img);
    //cv::waitKey(5);
    pub_match.publish(ptr->toImageMsg());
}

LVI-SAM：

// 跳过第一帧图像，因为没有光流信息
if (!init_pub)
{
    init_pub = 1;
}
else
    pub_feature.publish(feature_points);

// 在图像中发布特征
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)
            {
                // 计算跟踪的特征点数量
                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());
}

4. 引用

[1708.03852] VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

（三十八）稀疏光流----KLT - 知乎

KLT 光流算法详解 | LeijieZhang

随机抽样一致 - 维基百科，自由的百科全书

Parallax - Wikipedia