Skip to content
Snippets Groups Projects
Commit c5b36b04 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

adapted to lists and maps copy return

parent e54ef272
No related branches found
No related tags found
1 merge request!443Resolve "Work on const / non-const in wolf base classes"
Pipeline #9570 failed
Hide whitespace changes
Inline Side-by-side
include/core/trajectory/trajectory_base.h
+ 4
− 0
View file @ c5b36b04
......@@ -59,6 +59,10 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
FrameBasePtr getLastFrame();
FrameBaseConstPtr getFirstFrame() const;
FrameBasePtr getFirstFrame();
FrameBaseConstPtr getNextFrame(FrameBaseConstPtr) const;
FrameBasePtr getNextFrame(FrameBasePtr);
FrameBaseConstPtr getPreviousFrame(FrameBaseConstPtr) const;
FrameBasePtr getPreviousFrame(FrameBasePtr);
TimeStamp closestTimeStampToTimeStamp(const TimeStamp& _ts) const;
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
......
src/frame/frame_base.cpp
+ 4
− 32
View file @ c5b36b04
......@@ -160,56 +160,28 @@ FrameBaseConstPtr FrameBase::getPreviousFrame() const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
if (current_frame_it == getTrajectory()->getFrameMap().begin())
return nullptr;
return std::prev(current_frame_it)->second;
return getTrajectory()->getPreviousFrame(shared_from_this());
}
FrameBasePtr FrameBase::getPreviousFrame()
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
if (current_frame_it == getTrajectory()->getFrameMap().begin())
return nullptr;
return std::prev(current_frame_it)->second;
return getTrajectory()->getPreviousFrame(shared_from_this());
}
FrameBaseConstPtr FrameBase::getNextFrame() const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
return nullptr;
return std::next(current_frame_it)->second;
return getTrajectory()->getNextFrame(shared_from_this());
}
FrameBasePtr FrameBase::getNextFrame()
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
return nullptr;
return std::next(current_frame_it)->second;
return getTrajectory()->getNextFrame(shared_from_this());
}
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
......
src/processor/processor_motion.cpp
+ 6
− 4
View file @ c5b36b04
......@@ -901,8 +901,9 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
FrameBaseConstPtr frame = nullptr;
CaptureBaseConstPtr capture = nullptr;
CaptureMotionConstPtr capture_motion = nullptr;
for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameMap().rbegin();
frame_rev_iter != getProblem()->getTrajectory()->getFrameMap().rend();
auto frame_map = getProblem()->getTrajectory()->getFrameMap();
for (auto frame_rev_iter = frame_map.rbegin();
frame_rev_iter != frame_map.rend();
++frame_rev_iter)
{
frame = frame_rev_iter->second;
......@@ -951,8 +952,9 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameMap().rbegin();
frame_rev_iter != getProblem()->getTrajectory()->getFrameMap().rend();
auto frame_map = getProblem()->getTrajectory()->getFrameMap();
for (auto frame_rev_iter = frame_map.rbegin();
frame_rev_iter != frame_map.rend();
++frame_rev_iter)
{
frame = frame_rev_iter->second;
......
src/trajectory/trajectory_base.cpp
+ 48
− 3
View file @ c5b36b04
......@@ -49,8 +49,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
// add to list
// frame_map_.erase(_frame_ptr);
frame_map_.erase(_frame_ptr->getTimeStamp());
frame_const_map_.erase(_frame_ptr->getTimeStamp());
}
......@@ -96,7 +94,7 @@ TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) cons
return upper_bound->first;
// unreachable
return TimeStamp();
return TimeStamp::Invalid();
}
FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
......@@ -123,10 +121,56 @@ FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
return nullptr;
}
FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame) const
{
auto frame_it = frame_const_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_const_map_.end(), "Frame not found in the frame map!");
if (frame_it == frame_const_map_.end() or std::next(frame_it) == frame_const_map_.end())
return nullptr;
return std::next(frame_it)->second;
}
FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame)
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
if (frame_it == frame_map_.end() or std::next(frame_it) == frame_map_.end())
return nullptr;
return std::next(frame_it)->second;
}
FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame) const
{
auto frame_it = frame_const_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_const_map_.end(), "Frame not found in the frame map!");
if (frame_it == frame_const_map_.end() or frame_it == frame_const_map_.begin())
return nullptr;
return std::prev(frame_it)->second;
}
FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame)
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
if (frame_it == frame_map_.end() or frame_it == frame_map_.begin())
return nullptr;
return std::prev(frame_it)->second;
}
void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl;
}
void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
......@@ -152,6 +196,7 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBaseConstPtr _trj_p
log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
return log;
}
bool TrajectoryBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto trj_ptr = std::static_pointer_cast<const TrajectoryBase>(_node_ptr);
......
test/gtest_frame_base.cpp
+ 30
− 20
View file @ c5b36b04
......@@ -140,15 +140,25 @@ TEST(FrameBase, Frames)
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
P->print(2,0,0,0);
// tree is now consistent
ASSERT_TRUE(P->check(0));
......@@ -170,28 +180,28 @@ TEST(FrameBase, Frames)
ASSERT_EQ(F9, T->closestFrameToTimeStamp(9));
// Next frame
ASSERT_EQ(F1, F0->getNextFrame());
ASSERT_EQ(F2, F1->getNextFrame());
ASSERT_EQ(F3, F2->getNextFrame());
ASSERT_EQ(F4, F3->getNextFrame());
ASSERT_EQ(F5, F4->getNextFrame());
ASSERT_EQ(F6, F5->getNextFrame());
ASSERT_EQ(F7, F6->getNextFrame());
ASSERT_EQ(F8, F7->getNextFrame());
ASSERT_EQ(F9, F8->getNextFrame());
ASSERT_EQ(nullptr, F9->getNextFrame());
EXPECT_EQ(F1->id(), F0->getNextFrame()->id());
EXPECT_EQ(F2->id(), F1->getNextFrame()->id());
EXPECT_EQ(F3->id(), F2->getNextFrame()->id());
EXPECT_EQ(F4->id(), F3->getNextFrame()->id());
EXPECT_EQ(F5->id(), F4->getNextFrame()->id());
EXPECT_EQ(F6->id(), F5->getNextFrame()->id());
EXPECT_EQ(F7->id(), F6->getNextFrame()->id());
EXPECT_EQ(F8->id(), F7->getNextFrame()->id());
EXPECT_EQ(F9->id(), F8->getNextFrame()->id());
EXPECT_EQ(nullptr, F9->getNextFrame());
// Prev frame
ASSERT_EQ(nullptr, F0->getPreviousFrame());
ASSERT_EQ(F0, F1->getPreviousFrame());
ASSERT_EQ(F1, F2->getPreviousFrame());
ASSERT_EQ(F2, F3->getPreviousFrame());
ASSERT_EQ(F3, F4->getPreviousFrame());
ASSERT_EQ(F4, F5->getPreviousFrame());
ASSERT_EQ(F5, F6->getPreviousFrame());
ASSERT_EQ(F6, F7->getPreviousFrame());
ASSERT_EQ(F7, F8->getPreviousFrame());
ASSERT_EQ(F8, F9->getPreviousFrame());
EXPECT_EQ(nullptr, F0->getPreviousFrame());
EXPECT_EQ(F0, F1->getPreviousFrame());
EXPECT_EQ(F1, F2->getPreviousFrame());
EXPECT_EQ(F2, F3->getPreviousFrame());
EXPECT_EQ(F3, F4->getPreviousFrame());
EXPECT_EQ(F4, F5->getPreviousFrame());
EXPECT_EQ(F5, F6->getPreviousFrame());
EXPECT_EQ(F6, F7->getPreviousFrame());
EXPECT_EQ(F7, F8->getPreviousFrame());
EXPECT_EQ(F8, F9->getPreviousFrame());
}
#include "core/state_block/state_quaternion.h"
......
test/gtest_odom_2d.cpp
+ 3
− 3
View file @ c5b36b04
......@@ -399,7 +399,6 @@ TEST(Odom2d, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data
auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
std::cout << "t: " << t << std::endl;
for (int n=1; n<=N; n++)
......@@ -407,7 +406,8 @@ TEST(Odom2d, KF_callback)
t += dt;
// re-use capture with updated timestamp
capture->setTimeStamp(t);
auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
//capture->setTimeStamp(t);
// Processor
capture->process();
......@@ -452,7 +452,7 @@ TEST(Odom2d, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_2);
ASSERT_TRUE(problem->check(0));
t += dt;
capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment