终于有时间更新实战篇了，本节实战几乎包含了SLAM后端的所有技巧，其中包括：舒尔补/先验Factor/鲁棒核函数/FEJ/BA优化等滑动窗口法的相关技巧，其中构建2D轮式里程计预积分以及绝对位姿观测的10帧滑动窗口，并边缘化最老帧，其中所有雅可比及其残差都可以在本系列博客之前章节找到对应的数学原理，完整版代码可在如下地址找到
https://gitee.com/zl_vslam/slam_optimizer/tree/master/2d_optimize/ch16接下来请看实战。
一. 主函数
int main() {
double acc_noise_std = 0.1;
double init_bg = 0.0;
std::vector<double> timestamps;
std::vector<Eigen::Vector3d> imu_data;
std::vector<Eigen::Vector3d> odometry_data;
std::vector<Eigen::Vector3d> vel_data;
std::vector<Eigen::Vector3d> pose_data;
std::vector<Eigen::Vector3d> gps_data;
read_simulate(init_bg, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);
// initial wheel
WheelOptions wheel_options;
wheel_options.sigma_x = 1e-8;
wheel_options.sigma_y = 1e-8;
wheel_options.sigma_t = 1e-8;
std::shared_ptr<WheelPreIntegration> wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);
Optimizer optimizer(wheel_preintegration);
// initial state
State state;
memset(&state, 0.0, sizeof(state));
std::vector<Eigen::Vector3d> pose_gt = pose_data;
state.timestamp = timestamps[0];
state.p = pose_gt[0].head<2>();
state.R = SO2(pose_gt[0][2]);
state.type = "s";
State last_state = state;
Eigen::Vector3d last_pose;
int id = optimizer.AddVertex(state, true);
int last_id = id;
for(unsigned int i = 1; i < timestamps.size(); i++) {
cout << "\ncurr index ==== : " << i << std::endl;
double delta_time = timestamps[i]-timestamps[i-1];
// cout << "delta_time ==== : " << delta_time << std::endl;
Eigen::Vector3d last_odometry_o = odometry_data[i-1];
Eigen::Vector3d curr_odometry_o = odometry_data[i];
Eigen::Vector3d delta_odometry = curr_odometry_o - last_odometry_o;
std::cout << " last_odometry_o =======: " << last_odometry_o.transpose() << std::endl;
std::cout << " curr_odometry_o =======: " << curr_odometry_o.transpose() << std::endl;
std::cout << " delta_odometry =======: " << delta_odometry.transpose() << std::endl;
Eigen::Vector3d last_pose = pose_data[i-1];
Eigen::Vector3d curr_pose = pose_data[i];
SE2 last_odometry = SE2(last_odometry_o[0], last_odometry_o[1], last_odometry_o[2]);
SE2 current_odometry = SE2(curr_odometry_o[0], curr_odometry_o[1], curr_odometry_o[2]);
wheel_preintegration->Integrate(last_odometry, current_odometry);
State& last_state_t = state;
wheel_preintegration->Predict(last_state_t);
state.type = "s";
id = optimizer.AddVertex(state);
State last_pose_state;
last_pose_state.timestamp = timestamps[i-1];
last_pose_state.p = last_pose.head<2>();
last_pose_state.R = SO2(last_pose[2]);
last_pose_state.type = "p";
State pose_state;
pose_state.timestamp = timestamps[i];
pose_state.p = curr_pose.head<2>();
pose_state.R = SO2(curr_pose[2]);
pose_state.type = "p";
if(i > 0) {
Matrix3d info = Matrix3d::Identity();
State wheel_state;
wheel_state.timestamp = timestamps[i];
wheel_state.p = wheel_preintegration->GetTranslation();
wheel_state.R = wheel_preintegration->GetSO2();
wheel_state.type = "w";
info = wheel_preintegration->GetCov().inverse();
optimizer.AddEdge(PoseGraphEdge(id-1, id, wheel_state, info));
Matrix3d pose_info = 100 * Matrix3d::Identity();
double last_pose_sigma = 1e-15;
double curr_pose_sigma = 1e-15;
Eigen::Matrix<double, 3, 3> last_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();
last_pose_covariance(0,0) = std::pow(last_pose_sigma,2);
last_pose_covariance(1,1) = std::pow(last_pose_sigma,2);
last_pose_covariance(2,2) = std::pow(last_pose_sigma,2);
Eigen::Matrix<double, 3, 3> last_information_matrix = last_pose_covariance.inverse();
Eigen::Matrix<double, 3, 3> curr_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();
curr_pose_covariance(0,0) = std::pow(curr_pose_sigma,2);
curr_pose_covariance(1,1) = std::pow(curr_pose_sigma,2);
curr_pose_covariance(2,2) = std::pow(curr_pose_sigma,2);
Eigen::Matrix<double, 3, 3> curr_information_matrix = curr_pose_covariance.inverse();
optimizer.AddEdge(PoseGraphEdge(id-1, id-1, last_pose_state, last_information_matrix));
optimizer.AddEdge(PoseGraphEdge(id, id, pose_state, curr_information_matrix));
std::cout << " wheel dp_ =======: " << wheel_preintegration->dp_.transpose() << std::endl;
std::cout << " wheel dR_ =======: " << wheel_preintegration->dR_.log() << std::endl;
optimizer.Optimize(20);
optimizer.UpdateState(state, id);
wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);
cout << "After step " << i << ":" << endl;
optimizer.PrintPoses();
cout << "----------------------" << endl;
// cout << "curr_odometry ==== : " << curr_odometry_o.transpose() << std::endl;
// cout << "delta_odometry ==== : " << delta_odometry.transpose() << std::endl;
// cout << "curr_velocity ==== : " << curr_velocity.transpose() << std::endl;
cout << "gt_pose ==== : " << curr_pose.transpose() << std::endl;
// std::cout << "optimize state.R.log() ============= : " << state.R.log() << std::endl;
std::cout << "optimize state.pose ============= : " << state.p.transpose() << " " << state.R.log() << std::endl;
// std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;
last_id = id;
last_state = state;
usleep(150000);
}
}
return 0;
}与之前相似，相信大家能够明白
二. 轮式里程计预积分
void WheelPreIntegration::Integrate(const SE2& last_odometry, const SE2& current_odometry) {
// preintegration
SE2 odok = current_odometry - last_odometry;
Eigen::Vector2d odork = odok.translation();
Eigen::Matrix2d Phi_ik = dR_.matrix();
dp_ += Phi_ik * odork;
dR_ = dR_ * odok.so2();
// std::cout << " Phi_ik =======: " << Phi_ik << std::endl;
// std::cout << " odork =======: " << odork << std::endl;
// std::cout << " wheel dp_0 =======: " << dp_.transpose() << std::endl;
// std::cout << " wheel dR_0 =======: " << dR_.log() << std::endl;
Eigen::Matrix3d Ak = Eigen::Matrix3d::Identity();
Eigen::Matrix3d Bk = Eigen::Matrix3d::Identity();
Ak.block<2,1>(1,0) = Phi_ik * Eigen::Vector2d(-odork[1], odork[0]);
Bk.block<2,2>(1,1) = Phi_ik;
// Ak.block<2,1>(0,2) = Phi_ik * Eigen::Vector2d(-odork[1], odork[0]);
// Bk.block<2,2>(0,0) = Phi_ik;
Eigen::Matrix3d Sigma_vk = Eigen::Matrix3d::Identity();
Sigma_vk(0,0) = options_.sigma_t * options_.sigma_t;
Sigma_vk(1,1) = options_.sigma_x * options_.sigma_x;
Sigma_vk(2,2) = options_.sigma_y * options_.sigma_y;
cov_ = Ak * cov_ * Ak.transpose() + Bk * Sigma_vk * Bk.transpose();
}这段代码的原理在之前章节也可以找到
三. 预积分Factor
void WheelFactor::ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 3, 1> &residual, Eigen::Matrix<double, 3, 3> &jacobianXi, Eigen::Matrix<double, 3, 3> &jacobianXj) {
Eigen::Matrix2d Ri = SO2(last_state.R).matrix();
Eigen::Vector2d ri = last_state.p;
double ai = SO2(last_state.R).log();
double aj = SO2(state.R).log();
Eigen::Vector2d rj = state.p;
// 公式（24）
residual.head<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;
residual[2] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log();
// compute jacobian matrix
Eigen::Vector2d rij = rj-ri;
Eigen::Vector2d rij_x(-rij[1], rij[0]);
// 公式（25）
jacobianXi.block<2,2>(0,0) = -Ri.transpose();
jacobianXi.block<2,1>(0,2) = -Ri.transpose() * rij_x;
jacobianXi.block<1,2>(2,0).setZero();
jacobianXi(2,2) = -1;
jacobianXj.setIdentity();
jacobianXj.block<2,2>(0,0) = Ri.transpose();
}四.位姿Factor
// predict - obs
void PoseObserve::ComputeResidualAndJacobianPoseGraph(const State& state, const State& pose_state, Eigen::Matrix<double, 3, 1> &residualXi, Eigen::Matrix<double, 3, 3> &jacobianXi) {
Eigen::Vector2d res_p = state.p - pose_state.p;
double res_theta = math_utils::theta_normalize(SO2(state.R).log() - SO2(pose_state.R).log());
residualXi.block<2,1>(0,0) = res_p;
residualXi(2,0) = res_theta;
// cout << "residualXi ================== : " << residualXi << endl;
jacobianXi = Eigen::Matrix<double, 3, 3>::Identity();
// cout << "jacobianXi == : \n" << jacobianXi << endl;
}五. 相关修改位置
for (size_t edge_idx : related_edges) {
const auto& edge = edges_[edge_idx];
if (!vertices_.count(edge.id1) || !vertices_.count(edge.id2)) continue;
State xi = vertices_.at(edge.id1), xj = vertices_.at(edge.id2);
Vector3d e;
Eigen::Matrix<double, 3, 3> jacobianXi, jacobianXj;
if(edge.measurement.type == "w") {
wheel_factor_->ComputeResidualAndJacobianPoseGraph(xi, xj, e, jacobianXi, jacobianXj);
MatrixXd weighted_H;
VectorXd weighted_e;
std::tie(weighted_H, weighted_e) = robust_kernel_->robustify(e);
Matrix3d Omega_robust = edge.information * weighted_H;
Vector3d e_robust = weighted_e;
int idx1 = local_indices[edge.id1], idx2 = local_indices[edge.id2];
// cout << "w idx1 ==== : " << idx1 << " , " << idx2 << std::endl;
H_local.block<3, 3>(3*idx1, 3*idx1) += jacobianXi.transpose() * Omega_robust * jacobianXi;
H_local.block<3, 3>(3*idx1, 3*idx2) += jacobianXi.transpose() * Omega_robust * jacobianXj;
H_local.block<3, 3>(3*idx2, 3*idx1) += jacobianXj.transpose() * Omega_robust * jacobianXi;
H_local.block<3, 3>(3*idx2, 3*idx2) += jacobianXj.transpose() * Omega_robust * jacobianXj;
b_local.segment<3>(3*idx1) += jacobianXi.transpose() * Omega_robust * e_robust;
b_local.segment<3>(3*idx2) += jacobianXj.transpose() * Omega_robust * e_robust;
}
// cout << "edge.id1 ==== : " << edge.id1 << std::endl;
// cout << "edge.id2 ==== : " << edge.id2 << std::endl;
if(edge.measurement.type == "p") {
if(edge.id1 == edge.id2) {
PoseObserve::ComputeResidualAndJacobianPoseGraph(xi, edge.measurement, e, jacobianXi);
MatrixXd weighted_H;
VectorXd weighted_e;
std::tie(weighted_H, weighted_e) = robust_kernel_->robustify(e);
Matrix3d Omega_robust = edge.information * weighted_H;
Vector3d e_robust = weighted_e;
int idx1 = local_indices[edge.id1], idx2 = local_indices[edge.id2];
// cout << "p idx1 ==== : " << idx1 << " , " << idx2 << std::endl;
H_local.block<3, 3>(3*idx1, 3*idx1) += jacobianXi.transpose() * Omega_robust * jacobianXi;
// H_local.block<3, 3>(3*idx1, 3*idx2) += jacobianXi.transpose() * Omega_robust * jacobianXj;
// H_local.block<3, 3>(3*idx2, 3*idx1) += jacobianXj.transpose() * Omega_robust * jacobianXi;
// H_local.block<3, 3>(3*idx2, 3*idx2) += jacobianXj.transpose() * Omega_robust * jacobianXj;
b_local.segment<3>(3*idx1) += jacobianXi.transpose() * Omega_robust * e_robust;
// b_local.segment<3>(3*idx2) += jacobianXj.transpose() * Omega_robust * e_robust;
}
}
}主要是替换上节代码相应位置的残差及雅可比，其它基本没有改动，就不作过多说明了
总结
经过这样一个10帧的滑动窗口法，算法即可获得优化效果，其中需要调节协方差相关参数，即可影响结果，本代码中，融合的是真值位姿，因此最终结果也收敛到了真值上，大家若想体验其它效果，可自行调节sigma之类的参数，本节更新较晚，主要还是代码调试需要时间，本节更新完，2D图优化也将进入终极篇了，终极篇预告：2D视觉惯性VIO，敬请期待