VINS_estimator
VINS_estimator是VINS_Fusion的節點，其不包含回環檢測部分，該節點可以單獨對相機進行位姿估計。
在閱讀源碼之前，可以先了解一下VINS特征追蹤策略:VINS_Fusion 特征追蹤策略
文件樹目錄
guoben@guoben-WRT-WX9:~/Project/VIO/Source/VINS_VR/vins_estimator$ tree
.
├── cmake
│ └── FindEigen.cmake
├── CMakeLists.txt
├── launch
│ └── vins_rviz.launch
├── package.xml
└── src
├── estimator
│ ├── estimator.cpp
│ ├── estimator.h
│ ├── feature_manager.cpp
│ ├── feature_manager.h
│ ├── parameters.cpp
│ └── parameters.h
├── factor
│ ├── imu_factor.h
│ ├── initial_bias_factor.h
│ ├── initial_pose_factor.h
│ ├── integration_base.h
│ ├── marginalization_factor.cpp
│ ├── marginalization_factor.h
│ ├── pose_local_parameterization.cpp
│ ├── pose_local_parameterization.h
│ ├── projection_factor.cpp
│ ├── projection_factor.h
│ ├── projectionOneFrameTwoCamFactor.cpp
│ ├── projectionOneFrameTwoCamFactor.h
│ ├── projectionTwoFrameOneCamFactor.cpp
│ ├── projectionTwoFrameOneCamFactor.h
│ ├── projectionTwoFrameTwoCamFactor.cpp
│ └── projectionTwoFrameTwoCamFactor.h
├── featureTracker
│ ├── feature_tracker.cpp
│ └── feature_tracker.h
├── initial
│ ├── initial_aligment.cpp
│ ├── initial_alignment.h
│ ├── initial_ex_rotation.cpp
│ ├── initial_ex_rotation.h
│ ├── initial_sfm.cpp
│ ├── initial_sfm.h
│ ├── solve_5pts.cpp
│ └── solve_5pts.h
├── rosNodeTest.cpp
└── utility
├── CameraPoseVisualization.cpp
├── CameraPoseVisualization.h
├── tic_toc.h
├── utility.cpp
├── utility.h
├── visualization.cpp
└── visualization.h
8 directories, 44 files
rosNodeTest.cpp
rosNodeTest.cpp是vins_estimator節點的程序入口。主要實現以下函數。
主程序基本流程
-
讀取配置文件參數 readParameter()
-
訂閱了四個話題，分別是imu、雙目相機圖像以及feature_tracker所提供的跟蹤光流點，收到各個話題的消息后執行回調函數，對各個數據進行相應的處理
-
開啟一個新線程sync_process。
該線程的作用：若圖像buffer里面有數據,讀入數據並且添加到estimator中。利用圖片攜帶的時間戳信息能夠檢測兩圖片是否同步，若兩圖片的時間戳差距在一定范圍內，則添加到estimator中中，否則丟棄兩幀圖片。
主要成員
Estimator estimator; //位姿估計器
queue<sensor_msgs::ImuConstPtr> imu_buf; //IMU緩存buff
queue<sensor_msgs::PointCloudConstPtr> feature_buf; //特征點緩存buff
queue<sensor_msgs::ImageConstPtr> img0_buf; //圖像緩存buff
queue<sensor_msgs::ImageConstPtr> img1_buf; //圖像緩存buff
std::mutex m_buf; //互斥訪問信號
主要函數
| 函數 | 功能 | |
|---|---|---|
| 1 | void img0_callback(const sensor_msgs::ImageConstPtr &img_msg) | 存儲左目圖片信息 |
| 2 | void img1_callback(const sensor_msgs::ImageConstPtr &img_msg) | 存儲右目圖片信息 |
| 3 | cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg) | 從話題數據中提取圖像信息，返回圖像矩陣(拷貝) |
| 4 | void sync_process() | 從具有相同時間戳的話題中提取圖片 |
| 5 | void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) | 從IMU話題信息中提取數據保存到acc向量和gyr向量中，然后把{t,acc,gyr}保存到估計器中。 |
| 6 | void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg) | 獲取前端得到的點雲數據， |
| 7 | void restart_callback(const std_msgs::BoolConstPtr &restart_msg) | 如果有重啟信號輸入，則重新初始化進程；estimator.clearState();estimator.setParameter(); |
| 8 | void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg) | 用來切換狀態：是否使用IMU數據 |
| 9 | void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg) | 用來切換圖片信息是單目還是雙目的 |
注意事項：
- 超參數：雙目圖片之間的時間差設置在0.003s
estimator節點
VIO系統的整個程序從Estimator estimator開啟。
1. estimator
estimator類的定義由estimator.h和estimator.cpp兩個文件完成。
VINS_VR/vins_estimator/src/estimator$ tree.
├── estimator.cpp
├── estimator.h
├── feature_manager.cpp
├── feature_manager.h
├── parameters.cpp
└── parameters.h
主要成員
以下是該類中包含的幾組成員
| 類的主要成員 | 功能 | |
|---|---|---|
| 1 | queue<pair<double, Eigen::Vector3d>> accBuf; queue<pair<double, Eigen::Vector3d>> gyrBuf; queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf; |
加速度計數據緩沖區； 陀螺儀數據緩沖區 特征點（7維）數據緩沖區 |
| 2 | std::mutex mProcess; std::mutex mBuf; std::mutex mPropagate |
進程互斥信號 緩存空間互斥信號 傳播的互斥訪問信號 |
| 3 | double prevTime, curTime; | 前一幀時間，當前幀時間 |
| 4 | bool openExEstimation; | |
| 5 | std::thread trackThread; std::thread processThread; |
追蹤線程 處理線程 |
| 6 | FeatureTracker featureTracker; | 特征追蹤器 用來對原始圖像進行畸變校正，特征點采集，光流跟蹤 * vins-mono中該部分作為一個獨立Node存在 |
| 7 | SolverFlag solver_flag; MarginalizationFlag marginalization_flag; |
|
| 8 | Vector3d g; | 重力加速度在各個方向上的分量； 最開始由參數設置函數確定，在初始化過程中還要進行后續優化 |
| 9 | Matrix3d ric[2]; Vector3d tic[2]; |
|
| 10 | Vector3d Ps[(WINDOW_SIZE + 1)]; Vector3d Vs[(WINDOW_SIZE + 1)]; Matrix3d Rs[(WINDOW_SIZE + 1)]; Vector3d Bas[(WINDOW_SIZE + 1)]; Vector3d Bgs[(WINDOW_SIZE + 1)]; |
滑動窗口中的數據； 位置、速度、旋轉矩陣、加速度偏差、角速度偏差 |
| 11 | double dt; | 時間變化量 |
| 12 | Matrix3d back_R0, last_R, last_R0; Vector3d back_P0, last_P, last_P0; |
|
| 13 | double Headers[(WINDOW_SIZE + 1)]; | 窗口內所有幀的時間戳 |
| 14 | IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)]; | 滑動窗口里邊存放的imu預積分 |
| 15 | Vector3d acc_0, gyr_0; | IMU的加速度，陀螺儀初始值 |
| 16 | vector
vector vector |
滑動窗口 成員：dt（時間）、加速度數據、角速度數據 |
| 17 | int frame_count; int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid; int inputImageCnt; |
窗口內第幾幀，最大值位WINDOW+SIZE+1 外點個數，后點個數，前點個數，無效點個數 輸入圖片數目 |
| 18 | FeatureManager f_manager | 特征點管理器 用來對滑動窗口內所有特征點的管理。 |
| 19 | MotionEstimator m_estimator; | 運動估計器 |
| 20 | InitialEXRotation initial_ex_rotation; | 定義一個估計外部參數校准的對象 |
| 21 | bool first_imu; bool is_valid, is_key; bool failure_occur; |
該圖像之后的第一個imu FIXME: 沒有用到 檢測是否發生了錯誤,在failureDetection中 |
| 22 | vector
vector |
點雲 邊緣雲 |
| 23 | vector
double initial_timestamp; |
關鍵姿態 初始時間戳 |
| 24 | double para_Pose[WINDOW_SIZE + 1][SIZE_POSE];double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS];double para_Feature[NUM_OF_F][SIZE_FEATURE];double para_Ex_Pose[2][SIZE_POSE];double para_Retrive_Pose[SIZE_POSE]; double para_Td[1][1];double para_Tr[1][1]; |
參數:11*7, 滑窗內幀的位姿 滑窗內速度偏差 外參 pose |
| 25 | int loop_window_index; | |
| 26 | MarginalizationInfo *last_marginalization_info; | 上一時刻的先驗信息,也就是上一個H矩陣matg掉一部分后剩下的內容 |
| 27 | vector<double *> last_marginalization_parameter_blocks; | 最新的邊緣化參數塊 |
| 28 | map<double, ImageFrame> all_image_frame; | 圖像幀的哈希表<時間戳，圖像幀>； |
| 29 | IntegrationBase *tmp_pre_integration; | 輸入到圖像中的預積分值 |
| 30 | Eigen::Vector3d initP; Eigen::Matrix3d initR; |
初始化的位置 初始化的姿態 |
| 31 | double latest_time; Eigen::Vector3d latest_P, latest_V, latest_Ba, latest_Bg, latest_acc_0, latest_gyr_0; |
最新的系統狀態 |
| 32 | bool initFirstPoseFlag; bool initThreadFlag; |
IMU初始位姿的flag FIXME: 在哪里進行初始化的?線程是否進行初始化了 |
超參數
| 超參數 | 值 |
|---|---|
| WINDOW_SIZE | 10 |
| 初始的G | {0 0 ,9.8} |
estimator類中包含兩類成員函數：
類的必備函數
| 函數 | 功能 |
|---|---|
| Estimator(); | 構造函數; 將initThreadFlag設為flase；然后將估計器狀態清空（初始化）。 |
| ~Estimator(); | 析構函數 調用processsThread.join(),一直等待該線程死亡 |
| void setParameter(); | 參數設置函數 |
1. 接口函數
包括以下8個函數：
| 接口函數 | 功能 | |
|---|---|---|
| 1 | initFirstPose() | 初始化初始位姿； |
| 2 | inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity) | 輸入IMU數據 1.把IMU數據與時間戳一起輸入到加速度緩沖區和角速度緩沖區中 2.如果solver_flag是非線性的，執行快速預測IMU，輸出最新里程計數據； |
| 3 | inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1) |
輸入雙目圖片數據到緩沖區; |
| 4 | inputFeature(double t, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &featureFrame) | 輸入特征到緩沖區; |
| 5 | ProcessIMU() | 處理IMU數據，對IMU進行預積分； 如果是第一個IMU數據，則設置acc初始值、gyr初始值 如果幀數不是0的話，進行預計分計算，更新該幀的系統狀態。 |
| 6 | ProcessImage() | 處理相機數據； 1. 基於特征點的視差來判斷當前幀是否屬於關鍵幀； 2. 判斷相機到IMU的外參是否有校正，若無則用手眼標定法進行標定,具體在CalibrationExRotation里，此處只標定旋轉矩陣，未標定平移矩陣，原因是系統對旋轉矩陣較敏感，系統易因為小幅度的角度偏差而崩潰； 3. 判斷是否有進行初始化;若已完成初始化，則調用optimization( )，用ceres_solver對滑窗進行非線性優化的求解，優化項主要有四項：邊緣化殘差、 imu殘差、相機重投影殘差以及相機與Imu間同步時間差的殘差項。否則進行相應的初始化過程。 4. 本函數中包含一個failureDetection()函數,用於判斷系統在一定條件下是否崩潰，比如非線性求解器中的解有大跳動，求解出相機IMU的外參矩陣或IMU偏移等等，系統掛掉就清空狀態，重新初始化。 |
| 7 | ProcessMeasurements() | 處理測量值； 處理各buffer里的數據，當featureBuf不等於空時，開始進行以下處理（為什么是featureBuf，因為當有圖像buffer數據的時候，才會有featuretracker.push(make_pair(t,featureFrame))，即有圖像數據后，程序才發給跟蹤器叫他產生feature，因此當featureBuf不等於空，所有的buffer，包括imu,圖像，都不為空）： |
| 8 | void Estimator::changeSensorType(int use_imu, int use_stereo) | 改變傳感器類型，用於確定是否使用IMU，使用單目相機還是雙目相機. |
2. 內部函數
類的初始化函數 Estimator( ) ，由於Estimator類成員內部有兩個比較重要的自定義類成員：
（1）Feature_Tracker featuretracker;
（2）FeatureManager f_manager;
簡單設置了一些參數后，系統進入main()。
接着main()與Estimator estimator兩者開始發生聯系：
main()中estimator.setParameter()開啟了滑動窗口估計的一個新線程
由於我們在配置文件中 多線程MULTIPLE_THREAD設置為1，因此當setParameter()時候，就開啟了一個Estimator類內的新線程：processMeasurements();
pub VIO的各種話題，包括里程計信息，tf變換，相機姿態，點雲信息，並且發布關鍵幀。
| 內部函數 | 功能 | |
|---|---|---|
| 1 | void clearState() | 清空估計器狀態 主要需要做的操作有：清空buff緩沖區；設置時間、初始化位姿；把滑動窗口中的數據都清空（刪除預積分指針）；清空tic、ric；特征管理器。 |
| 2 | bool initialStructure() | 初始化結構，SFM 利用SFM進行初始化； 視覺的結構初始化，首先得到純視覺的,所有圖像在IMU坐標系下的,一個初始化結果,也就是RT然后進行視覺imu對其,陀螺儀偏執估計等等 如果IMU的方差小於0.25，則輸出IMU激勵過小（返回失敗）； |
| 3 | bool visualInitialAlign() | 視覺初始化對齊 1. 求取scale尺度 2. 把所有圖像幀都設置為關鍵幀 3.三角化 |
| 4 | bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l) | 相對位姿; 查找到與最新幀中包含足夠的對應點對和視差的關鍵幀 |
| 5 | void slideWindow() | 滑動窗口函數 |
| 6 | void slideWindowNew(); | 滑到倒數第二幀,作用主要是刪除特征點 |
| 7 | void slideWindowOld(); | 滑掉最老的那一幀,,作用主要是刪除特征點 |
| 8 | void optimization() | 后端優化函數 基於滑動窗口的緊耦合的非線性優化，殘差項的構造和求解 |
| 9 | void vector2double(); | 把滑動窗口向量轉為double數據； 主要包括：1. 位置；2、姿態；3、IMU速度4、加速度偏差；5、角速度偏差； |
| 10 | void double2vector(); | double轉向量 |
| 11 | bool failureDetection() | 系統失敗檢測；發生以下情況則認定為失敗 1. 檢測數目<2; 2. IMU的加速度偏差>2.5； 3. IMU的角速度偏差大於1.0； 4. 最新幀與上一幀的平移量P的模>5； 5. 最新幀與上一幀z值變化>1; 6. |
| 12 | bool getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, vector<pair<double, Eigen::Vector3d>> &gyrVector) | 對imu的時間進行判斷，將隊列里的imu數據放入到accVector和gyrVector中，完成之后返回true |
| 13 | void getPoseInWorldFrame ( Eigen::Matrix4d &T)； void getPoseInWorldFrame(int index, Eigen::Matrix4d &T) |
獲得世界坐標系下的位置 |
| 14 | ||
| 15 | void predictPtsInNextFrame() | |
| 16 | void outliersRejection(set
|
外點移除函數 |
| 17 | double reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici, | 計算重投影誤差； 輸入：第i與第j幀之間的像素點位置，第i幀像素點的深度，相機模型轉換矩陣；用head提取前兩個元素以計算殘差； |
| 18 | void updateLatestStates() | 更新估計器的最新狀態 主要是利用滑動窗口中的最近幀的數據來更新最新的time,P,Q,V,Ba,Bg,acc_0,gyr_0；tmp_accBuf,tmp_gyrBuf,acc,gyr;再次調用IMU的數據快速預測狀態； |
| 19 | void fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity) | 快速預測IMU； 利用IMU數據更新Latest_P，Latest_V，Latest_acc_0,Latest_gyr_0; |
| 20 | bool IMUAvailable(double t) | |
| 21 | void initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector) |
2. Initial初始化
文件樹目錄
├── initial_aligment.cpp
├── initial_alignment.h
├── initial_ex_rotation.cpp
├── initial_ex_rotation.h
├── initial_sfm.cpp
├── initial_sfm.h
├── solve_5pts.cpp
└── solve_5pts.h
初始化流程圖
從上面的流程圖中可以看出，初始化共分為兩大步，第一是純視覺初始化，第二是視覺慣性聯合初始化。另外，如果沒有提供相機和IMU之間的外參，VINS-Mono還提供了相應的標定程序。因此我們梳理代碼就按照這三部分來梳理。
首先看文件結構，初始化流程相關的代碼在vins_estimator/src/estimator.cpp中，具體講就是initialStructure這個函數，它實現初始化所需要的所有既定步驟。其實就是下面這張圖里的的功能和函數調用
| 文件 | 作用 |
|---|---|
| initial_alignment.h | IMU與視覺信息的聯合初始化 |
| initial_ex_rotation.h | 外參旋轉初始化 |
| initial_sfm.h | 初始化SFM |
| solve_5pts.h | 5點法求解 |
各文件中的函數
| 函數 | 功能 | |
|---|---|---|
| 1 | void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs); | |
| 1 | bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x); |
3. FeatureTracker
主要作用跟蹤特征並三角化恢復深度以后發給后端去優化。
FeatureTracker 里邊存放的是用於特征跟蹤的類
文件樹目錄
├── feature_tracker.cpp
└── feature_tracker.h
特征追蹤流程圖
| 類成員 | 功能 |
|---|---|
| int row, col; | 圖片的寬、長； |
| cv::Mat imTrack; | 當前追蹤的圖像； |
| cv::Mat mask; | 用於標記點的圖像； |
| cv::Mat fisheye_mask; | 魚眼mask； |
| cv::Mat prev_img, cur_img; | 先前和現在的圖像；雙目中，特指左目 |
vector<cv::Point2f> n_pts; |
從圖片上返回的特征，shi-tomasi角點（Harris角點） |
vector<cv::Point2f> predict_pts; |
|
vector<cv::Point2f> predict_pts_debug; |
etPrediction生成的，暫時不知道作用 |
vector<cv::Point2f> prev_pts, cur_pts, cur_right_pts; |
cur_pts當前幀上的特征點，雙目中的左目，並且應該像素坐標 |
vector<cv::Point2f> pts_velocity, right_pts_velocity; |
像素移動速度 |
| vector
|
ids這個好像就是當前幀特征點數目的索引 |
| vector
|
保存了當前追蹤到的角點一共被多少幀圖像追蹤到 |
| map<int, cv::Point2f> cur_un_pts_map, prev_un_pts_map; | cur_un_pts_map中存放ids[i]和cur_un_pts[i]構成的鍵值對。 |
| map<int, cv::Point2f> cur_un_right_pts_map, prev_un_right_pts_map; | 當前右目上的點 |
| map<int, cv::Point2f> prevLeftPtsMap; | 上一幀的左目中的點 |
vector<camodocal::CameraPtr> m_camera; |
相機類，雙目的話有兩個 |
| double cur_time; double prev_time; |
當前幀的時間； 上一幀的時間； |
| bool stereo_cam; | 是否是雙目相機 |
| int n_id; | |
| bool hasPrediction; |
| 函數 | 功能 |
|---|---|
| FeatureTracker(); | 構造函數 |
| map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat()); | 追蹤圖片 對圖片進行了一系列操作,返回特征點featureFrame.執行的操作包括： 圖像處理、區域mask、檢測特征點、計算像素速度等 |
| void setMask() | 對跟蹤點進行排序並依次選點，設置mask：去掉密集點，使特征點分布均勻。 |
| void readIntrinsicParameter(const vector
|
讀取內參。生成相機類m_camera |
| void showUndistortion(const string &name); | |
| void rejectWithF(); | 使用F矩陣進行拒絕，刪除一些點 涉及到FM_RANSAC, 拒絕的是一些光流跟蹤錯誤的點, /TODO: 這里可能涉及到動態檢測 首先將將圖像坐標畸變矯正后轉換為深度歸一化坐標，通過cv::findFundamentalMat() 計算F矩陣，利用得到的status通過reduceVector()去除outliers。 |
| void undistortedPoints(); | 將像素座標系下的座標，轉換為歸一化相機座標系下的座標 即un_pts為歸一化相機座標系下的座標。 |
vector<cv::Point2f> undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam); |
將像素座標系下的坐標，轉換為歸一化相機座標系下的坐標 即un_pts為歸一化相機座標系下的座標。 |
vector<cv::Point2f> ptsVelocity(vector
<cv::Point2f> &pts, map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts);
|
其為當前幀相對於前一幀 特征點沿x,y方向的像素移動速度 |
void showTwoImage(const cv::Mat &img1, const cv::Mat &img2, vector<cv::Point2f> pts1, vector<cv::Point2f> pts2); |
|
void drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight, vector
<cv::Point2f> &curLeftPts, vector
<cv::Point2f> &curRightPts,map<int, cv::Point2f> &prevLeftPtsMap);
|
//在imTrack圖像上畫出特征點 |
| void setPrediction(map<int, Eigen::Vector3d> &predictPts); | 把上一幀3d點預測到歸一化平面， 預測方法：：好像就是直接把3D點投影下來。 調用了四個函數 m_camera[0]->spaceToPlane(itPredict->second, tmp_uv); |
| double distance(cv::Point2f &pt1, cv::Point2f &pt2); | 計算兩點之間的距離 |
| void removeOutliers(set
|
移除外點，代碼很容易懂 |
| cv::Mat getTrackImage(); | 直接返回ImTrack，imTrack是在drawTrack函數中畫出來的 |
| bool inBorder(const cv::Point2f &pt); | 返回是否在圖像邊界內 |
重要的函數
readImage( const cv::Mat &_img, //圖像double _cur_time) //時間戳
它位於feature_tracker.cpp文件中，是整個特征跟蹤功能的全部，其他函數主要是為了幫助他實現功能，是它的子函數
它的主要流程包括：
1）createCLAHE() 判斷並對圖像進行自適應直方圖均衡化；
2）calcOpticalFlowPyrLK() 從cur_pts到forw_pts做LK金字塔光流法；
3）根據status，把跟蹤失敗的和位於圖像邊界外的點剔除，剔除時不僅要從當前幀數據forw_pts中剔除，而且還要從cur_un_pts、prev_pts、cur_pts，記錄特征點id的ids，和記錄特征點被跟蹤次數的track_cnt中剔除；
4）setMask() 對跟蹤點進行排序並依次選點，設置mask：去掉密集點，使特征點分布均勻
5）rejectWithF() 通過基本矩陣F剔除outliers
6）goodFeaturesToTrack() 尋找新的特征點(shi-tomasi角點)，添加(MAX_CNT - forw_pts.size())個點以確保每幀都有足夠的特征點
7）addPoints()向forw_pts添加新的追蹤點，id初始化-1，track_cnt初始化為1
8）undistortedPoints() 對特征點的圖像坐標根據不同的相機模型進行去畸變矯正和深度歸一化，計算每個角點的速度
4. factor
├── imu_factor.h
├── initial_bias_factor.h
├── initial_pose_factor.h
├── integration_base.h
├── marginalization_factor.cpp
├── marginalization_factor.h
├── pose_local_parameterization.cpp
├── pose_local_parameterization.h
├── projection_factor.cpp
├── projection_factor.h
├── projectionOneFrameTwoCamFactor.cpp
├── projectionOneFrameTwoCamFactor.h
├── projectionTwoFrameOneCamFactor.cpp
├── projectionTwoFrameOneCamFactor.h
├── projectionTwoFrameTwoCamFactor.cpp
└── projectionTwoFrameTwoCamFactor.h
5. utility
文件樹目錄
guoben@guoben-WRT-WX9: ~/vins_estimator/src/utility$ tree
.
├── CameraPoseVisualization.cpp
├── CameraPoseVisualization.h
├── tic_toc.h
├── utility.cpp
├── utility.h
├── visualization.cpp
└── visualization.h
0 directories, 7 files
| 文件 | 功能 |
|---|---|
| CameraPoseVisualization.h | 包含了相機位資可視化函數 |
| TicToc.h | 該庫主要用於計算時間 |
| utility.h | 重新定義了Eigen里面四元數的一些計算函數 |
| visualization.h | 把各種數據發送到ROS系統中以進行可視化 |