From 81fc0f4d794a0e2f1d3c94761c3f8e14f840dc3a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 3 May 2022 15:35:23 +0200
Subject: [PATCH] Resolve "Adapt to const nonconst new API"
---
include/imu/factor/factor_imu.h | 4 ++--
include/imu/processor/processor_imu.h | 2 +-
include/imu/processor/processor_imu2d.h | 2 +-
src/processor/processor_imu.cpp | 2 +-
src/processor/processor_imu2d.cpp | 4 ++--
test/gtest_imu2d_static_init.cpp | 9 +++------
test/gtest_imu_static_init.cpp | 9 +++------
test/gtest_processor_imu.cpp | 2 +-
test/gtest_processor_imu2d.cpp | 2 +-
...est_processor_motion_intrinsics_update.cpp | 19 +++++++------------
10 files changed, 22 insertions(+), 33 deletions(-)
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 7bbae22c3..8674d4db5 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -478,8 +478,8 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
inline Eigen::VectorXd FactorImu::expectation() const
{
- FrameBasePtr frm_current = getFeature()->getFrame();
- FrameBasePtr frm_previous = getFrameOther();
+ auto frm_current = getFeature()->getFrame();
+ auto frm_previous = getFrameOther();
//get information on current_frame in the FactorImu
const Eigen::Vector3d frame_current_pos = (frm_current->getP()->getState());
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 12676c83d..12ebd1bb3 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -81,7 +81,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 = nullptr) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _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/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index 433ccf6b9..7ccf86a70 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu2d.h
@@ -81,7 +81,7 @@ class ProcessorImu2d : 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 CaptureBaseConstPtr _capture) 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 69dbf111b..ec1db9e43 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -102,7 +102,7 @@ FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
return feature;
}
-VectorXd ProcessorImu::getCalibration (const CaptureBasePtr _capture) const
+VectorXd ProcessorImu::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 036d488e8..dc35d28c0 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -111,7 +111,7 @@ namespace wolf {
return feature;
}
- VectorXd ProcessorImu2d::getCalibration (const CaptureBasePtr _capture) const
+ VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
@@ -214,7 +214,7 @@ namespace wolf {
*
* In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used
*/
- if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() )
+ if( std::static_pointer_cast<const SensorImu2d>(getSensor())->isGravityOrthogonal() )
_x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
else
_x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
index ddf1624c4..63d20e2e9 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -329,11 +329,9 @@ class ProcessorImu2dStaticInitTest : public testing::Test
// add zero displacement and rotation capture & feature & factor with all previous frames
assert(sensor_ptr_->getProblem());
- for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin();
- frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
- frm_pair++)
+ for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
{
- if (frm_pair->second == last_frame_)
+ if (frm_pair.second == last_frame_)
break;
auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
@@ -344,7 +342,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
FactorBase::emplace<FactorRelativePose2d>(feature_zero,
feature_zero,
- frm_pair->second,
+ frm_pair.second,
nullptr,
false,
TOP_MOTION);
@@ -443,7 +441,6 @@ class ProcessorImu2dStaticInitTest : public testing::Test
};
-
TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero)
{
Vector3d body_magnitudes = Vector3d::Zero();
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index e0c769b4a..e4eb78b29 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -339,11 +339,9 @@ class ProcessorImuStaticInitTest : public testing::Test
// add zero displacement and rotation capture & feature & factor with all previous frames
assert(sensor_ptr_->getProblem());
- for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin();
- frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
- frm_pair++)
+ for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
{
- if (frm_pair->second == last_frame_)
+ if (frm_pair.second == last_frame_)
break;
auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
@@ -354,11 +352,10 @@ class ProcessorImuStaticInitTest : public testing::Test
FactorBase::emplace<FactorRelativePose3d>(feature_zero,
feature_zero,
- frm_pair->second,
+ frm_pair.second,
nullptr,
false,
TOP_MOTION);
-
}
// impose static
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp
index 6255fa385..9af67f31f 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -196,7 +196,7 @@ TEST(ProcessorImu, voteForKeyFrame)
- 1 KeyFrame at origin
- 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
*/
- ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(),(unsigned int) 2);
+ ASSERT_EQ(problem->getTrajectory()->size(),(unsigned int) 2);
/* if max_time_span == 2, Wolf tree should be
diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp
index 87c3619d3..eabb79523 100644
--- a/test/gtest_processor_imu2d.cpp
+++ b/test/gtest_processor_imu2d.cpp
@@ -137,7 +137,7 @@ TEST_F(ProcessorImu2dTest, Prior)
auto KF0 = problem->setPriorFix(x0c, t0);
processor_motion->setOrigin(KF0);
//WOLF_DEBUG("x0 =", x0c);
- //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t)->getState("POV"));
+ //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t0)->getState("POV"));
}
TEST_F(ProcessorImu2dTest, MRUA)
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 93ba8561b..0d77493e9 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -221,7 +221,7 @@ TEST_F(ProcessorImuTest, getState)
#endif
- if (problem_->getTrajectory()->getFrameMap().size() > nb_kf){
+ if (problem_->getTrajectory()->size() > nb_kf){
FrameBasePtr KF = problem_->getTrajectory()->getLastFrame();
KF->setState(integrateX(AX, KF->getTimeStamp().get()));
KF->fix();
@@ -229,7 +229,7 @@ TEST_F(ProcessorImuTest, getState)
solver_->solve();
problem_->print(4, true, true, true);
- nb_kf = problem_->getTrajectory()->getFrameMap().size();
+ nb_kf = problem_->getTrajectory()->size();
#if WRITE_CSV_FILE
// post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
@@ -262,22 +262,17 @@ TEST_F(ProcessorImuTest, getState)
* though we only test at KFs so t = { 0, 1, 2, 3 }
*/
- for (auto F : *problem_->getTrajectory() )
+ for (auto F_pair : problem_->getTrajectory()->getFrameMap() )
{
- auto ts = F->getTimeStamp();
+ auto ts = F_pair.second->getTimeStamp();
double bias = (t == 0 ? 0.42 : 0.0);
- ASSERT_NEAR(F->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
- ASSERT_NEAR(F->getP()->getState()(0),0.0,1e-6);
- ASSERT_NEAR(F->getV()->getState()(0),0.0,1e-6);
+ ASSERT_NEAR(F_pair.second->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
+ ASSERT_NEAR(F_pair.second->getP()->getState()(0),0.0,1e-6);
+ ASSERT_NEAR(F_pair.second->getV()->getState()(0),0.0,1e-6);
}
-
}
-
-
-
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
--
GitLab