本篇筆記緊接著VINS-Mono代碼閱讀筆記（十二）：將關鍵幀加入位姿圖當中，來學習pose_graph當中的緊耦合優化部分。

在重定位完成之后，進行位姿圖優化是為了將已經產生的所有位姿統一到一個全局一致的配置當中。如論文中展示的下圖所示，參考幀處于世界坐標系下，當相機運動的時候會相對于參考幀發生變化。而由于重力向量始終不會發生變化，所以從重力方向得到的水平面也不會發生變化，進而該水平面對應的兩個向量也不會發生變換。所以，系統中需要計算并且優化的向量只有（也就是位置和旋轉），這就是4自由度優化的由來。

1.加入關鍵幀到位姿圖當中

通過代碼可以發現，當滑動窗口完成一次邊緣化（滑出最舊的幀或者滑動窗口中倒數第二幀）后，在后邊的pubKeyframe函數中會將the second latest frame關鍵幀的位姿信息作為topic發出來。pose_graph節點中接收到這個topic后，構造出對應的關鍵幀并加入位姿圖當中。

1）相關代碼

vins-estimator中發送關鍵幀位姿的代碼如下：

void pubKeyframe(const Estimator &estimator) {// pub camera pose, 2D-3D points of keyframe//estimator.marginalization_flag == 0,表示MARGIN_OLD，邊緣化刪除了最舊的關鍵幀//estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR表示視覺和慣導初始化成功if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR && estimator.marginalization_flag == 0){int i = WINDOW_SIZE - 2;//Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0];Vector3d P = estimator.Ps[i];Quaterniond R = Quaterniond(estimator.Rs[i]);nav_msgs::Odometry odometry;odometry.header = estimator.Headers[WINDOW_SIZE - 2];odometry.header.frame_id = "world";odometry.pose.pose.position.x = P.x();odometry.pose.pose.position.y = P.y();odometry.pose.pose.position.z = P.z();odometry.pose.pose.orientation.x = R.x();odometry.pose.pose.orientation.y = R.y();odometry.pose.pose.orientation.z = R.z();odometry.pose.pose.orientation.w = R.w();//printf("time: %f t: %f %f %f r: %f %f %f %f\n", odometry.header.stamp.toSec(), P.x(), P.y(), P.z(), R.w(), R.x(), R.y(), R.z());pub_keyframe_pose.publish(odometry);

而在pose-graph中將該關鍵幀加入位姿圖的代碼如下：

/*** process線程入口函數 */ void process() {.......//創建關鍵幀KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,point_3d, point_2d_uv, point_2d_normal, point_id, sequence);m_process.lock();start_flag = 1;//位姿圖中加入關鍵幀，flag_detect_loop設置為1posegraph.addKeyFrame(keyframe, 1);

2）位姿圖中的頂點和邊

每一個關鍵幀在位姿圖中作為一個頂點存在，它和其他關鍵幀以序列變和閉環邊兩種類型的邊進行連接，如下圖所示：

序列邊（Sequential Edge）:

一個關鍵幀將與其前邊的多個關鍵幀建立序列邊（如上圖所示）。一個序列邊表示兩個關鍵幀之間的相對變換，這個可以直接從VIO中得出。假設關鍵幀和其前邊的一個關鍵幀,兩個關鍵幀構成的序列邊只包含相對位置和偏航角，則這兩個值表達如下：

?????????????????????????????????????????????????????????????????

?????????????????????????????????????????????????????????????????

閉環邊（Loop-Closure Edge）：

如果一個關鍵幀有閉環連接，那么它在位姿圖中和閉環幀之間的連接為閉環邊。相似的，閉環邊只包括一個4自由度的相對位姿變換，定義和上邊的序列變相同。閉環邊的值，是通過重定位得到的。

2.四自由度的位姿圖優化

我們定義關鍵幀和之間的邊的最小化殘差如下：

???????????????????????????????????????????????????????????????

這里的和兩個值，是從單目VIO中估計得到的roll和pitch的角度，是固定的。

序列邊和閉環邊的所有圖通過最小化下面的損失函數來進行優化：

???????????????????????????????????????????????????????????????

這里是所有序列變的集合，是所有閉環邊的集合。盡管緊耦合的重定位已經幫助消除了錯誤的閉環，在這里增加另一個Huber核函數以進一步減少任何可能的錯誤閉環的影響。

優化部分在pose_graph節點構造posegraph對象的時候，在構造函數當中新啟了一個線程來完成。PoseGraph的構造函數代碼如下：

/*** PoseGraph構造函數 */ PoseGraph::PoseGraph() {posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0);posegraph_visualization->setScale(0.1);posegraph_visualization->setLineWidth(0.01);//創建位姿優化線程t_optimization = std::thread(&PoseGraph::optimize4DoF, this);earliest_loop_index = -1;t_drift = Eigen::Vector3d(0, 0, 0);yaw_drift = 0;r_drift = Eigen::Matrix3d::Identity();w_t_vio = Eigen::Vector3d(0, 0, 0);w_r_vio = Eigen::Matrix3d::Identity();global_index = 0;sequence_cnt = 0;sequence_loop.push_back(0);base_sequence = 1;}

位姿優化的線程入口函數為optimize4DoF，optimize4DoF代碼如下：

/*** 位姿圖中的優化，這里是4個自由度的位姿優化 */ void PoseGraph::optimize4DoF() {while(true){int cur_index = -1;int first_looped_index = -1;m_optimize_buf.lock();//從優化隊列當中獲取最新的一個關鍵幀的indexwhile(!optimize_buf.empty()){cur_index = optimize_buf.front();//earliest_loop_index當中存放的是數據庫中第一個和滑動窗口中關鍵幀形成閉環的關鍵幀的indexfirst_looped_index = earliest_loop_index;optimize_buf.pop();}m_optimize_buf.unlock();//optimize_buf中取出來的cur_index都是閉環幀的indexif (cur_index != -1){printf("optimize pose graph \n");TicToc tmp_t;m_keyframelist.lock();KeyFrame* cur_kf = getKeyFrame(cur_index);//max_length為要優化的變量最大個數int max_length = cur_index + 1;// w^t_i w^q_idouble t_array[max_length][3];//平移數組，其中存放每個關鍵幀的平移向量Quaterniond q_array[max_length];//旋轉數組，其中存放每個關鍵幀的旋轉四元數double euler_array[max_length][3];double sequence_array[max_length];ceres::Problem problem;ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;//options.minimizer_progress_to_stdout = true;//options.max_solver_time_in_seconds = SOLVER_TIME * 3;options.max_num_iterations = 5;ceres::Solver::Summary summary;ceres::LossFunction *loss_function;loss_function = new ceres::HuberLoss(0.1);//loss_function = new ceres::CauchyLoss(1.0);//AngleLocalParameterization類的主要作用是指定yaw角優化變量的迭代更新，重載了括號運算ceres::LocalParameterization* angle_local_parameterization =AngleLocalParameterization::Create();list<KeyFrame*>::iterator it;int i = 0;//遍歷關鍵幀列表for (it = keyframelist.begin(); it != keyframelist.end(); it++){//first_looped_index為第一次閉環幀的index,需要優化的關鍵幀為從第一次閉環幀到當前幀間的所有關鍵幀if ((*it)->index < first_looped_index)continue;(*it)->local_index = i;Quaterniond tmp_q;Matrix3d tmp_r;Vector3d tmp_t;//獲取關鍵幀it的位姿(*it)->getVioPose(tmp_t, tmp_r);tmp_q = tmp_r;t_array[i][0] = tmp_t(0);t_array[i][1] = tmp_t(1);t_array[i][2] = tmp_t(2);q_array[i] = tmp_q;//將矩陣轉換為向量Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());euler_array[i][0] = euler_angle.x();euler_array[i][1] = euler_angle.y();euler_array[i][2] = euler_angle.z();sequence_array[i] = (*it)->sequence;//將關鍵幀列表中所有index>=first_looped_index的關鍵幀的位姿加入到參數塊當中problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);problem.AddParameterBlock(t_array[i], 3);//設置約束：如果該幀是最早的閉環幀的情況下，則固定它的位姿if ((*it)->index == first_looped_index || (*it)->sequence == 0){ problem.SetParameterBlockConstant(euler_array[i]);problem.SetParameterBlockConstant(t_array[i]);}//add edge 這里添加的是序列邊，是指通過VIO計算的兩幀之間的相對位姿，每幀分別與其前邊最多四幀構成序列邊/*** 順序邊的測量方程：p?_{ij}^{i} = {R?_i^w}^{-1} (p?_j^w - p?_i^w)* \hat{ψ}_ij = \hat{ψ}_j ? \hat{ψ?}_i* 兩個關鍵幀之間的相對位姿，由兩個關鍵幀之間的VIO位姿估計變換得到* |------------------------------------|* | |----------------------------|* | | |-------------------|* | | | |---------|* |幀1| |幀2| |幀3| |幀4| |幀5|*/for (int j = 1; j < 5; j++){if (i - j >= 0 && sequence_array[i] == sequence_array[i-j]){Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());//p?_j^w - p?_i^w 計算平移量的偏差Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);//p?_{ij}^{i} = {R?_i^w}^{-1} (p?_j^w - p?_i^w)//p? ij= (R? iw ) (p? jw ? p? iw )relative_t = q_array[i-j].inverse() * relative_t;//ψ? _ij = ψ? _j ? ψ? _idouble relative_yaw = euler_array[i][0] - euler_array[i-j][0];ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], t_array[i-j], euler_array[i], t_array[i]);}}//add loop edge//這里添加的是閉環邊，是指檢測到閉環的兩幀if((*it)->has_loop){assert((*it)->loop_index >= first_looped_index);int connected_index = getKeyFrame((*it)->loop_index)->local_index;Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());Vector3d relative_t;relative_t = (*it)->getLoopRelativeT();double relative_yaw = (*it)->getLoopRelativeYaw();ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], t_array[connected_index], euler_array[i], t_array[i]);}if ((*it)->index == cur_index)break;i++;}m_keyframelist.unlock();//開始優化ceres::Solve(options, &problem, &summary);//std::cout << summary.BriefReport() << "\n";//printf("pose optimization time: %f \n", tmp_t.toc());/*for (int j = 0 ; j < i; j++){printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );}*/m_keyframelist.lock();//優化完成，使用優化后的位姿來更新關鍵幀列表中index大于等于first_looped_index的所有關鍵幀的位姿i = 0;for (it = keyframelist.begin(); it != keyframelist.end(); it++){if ((*it)->index < first_looped_index)continue;Quaterniond tmp_q;//向量轉換為矩陣tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);Matrix3d tmp_r = tmp_q.toRotationMatrix();(*it)-> updatePose(tmp_t, tmp_r);if ((*it)->index == cur_index)break;i++;}//根據計算出當前幀的drift，更新全部關鍵幀位姿Vector3d cur_t, vio_t;Matrix3d cur_r, vio_r;//獲取優化后當前幀的位姿cur_t,cur_rcur_kf->getPose(cur_t, cur_r);//獲取優化前有漂移的當前幀的位姿vio_t,vio_rcur_kf->getVioPose(vio_t, vio_r);m_drift.lock();yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));t_drift = cur_t - r_drift * vio_t;m_drift.unlock();//cout << "t_drift " << t_drift.transpose() << endl;//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;//cout << "yaw drift " << yaw_drift << endl;it++;//下面代碼為把當前關鍵幀it之后的關鍵幀的位姿通過求解的偏移量轉換到world坐標系下for (; it != keyframelist.end(); it++){Vector3d P;Matrix3d R;(*it)->getVioPose(P, R);P = r_drift * P + t_drift;R = r_drift * R;(*it)->updatePose(P, R);}m_keyframelist.unlock();//優化完后更新pathupdatePath();}std::chrono::milliseconds dura(2000);std::this_thread::sleep_for(dura);} }

這里除了梳理優化的函數中的代碼流程，還需要對比以上的殘差公式來看一下序列邊和閉環邊的殘差計算。FourDOFError類中的operator()中是序列邊的殘差計算代碼。

struct FourDOFError {FourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i):t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}template <typename T>bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const{T t_w_ij[3];t_w_ij[0] = tj[0] - ti[0];t_w_ij[1] = tj[1] - ti[1];t_w_ij[2] = tj[2] - ti[2];// euler to rotationT w_R_i[9];YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);// rotation transposeT i_R_w[9];//求出旋轉矩陣的轉置RotationMatrixTranspose(w_R_i, i_R_w);// rotation matrix rotate pointT t_i_ij[3];RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);//計算殘差residuals[0] = (t_i_ij[0] - T(t_x));residuals[1] = (t_i_ij[1] - T(t_y));residuals[2] = (t_i_ij[2] - T(t_z));residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));return true;}static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,const double relative_yaw, const double pitch_i, const double roll_i) {return (new ceres::AutoDiffCostFunction<FourDOFError, 4, 1, 3, 1, 3>(new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));}double t_x, t_y, t_z;double relative_yaw, pitch_i, roll_i;};

FourDOFWeightError類中的operator()是閉環邊的殘差計算代碼。

struct FourDOFWeightError {FourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i):t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){weight = 1;}template <typename T>bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const{T t_w_ij[3];t_w_ij[0] = tj[0] - ti[0];t_w_ij[1] = tj[1] - ti[1];t_w_ij[2] = tj[2] - ti[2];// euler to rotationT w_R_i[9];YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);// rotation transposeT i_R_w[9];RotationMatrixTranspose(w_R_i, i_R_w);// rotation matrix rotate pointT t_i_ij[3];RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);//計算殘差residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight);residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight);residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight);residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0);return true;}static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,const double relative_yaw, const double pitch_i, const double roll_i) {return (new ceres::AutoDiffCostFunction<FourDOFWeightError, 4, 1, 3, 1, 3>(new FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));}double t_x, t_y, t_z;double relative_yaw, pitch_i, roll_i;double weight;};

VINS-Mono代碼閱讀筆記（十四）：posegraph的存儲和加載

總結

以上是生活随笔為你收集整理的VINS-Mono代码阅读笔记（十三）：posegraph中四自由度位姿优化的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。