本篇筆記緊接着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設置爲1
    posegraph.addKeyFrame(keyframe, 1);

2）位姿圖中的頂點和邊

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

序列邊（Sequential Edge）:

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

$\hat{p}_{ij}^i = \hat{R}_i^{w^{-1}}(\hat{p}_j^{w}-\hat{p}_i^{w})$

$\hat{\psi }_{ij}=\hat{\psi }_j-\hat{\psi }_i.$

閉環邊（Loop-Closure Edge）：

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

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

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

$r_{i,j}(p_i^{w},\psi _i,p_j^w,\psi _j)=\begin{bmatrix} R(\hat{\phi}_i,\hat{\theta }_i,\psi _i)^{-1}(p_j^w-p_i^w)-\hat{p}_{ij}^i\\ \psi _j-\psi_i-\hat{\psi}_{ij} \end{bmatrix}$

這裏的 $\hat{\phi }_i$ 和 $\hat{\theta }_i$ 兩個值，是從單目VIO中估計得到的roll和pitch的角度，是固定的。

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

$\underset{p,\psi }{min}\left \{ \sum_{(i,j)\in S}\left \| r_{i,j} \right \|^2+\sum_{(i,j)\in L} \rho (\left \| r_{i,j} \right \|^2)\right \}$

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

優化部分在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();
        //從優化隊列當中獲取最新的一個關鍵幀的index
        while(!optimize_buf.empty())
        {
            cur_index = optimize_buf.front();
            //earliest_loop_index當中存放的是數據庫中第一個和滑動窗口中關鍵幀形成閉環的關鍵幀的index
            first_looped_index = earliest_loop_index;
            optimize_buf.pop();
        }
        m_optimize_buf.unlock();
        //optimize_buf中取出來的cur_index都是閉環幀的index
        if (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_i
            double 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 − ψ̂ _i
                        double 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_r
            cur_kf->getPose(cur_t, cur_r);
            //獲取優化前有漂移的當前幀的位姿vio_t,vio_r
            cur_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();
            //優化完後更新path
            updatePath();
        }

        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 rotation
		T w_R_i[9];
		YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
		// rotation transpose
		T i_R_w[9];
		//求出旋轉矩陣的轉置
		RotationMatrixTranspose(w_R_i, i_R_w);
		// rotation matrix rotate point
		T 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 rotation
		T w_R_i[9];
		YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
		// rotation transpose
		T i_R_w[9];
		RotationMatrixTranspose(w_R_i, i_R_w);
		// rotation matrix rotate point
		T 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中四自由度位姿優化

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

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

如何使用 JS 判斷用戶是否處於活躍狀態

通過HPA+CronHPA組合應對業務複雜彈性伸縮場景

❤️‍🔥 Solon Cloud Event 新的事務特性與應用

ORB-SLAM2系統中的優化函數

ORB-SLAM2中LoopClosing線程主要流程梳理

VINS-Mono代碼閱讀筆記（八）：vins_estimator中的相機-IMU初始化

VINS-Mono代碼閱讀筆記（七）：vins_estimator中相機與IMU的外參標定

VINS-Mono代碼閱讀筆記（三）：vins_estimator中main函數分析

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結