VINS-Mono 代碼詳細解讀——feature_manager.cpp

LZ發現estimator.cpp中關鍵函數爲processImage（），裏面包含了IMU預積分、圖像處理特徵點跟蹤等一系列流程，上一節中對processIMU（）以及預積分的integrationBase類進行解讀，本節繼續做基礎儲備，對與estimator.cpp中的feature_manager.cpp進行詳細介紹，主要是對特徵點管理。特徵點管理器主要就是FeatureManager類

目錄

1. feature_manager.h中三個類FeatureManager、FeaturePerId、FeaturePerFrame的關係
2. 類的成員函數

函數名	功能
FeaturePerId::endFrame()	返回最後一個觀測到這個特徵點的圖像幀ID
FeatureManager()::getFeatureCount()	窗口中被跟蹤的特徵點數量
FeatureManager()::addFeatureCheckParallax()	特徵點進入時檢查視差，是否爲關鍵幀
FeatureManager()::getCorresponding()	前後兩幀之間匹配特徵點3D座標
FeatureManager()::setDepth()	設置特徵點逆深度
FeatureManager()::triangulate()	特徵點三角化求深度（SVD分解）
FeatureManager()::removeOutlier()	移除外點
FeatureManager()::removeBackShiftDepth()
FeatureManager()::removeBack()	邊緣化最老幀，直接將特徵點保存的幀號前移
FeatureManager()::removeFront()	邊緣化次新幀，對特徵點在次新幀的信息移除

---

一、FeatureManager、FeaturePerId、FeaturePerFrame

 f_manager是特徵管理器類的對象

FeatureManager f_manager;//特徵管理器類

這裏f_mangager.feature主要指的是一個list容器

feature_manager.h主要三個類：

FeatureManager管理所有特徵點，通過list容器存儲特徵點屬性

FeaturePerId指的是某feature_id下的所有FeaturePerFrame。常用feature_id和觀測第一幀start_frame、最後一幀endFrame()

FeaturePerFrame指的是每幀基本的數據：特徵點[x,y,z,u,v,vx,vy]和td IMU與cam同步時間差

FeaturePerId，某feature_id下的所有FeaturePerFrame 

feature_id 特徵點ID、start_frame 出現該角點的第一幀的id--start_frame

class FeaturePerId
{
  public:
    const int feature_id;// 特徵點ID索引
    int start_frame;// 首次被觀測到時，該幀的索引
    vector<FeaturePerFrame> feature_per_frame; // 能夠觀測到某個特徵點的所有相關幀

    int used_num;// 該特徵出現的次數
    bool is_outlier;// 是否外點
    bool is_margin;// 是否Marg邊緣化
    double estimated_depth; // 估計的逆深度
    int solve_flag; // 求解器 0 haven't solve yet; 1 solve succ; 2 solve fail;

    Vector3d gt_p; // ？？？

    FeaturePerId(int _feature_id, int _start_frame) 
        : feature_id(_feature_id), start_frame(_start_frame),
          used_num(0), estimated_depth(-1.0), solve_flag(0)
    {
    }

    int endFrame();// 返回最後一次觀測到這個特徵點的幀數ID
};

 FeaturePerFrame

 _point 每幀的特徵點[x,y,z,u,v,vx,vy]， td IMU和cam同步時間差

class FeaturePerFrame
{
  public:
    FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td)
    {
        point.x() = _point(0);
        point.y() = _point(1);
        point.z() = _point(2);
        uv.x() = _point(3);
        uv.y() = _point(4);
        velocity.x() = _point(5); 
        velocity.y() = _point(6); 
        cur_td = td;
    }
    double cur_td;
    Vector3d point;
    Vector2d uv;
    Vector2d velocity;

    double z; // 特徵點的深度
    bool is_used;// 是否被用了
    double parallax;// 視差
    MatrixXd A; //變換矩陣
    VectorXd b;
    double dep_gradient; // ？？？
};

三者串聯最好的例子是：從f_manager到it_per_id再到底層的it_per_frame，就可以得到基本數據point了

for (auto &it_per_id : f_manager.feature)
{
   ......
   for (auto &it_per_frame : it_per_id.feature_per_frame)
   {
     Vector3d pts_j = it_per_frame.point;// 3D特徵點座標
 }
}

class FeatureManager中 list<FeaturePerId> feature;// 重要！！ 通過FeatureManager可以得到滑動窗口內所有的角點信息

class FeaturePerId中 vector<FeaturePerFrame> feature_per_frame; // 能夠觀測到某個特徵點的所有相關幀

---

 二、主要函數

函數名	功能
FeaturePerId::endFrame()	返回最後一個觀測到這個特徵點的圖像幀ID
FeatureManager()::getFeatureCount()	窗口中被跟蹤的特徵點數量
FeatureManager()::addFeatureCheckParallax()	特徵點進入時檢查視差，是否爲關鍵幀
FeatureManager()::getCorresponding()	前後兩幀之間匹配特徵點3D座標
FeatureManager()::setDepth()	設置特徵點逆深度
FeatureManager()::triangulate()	特徵點三角化求深度（SVD分解）
FeatureManager()::removeOutlier()	移除外點
FeatureManager()::removeBackShiftDepth()
FeatureManager()::removeBack()	邊緣化最老幀，直接將特徵點保存的幀號前移
FeatureManager()::removeFront()	邊緣化次新幀，對特徵點在次新幀的信息移除

endFrame()

返回最後一個觀測到這個特徵點的圖像幀ID

int FeaturePerId::endFrame()
{
    return start_frame + feature_per_frame.size() - 1;
}

其中，feature_per_frame的數量爲vector容器中，FeaturePerFrame基本類的數量。代表能夠觀測到某個特徵點的所有幀。

vector<FeaturePerFrame> feature_per_frame; // 能夠觀測到某個特徵點的所有相關幀

getFeatureCount()

窗口中被跟蹤特徵點的數量。

標準：該特徵點被兩幀以上觀測到了，且第一次觀測到的幀數不是在最後面。

int FeatureManager::getFeatureCount()
{
    int cnt = 0;
    for (auto &it : feature)// 遍歷feature
    {

        it.used_num = it.feature_per_frame.size(); // 所有特徵點被觀測到的幀數
        // 如果該特徵點有兩幀以上觀測到了  且第一次觀測到幀數不是在最後
        if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2)
        {
            cnt++;// 這個特徵點是有效的
        }
    }
    return cnt;
}

addFeatureCheckParallax（）

爲什麼要檢查視差？

VINS中爲了控制優化計算量，只對當前幀之前某一部分幀進行優化，而不是全部歷史幀，局部優化幀數量的大小就是窗口大小。

爲了維持窗口大小，需要去除舊幀添加新幀，也就是邊緣化Marginalization。到底是刪去最舊的幀（MARGIN_OLD）還是刪去剛剛進來窗口倒數第二幀(MARGIN_SECOND_NEW)，就需要對當前幀與之前幀進行視差比較，如果是當前幀變化很小，就會刪去倒數第二幀，如果變化很大，就刪去最舊的幀。

通過檢測兩幀之間的視差以及特徵點數量決定 次新幀是否作爲關鍵幀

關鍵幀選取：

1、當前幀相對最近的關鍵幀的特徵平均視差大於一個閾值就爲關鍵幀（因爲視差可以根據平移和旋轉共同得到，而純旋轉則導致不能三角化成功，所以這一步需要IMU預積分進行補償）

2、當前幀跟蹤到的特徵點數量小於閾值視爲關鍵幀；

輸入的是特徵點，但是會把能觀測到這個特徵點的所有幀也都放進去，第一個索引是特徵點ID，第二個索引是觀測到該特徵點的相機幀 ID。

先把特徵點從image放入feature的list容器中，計算每一個點跟蹤次數小於閾值 和 它在次新幀和次次新幀間所有特徵點的平均視差大於閾值，返回是否是關鍵幀

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    ROS_DEBUG("input feature: %d", (int)image.size());// 特徵點數量
    ROS_DEBUG("num of feature: %d", getFeatureCount()); // 能夠作爲特徵點的數量
    double parallax_sum = 0;// 所有特徵點視差總和
    int parallax_num = 0;
    last_track_num = 0;// 被跟蹤的個數

    // 1. 把image map中的所有特徵點放入feature list容器中
    // 遍歷特徵點,看該特徵點是否在特徵點的列表中,如果沒在,則將<FeatureID,Start_frame>存入到Feature列表中；否則統計數目
    for (auto &id_pts : image)// 遍歷所有特徵點
    {
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);// _point 每幀的特徵點[x,y,z,u,v,vx,vy]和td IMU和cam同步時間差

        // 1.1迭代器尋找feature list中是否有這feature_id
        int feature_id = id_pts.first;// 特徵點ID
        // 第三個參數是 Lambda表達式
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {
            return it.feature_id == feature_id;
                          });

        // 1.2 如果沒有則新建一個，並在feature管理器的list容器最後添加：FeaturePerId、FeaturePerFrame
        if (it == feature.end())
        {
            feature.push_back(FeaturePerId(feature_id, frame_count));// （特徵點ID，首次觀測到特徵點的圖像幀ID）
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        // 1.3 之前有的話在FeaturePerFrame添加此特徵點在此幀的位置和其他信息，並統計數目。
        else if (it->feature_id == feature_id)
        {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++; // 此幀有多少相同特徵點被跟蹤
        }
    }

    // 2. 追蹤次數小於20或者窗口內幀的數目小於2，是關鍵幀
    if (frame_count < 2 || last_track_num < 20)
        return true;

    // 3.計算每個特徵在次新幀和次次新幀中的視差
    for (auto &it_per_id : feature)
    {
        // 觀測該特徵點的：起始幀小於倒數第三幀，終止幀要大於倒數第二幀，保證至少有兩幀能觀測到。
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
            // 總視差：該特徵點在兩幀的歸一化平面上的座標點的距離ans
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;// 個數
        }
    }
    // 4.1 第一次加進去的，是關鍵幀
    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        // 4.2 平均視差大於閾值的是關鍵幀
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
}

其中，find_if函數可以參考 C++ STL find和find_if。意爲：遍歷feature list 容器看看之前是否出現過當前的feature_id

compensatedParallax2（）

計算某個特徵點it_per_id在次新幀和次次新幀的視差ans

判斷觀測到該特徵點的frame中倒數第二幀和倒數第三幀的共視關係 實際是求取該特徵點在兩幀的歸一化平面上的座標點的距離ans

double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
    //check the second last frame is keyframe or not
    //parallax betwwen seconde last frame and third last frame
    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];// 倒數第三幀
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];// 倒數第二幀

    double ans = 0;

    Vector3d p_j = frame_j.point;// 3D路標點（倒數第二幀j）
    double u_j = p_j(0);
    double v_j = p_j(1);

    Vector3d p_i = frame_i.point;// 3D路標點（倒數第三幀i）
    Vector3d p_i_comp;

    //int r_i = frame_count - 2;
    //int r_j = frame_count - 1;
    //p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
    p_i_comp = p_i;

    double dep_i = p_i(2);
    double u_i = p_i(0) / dep_i;
    double v_i = p_i(1) / dep_i;
    double du = u_i - u_j, dv = v_i - v_j;

    double dep_i_comp = p_i_comp(2);
    double u_i_comp = p_i_comp(0) / dep_i_comp;
    double v_i_comp = p_i_comp(1) / dep_i_comp;
    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;
    // 算斜邊
    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));

    return ans;
}

其中，獲取倒數第三幀方式爲：

const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];// 倒數第三幀

 getCorresponding（）

得到給定兩幀之間的對應特徵點3D座標

vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
    vector<pair<Vector3d, Vector3d>> corres;
    for (auto &it : feature)// 遍歷feature的list容器
    {
        // 要找特徵點的兩幀在窗口範圍內，可以直接取。窗口爲：觀測到當前特徵點的所有圖像幀
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
        {
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
            int idx_l = frame_count_l - it.start_frame;// 當前幀-第一次觀測到特徵點的幀數
            int idx_r = frame_count_r - it.start_frame;

            a = it.feature_per_frame[idx_l].point;
            b = it.feature_per_frame[idx_r].point;
            
            corres.push_back(make_pair(a, b));
        }
    }
    return corres;
}

setDepth（）

設置特徵點的逆深度估計值

void FeatureManager::setDepth(const VectorXd &x)
{
    int feature_index = -1;// 先給feature ID賦值-1
    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;

        // 求解逆深度
        it_per_id.estimated_depth = 1.0 / x(++feature_index);
        //ROS_INFO("feature id %d , start_frame %d, depth %f ", it_per_id->feature_id, it_per_id-> start_frame, it_per_id->estimated_depth);
        // 深度小於0估計失敗
        if (it_per_id.estimated_depth < 0)
        {
            it_per_id.solve_flag = 2;//失敗估計
        }
        else
            it_per_id.solve_flag = 1;//成功估計
    }
}

問題：

// 求解逆深度

it_per_id.estimated_depth = 1.0 / x(++feature_index);

最後乘上的(++feature_index)是做什麼用的？

trangulate()

對特徵點進行三角化求深度（SVD分解）

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幀相機座標系到世界座標系的變換矩陣Rwc
        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++;
            //R t爲第j幀相機座標系到第i幀相機座標系的變換矩陣，P爲i到j的變換矩陣
            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            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);
        }
        */
    }
}

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

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