From 36295f0a1f8f445ca15941c18c4e9d530dfecde5 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 11 Apr 2022 17:17:47 +0200
Subject: [PATCH] new trajectory::size()
---
demos/demo_analytic_autodiff_factor.cpp | 4 +-
demos/demo_wolf_imported_graph.cpp | 4 +-
demos/solver/test_iQR_wolf2.cpp | 8 +--
include/core/trajectory/trajectory_base.h | 6 +++
src/sensor/sensor_base.cpp | 51 +++++++++----------
.../tree_manager_sliding_window.cpp | 2 +-
.../tree_manager_sliding_window_dual_rate.cpp | 2 +-
test/gtest_odom_2d.cpp | 2 +-
test/gtest_problem.cpp | 2 +-
test/gtest_solver_ceres_multithread.cpp | 2 +-
test/gtest_solver_manager_multithread.cpp | 2 +-
11 files changed, 44 insertions(+), 41 deletions(-)
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 0625a61b5..29a27f5d9 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -295,8 +295,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFirstFrame();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFirstFrame();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index 037ff094a..bc8982ca2 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -322,8 +322,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFirstFrame();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFirstFrame();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 821ffefeb..2e8ee85c2 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -384,13 +384,13 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().size() * 3);
- for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().end(); frame_it++)
+ Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->size() * 3);
+ for (auto frame : wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap())
{
if (complex_angle)
- state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
+ state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), atan2(*frame->getO()->get(), *(frame->getO()->get() + 1));
else
- state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
+ state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), *frame->getO()->get();
i += 3;
}
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index cc6a8abaf..becf854fa 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -53,6 +53,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
~TrajectoryBase() override;
// Frames
+ SizeEigen size() const;
FrameConstPtrMap getFrameMap() const;
FramePtrMap getFrameMap();
FrameBaseConstPtr getLastFrame() const;
@@ -130,6 +131,11 @@ inline FrameBasePtr TrajectoryBase::getLastFrame()
return frame_map_.rbegin()->second;
}
+inline SizeEigen TrajectoryBase::size() const
+{
+ return frame_const_map_.size();
+}
+
} // namespace wolf
#endif
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index f18d627aa..1958a8615 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -228,21 +228,20 @@ void SensorBase::setLastCapture(CaptureBasePtr cap)
void SensorBase::updateLastCapture()
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
- if (getProblem())
- {
- auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ if (not getProblem())
+ return;
- auto frame_rev_it = frame_map.rbegin();
- while (frame_rev_it != frame_map.rend())
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
+ {
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
+ if (capture and not capture->isRemoving())
{
- auto capture = frame_rev_it->second->getCaptureOf(shared_from_this());
- if (capture and not capture->isRemoving())
- {
- // found the most recent Capture made by this sensor !
- last_capture_ = capture;
- return;
- }
- frame_rev_it++;
+ // found the most recent Capture made by this sensor !
+ last_capture_ = capture;
+ return;
}
}
@@ -256,20 +255,19 @@ CaptureBaseConstPtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) cons
if (not getProblem())
return nullptr;
- auto frame_map = getProblem()->getTrajectory()->getFrameMap();
-
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- auto frame_rev_it = frame_map.rbegin();
- while (frame_rev_it != frame_map.rend())
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- if (frame_rev_it->second->getTimeStamp() <= _ts)
+ if (frame_rev_iter->second->getTimeStamp() <= _ts)
{
- auto capture = frame_rev_it->second->getCaptureOf(shared_from_this());
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
return capture;
}
- frame_rev_it++;
}
return nullptr;
@@ -281,20 +279,19 @@ CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts)
if (not getProblem())
return nullptr;
- auto frame_map = getProblem()->getTrajectory()->getFrameMap();
-
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- auto frame_rev_it = frame_map.rbegin();
- while (frame_rev_it != frame_map.rend())
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
+ ++frame_rev_iter)
{
- if (frame_rev_it->second->getTimeStamp() <= _ts)
+ if (frame_rev_iter->second->getTimeStamp() <= _ts)
{
- auto capture = frame_rev_it->second->getCaptureOf(shared_from_this());
+ auto capture = frame_rev_iter->second->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
return capture;
}
- frame_rev_it++;
}
return nullptr;
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 5d6f1345e..1a0f44507 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -26,7 +26,7 @@ namespace wolf
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->getFrameMap().size();
+ int n_f = getProblem()->getTrajectory()->size();
bool remove_first = (n_f > params_sw_->n_frames);
int n_fix = (n_f >= params_sw_->n_frames ?
params_sw_->n_fix_first_frames :
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index 3b8800bfc..85b9c4cf7 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -36,7 +36,7 @@ TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeMan
void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
{
- int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+ int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames
// recent segment not complete
if (n_f <= params_swdr_->n_frames_recent)
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 7391619e1..04d838944 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -444,7 +444,7 @@ TEST(Odom2d, KF_callback)
FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
// there must be 2KF and one F
- ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
+ ASSERT_EQ(problem->getTrajectory()->size(), 2);
// The last KF must have TS = 0.08
ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d86942de2..ffecd322b 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -248,7 +248,7 @@ TEST(Problem, emplaceFrame_factory)
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
+ ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 3);
ASSERT_EQ(f0->getStateVector().size(), 3 );
ASSERT_EQ(f1->getStateVector().size(), 7 );
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index e883a1597..8c5a2685c 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -84,7 +84,7 @@ TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
ts += 1.0;
- if (P->getTrajectory()->getFrameMap().size() > 10)
+ if (P->getTrajectory()->size() > 10)
P->getTrajectory()->getFirstFrame()->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 267a202f3..099297e77 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -79,7 +79,7 @@ TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
ts += 1.0;
- if (P->getTrajectory()->getFrameMap().size() > 10)
+ if (P->getTrajectory()->size() > 10)
P->getTrajectory()->getFirstFrame()->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
--
GitLab