diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h index 7bbae22c3f2f0598b60c19f1bad5d45e177ab0c0..8674d4db5792df5d90454fe65531342254a77187 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 12676c83d8536bc8f2ccbcc78eb288dfe74804cd..12ebd1bb39f87ad420005b99b60ff7f11662095c 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 433ccf6b92724169420813bdea8692cf5bc6777e..7ccf86a70690b9936a29be8e32b83ec26a56526c 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 69dbf111bc7a5bbcd4ad11796633338c5e877083..ec1db9e4391f0015ad2f6e084110063ba17f5203 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 036d488e88371d3f4a9628452f1ecd7e7f0e172a..dc35d28c0a43844868505597948fed1315b83af3 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 ddf1624c4a9c7c9d44c6ce43c35d8f975c8d2494..63d20e2e9b3a9d2b813802aca3f0ab0af187cefe 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 e0c769b4a30a4d1dc122be4ebe88622634186e1a..e4eb78b29764bb30340448a3235199fb640e51c8 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 6255fa38517be1218fcd6e39647c18e2151974c6..9af67f31fd02d661d75c5b15e54947f9cf3f5437 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 87c3619d3fbe37425bfc62bb6c0e2b17cfc40d9c..eabb79523f4952dd885c8432363071aef3dab7a9 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 93ba8561b5b4fea55e814cc9d3b68501ae2439ac..0d77493e9c809bb3cc9094584714142777baf6bc 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);