open-vins-学习记录2-各个构造函数

VioManager的构造函数

先看estimator_config.yaml设置为true的部分。

OpenVINS并没有说直接用双目的匹配初始化特征点3D坐标值，而是把双目看成两个相对关系的单目，然后用更多两两配对的单目（已知位姿）构成的sfm问题来求解特征点的3D坐标。

=== params.use_klt设置为true，使用光流法

TrackKLT调用基类TrackBase中的构造函数

部分内容借鉴自：https://zhuanlan.zhihu.com/p/383627934

launch文件中部分参数说明"max_cameras"：指相机个数，可以为1或2；“use_stereo”：表示使用单目还是双目，双目为true，单目为false；“bag_start”：一般为0就好；“bag”：写自己数据集的路径；“topic_imu”：存放imu数据的地址，一般为/imu0；“topic_camera0”，“topic_camera1”：存放相机数据地址。一般为/cam0/image_raw，/cam1/image_raw；“use_fej”：指是否使用first estimate jacobian，使用为true，不使用为false；“use_imuavg”：指是否使用中值法求预积分，使用为true，不使用为false；“use_rk4int”：指是否使用四阶龙格库塔求预积分，使用为true，不使用为false；“calib_cam_extrinsics”：指是否使用相机外参，使用为true，不使用为false；“calib_cam_intrinsics”：指是否使用相机内参，使用为true，不使用为false；“calib_cam_timeoffset”：指是否使用相机时间补偿，使用为true，不使用为false；“calib_camimu_dt”：指相机与IMU量测时间戳上的误差，初始化为0.0；“max_clones”：指滑窗的长度；“gravity”：指重力加速度；“use_klt”：指是否使用光流法，使用为true，如果为false则使用描述子法；“num_pts”：指管理的特征点数；“T_C0toI”：指第一个相机到IMU坐标系的变换矩阵；“T_C1toI”：指第二个相机到IMU坐标系的变换矩阵；

VioManager这个类其实管理了支撑整个系统的各个功能模块。我们可以通过调用类中的各个函数完成整个视觉惯性融合定位。
RosVisualizer这个类用于管理发布结果，显示效果等功能模块。

// 下面是一些策略，还没看用没用
（1）判断如果上一次左相机或者右相机没有跟踪上点那么重新提取特征点并使函数返回，如果上一帧左右相机均有跟踪上的点那么继续执行光流跟踪
（2）左右图像特征点匹配去除奇异点
（3）给挑选好的点去除畸变并更新保存特征点的变量database

// 直方图均衡化
https://zhuanlan.zhihu.com/p/384035819

TrackBase::TrackBase(std::unordered_map<size_t, std::shared_ptr<CamBase>> cameras, int numfeats, int numaruco, bool stereo,
                     HistogramMethod histmethod)
    : camera_calib(cameras), database(new FeatureDatabase()), num_features(numfeats), use_stereo(stereo), histogram_method(histmethod) {
  // Our current feature ID should be larger then the number of aruco tags we have (each has 4 corners)
  currid = 4 * (size_t)numaruco + 1;
  // Create our mutex array based on the number of cameras we have
  // See https://stackoverflow.com/a/24170141/7718197
  if (mtx_feeds.empty() || mtx_feeds.size() != camera_calib.size()) {
    std::vector<std::mutex> list(camera_calib.size());
    mtx_feeds.swap(list);
  }
}

=== propagator状态传播器的创建

无构造函数的调用

=== initializer初始化器的创建

InertialInitializer::InertialInitializer(InertialInitializerOptions &params_, std::shared_ptr<ov_core::FeatureDatabase> db)
    : params(params_), _db(db) {

  // Vector of our IMU data
  // ImuData为存储imu数据的结构体
  imu_data = std::make_shared<std::vector<ov_core::ImuData>>();

  // Create initializers
  // StaticInitializer构造函数体为空
  init_static = std::make_shared<StaticInitializer>(params, _db, imu_data);
  init_dynamic = std::make_shared<DynamicInitializer>(params, _db, imu_data);
}

=== updaterMSCKF更新器的创建

UpdaterMSCKF::UpdaterMSCKF(UpdaterOptions &options, ov_core::FeatureInitializerOptions &feat_init_options) : _options(options) {

  // Save our raw pixel noise squared
  _options.sigma_pix_sq = std::pow(_options.sigma_pix, 2);

  // Save our feature initializer
  initializer_feat = std::shared_ptr<ov_core::FeatureInitializer>(new ov_core::FeatureInitializer(feat_init_options));

  // Initialize the chi squared test table with confidence level 0.95
  // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221
  for (int i = 1; i < 500; i++) {
    boost::math::chi_squared chi_squared_dist(i);
    chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95);
  }
}

=== updaterSLAM更新器的创建

UpdaterSLAM::UpdaterSLAM(UpdaterOptions &options_slam, UpdaterOptions &options_aruco, ov_core::FeatureInitializerOptions &feat_init_options)
    : _options_slam(options_slam), _options_aruco(options_aruco) {

  // Save our raw pixel noise squared
  _options_slam.sigma_pix_sq = std::pow(_options_slam.sigma_pix, 2);
  _options_aruco.sigma_pix_sq = std::pow(_options_aruco.sigma_pix, 2);

  // Save our feature initializer
  initializer_feat = std::shared_ptr<ov_core::FeatureInitializer>(new ov_core::FeatureInitializer(feat_init_options));

  // Initialize the chi squared test table with confidence level 0.95
  // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221
  for (int i = 1; i < 500; i++) {
    boost::math::chi_squared chi_squared_dist(i);
    chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95);
  }
}

=== imu的回调

ROS1Visualizer::callback_inertial()