Added initialization parameters to config files

data/euroc_config.json

@@ -23,6 +23,9 @@
         "config.vio_use_lm": false,
         "config.vio_lm_lambda_min": 1e-32,
         "config.vio_lm_lambda_max": 1e2,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,

data/euroc_config_no_factors.json

@@ -23,6 +23,9 @@
         "config.vio_use_lm": false,
         "config.vio_lm_lambda_min": 1e-32,
         "config.vio_lm_lambda_max": 1e2,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,

data/euroc_config_no_weights.json

@@ -23,6 +23,9 @@
         "config.vio_use_lm": false,
         "config.vio_lm_lambda_min": 1e-32,
         "config.vio_lm_lambda_max": 1e2,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,

data/kitti_config.json

@@ -23,6 +23,9 @@
         "config.vio_use_lm": false,
         "config.vio_lm_lambda_min": 1e-32,
         "config.vio_lm_lambda_max": 1e2,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,

data/tumvi_512_config.json

@@ -24,6 +24,9 @@
         "config.vio_use_lm": false,
         "config.vio_lm_lambda_min": 1e-32,
         "config.vio_lm_lambda_max": 1e2,
+        "config.vio_init_pose_weight": 1e8,
+        "config.vio_init_ba_weight": 1e1,
+        "config.vio_init_bg_weight": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,

include/basalt/utils/vio_config.h

@@ -72,6 +72,10 @@ struct VioConfig {
   double vio_lm_lambda_min;
   double vio_lm_lambda_max;
 
+  double vio_init_pose_weight;
+  double vio_init_ba_weight;
+  double vio_init_bg_weight;
+
   double mapper_obs_std_dev;
   double mapper_obs_huber_thresh;
   int mapper_detection_num_points;

include/basalt/vi_estimator/keypoint_vio.h

@@ -63,7 +63,7 @@ class KeypointVioEstimator : public VioEstimatorBase,
   typedef Eigen::Matrix<double, N, N> MatNN;
   typedef Eigen::Matrix<double, N, 3> MatN3;
 
-  KeypointVioEstimator(double int_std_dev, const Eigen::Vector3d& g,
+  KeypointVioEstimator(const Eigen::Vector3d& g,
                        const basalt::Calibration<double>& calib,
                        const VioConfig& config);
 

include/basalt/vi_estimator/keypoint_vo.h

@@ -63,8 +63,7 @@ class KeypointVoEstimator : public VioEstimatorBase,
   typedef Eigen::Matrix<double, N, N> MatNN;
   typedef Eigen::Matrix<double, N, 3> MatN3;
 
-  KeypointVoEstimator(double int_std_dev,
+  KeypointVoEstimator(const basalt::Calibration<double>& calib,
-                      const basalt::Calibration<double>& calib,
                       const VioConfig& config);
 
   void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,

include/basalt/vi_estimator/vio_estimator.h

@@ -100,7 +100,6 @@ class VioEstimatorFactory {
  public:
   static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config,
                                                const Calibration<double>& cam,
-                                               double int_std_dev,
                                                const Eigen::Vector3d& g,
                                                bool use_imu);
 };

src/mapper_sim_naive.cpp

@@ -723,7 +723,7 @@ void setup_vio() {
   basalt::VioConfig config;
   config.vio_debug = true;
 
-  vio.reset(new basalt::KeypointVioEstimator(0.0001, g, calib, config));
+  vio.reset(new basalt::KeypointVioEstimator(g, calib, config));
   vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
                   gt_accel_bias.front());
 

src/rs_t265_vio.cpp

@@ -169,7 +169,7 @@ int main(int argc, char** argv) {
   t265_device->image_data_queue = &opt_flow_ptr->input_queue;
 
   vio = basalt::VioEstimatorFactory::getVioEstimator(
-      vio_config, calib, 0.0001, basalt::constants::g, true);
+      vio_config, calib, basalt::constants::g, true);
   vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
   t265_device->imu_data_queue = &vio->imu_data_queue;
 

src/utils/vio_config.cpp

@@ -72,6 +72,10 @@ VioConfig::VioConfig() {
   vio_lm_lambda_min = 1e-32;
   vio_lm_lambda_max = 1e2;
 
+  vio_init_pose_weight = 1e8;
+  vio_init_ba_weight = 1e1;
+  vio_init_bg_weight = 1e2;
+
   mapper_obs_std_dev = 0.25;
   mapper_obs_huber_thresh = 1.5;
   mapper_detection_num_points = 800;
@@ -145,6 +149,10 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.vio_lm_lambda_min));
   ar(CEREAL_NVP(config.vio_lm_lambda_max));
 
+  ar(CEREAL_NVP(config.vio_init_pose_weight));
+  ar(CEREAL_NVP(config.vio_init_ba_weight));
+  ar(CEREAL_NVP(config.vio_init_bg_weight));
+
   ar(CEREAL_NVP(config.mapper_obs_std_dev));
   ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
   ar(CEREAL_NVP(config.mapper_detection_num_points));

src/vi_estimator/keypoint_vio.cpp

@@ -47,8 +47,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace basalt {
 
 KeypointVioEstimator::KeypointVioEstimator(
-    double int_std_dev, const Eigen::Vector3d& g,
+    const Eigen::Vector3d& g, const basalt::Calibration<double>& calib,
-    const basalt::Calibration<double>& calib, const VioConfig& config)
+    const VioConfig& config)
     : take_kf(true),
       frames_after_kf(0),
       g(g),
@@ -66,16 +66,14 @@ KeypointVioEstimator::KeypointVioEstimator(
   marg_H.setZero(POSE_VEL_BIAS_SIZE, POSE_VEL_BIAS_SIZE);
   marg_b.setZero(POSE_VEL_BIAS_SIZE);
 
-  double prior_weight = 1.0 / (int_std_dev * int_std_dev);
-
   // prior on position
-  marg_H.diagonal().head<3>().setConstant(prior_weight);
+  marg_H.diagonal().head<3>().setConstant(config.vio_init_pose_weight);
   // prior on yaw
-  marg_H(5, 5) = prior_weight;
+  marg_H(5, 5) = config.vio_init_pose_weight;
 
   // small prior to avoid jumps in bias
-  marg_H.diagonal().segment<3>(9).array() = 1e2;
+  marg_H.diagonal().segment<3>(9).array() = config.vio_init_ba_weight;
-  marg_H.diagonal().segment<3>(12).array() = 1e3;
+  marg_H.diagonal().segment<3>(12).array() = config.vio_init_bg_weight;
 
   std::cout << "marg_H\n" << marg_H << std::endl;
 

src/vi_estimator/keypoint_vo.cpp

@@ -47,8 +47,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace basalt {
 
 KeypointVoEstimator::KeypointVoEstimator(
-    double int_std_dev, const basalt::Calibration<double>& calib,
+    const basalt::Calibration<double>& calib, const VioConfig& config)
-    const VioConfig& config)
     : take_kf(true),
       frames_after_kf(0),
       initialized(false),
@@ -65,10 +64,8 @@ KeypointVoEstimator::KeypointVoEstimator(
   marg_H.setZero(POSE_SIZE, POSE_SIZE);
   marg_b.setZero(POSE_SIZE);
 
-  double prior_weight = 1.0 / (int_std_dev * int_std_dev);
-
   // prior on pose
-  marg_H.diagonal().setConstant(prior_weight);
+  marg_H.diagonal().setConstant(config.vio_init_pose_weight);
 
   std::cout << "marg_H\n" << marg_H << std::endl;
 

src/vi_estimator/vio_estimator.cpp

@@ -41,14 +41,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace basalt {
 
 VioEstimatorBase::Ptr VioEstimatorFactory::getVioEstimator(
-    const VioConfig& config, const Calibration<double>& cam, double int_std_dev,
+    const VioConfig& config, const Calibration<double>& cam,
     const Eigen::Vector3d& g, bool use_imu) {
   VioEstimatorBase::Ptr res;
 
   if (use_imu) {
-    res.reset(new KeypointVioEstimator(int_std_dev, g, cam, config));
+    res.reset(new KeypointVioEstimator(g, cam, config));
   } else {
-    res.reset(new KeypointVoEstimator(int_std_dev, cam, config));
+    res.reset(new KeypointVoEstimator(cam, config));
   }
 
   return res;

src/vio.cpp

@@ -269,7 +269,7 @@ int main(int argc, char** argv) {
   const int64_t start_t_ns = vio_dataset->get_image_timestamps().front();
   {
     vio = basalt::VioEstimatorFactory::getVioEstimator(
-        vio_config, calib, 0.0001, basalt::constants::g, use_imu);
+        vio_config, calib, basalt::constants::g, use_imu);
     vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
 
     opt_flow_ptr->output_queue = &vio->vision_data_queue;

src/vio_sim.cpp

@@ -878,9 +878,8 @@ void setup_vio() {
 
   basalt::VioConfig config;
 
-  // vio.reset(new basalt::KeypointVioEstimator(0.0001, g, calib, config));
   vio = basalt::VioEstimatorFactory::getVioEstimator(
-      config, calib, 0.0001, basalt::constants::g, use_imu);
+      config, calib, basalt::constants::g, use_imu);
   vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
                   gt_accel_bias.front());