//todo
/**
*IMU预积分中采用中值积分地推jacobian和covariance
**/
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)中值法计算预积分
处理IMU数据的函数。采用的是中值法计算预积分，对应的公式为
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);//(4-1)
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;//(5) w
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);//(3)--->un_gyr(0) * _dt / 2 (5)
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);//(4-2)
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);//(4)
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;//(1)
result_delta_v = delta_v + un_acc * _dt;//(2) --> un_acc(4)
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg; 推jacobian和covariance
jacobian.setIdentity();
covariance.setZero();
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;//反对称矩阵
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.25 * 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) = -1.0 * 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();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * 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.25 * 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;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
noise = Eigen::Matrix<double, 18, 18>::Zero();
noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();雅克比矩阵的使用
void EdgeImu::ComputeJacobians() {
...............
}
jacobians_[3] Eigen::Quaterniond corrected_delta_q /jacobian_speedbias_i
covariance的使用
评估残差: residual = pre_integration->evaluate;
covariance = F * covariance * F.transpose() + V * noise * V.transpose(); Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();
residual = sqrt_info * residual;计算残差中使用 (设置信息矩阵)
void EdgeImu::ComputeResidual() {
.....................
residual_ = pre_integration_->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
// Mat1515 sqrt_info = Eigen::LLT< Mat1515 >(pre_integration_->covariance.inverse()).matrixL().transpose();
SetInformation(pre_integration_->covariance.inverse());
}edge.h
/// 返回残差
VecX Residual() const { return residual_; }
/// 返回雅可比
std::vector<MatXX> Jacobians() const { return jacobians_; }
/// 设置信息矩阵
void SetInformation(const MatXX &information) {
information_ = information;
// sqrt information
sqrt_information_ = Eigen::LLT<MatXX>(information_).matrixL().transpose();
}Eigen::LLT<MatXX>(information_)LLT.h
template<typename InputType>
explicit LLT(const EigenBase<InputType>& matrix)
: m_matrix(matrix.rows(), matrix.cols()),
m_isInitialized(false)
{
compute(matrix.derived());
}
信息矩阵
1. 协方差矩阵
在统计学中，方差是用来度量单个随机变量的离散程度，而协方差则一般用来刻画两个随机变量的相似程度. 其中，方差的计算公式为
其中，n 表示样本量，符号
表示观测样本的均值。
在此基础上，协方差的计算公式被定义为
在公式中，符号
分别表示两个随机变量所对应的观测样本均值，据此，我们发现：方差
可视作随机变量
关于其自身的协方差 .
2. 信息矩阵和协方差矩阵
1：运动方程有协方差矩阵R，观测方程有协方差矩阵Q。它们表示的意义是，到当前时刻t为止，所有测量的样本的协方差矩阵，用来衡量本次测量的不确定性。
2：信息矩阵是协方差矩阵的逆，用来表示本次测量的可靠性，即不确定越小，则可靠性就越大。
3：因此，公式推导里出现的相邻两个状态之间的协方差矩阵，实际上是到当前状态为止，之前所有样本的协方差矩阵。也就是说协方差矩阵随着样本的增加在不断的更新。
3. 协方差矩阵和信息矩阵在SLAM中的应用
经过优化后，误差只是减少并不是完全消除，不能消除的误差去那了呢，当然是每条边分摊喽，那么问题来了，每条边误差分担多少呢,难道每条边都分担一样多的误差吗，当然也是可以的，但这样明显不科学，因为有些边在构建的时候已经很准确了，所以这个时候就有了信息矩阵，它表达的是每条边要分摊总误差的多少，嗯 ，这样就科学多了。提一句，如果你的信息矩阵是单位矩阵，那么误差就是均摊啦。(误差项的权重)
但是信息矩阵是协方差矩阵的一个逆矩阵，这个怎么理解呢？没什么意义，香农形式，这里是推导出来的，只是换了一种数学表达形式，那为什么需要信息矩阵呢？
因为信息矩阵在计算条件概率分布明显比协方差矩阵要方便，显然，协方差矩阵要求逆矩阵，所以时间复杂度是O(n^3). 之后我们可以在图优化slam中可以看到，因为图优化优化后的解是无穷多个的，比如说x1->x2->x3, 每个xi相隔1m这是我们实际观测出来的，优化后，我们会得出永远得不出x1 x2 x3的唯一解，因为他们有可能123 可能是234 blabla 但是如果我们提供固定值比如说x2 坐标是3那么解那么就有唯一解234，提供固定值x2这件事情其实就是个先验信息，提供先验信息，求分布，那就是条件分布，也就是这里我们要用到信息矩阵。
4.最小二乘计算误差
另外，因为是最小二乘计算误差，误差项取平方，所以误差项的权重（信息矩阵）也要对应σ的平方。
极大似然估计中，信息矩阵、Hessian矩阵和协方差矩阵的关系
1. Fisher Information Matrix 和 Hessian of Log Likelihood
这个博客根据Fisher Information的定义，非常清晰地证明了为什么Fisher Information Matrix和负的Hessian of log likelihood是相等的（关键步骤是二阶导运算符和积分可以互换位置！）。
2. Hessian of Negative Log Likelihood 和 Covariance Matrix
高斯分布假设下，maximum likelihood的等效结果是minimize negative log likelihood（根据高斯分布的概率密度函数可以看出）。同时注意到，negative log likelihood的二阶导数（也就是其Hessian），正好是协方差的逆，也就是说此Hessian of Negative Log Likelihood即Inverse of Covariance Matrix。
这个结论可以继续往下推广：
当高斯分布的均值是关于状态的线性函数时，negative log likelihood的二阶导数（也就是其Hessian），正好是这个线性变换后的新状态的的协方差的逆，此时也有Hessian of Negative Log Likelihood (about the original state)等于Inverse of (new) Covariance Matrix。
当高斯分布的均值是关于状态的非线性函数时，可以做一个线性化将其展开乘线性形式，于是根据上一段的结论，此时Approximate Hessian of Negative Log Likelihood (about the original state)等于Approximate Inverse of (new) Covariance Matrix近似于Inverse of (new) Covariance Matrix。
另外，这里也有一份pdf阐述了我的上述理解。
3. 总结
注意到negative log likelihood其实就得到了我们非常熟悉的指标函数了，在高斯牛顿法中，指标函数做线性展开时得到的Hessian，实际就是前面所说的Approximate Hessian of Negative Log Likelihood (about the original state)，这个Hessian，从一个方向近似等于Fisher信息矩阵，从另一个方向则近似等于协方差矩阵的逆。
REF：
1. 如何直观理解协方差矩阵
3. OrbSLAM的Optimizer函数用到的信息矩阵如何理解
4. SLAM优化中信息矩阵
5. PCA中的协方差
6. 拓展学习： 高斯分布与边缘化
583
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
