diff --git a/src/capture_motion.cpp b/src/capture_motion.cpp
index 93203d74dffb7ade049df63a2874d3882082e320..a4d74cbfbb67a5f60e9ec5debbcbbf72a6cc83ff 100644
--- a/src/capture_motion.cpp
+++ b/src/capture_motion.cpp
@@ -41,7 +41,7 @@ CaptureMotion::CaptureMotion(const TimeStamp& _ts,
buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
origin_frame_ptr_(_origin_frame_ptr)
{
- // TODO put something in the buffer to start!
+ //
}
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index d0b262886d00266461bafb2d34b6a0d6e443fd16..b72c7f4c741e3b650e78fbd761ab983e8ce231ac 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -169,7 +169,7 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame with covariance
- problem.setPrior(ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1, ts);
+ problem.setPrior(ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1, ts, 0.1);
// Ceres wrapper
ceres::Solver::Options ceres_options;
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 6cf91674418086f3d1d884c7f831bb4fa996e500..34f625fa16e5efc5f7a4feb1b64dd03e4a79b690 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -58,7 +58,7 @@ ConstraintBasePtr FeatureBase::addConstraint(ConstraintBasePtr _co_ptr)
if (getProblem() != nullptr)
getProblem()->addConstraintPtr(_co_ptr);
else
- std::cout << "WARNING: ADDING CONSTRAINT TO A FEATURE NOT CONNECTED WITH PROBLEM." << std::endl;
+ WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM.");
return _co_ptr;
}
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 326cc899c810a962689807bac2dc59cc3c0fe1bf..3318d01708db96e5bbd836cb92ecfb2437dda804 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -23,11 +23,15 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
state_block_vec_[0] = _p_ptr;
state_block_vec_[1] = _o_ptr;
state_block_vec_[2] = _v_ptr;
- //
-// if (isKey())
-// std::cout << "constructed +KF" << id() << std::endl;
+
+// if ( isKey() )
+// {
+// WOLF_DEBUG("New KF", this->id() );
+// }
// else
-// std::cout << "constructed +F" << id() << std::endl;
+// {
+// WOLF_DEBUG("New F", this->id() );
+// }
}
FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
@@ -43,10 +47,14 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
state_block_vec_[1] = _o_ptr;
state_block_vec_[2] = _v_ptr;
-// if (isKey())
-// std::cout << "constructed +KF" << id() << std::endl;
+// if ( isKey() )
+// {
+// WOLF_DEBUG("New KF", this->id() );
+// }
// else
-// std::cout << "constructed +F" << id() << std::endl;
+// {
+// WOLF_DEBUG("New F", this->id() );
+// }
}
FrameBase::~FrameBase()
@@ -124,6 +132,8 @@ void FrameBase::setKey()
getTrajectoryPtr()->setLastKeyFramePtr(shared_from_this());
getTrajectoryPtr()->sortFrame(shared_from_this());
+
+ WOLF_DEBUG("Set KF", this->id());
}
}
diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp
index f424fedc230a8c9186eb12ad0390ed49517010d2..27eb81987b9918e570c7a94420581f18b87b1651 100644
--- a/src/hello_wolf/hello_wolf.cpp
+++ b/src/hello_wolf/hello_wolf.cpp
@@ -167,7 +167,7 @@ int main()
TimeStamp t(0.0);
Vector3s x(0,0,0);
Matrix3s P = Matrix3s::Identity() * 0.1;
- problem->setPrior(x, P, t); // KF1
+ problem->setPrior(x, P, t, 0.5); // KF1
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
diff --git a/src/hello_wolf/processor_range_bearing.cpp b/src/hello_wolf/processor_range_bearing.cpp
index 0974d8b0d99d4e6d883ab238427f42dfc2fad390..3991741d59036ae0fb476e0edbca531866ff5d78 100644
--- a/src/hello_wolf/processor_range_bearing.cpp
+++ b/src/hello_wolf/processor_range_bearing.cpp
@@ -22,6 +22,18 @@ ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor
void ProcessorRangeBearing::process(CaptureBasePtr _capture)
{
+
+ if ( !kf_pack_buffer_.empty() )
+ {
+ // Select using incoming_ptr
+ KFPackPtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), time_tolerance_ );
+
+ if (pack!=nullptr)
+ keyFrameCallback(pack->key_frame,pack->time_tolerance);
+
+ kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() );
+ }
+
CaptureRangeBearingPtr capture = std::static_pointer_cast<CaptureRangeBearing>(_capture);
// 1. get KF -- we assume a KF is available to hold this _capture (checked in assert below)
diff --git a/src/hello_wolf/processor_range_bearing.h b/src/hello_wolf/processor_range_bearing.h
index 804cdd402d2c2312029667b55154d22be958c4f7..d21f59052da4ff6c10f05feec483f8c34e8c8ce6 100644
--- a/src/hello_wolf/processor_range_bearing.h
+++ b/src/hello_wolf/processor_range_bearing.h
@@ -45,7 +45,7 @@ class ProcessorRangeBearing : public ProcessorBase
// Implementation of pure virtuals from ProcessorBase
virtual void process (CaptureBasePtr _capture) override;
virtual bool voteForKeyFrame () override {return false;}
- virtual bool keyFrameCallback (FrameBasePtr _key_frame, const Scalar& _time_tolerance) override;
+ virtual bool keyFrameCallback (FrameBasePtr _key_frame, const Scalar& _time_tolerance) ;
// landmark observation models -- they would be better off in a separate library e.g. range_bearing_tools.h
Eigen::Vector2s observe (const Eigen::Vector2s& lmk_w) const;
diff --git a/src/motion_buffer.cpp b/src/motion_buffer.cpp
index a6c9ad4f1b4360a514e21736ca784fdf4d7e91bd..5356ea9f804a6d85442a2522c299e55c90eae8b8 100644
--- a/src/motion_buffer.cpp
+++ b/src/motion_buffer.cpp
@@ -73,6 +73,7 @@ MotionBuffer::MotionBuffer(Size _data_size,
calib_size_(_calib_size),
calib_preint_(_calib_size)
{
+ container_.clear();
}
const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
diff --git a/src/problem.cpp b/src/problem.cpp
index e801e145080c655d08a7db25feb8c2649881b9d3..63775a5ad6fedd519e993604ea745679c37a3f2d 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -31,7 +31,7 @@ Problem::Problem(const std::string& _frame_structure) :
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
processor_motion_ptr_(),
- origin_is_set_(false),
+ prior_is_set_(false),
state_size_(0),
state_cov_size_(0)
{
@@ -121,7 +121,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
_corresponding_sensor_ptr->addProcessor(prc_ptr);
// setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && origin_is_set_)
+ if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFramePtr());
// setting the main processor motion
@@ -330,7 +330,8 @@ bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
{
- //std::cout << "Problem::keyFrameCallback: processor " << _processor_ptr->getName() << std::endl;
+ WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor && (processor != _processor_ptr) )
@@ -628,12 +629,12 @@ StateBlockList& Problem::getStateBlockList()
-FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts)
+FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
{
- if (!origin_is_set_)
+ if ( ! prior_is_set_ )
{
// Create origin frame
- FrameBasePtr origin_frame_ptr = emplaceFrame(KEY_FRAME, _prior_state, _ts);
+ FrameBasePtr origin_keyframe = emplaceFrame(KEY_FRAME, _prior_state, _ts);
// create origin capture with the given state as data
// Capture fix only takes 3D position and Quaternion orientation
@@ -643,20 +644,27 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
else
init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
- origin_frame_ptr->addCapture(init_capture);
+ origin_keyframe->addCapture(init_capture);
// emplace feature and constraint
init_capture->emplaceFeatureAndConstraint();
- // notify all motion processors about the origin keyframe
- for (auto sensor_ptr : hardware_ptr_->getSensorList())
- for (auto processor_ptr : sensor_ptr->getProcessorList())
- if (processor_ptr->isMotion())
- (std::static_pointer_cast<ProcessorMotion>(processor_ptr))->setOrigin(origin_frame_ptr);
+ // Notify all processors about the prior KF
+ for (auto sensor : hardware_ptr_->getSensorList())
+ for (auto processor : sensor->getProcessorList())
+ if (processor->isMotion())
+ // Motion processors will set its origin at the KF
+ (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe);
+
+ prior_is_set_ = true;
- origin_is_set_ = true;
+ // Notify all other processors about the origin KF --> they will join it or not depending on their received data
+ for (auto sensor : hardware_ptr_->getSensorList())
+ for (auto processor : sensor->getProcessorList())
+ if ( !processor->isMotion() )
+ processor->keyFrameCallback(origin_keyframe, _time_tolerance);
- return origin_frame_ptr;
+ return origin_keyframe;
}
else
throw std::runtime_error("Origin already set!");
@@ -696,7 +704,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
if (i==0) cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
if (i==2) cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlockPtrAuto(i);
+ auto sb = S->getStateBlockPtrDynamic(i);
if (sb)
{
cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
@@ -735,10 +743,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
std::cout << " pm" << p->id() << " " << p->getType() << endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOriginPtr())
- cout << " o: C" << pm->getOriginPtr()->id() << " - F"
+ cout << " o: C" << pm->getOriginPtr()->id() << " - " << (pm->getOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
<< pm->getOriginPtr()->getFramePtr()->id() << endl;
if (pm->getLastPtr())
- cout << " l: C" << pm->getLastPtr()->id() << " - F"
+ cout << " l: C" << pm->getLastPtr()->id() << " - " << (pm->getLastPtr()->getFramePtr()->isKey() ? " KF" : " F")
<< pm->getLastPtr()->getFramePtr()->id() << endl;
if (pm->getIncomingPtr())
cout << " i: C" << pm->getIncomingPtr()->id() << endl;
@@ -750,10 +758,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
if (pt)
{
if (pt->getOriginPtr())
- cout << " o: C" << pt->getOriginPtr()->id() << " - F"
+ cout << " o: C" << pt->getOriginPtr()->id() << " - " << (pt->getOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
<< pt->getOriginPtr()->getFramePtr()->id() << endl;
if (pt->getLastPtr())
- cout << " l: C" << pt->getLastPtr()->id() << " - F"
+ cout << " l: C" << pt->getLastPtr()->id() << " - " << (pt->getLastPtr()->getFramePtr()->isKey() ? " KF" : " F")
<< pt->getLastPtr()->getFramePtr()->id() << endl;
if (pt->getIncomingPtr())
cout << " i: C" << pt->getIncomingPtr()->id() << endl;
@@ -833,9 +841,9 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
try
{
CaptureMotionPtr CM = std::static_pointer_cast<CaptureMotion>(C);
+ cout << " buffer size : " << CM->getBuffer().get().size() << endl;
if ( CM->getCalibSize() > 0 && ! CM->getBuffer().get().empty())
{
- cout << " buffer size : " << CM->getBuffer().get().size() << endl;
cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
@@ -853,7 +861,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// Features
for (auto f : C->getFeatureList())
{
- cout << " f" << f->id() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c " : "");
+ cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c " : "");
if (constr_by)
{
cout << " <--\t";
diff --git a/src/problem.h b/src/problem.h
index af39c70369a732e7ab624d00b3113859f2450565..adaa9c823bd21f59a710dbf613b081f0c6fbf7c8 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -54,7 +54,7 @@ class Problem : public std::enable_shared_from_this<Problem>
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
std::list<StateBlockNotification> state_block_notification_list_;
std::list<ConstraintNotification> constraint_notification_list_;
- bool origin_is_set_;
+ bool prior_is_set_;
Size state_size_, state_cov_size_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
@@ -143,7 +143,8 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr getTrajectoryPtr();
virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
const Eigen::MatrixXs& _prior_cov, //
- const TimeStamp& _ts);
+ const TimeStamp& _ts,
+ const Scalar _time_tolerance);
/** \brief Emplace frame from string frame_structure
* \param _frame_structure String indicating the frame structure.
@@ -311,6 +312,10 @@ class Problem : public std::enable_shared_from_this<Problem>
bool state_blocks = false);
bool check(int verbose_level = 0);
+ bool priorIsSet() const
+ {
+ return prior_is_set_;
+ }
};
} // namespace wolf
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index b14237b0366db2f567c8582f6969db8a20cbf933..785cccc5e14383d41774d6ea322b0dd131d681a6 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -9,10 +9,10 @@ unsigned int ProcessorBase::processor_id_count_ = 0;
ProcessorBase::ProcessorBase(const std::string& _type, const Scalar& _time_tolerance) :
NodeBase("PROCESSOR", _type),
- sensor_ptr_(),
- is_removing_(false),
processor_id_(++processor_id_count_),
- time_tolerance_(_time_tolerance)
+ time_tolerance_(_time_tolerance),
+ sensor_ptr_(),
+ is_removing_(false)
{
// WOLF_DEBUG("constructed +p" , id());
}
@@ -45,6 +45,13 @@ FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _captur
return new_frame_ptr;
}
+void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
+{
+ if (_keyframe_ptr != nullptr)
+ kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other);
+
+}
+
void ProcessorBase::remove()
{
if (!is_removing_)
@@ -67,4 +74,113 @@ void ProcessorBase::remove()
}
}
+
+
+/////////////////////////////////////////////////////////////////////////////////////////
+
+void KFPackBuffer::removeUpTo(const TimeStamp& _time_stamp)
+{
+ KFPackBuffer::Iterator post = container_.upper_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
+void KFPackBuffer::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
+{
+ TimeStamp time_stamp = _key_frame->getTimeStamp();
+ KFPackPtr kfpack = std::make_shared<KFPack>(_key_frame, _time_tolerance);
+ container_.emplace(time_stamp, kfpack);
+}
+
+KFPackPtr KFPackBuffer::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+{
+ if (container_.empty())
+ return nullptr;
+
+ KFPackBuffer::Iterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+ bool post_exists = (post != container_.end());
+
+ bool post_ok = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
+
+ if (prev_exists)
+ {
+ KFPackBuffer::Iterator prev = std::prev(post);
+
+ bool prev_ok = checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
+
+ if (prev_ok && !post_ok)
+ return prev->second;
+
+ else if (!prev_ok && post_ok)
+ return post->second;
+
+ else if (prev_ok && post_ok)
+ {
+ if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+ return post->second;
+ else
+ return prev->second;
+ }
+ }
+ else if (post_ok)
+ return post->second;
+
+ return nullptr;
+}
+KFPackPtr KFPackBuffer::selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+{
+ return selectPack(_capture->getTimeStamp(), _time_tolerance);
+}
+
+KFPackPtr KFPackBuffer::selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+{
+ if (container_.empty())
+ return nullptr;
+
+ KFPackBuffer::Iterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+
+ if (prev_exists)
+ return container_.begin()->second;
+
+ else
+ {
+ bool post_exists = (post != container_.end());
+ bool post_ok = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
+
+ if (post_ok)
+ return post->second;
+ }
+
+ return nullptr;
+}
+
+KFPackPtr KFPackBuffer::selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+{
+ return selectPackBefore(_capture->getTimeStamp(), _time_tolerance);
+}
+
+void KFPackBuffer::print(void)
+{
+ std::cout << "[ ";
+ for (auto iter : container_)
+ {
+ std::cout << "( tstamp: " << iter.first << ", id: " << iter.second->key_frame->id() << ") ";
+ }
+ std::cout << "]" << std::endl;
+}
+
+bool KFPackBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
+ const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
+{
+ Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
+ Scalar time_tol = std::min(_time_tolerance1, _time_tolerance2);
+ bool pass = time_diff <= time_tol;
+ return pass;
+}
+
+
+
} // namespace wolf
diff --git a/src/processor_base.h b/src/processor_base.h
index e04aba1e24309e17e3bacd23beead112ac50c9a1..ce506925f2bb06118633912d70dfbfd44f5c9237 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -6,16 +6,99 @@ namespace wolf{
class SensorBase;
}
-//Wolf includes
+// Wolf includes
#include "wolf.h"
#include "node_base.h"
+#include "time_stamp.h"
// std
#include <memory>
+#include <map>
namespace wolf {
+/** \brief Key frame class pack
+ *
+ * To store a key_frame with an associated time tolerance.
+ */
+class KFPack
+{
+ public:
+ KFPack(const FrameBasePtr _key_frame, const Scalar _time_tolerance) : key_frame(_key_frame), time_tolerance(_time_tolerance) {};
+ ~KFPack(){};
+ FrameBasePtr key_frame;
+ Scalar time_tolerance;
+};
+
+WOLF_PTR_TYPEDEFS(KFPack);
+
+
+
+/** \brief Buffer of Key frame class objects
+ *
+ * Object and functions to manage a buffer of KFPack objects.
+ */
+class KFPackBuffer
+{
+ public:
+
+ typedef std::map<TimeStamp,KFPackPtr>::iterator Iterator; // buffer iterator
+
+ KFPackBuffer(void);
+ ~KFPackBuffer(void);
+
+ /**\brief Select a Pack from the buffer
+ *
+ * Select from the buffer the closest pack (w.r.t. time stamp),
+ * respecting a defined time tolerances
+ */
+ KFPackPtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+ KFPackPtr selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
+
+ KFPackPtr selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+ KFPackPtr selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
+
+ /**\brief Buffer size
+ *
+ */
+ size_t size(void);
+
+ /**\brief Add a pack to the buffer
+ *
+ */
+ void add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance);
+
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpTo(const TimeStamp& _time_stamp);
+
+ /**\brief Check time tolerance
+ *
+ * Check if the time distance between two time stamps is smaller than
+ * the minimum time tolerance of the two frames.
+ */
+ bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
+
+ /**\brief Clear the buffer
+ *
+ */
+ void clear();
+ /**\brief Empty the buffer
+ *
+ */
+ bool empty();
+
+ /**\brief Print buffer information
+ *
+ */
+ void print();
+
+ private:
+
+ std::map<TimeStamp,KFPackPtr> container_; // Main buffer container
+};
/** \brief base struct for processor parameters
*
@@ -34,6 +117,11 @@ struct ProcessorParamsBase
//class ProcessorBase
class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
{
+ protected:
+ unsigned int processor_id_;
+ Scalar time_tolerance_; ///< self time tolerance for adding a capture into a frame
+ KFPackBuffer kf_pack_buffer_;
+
private:
SensorBaseWPtr sensor_ptr_;
@@ -78,7 +166,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*/
FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state);
- virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) = 0;
+// virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) = 0;
+
+ void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
SensorBasePtr getSensorPtr();
const SensorBasePtr getSensorPtr() const;
@@ -88,9 +178,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setTimeTolerance(Scalar _time_tolerance);
- protected:
- unsigned int processor_id_;
- Scalar time_tolerance_; ///< self time tolerance for adding a capture into a frame
};
}
@@ -125,6 +212,31 @@ inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
time_tolerance_ = _time_tolerance;
}
+inline KFPackBuffer::KFPackBuffer(void)
+{
+
+}
+
+inline KFPackBuffer::~KFPackBuffer(void)
+{
+
+}
+
+inline void KFPackBuffer::clear()
+{
+ container_.clear();
+}
+
+inline bool KFPackBuffer::empty()
+{
+ return container_.empty();
+}
+
+inline size_t KFPackBuffer::size(void)
+{
+ return container_.size();
+}
+
} // namespace wolf
#endif
diff --git a/src/processor_capture_holder.h b/src/processor_capture_holder.h
index dd3b6bd71c146be7d8652fce423751e4658ae973..def28671964c642e1411f57c6ed4b2b857b09ddb 100644
--- a/src/processor_capture_holder.h
+++ b/src/processor_capture_holder.h
@@ -48,7 +48,7 @@ public:
virtual bool voteForKeyFrame() override { return false; }
virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tolerance) override;
+ const Scalar& _time_tolerance) ;
/**
* \brief Finds the capture that contains the closest previous motion of _ts
diff --git a/src/processor_image_feature.cpp b/src/processor_image_feature.cpp
index 0fe5993ca0076ad5cb750ee4f2fa6865e3c63d6a..c728686d73a4d9ce85ef5c61cf6dfd381522a1b6 100644
--- a/src/processor_image_feature.cpp
+++ b/src/processor_image_feature.cpp
@@ -214,8 +214,6 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur
incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
_feature_list_out.push_back(incoming_point_ptr);
- incoming_point_ptr->setTrackId(feature_ptr->trackId());
-
_feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
}
else
@@ -321,7 +319,6 @@ unsigned int ProcessorImageFeature::detectNewFeatures(const unsigned int& _max_n
new_descriptors.row(index),
Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var);
point_ptr->setIsKnown(false);
- point_ptr->setTrackId(point_ptr->id());
addNewFeatureLast(point_ptr);
active_search_grid_.hitCell(new_keypoints[0]);
@@ -369,10 +366,10 @@ unsigned int ProcessorImageFeature::detect(cv::Mat _image, cv::Rect& _roi, std::
detector_descriptor_ptr_->detect(_image_roi, _new_keypoints);
detector_descriptor_ptr_->compute(_image_roi, _new_keypoints, new_descriptors);
- for (unsigned int i = 0; i < _new_keypoints.size(); i++)
+ for (auto point : _new_keypoints)
{
- _new_keypoints[i].pt.x = _new_keypoints[i].pt.x + _roi.x;
- _new_keypoints[i].pt.y = _new_keypoints[i].pt.y + _roi.y;
+ point.pt.x += _roi.x;
+ point.pt.y += _roi.y;
}
return _new_keypoints.size();
}
diff --git a/src/processor_image_feature.h b/src/processor_image_feature.h
index e489c5a4a1e89346c2182b3661c93646168ed1d9..aa007e00780b8372c1493e15541a264d5fbe68e1 100644
--- a/src/processor_image_feature.h
+++ b/src/processor_image_feature.h
@@ -87,15 +87,15 @@ class ProcessorImageFeature : public ProcessorTrackerFeature
*/
void postProcess();
- void advance()
+ void advanceDerived()
{
- ProcessorTrackerFeature::advance();
+ ProcessorTrackerFeature::advanceDerived();
image_last_ = image_incoming_;
}
- void reset()
+ void resetDerived()
{
- ProcessorTrackerFeature::reset();
+ ProcessorTrackerFeature::resetDerived();
image_last_ = image_incoming_;
}
diff --git a/src/processor_image_landmark.h b/src/processor_image_landmark.h
index a3732ba631b1b1ec3b28bbacf5fe324395c52832..489502b529eaf72dead7ff4938b3c32514ca437c 100644
--- a/src/processor_image_landmark.h
+++ b/src/processor_image_landmark.h
@@ -101,15 +101,15 @@ class ProcessorImageLandmark : public ProcessorTrackerLandmark
*/
void preProcess();
- void advance()
+ void advanceDerived()
{
- ProcessorTrackerLandmark::advance();
+ ProcessorTrackerLandmark::advanceDerived();
image_last_ = image_incoming_;
}
- void reset()
+ void resetDerived()
{
- ProcessorTrackerLandmark::reset();
+ ProcessorTrackerLandmark::resetDerived();
image_last_ = image_incoming_;
}
diff --git a/src/processor_loopclosure_base.h b/src/processor_loopclosure_base.h
index 3b7e24fafe4320610f0d11de804db24f2f35cd8d..8c5f1493865816820ace7e78b4e556a7ccd8bde2 100644
--- a/src/processor_loopclosure_base.h
+++ b/src/processor_loopclosure_base.h
@@ -57,7 +57,7 @@ public:
virtual void process(CaptureBasePtr _incoming_ptr) override;
virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tol_other) override;
+ const Scalar& _time_tol_other) ;
const std::vector<FrameBasePtr>& getCandidates() const noexcept;
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index a8e94f2432955dc03d0068f8f9f46a4079d7cb78..731cb7f2f635dc405c0e6ff50cb1e06fc1f8d451 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -10,6 +10,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
Scalar _time_tolerance,
Size _calib_size) :
ProcessorBase(_type, _time_tolerance),
+ processing_step_(RUNNING_WITHOUT_PACK),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
@@ -40,57 +41,171 @@ ProcessorMotion::~ProcessorMotion()
void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
{
- if (_incoming_ptr == nullptr)
- {
- WOLF_ERROR("Process got a nullptr !");
- return;
- }
+ if (_incoming_ptr == nullptr)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+
+ incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+
+ preProcess(); // Derived class operations
+
+ KFPackPtr pack = computeProcessingStep();
+ if (pack)
+ kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
- if (status_ == IDLE)
+ switch(processing_step_)
{
- TimeStamp t0 = _incoming_ptr->getTimeStamp();
- if (origin_ptr_ == nullptr)
+ case RUNNING_WITHOUT_PACK :
+ case RUNNING_WITH_PACK_ON_ORIGIN :
+ break;
+
+ case RUNNING_WITH_PACK_BEFORE_ORIGIN :
{
- auto frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t0);
- if (frm && fabs(frm->getTimeStamp() - t0) < time_tolerance_)
+ // extract pack elements
+ FrameBasePtr keyframe_from_callback = pack->key_frame;
+ TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+
+ // find the capture whose buffer is affected by the new keyframe
+ auto existing_capture = findCaptureContainingTimeStamp(ts_from_callback);
+
+ // Find the frame acting as the capture's origin
+ auto keyframe_origin = existing_capture->getOriginFramePtr();
+
+ // emplace a new motion capture to the new keyframe
+ auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
+ getSensorPtr(),
+ ts_from_callback,
+ Eigen::VectorXs::Zero(data_size_),
+ existing_capture->getDataCovariance(),
+ existing_capture->getCalibration(),
+ existing_capture->getCalibration(),
+ keyframe_origin);
+
+ // split the buffer
+ // and give the part of the buffer before the new keyframe to the capture for the KF callback
+ existing_capture->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
+
+
+ // interpolate individual delta
+ if (!existing_capture->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
{
- std::cout << "PM: join KF" << std::endl;
- // Join existing KF
- setOrigin(frm);
+ // interpolate Motion at the new time stamp
+ Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer
+ existing_capture->getBuffer().get().front(), // first motion of new buffer
+ ts_from_callback);
+ // add to old buffer
+ capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
}
- else
+
+
+ // create motion feature and add it to the capture
+ auto new_feature = emplaceFeature(capture_for_keyframe_callback);
+
+ // create motion constraint and add it to the feature, and constrain to the other capture (origin)
+ emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
+
+ // Update the existing capture
+ existing_capture->setOriginFramePtr(keyframe_from_callback);
+
+ // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
+ reintegrateBuffer(existing_capture);
+
+ // modify existing feature and constraint (if they exist in the existing capture)
+ if (!existing_capture->getFeatureList().empty())
{
- // Create new KF for origin
- std::cout << "PM: make KF" << std::endl;
- VectorXs x0 = getProblem()->zeroState();
- setOrigin(x0, t0);
+ auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
+
+ // Modify existing feature --------
+ existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_);
+ existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
+
+ // Modify existing constraint --------
+ // Instead of modifying, we remove one ctr, and create a new one.
+ auto ctr_to_remove = existing_feature->getConstraintList().back(); // there is only one constraint!
+ auto new_ctr = emplaceConstraint(existing_feature, capture_for_keyframe_callback);
+ ctr_to_remove ->remove(); // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
+ break;
}
- status_ = RUNNING;
- }
- incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
- /// @todo Anything else to do ?
- if (incoming_ptr_ == nullptr) return;
+ case RUNNING_WITH_PACK_AFTER_ORIGIN :
+ {
+ // extract pack elements
+ FrameBasePtr keyframe_from_callback = pack->key_frame;
+ TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+
+ // Find the frame acting as the capture's origin
+ auto keyframe_origin = last_ptr_->getOriginFramePtr();
+
+ // emplace a new motion capture to the new keyframe
+ VectorXs calib = last_ptr_->getCalibration();
+ auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
+ getSensorPtr(),
+ ts_from_callback,
+ Eigen::VectorXs::Zero(data_size_),
+ last_ptr_->getDataCovariance(),
+ calib,
+ calib,
+ keyframe_origin);
+
+ // split the buffer
+ // and give the part of the buffer before the new keyframe to the capture for the KF callback
+ last_ptr_->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
+
+ // interpolate individual delta
+ if (!last_ptr_->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
+ {
+ // interpolate Motion at the new time stamp
+ Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer
+ last_ptr_->getBuffer().get().front(), // first motion of new buffer
+ ts_from_callback);
+ // add to old buffer
+ capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
+ }
+
+ // create motion feature and add it to the capture
+ auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
+
+ // create motion constraint and add it to the feature, and constrain to the other capture (origin)
+ emplaceConstraint(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensorPtr()) );
+
+ // reset processor origin
+ origin_ptr_ = capture_for_keyframe_callback;
+
+ // Update the existing capture
+ last_ptr_->setOriginFramePtr(keyframe_from_callback);
+
+ // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
+ reintegrateBuffer(last_ptr_);
+
+ break;
+ }
+
- preProcess();
+
+ default :
+ break;
+ }
+
+ ////////////////////////////////////////////////////
+ // NOW on with the received data
// integrate data
integrateOneStep();
// Update state and time stamps
- last_ptr_->setTimeStamp(incoming_ptr_->getTimeStamp());
- last_ptr_->getFramePtr()->setTimeStamp(last_ptr_->getTimeStamp());
+ last_ptr_->setTimeStamp(getCurrentTimeStamp());
+ last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp());
last_ptr_->getFramePtr()->setState(getCurrentState());
if (voteForKeyFrame() && permittedKeyFrame())
{
// Set the frame of last_ptr as key
auto key_frame_ptr = last_ptr_->getFramePtr();
- key_frame_ptr->setState(getCurrentState());
- key_frame_ptr->setTimeStamp(getCurrentTimeStamp());
key_frame_ptr->setKey();
// create motion feature and add it to the key_capture
@@ -113,7 +228,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
last_ptr_->getCalibration(),
key_frame_ptr);
// reset the new buffer
- new_capture_ptr->getBuffer().get().clear();
new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
// reset integrals
@@ -123,28 +237,30 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
delta_integrated_cov_ . setZero();
jacobian_calib_ . setZero();
- // reset processor origin to the new keyframe's capture
- origin_ptr_ = last_ptr_;
- last_ptr_ = new_capture_ptr;
-
// reset derived things
resetDerived();
+ // Update pointers
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = new_capture_ptr;
+
// callback to other processors
getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
}
-
- postProcess();
+ resetDerived(); // TODO see where to put this
// clear incoming just in case
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+
+ postProcess();
}
+
void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
{
CaptureMotionPtr capture_motion;
- if (_ts >= origin_ptr_->getTimeStamp())
+ if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
// timestamp found in the current processor buffer
capture_motion = last_ptr_;
else
@@ -160,8 +276,12 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
statePlusDelta(state_0, delta, dt, _x);
}
else
+ {
// We could not find any CaptureMotion for the time stamp requested
- std::runtime_error("Could not find any Capture for the time stamp requested");
+ WOLF_ERROR("Could not find any Capture for the time stamp requested. ");
+ WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
+ throw std::runtime_error("Could not find any Capture for the time stamp requested. Did you forget to call Problem::setPrior() in your application?");
+ }
}
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
@@ -235,7 +355,6 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
_origin_frame);
// clear and reset buffer
- getBuffer().get().clear();
getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
// Reset integrals
@@ -249,91 +368,10 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
resetDerived();
}
-bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& _time_tol_other)
-{
-
- assert(_new_keyframe->getTrajectoryPtr() != nullptr
- && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
-
- // get keyframe's time stamp
- TimeStamp new_ts = _new_keyframe->getTimeStamp();
-
- // find the capture whose buffer is affected by the new keyframe
- auto existing_capture = findCaptureContainingTimeStamp(new_ts);
- assert(existing_capture != nullptr
- && "ProcessorMotion::keyFrameCallback: no motion capture containing the required TimeStamp found");
-
- // Find the frame acting as the capture's origin
- auto keyframe_origin = existing_capture->getOriginFramePtr();
-
- // emplace a new motion capture to the new keyframe
- auto new_capture = emplaceCapture(_new_keyframe,
- getSensorPtr(),
- new_ts,
- Eigen::VectorXs::Zero(data_size_),
- existing_capture->getDataCovariance(),
- existing_capture->getCalibration(),
- existing_capture->getCalibration(),
- keyframe_origin);
-
- // split the buffer
- // and give the part of the buffer before the new keyframe to the key_capture
- existing_capture->getBuffer().split(new_ts, new_capture->getBuffer());
-
- // interpolate individual delta
- if (!existing_capture->getBuffer().get().empty() && new_capture->getBuffer().get().back().ts_ != new_ts)
- {
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(new_capture->getBuffer().get().back(), // last Motion of old buffer
- existing_capture->getBuffer().get().front(), // first motion of new buffer
- new_ts);
- // add to old buffer
- new_capture->getBuffer().get().push_back(motion_interpolated);
- }
-
- // create motion feature and add it to the capture
- auto new_feature = emplaceFeature(new_capture);
-// reintegrateBuffer(new_capture);
-
- // create motion constraint and add it to the feature, and constrain to the other capture (origin)
- emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
-
-
-
- /////////////////////////////////////////////////////////
- // Update the existing capture
- if (existing_capture == last_ptr_)
- // reset processor origin
- origin_ptr_ = new_capture;
-
- existing_capture->setOriginFramePtr(_new_keyframe);
-
- // reintegrate existing buffer -- note: the result of re-integration is stored in the same buffer!
- reintegrateBuffer(existing_capture);
-
- // modify existing feature and constraint (if they exist in the existing capture)
- if (!existing_capture->getFeatureList().empty())
- {
- auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
-
- // Modify existing feature --------
- existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_);
- existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
-
- // Modify existing constraint --------
- // Instead of modifying, we remove one ctr, and create a new one.
- auto ctr_to_remove = existing_feature->getConstraintList().back(); // there is only one constraint!
- auto new_ctr = emplaceConstraint(existing_feature, new_capture);
- ctr_to_remove ->remove(); // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
- }
-
- return true;
-}
-
void ProcessorMotion::integrateOneStep()
{
// Set dt
- updateDt();
+ dt_ = updateDt();
// get vector of parameters to calibrate
calib_ = getBuffer().getCalibrationPreint();
@@ -524,5 +562,40 @@ FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion)
return feature;
}
+KFPackPtr ProcessorMotion::computeProcessingStep()
+{
+ if (!getProblem()->priorIsSet())
+ {
+ WOLF_WARN ("||*||");
+ WOLF_INFO (" ... It seems you missed something!");
+ WOLF_ERROR("ProcessorMotion received data before being initialized.");
+ WOLF_INFO ("Did you forget to issue a Problem::setPrior()?");
+ throw std::runtime_error("ProcessorMotion received data before being initialized.");
+ }
+
+ KFPackPtr pack = kf_pack_buffer_.selectPackBefore(last_ptr_, time_tolerance_);
+
+ if (pack)
+ {
+ if (kf_pack_buffer_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), time_tolerance_))
+ {
+ WOLF_WARN("||*||");
+ WOLF_INFO(" ... It seems you missed something!");
+ WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
+ // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
+ processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN;
+ }
+ else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp() - time_tolerance_)
+ processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
+
+ else
+ processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
+
+ }
+ else
+ processing_step_ = RUNNING_WITHOUT_PACK;
+
+ return pack;
+}
}
diff --git a/src/processor_motion.h b/src/processor_motion.h
index 4def813e2b5b2588f95b9b71ccd7998378fb5599..93d1af04a43ee77e02661331d1beb8031e2ed49a 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -99,6 +99,17 @@ namespace wolf
*/
class ProcessorMotion : public ProcessorBase
{
+ public:
+ typedef enum {
+ RUNNING_WITHOUT_PACK,
+ RUNNING_WITH_PACK_BEFORE_ORIGIN,
+ RUNNING_WITH_PACK_ON_ORIGIN,
+ RUNNING_WITH_PACK_AFTER_ORIGIN
+ } ProcessingStep ;
+
+ protected:
+ ProcessingStep processing_step_; ///< State machine controlling the processing step
+
private:
enum
{
@@ -119,6 +130,7 @@ class ProcessorMotion : public ProcessorBase
// Instructions to the processor:
+ void process2(CaptureBasePtr _incoming_ptr);
void process(CaptureBasePtr _incoming_ptr);
virtual void resetDerived();
@@ -182,8 +194,6 @@ class ProcessorMotion : public ProcessorBase
*/
FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
- virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol);
-
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
@@ -191,7 +201,7 @@ class ProcessorMotion : public ProcessorBase
// Helper functions:
protected:
- void updateDt();
+ Scalar updateDt();
void integrateOneStep();
void splitBuffer(const TimeStamp& _t_split, MotionBuffer& _oldest_part);
void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
@@ -221,6 +231,8 @@ class ProcessorMotion : public ProcessorBase
*/
virtual void postProcess() { };
+ KFPackPtr computeProcessingStep();
+
// These are the pure virtual functions doing the mathematics
protected:
@@ -498,9 +510,9 @@ inline bool ProcessorMotion::isMotion()
return true;
}
-inline void ProcessorMotion::updateDt()
+inline Scalar ProcessorMotion::updateDt()
{
- dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
+ return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
}
inline const MotionBuffer& ProcessorMotion::getBuffer() const
diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index cc2b764e11126f549d8910ae60f838eacc0fb5fe..1268c2debc5c2a7320e1479009657820f728ddcb 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -15,7 +15,11 @@ namespace wolf
{
ProcessorTracker::ProcessorTracker(const std::string& _type, const unsigned int _max_new_features, const Scalar& _time_tolerance) :
- ProcessorBase(_type, _time_tolerance), origin_ptr_(nullptr), last_ptr_(nullptr), incoming_ptr_(nullptr),
+ ProcessorBase(_type, _time_tolerance),
+ processing_step_(FIRST_TIME_WITHOUT_PACK),
+ origin_ptr_(nullptr),
+ last_ptr_(nullptr),
+ incoming_ptr_(nullptr),
max_new_features_(_max_new_features)
{
//
@@ -28,231 +32,230 @@ ProcessorTracker::~ProcessorTracker()
void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
-
using std::abs;
+ if (_incoming_ptr == nullptr)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+
incoming_ptr_ = _incoming_ptr;
- preProcess();
+ preProcess(); // Derived class operations
- // FIRST TIME
- if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
+ computeProcessingStep();
+
+ switch(processing_step_)
{
- WOLF_DEBUG( "FIRST TIME" );
+ case FIRST_TIME_WITH_PACK :
+ {
+ KFPackPtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, time_tolerance_);
+ kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
- // advance
- advance();
+ WOLF_DEBUG( "PT: KF" , pack->key_frame->id() , " callback received with ts= " , pack->key_frame->getTimeStamp().get() );
- // advance this
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
+ // Append incoming to KF
+ pack->key_frame->addCapture(incoming_ptr_);
- // keyframe creation on last
- FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
- if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - last_ptr_->getTimeStamp()) <= time_tolerance_)
- {
- // Set KF
- closest_key_frm->addCapture(last_ptr_);
- closest_key_frm->setKey();
- WOLF_DEBUG( "Last appended to existing F, set KF" , closest_key_frm->id() );
+ // Process info
+ // We only process new features in Last, here last = nullptr, so we do not have anything to do.
+
+ // Update pointers
+ resetDerived();
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
+
+ break;
}
- else
+ case FIRST_TIME_WITHOUT_PACK :
{
- // Make KF
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME,
- getProblem()->getState(last_ptr_->getTimeStamp()),
- last_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
- WOLF_DEBUG( "Last appended to new KF" , new_frame_ptr->id() );
-
- getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), time_tolerance_);
- }
+ FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp());
+ kfrm->addCapture(incoming_ptr_);
- // Detect new Features, initialize Landmarks, create Constraints, ...
- processNew(max_new_features_);
-
- // Establish constraints from last
- establishConstraints();
- }
+ // Process info
+ // We only process new features in Last, here last = nullptr, so we do not have anything to do.
- // SECOND TIME or after KEY FRAME CALLBACK
- else if (origin_ptr_ == nullptr)
- {
- WOLF_DEBUG("SECOND TIME or after KEY FRAME CALLBACK");
+ // Issue KF callback with new KF
+ getProblem()->keyFrameCallback(kfrm, shared_from_this(), time_tolerance_);
- // First we track the known Features
- processKnown();
+ // Update pointers
+ resetDerived();
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
- // Create a new non-key Frame in the Trajectory with the incoming Capture
- FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
- if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp()) <= time_tolerance_)
- {
- // Just append the Capture to the existing keyframe
- closest_key_frm->addCapture(incoming_ptr_);
- WOLF_DEBUG("Incoming appended to F" , closest_key_frm->id() );
+ break;
}
- else
+ case SECOND_TIME_WITH_PACK :
+ case SECOND_TIME_WITHOUT_PACK :
{
- // Create a frame to hold what will become the last Capture
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
- WOLF_DEBUG("Incoming in new F" , new_frame_ptr->id() );
- }
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+ frm->addCapture(incoming_ptr_);
- // Reset the derived Tracker
- reset();
+ // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
- // Reset this
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+ // Process info
+ processNew(max_new_features_);
+ // Update pointers
+ resetDerived();
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
- }
+ break;
+ }
+ case RUNNING_WITH_PACK :
+ {
+ KFPackPtr pack = kf_pack_buffer_.selectPack( last_ptr_ , time_tolerance_);
+ kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
- // OTHER TIMES
- else
- {
- WOLF_DEBUG("OTHER TIMES");
+ WOLF_DEBUG( "PT: KF" , pack->key_frame->id() , " callback received with ts= " , pack->key_frame->getTimeStamp().get() );
- // 1. First we track the known Features and create new constraints as needed
+ processKnown();
- processKnown();
+ // Capture last_ is added to the new keyframe
+ FrameBasePtr last_old_frame = last_ptr_->getFramePtr();
+ last_old_frame->unlinkCapture(last_ptr_);
+ last_old_frame->remove();
+ pack->key_frame->addCapture(last_ptr_);
- // 2. Then we see if we want and we are allowed to create a KeyFrame
- // Three conditions to make a KF:
- // - We vote for KF
- // - Problem allows us to make keyframe
- // - There is no existing KF very close to our Time Stamp <--- NOT SURE OF THIS
+ // Create new frame
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+ frm->addCapture(incoming_ptr_);
+
+ // Detect new Features, initialize Landmarks, create Constraints, ...
+ processNew(max_new_features_);
- FrameBasePtr closest_key_frm_to_last = last_ptr_->getFramePtr(); // start with the same last's frame
- if ( ! ( closest_key_frm_to_last && closest_key_frm_to_last->isKey() ) ) // last F is not KF
- closest_key_frm_to_last = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
+ // Establish constraints
+ establishConstraints();
- if (closest_key_frm_to_last && abs(closest_key_frm_to_last->getTimeStamp() - last_ptr_->getTimeStamp()) > time_tolerance_) // closest KF is not close enough
- closest_key_frm_to_last = nullptr;
+ // Update pointers
+ resetDerived();
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
- if ( !( (voteForKeyFrame() && permittedKeyFrame() ) || closest_key_frm_to_last ) )
+ break;
+ }
+ case RUNNING_WITHOUT_PACK :
{
+ processKnown();
- // 2.a. We did not create a KeyFrame:
+ if (voteForKeyFrame() && permittedKeyFrame())
+ {
+ // We create a KF
- // advance the derived tracker
- advance();
+ // set KF on last
+ last_ptr_->getFramePtr()->setKey();
- // Advance this
- last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
- last_ptr_->remove();
- incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
- last_ptr_ = incoming_ptr_; // Incoming Capture takes the place of last Capture
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+ // make F; append incoming to new F
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
+ frm->addCapture(incoming_ptr_);
- WOLF_DEBUG("last <-- incoming");
+ // process
+ processNew(max_new_features_);
- }
- else
- {
+ // Set key
+ last_ptr_->getFramePtr()->setKey();
- // 2.b. We create a KF
+ // Set state to the keyframe
+ last_ptr_->getFramePtr()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- // Detect new Features, initialize Landmarks, create Constraints, ...
- processNew(max_new_features_);
+ // Establish constraints
+ establishConstraints();
+
+ // Call the new keyframe callback in order to let the other processors to establish their constraints
+ getProblem()->keyFrameCallback(last_ptr_->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), time_tolerance_);
+
+ // Update pointers
+ resetDerived();
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
- FrameBasePtr key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
- if ( abs(key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp() ) < time_tolerance_)
- {
- // Append incoming to existing key-frame
- key_frm->addCapture(incoming_ptr_);
- WOLF_DEBUG("Incoming adhered to existing KF" , key_frm->id());
}
else
{
- // Create a new non-key Frame in the Trajectory with the incoming Capture
- // Make a non-key-frame to hold incoming
- FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
- new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
- WOLF_DEBUG( "Incoming adhered to new F" , key_frm->id() );
-
- // Make the last Capture's Frame a KeyFrame
- setKeyFrame(last_ptr_);
- WOLF_DEBUG( "Set KEY to last F" , key_frm->id() );
- }
-
- // Establish constraints between last and origin
- establishConstraints();
-
- // Reset the derived Tracker
- reset();
-
- // Reset this
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
-
-}
+ // We do not create a KF
+ // Advance this
+ last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+ last_ptr_->remove();
+ incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
+ // Update pointers
+ advanceDerived();
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
+ }
+ break;
+ }
+ default :
+ break;
}
- postProcess();
- //std::cout << "-----End of process():" << std::endl;
+ postProcess();
}
-bool ProcessorTracker::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
+void ProcessorTracker::computeProcessingStep()
{
- WOLF_DEBUG( "PT: KF" , _keyframe_ptr->id() , " callback received at ts= " , _keyframe_ptr->getTimeStamp().get() );
-
- assert((last_ptr_ == nullptr || last_ptr_->getFramePtr() != nullptr) && "ProcessorTracker::keyFrameCallback: last_ptr_ must have a frame always");
-
- Scalar time_tol = std::min(time_tolerance_, _time_tol_other);
+ // First determine the processing phase by checking the status of the tracker pointers
+ enum {FIRST_TIME, SECOND_TIME, RUNNING} step;
+ if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
+ step = FIRST_TIME;
+ else if (origin_ptr_ == last_ptr_)
+ step = SECOND_TIME;
+ else
+ step = RUNNING;
- // Nothing to do if:
- // - there is no last
- // - last frame is already a key frame
- // - last frame is too far in time from keyframe
- if (last_ptr_ == nullptr || last_ptr_->getFramePtr()->isKey() || std::abs(last_ptr_->getTimeStamp() - _keyframe_ptr->getTimeStamp()) > time_tol)
+ // Then combine with the existence (or not) of a keyframe callback pack
+ switch (step)
{
- WOLF_DEBUG( " --> nothing done" );
- return false;
- }
-
- WOLF_DEBUG( " --> appended last capture" );
- //std::cout << "ProcessorTracker::keyFrameCallback in sensor " << getSensorPtr()->id() << std::endl;
+ case FIRST_TIME :
- // Capture last_ is added to the new keyframe
- FrameBasePtr last_old_frame = last_ptr_->getFramePtr();
- last_old_frame->unlinkCapture(last_ptr_);
- last_old_frame->remove();
- _keyframe_ptr->addCapture(last_ptr_);
+ if (kf_pack_buffer_.selectPack(incoming_ptr_, time_tolerance_))
+ processing_step_ = FIRST_TIME_WITH_PACK;
+ else // ! last && ! pack(incoming)
+ processing_step_ = FIRST_TIME_WITHOUT_PACK;
+ break;
- // Detect new Features, initialize Landmarks, create Constraints, ...
- processNew(max_new_features_);
+ case SECOND_TIME :
- // Establish constraints between last and origin
- establishConstraints();
-
- // Set ready to go to 2nd case in process()
- origin_ptr_ = nullptr;
+ if (kf_pack_buffer_.selectPack(last_ptr_, time_tolerance_))
+ processing_step_ = SECOND_TIME_WITH_PACK;
+ else
+ processing_step_ = SECOND_TIME_WITHOUT_PACK;
+ break;
- return true;
+ case RUNNING :
+ default :
+ if (kf_pack_buffer_.selectPack(last_ptr_, time_tolerance_))
+ {
+ if (last_ptr_->getFramePtr()->isKey())
+ {
+ WOLF_WARN("||*||");
+ WOLF_INFO(" ... It seems you missed something!");
+ WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
+ WOLF_INFO("Check the following for correctness:");
+ WOLF_INFO(" - You have all processors installed before starting receiving any data");
+ WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed ---> ", (getProblem()->priorIsSet() ? "OK" : "NOK"));
+ WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
+ WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances).");
+ }
+ processing_step_ = RUNNING_WITH_PACK;
+ }
+ else
+ processing_step_ = RUNNING_WITHOUT_PACK;
+ break;
+ }
}
-void ProcessorTracker::setKeyFrame(CaptureBasePtr _capture_ptr)
-{
- assert(_capture_ptr != nullptr && _capture_ptr->getFramePtr() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame");
- if (!_capture_ptr->getFramePtr()->isKey())
- {
- // Set key
- _capture_ptr->getFramePtr()->setKey();
- // Set state to the keyframe
- _capture_ptr->getFramePtr()->setState(getProblem()->getState(_capture_ptr->getTimeStamp()));
- // Call the new keyframe callback in order to let the other processors to establish their constraints
- getProblem()->keyFrameCallback(_capture_ptr->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), time_tolerance_);
- }
-}
} // namespace wolf
diff --git a/src/processor_tracker.h b/src/processor_tracker.h
index fb1029d72456666131130f25a9f7665153def6e6..08304472ab9c433f89a27d064d17aeddd47c355b 100644
--- a/src/processor_tracker.h
+++ b/src/processor_tracker.h
@@ -67,7 +67,18 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
*/
class ProcessorTracker : public ProcessorBase
{
+ public:
+ typedef enum {
+ FIRST_TIME_WITH_PACK,
+ FIRST_TIME_WITHOUT_PACK,
+ SECOND_TIME_WITH_PACK,
+ SECOND_TIME_WITHOUT_PACK,
+ RUNNING_WITH_PACK,
+ RUNNING_WITHOUT_PACK
+ } ProcessingStep ;
+
protected:
+ ProcessingStep processing_step_; ///< State machine controlling the processing step
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
@@ -89,7 +100,12 @@ class ProcessorTracker : public ProcessorBase
void setMaxNewFeatures(const unsigned int& _max_new_features);
unsigned int getMaxNewFeatures();
- virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
+// virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
+
+ bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
+ bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
+ bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
+ bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
virtual CaptureBasePtr getOriginPtr();
virtual CaptureBasePtr getLastPtr();
@@ -159,7 +175,7 @@ class ProcessorTracker : public ProcessorBase
* Call this when the tracking and keyframe policy work is done and
* we need to get ready to accept a new incoming Capture.
*/
- virtual void advance() = 0;
+ virtual void advanceDerived() = 0;
/**\brief Process new Features or Landmarks
*
@@ -171,13 +187,9 @@ class ProcessorTracker : public ProcessorBase
*/
virtual void establishConstraints()=0;
- /**\brief set key Frame to the provided Capture's frame
- */
- virtual void setKeyFrame(CaptureBasePtr _capture_ptr);
-
/** \brief Reset the tracker using the \b last Capture as the new \b origin.
*/
- virtual void reset() = 0;
+ virtual void resetDerived() = 0;
public:
@@ -185,11 +197,14 @@ class ProcessorTracker : public ProcessorBase
protected:
+ void computeProcessingStep();
+
void addNewFeatureLast(FeatureBasePtr _feature_ptr);
FeatureBaseList& getNewFeaturesListIncoming();
void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
+
};
inline void ProcessorTracker::setMaxNewFeatures(const unsigned int& _max_new_features)
@@ -217,6 +232,26 @@ inline FeatureBaseList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
+inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
+{
+ return (std::fabs(_ts2 - _ts2) < time_tolerance_);
+}
+
+inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
+{
+ return checkTimeTolerance(_cap->getTimeStamp(), _ts);
+}
+
+inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts)
+{
+ return checkTimeTolerance(_frm->getTimeStamp(), _ts);
+}
+
+inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap)
+{
+ return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
+}
+
inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index c81828b0798450cdd72f57ae8f282802f86f40af..e7da3b55749c25a00a0027772f6483d659d8a9f7 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -19,11 +19,45 @@ ProcessorTrackerFeature::~ProcessorTrackerFeature()
{
}
-unsigned int ProcessorTrackerFeature::processKnown()
+unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_features)
{
+ /* Rationale: A keyFrame will be created using the last Capture.
+ * First, we work on the last Capture to detect new Features,
+ * When done, we need to track these new Features to the incoming Capture.
+ * At the end, all new Features are appended to the lists of known Features in
+ * the last and incoming Captures.
+ */
+
+ // Populate the last Capture with new Features. The result is in new_features_last_.
+ unsigned int n = detectNewFeatures(_max_new_features);
+ for (auto ftr : new_features_last_)
+ {
+ ftr->setTrackId( ftr->id() );
+ WOLF_DEBUG("Det track: ", ftr->trackId(), ", last: ", ftr->id());
+ }
-// std::cout << "ProcessorTrackerFeature::processKnown()" << std::endl;
+ // Track new features from last to incoming. This will append new correspondences to matches_last_incoming
+ trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
+ for (auto ftr : new_features_incoming_)
+ {
+ ftr->setTrackId(matches_last_from_incoming_[ftr]->feature_ptr_->trackId());
+
+ // Print new tracked features
+ WOLF_DEBUG("New track: ", ftr->trackId(), ", last: ", matches_last_from_incoming_[ftr]->feature_ptr_->id(), ", inc: ", ftr->id());
+ }
+ // Append all new Features to the incoming Captures' list of Features
+ incoming_ptr_->addFeatureList(new_features_incoming_);
+
+ // Append all new Features to the last Captures' list of Features
+ last_ptr_->addFeatureList(new_features_last_);
+
+ // return the number of new features detected in \b last
+ return n;
+}
+
+unsigned int ProcessorTrackerFeature::processKnown()
+{
assert(incoming_ptr_->getFeatureList().size() == 0
&& "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
assert(matches_last_from_incoming_.size() == 0
@@ -31,8 +65,8 @@ unsigned int ProcessorTrackerFeature::processKnown()
// Track features from last_ptr_ to incoming_ptr_
trackFeatures(last_ptr_->getFeatureList(), known_features_incoming_, matches_last_from_incoming_);
-
-// std::cout << "Tracked: " << known_features_incoming_.size() << std::endl;
+ for (auto match : matches_last_from_incoming_)
+ match.first->setTrackId( match.second->feature_ptr_->trackId() );
// Check/correct incoming-origin correspondences
if (origin_ptr_ != nullptr)
@@ -53,16 +87,21 @@ unsigned int ProcessorTrackerFeature::processKnown()
known_incoming_feature_it++;
}
}
- // Append not destructed incoming features -> this empties known_features_incoming_
+
incoming_ptr_->addFeatureList(known_features_incoming_);
- std::cout << "Added to incoming features: " << incoming_ptr_->getFeatureList().size() << std::endl;
+ known_features_incoming_.clear();
+
+ // Print resulting list of matches
+ for (auto match : matches_last_from_incoming_)
+ {
+ WOLF_DEBUG("Known track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
+ }
return matches_last_from_incoming_.size();
}
-void ProcessorTrackerFeature::advance()
+void ProcessorTrackerFeature::advanceDerived()
{
- // std::cout << "ProcessorTrackerFeature::advance()" << std::endl;
// Compose correspondences to get origin_from_incoming
for (auto match : matches_last_from_incoming_)
{
@@ -70,53 +109,32 @@ void ProcessorTrackerFeature::advance()
matches_origin_from_last_[matches_last_from_incoming_[match.first]->feature_ptr_];
}
matches_origin_from_last_ = std::move(matches_last_from_incoming_);
- // std::cout << "advanced correspondences: " << std::endl;
- // std::cout << "\tincoming 2 last: " << matches_last_from_incoming_.size() << std::endl;
- // std::cout << "\tlast 2 origin: " << std::endl;
- // for (auto match : matches_origin_from_last_)
- // std::cout << "\t\t" << match.first->getMeasurement() << " to " << match.second.feature_ptr_->getMeasurement() << std::endl;
+
+ // We set problem here because we could not determine Problem from incoming capture at the time of adding the features to incoming's feature list.
+ for (auto ftr : incoming_ptr_->getFeatureList())
+ ftr->setProblem(getProblem());
+
+ // Print resulting list
+ for (auto match: matches_origin_from_last_)
+ {
+ WOLF_DEBUG("Matches advanced: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
+ }
}
-void ProcessorTrackerFeature::reset()
+void ProcessorTrackerFeature::resetDerived()
{
- // std::cout << "ProcessorTrackerFeature::reset()" << std::endl;
// We also reset here the list of correspondences, which passes from last--incoming to origin--last.
matches_origin_from_last_ = std::move(matches_last_from_incoming_);
-}
-
-unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_features)
-{
- // std::cout << "ProcessorTrackerFeature::processNew()" << std::endl;
-
- /* Rationale: A keyFrame will be created using the last Capture.
- * First, we create the constraints from the existing Features in last,
- * because the ones that we want to detect later on will not be constrained by anyone yet.
- * Then, we work on this last Capture to detect new Features,
- * When done, we need to track these new Features to the incoming Capture.
- * At the end, all new Features are appended to the lists of known Features in
- * the last and incoming Captures.
- */
+ // Update features according to the move above.
+ for (auto match: matches_origin_from_last_)
+ match.first->setProblem(getProblem()); // Since these features were in incoming_, they had no Problem assigned.
- // Populate the last Capture with new Features. The result is in new_features_last_.
- unsigned int n = detectNewFeatures(_max_new_features);
-
- // std::cout << "detected " << n << " new features!" << std::endl;
-
- // Track new features from last to incoming. This will append new correspondences to matches_last_incoming
- if (incoming_ptr_ != last_ptr_ && incoming_ptr_ != nullptr) // we do not do it the first time.
+ // Print resulting list
+ for (auto match: matches_origin_from_last_)
{
- trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
-
- // Append all new Features to the incoming Captures' list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
+ WOLF_DEBUG("Matches reset: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
}
-
- // Append all new Features to the last Captures' list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
- // return the number of new features detected in \b last
- return n;
}
void ProcessorTrackerFeature::establishConstraints()
@@ -127,6 +145,12 @@ void ProcessorTrackerFeature::establishConstraints()
match.first->addConstraint(ctr);
match.second->feature_ptr_->addConstrainedBy(ctr);
}
+ for (auto match : matches_origin_from_last_)
+ {
+ WOLF_DEBUG( "Constraint: track: " , match.second->feature_ptr_->trackId() ,
+ " origin: " , match.second->feature_ptr_->id() ,
+ " from last: " , match.first->id() );
+ }
}
} // namespace wolf
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index 7e1d625a56a77ccb4210bed0a85bee99bfeb9d44..82253eefcd6291d9233dc9025de89ecc236a7d6b 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -128,8 +128,8 @@ class ProcessorTrackerFeature : public ProcessorTracker
virtual bool voteForKeyFrame() = 0;
// We overload the advance and reset functions to update the lists of matches
- void advance();
- void reset();
+ void advanceDerived();
+ void resetDerived();
/**\brief Process new Features
*
@@ -137,14 +137,12 @@ class ProcessorTrackerFeature : public ProcessorTracker
virtual unsigned int processNew(const unsigned int& _max_features);
/** \brief Detect new Features
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
+ * \param _max_features maximum number of features detected
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
+ * The function sets the member new_features_last_, the list of newly detected features.
*/
virtual unsigned int detectNewFeatures(const unsigned int& _max_features) = 0;
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index 9e9ae49f8bfa7fc89b35a61dc4435f3e3e4c6f1d..33e670d69e048bf3ceae06a751656f7866c1de45 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/src/processor_tracker_feature_corner.cpp
@@ -63,9 +63,9 @@ void ProcessorTrackerFeatureCorner::preProcess()
t_current_prev_ = R_world_sensor_.transpose() * (t_world_sensor_prev_ - t_world_sensor_);
}
-void ProcessorTrackerFeatureCorner::advance()
+void ProcessorTrackerFeatureCorner::advanceDerived()
{
- ProcessorTrackerFeature::advance();
+ ProcessorTrackerFeature::advanceDerived();
corners_last_ = std::move(corners_incoming_);
}
diff --git a/src/processor_tracker_feature_corner.h b/src/processor_tracker_feature_corner.h
index d80c6572f04e92603a2874880481ce9295a1143e..7fe63a9ef664db4b5f57f5aa8fbd0beef0d3f4d0 100644
--- a/src/processor_tracker_feature_corner.h
+++ b/src/processor_tracker_feature_corner.h
@@ -72,7 +72,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
virtual void preProcess();
- void advance();
+ void advanceDerived();
/** \brief Track provided features from \b last to \b incoming
* \param _feature_list_in input list of features in \b last to track
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index 4181a7814339c9effb00e8ce509c1c15196ff45a..c25ac9b4b7024828e34d0548f12c030ee2a0fcb2 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -11,26 +11,27 @@
namespace wolf
{
+unsigned int ProcessorTrackerFeatureDummy::count_ = 0;
+
unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _feature_list_in,
FeatureBaseList& _feature_list_out,
FeatureMatchMap& _feature_correspondences)
{
- std::cout << "tracking " << _feature_list_in.size() << " features..." << std::endl;
+ WOLF_INFO("tracking " , _feature_list_in.size() , " features...");
- // loosing the track of the first 2 features
- auto features_lost = 0;
- for (auto feat_in_ptr : _feature_list_in)
+ for (auto feat_in : _feature_list_in)
{
- if (features_lost < 2)
+ if (++count_ % 3 == 2) // lose one every 3 tracks
{
- features_lost++;
- std::cout << "feature " << feat_in_ptr->getMeasurement() << " lost!" << std::endl;
+ WOLF_INFO("track: " , feat_in->trackId() , " feature: " , feat_in->id() , " lost!");
}
else
{
- _feature_list_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance()));
- _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
- std::cout << "feature " << feat_in_ptr->getMeasurement() << " tracked!" << std::endl;
+ FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in->getMeasurement(), feat_in->getMeasurementCovariance()));
+ _feature_list_out.push_back(ftr);
+ _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+
+ WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
}
}
@@ -39,26 +40,32 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
{
- std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
+ WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() );
+
bool vote = incoming_ptr_->getFeatureList().size() < min_feat_for_keyframe_;
- std::cout << (vote ? "Vote ": "Not vote ") << "for KF" << std::endl;
+
+ WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
return incoming_ptr_->getFeatureList().size() < min_feat_for_keyframe_;
}
unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features)
{
- std::cout << "Detecting " << _max_features << " new features..." << std::endl;
+ WOLF_INFO("Detecting " , _max_features , " new features..." );
// detecting new features
for (unsigned int i = 1; i <= _max_features; i++)
{
n_feature_++;
- new_features_last_.push_back(
- std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
- //std::cout << "feature " << new_features_last_.back()->getMeasurement() << " detected!" << std::endl;
+ FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE",
+ n_feature_* Eigen::Vector1s::Ones(),
+ Eigen::MatrixXs::Ones(1, 1)));
+ new_features_last_.push_back(ftr);
+
+ WOLF_INFO("feature " , ftr->id() , " detected!" );
}
- std::cout << new_features_last_.size() << " features detected!" << std::endl;
+
+ WOLF_INFO(new_features_last_.size() , " features detected!");
return new_features_last_.size();
}
@@ -66,11 +73,11 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- // std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
- // << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl;
+ WOLF_INFO( "creating constraint: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
+ , " with origin feature " , _feature_other_ptr->id() );
+
auto ctr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
- // _feature_ptr->addConstraint(ctr);
- // _feature_other_ptr->addConstrainedBy(ctr);
+
return ctr;
}
diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h
index 1cb423f364a09b7834c171db727594db3f32444f..6fa449695f09a56fa93932dddf4d359e29000369 100644
--- a/src/processor_tracker_feature_dummy.h
+++ b/src/processor_tracker_feature_dummy.h
@@ -27,6 +27,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
protected:
+ static unsigned int count_;
unsigned int n_feature_, min_feat_for_keyframe_;
// virtual void preProcess() { }
diff --git a/src/processor_tracker_landmark.cpp b/src/processor_tracker_landmark.cpp
index 86fa4298b34674a4217a64ddd49cf789ecdeb27a..9c06036595520a7c30fb65fcb1b0a91f26310ba3 100644
--- a/src/processor_tracker_landmark.cpp
+++ b/src/processor_tracker_landmark.cpp
@@ -33,7 +33,7 @@ ProcessorTrackerLandmark::~ProcessorTrackerLandmark()
// }
}
-void ProcessorTrackerLandmark::advance()
+void ProcessorTrackerLandmark::advanceDerived()
{
for (auto match : matches_landmark_from_last_)
{
@@ -45,7 +45,7 @@ void ProcessorTrackerLandmark::advance()
// std::cout << "\t" << match.first->id() << " to " << match.second->landmark_ptr_->id() << std::endl;
}
-void ProcessorTrackerLandmark::reset()
+void ProcessorTrackerLandmark::resetDerived()
{
//std::cout << "ProcessorTrackerLandmark::reset" << std::endl;
for (auto match : matches_landmark_from_last_)
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index a9fbd87a52c48699bee65320b4f15b1217ac0f95..44db01813ba1002ded7dc1309f10a3bb964164c8 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -120,8 +120,8 @@ class ProcessorTrackerLandmark : public ProcessorTracker
virtual bool voteForKeyFrame() = 0;
// We overload the advance and reset functions to update the lists of matches
- void advance();
- void reset();
+ void advanceDerived();
+ void resetDerived();
/** \brief Process new Features
*
diff --git a/src/processor_tracker_landmark_corner.h b/src/processor_tracker_landmark_corner.h
index 29f711137aa9ae678b2f917e11650fc9aa118815..734acc5083260b65e1933d897d7fb1214f9ae075 100644
--- a/src/processor_tracker_landmark_corner.h
+++ b/src/processor_tracker_landmark_corner.h
@@ -96,12 +96,12 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
virtual void preProcess();
// virtual void postProcess() { }
- void advance()
+ void advanceDerived()
{
//std::cout << "\tProcessorTrackerLandmarkCorner::advance:" << std::endl;
//std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
//std::cout << "\t\tcorners_incoming_: " << corners_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::advance();
+ ProcessorTrackerLandmark::advanceDerived();
while (!corners_last_.empty())
{
corners_last_.front()->remove();
@@ -110,12 +110,12 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
corners_last_ = std::move(corners_incoming_);
}
- void reset()
+ void resetDerived()
{
//std::cout << "\tProcessorTrackerLandmarkCorner::reset:" << std::endl;
//std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
//std::cout << "\t\tcorners_incoming_: " << corners_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::reset();
+ ProcessorTrackerLandmark::resetDerived();
corners_last_ = std::move(corners_incoming_);
}
diff --git a/src/processor_tracker_landmark_polyline.h b/src/processor_tracker_landmark_polyline.h
index 11d74666ab0310ae4bfb6158a299fd7b52e1f213..0f5c58045241f922147feaa03e367e0b145546b9 100644
--- a/src/processor_tracker_landmark_polyline.h
+++ b/src/processor_tracker_landmark_polyline.h
@@ -126,9 +126,9 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
void computeTransformations(const TimeStamp& _ts);
virtual void postProcess();
- void advance();
+ void advanceDerived();
- void reset();
+ void resetDerived();
/** \brief Find provided landmarks in the incoming capture
* \param _landmark_list_in input list of landmarks to be found in incoming
@@ -219,24 +219,24 @@ inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const un
return new_features_last_.size();
}
-inline void ProcessorTrackerLandmarkPolyline::advance()
+inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
{
//std::cout << "\tProcessorTrackerLandmarkPolyline::advance:" << std::endl;
//std::cout << "\t\tcorners_last: " << polylines_last_.size() << std::endl;
//std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::advance();
+ ProcessorTrackerLandmark::advanceDerived();
for (auto polyline : polylines_last_)
polyline->remove();
polylines_last_ = std::move(polylines_incoming_);
//std::cout << "advanced" << std::endl;
}
-inline void ProcessorTrackerLandmarkPolyline::reset()
+inline void ProcessorTrackerLandmarkPolyline::resetDerived()
{
//std::cout << "\tProcessorTrackerLandmarkPolyline::reset:" << std::endl;
//std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
//std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::reset();
+ ProcessorTrackerLandmark::resetDerived();
polylines_last_ = std::move(polylines_incoming_);
}
diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp
index 83a16f77ac838af7bf943b223e4cfdaf71111677..43eb7fed018937d7eae6228e92b5bc1a4558fc6a 100644
--- a/src/processors/processor_diff_drive.cpp
+++ b/src/processors/processor_diff_drive.cpp
@@ -242,10 +242,10 @@ FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motio
return key_feature_ptr;
}
-bool ProcessorDiffDrive::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol)
-{
- return ProcessorMotion::keyFrameCallback(_keyframe_ptr, _time_tol);
-}
+//bool ProcessorDiffDrive::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol)
+//{
+// return ProcessorMotion::keyFrameCallback(_keyframe_ptr, _time_tol);
+//}
ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params,
diff --git a/src/processors/processor_diff_drive.h b/src/processors/processor_diff_drive.h
index 064725fdfa025a7625498daac699c5c52b6bfc82..030cb9516036bfbae7fd22c2a79073c3a3400146 100644
--- a/src/processors/processor_diff_drive.h
+++ b/src/processors/processor_diff_drive.h
@@ -110,7 +110,7 @@ protected:
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
- bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol) override;
+// bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol) override;
public:
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 4887a088ca3b0ff55249682597f81109d5fb4bf2..83578afef1d86452eeac5473b397ae87403617b4 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -218,6 +218,23 @@ void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
noise_cov_ = _noise_cov;
}
+CaptureBasePtr SensorBase::lastCapture(void)
+{
+ // we search for the most recent Capture of this sensor
+ CaptureBasePtr capture = nullptr;
+ FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
+ FrameBaseRevIter frame_rev_it = frame_list.rbegin();
+ while (frame_rev_it != frame_list.rend())
+ {
+ CaptureBasePtr capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
+ if (capture)
+ // found the most recent Capture made by this sensor !
+ break;
+ frame_rev_it++;
+ }
+ return capture;
+}
+
CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
{
// we search for the most recent Capture of this sensor before _ts
@@ -232,9 +249,8 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
if (capture)
// found the most recent Capture made by this sensor !
break;
-
- frame_rev_it++;
}
+ frame_rev_it++;
}
return capture;
}
@@ -256,44 +272,17 @@ StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
StateBlockPtr SensorBase::getPPtr()
{
- ProblemPtr P = getProblem();
- if (P)
- {
- FrameBasePtr KF = P->getLastKeyFramePtr();
- if (KF)
- {
- return getPPtr(KF->getTimeStamp());
- }
- }
- return state_block_vec_[0];
+ return getStateBlockPtrDynamic(0);
}
StateBlockPtr SensorBase::getOPtr()
{
- ProblemPtr P = getProblem();
- if (P)
- {
- FrameBasePtr KF = P->getLastKeyFramePtr();
- if (KF)
- {
- return getOPtr(KF->getTimeStamp());
- }
- }
- return state_block_vec_[1];
+ return getStateBlockPtrDynamic(1);
}
StateBlockPtr SensorBase::getIntrinsicPtr()
{
- ProblemPtr P = getProblem();
- if (P)
- {
- FrameBasePtr KF = P->getLastKeyFramePtr();
- if (KF)
- {
- return getIntrinsicPtr(KF->getTimeStamp());
- }
- }
- return state_block_vec_[2];
+ return getStateBlockPtrDynamic(2);
}
wolf::Size SensorBase::computeCalibSize() const
@@ -347,30 +336,32 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
return _proc_ptr;
}
-StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i)
{
- assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- if (((_i < 2) && extrinsicsInCaptures()) || ((_i >= 2) && intrinsicsInCaptures()))
+ if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
- CaptureBasePtr cap = lastCapture(_ts);
- return cap->getStateBlockPtr(_i);
+ CaptureBasePtr cap = lastCapture();
+ if (cap)
+ return cap->getStateBlockPtr(_i);
+ else
+ return getStateBlockPtrStatic(_i);
}
else
- return state_block_vec_[_i];
+ return getStateBlockPtrStatic(_i);
}
-StateBlockPtr SensorBase::getStateBlockPtrAuto(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts)
{
- ProblemPtr P = getProblem();
- if (P)
+ if ((_i<2 && this->extrinsicsInCaptures()) || (_i>=2 && intrinsicsInCaptures()))
{
- FrameBasePtr KF = P->getLastKeyFramePtr();
- if (KF)
- {
- return getStateBlockPtrDynamic(_i, KF->getTimeStamp());
- }
+ CaptureBasePtr cap = lastCapture(_ts);
+ if (cap)
+ return cap->getStateBlockPtr(_i);
+ else
+ return getStateBlockPtrStatic(_i);
}
- return state_block_vec_[_i];
+ else
+ return getStateBlockPtrStatic(_i);
}
} // namespace wolf
diff --git a/src/sensor_base.h b/src/sensor_base.h
index ba16f298480705f209e9ceda483854e946e630cf..8c6b6c168d96023e12f80f819bd467aa1aa7217a 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -102,6 +102,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
ProcessorBaseList& getProcessorList();
+ CaptureBasePtr lastCapture(void);
CaptureBasePtr lastCapture(const TimeStamp& _ts);
bool process(const CaptureBasePtr capture_ptr);
@@ -110,8 +111,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
+ StateBlockPtr getStateBlockPtrDynamic(unsigned int _i);
StateBlockPtr getStateBlockPtrDynamic(unsigned int _i, const TimeStamp& _ts);
- StateBlockPtr getStateBlockPtrAuto(unsigned int _i);
void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(int _size);
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 738bccd2056638606f1d1a69a94e6978f49f4756..27f02386aaa0f0a9489573f33b7b34a457882546 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -64,6 +64,10 @@ target_link_libraries(gtest_frame_base ${PROJECT_NAME})
wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
# target_link_libraries(gtest_imu_tools ${PROJECT_NAME}) // WOLF library not needed
+# IMU tools test
+wolf_add_gtest(gtest_KF_pack_buffer gtest_KF_pack_buffer.cpp)
+target_link_libraries(gtest_KF_pack_buffer ${PROJECT_NAME})
+
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
target_link_libraries(gtest_local_param ${PROJECT_NAME})
@@ -76,6 +80,10 @@ target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
wolf_add_gtest(gtest_problem gtest_problem.cpp)
target_link_libraries(gtest_problem ${PROJECT_NAME})
+# ProcessorBase class test
+wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
+target_link_libraries(gtest_processor_base ${PROJECT_NAME})
+
# ProcessorMotion class test
wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp
index 05eb3b5b564ff51bf77f7676478e88fdd914f41f..d126a7a23f659587939472b5201f35f018375f96 100644
--- a/src/test/gtest_IMU.cpp
+++ b/src/test/gtest_IMU.cpp
@@ -95,6 +95,9 @@ class Process_Constraint_IMU : public testing::Test
sensor_imu = static_pointer_cast<SensorIMU> (sensor);
processor_imu = static_pointer_cast<ProcessorIMU>(processor);
+ dt = 0.01;
+ processor_imu->setTimeTolerance(dt/2);
+
// Some initializations
bias_null .setZero();
x0 .resize(10);
@@ -277,7 +280,7 @@ class Process_Constraint_IMU : public testing::Test
DT = num_integrations * dt;
// wolf objects
- KF_0 = problem->setPrior(x0, P0, t0);
+ KF_0 = problem->setPrior(x0, P0, t0, dt/2);
C_0 = processor_imu->getOriginPtr();
processor_imu->getLastPtr()->setCalibrationPreint(bias_preint);
@@ -374,7 +377,14 @@ class Process_Constraint_IMU : public testing::Test
FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t);
// ===================================== IMU CALLBACK
- processor_imu->keyFrameCallback(KF, 0.01);
+ processor_imu->keyFrameCallback(KF, dt/2);
+
+
+
+
+ data = Vector6s::Zero();
+ capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
+ processor_imu->process(capture_imu);
KF_1 = problem->getLastKeyFramePtr();
C_1 = KF_1->getCaptureList().front(); // front is IMU
@@ -483,6 +493,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
// Wolf objects
SensorOdom3DPtr sensor_odo;
ProcessorOdom3DPtr processor_odo;
+ CaptureOdom3DPtr capture_odo;
virtual void SetUp( ) override
{
@@ -497,6 +508,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/src/examples/processor_odom_3D.yaml");
sensor_odo = static_pointer_cast<SensorOdom3D>(sensor);
processor_odo = static_pointer_cast<ProcessorOdom3D>(processor);
+ processor_odo->setTimeTolerance(dt/2);
// prevent this processor from voting by setting high thresholds :
processor_odo->setAngleTurned(2.0);
@@ -530,6 +542,11 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
// ===================================== ODO
processor_odo->keyFrameCallback(KF_1, 0.1);
+
+ data = Vector6s::Zero();
+ capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
+ processor_odo->process(capture_odo);
+
}
};
@@ -543,7 +560,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -589,7 +605,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -638,7 +653,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -685,7 +699,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -732,7 +745,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -779,7 +791,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -825,7 +836,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -871,7 +881,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -917,7 +926,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -963,7 +971,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -1009,7 +1016,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -1040,7 +1046,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe
// ===================================== RUN ALL
string report = runAll(1);
- // printAll(report);
+ // printAll(report);
assertAll();
@@ -1055,7 +1061,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -1101,7 +1106,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -1147,7 +1151,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -1193,7 +1196,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -1239,7 +1241,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
@@ -1285,7 +1286,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
//
// ---------- time
t0 = 0;
- dt = 0.01;
num_integrations = 50;
// ---------- initial pose
diff --git a/src/test/gtest_KF_pack_buffer.cpp b/src/test/gtest_KF_pack_buffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb325fd84be395d3dc5fe7cb0e926e31b71e9973
--- /dev/null
+++ b/src/test/gtest_KF_pack_buffer.cpp
@@ -0,0 +1,244 @@
+/*
+ * gtest_KF_pack_buffer.cpp
+ *
+ * Created on: Mar 5, 2018
+ * Author: jsola
+ */
+//Wolf
+#include "utils_gtest.h"
+
+#include "processor_odom_2D.h"
+#include "sensor_odom_2D.h"
+
+#include "processor_tracker_feature_dummy.h"
+#include "capture_void.h"
+
+#include "problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class KFPackBufferTest : public testing::Test
+{
+ public:
+
+ KFPackBuffer kfpackbuffer;
+ FrameBasePtr f10, f20, f21, f28;
+ Scalar tt10, tt20, tt21, tt28;
+ TimeStamp timestamp;
+ Scalar timetolerance;
+
+ void SetUp(void)
+ {
+ f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
+ f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
+ f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
+ f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
+
+ tt10 = 1.0;
+ tt20 = 1.0;
+ tt21 = 1.0;
+ tt28 = 1.0;
+ };
+};
+
+TEST_F(KFPackBufferTest, empty)
+{
+ ASSERT_TRUE(kfpackbuffer.empty());
+}
+
+TEST_F(KFPackBufferTest, add)
+{
+ kfpackbuffer.add(f10, tt10);
+ ASSERT_EQ(kfpackbuffer.size(),1);
+ kfpackbuffer.add(f20, tt20);
+ ASSERT_EQ(kfpackbuffer.size(),2);
+}
+
+TEST_F(KFPackBufferTest, clear)
+{
+ kfpackbuffer.add(f10, tt10);
+ kfpackbuffer.add(f20, tt20);
+ ASSERT_EQ(kfpackbuffer.size(),2);
+ kfpackbuffer.clear();
+ ASSERT_TRUE(kfpackbuffer.empty());
+}
+
+//TEST_F(KFPackBufferTest, print)
+//{
+// kfpackbuffer.clear();
+// kfpackbuffer.add(f10, tt10);
+// kfpackbuffer.add(f20, tt20);
+// kfpackbuffer.print();
+//}
+
+TEST_F(KFPackBufferTest, checkTimeTolerance)
+{
+ kfpackbuffer.clear();
+ kfpackbuffer.add(f10, tt10);
+ kfpackbuffer.add(f20, tt20);
+ // min time tolerance > diff between time stamps. It should return true
+ ASSERT_TRUE(kfpackbuffer.checkTimeTolerance(10, 20, 20, 20));
+ // min time tolerance < diff between time stamps. It should return true
+ ASSERT_FALSE(kfpackbuffer.checkTimeTolerance(10, 1, 20, 20));
+}
+
+TEST_F(KFPackBufferTest, selectPack)
+{
+ // Evaluation using two packs (p1,p2)
+ // with different time tolerances (tp1,tp2)
+ // using a query pack (q) with also different time tolerances
+ // depending on these tolerances we will get one (p1) or the other (p2)
+ // packages from the buffer (res).
+ // This can be summarized in the table hereafter:
+ //
+ // p1 p2 q | resulting pack time stamp
+ // --------------------------------
+ // 2 2 2 | nullptr
+ // 2 2 5 | nullptr
+ // 2 2 7 | nullptr
+ // 2 7 2 | nullptr
+ // 2 7 5 | 20
+ // 2 7 7 | 20
+ // 7 2 2 | nullptr
+ // 7 2 5 | nullptr
+ // 7 2 7 | 10
+ // 7 7 2 | nullptr
+ // 7 7 5 | 20
+ // 7 7 7 | 20
+
+ kfpackbuffer.clear();
+
+ // input packages
+ std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances
+ std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances
+ std::vector<int> q = {2, 5, 7}; // Query pack time tolerances
+
+ // Solution matrix
+ Eigen::VectorXi res = Eigen::VectorXi::Zero(12);
+ res(4) = 20;
+ res(5) = 20;
+ res(8) = 10;
+ res(10) = 20;
+ res(11) = 20;
+
+ // test
+ for (auto ip1=0;ip1<p1.size();++ip1)
+ {
+ for (auto ip2=0;ip2<p2.size();++ip2)
+ {
+ kfpackbuffer.add(f10, p1[ip1]);
+ kfpackbuffer.add(f20, p2[ip2]);
+ for (auto iq=0;iq<q.size();++iq)
+ {
+ KFPackPtr packQ = kfpackbuffer.selectPack(16, q[iq]);
+ if (packQ!=nullptr)
+ ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+ }
+ kfpackbuffer.clear();
+ }
+ }
+}
+
+TEST_F(KFPackBufferTest, selectPackBefore)
+{
+ kfpackbuffer.clear();
+
+ kfpackbuffer.add(f10, tt10);
+ kfpackbuffer.add(f20, tt20);
+ kfpackbuffer.add(f21, tt21);
+
+ // input time stamps
+ std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
+ Scalar tt = 0.01;
+
+ // Solution matrix
+ // q_ts | pack
+ //=================
+ // first time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 10
+ // 10,005 10
+ // 19.5 10
+ // 20.5 10
+ // 21.5 10
+ // second time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 null
+ // 10,005 null
+ // 19.5 null
+ // 20.5 20
+ // 21.5 20
+ // third time
+ //-----------------
+ // 9.5 nullptr
+ // 9.995 null
+ // 10,005 null
+ // 19.5 null
+ // 20.5 null
+ // 21.5 21
+
+ Eigen::MatrixXs truth(3,6), res(3,6);
+ truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
+ res.setZero();
+
+ for (int i=0; i<3; i++)
+ {
+ KFPackPtr packQ;
+ int j = 0;
+ for (auto ts : q_ts)
+ {
+ packQ = kfpackbuffer.selectPackBefore(ts, tt);
+ if (packQ)
+ res(i,j) = packQ->key_frame->getTimeStamp().get();
+
+ j++;
+ }
+ kfpackbuffer.removeUpTo(packQ->key_frame->getTimeStamp());
+ }
+
+ ASSERT_MATRIX_APPROX(res, truth, 1e-6);
+}
+
+TEST_F(KFPackBufferTest, removeUpTo)
+{
+ // Small time tolerance for all test asserts
+ Scalar tt = 0.1;
+ kfpackbuffer.clear();
+ kfpackbuffer.add(f10, tt10);
+ kfpackbuffer.add(f20, tt20);
+ kfpackbuffer.add(f21, tt21);
+
+ // it should remove f20 and f10, thus size should be 1 after removal
+ // Specifically, only f21 should remain
+ KFPackPtr pack20 = std::make_shared<KFPack>(f20,tt20);
+ kfpackbuffer.removeUpTo( pack20->key_frame->getTimeStamp() );
+ ASSERT_EQ(kfpackbuffer.size(),1);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f10->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f20->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f21->getTimeStamp(),tt)!=nullptr);
+
+ // Chech removal of an imprecise time stamp
+ // Specifically, only f28 should remain
+ kfpackbuffer.add(f28, tt28);
+ ASSERT_EQ(kfpackbuffer.size(),2);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
+ KFPackPtr pack22 = std::make_shared<KFPack>(f22,5);
+ kfpackbuffer.removeUpTo( pack22->key_frame->getTimeStamp() );
+ ASSERT_EQ(kfpackbuffer.size(),1);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f21->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(kfpackbuffer.selectPack(f28->getTimeStamp(),tt)!=nullptr);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp
index bf0956ac15fbeb34f65fbcd227d88382637a7093..6609f97080de513b1d01ee73f749ff1685490332 100644
--- a/src/test/gtest_feature_imu.cpp
+++ b/src/test/gtest_feature_imu.cpp
@@ -52,12 +52,13 @@ class FeatureIMU_test : public testing::Test
// Set the origin
Eigen::VectorXs x0(10);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- origin_frame = problem->getProcessorMotionPtr()->setOrigin(x0, t); //create a keyframe at origin
+ MatrixXs P0; P0.setIdentity(9,9);
+ origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
- imu_ptr->setFramePtr(origin_frame); //to get ptr to Frm ni processorIMU and then get biases
+ imu_ptr->setFramePtr(origin_frame); //to get ptr to Frm in processorIMU and then get biases
//process data
data_ << 2, 0, 9.8, 0, 0, 0;
diff --git a/src/test/gtest_feature_motion.cpp b/src/test/gtest_feature_motion.cpp
deleted file mode 100644
index 1620131a6dd4b2b8957c783e5693bab3c4e07f04..0000000000000000000000000000000000000000
--- a/src/test/gtest_feature_motion.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * gtest_feature_motion.cpp
- *
- * Created on: Aug 11, 2017
- * Author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "/Users/jsola/dev/wolf/src/feature_motion.h"
-
-namespace wolf
-{
-
-TEST(DummyGroup, DummyTestExample)
-{
- // TODO: Automatically generated TEST stub
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
-} /* namespace wolf */
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index 76bd0e6db0a116bdf304bf23fcbc5dbc8c976f9a..bd101f24074cf8fc426a8551bcce85e1bd94e97a 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -127,7 +127,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
CeresManager ceres_manager(Pr);
// KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPrior(x0, P0,t0);
+ FrameBasePtr F0 = Pr->setPrior(x0, P0,t0, dt/2);
// KF1 and motion from KF0
t += dt;
@@ -210,7 +210,7 @@ TEST(Odom2D, VoteForKfAndSolve)
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0);
+ FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2);
ceres_manager.solve(0);
ceres_manager.computeCovariances(ALL_MARGINALS);
@@ -322,12 +322,13 @@ TEST(Odom2D, KF_callback)
Matrix3s unmeasured_cov = params->unmeasured_perturbation_std_*params->unmeasured_perturbation_std_*Matrix3s::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
+ processor_odom2d->setTimeTolerance(dt/2);
// Ceres wrapper
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0);
+ FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0, dt/2);
// Check covariance values
Eigen::Vector3s integrated_pose = x0;
@@ -390,7 +391,11 @@ TEST(Odom2D, KF_callback)
FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_2, 0);
+ processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
+ ASSERT_TRUE(problem->check(0));
+ t += dt;
+ capture->setTimeStamp(t);
+ processor_odom2d->process(capture);
ASSERT_TRUE(problem->check(0));
CaptureMotionPtr key_capture_n = std::static_pointer_cast<CaptureMotion>(keyframe_2->getCaptureList().front());
@@ -402,7 +407,7 @@ TEST(Odom2D, KF_callback)
ceres_manager.computeCovariances(ALL_MARGINALS);
ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6);
- ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6);
+ ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
// Split between keyframes, exact timestamp
@@ -410,11 +415,17 @@ TEST(Odom2D, KF_callback)
t_split = t0 + m_split*dt;
// std::cout << "-----------------------------\nSplit between KFs; time: " << t_split - t0 << std::endl;
+ problem->print(4,1,1,1);
+
x_split = processor_odom2d->getState(t_split);
FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split);
ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_1, 0);
+ processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
+ ASSERT_TRUE(problem->check(0));
+ t += dt;
+ capture->setTimeStamp(t);
+ processor_odom2d->process(capture);
ASSERT_TRUE(problem->check(0));
CaptureMotionPtr key_capture_m = std::static_pointer_cast<CaptureMotion>(keyframe_1->getCaptureList().front());
@@ -427,7 +438,6 @@ TEST(Odom2D, KF_callback)
keyframe_2->setState(Vector3s(3,1,2));
report = ceres_manager.solve(1);
-// std::cout << report << std::endl;
ceres_manager.computeCovariances(ALL_MARGINALS);
// check the split KF
diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp
index d2b0ef0152083a7fac41377d376e502745fd5451..0bbcf9d1872735c5bf6b41aa29405b255f7dd7eb 100644
--- a/src/test/gtest_problem.cpp
+++ b/src/test/gtest_problem.cpp
@@ -103,7 +103,7 @@ TEST(Problem, SetOrigin_PO_2D)
Eigen::VectorXs x0(3); x0 << 1,2,3;
Eigen::MatrixXs P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
- P->setPrior(x0, P0, t0);
+ P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
ASSERT_EQ(P->getHardwarePtr()->getSensorList().size(), 0);
@@ -142,7 +142,7 @@ TEST(Problem, SetOrigin_PO_3D)
Eigen::VectorXs x0(7); x0 << 1,2,3,4,5,6,7;
Eigen::MatrixXs P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
- P->setPrior(x0, P0, t0);
+ P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
ASSERT_EQ(P->getHardwarePtr()->getSensorList().size(), 0);
diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce31eeed39d927a660b33e249c76981c5c0bef50
--- /dev/null
+++ b/src/test/gtest_processor_base.cpp
@@ -0,0 +1,98 @@
+/*
+ * gtest_capture_base.cpp
+ *
+ * Created on: Feb 15, 2018
+ * Author: asantamaria
+ */
+
+//Wolf
+#include "utils_gtest.h"
+
+#include "processor_odom_2D.h"
+#include "sensor_odom_2D.h"
+
+#include "processor_tracker_feature_dummy.h"
+#include "capture_void.h"
+
+#include "problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+TEST(ProcessorBase, KeyFrameCallback)
+{
+
+ using namespace wolf;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using Eigen::Vector2s;
+
+ Scalar dt = 0.01;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO 2D");
+
+ // Install tracker (sensor and processor)
+ SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(5, 5);
+ proc_trk->setTimeTolerance(dt/2);
+
+ problem->addSensor(sens_trk);
+ sens_trk->addProcessor(proc_trk);
+
+ // Install odometer (sensor and processor)
+ SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
+ ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
+ ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odometer", sens_odo, proc_odo_params);
+ proc_odo->setTimeTolerance(dt/2);
+
+ std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+ // Sequence to test KeyFrame creations (callback calls)
+
+ // initialize
+ TimeStamp t(0.0);
+ Vector3s x(0,0,0);
+ Matrix3s P = Matrix3s::Identity() * 0.1;
+ problem->setPrior(x, P, t, dt/2); // KF1
+
+ CaptureOdom2DPtr capt_odo = make_shared<CaptureOdom2D>(t, sens_odo, Vector2s(0.5,0));
+
+ // Track
+ CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
+ proc_trk->process(capt_trk);
+
+ for (size_t ii=0; ii<10; ii++ )
+ {
+ // Move
+ t = t+dt;
+ WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+
+ capt_odo->setTimeStamp(t);
+ proc_odo->process(capt_odo);
+
+ // Track
+ capt_trk = make_shared<CaptureVoid>(t, sens_trk);
+ proc_trk->process(capt_trk);
+
+// problem->print(4,1,1,0);
+
+ // Only odom creating KFs
+ ASSERT_TRUE( problem->getLastKeyFramePtr()->getType().compare("PO 2D")==0 );
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/src/test/gtest_processor_imu.cpp b/src/test/gtest_processor_imu.cpp
index e4f0e54c3e3033fab43d0f127a03408caf856e90..a3b57eb668f9285fb7ce619873a107fc0958b4c6 100644
--- a/src/test/gtest_processor_imu.cpp
+++ b/src/test/gtest_processor_imu.cpp
@@ -29,6 +29,7 @@ class ProcessorIMUt : public testing::Test
wolf::SensorBasePtr sensor_ptr;
wolf::TimeStamp t;
wolf::Scalar mti_clock, tmp;
+ wolf::Scalar dt;
Vector6s data;
Matrix6s data_cov;
VectorXs x0;
@@ -148,7 +149,8 @@ TEST(ProcessorIMU, voteForKeyFrame)
VectorXs x0(10);
TimeStamp t(0);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- problem->getProcessorMotionPtr()->setOrigin(x0, t); //this also creates a keyframe at origin
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
//data variable and covariance matrix
// since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
@@ -211,8 +213,8 @@ TEST_F(ProcessorIMUt, interpolate)
t.set(0);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 0, 0, 0, 0; // only acc_x
cap_imu_ptr->setData(data);
@@ -257,8 +259,8 @@ TEST_F(ProcessorIMUt, acc_x)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
@@ -283,8 +285,8 @@ TEST_F(ProcessorIMUt, acc_y)
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
@@ -320,8 +322,8 @@ TEST_F(ProcessorIMUt, acc_z)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
@@ -358,8 +360,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
@@ -379,8 +381,8 @@ TEST_F(ProcessorIMUt, gyro_x)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
@@ -430,9 +432,10 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
Vector3s acc_bias = bias.head(3);
// state
x0 << 0,0,0, 0,0,0,1, 0,0,0;
+ MatrixXs P0(9,9); P0.setIdentity();
// init things
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ problem->setPrior(x0, P0, t, 0.01);
std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
@@ -484,10 +487,12 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
t.set(0); // clock in 0,1 ms ticks
wolf::Scalar abx(0.002), aby(0.01);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
+
Vector6s bias; bias << abx,aby,0, 0,0,0;
Vector3s acc_bias = bias.head(3);
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
@@ -534,8 +539,8 @@ TEST_F(ProcessorIMUt, gyro_z)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
@@ -572,8 +577,8 @@ TEST_F(ProcessorIMUt, gyro_xyz)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
Vector3s tmp_vec; //will be used to store rate of turn data
Quaternions quat_comp(Quaternions::Identity());
@@ -659,8 +664,8 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
@@ -698,8 +703,8 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
@@ -742,8 +747,8 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
@@ -786,8 +791,8 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
@@ -829,8 +834,8 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
Vector3s tmp_vec; //will be used to store rate of turn data
Quaternions quat_comp(Quaternions::Identity());
@@ -918,8 +923,8 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
@@ -967,8 +972,8 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
@@ -1016,8 +1021,8 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
wolf::Scalar dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
-
- problem->getProcessorMotionPtr()->setOrigin(x0, t);
+ MatrixXs P0(9,9); P0.setIdentity();
+ problem->setPrior(x0, P0, t, 0.01);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
diff --git a/src/test/gtest_processor_motion.cpp b/src/test/gtest_processor_motion.cpp
index 228570f147419c5a08f1a52429d053beb9f82807..6cd5e06450c0a99ab691c27081db20cd28f92cbd 100644
--- a/src/test/gtest_processor_motion.cpp
+++ b/src/test/gtest_processor_motion.cpp
@@ -44,7 +44,8 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{
capture = std::make_shared<CaptureMotion>(0.0, sensor, data, data_cov, 3, 3, nullptr);
Vector3s x0; x0 << 0, 0, 0;
- processor->setOrigin(x0, 0.0);
+ Matrix3s P0; P0.setIdentity();
+ problem->setPrior(x0, P0, 0.0, 0.01);
}
virtual void TearDown(){}