方法来源 高翔《自动驾驶与机器人中的SLAM技术》,所在slam_in_autonomous_driving-master/src/ch3/static_imu_init.cc
源码链接:https://github.com/gaoxiang12/slam_in_autonomous_driving
//Vec3d 为Eigen::Vector3d
//init_imu_deque 为存放imu的队列
bool StaticIMUInit::TryInit() {
if (init_imu_deque_.size() < 10) {
return false;
}
// 计算均值和方差
Vec3d mean_gyro, mean_acce;
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU& imu) { return imu.gyro_; });
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });
// 以acce均值为方向,取9.8长度为重力
LOG(INFO) << "mean acce: " << mean_acce.transpose();
gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;
// 重新计算加计的协方差
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,
[this](const IMU& imu) { return imu.acce_ + gravity_; });
// 检查IMU噪声
if (cov_gyro_.norm() > options_.max_static_gyro_var) {
LOG(ERROR) << "陀螺仪测量噪声太大" << cov_gyro_.norm() << " > " << options_.max_static_gyro_var;
return false;
}
if (cov_acce_.norm() > options_.max_static_acce_var) {
LOG(ERROR) << "加计测量噪声太大" << cov_acce_.norm() << " > " << options_.max_static_acce_var;
return false;
}
// 估计测量噪声和零偏
init_bg_ = mean_gyro;
init_ba_ = mean_acce;
LOG(INFO) << "IMU 初始化成功,初始化时间= " << current_time_ - init_start_time_ << ", bg = " << init_bg_.transpose()
<< ", ba = " << init_ba_.transpose() << ", gyro sq = " << cov_gyro_.transpose()
<< ", acce sq = " << cov_acce_.transpose() << ", grav = " << gravity_.transpose()
<< ", norm: " << gravity_.norm();
LOG(INFO) << "mean gyro: " << mean_gyro.transpose() << " acce: " << mean_acce.transpose();
init_success_ = true;
return true;
}
/**
* 计算一个容器内数据的均值与对角形式协方差
* @tparam C 容器类型
* @tparam D 结果类型
* @tparam Getter 获取数据函数, 接收一个容器内数据类型,返回一个D类型
*/
template <typename C, typename D, typename Getter>
void ComputeMeanAndCovDiag(const C& data, D& mean, D& cov_diag, Getter&& getter) {
size_t len = data.size();
assert(len > 1);
// clang-format off
mean = std::accumulate(data.begin(), data.end(), D::Zero().eval(),
[&getter](const D& sum, const auto& data) -> D { return sum + getter(data); }) / len;
cov_diag = std::accumulate(data.begin(), data.end(), D::Zero().eval(),
[&mean, &getter](const D& sum, const auto& data) -> D {
return sum + (getter(data) - mean).cwiseAbs2().eval();
}) / (len - 1);
// clang-format on
}