diff --git a/data/euroc_config.json b/data/euroc_config.json index ec47ec1..8fede15 100644 --- a/data/euroc_config.json +++ b/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, diff --git a/data/euroc_config_no_factors.json b/data/euroc_config_no_factors.json index 59391aa..95a4ffe 100644 --- a/data/euroc_config_no_factors.json +++ b/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, diff --git a/data/euroc_config_no_weights.json b/data/euroc_config_no_weights.json index 844aaac..fbc4fcc 100644 --- a/data/euroc_config_no_weights.json +++ b/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, diff --git a/data/kitti_config.json b/data/kitti_config.json index daf90e6..e644987 100644 --- a/data/kitti_config.json +++ b/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, diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json index ca9bd6e..b10f083 100644 --- a/data/tumvi_512_config.json +++ b/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, diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index dcad82a..8c862ea 100644 --- a/include/basalt/utils/vio_config.h +++ b/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; diff --git a/include/basalt/vi_estimator/keypoint_vio.h b/include/basalt/vi_estimator/keypoint_vio.h index 8f47e99..2e9f13b 100644 --- a/include/basalt/vi_estimator/keypoint_vio.h +++ b/include/basalt/vi_estimator/keypoint_vio.h @@ -63,7 +63,7 @@ class KeypointVioEstimator : public VioEstimatorBase, typedef Eigen::Matrix MatNN; typedef Eigen::Matrix MatN3; - KeypointVioEstimator(double int_std_dev, const Eigen::Vector3d& g, + KeypointVioEstimator(const Eigen::Vector3d& g, const basalt::Calibration& calib, const VioConfig& config); diff --git a/include/basalt/vi_estimator/keypoint_vo.h b/include/basalt/vi_estimator/keypoint_vo.h index ffc37bf..d3c1afe 100644 --- a/include/basalt/vi_estimator/keypoint_vo.h +++ b/include/basalt/vi_estimator/keypoint_vo.h @@ -63,8 +63,7 @@ class KeypointVoEstimator : public VioEstimatorBase, typedef Eigen::Matrix MatNN; typedef Eigen::Matrix MatN3; - KeypointVoEstimator(double int_std_dev, - const basalt::Calibration& calib, + KeypointVoEstimator(const basalt::Calibration& calib, const VioConfig& config); void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i, diff --git a/include/basalt/vi_estimator/vio_estimator.h b/include/basalt/vi_estimator/vio_estimator.h index bf35bbf..6f67ab5 100644 --- a/include/basalt/vi_estimator/vio_estimator.h +++ b/include/basalt/vi_estimator/vio_estimator.h @@ -100,7 +100,6 @@ class VioEstimatorFactory { public: static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config, const Calibration& cam, - double int_std_dev, const Eigen::Vector3d& g, bool use_imu); }; diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp index 22b9d73..d992056 100644 --- a/src/mapper_sim_naive.cpp +++ b/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()); diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index 14b886b..1cdd382 100644 --- a/src/rs_t265_vio.cpp +++ b/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; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index ab630c1..d43e8ff 100644 --- a/src/utils/vio_config.cpp +++ b/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)); diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 6385e8f..fb4f21b 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/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 basalt::Calibration& calib, const VioConfig& config) + const Eigen::Vector3d& g, const basalt::Calibration& calib, + 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>(12).array() = 1e3; + marg_H.diagonal().segment<3>(9).array() = config.vio_init_ba_weight; + marg_H.diagonal().segment<3>(12).array() = config.vio_init_bg_weight; std::cout << "marg_H\n" << marg_H << std::endl; diff --git a/src/vi_estimator/keypoint_vo.cpp b/src/vi_estimator/keypoint_vo.cpp index 3840037..7e57142 100644 --- a/src/vi_estimator/keypoint_vo.cpp +++ b/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& calib, - const VioConfig& config) + const basalt::Calibration& calib, 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; diff --git a/src/vi_estimator/vio_estimator.cpp b/src/vi_estimator/vio_estimator.cpp index 068f129..916c713 100644 --- a/src/vi_estimator/vio_estimator.cpp +++ b/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& cam, double int_std_dev, + const VioConfig& config, const Calibration& 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; diff --git a/src/vio.cpp b/src/vio.cpp index 82a625b..439442f 100644 --- a/src/vio.cpp +++ b/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; diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp index 2700004..012163c 100644 --- a/src/vio_sim.cpp +++ b/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());