diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 0625a61b5fce5bd66d240e3ac1da89e5d584e455..29a27f5d9c1662a6e6ab7bd4c9d7c02515476c4e 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 037ff094a158592c24adee70c28ddfb8cddb1d35..bc8982ca2ce696719aab35105f059639d6c72f23 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 821ffefeb052acb75f81ec5d3327ebdaf04c4cd7..2e8ee85c22155f08307637e0f6f41101a9d4e8b9 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 cc6a8abafc9e65062fc2a83580977b9bb4193658..becf854fa604e4b496e51cc2fcede63c7274eacc 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 f18d627aa37dec917ecbbf77c982294724a6b589..1958a86156c773665f3f157bbb8193cec840e731 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 5d6f1345ee09674750d129e64a89a87ca081902d..1a0f44507a010891a792ce2f01af16d2233ce6cb 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 3b8800bfc4cd1de2803a9ac29b7cabf9b7a088e7..85b9c4cf7f92164f93aa1c10a46c22fa53c07ef8 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 7391619e1b59f1e3258a740ef034936e1a757d2d..04d838944dd50139da61d890c0171e0b41a33cb0 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 d86942de20f525f0b0995e2a5b43e18750311697..ffecd322bc2a021abf7ae01850c3a6b38369208c 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 e883a1597c26f8418c06348a4b5b3db50cad01f4..8c5a2685c5155a9c5a5efbe28fb2ea26a7c899a6 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 267a202f370c59e92371136fb6c6cf587f9985f0..099297e77a3e3a4d9e2663b078b50919d1a52d24 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)