文章目錄
0 前情回顧
上一個博客講到了主程序rosNodeTest.cpp。在程序最後，會進入sync_process線程進行處理。本篇博客接着進行講解。
本次工作
我首先一步步的把代碼全部註釋了，十分的詳細，對於C++和OpenCV的一些操作也進行了詳細的註釋，對於剛入門的同學應該還是有幫助的。之後我將代碼開源，並寫了相應的博客進行講解。
開源程序:
https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git
相應博客：
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(1)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(2)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(3)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(4)
1 sync_process
本程序的作用，判斷是否雙目，雙目的話判斷時間是否同步，之後講圖像image（單目），或者image0和image1通過inputImage輸入到estimator中。
inputImage
// 給Estimator輸入圖像
// 其實是給featureTracker.trackImage輸入圖像，之後返回圖像特徵featureFrame。填充featureBuf
// 之後執行processMeasurements
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
首先設置參數，並開啓processMeasurements線程
setParameter();
然後追蹤圖像上的特徵。trackImage，之後會進行詳解,其中得到了featureFrame
if(_img1.empty())
featureFrame = featureTracker.trackImage(t, _img);// 追蹤單目
else
featureFrame = featureTracker.trackImage(t, _img, _img1);// 追蹤雙目
然後，getTrackImage對特徵到跟蹤的圖像進行一些處理。並把追蹤的圖片imgTrack發佈出去.
if (SHOW_TRACK)//這個應該是展示軌跡
{
cv::Mat imgTrack = featureTracker.getTrackImage();
pubTrackImage(imgTrack, t);
}
然後，填充featureBuf
最後執行processMeasurements，之後會進行詳細講解
2、trackImage
得到featureFrame
// 對圖片進行一系列操作，返回特徵點featureFrame。
// 其中還包含了：圖像處理、區域mask、檢測特徵點、計算像素速度等
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
2.1 圖像處理
可以添加圖像處理的部分，比如直方圖均衡等等方法。
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//createCLAHE 直方圖均衡
clahe->apply(cur_img, cur_img);
if(!rightImg.empty())
clahe->apply(rightImg, rightImg);
}
2.2 hasPrediction
會對上一陣的點進行預測。但是具體是什麼作用還不是很清楚
2.3 if(SHOW_TRACK)
drawTrack 畫出追蹤情況，就是在圖像上的特徵點位置出畫圈圈，如果是雙目的話就連線。
//在imTrack圖像上畫出特徵點
void FeatureTracker::drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight,
vector<int> &curLeftIds,
vector<cv::Point2f> &curLeftPts,
vector<cv::Point2f> &curRightPts,
map<int, cv::Point2f> &prevLeftPtsMap)
2.4 setMask
在已跟蹤到角點的位置上，將mask對應位置上設爲0,
意爲在cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
進行操作時在該點不再重複進行角點檢測，這樣可以使角點分佈更加均勻
具體詳情見開源的註釋代碼。
// 把追蹤到的點進行標記
// 設置遮擋部分（魚眼相機）
// 對檢測到的特徵點按追蹤到的次數排序
// 在mask圖像中將追蹤到點的地方設置爲0，否則爲255，目的是爲了下面做特徵點檢測的時候可以選擇沒有特徵點的區域進行檢測。
// 在同一區域內，追蹤到次數最多的點會被保留，其他的點會被刪除
void FeatureTracker::setMask()
2.5 goodFeaturesToTrack
如果當前圖像的特徵點cur_pts數目小於規定的最大特徵點數目MAX_CNT，則進行提取。
提取使用的cv::goodFeaturesToTrack。將點保存到n_pts
/* goodFeaturesToTrack
_image：8位或32位浮點型輸入圖像，單通道
_corners：保存檢測出的角點
maxCorners：角點數目最大值，如果實際檢測的角點超過此值，則只返回前maxCorners個強角點
qualityLevel：角點的品質因子
minDistance：對於初選出的角點而言，如果在其周圍minDistance範圍內存在其他更強角點，則將此角點刪除
_mask：指定感興趣區，如不需在整幅圖上尋找角點，則用此參數指定ROI
blockSize：計算協方差矩陣時的窗口大小
useHarrisDetector：指示是否使用Harris角點檢測，如不指定，則計算shi-tomasi角點
harrisK：Harris角點檢測需要的k值 */
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
// mask 這裏肯定是指定感興趣區，如不需在整幅圖上尋找角點，則用此參數指定ROI
之後將n_pts保存到cur_pts之中
2.3.1 undistortedPts
將像素座標系下的座標，轉換爲歸一化相機座標系下的座標 即un_pts爲歸一化相機座標系下的座標。
// 將像素座標系下的座標，轉換爲歸一化相機座標系下的座標 即un_pts爲歸一化相機座標系下的座標。
vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)
cam->liftProjective(a, b);
這個函數是對魚眼相機模型的標定及去畸變過程
/**
* \brief Lifts a point from the image plane to its projective ray
* \param p image coordinates
* \param P coordinates of the projective ray
* 這個函數是對魚眼相機模型的標定及去畸變過程
*/
void
PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
2.3.2 ptsVelocity
計算當前幀相對於前一幀 特徵點沿x,y方向的像素移動速度
// 其爲當前幀相對於前一幀 特徵點沿x,y方向的像素移動速度
vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts,
map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)
2.4 如果是雙目
如果是雙目相機，那麼在右目上追蹤左目的特徵點。使用的函數是calcOpticalFlowPyrLK
/*光流跟蹤是在左右兩幅圖像之間進行cur left ---- cur right
prevImg 第一幅8位輸入圖像 或 由buildOpticalFlowPyramid()構造的金字塔。
nextImg 第二幅與preImg大小和類型相同的輸入圖像或金字塔。
prevPts 光流法需要找到的二維點的vector。點座標必須是單精度浮點數。
nextPts 可以作爲輸入，也可以作爲輸出。包含輸入特徵在第二幅圖像中計算出的新位置的二維點（單精度浮點座標）的輸出vector。當使用OPTFLOW_USE_INITIAL_FLOW 標誌時，nextPts的vector必須與input的大小相同。
status 輸出狀態vector(類型：unsigned chars)。如果找到了對應特徵的流，則將向量的每個元素設置爲1；否則，置0。
err 誤差輸出vector。vector的每個元素被設置爲對應特徵的誤差，可以在flags參數中設置誤差度量的類型；如果沒有找到流，則未定義誤差（使用status參數來查找此類情況）。
winSize 每級金字塔的搜索窗口大小。
maxLevel 基於最大金字塔層次數。如果設置爲0，則不使用金字塔（單級）；如果設置爲1，則使用兩個級別，等等。如果金字塔被傳遞到input，那麼算法使用的級別與金字塔同級別但不大於MaxLevel。
criteria 指定迭代搜索算法的終止準則（在指定的最大迭代次數標準值（criteria.maxCount）之後，或者當搜索窗口移動小於criteria.epsilon。）
flags 操作標誌，可選參數：
OPTFLOW_USE_INITIAL_FLOW：使用初始估計，存儲在nextPts中；如果未設置標誌，則將prevPts複製到nextPts並被視爲初始估計。
OPTFLOW_LK_GET_MIN_EIGENVALS：使用最小本徵值作爲誤差度量（見minEigThreshold描述）；如果未設置標誌，則將原始周圍的一小部分和移動的點之間的 L1 距離除以窗口中的像素數，作爲誤差度量。
minEigThreshold
算法所計算的光流方程的2x2標準矩陣的最小本徵值（該矩陣稱爲[Bouguet00]中的空間梯度矩陣）÷ 窗口中的像素數。如果該值小於MinEigThreshold，則過濾掉相應的特徵，相應的流也不進行處理。因此可以移除不好的點並提升性能。 */
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
if(FLOW_BACK)
如果這個打開，就想前邊的左右目圖像的位置換一下，在進行一次特徵跟蹤，目的是反向跟蹤，得到左右目都匹配到的點
cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
之後undistortedPts ptsVelocity
2.6 製作featureFrame
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
// 數據格式爲feature_id camera_id（0或1） xyz_uv_velocity（空間座標，像素座標和像素速度）
其中，camera_id = 0爲左目上的點，camera_id = 1，爲右目上的點。
3 processMeasurements
這是處理全部量測的線程,IMU的預積分,特徵點的處理等等都在這裏進行.
3.1 對imu的處理
3.1.1 判斷IMU數據是否可用
if ((!USE_IMU || IMUAvailable(feature.first + td)))//如果不用imu或者
其中
// 判斷輸入的時間t時候的imu是否可用
bool Estimator::IMUAvailable(double t)
3.1.2 獲得accVector和gyrVector
對imu的時間進行判斷，講隊列裏的imu數據放入到accVector和gyrVector中，
// 對imu的時間進行判斷，講隊列裏的imu數據放入到accVector和gyrVector中，完成之後返回true
bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector,
vector<pair<double, Eigen::Vector3d>> &gyrVector)
3.1.3 初始化IMU的姿態
initFirstIMUPose,其實很簡單,就是求一個姿態角,然後把航向角設爲0
//初始第一個imu位姿
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)
3.1.4 處理IMU數據,運行processIMU
/* 對imu計算預積分
傳進來的是一個imu數據 得到預積分值pre_integrations 還有一個tmp_pre_integration */
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
其中frame_count是值窗內的第幾幀圖像
下邊是新建一個預積分項目u
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
預積分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
// push_back進行了重載，的時候就已經進行了預積分
其中的push_back
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr);
}
其中的propagate
// IMU預積分傳播方程
// 積分計算兩個關鍵幀之間IMU測量的變化量
// 同時維護更新預積分的Jacobian和Covariance,計算優化時必要的參數
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
其中的midPointIntegration.這裏邊就涉及到了IMU的傳播方針和協方差矩陣.雅克比矩陣等等.哪裏不懂可以VIO的理論知識.
【泡泡讀者來稿】VINS 論文推導及代碼解析（一）
【泡泡讀者來稿】VINS 論文推導及代碼解析（二）
【泡泡讀者來稿】VINS 論文推導及代碼解析（三）
【泡泡讀者來稿】VINS 論文推導及代碼解析（四）
// 中值積分遞推Jacobian和Covariance
// _acc_0上次測量加速度 _acc_1本次測量加速度 delta_p上一次的位移 result_delta_p位置變化量計算結果 update_jacobian是否更新雅克比基本方法就涉及到了IMU的創博方針和器方差矩陣的窗哦sdf
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
之後計算對應絕對座標系下的位置等
// Rs Ps Vs是frame_count這一個圖像幀開始的預積分值,是在絕對座標系下的.
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//移除了偏執的加速度
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];//移除了偏執的gyro
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
3.2 對圖像的處理
請見下一博客