From bfc499a733c93508b665919d57d06fb2f00433e3 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 10 Sep 2021 12:32:36 +0200
Subject: [PATCH] bugfix: covariance size for 3d case
---
src/subscriber_imu_enableable.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/subscriber_imu_enableable.cpp b/src/subscriber_imu_enableable.cpp
index 2cfb082..15edd1b 100644
--- a/src/subscriber_imu_enableable.cpp
+++ b/src/subscriber_imu_enableable.cpp
@@ -132,6 +132,7 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
// add zero displacement and rotation capture & feature & factor with all previous frames
assert(sensor_ptr_->getProblem());
auto size = sensor_ptr_->getProblem()->getDim() == 3 ? 7:3;
+ auto cov_size = sensor_ptr_->getProblem()->getDim() == 3 ? 6:3;
Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
if(sensor_ptr_->getProblem()->getDim() == 3)
zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
@@ -145,7 +146,7 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
"FeatureZeroOdom",
zero_motion,
- Eigen::MatrixXd::Identity(size,size) * 0.01);
+ Eigen::MatrixXd::Identity(cov_size,cov_size) * 0.01);
if(sensor_ptr_->getProblem()->getDim() == 3)
FactorBase::emplace<FactorRelativePose3d>(feature_zero,
feature_zero,
--
GitLab