vins estimator FeatureManager

xiaoma_bk

已于 2022-06-01 00:32:03 修改

阅读量626

点赞数

分类专栏： # vins 文章标签：人工智能

于 2022-06-01 00:31:39 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/125076086

版权

vins 专栏收录该内容

15 篇文章 3 订阅

订阅专栏

本文详细介绍了特征管理在角点跟踪中的应用，包括特征PerId结构、视差补偿、关键帧判断、三角化求深度的过程。通过FeatureManager类实现特征的增删操作，以及边缘化策略来维护特征的有效性和深度估计。

摘要由CSDN通过智能技术生成

文章目录

特征管理

特征管理

FeatureManager
Public：
- list<FeaturePerId> feature; // 可以得到滑动窗口内所有的角点信息
- int last_track_num;

特征相关结构体

每一Id的特征 FeaturePerId

brief：某feature_id下的所有FeaturePerFrame
构造：帧Id + 出现该帧角点的第一帧id
成员变量：
- 特征Id feature_id
- 该特征在每一帧的参数 vector<FeaturePerFrame> feature_per_frame
- 被使用次数 used_num
- 是外点？ is_outlier
- 需边缘化的点？ margin
- 估计的深度 estimated_depth
- 求解标记 // 0 还没有解决； 1 解决成功； 2解决失败；
- gt_p
成员参数：endFrame
- 起始帧id + 出现帧得到个数 -1

每一frame的特征 FeaturePerFrame

Point :
- point [x,y,z] + [u,v] + [vel_x,vel_y] + cur_td
其他参数

double z;
bool is_used;
double parallax;
MatrixXd A;
VectorXd b;
double dep_gradient;

特征管理相关函数

compensatedParallax2

函数：计算某个特征点在次新帧和次次新帧的视差
参数：
- FeaturePerId 某一Id的特征
- frame_count
Code：
- 取到次新帧frame_j和次次新帧frame_i
  - frame_count - (1,2) - it_per_id.start_frame
- 次新帧和次次新帧的 du,dv
  - u_j,v_j
  - u_i= p_i(0)/dep_i,v_i= p_i(1)/dep_i
  - du=u_i-u_j , dv=v_i-v_j
- 返回视差的欧氏距离

addFeatureCheckParallax

brief：把特征点放入feature的list容器中，计算每一个点跟踪次数和它在次新帧和次次新帧间的视差，返回是否是关键帧
Param：
- frame_count 帧的序号
- image 某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]s构成的map,索引为feature_id
- td IMU和cam同步时间差
流程：
- 遍历image_map，将其所有特征放入 feature list 容器中
  - 构造 FeaturePerFrame (points_7+td)
  - feature list中迭代寻找是否有该feature_id的特征
  - 如果没有，则新建并添加这图像帧
  - 否则，只将图像帧中加进去，last_track_num++
- 若滑窗中帧个数小于2 或跟踪数小于20时
  - 返回 true 即关键帧
- 遍历每一特征，计算每个特征在次新帧和次次新帧中的视差
  - 若当前帧中次新帧和次次新帧在当前特征中存在时
    - 计算其视差 compensatedParallax2
    - 统计其存在个数 parallax_num++;
- 若每个特征在次新帧和次次新帧中的视差都不存在，则返回true
- 否则，若均值视差大于预设值，则返回true，否则返回false
  - 均值视差，两两视差之和/总个数

triangulate

原理：
- 在不同位置观测到同一个三维点 $P (x, y, z)$ ，已知在不同位置处观察到的三维点的二维投影 $x_1(x_1,y_1)$ 、 ${x_2(x_2,y_2)}$ ，利用三角关系，恢复出三维点的深度信息 $z$
- 三角化求解：
  - 利用叉乘进行消元 ${s_1x_1 = s_2Rx_2+t}$
  - 左右两边同时乘以 $x_1$ 的反对称矩阵，可得： ${s1x_1^\wedge x_1=s_2x_1^\wedge TRx_2+x_1t}$
  - 将 $s_2$ 带入公式1，可求得 $s_1$ ：
- 线性三角化
  - 对于 $X$ 为三维空间点在世界坐标系下的齐次坐标 ${x=[x,y,z,1]^T}$ ，和 ${T=[r_1,r_2,r_3]^T=[R,t]}$ 为世界坐标系到相机坐标系的变换，和 ${x=[u,v,1]^T}$ 为归一化平面坐标， $\lambda$ 为深度
  - ${\lambda x=TX}$ => ${x^\wedge TX =0}$
  - 带入有： ${\begin{bmatrix} 0 & -1 & v\\ 1 & 0 & -u \\ -v & u & 0 \end{bmatrix} \begin{bmatrix} r_1 \\ r_2 \\ r_3 \end{bmatrix}X = \begin{bmatrix}-r_2+v r_3 \\ r_1 - ur_3 \\ -vr_1 +ur_2 \end{bmatrix}X}$
  - 第一行 ${\times -u}$ ，第二行 $\times -v$ 相加，即可得到第三行，故由线性相关性，只保留前两行
  - 即一个归一化平面坐标可以构建两个关于 $X$ 的线性方程组，多个点可以构建多组线性方程
  - 多组方程无非零解，使用svd分解求最小二乘，解可能不满足齐次坐标形式，因此 ${X=[x,y,z,1]^T=[x_0/x_3,x_1/x_3,x_2/x_3,x_3/x_3]^T}$
brief：对特征点进行三角化求深度（SVD分解）
Code：

void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{
    // 遍历 滑窗内的特征
    for (auto &it_per_id : feature)
    {	
        // 若该特征使用次数过少，则跳过
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
		
        // 若该特征已经有深度了，则跳过
        if (it_per_id.estimated_depth > 0)
            continue;
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

        ROS_ASSERT(NUM_OF_CAM == 1);
        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
        int svd_idx = 0;

        //R0 t0为第i帧相机坐标系到世界坐标系的变换矩阵
        Eigen::Matrix<double, 3, 4> P0;
        Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
        Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
        P0.leftCols<3>() = Eigen::Matrix3d::Identity();
        P0.rightCols<1>() = Eigen::Vector3d::Zero();

        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;
            //R1 t1为第j帧相机坐标系到世界坐标系的变换矩阵
            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            // t,R 为i到j的转换，
            Eigen::Vector3d t = R0.transpose() * (t1 - t0);
            Eigen::Matrix3d R = R0.transpose() * R1;
            Eigen::Matrix<double, 3, 4> P;
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            Eigen::Vector3d f = it_per_frame.point.normalized();
            //P = [P1 P2 P3]^T 
            //AX=0      A = [A(2*i) A(2*i+1) A(2*i+2) A(2*i+3) ...]^T
            //A(2*i)   = x(i) * P3 - z(i) * P1
            //A(2*i+1) = y(i) * P3 - z(i) * P2
            svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
            svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);

            if (imu_i == imu_j)
                continue;
        }
        //对A的SVD分解得到其最小奇异值对应的单位奇异向量(x,y,z,w)，深度为z/w
        ROS_ASSERT(svd_idx == svd_A.rows());
        Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
        double svd_method = svd_V[2] / svd_V[3];
        //it_per_id->estimated_depth = -b / A;
        //it_per_id->estimated_depth = svd_V[2] / svd_V[3];

        it_per_id.estimated_depth = svd_method;
        //it_per_id->estimated_depth = INIT_DEPTH;

        if (it_per_id.estimated_depth < 0.1)
        {
            it_per_id.estimated_depth = INIT_DEPTH;
        }

    }
}

边缘化

直接将属于那一帧的特征给删除了？？

removeBackShiftDepth

//边缘化最老帧时，处理特征点保存的帧号，将起始帧是最老帧的特征点的深度值进行转移
//marg_R、marg_P为被边缘化的位姿，new_R、new_P为在这下一帧的位姿
void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        //特征点起始帧不是最老帧则将帧号减一
        if (it->start_frame != 0)
            it->start_frame--;
        else
        {
            //特征点起始帧是最老帧
            Eigen::Vector3d uv_i = it->feature_per_frame[0].point;  
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            //特征点只在最老帧被观测则直接移除
            if (it->feature_per_frame.size() < 2)
            {
                feature.erase(it);
                continue;
            }
            else
            {
                //pts_i为特征点在最老帧坐标系下的三维坐标
                //w_pts_i为特征点在世界坐标系下的三维坐标
                //将其转换到在下一帧坐标系下的坐标pts_j
                Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
                Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
                Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
                double dep_j = pts_j(2);
                if (dep_j > 0)
                    it->estimated_depth = dep_j;
                else
                    it->estimated_depth = INIT_DEPTH;
            }
        }
        // remove tracking-lost feature after marginalize
        /*
        if (it->endFrame() < WINDOW_SIZE - 1)
        {
            feature.erase(it);
        }
        */
    }
}

removeFront

//边缘化次新帧时，对特征点在次新帧的信息进行移除处理
void FeatureManager::removeFront(int frame_count)
{
    for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)
    {
        it_next++;
        //起始帧为最新帧的滑动成次新帧
        if (it->start_frame == frame_count)
        {
            it->start_frame--;
        }
        else
        {
            int j = WINDOW_SIZE - 1 - it->start_frame;
            //如果次新帧之前已经跟踪结束则什么都不做
            if (it->endFrame() < frame_count - 1)
                continue;
            //如果在次新帧仍被跟踪，则删除feature_per_frame中次新帧对应的FeaturePerFrame
            //如果feature_per_frame为空则直接删除特征点
            it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

removeBack

//边缘化最老帧时，直接将特征点所保存的帧号向前滑动
void FeatureManager::removeBack()
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        //如果特征点起始帧号start_frame不为零则减一
        if (it->start_frame != 0)
            it->start_frame--;
        //如果start_frame为0则直接移除feature_per_frame的第0帧FeaturePerFrame
        //如果feature_per_frame为空则直接删除特征点
        else
        {
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

其他函数

clearState 清除滑动窗口内的所有角点信息
getFeatureCount 窗口中被跟踪的特征数量
- 遍历滑窗内的特征，若特征被多帧使用，则个数加1
  - 使用个数=该特征被frame观察到的次数
  - 若使用个数大于 2 且起始帧不是次次帧，则满足
- 返回总的符合条件的帧的个数
getCorresponding 得到frame_count_l与frame_count_r两帧之间的对应特征点
- 遍历滑窗内的特征
  - 如果 frame_count l、r 在滑窗内
  - 则计算两帧的index index_l,index_r
  - 得到三维点坐标，并push到容器
- 返回对应的角点
setDepth 设置特征点的逆深度估计值
- 遍历滑窗内的特征
  - 得到特征的使用次数 used_num
  - 其中使用个数大于 2 且起始帧不是次次帧，不满足，跳过该特征
  - 计算估计深度的逆值 1./x(++feature_index)
  - 若逆深度值小于0 则估计失败 solve_flag = 2
  - 否则估计成功 solve_flag = 1
removeFailures 剔除feature中估计失败的点
- 遍历滑窗内的特征点，若标记为失败则删除
clearDepth 清除失败
- 遍历滑窗内的特征，并逆深度赋值
  - 其中使用个数大于 2 且起始帧不是次次帧，不满足，跳过该特征
getDepthVector 得到深度向量
- 遍历滑窗内的特征
  - 其中使用个数大于 2 且起始帧不是次次帧，不满足，跳过该特征
  - 基于逆深度求出深度
- 返回各个特征的深度信息