20191127

week3滑动窗口/course4/nullspace_test/hessian_nullspace_test.cpp

@@ -66,11 +66,11 @@ int main()
             jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
                             -(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;
 
-            H.block(i * 6, i * 6, 6, 6) += jacobian_Ti.transpose() * jacobian_Ti;
+            H.block(i * 6, i * 6, 6, 6) += jacobian_Ti.transpose() * jacobian_Ti;//左上角
             /// 请补充完整作业信息矩阵块的计算
-	        H.block(j * 3 + 6 * poseNums, j * 3 + 6 * poseNums, 3, 3) += jacobian_Pj.transpose() * jacobian_Pj;
-	        H.block(j * 3 + 6 * poseNums, i * 6, 3, 6) += jacobian_Pj.transpose() * jacobian_Ti;
-            H.block(i * 6, j * 3 + 6 * poseNums, 6, 3) += jacobian_Ti.transpose() * jacobian_Pj;
+	        H.block(j * 3 + 6 * poseNums, j * 3 + 6 * poseNums, 3, 3) += jacobian_Pj.transpose() * jacobian_Pj;// 右下角
+	        H.block(j * 3 + 6 * poseNums, i * 6, 3, 6) += jacobian_Pj.transpose() * jacobian_Ti; // 左下角
+            H.block(i * 6, j * 3 + 6 * poseNums, 6, 3) += jacobian_Ti.transpose() * jacobian_Pj; // 右上角
         }
     }
 

week4/hw_course5_new/backend/problem.cc

@@ -314,10 +314,12 @@ void Problem::MakeHessian() {
                 MatXX hessian = JtW * jacobian_j;
                 // 所有的信息矩阵叠加起来
                 // TODO:: home work. 完成 H index 的填写.
+				H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
                 // H.block(?,?, ?, ?).noalias() += hessian;
                 if (j != i) {
                     // 对称的下三角
 		    // TODO:: home work. 完成 H index 的填写.
+					H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();
                     // H.block(?,?, ?, ?).noalias() += hessian.transpose();
                 }
             }
@@ -367,11 +369,12 @@ void Problem::SolveLinearSystem() {
         int marg_size = ordering_landmarks_;
 
         // TODO:: home work. 完成矩阵块取值，Hmm，Hpm，Hmp，bpp，bmm
-        // MatXX Hmm = Hessian_.block(?,?, ?, ?);
-        // MatXX Hpm = Hessian_.block(?,?, ?, ?);
-        // MatXX Hmp = Hessian_.block(?,?, ?, ?);
-        // VecX bpp = b_.segment(?,?);
-        // VecX bmm = b_.segment(?,?);
+        MatXX Hmm = Hessian_.block(reserve_size, reserve_size, marg_size, marg_size);//左上角
+        MatXX Hpm = Hessian_.block(0 , reserve_size, reserve_size, marg_size);//左下角
+        MatXX Hmp = Hessian_.block(reserve_size, 0, marg_size, reserve_size);//右上角
+
+        VecX bpp = b_.segment(0,reserve_size);
+		VecX bmm = b_.segment(reserve_size,marg_size);
 
         // Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
         MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
@@ -383,8 +386,9 @@ void Problem::SolveLinearSystem() {
 
         // TODO:: home work. 完成舒尔补 Hpp, bpp 代码
         MatXX tempH = Hpm * Hmm_inv;
-        // H_pp_schur_ = Hessian_.block(?,?,?,?) - tempH * Hmp;
-        // b_pp_schur_ = bpp - ? * ?;
+        H_pp_schur_ = Hessian_.block(0,0,reserve_size,reserve_size) - tempH * Hpm.transpose();
+        b_pp_schur_ = bpp - tempH * bmm;
+
 
         // step2: solve Hpp * delta_x = bpp
         VecX delta_x_pp(VecX::Zero(reserve_size));
@@ -401,6 +405,7 @@ void Problem::SolveLinearSystem() {
         // TODO:: home work. step3: solve landmark
         VecX delta_x_ll(marg_size);
         // delta_x_ll = ???;
+		delta_x_ll = Hmm_inv * (bmm - Hpm.transpose() * delta_x_pp);//注意b的正负号
         delta_x_.tail(marg_size) = delta_x_ll;
 
     }
@@ -579,6 +584,9 @@ void Problem::TestMarginalize() {
     Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size);
     // H_marg.block(?,?,?,?) = temp_botRows;
     // H_marg.block(?,?,?,?) = temp_rows;
+	H_marg.block(idx, 0, reserve_size - idx - dim, reserve_size) = temp_botRows;
+	H_marg.block(reserve_size-dim, 0, dim, reserve_size) = temp_rows;
+
 
     // 将 col i 移动矩阵最右边
     Eigen::MatrixXd temp_cols = H_marg.block(0, idx, reserve_size, dim);
@@ -604,6 +612,9 @@ void Problem::TestMarginalize() {
     //Eigen::MatrixXd Arm = H_marg.block(?,?,?,?);
     //Eigen::MatrixXd Amr = H_marg.block(?,?,?,?);
     //Eigen::MatrixXd Arr = H_marg.block(?,?,?,?);
+	Eigen::MatrixXd Arm = H_marg.block(0, n2, n2, m2);
+	Eigen::MatrixXd Amr = H_marg.block(n2, 0, m2, n2);
+	Eigen::MatrixXd Arr = H_marg.block(0, 0, n2, n2);
 
     Eigen::MatrixXd tempB = Arm * Amm_inv;
     Eigen::MatrixXd H_prior = Arr - tempB * Amr;