VINS-mono 解析 新特徵

在17.12.29，VINS更新了代碼加入了新的特徵，包括map merge( 地圖合併), pose-graph reuse(位姿圖重利用), online temporal calibration function(在線時間校準函數), and support rolling shutter camera(支持捲簾快門相機)。

論文見https://arxiv.org/abs/1803.01549

1 map merge( 地圖合併)

map merge 最初是MapLab中出現的特徵，VINS 將這個特徵功能合並過來，並使用CLA數據集來測試。除了介紹頁給出的ETH CLA 數據集下載外，MapLab的數據介紹詳見Github網址。

在播放完MH_01_easy.bag後，繼續播放MH_02_easy.bag和MH_03_medium.bag, RVIZ可以得到如下合併的地圖。左側中間添加了迴環檢測匹配的圖像。

圖中有三個顏色不同的軌跡線綠色的是MH_01, 紅色軌跡是MH_02， 藍色軌跡是MH_03。MH_02和MH_01軌跡位置相近，因此迴環檢測比較多。這些地圖序列拼接，初始點的拼接輸出

[ WARN] [1520921503.762849522]: restart the estimator!
[ WARN] [1520921503.777266208]: image discontinue! detect a new sequence!
new sequence
sequence cnt 2 

    // detect unstable camera stream
    if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < 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;
    }

void image_callback(const sensor_msgs::ImageConstPtr &image_msg)
{
    //ROS_INFO("image_callback!");
    if(!LOOP_CLOSURE)
        return;
    m_buf.lock();
    image_buf.push(image_msg);
    m_buf.unlock();
    //printf(" image time %f \n", image_msg->header.stamp.toSec());

    // detect unstable camera stream
    if (last_image_time == -1)
        last_image_time = image_msg->header.stamp.toSec();
    else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || image_msg->header.stamp.toSec() < last_image_time)
    {
        ROS_WARN("image discontinue! detect a new sequence!");
        new_sequence();
    }
    last_image_time = image_msg->header.stamp.toSec();
}

從代碼上看，新的序列就是檢測新到達的圖像時間是否已經超過上一幀圖像時間1s，如果時間太長則重新初始化，重新構建地圖。

2 pose-graph reuse(位姿圖重利用)

首先修改對應的config.yaml，設置路徑 pose_graph_save_path 後，播放需要建圖的bag文件，播放完成在vins_estimator控制檯鍵入‘s’，當前包的所有位姿圖會存儲下來。

代碼

void PoseGraph::savePoseGraph()
{
    m_keyframelist.lock();
    TicToc tmp_t;
    FILE *pFile;
    printf("pose graph path: %s\n",POSE_GRAPH_SAVE_PATH.c_str());
    printf("pose graph saving... \n");
    string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt";
    pFile = fopen (file_path.c_str(),"w");
    //fprintf(pFile, "index time_stamp Tx Ty Tz Qw Qx Qy Qz loop_index loop_info\n");
    list<KeyFrame*>::iterator it;
    for (it = keyframelist.begin(); it != keyframelist.end(); it++)
    {
        std::string image_path, descriptor_path, brief_path, keypoints_path;
        if (DEBUG_IMAGE)
        {
            image_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_image.png";
            imwrite(image_path.c_str(), (*it)->image);
        }
        Quaterniond VIO_tmp_Q{(*it)->vio_R_w_i};
        Quaterniond PG_tmp_Q{(*it)->R_w_i};
        Vector3d VIO_tmp_T = (*it)->vio_T_w_i;
        Vector3d PG_tmp_T = (*it)->T_w_i;

        fprintf (pFile, " %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f %f %d\n",(*it)->index, (*it)->time_stamp, 
                                    VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(), 
                                    PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(), 
                                    VIO_tmp_Q.w(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(), 
                                    PG_tmp_Q.w(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(), 
                                    (*it)->loop_index, 
                                    (*it)->loop_info(0), (*it)->loop_info(1), (*it)->loop_info(2), (*it)->loop_info(3),
                                    (*it)->loop_info(4), (*it)->loop_info(5), (*it)->loop_info(6), (*it)->loop_info(7),
                                    (int)(*it)->keypoints.size());

        // write keypoints, brief_descriptors   vector<cv::KeyPoint> keypoints vector<BRIEF::bitset> brief_descriptors;
        assert((*it)->keypoints.size() == (*it)->brief_descriptors.size());
        brief_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_briefdes.dat";
        std::ofstream brief_file(brief_path, std::ios::binary);
        keypoints_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_keypoints.txt";
        FILE *keypoints_file;
        keypoints_file = fopen(keypoints_path.c_str(), "w");
        for (int i = 0; i < (int)(*it)->keypoints.size(); i++)
        {
            brief_file << (*it)->brief_descriptors[i] << endl;
            fprintf(keypoints_file, "%f %f %f %f\n", (*it)->keypoints[i].pt.x, (*it)->keypoints[i].pt.y, 
                                                     (*it)->keypoints_norm[i].pt.x, (*it)->keypoints_norm[i].pt.y);
        }
        brief_file.close();
        fclose(keypoints_file);
    }
    fclose(pFile);

    printf("save pose graph time: %f s\n", tmp_t.toc() / 1000);
    m_keyframelist.unlock();
}

從代碼中可見，存儲的是關鍵幀、關鍵幀上的特徵點，特徵點的brief描述子(二進制dat文件)。

將config.yaml中的load_previous_pose_graph修改爲1，然後重新運行vins_estimator，系統則會加載 result/pose_graph下的位姿圖， 新的序列會和之前的位姿圖相重合。

Terminal輸出的加載信息

load pose graph
lode pose graph from: /home/.../src/vins_mono/result/pose_graph/pose_graph.txt 
pose graph loading...
load pose graph time: 33.301555 s
load pose graph finish

pose_graph.txt 每一行讀取

    int index;
    double time_stamp;
    double VIO_Tx, VIO_Ty, VIO_Tz;
    double PG_Tx, PG_Ty, PG_Tz;
    double VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz;
    double PG_Qw, PG_Qx, PG_Qy, PG_Qz;
    double loop_info_0, loop_info_1, loop_info_2, loop_info_3;
    double loop_info_4, loop_info_5, loop_info_6, loop_info_7;
    int loop_index;
    int keypoints_num;

加載的位姿圖是黃色軌跡線

在重新播放數據包之後與地圖對準的結果如下圖

save_image選項設置爲0最好，節省內存。

3 online temporal calibration function(在線時間校準)

這項功能的目的是因爲大多數VI傳感器兩部分不是時間同步的，所以將estimate_td設置爲1可以在線估計攝像頭和慣性單元之間的時間差。

這項參數td在Estimator::optimization()裏放在圖像殘差函數中進行非線性優化。

            if (ESTIMATE_TD)
            {
                    ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity, it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td, it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                    problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
                    /*
                    double **para = new double *[5];
                    para[0] = para_Pose[imu_i];
                    para[1] = para_Pose[imu_j];
                    para[2] = para_Ex_Pose[0];
                    para[3] = para_Feature[feature_index];
                    para[4] = para_Td[0];
                    f_td->check(para);
                    */
            }
            else
            {
                ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
            }

將特徵點的速度和這個時間偏差考慮到視覺殘差函數即重投影誤差中

    pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;
    pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;

4 supporting rolling shutter (捲簾相機)

大部分單反相機都是捲簾相機，其拍攝視頻採用捲簾快門方式。這裏將rolling_shutter設置爲1，同樣需要從傳感器表得到讀取rolling_shutter_tr的值。不要使用網絡攝像頭webcam，不適合用。