0. 简介
上一节我们将while内部的IKD-Tree部分全部讲完,下面将是最后一部分,关于后端优化更新的部分。
1. 迭代更新
最后一块主要做的就是,拿当前帧与IKD-Tree建立的地图算出的残差,然后去计算更新自己的位置,并将更新后的结果通过map_incremental函数插入到由ikd-Tree表示的映射中。
// 外参,旋转矩阵转欧拉角 V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I); fout_pre << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose() << " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << endl; //输出预测的结果 if (0) // If you need to see map point, change to "if(1)" { // 释放PCL_Storage的内存 PointVector().swap(ikdtree.PCL_Storage); // 把树展平用于展示 ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); featsFromMap->clear(); featsFromMap->points = ikdtree.PCL_Storage; } pointSearchInd_surf.resize(feats_down_size); //搜索索引 Nearest_Points.resize(feats_down_size); //将降采样处理后的点云用于搜索最近点 int rematch_num = 0; bool nearest_search_en = true; // t2 = omp_get_wtime(); /*** 迭代状态估计 ***/ double t_update_start = omp_get_wtime(); double solve_H_time = 0; //迭代卡尔曼滤波更新,更新地图信息 kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); state_point = kf.get_x(); euler_cur = SO3ToEuler(state_point.rot); pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; geoQuat.x = state_point.rot.coeffs()[0]; geoQuat.y = state_point.rot.coeffs()[1]; geoQuat.z = state_point.rot.coeffs()[2]; geoQuat.w = state_point.rot.coeffs()[3]; double t_update_end = omp_get_wtime(); /******* 发布里程计 *******/ publish_odometry(pubOdomAftMapped); /*** 向映射kdtree添加特性点 ***/ t3 = omp_get_wtime(); map_incremental(); t5 = omp_get_wtime(); /******* 发布轨迹和点 *******/ publish_path(pubPath); if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull); if (scan_pub_en && scan_body_pub_en) { publish_frame_body(pubLaserCloudFull_body); publish_frame_lidar(pubLaserCloudFull_lidar); } // publish_effect_world(pubLaserCloudEffect); // publish_map(pubLaserCloudMap); /*** Debug 参数 ***/ if (runtime_pos_log) { frame_num++; kdtree_size_end = ikdtree.size(); aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t_update_end - t_update_start) / frame_num; aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num; aver_time_incre = aver_time_incre * (frame_num - 1) / frame_num + (kdtree_incremental_time) / frame_num; aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time + solve_H_time) / frame_num; aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_time / frame_num; T1[time_log_counter] = Measures.lidar_beg_time; s_plot[time_log_counter] = t5 - t0; //整个流程总时间 s_plot2[time_log_counter] = feats_undistort->points.size(); //特征点数量 s_plot3[time_log_counter] = kdtree_incremental_time; // kdtree增量时间 s_plot4[time_log_counter] = kdtree_search_time; // kdtree搜索耗时 s_plot5[time_log_counter] = kdtree_delete_counter; // kdtree删除点数量 s_plot6[time_log_counter] = kdtree_delete_time; // kdtree删除耗时 s_plot7[time_log_counter] = kdtree_size_st; // kdtree初始大小 s_plot8[time_log_counter] = kdtree_size_end; // kdtree结束大小 s_plot9[time_log_counter] = aver_time_consu; //平均消耗时间 s_plot10[time_log_counter] = add_point_size; //添加点数量 time_log_counter++; printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n", t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, aver_time_const_H_time); ext_euler = SO3ToEuler(state_point.offset_R_L_I); fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose() << " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << " " << feats_undistort->points.size() << endl; dump_lio_state_to_log(fp); } } status = ros::ok(); rate.sleep(); } /**************** save map ****************/ /* 1. make sure you have enough memories /* 2. pcd save will largely influence the real-time performences **/ if (pcl_wait_save->size() > 0 && pcd_save_en) { string file_name = string("scans.pcd"); string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); pcl::PCDWriter pcd_writer; cout << "current scan saved to /PCD/" << file_name << endl; pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); } fout_out.close(); fout_pre.close(); if (runtime_pos_log) { vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7; FILE *fp2; string log_dir = root_dir + "/Log/fast_lio_time_log.csv"; fp2 = fopen(log_dir.c_str(), "w"); fprintf(fp2, "time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n"); for (int i = 0; i < time_log_counter; i++) { fprintf(fp2, "%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n", T1[i], s_plot[i], int(s_plot2[i]), s_plot3[i], s_plot4[i], int(s_plot5[i]), s_plot6[i], int(s_plot7[i]), int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]); t.push_back(T1[i]); s_vec.push_back(s_plot9[i]); s_vec2.push_back(s_plot3[i] + s_plot6[i]); s_vec3.push_back(s_plot4[i]); s_vec5.push_back(s_plot[i]); } fclose(fp2); } return 0; }
// 迭代错误状态EKF更新修改为一个特定的系统 void update_iterated_dyn_share_modified(double R, double &solve_time) { dyn_share_datastruct<scalar_type> dyn_share; dyn_share.valid = true; dyn_share.converge = true; int t = 0; //获取上一次的状态和协方差矩阵 state x_propagated = x_; cov P_propagated = P_; int dof_Measurement; Matrix<scalar_type, n, 1> K_h; Matrix<scalar_type, n, n> K_x; vectorized_state dx_new = vectorized_state::Zero(); // 最多进行maximum_iter次迭代优化 for (int i = -1; i < maximum_iter; i++) { dyn_share.valid = true; // 计算测量模型方程的雅克比,也就是点面残差的导数 H(代码里是h_x) h_dyn_share(x_, dyn_share); // Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share); #ifdef USE_sparse spMt h_x_ = dyn_share.h_x.sparseView(); #else // 获取测量模型的雅克比d(pos, rot, 0, 0)/dx Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x; #endif double solve_start = omp_get_wtime(); dof_Measurement = h_x_.rows(); // 观测方程个数m vectorized_state dx; // 定义误差状态 x_.boxminus(dx, x_propagated); //获取误差dx dx_new = dx; // 用于迭代的误差状态 if (!dyn_share.valid) { continue; } // 预测得到的误差状态协方差矩阵 //协方差矩阵在迭代过程中不会代入下一次迭代,直到最后一次退出时更新,在迭代过程中更新的只是先验 P_ = P_propagated; // 这一大段都在求协方差的先验更新,大致上是P=(J^-1)*P*(J^-T)如论文式16~18 Matrix<scalar_type, 3, 3> res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx(idx + i); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix<scalar_type, 2, 2> res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; int dim = (*it).second; for (int i = 0; i < 2; i++) { seg_S2(i) = dx(idx + i); } Eigen::Matrix<scalar_type, 2, 3> Nx; Eigen::Matrix<scalar_type, 3, 2> Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); for (int i = 0; i < n; i++) { P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } for (int i = 0; i < n; i++) { P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } // Matrix<scalar_type, n, Eigen::Dynamic> K_; // Matrix<scalar_type, n, 1> K_h; // Matrix<scalar_type, n, n> K_x; /* if(n > dof_Measurement) { K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R; } else { K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); } */ // 状态维度 n > 测量维度 dof_Measurement // 如果状态量维度大于观测方程 n > m,不满秩 if (n > dof_Measurement) { //#ifdef USE_sparse // Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose(); // spMt R_temp = h_v * R_ * h_v.transpose(); // K_temp += R_temp; Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n); //每一次迭代将重新计算增益K,即论文式18 h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; /* h_x_cur.col(0) = h_x_.col(0); h_x_cur.col(1) = h_x_.col(1); h_x_cur.col(2) = h_x_.col(2); h_x_cur.col(3) = h_x_.col(3); h_x_cur.col(4) = h_x_.col(4); h_x_cur.col(5) = h_x_.col(5); h_x_cur.col(6) = h_x_.col(6); h_x_cur.col(7) = h_x_.col(7); h_x_cur.col(8) = h_x_.col(8); h_x_cur.col(9) = h_x_.col(9); h_x_cur.col(10) = h_x_.col(10); h_x_cur.col(11) = h_x_.col(11); */ // 重新计算增益矩阵K Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose() / R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse() / R; K_h = K_ * dyn_share.h; K_x = K_ * h_x_cur; //#else // K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); //#endif } else { //避免求逆矩阵,K按稀疏矩阵分解的方法如论文式20 #ifdef USE_sparse // Eigen::Matrix<scalar_type, n, n> b = Eigen::Matrix<scalar_type, n, n>::Identity(); // Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; spMt A = h_x_.transpose() * h_x_; cov P_temp = (P_ / R).inverse(); P_temp.template block<12, 12>(0, 0) += A; P_temp = P_temp.inverse(); /* Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n); h_x_cur.col(0) = h_x_.col(0); h_x_cur.col(1) = h_x_.col(1); h_x_cur.col(2) = h_x_.col(2); h_x_cur.col(3) = h_x_.col(3); h_x_cur.col(4) = h_x_.col(4); h_x_cur.col(5) = h_x_.col(5); h_x_cur.col(6) = h_x_.col(6); h_x_cur.col(7) = h_x_.col(7); h_x_cur.col(8) = h_x_.col(8); h_x_cur.col(9) = h_x_.col(9); h_x_cur.col(10) = h_x_.col(10); h_x_cur.col(11) = h_x_.col(11); */ K_ = P_temp.template block<n, 12>(0, 0) * h_x_.transpose(); K_x = cov::Zero(); K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH; /* solver.compute(R_); Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b); spMt R_in =R_in_temp.sparseView(); spMt K_temp = h_x.transpose() * R_in * h_x; cov P_temp = P_.inverse(); P_temp += K_temp; K_ = P_temp.inverse() * h_x.transpose() * R_in; */ #else cov P_temp = (P_ / R).inverse(); // Eigen::Matrix<scalar_type, 12, Eigen::Dynamic> h_T = h_x_.transpose(); Eigen::Matrix<scalar_type, 12, 12> HTH = h_x_.transpose() * h_x_; P_temp.template block<12, 12>(0, 0) += HTH; /* Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n); //std::cout << "line 1767" << std::endl; h_x_cur.col(0) = h_x_.col(0); h_x_cur.col(1) = h_x_.col(1); h_x_cur.col(2) = h_x_.col(2); h_x_cur.col(3) = h_x_.col(3); h_x_cur.col(4) = h_x_.col(4); h_x_cur.col(5) = h_x_.col(5); h_x_cur.col(6) = h_x_.col(6); h_x_cur.col(7) = h_x_.col(7); h_x_cur.col(8) = h_x_.col(8); h_x_cur.col(9) = h_x_.col(9); h_x_cur.col(10) = h_x_.col(10); h_x_cur.col(11) = h_x_.col(11); */ cov P_inv = P_temp.inverse(); // std::cout << "line 1781" << std::endl; K_h = P_inv.template block<n, 12>(0, 0) * h_x_.transpose() * dyn_share.h; // (H_T_H + P^-1)^-1 * H^T * h(残差) = K * h // std::cout << "line 1780" << std::endl; // cov HTH_cur = cov::Zero(); // HTH_cur. template block<12, 12>(0, 0) = HTH; K_x.setZero(); // = cov::Zero(); K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH; //(H_T_H + P^-1)^-1 * H_T_H = KH // K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); #endif } // K_x = K_ * h_x_; //由于是误差迭代KF,得到的是误差的最优估计! Matrix<scalar_type, n, 1> dx_ = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; //误差增量后验 K*h + (K*H - I) dx state x_before = x_; // 加上校正后的误差状态dx_ x_.boxplus(dx_); //根据计算得到的误差增量后验,更新状态量 // 判断迭代是否发散 dyn_share.converge = true; //判断已收敛的条件是误差的估计值小于阈值 for (int i = 0; i < n; i++) { if (std::fabs(dx_[i]) > limit[i]) { dyn_share.converge = false; break; } } if (dyn_share.converge) t++; if (!t && i == maximum_iter - 2) { dyn_share.converge = true; } // 迭代完成后更新误差状态协方差矩阵 //结束迭代后,更新协方差矩阵的后验值,大致上是P=(I-K*H)*P,如论文式19 if (t > 1 || i == maximum_iter - 1) { L_ = P_; // std::cout << "iteration time" << t << "," << i << std::endl; Matrix<scalar_type, 3, 3> res_temp_SO3; MTK::vect<3, scalar_type> seg_SO3; for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 3; i++) { seg_SO3(i) = dx_(i + idx); } res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); for (int i = 0; i < n; i++) { L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i)); } // if(n > dof_Measurement) // { // for(int i = 0; i < dof_Measurement; i++){ // K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); // } // } // else // { for (int i = 0; i < 12; i++) { K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i)); } //} for (int i = 0; i < n; i++) { L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); } } Matrix<scalar_type, 2, 2> res_temp_S2; MTK::vect<2, scalar_type> seg_S2; for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { int idx = (*it).first; for (int i = 0; i < 2; i++) { seg_S2(i) = dx_(i + idx); } Eigen::Matrix<scalar_type, 2, 3> Nx; Eigen::Matrix<scalar_type, 3, 2> Mx; x_.S2_Nx_yy(Nx, idx); x_propagated.S2_Mx(Mx, seg_S2, idx); res_temp_S2 = Nx * Mx; for (int i = 0; i < n; i++) { L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i)); } // if(n > dof_Measurement) // { // for(int i = 0; i < dof_Measurement; i++){ // K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); // } // } // else // { for (int i = 0; i < 12; i++) { K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i)); } //} for (int i = 0; i < n; i++) { L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose(); } } // if(n > dof_Measurement) // { // Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n); // h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; // /* // h_x_cur.col(0) = h_x_.col(0); // h_x_cur.col(1) = h_x_.col(1); // h_x_cur.col(2) = h_x_.col(2); // h_x_cur.col(3) = h_x_.col(3); // h_x_cur.col(4) = h_x_.col(4); // h_x_cur.col(5) = h_x_.col(5); // h_x_cur.col(6) = h_x_.col(6); // h_x_cur.col(7) = h_x_.col(7); // h_x_cur.col(8) = h_x_.col(8); // h_x_cur.col(9) = h_x_.col(9); // h_x_cur.col(10) = h_x_.col(10); // h_x_cur.col(11) = h_x_.col(11); // */ // P_ = L_ - K_*h_x_cur * P_; // } // else //{ P_ = L_ - K_x.template block<n, 12>(0, 0) * P_.template block<12, n>(0, 0); //} solve_time += omp_get_wtime() - solve_start; return; } solve_time += omp_get_wtime() - solve_start; } }