Estimator類
目錄
getCorresponding()函數返回兩幀匹配特徵點3D座標
solveRelativeRT()函數 利用五點法求解相機初始位姿
GlobalSFM::construct () 最重要的函數！！！堅持就是勝利
processImage()函數
1.檢查兩幀的視差判斷是否爲關鍵幀f_manager.addFeatureCheckParallax（）在 VINS-Mono 代碼詳細解讀——feature_manager.cpp中已經解釋
2.IMU預積分 IntegrationBase 在 VINS-Mono 代碼詳細解讀——IMU離散中值預積分 中解釋
3.在線標定外參 CalibrationExRotation 在 VINS-Mono 代碼詳細解讀——基礎儲備：在線Cam到IMU的外參標定 InitialEXRotation類 中已經解釋
本節將重點介紹初始化部分。
/**
* @brief 處理圖像特徵數據
* @Description addFeatureCheckParallax()添加特徵點到feature中，計算點跟蹤的次數和視差，判斷是否是關鍵幀
* 判斷並進行外參標定
* 進行視覺慣性聯合初始化或基於滑動窗口非線性優化的緊耦合VIO
* @param[in] image 某幀所有特徵點的[camera_id,[x,y,z,u,v,vx,vy]]s構成的map,索引爲feature_id
* @param[in] header 某幀圖像的頭信息
* @return void
*/
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
ROS_DEBUG("new image coming ------------------------------------------");
ROS_DEBUG("Adding feature points %lu", image.size());
// 1. 通過檢測兩幀之間的視差決定次新幀是否作爲關鍵幀
if (f_manager.addFeatureCheckParallax(frame_count, image, td))//添加之前檢測到的特徵點到feature容器中，計算每一個點跟蹤的次數，以及它的視差
marginalization_flag = MARGIN_OLD;//=0
else
marginalization_flag = MARGIN_SECOND_NEW;//=1
ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
ROS_DEBUG("Solving %d", frame_count);
ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());
Headers[frame_count] = header;
// 2. 填充imageframe的容器以及更新臨時預積分初始值
ImageFrame imageframe(image, header.stamp.toSec());//ImageFrame類包括特徵點、時間、位姿Rt、預積分對象pre_integration、是否關鍵幀
imageframe.pre_integration = tmp_pre_integration;// tmp_pre_integration是之前IMU 預積分計算的
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));// map<double, ImageFrame> all_image_frame;
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};//更新臨時預積分初始值
// 3. 如果沒有外參則標定IMU到相機的外參
if(ESTIMATE_EXTRINSIC == 2)
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
//得到兩幀之間歸一化特徵點
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
//標定從camera到IMU之間的外參數
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ROS_WARN("initial extrinsic rotation calib success");
ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;// 完成外參標定
}
}
}
// 4. 初始化
if (solver_flag == INITIAL)//初始化
{
//frame_count是滑動窗口中圖像幀的數量，一開始初始化爲0，滑動窗口總幀數WINDOW_SIZE=10
//確保有足夠的frame參與初始化
if (frame_count == WINDOW_SIZE)
{
bool result = false;
//有外參且當前幀時間戳大於初始化時間戳0.1秒，就進行初始化操作
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
// 4.1 重要！！！ 視覺慣性聯合初始化
result = initialStructure();
// 4.2 更新初始化時間戳
initial_timestamp = header.stamp.toSec();
}
if(result)//初始化成功
{
// 先進行一次滑動窗口非線性優化，得到當前幀與第一幀的位姿
solver_flag = NON_LINEAR;// 初始化更改爲非線性
solveOdometry(); // 4.3 非線性化求解里程計
slideWindow();// 4.4 滑動窗
f_manager.removeFailures();// 4.5 剔除feature中估計失敗的點（solve_flag == 2）0 haven't solve yet; 1 solve succ; 2 solve fail;
ROS_INFO("Initialization finish!");
// 4.6 初始化窗口中PVQ
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();// 初始化失敗則直接滑動窗口
}
else
frame_count++;// 圖像幀數量+1
}
else// 5. 緊耦合的非線性優化
{
TicToc t_solve;
solveOdometry();// 5.1 非線性化求解里程計
ROS_DEBUG("solver costs: %fms", t_solve.toc());
//5.2 故障檢測與恢復,一旦檢測到故障，系統將切換回初始化階段
if (failureDetection())
{
ROS_WARN("failure detection!");
failure_occur = 1;
clearState();// 清空狀態
setParameter();// 重設參數
ROS_WARN("system reboot!");
return;
}
TicToc t_margin;
slideWindow(); // 5.3 滑動窗口
f_manager.removeFailures();// 5.4 移除失敗的
ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
// prepare output of VINS
key_poses.clear();
for (int i = 0; i <= WINDOW_SIZE; i++)
key_poses.push_back(Ps[i]);// 關鍵位姿的位置 Ps
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
}
initialStructure()初始化函數
視覺結構初始化過程至關重要，多傳感器融合過程中，當單個傳感器數據不確定性較高，需要依賴其他傳感器降低不確定性。先對純視覺SFM初始化相機位姿，再和IMU對齊。
主要分爲1、純視覺SFM估計滑動窗內相機位姿和路標點逆深度。2、視覺慣性聯合校準，SFM與IMU積分對齊。
代碼部分
首先純視覺SFM初始化sfm.construct()函數，之後視覺慣性聯合初始化visualInitialAlign()函數。
視覺初始化入口在
bool Estimator::initialStructure()
初始化變量
Quaterniond Q[frame_count + 1];//旋轉四元數Q
Vector3d T[frame_count + 1]; // 平移矩陣T
map<int, Vector3d> sfm_tracked_points; //存儲SFM重建出特徵點的座標
vector<SFMFeature> sfm_f;// SFMFeature三角化狀態、特徵點索引、平面觀測、位置座標、深度
首先定義視覺SFM最重要的幾個變量，旋轉Q和平移T，map容器sfm_tracked_points保存SFM重建出的路標3D座標，最重要的是SFMFeature類型的vector容器sfm_f。SFMFeature數據結構爲：
// SFM 數據結構
struct SFMFeature
{
bool state;//特徵點的狀態（是否被三角化）
int id;//特徵點ID
vector<pair<int,Vector2d>> observation;//所有觀測到該特徵點的圖像幀ID和 2D像素座標
double position[3];//路標3D座標
double depth;//深度
};
SFM初始化
1、通過加速度標準差保證IMU有充分運動激勵，可以初始化
// 1. 通過加速度標準差判斷IMU是否有充分運動以初始化。
{
map<double, ImageFrame>::iterator frame_it;// 迭代器
// 1.1 先求均值 aver_g
Vector3d sum_g;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
sum_g += tmp_g;
}
Vector3d aver_g;
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);// 總幀數減1，因爲all_image_frame第一幀不算
// 1.2 再求標準差var,表示線加速度的標準差
double var = 0;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
//cout << "frame g " << tmp_g.transpose() << endl;
}
var = sqrt(var / ((int)all_image_frame.size() - 1));
//ROS_WARN("IMU variation %f!", var);
// 判斷，加速度標準差大於0.25則代表imu充分激勵，足夠初始化
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
}
}
求解標準差的過程需要先求解均值，再求每個值和均值的差，最後需要判斷加速度標準差大於0.25即可滿足imu充分激勵，可以初始化。
ImageFrame圖像幀的數據結構爲，就是頭文件initial_alignment.h全部內容。
image類型爲：
class ImageFrame
{
public:
ImageFrame(){};
ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>& _points, double _t):t{_t},is_key_frame{false}
{
points = _points;
};
// 圖像特徵點points，map第一索引是camera_id，內部第二層索引是feaure_id
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>> > > points;
double t;// ？？？
Matrix3d R;// 相機位姿
Vector3d T;
IntegrationBase *pre_integration;// IMU預積分
bool is_key_frame;// 是否關鍵幀
};
IntergrationBase積分類聲明爲：
class IntegrationBase
{
public:
IntegrationBase() = delete;
// 預積分類：加速度計、陀螺儀、線性加速度計ba、陀螺儀bg、雅克比矩陣初始化、協方差矩陣15*15、dt、PVQ
IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
: acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}
{
noise = Eigen::Matrix<double, 18, 18>::Zero();
noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
}
all_image_frame類的聲明爲：
map<double, ImageFrame> all_image_frame;
2、將f_manager中的所有feature保存到存有SFMFeature對象的sfm_f中
for (auto &it_per_id : f_manager.feature)// list<FeaturePerId> feature滑動窗口內所有角點
{
int imu_j = it_per_id.start_frame - 1;// 第一次觀測到特徵點的幀數-1
SFMFeature tmp_feature;
tmp_feature.state = false;// 狀態state
tmp_feature.id = it_per_id.feature_id;// 特徵點ID
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Vector3d pts_j = it_per_frame.point;// 3D特徵點座標
// 所有觀測到該特徵點的圖像幀ID和圖像座標
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
}
sfm_f.push_back(tmp_feature);
}
對SFMFeature類的數據結構進行填充，state、id、observation、position、depth，最終填充到sfm_f中，表示每個特徵點對應的所有被觀測數據。將特徵管理器中的特徵信息保存到SFMFeature對象sfm_f中sfm_f.push_back(tmp_feature)3、返回滑動窗口中第一個滿足視差的幀，爲l幀，以及RT,可以三角化。
f_manager是特徵管理器類的對象，具體在上一節feature_manager.cpp文章中
FeatureManager f_manager;//特徵管理器類
relativePose()函數
if (!relativePose(relative_R, relative_T, l))
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
/**
* @brief 返回滑動窗口中第一個滿足視差的幀，爲l幀，以及RT,可以三角化。
* @Description 判斷每幀到窗口最後一幀對應特徵點的平均視差是否大於30
solveRelativeRT()通過基礎矩陣計算當前幀與第l幀之間的R和T,並判斷內點數目是否足夠
* @param[out] relative_R 當前幀到第l幀之間的旋轉矩陣R
* @param[out] relative_T 當前幀到第l幀之間的平移向量T
* @param[out] L 從第一幀開始到滑動窗口中第一個滿足視差足夠的幀，這裏的l幀之後作爲參考幀做全局SFM用
* @return bool 1:可以進行初始化;0:不滿足初始化條件
*/
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
for (int i = 0; i < WINDOW_SIZE; i++)
{
// 尋找第i幀到窗口最後一幀的對應特徵點
vector<pair<Vector3d, Vector3d>> corres;
corres = f_manager.getCorresponding(i, WINDOW_SIZE);// 先得到第i幀和最後一幀的特徵匹配
if (corres.size() > 20) // 1. 首先corres數目足夠
{
// 2. 計算平均視差>30
double sum_parallax = 0;
double average_parallax;
for (int j = 0; j < int(corres.size()); j++)
{
//第j個對應點在第i幀和最後一幀的(x,y)
Vector2d pts_0(corres[j].first(0), corres[j].first(1));
Vector2d pts_1(corres[j].second(0), corres[j].second(1));
double parallax = (pts_0 - pts_1).norm();
sum_parallax = sum_parallax + parallax;
}
average_parallax = 1.0 * sum_parallax / int(corres.size());
//判斷是否滿足初始化條件：視差>30和內點數滿足要求
//同時返回窗口最後一幀（當前幀）到第l幀（參考幀）的Rt
if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
{
l = i;
ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
return true;
}
}
}
return false;
}
主要目的是找到滑動窗口中第一個與最後一幀具有足夠的共視特徵以及視差的幀的索引 l，具體方式爲在滑動窗內從第一幀開始計算每一幀和最後一幀匹配的特徵點corres = f_manager.getCorresponding(i, WINDOW_SIZE);。找到的第l幀作爲參考幀，並通過solveRelativeRT(corres, relative_R, relative_T)計算當前幀和最後一幀的相對位姿。這裏的R,T是在最後一幀相對於 l 幀座標系的位姿估計。
getCorresponding()函數返回兩幀匹配特徵點3D座標
//得到frame_count_l與frame_count_r兩幀之間的對應特徵點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;
}
又是通過feature的list容器，如果某個特徵點能被觀測到的第一幀和最後一幀範圍大，[start_frame,endFrame()]是個大範圍，而滑動窗 [i，WINDOW]被包含進去了，那麼可以直接獲取特徵點的3D座標。
for (auto &it : feature){
Vector3d a = it.feature_per_frame[idx_l].point;
}
solveRelativeRT()函數 利用五點法求解相機初始位姿
- cv::findFundamentalMat() 利用opencv函數求解F矩陣
- cv::recoverPose() 利用opencv函數分解F矩陣，得到相機旋轉量、位移量
/**
* @brief 通過求解本質矩陣得到R,t
* @Description findFundamentalMat()採用RANSAC算法求解本質矩陣E
* recoverPose()通過本質矩陣得到Rt
求Rt的對稱變換，判斷內點數大於12
* @param[in] corres 對應特徵點對
* @param[out] Rotation 當前幀到參考幀的旋轉矩陣
* @param[out] Translation 當前幀到參考幀的平移向量
* @return bool true:內點數大於12
*/
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15)
{
vector<cv::Point2f> ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
cv::Mat mask;
/**
* Mat cv::findFundamentalMat( 返回通過RANSAC算法求解兩幅圖像之間的本質矩陣E
* nputArray points1, 第一幅圖像點的數組
* InputArray points2, 第二幅圖像點的數組
* int method = FM_RANSAC, RANSAC 算法
* double param1 = 3., 點到對極線的最大距離，超過這個值的點將被捨棄
* double param2 = 0.99, 矩陣正確的可信度
* OutputArray mask = noArray() 在計算過程中沒有被捨棄的點
* )
*/
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
cv::Mat rot, trans;
/**
* int cv::recoverPose ( 通過本質矩陣得到Rt，返回通過手性校驗的內點個數
* InputArray E, 本質矩陣
* InputArray points1, 第一幅圖像點的數組
* InputArray points2, 第二幅圖像點的數組
* InputArray cameraMatrix, 相機內參
* OutputArray R, 第一幀座標系到第二幀座標系的旋轉矩陣
* OutputArray t, 第一幀座標系到第二幀座標系的平移向量
* InputOutputArray mask = noArray() 在findFundamentalMat()中沒有被捨棄的點
* )
*/
int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
//cout << "inlier_cnt " << inlier_cnt << endl;
//rr=R*ll+t ll=(R^T)*rr-(R^T)*t
Eigen::Matrix3d R;
Eigen::Vector3d T;
for (int i = 0; i < 3; i++)
{
T(i) = trans.at<double>(i, 0);
for (int j = 0; j < 3; j++)
R(i, j) = rot.at<double>(i, j);
}
//得到窗口最後一幀（當前幀）到第l幀（參考幀）的座標系變換Rt
Rotation = R.transpose();
Translation = -R.transpose() * T;
if(inlier_cnt > 12)
return true;
else
return false;
}
return false;
}
4、對窗口中每個圖像幀求解sfm問題
// 4. 對窗口中每個圖像幀求解sfm問題
// 得到所有圖像幀相對於參考幀l的姿態四元數Q、平移向量T和特徵點座標sfm_tracked_points。
GlobalSFM sfm;
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
// 求解失敗則邊緣化最早一幀並滑動窗口
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
}
裏面用到了GlobalSFM類定義爲：intial_sfm.h
/ SFM類
class GlobalSFM
{
public:
GlobalSFM();
bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points);
private:
// PnP得到當前幀相對於第l幀的位姿
bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector<SFMFeature> &sfm_f);
// 三角化點
void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Vector2d &point0, Vector2d &point1, Vector3d &point_3d);
// 三角化兩幀
void triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,
int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
vector<SFMFeature> &sfm_f);
int feature_num; // 特徵點數量
};
GlobalSFM::construct () 最重要的函數！！！堅持就是勝利
首先看函數聲明，爲了求解滑動窗內所有圖像幀相對於第l幀（參考幀）的位姿和三角化特徵點3D座標。輸出所有圖像幀相對於參考幀l的姿態四元數Q、平移向量T和特徵點座標，放在sfm_f和sfm_tracked_points內。
/**
* @brief 純視覺sfm，求解窗口中的所有圖像幀相對於第l幀的位姿和三角化的特徵點座標
* @param[in] frame_num 窗口總幀數（frame_count + 1）
* @param[out] q 窗口內圖像幀的旋轉四元數q（相對於第l幀）Q[frame_count + 1]
* @param[out] T 窗口內圖像幀的平移向量T（相對於第l幀）
* @param[in] l 第l幀
* @param[in] relative_R 當前幀到第l幀的旋轉矩陣
* @param[in] relative_T 當前幀到第l幀的平移向量
* @param[in] sfm_f 所有特徵點
* @param[out] sfm_tracked_points 所有在sfm中三角化的特徵點ID和座標
* @return bool true:sfm求解成功
*/
bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
假設第l幀爲原點，初始化第l幀（q[l]和T[l]）和當前幀（q[frame_num-1]和T[frame_num-1]）並賦值到Pose中
最後得到：Pose[i]的i是l 和 frame_num-1，第l幀和第frame_num-1幀的姿態初始化得到了。下一步可以先對這兩幀三角化。
//假設第l幀爲原點，根據當前幀到第l幀的relative_R，relative_T，得到當前幀位姿
q[l].w() = 1;// 第l幀
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
T[l].setZero();
q[frame_num - 1] = q[l] * Quaterniond(relative_R);// 當前幀
T[frame_num - 1] = relative_T;
//cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl;
//cout << "init t_l " << T[l].transpose() << endl;
//rotate to cam frame
Matrix3d c_Rotation[frame_num];// 旋轉
Vector3d c_Translation[frame_num];// 平移
Quaterniond c_Quat[frame_num]; // 旋轉四元數
double c_rotation[frame_num][4];
double c_translation[frame_num][3];
Eigen::Matrix<double, 3, 4> Pose[frame_num];// 位姿3*4，第l幀到每一幀變換矩陣
// 第l幀
c_Quat[l] = q[l].inverse();
c_Rotation[l] = c_Quat[l].toRotationMatrix();
c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
Pose[l].block<3, 1>(0, 3) = c_Translation[l];
// 滑動窗最後一幀，即當前幀
c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];
1）上部分初始化了第l幀和第frame_num-1幀的姿態，以第l幀的姿態作爲世界座標系，三角化 l 和framenum-1幀 。
2）再用pnp估計出下一幀的位姿，下一幀再與最後一幀三角定位估計更多三維點。三角化 l+1,l+2...frmaenum-2和framenum-1幀
3）從l幀開始往第一幀，逐漸幀pnp，再與第l幀進行三角定位得到更多的三維點。三角化 l-1, l-2, l-3, …, 0幀與 l 幀
每幀pnp時位姿初值都用的上一個關鍵幀的位姿。
4）剩下的沒有三角化的特徵點，通過它被觀察的第一幀和最後一幀進行三角定位。
1)、pnp得到 l,l+1,l+2...frmaenum-2相機位姿，三角化l,l+1,l+2...frmaenum-2和framenum-1幀
for (int i = l; i < frame_num - 1 ; i++)
{
if (i > l)
{
Matrix3d R_initial = c_Rotation[i - 1];
Vector3d P_initial = c_Translation[i - 1];
// 求解PnP得到RT
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
}
// l,l+1,l+2.....framenum-2和framenum-1 三角化恢復路標3D座標
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}
這裏其實是兩部分，當i==l時，不進入if 語句，直接進行三角化triangulateTwoFrames。第l幀和第frame_num-1幀之間。
下一步i=l+1， l+2 ,l+3......都是先用solveFrameByPnP函數用PnP得到當前幀相機位姿，再三角化 triangulateTwoFrames
最終得到的是三角化l,l+1,l+2...frmaenum-2和framenum-1幀
後面會對三個函數solveFrameByPnP函數和triangulateTwoFrames()和triangulatePoint()進行詳細講解
都在initial_sfm.cpp中
solveFrameByPnP（）函數得到第i幀的RT
/* @brief PNP方法得到第l幀到第i幀的位姿
* @param[in] i 第i幀
* @param[update] R_initial、P_initial、sfm_f
*/
bool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,
vector<SFMFeature> &sfm_f)
{
vector<cv::Point2f> pts_2_vector;
vector<cv::Point3f> pts_3_vector;
// 遍歷所有特徵點
for (int j = 0; j < feature_num; j++)
{
if (sfm_f[j].state != true)// 檢查狀態，該點是否被三角化
continue;
Vector2d point2d;
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
{
if (sfm_f[j].observation[k].first == i)// 判斷圖像幀索引是否相同
{
// 通過觀測得到2D像素點
Vector2d img_pts = sfm_f[j].observation[k].second;
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
// 通過position得到路標3D座標
cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);
pts_3_vector.push_back(pts_3);
break;
}
}
}
if (int(pts_2_vector.size()) < 15)// 判斷像素點數量是否足夠
{
printf("unstable features tracking, please slowly move you device!\n");
if (int(pts_2_vector.size()) < 10)
return false;
}
cv::Mat r, rvec, t, D, tmp_r;
cv::eigen2cv(R_initial, tmp_r);
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_initial, t);
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
bool pnp_succ;
pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);
if(!pnp_succ)
{
return false;
}
cv::Rodrigues(rvec, r);
//cout << "r " << endl << r << endl;
MatrixXd R_pnp;
cv::cv2eigen(r, R_pnp);
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
R_initial = R_pnp;
P_initial = T_pnp;
return true;
}
triangulateTwoFrames（）函數三角化兩幀特徵點，更新sfm_f.position
/* @brief 三角化frame0和frame1間所有對應點
* @param[in] frame,Pose 幀索引和位姿數據
* @param[out] sfm_f的state和position 3D座標
*/
//
void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,
int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
vector<SFMFeature> &sfm_f)
{
assert(frame0 != frame1);
for (int j = 0; j < feature_num; j++)// 遍歷所有特徵點
{
if (sfm_f[j].state == true) // 沒有三角化的話纔開始三角化
continue;
//
bool has_0 = false, has_1 = false;
Vector2d point0; Vector2d point1;
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
{ // 找到對應的幀索引，取出2D像素座標
if (sfm_f[j].observation[k].first == frame0)
{
point0 = sfm_f[j].observation[k].second;
has_0 = true;
}
if (sfm_f[j].observation[k].first == frame1)
{
point1 = sfm_f[j].observation[k].second;
has_1 = true;
}
}
// 如果2D像素座標都匹配上了，三角化
if (has_0 && has_1)
{
Vector3d point_3d;
triangulatePoint(Pose0, Pose1, point0, point1, point_3d);
sfm_f[j].state = true; // 實現三角化了
sfm_f[j].position[0] = point_3d(0);
sfm_f[j].position[1] = point_3d(1);
sfm_f[j].position[2] = point_3d(2);
//cout << "trangulated : " << frame1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}
}
線性三角化法DLT triangulatePoint()已知兩幀對應的2D點和兩幀的相機位姿，求解路標3D座標。
路標點P（XYZ），在兩幀上成像爲 x1(u1,v1)和 x2(u2,v2)，外參矩陣分別爲P1和P2,
二者變換方程：
兩邊同時乘以x1的反對稱矩陣，可得：
這裏的T不是轉置矩陣，是反對稱矩陣的意思。
第三個式子是前兩個式子相減得到，每對特徵點可以做成兩個式子，最少需要兩對特徵點才能解。
兩個特徵點各自取兩個方程，組成 AX=0
這裏的T不是轉置矩陣，是反對稱矩陣的意思。
/* @brief 三角化兩幀間某個對應特徵點的深度
* @param[in] Pose和point
* @param[out] point_3d
*/
void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
Matrix4d design_matrix = Matrix4d::Zero();
design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
Vector4d triangulated_point;
triangulated_point =
design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
point_3d(0) = triangulated_point(0) / triangulated_point(3);
point_3d(1) = triangulated_point(1) / triangulated_point(3);
point_3d(2) = triangulated_point(2) / triangulated_point(3);
}
2)、三角化 l+1,l+2......framenum-2和 l 幀
for (int i = l + 1; i < frame_num - 1; i++)
triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
3)、pnp得到l-1,l-2.....0相機位姿，三角化 l-1, l-2, l-3, …, 0幀與l幀
for (int i = l - 1; i >= 0; i--)
{
//solve pnp
Matrix3d R_initial = c_Rotation[i + 1];
Vector3d P_initial = c_Translation[i + 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
//triangulate
triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
}
4)、剩下的沒有三角化的特徵點，通過它被觀察的第一幀和最後一幀進行三角定位。
// 5、三角化其他未恢復的特徵點,即sfm_f[j].state==false的點
// 選擇它被觀測到的第一幀和最後一幀進行三角定位
// 至此得到了滑動窗口中所有圖像幀的位姿以及特徵點的3d座標
for (int j = 0; j < feature_num; j++)
{
if (sfm_f[j].state == true)
continue;
if ((int)sfm_f[j].observation.size() >= 2)
{
Vector2d point0, point1;
int frame_0 = sfm_f[j].observation[0].first;// 第一幀
point0 = sfm_f[j].observation[0].second;
int frame_1 = sfm_f[j].observation.back().first;// 最後一幀
point1 = sfm_f[j].observation.back().second;
Vector3d point_3d;
triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
sfm_f[j].state = true;
sfm_f[j].position[0] = point_3d(0);
sfm_f[j].position[1] = point_3d(1);
sfm_f[j].position[2] = point_3d(2);
//cout << "trangulated : " << frame_0 << " " << frame_1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}
5)、使用cares進行全局BA優化（相機位姿和特徵點座標）
固定住l幀的位置和姿態，固定住最後一幀的位置。因爲這時候的圖像位姿和點的位置都不太準，所以用ceres統一一起優化圖像位姿和三維點位置，優化重投影誤差。優化的測量值是，特徵點在每幀中被觀察到的位置，可以轉成重投影誤差約束。有關的自變量是，每幀圖像的位姿，特徵點的三維座標。
優化完成之後，即用ceres優化出這些關鍵幀的位姿和地圖點後，再用pnp算出在這段時間區域內的所有圖像的位姿。每個圖像的計算都用下一個關鍵幀的位姿來當pnp的初值
ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//cout << " begin full BA " << endl;
for (int i = 0; i < frame_num; i++)
{
//double array for ceres
c_translation[i][0] = c_Translation[i].x();
c_translation[i][1] = c_Translation[i].y();
c_translation[i][2] = c_Translation[i].z();
c_rotation[i][0] = c_Quat[i].w();
c_rotation[i][1] = c_Quat[i].x();
c_rotation[i][2] = c_Quat[i].y();
c_rotation[i][3] = c_Quat[i].z();
problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
problem.AddParameterBlock(c_translation[i], 3);
if (i == l)
{
problem.SetParameterBlockConstant(c_rotation[i]);
}
if (i == l || i == frame_num - 1)
{
problem.SetParameterBlockConstant(c_translation[i]);
}
}
for (int i = 0; i < feature_num; i++)
{
if (sfm_f[i].state != true)
continue;
for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
{
int l = sfm_f[i].observation[j].first;
ceres::CostFunction* cost_function = ReprojectionError3D::Create(
sfm_f[i].observation[j].second.x(),
sfm_f[i].observation[j].second.y());
problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],
sfm_f[i].position);
}
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.minimizer_progress_to_stdout = true;
options.max_solver_time_in_seconds = 0.2;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
{
//cout << "vision only BA converge" << endl;
}
else
{
//cout << "vision only BA not converge " << endl;
return false;
}
//這裏得到的是第l幀座標系到各幀的變換矩陣，應將其轉變爲各幀在第l幀座標系上的位姿
for (int i = 0; i < frame_num; i++)
{
q[i].w() = c_rotation[i][0];
q[i].x() = c_rotation[i][1];
q[i].y() = c_rotation[i][2];
q[i].z() = c_rotation[i][3];
q[i] = q[i].inverse();
//cout << "final q" << " i " << i <<" " <<q[i].w() << " " << q[i].vec().transpose() << endl;
}
for (int i = 0; i < frame_num; i++)
{
T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
//cout << "final t" << " i " << i <<" " << T[i](0) <<" "<< T[i](1) <<" "<< T[i](2) << endl;
}
for (int i = 0; i < (int)sfm_f.size(); i++)
{
if(sfm_f[i].state)
sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
}
return true;
}
以上是對sfm.construct（）函數進行了詳細解讀，第一部分純視覺SFM結束，需要強調的是這裏的Q、T存儲的是每一幀相對於第l幀(參考幀)的相對位姿。
前面只得到了滑動窗口內所有關鍵幀的位姿，但由於並不是第一次視覺初始化就能成功，此時圖像幀數目有可能會超過滑動窗口的大小。此時要對那些不被包括在滑動窗口內的圖像幀位姿進行求解。
solve pnp for all frame
5、對於所有的圖像幀，包括不在滑動窗口中的，提供初始的RT估計
//5. 對於所有的圖像幀，包括不在滑動窗口中的，提供初始的RT估計，然後solvePnP進行優化,得到每一幀的姿態
map<double, ImageFrame>::iterator frame_it;
map<int, Vector3d>::iterator it;
frame_it = all_image_frame.begin( );
for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
{
cv::Mat r, rvec, t, D, tmp_r;
// 初始化估計值
if((frame_it->first) == Headers[i].stamp.toSec())
{
frame_it->second.is_key_frame = true;
frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
frame_it->second.T = T[i];
i++;
continue;
}
if((frame_it->first) > Headers[i].stamp.toSec())
{
i++;
}
// Q和T是圖像幀的位姿，而不是求解PNP時所用的座標系變換矩陣
Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
Vector3d P_inital = - R_inital * T[i];
cv::eigen2cv(R_inital, tmp_r);
//羅德里格斯公式將旋轉矩陣轉換成旋轉向量
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_inital, t);
frame_it->second.is_key_frame = false;
vector<cv::Point3f> pts_3_vector; // 3D座標
vector<cv::Point2f> pts_2_vector; // 2D座標
for (auto &id_pts : frame_it->second.points)
{
int feature_id = id_pts.first;// 特徵點索引
for (auto &i_p : id_pts.second)
{
it = sfm_tracked_points.find(feature_id);
// 出現過，否則it就等於.end()了
if(it != sfm_tracked_points.end())
{
//
Vector3d world_pts = it->second;
cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
pts_3_vector.push_back(pts_3);
Vector2d img_pts = i_p.second.head<2>();
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
}
}
}
//保證特徵點數大於5
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
if(pts_3_vector.size() < 6)
{
cout << "pts_3_vector size " << pts_3_vector.size() << endl;
ROS_DEBUG("Not enough points for solve pnp !");
return false;
}
/**
*bool cv::solvePnP( 求解pnp問題
* InputArray objectPoints, 特徵點的3D座標數組
* InputArray imagePoints, 特徵點對應的圖像座標
* InputArray cameraMatrix, 相機內參矩陣
* InputArray distCoeffs, 失真係數的輸入向量
* OutputArray rvec, 旋轉向量
* OutputArray tvec, 平移向量
* bool useExtrinsicGuess = false, 爲真則使用提供的初始估計值
* int flags = SOLVEPNP_ITERATIVE 採用LM優化
*)
*/
if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
{
ROS_DEBUG("solve pnp fail!");
return false;
}
cv::Rodrigues(rvec, r);
MatrixXd R_pnp,tmp_R_pnp;
cv::cv2eigen(r, tmp_R_pnp);
//這裏也同樣需要將座標變換矩陣轉變成圖像幀位姿，並轉換爲IMU座標系的位姿
R_pnp = tmp_R_pnp.transpose();
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
T_pnp = R_pnp * (-T_pnp);
frame_it->second.R = R_pnp * RIC[0].transpose();
frame_it->second.T = T_pnp;
}
6、進行視覺慣性聯合初始化
if (visualInitialAlign())
return true;
else
{
ROS_INFO("misalign visual structure with IMU");
return false;
}