源码阅读，能力有限，如有某处理解错误，请指出，谢谢。
PriorFactor.h：基于速度残差构建非线性优化问题
#pragma once
#include <ros/assert.h>
#include <iostream>
#include <eigen3/Eigen/Dense>
#include "utils/math_tools.h"
#include <ceres/ceres.h>
struct SpeedBiasPriorFactorAutoDiff
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
SpeedBiasPriorFactorAutoDiff(vector<double> speedBias) : speedBias_(speedBias) {}
template <typename T>
bool operator()(const T *speedBias, T *residuals) const
{
residuals[0] = T(8) * (speedBias[0] - T(speedBias_[0]));
residuals[1] = T(8) * (speedBias[1] - T(speedBias_[1]));
residuals[2] = T(1) * (speedBias[2] - T(speedBias_[2]));
residuals[3] = T(1) * (speedBias[3] - T(speedBias_[3]));
residuals[4] = T(1) * (speedBias[4] - T(speedBias_[4]));
residuals[5] = T(1) * (speedBias[5] - T(speedBias_[5]));
residuals[6] = T(1) * (speedBias[6] - T(speedBias_[6]));
residuals[7] = T(1) * (speedBias[7] - T(speedBias_[7]));
residuals[8] = T(1) * (speedBias[8] - T(speedBias_[8]));
return true;
}
static ceres::CostFunction *Create(vector<double> speedBias_)
{
return (new ceres::AutoDiffCostFunction<SpeedBiasPriorFactorAutoDiff, 9, 9>(
new SpeedBiasPriorFactorAutoDiff(speedBias_)));
}
private:
vector<double> speedBias_;
};Preintegration.h：imu预积分及速度评估
#ifndef INTEGRATIONBASE_H_
#define INTEGRATIONBASE_H_
#include <Eigen/Eigen>
#include "utils/math_tools.h"
#include "utils/common.h"
using Eigen::Vector3d;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::MatrixXd;
using Eigen::Quaterniond;//四元数
enum StateOrder {
O_P = 0,
O_R = 3,
O_V = 6,
O_BA = 9,
O_BG = 12,
};
class Preintegration {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Preintegration() = delete;
//将imu基础一些参数进行赋值和计算
//ba、bg表示加速度计和陀螺仪的零偏误差
Preintegration(const Vector3d &acc0, const Vector3d &gyr0,
const Vector3d &linearized_ba, const Vector3d &linearized_bg)
: acc0_{acc0},
gyr0_{gyr0},
linearized_acc_{acc0},
linearized_gyr_{gyr0},
linearized_ba_{linearized_ba},
linearized_bg_{linearized_bg},
jacobian_{Matrix<double, 15, 15>::Identity()},
sum_dt_{0.0},
delta_p_{Vector3d::Zero()},
delta_q_{Quaterniond::Identity()},
delta_v_{Vector3d::Zero()} {
// acc_n = 0.00059;
// gyr_n = 0.000061;
// acc_w = 0.000011;
// gyr_w = 0.000001;
//陀螺仪、加速度计的高斯白噪声和随机游走
nh.param<double>("/IMU/acc_n", acc_n, 0.00059);
nh.param<double>("/IMU/gyr_n", gyr_n, 0.000061);
nh.param<double>("/IMU/acc_w", acc_w, 0.000011);
nh.param<double>("/IMU/gyr_w", gyr_w, 0.000001);
covariance_ = 0.001 * Matrix<double, 15, 15>::Identity();
g_vec_ = -Eigen::Vector3d(0, 0, 9.805);
noise_ = Matrix<double, 18, 18>::Zero();
noise_.block<3, 3>(0, 0) = (acc_n * acc_n) * Matrix3d::Identity();
noise_.block<3, 3>(3, 3) = (gyr_n * gyr_n) * Matrix3d::Identity();
noise_.block<3, 3>(6, 6) = (acc_n * acc_n) * Matrix3d::Identity();
noise_.block<3, 3>(9, 9) = (gyr_n * gyr_n) * Matrix3d::Identity();
noise_.block<3, 3>(12, 12) = (acc_w * acc_w) * Matrix3d::Identity();
noise_.block<3, 3>(15, 15) = (gyr_w * gyr_w) * Matrix3d::Identity();
}
//利用数组，保存时间、加速度计、陀螺仪数据
void push_back(double dt, const Vector3d &acc, const Vector3d &gyr) {
dt_buf_.push_back(dt);
acc_buf_.push_back(acc);
gyr_buf_.push_back(gyr);
Propagate(dt, acc, gyr);
}
void Repropagate(const Vector3d &linearized_ba, const Vector3d &linearized_bg) {
sum_dt_ = 0.0;
acc0_ = linearized_acc_;
gyr0_ = linearized_gyr_;
delta_p_.setZero();
delta_q_.setIdentity();
delta_v_.setZero();
linearized_ba_ = linearized_ba;
linearized_bg_ = linearized_bg;
jacobian_.setIdentity();
covariance_.setZero();
for (size_t i = 0; i < dt_buf_.size(); ++i) {
Propagate(dt_buf_[i], acc_buf_[i], gyr_buf_[i]);
}
}
void MidPointIntegration(double dt,
const Vector3d &acc0, const Vector3d &gyr0,
const Vector3d &acc1, const Vector3d &gyr1,
const Vector3d &delta_p, const Quaterniond &delta_q,
const Vector3d &delta_v, const Vector3d &linearized_ba,
const Vector3d &linearized_bg, Vector3d &result_delta_p,
Quaterniond &result_delta_q, Vector3d &result_delta_v,
Vector3d &result_linearized_ba, Vector3d &result_linearized_bg,
bool update_jacobian) {
// 更新离散中值预积分
Vector3d un_acc_0 = delta_q * (acc0 - linearized_ba);
Vector3d un_gyr = 0.5 * (gyr0 + gyr1) - linearized_bg;// 认为前后两帧偏置linearized_bg不变
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * dt / 2, un_gyr(1) * dt / 2, un_gyr(2) * dt / 2);
Vector3d un_acc_1 = result_delta_q * (acc1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
//离散预积分
result_delta_p = delta_p + delta_v * dt + 0.5 * un_acc * dt * dt;
result_delta_v = delta_v + un_acc * dt;
//偏置不变
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
//求出预积分运动方程关于上一刻预积分的jacobian以及预积分误差状态的协方差矩阵
if (update_jacobian) {
Vector3d w_x = 0.5 * (gyr0 + gyr1) - linearized_bg;
Vector3d a_0_x = acc0 - linearized_ba;
Vector3d a_1_x = acc1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
//反对称形式
R_w_x << 0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x << 0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x << 0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
//误差状态运动方程 deltaZi+1 = F*deltaZi + V*N
//F 为jacobian矩阵
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25*delta_q.toRotationMatrix() * R_a_0_x * dt * dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * dt) * dt * dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3) * dt;
F.block<3, 3>(0, 9) = -0.25*(delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * dt * dt;
F.block<3, 3>(0, 12) = -0.1667*result_delta_q.toRotationMatrix() * R_a_1_x * dt * dt * -dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * dt;
F.block<3, 3>(3, 12) = -MatrixXd::Identity(3, 3) * dt;
F.block<3, 3>(6, 3) = -0.5*delta_q.toRotationMatrix() * R_a_0_x * dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * dt) * dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5*(delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * dt;
F.block<3, 3>(6, 12) = -0.5*result_delta_q.toRotationMatrix() * R_a_1_x * dt * -dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
// NOTE: V = Fd * G_c
// FIXME: verify if it is right, the 0.25 part
MatrixXd V = MatrixXd::Zero(15, 18);
V.block<3, 3>(0, 0) = 0.5 * delta_q.toRotationMatrix() * dt * dt;
V.block<3, 3>(0, 3) = -0.25*result_delta_q.toRotationMatrix() * R_a_1_x * dt * dt * 0.5 * dt;
V.block<3, 3>(0, 6) = 0.5 * result_delta_q.toRotationMatrix() * dt * dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5*MatrixXd::Identity(3, 3) * dt;
V.block<3, 3>(3, 9) = 0.5*MatrixXd::Identity(3, 3) * dt;
V.block<3, 3>(6, 0) = 0.5*delta_q.toRotationMatrix() * dt;
V.block<3, 3>(6, 3) = 0.5*-result_delta_q.toRotationMatrix() * R_a_1_x * dt * 0.5 * dt;
V.block<3, 3>(6, 6) = 0.5*result_delta_q.toRotationMatrix() * dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3, 3) * dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3, 3) * dt;
jacobian_ = F * jacobian_; // F的迭代
covariance_ = F * covariance_ * F.transpose() + V * noise_ * V.transpose(); // 当前预积分误差状态的协方差矩阵
}
}
}
void Propagate(double dt, const Vector3d &acc1, const Vector3d &gyr1) {
dt_ = dt;
acc1_ = acc1;
gyr1_ = gyr1;
Vector3d result_delta_p;//delta位置
Quaterniond result_delta_q;//delta姿态
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
//调用MidPointIntegration()函数，中点积分
MidPointIntegration(dt, acc0_, gyr0_, acc1, gyr1, delta_p_, delta_q_, delta_v_,
linearized_ba_, linearized_bg_,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, true);
// 将递推得到的当前预积分量 设置为上一刻 用于递推
delta_p_ = result_delta_p;
delta_q_ = result_delta_q;
delta_v_ = result_delta_v;
linearized_ba_ = result_linearized_ba;
linearized_bg_ = result_linearized_bg;
delta_q_.normalize();// 注意旋转四元数 要归一化
sum_dt_ += dt_;
acc0_ = acc1_;
gyr0_ = gyr1_;
}
Matrix<double, 15, 1> evaluate(const Vector3d &Pi,
const Quaterniond &Qi,
const Vector3d &Vi,
const Vector3d &Bai,
const Vector3d &Bgi,
const Vector3d &Pj,
const Quaterniond &Qj,
const Vector3d &Vj,
const Vector3d &Baj,
const Vector3d &Bgj) {
// NOTE: low cost update jacobian here
Matrix<double, 15, 1> residuals;
residuals.setZero();
Matrix3d dp_dba = jacobian_.block<3, 3>(O_P, O_BA);
Matrix3d dp_dbg = jacobian_.block<3, 3>(O_P, O_BG);
Matrix3d dq_dbg = jacobian_.block<3, 3>(O_R, O_BG);
Matrix3d dv_dba = jacobian_.block<3, 3>(O_V, O_BA);
Matrix3d dv_dbg = jacobian_.block<3, 3>(O_V, O_BG);
Vector3d dba = Bai - linearized_ba_;
Vector3d dbg = Bgi - linearized_bg_; // NOTE: optimized one minus the linearized one
// IMU预积分的结果,消除掉acc bias和gyro bias的影响, 对应IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
Quaterniond corrected_delta_q = delta_q_ * deltaQ(dq_dbg * dbg);
Vector3d corrected_delta_v = delta_v_ + dv_dba * dba + dv_dbg * dbg;
Vector3d corrected_delta_p = delta_p_ + dp_dba * dba + dp_dbg * dbg;
// IMU项residual计算,输入参数是状态的估计值, 上面correct_delta_*是预积分值, 二者求'diff'得到residual
residuals.block<3, 1>(O_P, 0) =
Qi.inverse() * (-0.5 * g_vec_ * sum_dt_ * sum_dt_ + Pj - Pi - Vi * sum_dt_) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2.0 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).normalized().vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (-g_vec_ * sum_dt_ + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}
double dt_;
Vector3d acc0_, gyr0_;
Vector3d acc1_, gyr1_;
const Vector3d linearized_acc_, linearized_gyr_;
Vector3d linearized_ba_, linearized_bg_;
Matrix<double, 15, 15> jacobian_, covariance_;
Matrix<double, 18, 18> noise_;
double sum_dt_;
Vector3d delta_p_;
Quaterniond delta_q_;
Vector3d delta_v_;
std::vector<double> dt_buf_;
std::vector<Vector3d> acc_buf_;
std::vector<Vector3d> gyr_buf_;
Eigen::Vector3d g_vec_;
double nf, cf;
double acc_n;
double gyr_n;
double acc_w;
double gyr_w;
ros::NodeHandle nh;
};
#endif //INTEGRATIONBASE_H_参考链接：
Vins-mono 源码笔记 (3) IMU预积分_SLAM不dunk的博客-CSDN博客
VINS-MONO代码解读---processIMU()+intergrationBase类+imu_factor.h_可即的博客-CSDN博客
MarginalizationFactor.h：定义一些类
#ifndef MARGINALIZATIONFACTOR_H_
#define MARGINALIZATIONFACTOR_H_
#include <ceres/ceres.h>
#include <Eigen/Eigen>
#include <unordered_map>
#include "utils/common.h"
#include "utils/math_tools.h"
const int NUM_THREADS = 4;
//这个类保存了待marg变量（xm）与相关联变量（xb）之间的一个约束因子关系 - Zm
struct ResidualBlockInfo {
ResidualBlockInfo(ceres::CostFunction *_cost_function,
ceres::LossFunction *_loss_function,
std::vector<double *> _parameter_blocks,
std::vector<int> _drop_set)
: cost_function(_cost_function),
loss_function(_loss_function),
parameter_blocks(_parameter_blocks),
drop_set(_drop_set) {}
void Evaluate();//调用cost_function的evaluate函数计算残差 和 雅克比矩阵
ceres::CostFunction *cost_function;//对应ceres中表示约束因子的类
ceres::LossFunction *loss_function;
std::vector<double *> parameter_blocks;//该约束因子相关联的参数块变量
std::vector<int> drop_set;//parameter_blocks中待marg变量的索引
double **raw_jacobians;
std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
Eigen::VectorXd residuals;
};
//这个类保存了优化时上一步边缘化后保留下来的先验信息
class MarginalizationInfo {
public:
~MarginalizationInfo();
int LocalSize(int size) const;
void AddResidualBlockInfo(ResidualBlockInfo *residual_block_info);
void PreMarginalize();//得到每次IMU和Lidar观测对应的参数块，雅克比矩阵，残差值
void Marginalize();//开启多线程构建信息矩阵H和b ，同时从H,b中恢复出线性化雅克比和残差
std::vector<double *> GetParameterBlocks(std::unordered_map<long, double *> &addr_shift);
std::vector<ResidualBlockInfo *> factors;
//这里将参数块分为Xm,Xb,Xr,Xm表示被marg掉的参数块，Xb表示与Xm相连接的参数块，Xr表示剩余的参数块
//那么m=Xm的localsize之和，n为Xb的localsize之和，pos为（Xm+Xb）localsize之和
int m, n;
std::unordered_map<long, int> parameter_block_size; //global size 将被marg掉的约束边相关联的参数块，即将被marg掉的参数块以及与它们直接相连的参数快
int sum_block_size;
std::unordered_map<long, int> parameter_block_idx; //local size <参数块地址，参数块排序好后的索引>，对参数块进行排序，xm排在前面，xb排成后面，使用localsize
std::unordered_map<long, double *> parameter_block_data;//<参数块地址，参数块数据>，需要注意的是这里保存的参数块数据是原始参数块数据的一个拷贝，不再变化，用于记录这些参数块变量在marg时的状态
std::vector<int> keep_block_size; //global size<保留下来的参数块地址，参数块的globalsize>
std::vector<int> keep_block_idx; //local size<保留下来的参数块地址，参数块的索引>，保留下来的参数块是xb
std::vector<double *> keep_block_data;
Eigen::MatrixXd linearized_jacobians;
Eigen::VectorXd linearized_residuals;
const double eps = 1e-8;
};
//该类是优化时表示上一步边缘化后保留下来的先验信息代价因子，变量marginalization_info保存了类似约束测量信息
class MarginalizationFactor : public ceres::CostFunction {
public:
MarginalizationFactor(MarginalizationInfo* _marginalization_info);
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
MarginalizationInfo* marginalization_info;
};
struct ThreadsStruct {
std::vector<ResidualBlockInfo *> sub_factors;
Eigen::MatrixXd A;
Eigen::VectorXd b;
std::unordered_map<long, int> parameter_block_size; //global size
std::unordered_map<long, int> parameter_block_idx; //local size
};
#endif //MARGINALIZATIONFACTOR_H_
680
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
