From 077af5937f7e36a7f003daf0af902983f2f726bf Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Wed, 19 Jun 2019 11:44:25 +0200
Subject: [PATCH] small fix

---
 include/basalt/vi_estimator/keypoint_vio.h | 2 --
 src/vi_estimator/keypoint_vio.cpp          | 2 ++
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/basalt/vi_estimator/keypoint_vio.h b/include/basalt/vi_estimator/keypoint_vio.h
index da7b0e9..cd64527 100644
--- a/include/basalt/vi_estimator/keypoint_vio.h
+++ b/include/basalt/vi_estimator/keypoint_vio.h
@@ -63,8 +63,6 @@ class KeypointVioEstimator : public VioEstimatorBase,
   typedef Eigen::Matrix<double, N, N> MatNN;
   typedef Eigen::Matrix<double, N, 3> MatN3;
 
-  static constexpr double prior_weight = 1e8;
-
   KeypointVioEstimator(double int_std_dev, const Eigen::Vector3d& g,
                        const basalt::Calibration<double>& calib,
                        const VioConfig& config);
diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp
index cd4120e..8c769c2 100644
--- a/src/vi_estimator/keypoint_vio.cpp
+++ b/src/vi_estimator/keypoint_vio.cpp
@@ -62,6 +62,8 @@ 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);
   // prior on yaw