From 5b739bbf9523d45b7c31c4186c005c7f0159cb48 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 22 Dec 2020 17:58:44 +0100
Subject: [PATCH] Fix ProcImu::getCalibration() to get the intrinsics
---
include/imu/processor/processor_imu.h | 2 +-
src/processor/processor_imu.cpp | 7 ++++---
test/gtest_factor_imu.cpp | 2 +-
3 files changed, 6 insertions(+), 5 deletions(-)
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 3166c09bb..d82a0afe4 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -60,7 +60,7 @@ class ProcessorImu : public ProcessorMotion{
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
bool voteForKeyFrame() const override;
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index bf1fe72b6..6eb0edced 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -87,9 +87,10 @@ FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
VectorXd ProcessorImu::getCalibration (const CaptureBasePtr _capture) const
{
- assert(_capture && "called with a null capture");
- assert(_capture->getSensorIntrinsic() && "null sensor intrinsics state block");
- return _capture->getSensorIntrinsic()->getState();
+ if (_capture)
+ return _capture->getStateBlock('I')->getState();
+ else
+ return getSensor()->getStateBlockDynamic('I')->getState();
}
void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index f53f9af38..c22ac6eaf 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -915,7 +915,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
TimeStamp t_imu(0.0), t_odo(0.0);
double dt_imu(0.001), dt_odo(.01);
- capture_imu = std::make_shared<CaptureImu> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
+ capture_imu = std::make_shared<CaptureImu> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getIntrinsic()->getState(), nullptr);
capture_odo = std::make_shared<CaptureOdom3d>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
sensor_odo->process(capture_odo);
--
GitLab