ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

small fix

This commit is contained in:
Vladyslav Usenko 2019-06-19 11:44:25 +02:00
parent b50597159a
commit 077af5937f
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/vi_estimator/keypoint_vio.h
View File

@ -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);

2
src/vi_estimator/keypoint_vio.cpp
View File

@ -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