diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index e9cf00b45a14bbf5ddbab51c8cbfd093d02cd985..20ed2330334aea821c7ca8bc6510f7438c555063 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -81,7 +81,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) 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;
protected:
@@ -90,7 +90,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
};
-inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 66f40e6e62e32e6312270fe994e7e38ceb426822..7c0cf9b2c776be5808c853c54c5d5c3f0cca5ff7 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -210,7 +210,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
/** \brief Finds the capture that contains the closest previous motion of _ts
* \return a pointer to the capture (if it exists) or a nullptr (otherwise)
*/
- CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ CaptureMotionConstPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
+ CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts);
/** Set the origin of all motion for this processor
* \param _origin_frame the keyframe to be the origin
@@ -447,7 +448,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
* @param cap_prev : the first capture motion to be merged (input)
* @param cap_target : the second capture motion (modified)
*/
- void mergeCaptures(CaptureMotionConstPtr cap_prev,
+ void mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target);
protected:
@@ -487,13 +488,16 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
public:
- virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0;
+ virtual VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const = 0;
bool hasCalibration() const {return calib_size_ > 0;}
//getters
- CaptureMotionPtr getOrigin() const;
- CaptureMotionPtr getLast() const;
- CaptureMotionPtr getIncoming() const;
+ CaptureMotionConstPtr getOrigin() const;
+ CaptureMotionConstPtr getLast() const;
+ CaptureMotionConstPtr getIncoming() const;
+ CaptureMotionPtr getOrigin();
+ CaptureMotionPtr getLast();
+ CaptureMotionPtr getIncoming();
double getMaxTimeSpan() const;
double getMaxBuffLength() const;
@@ -609,17 +613,32 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
);
}
-inline CaptureMotionPtr ProcessorMotion::getOrigin() const
+inline CaptureMotionConstPtr ProcessorMotion::getOrigin() const
{
return origin_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getLast() const
+inline CaptureMotionConstPtr ProcessorMotion::getLast() const
{
return last_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getIncoming() const
+inline CaptureMotionConstPtr ProcessorMotion::getIncoming() const
+{
+ return incoming_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getOrigin()
+{
+ return origin_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getLast()
+{
+ return last_ptr_;
+}
+
+inline CaptureMotionPtr ProcessorMotion::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index f3a50080d135b9c7d19444c2b7bed4f0237f6e2a..fea39c4d6808a7a798a83d7b8b99dfbb47abec9d 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -107,7 +107,7 @@ class ProcessorOdom2d : public ProcessorMotion
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
@@ -128,7 +128,7 @@ inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& del
return delta_corrected;
}
-inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBasePtr _capture) const
+inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 83baf370b1b536bc8e472cab19200abdc34d0314..02cdbd5ef4480ccd1a0ba28ab0db5c44886fa58c 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -129,7 +129,7 @@ class ProcessorOdom3d : public ProcessorMotion
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 3d4d7326d059338a437ddf8442560c130337055a..3bec157836352109d794012456549c19412c411f 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -85,7 +85,7 @@ class HasStateBlocks
StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
- void registerNewStateBlocks(ProblemPtr _problem) const;
+ void registerNewStateBlocks(ProblemPtr _problem);
void removeStateBlocks(ProblemPtr _problem);
// States
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index c601d10c35287393c1837b5cc4e4c10e6f5cb7ab..e13e4f2cbd6c845e4a138e613a700f1b812ebd56 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -121,16 +121,17 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
FrameBasePtr getLastFrame();
FrameBaseConstPtr getFirstFrame() const;
FrameBasePtr getFirstFrame();
+ TimeStamp closestTimeStampToTimeStamp(const TimeStamp& _ts) const;
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
- TrajectoryIter begin();
- TrajectoryIter end();
- TrajectoryRevIter rbegin();
- TrajectoryRevIter rend();
- TrajectoryConstIter begin() const;
- TrajectoryConstIter end() const;
- TrajectoryConstRevIter rbegin() const;
- TrajectoryConstRevIter rend() const;
+ // TrajectoryIter begin();
+ // TrajectoryIter end();
+ // TrajectoryRevIter rbegin();
+ // TrajectoryRevIter rend();
+ // TrajectoryConstIter begin() const;
+ // TrajectoryConstIter end() const;
+ // TrajectoryConstRevIter rbegin() const;
+ // TrajectoryConstRevIter rend() const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -187,45 +188,45 @@ inline FrameBasePtr TrajectoryBase::getLastFrame()
return frame_map_.rbegin()->second;
}
-inline TrajectoryConstIter TrajectoryBase::begin() const
-{
- return frame_const_map_.begin();
-}
-
-inline TrajectoryIter TrajectoryBase::begin()
-{
- return frame_map_.begin();
-}
-
-inline TrajectoryConstIter TrajectoryBase::end() const
-{
- return frame_const_map_.end();
-}
-
-inline TrajectoryIter TrajectoryBase::end()
-{
- return frame_map_.end();
-}
-
-inline TrajectoryConstRevIter TrajectoryBase::rbegin() const
-{
- return frame_const_map_.rbegin();
-}
-
-inline TrajectoryRevIter TrajectoryBase::rbegin()
-{
- return frame_map_.rbegin();
-}
-
-inline TrajectoryConstRevIter TrajectoryBase::rend() const
-{
- return frame_const_map_.rend();
-}
-
-inline TrajectoryRevIter TrajectoryBase::rend()
-{
- return frame_map_.rend();
-}
+// inline TrajectoryConstIter TrajectoryBase::begin() const
+// {
+// return frame_const_map_.begin();
+// }
+
+// inline TrajectoryIter TrajectoryBase::begin()
+// {
+// return frame_map_.begin();
+// }
+
+// inline TrajectoryConstIter TrajectoryBase::end() const
+// {
+// return frame_const_map_.end();
+// }
+
+// inline TrajectoryIter TrajectoryBase::end()
+// {
+// return frame_map_.end();
+// }
+
+// inline TrajectoryConstRevIter TrajectoryBase::rbegin() const
+// {
+// return frame_const_map_.rbegin();
+// }
+
+// inline TrajectoryRevIter TrajectoryBase::rbegin()
+// {
+// return frame_map_.rbegin();
+// }
+
+// inline TrajectoryConstRevIter TrajectoryBase::rend() const
+// {
+// return frame_const_map_.rend();
+// }
+
+// inline TrajectoryRevIter TrajectoryBase::rend()
+// {
+// return frame_map_.rend();
+// }
} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 8275e5a087d61dcecf2bb23c33f86120eaea29a7..07e728fd6f138717b8f205af92744630d8dbcd03 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -543,13 +543,13 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::o
inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
<< " ---> Trj" << " @ " << trj_ptr
<< " -X-> Frm" << id();
- auto trj_has_frm = std::find_if(trj_ptr->begin(),
- trj_ptr->end(),
- [&_frm_ptr](TrajectoryIter frm_it)
+ auto trj_has_frm = std::find_if(trj_ptr->getFrameMap().begin(),
+ trj_ptr->getFrameMap().end(),
+ [&_frm_ptr](std::pair<TimeStamp,FrameBaseConstPtr> frm_it)
{
- return frm_it->second == _frm_ptr;
+ return frm_it.second == _frm_ptr;
});
- log.assertTrue(trj_has_frm != trj_ptr->end(), inconsistency_explanation);
+ log.assertTrue(trj_has_frm != trj_ptr->getFrameMap().end(), inconsistency_explanation);
// Captures
for(auto C : getCaptureList())
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index fd28b61869c75850d956ca6d69012b5c5194ef51..8a72934b9be01569080466a3548ce6c5782ed105 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -312,7 +312,9 @@ CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr,
<< " ---> Map" << map_ptr
<< " -X-> Lmk" << id();
auto map_lmk_list = map_ptr->getLandmarkList();
- auto map_has_lmk = std::find_if(map_lmk_list.begin(), map_lmk_list.end(), [&_lmk_ptr](LandmarkBasePtr lmk){ return lmk == _lmk_ptr;});
+ auto map_has_lmk = std::find(map_lmk_list.begin(),
+ map_lmk_list.end(),
+ _lmk_ptr);
log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
return log;
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 02d8beefd9d719a65218577bd594ed3bcb4f97bd..90e40f241c6d15a131b469f31db5c5b9b2938ee2 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -187,7 +187,7 @@ CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBaseConstPtr _prc_ptr
<< " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr
<< " -X-> Prc" << id();
auto sen_prc_list = sen_ptr->getProcessorList();
- auto sen_has_prc = std::find_if(sen_prc_list.begin(), sen_prc_list.end(), [&_prc_ptr](ProcessorBasePtr prc){ return prc == _prc_ptr;});
+ auto sen_has_prc = std::find(sen_prc_list.begin(), sen_prc_list.end(), _prc_ptr);
log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation);
return log;
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 2f222a50bec0aaf82c55f3053a292e912ce8d1a7..78b61297e5c746b3ab8ac543b460c2f52904323d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -82,7 +82,7 @@ ProcessorMotion::~ProcessorMotion()
}
-void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
+void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target)
{
assert(cap_prev != nullptr);
@@ -618,7 +618,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
// We need to search for the capture containing a motion buffer with the queried time stamp
- CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
+ auto capture_motion = findCaptureContainingTimeStamp(_ts);
if (capture_motion == nullptr) // we do not have any info of where to find a valid state
{
@@ -650,7 +650,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
*/
// Get state of origin
- CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
+ auto cap_orig = capture_motion->getOriginCapture();
const auto& x_origin = cap_orig->getFrame()->getState(state_structure_);
// Get motion at time stamp
@@ -876,7 +876,7 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
}
}
-CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
+CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
assert(_ts.ok());
// assert(last_ptr_ != nullptr);
@@ -901,9 +901,59 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
FrameBaseConstPtr frame = nullptr;
CaptureBaseConstPtr capture = nullptr;
CaptureMotionConstPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->rend();
- ++frame_rev_iter)
+ for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameMap().rbegin();
+ frame_rev_iter != getProblem()->getTrajectory()->getFrameMap().rend();
+ ++frame_rev_iter)
+ {
+ frame = frame_rev_iter->second;
+ auto sensor = getSensor();
+ capture = frame->getCaptureOf(sensor);
+ if (capture != nullptr)
+ {
+ assert(std::dynamic_pointer_cast<const CaptureMotion>(capture) != nullptr);
+ // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
+ capture_motion = std::static_pointer_cast<const CaptureMotion>(capture);
+
+ if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
+ {
+ // Found time stamp satisfying rule 3 above !! ==> break for loop
+ break;
+ }
+ else
+ capture_motion = nullptr;
+ }
+ }
+ return capture_motion;
+}
+
+CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts)
+{
+ assert(_ts.ok());
+ // assert(last_ptr_ != nullptr);
+
+ //Need to uncomment this line so that wolf_ros_apriltag doesn't crash
+ if(last_ptr_ == nullptr) return nullptr;
+
+ // First check if last_ptr is the one we are looking for
+ if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
+ return last_ptr_;
+
+
+ // Then look in the Wolf tree...
+ // -----------------------------
+ //
+
+ // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
+ // Note: since the buffer goes from a KF in the past until the next KF, we need to:
+ // 1. See that the KF contains a CaptureMotion
+ // 2. See that the TS is smaller than the KF's TS
+ // 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
+ 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();
+ ++frame_rev_iter)
{
frame = frame_rev_iter->second;
auto sensor = getSensor();
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index bf24a1d4e2d04c9111605792120df484e369f888..471dd627361266aad20356543345355a5d18bdbf 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -242,7 +242,7 @@ FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, Cap
return fac_odom;
}
-VectorXd ProcessorOdom3d::getCalibration (const CaptureBasePtr _capture) const
+VectorXd ProcessorOdom3d::getCalibration (const CaptureBaseConstPtr _capture) const
{
return VectorXd::Zero(0);
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 915dc1d190eb90bf0bcc6c70e31d551732d8f5e9..305561a0fe397184b929048577af70bcaf55370d 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -242,12 +242,12 @@ void SensorBase::updateLastCapture()
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
if (getProblem())
{
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
- TrajectoryRevIter frame_rev_it = trajectory->rbegin();
- while (frame_rev_it != trajectory->rend())
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+
+ TrajectoryRevIter frame_rev_it = frame_map.rbegin();
+ while (frame_rev_it != frame_map.rend())
{
- auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ 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 !
@@ -268,16 +268,15 @@ CaptureBaseConstPtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts) cons
if (not getProblem())
return nullptr;
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
+ 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 = trajectory->rbegin();
- while (frame_rev_it != trajectory->rend())
+ auto frame_rev_it = frame_map.rbegin();
+ while (frame_rev_it != frame_map.rend())
{
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ if (frame_rev_it->second->getTimeStamp() <= _ts)
{
- auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ auto capture = frame_rev_it->second->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
return capture;
@@ -294,16 +293,15 @@ CaptureBasePtr SensorBase::findLastCaptureBefore(const TimeStamp& _ts)
if (not getProblem())
return nullptr;
- // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
- auto trajectory = getProblem()->getTrajectory();
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- TrajectoryRevIter frame_rev_it = trajectory->rbegin();
- while (frame_rev_it != trajectory->rend())
+ TrajectoryRevIter frame_rev_it = frame_map.rbegin();
+ while (frame_rev_it != frame_map.rend())
{
- if ((*frame_rev_it)->getTimeStamp() <= _ts)
+ if (frame_rev_it->second->getTimeStamp() <= _ts)
{
- auto capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ auto capture = frame_rev_it->second->getCaptureOf(shared_from_this());
if (capture)
// found the most recent Capture made by this sensor !
return capture;
@@ -626,7 +624,7 @@ CheckLog SensorBase::localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std:
<< " ---> Hwd" << " @ " << hwd_ptr
<< " -X-> Sen" << id();
auto hwd_sen_list = hwd_ptr->getSensorList();
- auto hwd_has_sen = std::find_if(hwd_sen_list.begin(), hwd_sen_list.end(), [&_sen_ptr](SensorBasePtr sen){ return sen == _sen_ptr;});
+ auto hwd_has_sen = std::find(hwd_sen_list.begin(), hwd_sen_list.end(), _sen_ptr);
log.assertTrue(hwd_has_sen != hwd_sen_list.end(), inconsistency_explanation);
// Check processors
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 9cb6546c0216f4c8459ae1f4c5f434c885f8d953..350479924ff32b575836bbd4a6f51e9e2a52023f 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -40,7 +40,7 @@ StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlo
return _sb;
}
-void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) const
+void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem)
{
if (_problem != nullptr)
{
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 98f600fc1dae521ee0aac6f2e2745c759ee6a31b..c081dc0103bd0f1eb5dd25f5fe54bfa9febcb3b0 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -25,9 +25,10 @@
namespace wolf {
TrajectoryBase::TrajectoryBase() :
- NodeBase("TRAJECTORY", "TrajectoryBase")
+ NodeBase("TRAJECTORY", "TrajectoryBase"),
+ frame_map_(),
+ frame_const_map_()
{
- frame_map_ = FrameMap();
}
TrajectoryBase::~TrajectoryBase()
@@ -41,6 +42,7 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
+ frame_const_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
return _frame_ptr;
}
@@ -50,56 +52,75 @@ 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());
}
-void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
+void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
- for(auto fr_ptr : *this)
- fr_ptr->getFactorList(_fac_list);
+ for(auto fr_pair: frame_map_)
+ fr_pair.second->getFactorList(_fac_list);
}
+void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
+{
+ for(auto fr_pair: frame_map_)
+ fr_pair.second->getFactorList(_fac_list);
+}
-FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
+TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const
{
- FrameBasePtr closest_kf = nullptr;
//If frame_map_ is empty then closestFrameToTimeStamp is meaningless
- if(not frame_map_.empty())
- {
- //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
- //In the std they fulfill the following property:
- // lb is the first element such that ts <= lb, alternatively the smallest element that is NOT less than ts.
- // lb definition is NOT the ACTUAL lower bound but the following position so, lb = lb_true + 1.
-
- auto lower_bound = frame_map_.lower_bound(_ts);
- if((lower_bound != this->end() and lower_bound->first == _ts) or lower_bound == this->begin() ) closest_kf = lower_bound->second;
- else
- {
- auto upper_bound = lower_bound;
- //I find it easier to reason if lb < ts < ub. Remember that we have got rid of the
- //equality case and the out of bounds cases so this inequality is complete (it is not misssing cases).
- //Therefore, we need to decrease the lower_bound to the previous element
- lower_bound = std::prev(lower_bound);
-
- //If ub points to end() it means that the last frame is still less than _ts, therefore certainly
- //it will be the closest one
- if(upper_bound == this->end()) closest_kf = lower_bound->second;
- else
- {
- //Easy stuff just calculate the distance return minimum
- auto lb_diff = fabs(lower_bound->first - _ts);
- auto ub_diff = fabs(upper_bound->first - _ts);
- if(lb_diff < ub_diff)
- {
- closest_kf = lower_bound->second;
- }
- else
- {
- closest_kf = upper_bound->second;
- }
- }
- }
- }
- return closest_kf;
+ if(frame_map_.empty())
+ return TimeStamp::Invalid();
+
+ // Lower bound provides the first iterator that does not go before ts (maybe equal or greater)
+ auto lower_bound = frame_map_.lower_bound(_ts);
+
+ // If first frame does not go before ts, it is the closest one
+ if ( lower_bound == frame_map_.begin())
+ return frame_map_.begin()->first;
+
+ // If last frame goes before ts, it is the closest one as well
+ if ( lower_bound == frame_map_.end())
+ return frame_map_.rbegin()->first;
+
+ // Otherwise we have to compare lb and its previous
+ auto upper_bound = lower_bound;
+ lower_bound = std::prev(lower_bound);
+
+ auto lb_dist = fabs(lower_bound->first - _ts);
+ auto ub_dist = fabs(upper_bound->first - _ts);
+ if(lb_dist < ub_dist)
+ return lower_bound->first;
+ else
+ return upper_bound->first;
+
+ // unreachable
+ return TimeStamp();
+}
+
+FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
+{
+ auto closest_ts = closestTimeStampToTimeStamp(_ts);
+
+ assert(not closest_ts.ok() or frame_const_map_.count(closest_ts) != 0);
+
+ if (closest_ts.ok())
+ return frame_const_map_.at(closest_ts);
+
+ return nullptr;
+}
+
+FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
+{
+ auto closest_ts = closestTimeStampToTimeStamp(_ts);
+
+ assert(not closest_ts.ok() or frame_map_.count(closest_ts) != 0);
+
+ if (closest_ts.ok())
+ return frame_map_.at(closest_ts);
+
+ return nullptr;
}
void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
@@ -110,12 +131,12 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
- for (auto F : *this)
- if (F)
- F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+ for (auto F_pair : frame_const_map_)
+ if (F_pair.second)
+ F_pair.second->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
-CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBaseConstPtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
@@ -131,13 +152,14 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, s
log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
return log;
}
-bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+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);
auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs);
_log.compose(local_log);
- for (auto F : *this)
- F->check(_log, F, _verbose, _stream, _tabs + " ");
+ for (auto F_pair : frame_const_map_)
+ if (F_pair.second)
+ F_pair.second->check(_log, F_pair.second, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
} // namespace wolf
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 b2ab0e2fdc0d807e6953c0fab95c59ed391b5311..61276bc7755ae02071fd9bfa219260c41029bda1 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -46,9 +46,9 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
if (count_frames_ != 0)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
- FrameBasePtr remove_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
+ FrameBasePtr remove_recent_frame = std::next(getProblem()->getTrajectory()->getFrameMap().rbegin(),
params_swdr_->n_frames_recent)->second;
- FrameBasePtr keep_recent_frame = std::next(getProblem()->getTrajectory()->rbegin(),
+ FrameBasePtr keep_recent_frame = std::next(getProblem()->getTrajectory()->getFrameMap().rbegin(),
params_swdr_->n_frames_recent - 1)->second;
// compose motion captures for all processors motion