OpenVINS学习4——ov_init代码解析
2023-12-14 23:01:16
一、总体概述
ov_init文件夹内是初始化的代码程序，主要是静态初始化和动态初始化两种，原理已经在上一篇文章中讲过。根据官方文档，共有以下class:
class?DynamicInitializer
动态初始化
class?Factor_GenericPrior
特定类型的通用状态先验因子
class?Factor_ImageReprojCalib
带校准的特征承载观察（原始）因子
class?Factor_ImuCPIv1
IMU从第一张图片连续积分因子
class?InertialInitializer
VIO系统的初始化
struct?InertialInitializerOptions
存放状态估计所有需要选项的结构体
class?InitializerHelper
有一堆用于动态初始化的辅助函数（都是静态的）
class?SimulatorInit
生成视觉惯性测量的主模拟器类
class?State_JPLQuatLocal
JPL 四元数 CERES 状态参数化。
class?StaticInitializer
静态初始化
二、关键代码介绍
1、静态初始化
该实现假设 IMU 从静止状态开始。从静止状态初始化：
（1）收集所有惯性测量值
（2）查看在最后一个窗口内是否有加速度跳跃
（3）如果跳跃超过了我们的阈值，我们应该初始化（即我们已经开始移动）
（4）使用前一个窗口，该窗口应该是固定的来初始化方向
（5）返回与重力和偏差一致的横滚和俯仰。
这将检查我们的加速度是否有足够大的跳跃。如果有的话，我们将使用这次跳转之前的时间段来初始化状态。这假设我们的 imu 静止不动并且没有移动（因此如果我们经历恒定的加速，这将会失败）。如果我们不等待跳转（即 wait_for_jerk 为 false），那么系统将尝试尽快初始化。仅当您启用零速度更新来处理静止情况时才建议这样做。要在这种情况下进行初始化，我们需要使平均角度方差低于设定的阈值（即我们需要保持静止）。
静态初始化函数代码注释：
/*
功能：
尝试仅使用 imu 来静态初始化系统。
参数：
timestamp 我们已初始化状态的时间戳（为输出量）
covariance 返回状态的计算协方差（为输出量）
order 协方差矩阵的阶数（为输出量）
t_imu 我们的imu类型元素（为输出量）
wait_for_jerk 如果为 true，我们将等待“jerk”
返回值：
如果我们已成功初始化系统，则函数返回 True
*/
bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<Type>> &order,
std::shared_ptr<IMU> t_imu, bool wait_for_jerk) {
// Return if we don't have any measurements
//如果没有测量值就返回
if (imu_data->size() < 2) {
return false;
}
// Newest and oldest imu timestamp
//获得最新的和最老的IMU时间戳
double newesttime = imu_data->at(imu_data->size() - 1).timestamp;
double oldesttime = imu_data->at(0).timestamp;
// Return if we don't have enough for two windows
//如果时间窗口太短则返回
if (newesttime - oldesttime < params.init_window_time) {
PRINT_INFO(YELLOW "[init-s]: unable to select window of IMU readings, not enough readings\n" RESET);
return false;
}
// First lets collect a window of IMU readings from the newest measurement to the oldest
std::vector<ImuData> window_1to0, window_2to1;
for (const ImuData &data : *imu_data) {//收集时间窗口内的IMU测量数据，将时间窗口按照时间长度平分为前后两部分
if (data.timestamp > newesttime - 0.5 * params.init_window_time && data.timestamp <= newesttime - 0.0 * params.init_window_time) {
window_1to0.push_back(data);//后半段时间窗口数据
}
if (data.timestamp > newesttime - 1.0 * params.init_window_time && data.timestamp <= newesttime - 0.5 * params.init_window_time) {
window_2to1.push_back(data);//前半段时间窗口数据
}
}
// Return if both of these failed
//如果收集都失败则返回
if (window_1to0.size() < 2 || window_2to1.size() < 2) {
PRINT_INFO(YELLOW "[init-s]: unable to select window of IMU readings, not enough readings\n" RESET);
return false;
}
// Calculate the sample variance for the newest window from 1 to 0
//计算后半段时间窗口的样本方差
Eigen::Vector3d a_avg_1to0 = Eigen::Vector3d::Zero();
for (const ImuData &data : window_1to0) {
a_avg_1to0 += data.am;
}
a_avg_1to0 /= (int)window_1to0.size();
double a_var_1to0 = 0;
for (const ImuData &data : window_1to0) {
a_var_1to0 += (data.am - a_avg_1to0).dot(data.am - a_avg_1to0);
}
a_var_1to0 = std::sqrt(a_var_1to0 / ((int)window_1to0.size() - 1));
// Calculate the sample variance for the second newest window from 2 to 1
//计算前半段时间窗口的样本方差
Eigen::Vector3d a_avg_2to1 = Eigen::Vector3d::Zero();
Eigen::Vector3d w_avg_2to1 = Eigen::Vector3d::Zero();
for (const ImuData &data : window_2to1) {
a_avg_2to1 += data.am;
w_avg_2to1 += data.wm;
}
a_avg_2to1 = a_avg_2to1 / window_2to1.size();
w_avg_2to1 = w_avg_2to1 / window_2to1.size();
double a_var_2to1 = 0;
for (const ImuData &data : window_2to1) {
a_var_2to1 += (data.am - a_avg_2to1).dot(data.am - a_avg_2to1);
}
a_var_2to1 = std::sqrt(a_var_2to1 / ((int)window_2to1.size() - 1));
PRINT_DEBUG(YELLOW "[init-s]: IMU excitation stats: %.3f,%.3f\n" RESET, a_var_2to1, a_var_1to0);
// If it is below the threshold and we want to wait till we detect a jerk
//如果方差低于阈值，我们想要等待直到检测到急动加速（jerk）
if (a_var_1to0 < params.init_imu_thresh && wait_for_jerk) {
PRINT_INFO(YELLOW "[init-s]: no IMU excitation, below threshold %.3f < %.3f\n" RESET, a_var_1to0, params.init_imu_thresh);
return false;
}
// We should also check that the old state was below the threshold!
// This is the case when we have started up moving, and thus we need to wait for a period of stationary motion
//我们也需要检查过去的状态方差（前半段时间窗口）在阈值之下
//这是当我们开始运动时的情况，因此我们需要等待一段时间的静止运动
if (a_var_2to1 > params.init_imu_thresh && wait_for_jerk) {
PRINT_INFO(YELLOW "[init-s]: to much IMU excitation, above threshold %.3f > %.3f\n" RESET, a_var_2to1, params.init_imu_thresh);
return false;
}
// If it is above the threshold and we are not waiting for a jerk
// Then we are not stationary (i.e. moving) so we should wait till we are
//如果时间窗口前半段和后半段方差都大于阈值了，则不需要等待急动加速了
//这是说明不是静止状态，需要等待至静止状态
if ((a_var_1to0 > params.init_imu_thresh || a_var_2to1 > params.init_imu_thresh) && !wait_for_jerk) {
PRINT_INFO(YELLOW "[init-s]: to much IMU excitation, above threshold %.3f,%.3f > %.3f\n" RESET, a_var_2to1, a_var_1to0,
params.init_imu_thresh);
return false;
}
// Get rotation with z axis aligned with -g (z_in_G=0,0,1)
//通过z轴对齐重力方向获得一个旋转，从而对齐全局坐标系和惯性坐标系
Eigen::Vector3d z_axis = a_avg_2to1 / a_avg_2to1.norm();//令z轴为重力方向单位向量，静止时为重力向量除重力范数
Eigen::Matrix3d Ro;
InitializerHelper::gram_schmidt(z_axis, Ro);//给定一个重力方向的向量，计算从惯性参考系到该向量的旋转
Eigen::Vector4d q_GtoI = rot_2_quat(Ro);//旋转转为四元数
// Set our biases equal to our noise (subtract our gravity from accelerometer bias)
// 对于测量窗口前半段（都是静止状态），将我们的偏差设置为等于我们的噪声（从加速度计偏差中减去我们的重力）
//因为重力加速度一直影响着加速度计结果
Eigen::Vector3d gravity_inG;
gravity_inG << 0.0, 0.0, params.gravity_mag;
Eigen::Vector3d bg = w_avg_2to1;
Eigen::Vector3d ba = a_avg_2to1 - quat_2_Rot(q_GtoI) * gravity_inG;
// Set our state variables
//设置状态变量
timestamp = window_2to1.at(window_2to1.size() - 1).timestamp;//前半段静止测量窗口的时间戳
Eigen::VectorXd imu_state = Eigen::VectorXd::Zero(16);//IMU状态向量16维
imu_state.block(0, 0, 4, 1) = q_GtoI;//对齐全局坐标系和惯性坐标系的旋转四元数，4维
imu_state.block(10, 0, 3, 1) = bg;//角速度值，3维
imu_state.block(13, 0, 3, 1) = ba;//加速度值，3维
assert(t_imu != nullptr);
t_imu->set_value(imu_state);//设置估计的值
t_imu->set_fej(imu_state);//设置第一次估计的值
// Create base covariance and its covariance ordering
// 创建基础协方差及其协方差排序
order.clear();
order.push_back(t_imu);
covariance = std::pow(0.02, 2) * Eigen::MatrixXd::Identity(t_imu->size(), t_imu->size());
covariance.block(0, 0, 3, 3) = std::pow(0.02, 2) * Eigen::Matrix3d::Identity(); // q 3*3
covariance.block(3, 3, 3, 3) = std::pow(0.05, 2) * Eigen::Matrix3d::Identity(); // p 3*3
covariance.block(6, 6, 3, 3) = std::pow(0.01, 2) * Eigen::Matrix3d::Identity(); // v (static) 3*3
// A VIO system has 4dof unobservable directions which can be arbitrarily picked.
// This means that on startup, we can fix the yaw and position to be 100 percent known.
// TODO: why can't we set these to zero and get a good NEES realworld result?
// Thus, after determining the global to current IMU orientation after initialization, we can propagate the global error
// into the new IMU pose. In this case the position is directly equivalent, but the orientation needs to be propagated.
// We propagate the global orientation into the current local IMU frame
// R_GtoI = R_GtoI*R_GtoG -> H = R_GtoI
// Eigen::Matrix3d R_GtoI = quat_2_Rot(q_GtoI);
// covariance(2, 2) = std::pow(1e-4, 2);
// covariance.block(0, 0, 3, 3) = R_GtoI * covariance.block(0, 0, 3, 3) * R_GtoI.transpose();
// covariance.block(3, 3, 3, 3) = std::pow(1e-3, 2) * Eigen::Matrix3d::Identity();
// Return :D
return true;
}
2、动态初始化
此实现将尝试恢复系统的初始条件。此外，我们将尝试恢复系统的协方差。以任意运动初始化：
（1）预积分我们的系统以获得相对旋转变化（假设偏差已知）
（2）构建具有恢复速度特征的线性系统（用 |g| 约束求解）
（3）对所有校准执行大型 MLE 并恢复协方差。
参考文献：Dong-Si, Tue-Cuong, and Anastasios I. Mourikis. "Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration." 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012.
动态初始化函数代码注释：
/*
功能：
尝试获取初始化的系统。
参数：
timestamp 我们已初始化状态的时间戳（最后的imu状态）（为输出量）
covariance 返回状态的计算协方差 （为输出量）
order 协方差矩阵的阶数 （为输出量）
_imu 指向“活动”IMU 状态的指针（q_GtoI、p_IinG、v_IinG、bg、ba）
_clones_IMU 成像时间和克隆姿态之间的映射（q_GtoIi、p_IiinG）
_features_SLAM 我们当前的 SLAM 功能集（3d 位置）
返回值：
如果我们已成功初始化系统，则返回 True
*/
bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<ov_type::Type>> &order,
std::shared_ptr<ov_type::IMU> &_imu, std::map<double, std::shared_ptr<ov_type::PoseJPL>> &_clones_IMU,
std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> &_features_SLAM) {
// Get the newest and oldest timestamps we will try to initialize between!
//获得我们尝试初始化的最新和最老时间戳
auto rT1 = boost::posix_time::microsec_clock::local_time();//获得当地时间
double newest_cam_time = -1;//初始化最新摄像头时间
for (auto const &feat : _db->get_internal_data()) {//遍历所有特征的时间戳，找到最新的相机时间
for (auto const &camtimepair : feat.second->timestamps) {
for (auto const &time : camtimepair.second) {
newest_cam_time = std::max(newest_cam_time, time);
}
}
}
double oldest_time = newest_cam_time - params.init_window_time;//最老的时间等于最新相机时间减去测量窗口的大小
if (newest_cam_time < 0 || oldest_time < 0) {//如果最新的相机时间小于0，或者最新的相机时间与最老的时间之间的差距没有达到测量窗口的大小，则返回
return false;
}
// Remove all measurements that are older than our initialization window
// Then we will try to use all features that are in the feature database!
//对测量窗口内的数据进行处理
//移除所有比初始化窗口还要老的测量
//之后我们将使用在特征数据库中的所有特征
_db->cleanup_measurements(oldest_time);//移除数据库中老的测量
bool have_old_imu_readings = false;
auto it_imu = imu_data->begin();
while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + params.calib_camimu_dt) {//当imu数据的时间戳比最老的时间戳还要老（超出测量窗口）
have_old_imu_readings = true;//标记含有旧的imu测量
it_imu = imu_data->erase(it_imu);//删除当前的imu测量
}
if (_db->get_internal_data().size() < 0.75 * params.init_max_features) {//如果特征点不够则返回
PRINT_WARNING(RED "[init-d]: only %zu valid features of required (%.0f thresh)!!\n" RESET, _db->get_internal_data().size(),
0.95 * params.init_max_features);
return false;
}
if (imu_data->size() < 2 || !have_old_imu_readings) {//如果IMU测量值不够或含有旧的imu测量则返回
// PRINT_WARNING(RED "[init-d]: waiting for window to reach full size (%zu imu readings)!!\n" RESET, imu_data->size());
return false;
}
// Now we will make a copy of our features here
// We do this to ensure that the feature database can continue to have new
// measurements appended to it in an async-manor so this initialization
// can be performed in a secondary thread while feature tracking is still performed.
//现在将特征复制。确保特征数据库能够持续有新的测量值附加在异步中，
//因此此初始化可以在辅助线程中执行，同时仍执行特征跟踪。
std::unordered_map<size_t, std::shared_ptr<Feature>> features;
for (const auto &feat : _db->get_internal_data()) {
auto feat_new = std::make_shared<Feature>();
feat_new->featid = feat.second->featid;
feat_new->uvs = feat.second->uvs;
feat_new->uvs_norm = feat.second->uvs_norm;
feat_new->timestamps = feat.second->timestamps;
features.insert({feat.first, feat_new});
}
// ======================================================
// ======================================================
// Settings
//设置
const int min_num_meas_to_optimize = (int)params.init_window_time;//优化的最小测量数量，为测量窗口
const int min_valid_features = 8;//最小有效特征点
// Validation information for features we can use
//对于特征我们可以使用的有效信息
bool have_stereo = false;
int count_valid_features = 0;
std::map<size_t, int> map_features_num_meas;
int num_measurements = 0;
double oldest_camera_time = INFINITY;
std::map<double, bool> map_camera_times;
map_camera_times[newest_cam_time] = true; // always insert final pose始终插入最终姿态
std::map<size_t, bool> map_camera_ids;
double pose_dt_avg = params.init_window_time / (double)(params.init_dyn_num_pose + 1);//姿态的平均时间间隔，窗口时间/窗口时间内使用的姿态数量（均匀分布）
for (auto const &feat : features) {
// Loop through each timestamp and make sure it is a valid pose
//通过每个时间戳的循环确保是一个有效位姿
std::vector<double> times;
std::map<size_t, bool> camids;
for (auto const &camtime : feat.second->timestamps) {
for (double time : camtime.second) {
double time_dt = INFINITY;
for (auto const &tmp : map_camera_times) {
time_dt = std::min(time_dt, std::abs(time - tmp.first));
}
for (auto const &tmp : times) {
time_dt = std::min(time_dt, std::abs(time - tmp));
}
// either this pose is a new one at the desired frequency
// or it is a timestamp that we already have, thus can use for free
//要么这个姿态是所需频率的新姿态，或者它是我们已经拥有的时间戳，因此可以自由使用
if (time_dt >= pose_dt_avg || time_dt == 0.0) {
times.push_back(time);
camids[camtime.first] = true;
}
}
}
// This isn't a feature we should use if there are not enough measurements
//如果没有足够的测量，我们不应该使用此特征（看看特征的时间长度够不够测量窗口长度）
map_features_num_meas[feat.first] = (int)times.size();
if (map_features_num_meas[feat.first] < min_num_meas_to_optimize)
continue;
// If we have enough measurements we should append this feature!
//如果我们有足够的测量结果，我们应该添加此特征！
for (auto const &tmp : times) {
map_camera_times[tmp] = true;
oldest_camera_time = std::min(oldest_camera_time, tmp);
num_measurements += 2;
}
for (auto const &tmp : camids) {
map_camera_ids[tmp.first] = true;
}
if (camids.size() > 1) {
have_stereo = true;
}
count_valid_features++;
}
// Return if we do not have our full window or not enough measurements
// Also check that we have enough features to initialize with
//如果没有完整的窗口或者没有足够的测量则返回，也检查了是否有足够的特征点去初始化
if ((int)map_camera_times.size() < params.init_dyn_num_pose) {
return false;
}
if (count_valid_features < min_valid_features) {//如果没有足够的有效特征点则返回
PRINT_WARNING(RED "[init-d]: only %zu valid features of required %d!!\n" RESET, count_valid_features, min_valid_features);
return false;
}
// Bias initial guesses specified by the launch file
// We don't go through the effort to recover the biases right now since they should be
// Semi-well known before launching or can be considered to be near zero...
// 启动文件指定的偏差初始猜测
// 我们现在不努力恢复偏差，因为它们在启动之前应该是半众所周知的，或者可以被认为接近于零......
Eigen::Vector3d gyroscope_bias = params.init_dyn_bias_g;
Eigen::Vector3d accelerometer_bias = params.init_dyn_bias_a;
// Check that we have some angular velocity / orientation change
//检查我们是否有角速度/旋转的改变
double accel_inI_norm = 0.0;
double theta_inI_norm = 0.0;
double time0_in_imu = oldest_camera_time + params.calib_camimu_dt;
double time1_in_imu = newest_cam_time + params.calib_camimu_dt;
std::vector<ov_core::ImuData> readings = InitializerHelper::select_imu_readings(*imu_data, time0_in_imu, time1_in_imu);//读取imu数据
assert(readings.size() > 2);
for (size_t k = 0; k < readings.size() - 1; k++) {//利用前后两帧数据取平均法获得角速度和加速度，计算角度变化和速度变化
auto imu0 = readings.at(k);
auto imu1 = readings.at(k + 1);
double dt = imu1.timestamp - imu0.timestamp;
Eigen::Vector3d wm = 0.5 * (imu0.wm + imu1.wm) - gyroscope_bias;
Eigen::Vector3d am = 0.5 * (imu0.am + imu1.am) - accelerometer_bias;
theta_inI_norm += (-wm * dt).norm();
accel_inI_norm += am.norm();
}
accel_inI_norm /= (double)(readings.size() - 1);//加速度总体取平均
if (180.0 / M_PI * theta_inI_norm < params.init_dyn_min_deg) {//如果角度累积变化小于动态初始化最小角度设置则返回初始化失败
PRINT_WARNING(YELLOW "[init-d]: gyroscope only %.2f degree change (%.2f thresh)\n" RESET, 180.0 / M_PI * theta_inI_norm,
params.init_dyn_min_deg);
return false;
}
PRINT_DEBUG("[init-d]: |theta_I| = %.4f deg and |accel| = %.4f\n", 180.0 / M_PI * theta_inI_norm, accel_inI_norm);
// // Create feature bearing vector in the first frame
// // This gives us: p_FinI0 = depth * bearing
// Eigen::Vector4d q_ItoC = data_ori.camera_q_ItoC.at(cam_id);
// Eigen::Vector3d p_IinC = data_init.camera_p_IinC.at(cam_id);
// Eigen::Matrix3d R_ItoC = quat_2_Rot(q_ItoC);
// std::map<size_t, Eigen::Vector3d> features_bearings;
// std::map<size_t, int> features_index;
// for (auto const &feat : features) {
// if (map_features_num_meas[feat.first] < min_num_meas_to_optimize)
// continue;
// assert(feat->timestamps.find(cam_id) != feat->timestamps.end());
// double timestamp = data_ori.timestamps_cam.at(cam_id).at(0);
// auto it0 = std::find(feat->timestamps.at(cam_id).begin(), feat->timestamps.at(cam_id).end(), timestamp);
// if (it0 == feat->timestamps.at(cam_id).end())
// continue;
// auto idx0 = std::distance(feat->timestamps.at(cam_id).begin(), it0);
// Eigen::Vector3d bearing;
// bearing << feat->uvs_norm.at(cam_id).at(idx0)(0), feat->uvs_norm.at(cam_id).at(idx0)(1), 1;
// bearing = bearing / bearing.norm();
// bearing = R_ItoC.transpose() * bearing;
// features_bearings.insert({feat->featid, bearing});
// features_index.insert({feat->featid, (int)features_index.size()});
// }
auto rT2 = boost::posix_time::microsec_clock::local_time();
// ======================================================
// ======================================================
// We will recover position of feature, velocity, gravity
// Based on Equation (14) in the following paper:
// https://ieeexplore.ieee.org/abstract/document/6386235
// State ordering is: [features, velocity, gravity]
// Feature size of 1 will use the first ever bearing of the feature as true (depth only..)
//我们将恢复特征的位置、速度、重力
// 基于下面论文中的方程（14）：
// https://ieeexplore.ieee.org/abstract/document/6386235
// 状态排序为：[特征、速度、重力]
// 特征尺寸为 1 将使用该特征的第一个方位作为 true（仅深度..）
const bool use_single_depth = false;
int size_feature = (use_single_depth) ? 1 : 3;
int num_features = count_valid_features;
int system_size = size_feature * num_features + 3 + 3;
// Make sure we have enough measurements to fully constrain the system
//确保有足够的测量值充分约束系统
if (num_measurements < system_size) {//如果测量值小于系统需要的大小则返回
PRINT_WARNING(YELLOW "[init-d]: not enough feature measurements (%d meas vs %d state size)!\n" RESET, num_measurements, system_size);
return false;
}
// Now lets pre-integrate from the first time to the last
//现在让我们从第一次到最后一次进行预积分
assert(oldest_camera_time < newest_cam_time);
double last_camera_timestamp = 0.0;
std::map<double, std::shared_ptr<ov_core::CpiV1>> map_camera_cpi_I0toIi, map_camera_cpi_IitoIi1;
for (auto const &timepair : map_camera_times) {//遍历所有测量时间进行IMU预积分
// No preintegration at the first timestamp
//在第一个时间戳没有积分
double current_time = timepair.first;
if (current_time == oldest_camera_time) {//如果是窗口的第一个时间
map_camera_cpi_I0toIi.insert({current_time, nullptr});
map_camera_cpi_IitoIi1.insert({current_time, nullptr});
last_camera_timestamp = current_time;
continue;
}
// Perform our preintegration from I0 to Ii (used in the linear system)
//执行从 I0 到 Ii 的预积分（用于线性系统）
double cpiI0toIi1_time0_in_imu = oldest_camera_time + params.calib_camimu_dt;
double cpiI0toIi1_time1_in_imu = current_time + params.calib_camimu_dt;
auto cpiI0toIi1 = std::make_shared<ov_core::CpiV1>(params.sigma_w, params.sigma_wb, params.sigma_a, params.sigma_ab, true);
cpiI0toIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);
std::vector<ov_core::ImuData> cpiI0toIi1_readings =
InitializerHelper::select_imu_readings(*imu_data, cpiI0toIi1_time0_in_imu, cpiI0toIi1_time1_in_imu);
if (cpiI0toIi1_readings.size() < 2) {//如果窗口内数据量小于2则返回
PRINT_DEBUG(YELLOW "[init-d]: camera %.2f in has %zu IMU readings!\n" RESET, (cpiI0toIi1_time1_in_imu - cpiI0toIi1_time0_in_imu),
cpiI0toIi1_readings.size());
return false;
}
double cpiI0toIi1_dt_imu = cpiI0toIi1_readings.at(cpiI0toIi1_readings.size() - 1).timestamp - cpiI0toIi1_readings.at(0).timestamp;//获得读取数据的时间戳差
if (std::abs(cpiI0toIi1_dt_imu - (cpiI0toIi1_time1_in_imu - cpiI0toIi1_time0_in_imu)) > 0.01) {//如果读取数据的时间戳差和时间窗口的时间戳差不一致则返回
PRINT_DEBUG(YELLOW "[init-d]: camera IMU was only propagated %.3f of %.3f\n" RESET, cpiI0toIi1_dt_imu,
(cpiI0toIi1_time1_in_imu - cpiI0toIi1_time0_in_imu));
return false;
}
for (size_t k = 0; k < cpiI0toIi1_readings.size() - 1; k++) {
auto imu0 = cpiI0toIi1_readings.at(k);
auto imu1 = cpiI0toIi1_readings.at(k + 1);
cpiI0toIi1->feed_IMU(imu0.timestamp, imu1.timestamp, imu0.wm, imu0.am, imu1.wm, imu1.am);//将顺序计算预积分测量的主要函数。参数包含我们的时间戳和惯性信息，以及测量窗口第一个时间（我们可以丢弃之前测量的时间）。
}
// Perform our preintegration from Ii to Ii1 (used in the mle optimization)
//执行从 Ii 到 Ii1 的预积分（用于 mle 优化）
double cpiIitoIi1_time0_in_imu = last_camera_timestamp + params.calib_camimu_dt;
double cpiIitoIi1_time1_in_imu = current_time + params.calib_camimu_dt;
auto cpiIitoIi1 = std::make_shared<ov_core::CpiV1>(params.sigma_w, params.sigma_wb, params.sigma_a, params.sigma_ab, true);
cpiIitoIi1->setLinearizationPoints(gyroscope_bias, accelerometer_bias);//设置预积分的线性化点
std::vector<ov_core::ImuData> cpiIitoIi1_readings =
InitializerHelper::select_imu_readings(*imu_data, cpiIitoIi1_time0_in_imu, cpiIitoIi1_time1_in_imu);
if (cpiIitoIi1_readings.size() < 2) {
PRINT_DEBUG(YELLOW "[init-d]: camera %.2f in has %zu IMU readings!\n" RESET, (cpiIitoIi1_time1_in_imu - cpiIitoIi1_time0_in_imu),
cpiIitoIi1_readings.size());
return false;
}
double cpiIitoIi1_dt_imu = cpiIitoIi1_readings.at(cpiIitoIi1_readings.size() - 1).timestamp - cpiIitoIi1_readings.at(0).timestamp;
if (std::abs(cpiIitoIi1_dt_imu - (cpiIitoIi1_time1_in_imu - cpiIitoIi1_time0_in_imu)) > 0.01) {
PRINT_DEBUG(YELLOW "[init-d]: camera IMU was only propagated %.3f of %.3f\n" RESET, cpiIitoIi1_dt_imu,
(cpiIitoIi1_time1_in_imu - cpiIitoIi1_time0_in_imu));
return false;
}
for (size_t k = 0; k < cpiIitoIi1_readings.size() - 1; k++) {
auto imu0 = cpiIitoIi1_readings.at(k);
auto imu1 = cpiIitoIi1_readings.at(k + 1);
cpiIitoIi1->feed_IMU(imu0.timestamp, imu1.timestamp, imu0.wm, imu0.am, imu1.wm, imu1.am);
}
// Finally push back our integrations!
//将积分值保存
map_camera_cpi_I0toIi.insert({current_time, cpiI0toIi1});
map_camera_cpi_IitoIi1.insert({current_time, cpiIitoIi1});
last_camera_timestamp = current_time;
}
// Loop through each feature observation and append it!
// State ordering is: [features, velocity, gravity]
//循环遍历每个特征观测并附加它！
//状态顺序为【特征，速度，重力】
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_measurements, system_size);
Eigen::VectorXd b = Eigen::VectorXd::Zero(num_measurements);
PRINT_DEBUG("[init-d]: system of %d measurement x %d states created (%d features, %s)\n", num_measurements, system_size, num_features,
(have_stereo) ? "stereo" : "mono");
int index_meas = 0;
int idx_feat = 0;
std::map<size_t, int> A_index_features;
for (auto const &feat : features) {//遍历每个特征
if (map_features_num_meas[feat.first] < min_num_meas_to_optimize)
continue;
if (A_index_features.find(feat.first) == A_index_features.end()) {
A_index_features.insert({feat.first, idx_feat});
idx_feat += 1;
}
for (auto const &camtime : feat.second->timestamps) {//遍历每个特征的每个时间
// This camera
size_t cam_id = camtime.first;
Eigen::Vector4d q_ItoC = params.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);//相机和IMU的旋转四元数
Eigen::Vector3d p_IinC = params.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);//相机和IMU的位置平移
Eigen::Matrix3d R_ItoC = quat_2_Rot(q_ItoC);
// Loop through each observation
for (size_t i = 0; i < camtime.second.size(); i++) {//遍历每个观测
// Skip measurements we don't have poses for
//跳过没有位姿的测量
double time = feat.second->timestamps.at(cam_id).at(i);
if (map_camera_times.find(time) == map_camera_times.end())
continue;
// Our measurement
//我们的测量
Eigen::Vector2d uv_norm;
uv_norm << (double)feat.second->uvs_norm.at(cam_id).at(i)(0), (double)feat.second->uvs_norm.at(cam_id).at(i)(1);
// Preintegration values
//求得预积分值
double DT = 0.0;
Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
if (map_camera_cpi_I0toIi.find(time) != map_camera_cpi_I0toIi.end() && map_camera_cpi_I0toIi.at(time) != nullptr) {
DT = map_camera_cpi_I0toIi.at(time)->DT;
R_I0toIk = map_camera_cpi_I0toIi.at(time)->R_k2tau;
alpha_I0toIk = map_camera_cpi_I0toIi.at(time)->alpha_tau;
}
// Create the linear system based on the feature reprojection
// [ 1 0 -u ] p_FinCi = [ 0 ]
// [ 0 1 -v ] [ 0 ]
// where
// p_FinCi = R_C0toCi * R_ItoC * (p_FinI0 - p_IiinI0) + p_IinC
// = R_C0toCi * R_ItoC * (p_FinI0 - v_I0inI0 * dt - 0.5 * grav_inI0 * dt^2 - alpha) + p_IinC
Eigen::MatrixXd H_proj = Eigen::MatrixXd::Zero(2, 3);
H_proj << 1, 0, -uv_norm(0), 0, 1, -uv_norm(1);
Eigen::MatrixXd Y = H_proj * R_ItoC * R_I0toIk;
Eigen::MatrixXd H_i = Eigen::MatrixXd::Zero(2, system_size);
Eigen::MatrixXd b_i = Y * alpha_I0toIk - H_proj * p_IinC;
if (size_feature == 1) {
assert(false);
// Substitute in p_FinI0 = z*bearing_inC0_rotI0 - R_ItoC^T*p_IinC
// H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 1) = Y * features_bearings.at(feat.first);
// b_i += Y * R_ItoC.transpose() * p_IinC;
} else {
H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y; // feat
}
H_i.block(0, size_feature * num_features + 0, 2, 3) = -DT * Y; // vel
H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * DT * DT * Y; // grav
// Else lets append this to our system!
A.block(index_meas, 0, 2, A.cols()) = H_i;
b.block(index_meas, 0, 2, 1) = b_i;
index_meas += 2;
}
}
}
auto rT3 = boost::posix_time::microsec_clock::local_time();
// ======================================================
// ======================================================
// Solve the linear system without constraint
// Eigen::MatrixXd AtA = A.transpose() * A;
// Eigen::MatrixXd Atb = A.transpose() * b;
// Eigen::MatrixXd x_hat = AtA.colPivHouseholderQr().solve(Atb);
// Constrained solving |g| = 9.81 constraint
//约束求解 |g| = 9.81 约束
Eigen::MatrixXd A1 = A.block(0, 0, A.rows(), A.cols() - 3);
// Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).inverse();
Eigen::MatrixXd A1A1_inv = (A1.transpose() * A1).llt().solve(Eigen::MatrixXd::Identity(A1.cols(), A1.cols()));
Eigen::MatrixXd A2 = A.block(0, A.cols() - 3, A.rows(), 3);
Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose());
Eigen::MatrixXd D = Temp * A2;
Eigen::MatrixXd d = Temp * b;
Eigen::Matrix<double, 7, 1> coeff = InitializerHelper::compute_dongsi_coeff(D, d, params.gravity_mag);
// Create companion matrix of our polynomial
//创建多项式的伴随矩阵
// https://en.wikipedia.org/wiki/Companion_matrix
assert(coeff(0) == 1);
Eigen::Matrix<double, 6, 6> companion_matrix = Eigen::Matrix<double, 6, 6>::Zero(coeff.rows() - 1, coeff.rows() - 1);
companion_matrix.diagonal(-1).setOnes();
companion_matrix.col(companion_matrix.cols() - 1) = -coeff.reverse().head(coeff.rows() - 1);
Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> svd0(companion_matrix);
Eigen::MatrixXd singularValues0 = svd0.singularValues();
double cond0 = singularValues0(0) / singularValues0(singularValues0.rows() - 1);
PRINT_DEBUG("[init-d]: CM cond = %.3f | rank = %d of %d (%4.3e thresh)\n", cond0, (int)svd0.rank(), (int)companion_matrix.cols(),
svd0.threshold());
if (svd0.rank() != companion_matrix.rows()) {
PRINT_ERROR(RED "[init-d]: eigenvalue decomposition not full rank!!\n" RESET);
return false;
}
// Find its eigenvalues (can be complex)
// 找到它的特征值（可以很复杂）
Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> solver(companion_matrix, false);
if (solver.info() != Eigen::Success) {
PRINT_ERROR(RED "[init-d]: failed to compute the eigenvalue decomposition!!\n" RESET);
return false;
}
// Find the smallest real eigenvalue
// NOTE: we find the one that gives us minimal constraint cost
// NOTE: not sure if the best, but one that gives the correct mag should be good?
//找到最小的实特征值
// 注意：我们找到给我们带来最小约束成本的那个
// 注意：不确定是否是最好的，但给出正确mag的应该是好的？
bool lambda_found = false;
double lambda_min = -1;
double cost_min = INFINITY;
Eigen::MatrixXd I_dd = Eigen::MatrixXd::Identity(D.rows(), D.rows());
// double g2 = params.gravity_mag * params.gravity_mag;
// Eigen::MatrixXd ddt = d * d.transpose();
for (int i = 0; i < solver.eigenvalues().size(); i++) {
auto val = solver.eigenvalues()(i);
if (val.imag() == 0) {
double lambda = val.real();
// Eigen::MatrixXd mat = (D - lambda * I_dd) * (D - lambda * I_dd) - 1 / g2 * ddt;
// double cost = mat.determinant();
Eigen::MatrixXd D_lambdaI_inv = (D - lambda * I_dd).llt().solve(I_dd);
Eigen::VectorXd state_grav = D_lambdaI_inv * d;
double cost = std::abs(state_grav.norm() - params.gravity_mag);
// std::cout << lambda << " - " << cost << " -> " << state_grav.transpose() << std::endl;
if (!lambda_found || cost < cost_min) {
lambda_found = true;
lambda_min = lambda;
cost_min = cost;
}
}
}
if (!lambda_found) {//没找到最小实特征值的话返回
PRINT_ERROR(RED "[init-d]: failed to find a real eigenvalue!!!\n" RESET);
return false;
}
PRINT_DEBUG("[init-d]: smallest real eigenvalue = %.5f (cost of %f)\n", lambda_min, cost_min);
// Recover our gravity from the constraint!
//从约束中恢复zhongli
// Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).inverse();
Eigen::MatrixXd D_lambdaI_inv = (D - lambda_min * I_dd).llt().solve(I_dd);
Eigen::VectorXd state_grav = D_lambdaI_inv * d;
// Overwrite our state: [features, velocity, gravity]
//覆盖我们的状态：[特征、速度、重力]
Eigen::VectorXd state_feat_vel = -A1A1_inv * A1.transpose() * A2 * state_grav + A1A1_inv * A1.transpose() * b;
Eigen::MatrixXd x_hat = Eigen::MatrixXd::Zero(system_size, 1);
x_hat.block(0, 0, size_feature * num_features + 3, 1) = state_feat_vel;
x_hat.block(size_feature * num_features + 3, 0, 3, 1) = state_grav;
Eigen::Vector3d v_I0inI0 = x_hat.block(size_feature * num_features + 0, 0, 3, 1);
PRINT_INFO("[init-d]: velocity in I0 was %.3f,%.3f,%.3f and |v| = %.4f\n", v_I0inI0(0), v_I0inI0(1), v_I0inI0(2), v_I0inI0.norm());
// Check gravity magnitude to see if converged
//检查重力大小以查看是否收敛
Eigen::Vector3d gravity_inI0 = x_hat.block(size_feature * num_features + 3, 0, 3, 1);
double init_max_grav_difference = 1e-3;
if (std::abs(gravity_inI0.norm() - params.gravity_mag) > init_max_grav_difference) {
PRINT_WARNING(YELLOW "[init-d]: gravity did not converge (%.3f > %.3f)\n" RESET, std::abs(gravity_inI0.norm() - params.gravity_mag),
init_max_grav_difference);
return false;
}
PRINT_INFO("[init-d]: gravity in I0 was %.3f,%.3f,%.3f and |g| = %.4f\n", gravity_inI0(0), gravity_inI0(1), gravity_inI0(2),
gravity_inI0.norm());
auto rT4 = boost::posix_time::microsec_clock::local_time();
// ======================================================
// ======================================================
// Extract imu state elements
// 提取imu状态元素
std::map<double, Eigen::VectorXd> ori_I0toIi, pos_IiinI0, vel_IiinI0;
for (auto const &timepair : map_camera_times) {
// Timestamp of this pose
// 这个姿态的时间戳
double time = timepair.first;
// Get our CPI integration values
//获取我们的 CPI 积分值
double DT = 0.0;
Eigen::MatrixXd R_I0toIk = Eigen::MatrixXd::Identity(3, 3);
Eigen::MatrixXd alpha_I0toIk = Eigen::MatrixXd::Zero(3, 1);
Eigen::MatrixXd beta_I0toIk = Eigen::MatrixXd::Zero(3, 1);
if (map_camera_cpi_I0toIi.find(time) != map_camera_cpi_I0toIi.end() && map_camera_cpi_I0toIi.at(time) != nullptr) {
auto cpi = map_camera_cpi_I0toIi.at(time);
DT = cpi->DT;
R_I0toIk = cpi->R_k2tau;
alpha_I0toIk = cpi->alpha_tau;
beta_I0toIk = cpi->beta_tau;
}
// Integrate to get the relative to the current timestamp
//积分以获得相对于当前时间戳的值
Eigen::Vector3d p_IkinI0 = v_I0inI0 * DT - 0.5 * gravity_inI0 * DT * DT + alpha_I0toIk;
Eigen::Vector3d v_IkinI0 = v_I0inI0 - gravity_inI0 * DT + beta_I0toIk;
// Record the values all transformed to the I0 frame
//记录所有转换到I0帧的值
ori_I0toIi.insert({time, rot_2_quat(R_I0toIk)});
pos_IiinI0.insert({time, p_IkinI0});
vel_IiinI0.insert({time, v_IkinI0});
}
// Recover the features in the first IMU frame
//从第一个IMU帧恢复特征
count_valid_features = 0;
std::map<size_t, Eigen::Vector3d> features_inI0;
for (auto const &feat : features) {
if (map_features_num_meas[feat.first] < min_num_meas_to_optimize)
continue;
Eigen::Vector3d p_FinI0;
if (size_feature == 1) {
assert(false);
// double depth = x_hat(size_feature * A_index_features.at(feat.first), 0);
// p_FinI0 = depth * features_bearings.at(feat.first) - R_ItoC.transpose() * p_IinC;
} else {
p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first), 0, 3, 1);
}
bool is_behind = false;
for (auto const &camtime : feat.second->timestamps) {
size_t cam_id = camtime.first;
Eigen::Vector4d q_ItoC = params.camera_extrinsics.at(cam_id).block(0, 0, 4, 1);
Eigen::Vector3d p_IinC = params.camera_extrinsics.at(cam_id).block(4, 0, 3, 1);
Eigen::Vector3d p_FinC0 = quat_2_Rot(q_ItoC) * p_FinI0 + p_IinC;
if (p_FinC0(2) < 0) {
is_behind = true;
}
}
if (!is_behind) {
features_inI0.insert({feat.first, p_FinI0});
count_valid_features++;
}
}
if (count_valid_features < min_valid_features) {
PRINT_ERROR(YELLOW "[init-d]: not enough features for our mle (%zu < %d)!\n" RESET, count_valid_features, min_valid_features);
return false;
}
// Convert our states to be a gravity aligned global frame of reference
// Here we say that the I0 frame is at 0,0,0 and shared the global origin
// 将我们的状态转换为重力对齐的全局参考系
// 这里我们说I0帧位于0,0,0并且共享全局原点
Eigen::Matrix3d R_GtoI0;
InitializerHelper::gram_schmidt(gravity_inI0, R_GtoI0);
Eigen::Vector4d q_GtoI0 = rot_2_quat(R_GtoI0);
Eigen::Vector3d gravity;
gravity << 0.0, 0.0, params.gravity_mag;
std::map<double, Eigen::VectorXd> ori_GtoIi, pos_IiinG, vel_IiinG;
std::map<size_t, Eigen::Vector3d> features_inG;
for (auto const &timepair : map_camera_times) {
ori_GtoIi[timepair.first] = quat_multiply(ori_I0toIi.at(timepair.first), q_GtoI0);
pos_IiinG[timepair.first] = R_GtoI0.transpose() * pos_IiinI0.at(timepair.first);
vel_IiinG[timepair.first] = R_GtoI0.transpose() * vel_IiinI0.at(timepair.first);
}
for (auto const &feat : features_inI0) {
features_inG[feat.first] = R_GtoI0.transpose() * feat.second;
}
// ======================================================
// ======================================================
// Ceres problem stuff
// NOTE: By default the problem takes ownership of the memory
//Ceres问题
// 注意：默认情况下，问题会占用内存
ceres::Problem problem;
// Our system states (map from time to index)
//系统状态（从时间映射到索引）
std::map<double, int> map_states;
std::vector<double *> ceres_vars_ori;
std::vector<double *> ceres_vars_pos;
std::vector<double *> ceres_vars_vel;
std::vector<double *> ceres_vars_bias_g;
std::vector<double *> ceres_vars_bias_a;
// Feature states (3dof p_FinG)
//特征向量，3自由度
std::map<size_t, int> map_features;
std::vector<double *> ceres_vars_feat;
// Setup extrinsic calibration q_ItoC, p_IinC (map from camera id to index)
//设置外参标定（从相机 ID 到索引的映射）
std::map<size_t, int> map_calib_cam2imu;
std::vector<double *> ceres_vars_calib_cam2imu_ori;
std::vector<double *> ceres_vars_calib_cam2imu_pos;
// Setup intrinsic calibration focal, center, distortion (map from camera id to index)
//设置内参标定焦点、中心、畸变（从相机 ID 到索引的映射）
std::map<size_t, int> map_calib_cam;
std::vector<double *> ceres_vars_calib_cam_intrinsics;
// Helper lambda that will free any memory we have allocated
//Helper lambda 将释放我们分配的所有内存
auto free_state_memory = [&]() {
for (auto const &ptr : ceres_vars_ori)
delete[] ptr;
for (auto const &ptr : ceres_vars_pos)
delete[] ptr;
for (auto const &ptr : ceres_vars_vel)
delete[] ptr;
for (auto const &ptr : ceres_vars_bias_g)
delete[] ptr;
for (auto const &ptr : ceres_vars_bias_a)
delete[] ptr;
for (auto const &ptr : ceres_vars_feat)
delete[] ptr;
for (auto const &ptr : ceres_vars_calib_cam2imu_ori)
delete[] ptr;
for (auto const &ptr : ceres_vars_calib_cam2imu_pos)
delete[] ptr;
for (auto const &ptr : ceres_vars_calib_cam_intrinsics)
delete[] ptr;
};
// Set the optimization settings
// NOTE: We use dense schur since after eliminating features we have a dense problem
// NOTE: http://ceres-solver.org/solving_faqs.html#solving
//优化器设置
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
// options.linear_solver_type = ceres::SPARSE_SCHUR;
// options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
// options.preconditioner_type = ceres::SCHUR_JACOBI;
// options.linear_solver_type = ceres::ITERATIVE_SCHUR;
options.num_threads = params.init_dyn_mle_max_threads;
options.max_solver_time_in_seconds = params.init_dyn_mle_max_time;
options.max_num_iterations = params.init_dyn_mle_max_iter;
// options.minimizer_progress_to_stdout = true;
// options.linear_solver_ordering = ordering;
options.function_tolerance = 1e-5;
options.gradient_tolerance = 1e-4 * options.function_tolerance;
// Loop through each CPI integration and add its measurement to the problem
//循环遍历每个 CPI 集成并将其测量添加到问题中
double timestamp_k = -1;
for (auto const &timepair : map_camera_times) {
// Get our predicted state at the requested camera timestep
//获取我们在请求的相机时间步长的预测状态
double timestamp_k1 = timepair.first;
std::shared_ptr<ov_core::CpiV1> cpi = map_camera_cpi_IitoIi1.at(timestamp_k1);
Eigen::Matrix<double, 16, 1> state_k1;
state_k1.block(0, 0, 4, 1) = ori_GtoIi.at(timestamp_k1);
state_k1.block(4, 0, 3, 1) = pos_IiinG.at(timestamp_k1);
state_k1.block(7, 0, 3, 1) = vel_IiinG.at(timestamp_k1);
state_k1.block(10, 0, 3, 1) = gyroscope_bias;
state_k1.block(13, 0, 3, 1) = accelerometer_bias;
// ================================================================
// ADDING GRAPH STATE / ESTIMATES!
// ================================================================
//添加位姿图状态
// Load our state variables into our allocated state pointers
//将状态变量加载到分配的状态指针中
auto *var_ori = new double[4];
for (int j = 0; j < 4; j++) {
var_ori[j] = state_k1(0 + j, 0);
}
auto *var_pos = new double[3];
auto *var_vel = new double[3];
auto *var_bias_g = new double[3];
auto *var_bias_a = new double[3];
for (int j = 0; j < 3; j++) {
var_pos[j] = state_k1(4 + j, 0);
var_vel[j] = state_k1(7 + j, 0);
var_bias_g[j] = state_k1(10 + j, 0);
var_bias_a[j] = state_k1(13 + j, 0);
}
// Now actually create the parameter block in the ceres problem
//现在实际创建 ceres 问题中的参数块
auto ceres_jplquat = new State_JPLQuatLocal();
problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
problem.AddParameterBlock(var_pos, 3);
problem.AddParameterBlock(var_vel, 3);
problem.AddParameterBlock(var_bias_g, 3);
problem.AddParameterBlock(var_bias_a, 3);
// Fix this first ever pose to constrain the problem
// NOTE: If we don't do this, then the problem won't be full rank
// NOTE: Since init is over a small window, we are likely to be degenerate
// NOTE: Thus we need to fix these parameters
// 固定第一个姿态来限制问题
// 注意：如果我们不这样做，那么问题就不会是满秩的
// 注意：由于 init 是在一个小窗口上进行的，因此我们很可能会退化
// 注意：因此我们需要修复这些参数
if (map_states.empty()) {
// Construct state and prior
//构建状态和先验
Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(13, 1);
for (int j = 0; j < 4; j++) {
x_lin(0 + j) = var_ori[j];
}
for (int j = 0; j < 3; j++) {
x_lin(4 + j) = var_pos[j];
x_lin(7 + j) = var_bias_g[j];
x_lin(10 + j) = var_bias_a[j];
}
Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(10, 1);
Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(10, 10);
prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-5, 2); // 4dof unobservable yaw and position，四自由度不可观测的yaw和位置
prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(0.05, 2); // bias_g prior 角速度偏差先验
prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(0.10, 2); // bias_a prior 加速度偏差先验
// Construct state type and ceres parameter pointers
//构建状态类型和ceres参数指针
std::vector<std::string> x_types;
std::vector<double *> factor_params;
factor_params.push_back(var_ori);
x_types.emplace_back("quat_yaw");
factor_params.push_back(var_pos);
x_types.emplace_back("vec3");
factor_params.push_back(var_bias_g);
x_types.emplace_back("vec3");
factor_params.push_back(var_bias_a);
x_types.emplace_back("vec3");
// Append it to the problem
//将先验添加到问题中
auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
problem.AddResidualBlock(factor_prior, nullptr, factor_params);
}
// Append to our historical vector of states
//添加历史状态向量
map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
ceres_vars_ori.push_back(var_ori);
ceres_vars_pos.push_back(var_pos);
ceres_vars_vel.push_back(var_vel);
ceres_vars_bias_g.push_back(var_bias_g);
ceres_vars_bias_a.push_back(var_bias_a);
// ================================================================
// ADDING GRAPH FACTORS!
// ================================================================
// Append the new IMU factor
//添加新的IMU因子
if (cpi != nullptr) {
assert(timestamp_k != -1);
std::vector<double *> factor_params;
factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
auto *factor_imu = new Factor_ImuCPIv1(cpi->DT, gravity, cpi->alpha_tau, cpi->beta_tau, cpi->q_k2tau, cpi->b_a_lin, cpi->b_w_lin,
cpi->J_q, cpi->J_b, cpi->J_a, cpi->H_b, cpi->H_a, cpi->P_meas);
problem.AddResidualBlock(factor_imu, nullptr, factor_params);
}
// Move time forward
//推进时间
timestamp_k = timestamp_k1;
}
// First make sure we have calibration states added
//首先确保我们添加了校准状态
for (auto const &idpair : map_camera_ids) {
size_t cam_id = idpair.first;
if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end()) {
auto *var_calib_ori = new double[4];
for (int j = 0; j < 4; j++) {
var_calib_ori[j] = params.camera_extrinsics.at(cam_id)(0 + j, 0);
}
auto *var_calib_pos = new double[3];
for (int j = 0; j < 3; j++) {
var_calib_pos[j] = params.camera_extrinsics.at(cam_id)(4 + j, 0);
}
auto ceres_calib_jplquat = new State_JPLQuatLocal();
problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
problem.AddParameterBlock(var_calib_pos, 3);
map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);
// Construct state and prior
//构建状态和先验
Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
for (int j = 0; j < 4; j++) {
x_lin(0 + j) = var_calib_ori[j];
}
for (int j = 0; j < 3; j++) {
x_lin(4 + j) = var_calib_pos[j];
}
Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);
// Construct state type and ceres parameter pointers
//构造状态类型和 ceres 参数指针
std::vector<std::string> x_types;
std::vector<double *> factor_params;
factor_params.push_back(var_calib_ori);
x_types.emplace_back("quat");
factor_params.push_back(var_calib_pos);
x_types.emplace_back("vec3");
auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
problem.AddResidualBlock(factor_prior, nullptr, factor_params);
if (!params.init_dyn_mle_opt_calib) {
problem.SetParameterBlockConstant(var_calib_ori);
problem.SetParameterBlockConstant(var_calib_pos);
}
}
if (map_calib_cam.find(cam_id) == map_calib_cam.end()) {
auto *var_calib_cam = new double[8];
for (int j = 0; j < 8; j++) {
var_calib_cam[j] = params.camera_intrinsics.at(cam_id)->get_value()(j, 0);
}
problem.AddParameterBlock(var_calib_cam, 8);
map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);
// Construct state and prior
//构造状态和先验
Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
for (int j = 0; j < 8; j++) {
x_lin(0 + j) = var_calib_cam[j];
}
Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);
// Construct state type and ceres parameter pointers
//构造状态类型和 ceres 参数指针
std::vector<std::string> x_types;
std::vector<double *> factor_params;
factor_params.push_back(var_calib_cam);
x_types.emplace_back("vec8");
auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
problem.AddResidualBlock(factor_prior, nullptr, factor_params);
if (!params.init_dyn_mle_opt_calib) {
problem.SetParameterBlockConstant(var_calib_cam);
}
}
}
assert(map_calib_cam2imu.size() == map_calib_cam.size());
// Then, append new feature observations factors seen from all cameras
//然后，附加从所有相机看到的新特征观察因素
for (auto const &feat : features) {
// Skip features that don't have enough measurements
//条i过没有足够测量的特征
if (map_features_num_meas[feat.first] < min_num_meas_to_optimize)
continue;
// Features can be removed if behind the camera!
//如果位于相机后面，则可以移除特征！
if (features_inG.find(feat.first) == features_inG.end())
continue;
// Finally loop through each raw uv observation and append it as a factor
//最后循环遍历每个原始uv观察并将其附加为一个因素
for (auto const &camtime : feat.second->timestamps) {
// Get our ids and if the camera is a fisheye or not
//得到id和相机是不是鱼眼
size_t feat_id = feat.first;
size_t cam_id = camtime.first;
bool is_fisheye = (std::dynamic_pointer_cast<ov_core::CamEqui>(params.camera_intrinsics.at(cam_id)) != nullptr);
// Loop through each observation
//循环遍历每个观察结果
for (size_t i = 0; i < camtime.second.size(); i++) {
// Skip measurements we don't have poses for
//跳过我们没有姿态的测量
double time = feat.second->timestamps.at(cam_id).at(i);
if (map_camera_times.find(time) == map_camera_times.end())
continue;
// Our measurement
//我们的测量
Eigen::Vector2d uv_raw = feat.second->uvs.at(cam_id).at(i).block(0, 0, 2, 1).cast<double>();
// If we don't have the feature state we should create that parameter block
// The initial guess of the features are the scaled feature map from the SFM
// 如果我们没有特征状态，我们应该创建该参数块
// 最初猜测的特征是来自 SFM 的缩放特征图
if (map_features.find(feat_id) == map_features.end()) {
auto *var_feat = new double[3];
for (int j = 0; j < 3; j++) {
var_feat[j] = features_inG.at(feat_id)(j);
}
problem.AddParameterBlock(var_feat, 3);
map_features.insert({feat_id, (int)ceres_vars_feat.size()});
ceres_vars_feat.push_back(var_feat);
}
// Then lets add the factors
//添加因子
std::vector<double *> factor_params;
factor_params.push_back(ceres_vars_ori.at(map_states.at(time)));
factor_params.push_back(ceres_vars_pos.at(map_states.at(time)));
factor_params.push_back(ceres_vars_feat.at(map_features.at(feat_id)));
factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
auto *factor_pinhole = new Factor_ImageReprojCalib(uv_raw, params.sigma_pix, is_fisheye);
// ceres::LossFunction *loss_function = nullptr;
ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
}
}
}
assert(ceres_vars_ori.size() == ceres_vars_bias_g.size());
assert(ceres_vars_ori.size() == ceres_vars_vel.size());
assert(ceres_vars_ori.size() == ceres_vars_bias_a.size());
assert(ceres_vars_ori.size() == ceres_vars_pos.size());
auto rT5 = boost::posix_time::microsec_clock::local_time();
// Optimize the ceres graph
//优化ceres图
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
PRINT_INFO("[init-d]: %d iterations | %zu states, %zu feats (%zu valid) | %d param and %d res | cost %.4e => %.4e\n",
(int)summary.iterations.size(), map_states.size(), map_features.size(), count_valid_features, summary.num_parameters,
summary.num_residuals, summary.initial_cost, summary.final_cost);
auto rT6 = boost::posix_time::microsec_clock::local_time();
// Return if we have failed!
//失败则返回
timestamp = newest_cam_time;
if (params.init_dyn_mle_max_iter != 0 && summary.termination_type != ceres::CONVERGENCE) {
PRINT_WARNING(YELLOW "[init-d]: opt failed: %s!\n" RESET, summary.message.c_str());
free_state_memory();
return false;
}
PRINT_DEBUG("[init-d]: %s\n", summary.message.c_str());
//======================================================
//======================================================
// Helper function to get the IMU pose value from our ceres problem
//从我们的 ceres 问题中获取 IMU 位姿值的辅助函数
auto get_pose = [&](double timestamp) {
Eigen::VectorXd state_imu = Eigen::VectorXd::Zero(16);
for (int i = 0; i < 4; i++) {
state_imu(0 + i) = ceres_vars_ori[map_states[timestamp]][i];
}
for (int i = 0; i < 3; i++) {
state_imu(4 + i) = ceres_vars_pos[map_states[timestamp]][i];
state_imu(7 + i) = ceres_vars_vel[map_states[timestamp]][i];
state_imu(10 + i) = ceres_vars_bias_g[map_states[timestamp]][i];
state_imu(13 + i) = ceres_vars_bias_a[map_states[timestamp]][i];
}
return state_imu;
};
// Our most recent state is the IMU state!
//我们最近的状态是 IMU 状态！
assert(map_states.find(newest_cam_time) != map_states.end());
if (_imu == nullptr) {
_imu = std::make_shared<ov_type::IMU>();
}
Eigen::VectorXd imu_state = get_pose(newest_cam_time);
_imu->set_value(imu_state);
_imu->set_fej(imu_state);
// Append our IMU clones (includes most recent)
//附加我们的 IMU 克隆（包括最新的）
for (auto const &statepair : map_states) {
Eigen::VectorXd pose = get_pose(statepair.first);
if (_clones_IMU.find(statepair.first) == _clones_IMU.end()) {
auto _pose = std::make_shared<ov_type::PoseJPL>();
_pose->set_value(pose.block(0, 0, 7, 1));
_pose->set_fej(pose.block(0, 0, 7, 1));
_clones_IMU.insert({statepair.first, _pose});
} else {
_clones_IMU.at(statepair.first)->set_value(pose.block(0, 0, 7, 1));
_clones_IMU.at(statepair.first)->set_fej(pose.block(0, 0, 7, 1));
}
}
// Append features as SLAM features!
//将特征附加为 SLAM 特征！
for (auto const &featpair : map_features) {
Eigen::Vector3d feature;
feature << ceres_vars_feat[featpair.second][0], ceres_vars_feat[featpair.second][1], ceres_vars_feat[featpair.second][2];
if (_features_SLAM.find(featpair.first) == _features_SLAM.end()) {
auto _feature = std::make_shared<ov_type::Landmark>(3);
_feature->_featid = featpair.first;
_feature->_feat_representation = LandmarkRepresentation::Representation::GLOBAL_3D;
_feature->set_from_xyz(feature, false);
_feature->set_from_xyz(feature, true);
_features_SLAM.insert({featpair.first, _feature});
} else {
_features_SLAM.at(featpair.first)->_featid = featpair.first;
_features_SLAM.at(featpair.first)->_feat_representation = LandmarkRepresentation::Representation::GLOBAL_3D;
_features_SLAM.at(featpair.first)->set_from_xyz(feature, false);
_features_SLAM.at(featpair.first)->set_from_xyz(feature, true);
}
}
// If we optimized calibration, we should also save it to our state
如果我们优化了校准，我们还应该将其保存到我们的状态中
if (params.init_dyn_mle_opt_calib) {
// TODO: append our calibration states too if we are doing calibration!
// TODO: (if we are not doing calibration do not calibrate them....)
// TODO: std::shared_ptr<ov_type::Vec> _calib_dt_CAMtoIMU,
// TODO: std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>> &_calib_IMUtoCAM,
// TODO: std::unordered_map<size_t, std::shared_ptr<ov_type::Vec>> &_cam_intrinsics
}
// Recover the covariance here of the optimized states
// NOTE: for now just the IMU state is recovered, but we should be able to do everything
// NOTE: maybe having features / clones will make it more stable?
//恢复优化状态的协方差
// 注意：现在只是恢复了 IMU 状态，但我们应该能够做所有事情
// 注意：也许拥有功能/克隆会让它更稳定？
std::vector<std::pair<const double *, const double *>> covariance_blocks;
int state_index = map_states[newest_cam_time];
// diagonals
covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_ori[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_pos[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_vel[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index]));
// orientation
covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_pos[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_vel[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index]));
// position
covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_vel[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index]));
// velocity
covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index]));
covariance_blocks.push_back(std::make_pair(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index]));
// bias_g
covariance_blocks.push_back(std::make_pair(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index]));
// Finally, compute the covariance
//计算协方差
ceres::Covariance::Options options_cov;
options_cov.null_space_rank = (!params.init_dyn_mle_opt_calib) * ((int)map_calib_cam2imu.size() * (6 + 8));
options_cov.min_reciprocal_condition_number = params.init_dyn_min_rec_cond;
// options_cov.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD;
options_cov.apply_loss_function = true; // Better consistency if we use this
options_cov.num_threads = params.init_dyn_mle_max_threads;
ceres::Covariance problem_cov(options_cov);
bool success = problem_cov.Compute(covariance_blocks, &problem);
if (!success) {
PRINT_WARNING(YELLOW "[init-d]: covariance recovery failed...\n" RESET);
free_state_memory();
return false;
}
// construct the covariance we will return
//构建将返回的协方差
order.clear();
order.push_back(_imu);
covariance = Eigen::MatrixXd::Zero(_imu->size(), _imu->size());
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> covtmp = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();
// block diagonal
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], covtmp.data()));
covariance.block(0, 0, 3, 3) = covtmp.eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], covtmp.data()));
covariance.block(3, 3, 3, 3) = covtmp.eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], covtmp.data()));
covariance.block(6, 6, 3, 3) = covtmp.eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], covtmp.data()));
covariance.block(9, 9, 3, 3) = covtmp.eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], covtmp.data()));
covariance.block(12, 12, 3, 3) = covtmp.eval();
// orientation
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], covtmp.data()));
covariance.block(0, 3, 3, 3) = covtmp.eval();
covariance.block(3, 0, 3, 3) = covtmp.transpose();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], covtmp.data()));
covariance.block(0, 6, 3, 3) = covtmp.eval();
covariance.block(6, 0, 3, 3) = covtmp.transpose().eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], covtmp.data()));
covariance.block(0, 9, 3, 3) = covtmp.eval();
covariance.block(9, 0, 3, 3) = covtmp.transpose().eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], covtmp.data()));
covariance.block(0, 12, 3, 3) = covtmp.eval();
covariance.block(12, 0, 3, 3) = covtmp.transpose().eval();
// position
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], covtmp.data()));
covariance.block(3, 6, 3, 3) = covtmp.eval();
covariance.block(6, 3, 3, 3) = covtmp.transpose().eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], covtmp.data()));
covariance.block(3, 9, 3, 3) = covtmp.eval();
covariance.block(9, 3, 3, 3) = covtmp.transpose().eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], covtmp.data()));
covariance.block(3, 12, 3, 3) = covtmp.eval();
covariance.block(12, 3, 3, 3) = covtmp.transpose().eval();
// velocity
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], covtmp.data()));
covariance.block(6, 9, 3, 3) = covtmp.eval();
covariance.block(9, 6, 3, 3) = covtmp.transpose().eval();
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], covtmp.data()));
covariance.block(6, 12, 3, 3) = covtmp.eval();
covariance.block(12, 6, 3, 3) = covtmp.transpose().eval();
// bias_g
CHECK(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], covtmp.data()));
covariance.block(9, 12, 3, 3) = covtmp.eval();
covariance.block(12, 9, 3, 3) = covtmp.transpose().eval();
// inflate as needed
covariance.block(0, 0, 3, 3) *= params.init_dyn_inflation_orientation;
covariance.block(6, 6, 3, 3) *= params.init_dyn_inflation_velocity;
covariance.block(9, 9, 3, 3) *= params.init_dyn_inflation_bias_gyro;
covariance.block(12, 12, 3, 3) *= params.init_dyn_inflation_bias_accel;
// we are done >:D
covariance = 0.5 * (covariance + covariance.transpose());
Eigen::Vector3d sigmas_vel = covariance.block(6, 6, 3, 3).diagonal().transpose().cwiseSqrt();
Eigen::Vector3d sigmas_bg = covariance.block(9, 9, 3, 3).diagonal().transpose().cwiseSqrt();
Eigen::Vector3d sigmas_ba = covariance.block(12, 12, 3, 3).diagonal().transpose().cwiseSqrt();
PRINT_DEBUG("[init-d]: vel priors = %.3f, %.3f, %.3f\n", sigmas_vel(0), sigmas_vel(1), sigmas_vel(2));
PRINT_DEBUG("[init-d]: bg priors = %.3f, %.3f, %.3f\n", sigmas_bg(0), sigmas_bg(1), sigmas_bg(2));
PRINT_DEBUG("[init-d]: ba priors = %.3f, %.3f, %.3f\n", sigmas_ba(0), sigmas_ba(1), sigmas_ba(2));
// Set our position to be zero
//设置位置为0
Eigen::MatrixXd x = _imu->value();
x.block(4, 0, 3, 1).setZero();
_imu->set_value(x);
_imu->set_fej(x);
// Debug timing information about how long it took to initialize!!
// 调试有关初始化花费多长时间的计时信息！
auto rT7 = boost::posix_time::microsec_clock::local_time();
PRINT_DEBUG("[TIME]: %.4f sec for prelim tests\n", (rT2 - rT1).total_microseconds() * 1e-6);
PRINT_DEBUG("[TIME]: %.4f sec for linsys setup\n", (rT3 - rT2).total_microseconds() * 1e-6);
PRINT_DEBUG("[TIME]: %.4f sec for linsys\n", (rT4 - rT3).total_microseconds() * 1e-6);
PRINT_DEBUG("[TIME]: %.4f sec for ceres opt setup\n", (rT5 - rT4).total_microseconds() * 1e-6);
PRINT_DEBUG("[TIME]: %.4f sec for ceres opt\n", (rT6 - rT5).total_microseconds() * 1e-6);
PRINT_DEBUG("[TIME]: %.4f sec for ceres covariance\n", (rT7 - rT6).total_microseconds() * 1e-6);
PRINT_DEBUG("[TIME]: %.4f sec total for initialization\n", (rT7 - rT1).total_microseconds() * 1e-6);
free_state_memory();
return true;
}3、初始化函数
这将尝试对状态进行动态和状态初始化。用户可以请求等待 IMU 读数的跳跃（即设备被拾取）或尽快初始化。对于状态初始化，用户需要预先指定校准，否则始终使用动态。逻辑如下：
（1）尝试执行状态元素的动态初始化。
（2）如果失败并且我们进行了校准，那么我们可以尝试进行静态初始化
（3）如果单位是静止的并且我们正在等待加加速度，则返回，否则初始化状态！
该动态系统为Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration
https://ieeexplore.ieee.org/document/6386235
实现和扩展 ，它通过首先创建一个用于将相机恢复到 IMU 旋转的线性系统来解决初始化问题，然后速度、重力和特征位置，最后进行全面优化以允许协方差恢复。
读者可能感兴趣的另一篇论文是视觉惯性系统 IMU 初始化问题的分析解决方案，其中有一些关于比例恢复和加速度计偏差的详细实验。
bool InertialInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector<std::shared_ptr<ov_type::Type>> &order,
std::shared_ptr<ov_type::IMU> t_imu, bool wait_for_jerk) {
// Get the newest and oldest timestamps we will try to initialize between!
double newest_cam_time = -1;
for (auto const &feat : _db->get_internal_data()) {
for (auto const &camtimepair : feat.second->timestamps) {
for (auto const &time : camtimepair.second) {
newest_cam_time = std::max(newest_cam_time, time);
}
}
}
double oldest_time = newest_cam_time - params.init_window_time - 0.10;
if (newest_cam_time < 0 || oldest_time < 0) {
return false;
}
// Remove all measurements that are older then our initialization window
// Then we will try to use all features that are in the feature database!
_db->cleanup_measurements(oldest_time);
auto it_imu = imu_data->begin();
while (it_imu != imu_data->end() && it_imu->timestamp < oldest_time + params.calib_camimu_dt) {
it_imu = imu_data->erase(it_imu);
}
// Compute the disparity of the system at the current timestep
// If disparity is zero or negative we will always use the static initializer
bool disparity_detected_moving_1to0 = false;
bool disparity_detected_moving_2to1 = false;
if (params.init_max_disparity > 0) {
// Get the disparity statistics from this image to the previous
// Only compute the disparity for the oldest half of the initialization period
double newest_time_allowed = newest_cam_time - 0.5 * params.init_window_time;
int num_features0 = 0;
int num_features1 = 0;
double avg_disp0, avg_disp1;
double var_disp0, var_disp1;
FeatureHelper::compute_disparity(_db, avg_disp0, var_disp0, num_features0, newest_time_allowed);
FeatureHelper::compute_disparity(_db, avg_disp1, var_disp1, num_features1, newest_cam_time, newest_time_allowed);
// Return if we can't compute the disparity
int feat_thresh = 15;
if (num_features0 < feat_thresh || num_features1 < feat_thresh) {
PRINT_WARNING(YELLOW "[init]: not enough feats to compute disp: %d,%d < %d\n" RESET, num_features0, num_features1, feat_thresh);
return false;
}
// Check if it passed our check!
PRINT_INFO(YELLOW "[init]: disparity is %.3f,%.3f (%.2f thresh)\n" RESET, avg_disp0, avg_disp1, params.init_max_disparity);
disparity_detected_moving_1to0 = (avg_disp0 > params.init_max_disparity);
disparity_detected_moving_2to1 = (avg_disp1 > params.init_max_disparity);
}
// Use our static initializer!
// CASE1: if our disparity says we were static in last window and have moved in the newest, we have a jerk
// CASE2: if both disparities are below the threshold, then the platform has been stationary during both periods
bool has_jerk = (!disparity_detected_moving_1to0 && disparity_detected_moving_2to1);
bool is_still = (!disparity_detected_moving_1to0 && !disparity_detected_moving_2to1);
if (((has_jerk && wait_for_jerk) || (is_still && !wait_for_jerk)) && params.init_imu_thresh > 0.0) {
PRINT_DEBUG(GREEN "[init]: USING STATIC INITIALIZER METHOD!\n" RESET);
return init_static->initialize(timestamp, covariance, order, t_imu, wait_for_jerk);
} else if (params.init_dyn_use && !is_still) {
PRINT_DEBUG(GREEN "[init]: USING DYNAMIC INITIALIZER METHOD!\n" RESET);
std::map<double, std::shared_ptr<ov_type::PoseJPL>> _clones_IMU;
std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> _features_SLAM;
return init_dynamic->initialize(timestamp, covariance, order, t_imu, _clones_IMU, _features_SLAM);
} else {
std::string msg = (has_jerk) ? "" : "no accel jerk detected";
msg += (has_jerk || is_still) ? "" : ", ";
msg += (is_still) ? "" : "platform moving too much";
PRINT_INFO(YELLOW "[init]: failed static init: %s\n" RESET, msg.c_str());
}
return false;
}
文章来源:https://blog.csdn.net/weixin_46190814/article/details/135001516
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！