diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 73eab66545543855df753984a5fb5fb55d3607d6..102cc6a180264c402355be4a48e47a99cf2d86bf 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -283,6 +283,16 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void emplaceStatesToFrame(FrameBasePtr _frame_ptr, const StateKeys& _frame_keys);
+ /** \brief Take a frame and emplace to it the missing states
+ * \param _frame_ptr Frame to which states have to be emplaced
+ * \param _frame_states the states vectors to be added to the frame
+ *
+ * - The frame_types are taken from Problem.
+ * - Only the missing keys are emplaced.
+ * - The states are emplaced to the frame as unfixed.
+ */
+ void emplaceStatesToFrame(FrameBasePtr _frame_ptr, const VectorComposite& _frame_states);
+
// Frame getters
FrameBaseConstPtr getLastFrame() const;
FrameBasePtr getLastFrame();
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 4d8dcee2844eafebb27865d38329c6c6eb263c7e..6464addfa1cba48b45bbeab7ecabefe43f636204 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -503,8 +503,6 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
- FrameBasePtr last_frame_ptr_;
-
protected:
// helpers to avoid allocation
mutable double dt_; ///< Time step
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index a744c222d91986ca9ba35df0cbf26314c6968998..bd9a117fac1303ae286dd64ffaff26aedfea099d 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -538,6 +538,20 @@ void Problem::emplaceStatesToFrame(FrameBasePtr _frame_ptr, const StateKeys& _fr
_frame_ptr->emplaceStateBlock(vec_pair.first, frame_types_.at(vec_pair.first), vec_pair.second, false);
}
+void Problem::emplaceStatesToFrame(FrameBasePtr _frame_ptr, const VectorComposite& _frame_states)
+{
+ if (not _frame_ptr) throw std::runtime_error("Problem::emplaceStatesToFrame: the provided frame is nullptr");
+
+ if (not frame_types_.has(_frame_states.getKeys()))
+ throw std::runtime_error("Problem::emplaceStatesToFrame: any unknown type. Asked for key " + _frame_states.getKeys() +
+ " but problem only know the types of " + frame_types_.getKeys());
+
+ // emplace missing keys
+ for (auto key : _frame_states.getKeys())
+ if (not _frame_ptr->getState().has(key))
+ _frame_ptr->emplaceStateBlock(key, frame_types_.at(key), _frame_states.at(key), false);
+}
+
TimeStamp Problem::getTimeStamp() const
{
TimeStamp ts = TimeStamp::Invalid();
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 824af2cb3f399782f9d33c0158b8b4fa7c047442..eb49aa5815913b06f6b7a8e70838f5e96db113db 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -130,13 +130,13 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
}
CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin)
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin)
{
auto cap_motion =
CaptureBase::emplace<CaptureDiffDrive>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index abad4de30d6f6eb0b01839040e69216825dcfaf6..4bba66a5327b652977914b736bc779c593b384cd 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -41,7 +41,6 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
origin_ptr_(),
last_ptr_(),
incoming_ptr_(),
- last_frame_ptr_(),
dt_(0.0),
x_(_state_size),
delta_(_delta_size),
@@ -208,6 +207,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
preProcess(); // Derived class operations
+ FrameBasePtr new_keyframe = nullptr;
FrameBasePtr keyframe_from_callback = computeProcessingStep();
if (keyframe_from_callback) buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
@@ -215,12 +215,12 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
{
case FIRST_TIME_WITHOUT_KF: {
// There is no KF: create own origin
- setOrigin(getProblem()->stateZero(getStateKeys()), _incoming_ptr->getTimeStamp());
+ new_keyframe = setOrigin(getProblem()->stateZero(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_BEFORE_INCOMING: {
// cannot join to the KF: create own origin
- setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
+ new_keyframe = setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_ON_INCOMING: {
@@ -230,17 +230,16 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
}
case FIRST_TIME_WITH_KF_AFTER_INCOMING: {
// cannot join to the KF: create own origin
- setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
+ new_keyframe = setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
- case RUNNING_WITHOUT_KF:
- case RUNNING_WITH_KF_ON_ORIGIN:
- break;
-
default:
break;
}
+ // callback new keyframe to other processors
+ if (new_keyframe) getProblem()->keyFrameCallback(new_keyframe, shared_from_this());
+
// integrate motion data
// Done at this place because emplaceFirstFrame() needs
integrateOneStep();
@@ -295,12 +294,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = getState(timestamp_from_callback);
- for (auto pair_key_vec : proc_state)
- if (not keyframe_from_callback->has(pair_key_vec.first))
- keyframe_from_callback->emplaceStateBlock(
- pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
- else
- keyframe_from_callback->getStateBlock(pair_key_vec.first)->setState(pair_key_vec.second);
+ getProblem()->emplaceStatesToFrame(keyframe_from_callback, proc_state);
// Find the capture acting as the buffer's origin
auto capture_origin = capture_existing->getOriginCapture();
@@ -318,7 +312,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback =
- emplaceCapture(keyframe_from_callback, // j
+ emplaceCapture(keyframe_from_callback,
getSensor(),
timestamp_from_callback,
Eigen::VectorXd::Zero(data_size_),
@@ -331,18 +325,16 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// and give the part of the buffer before the new keyframe to the capture for the KF callback
splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
- // // create motion feature and add it to the capture
- // // create motion factor and add it to the feature, and constrain to the other capture (origin)
+ // emplace motion features and factors
emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
// modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty())
{
- // setMeasurement is not allowed: feature (and factor) will be removed and a new feature and factor
- // will be emplaced
+ // feature (and factor) will be removed and new ones will be emplaced
capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
- assert(capture_existing->getFeatureList().empty()); // there was only one feature!
+ assert(capture_existing->getFeatureList().empty()); // ensure that there was only one feature
emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing);
}
@@ -389,14 +381,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = this->getState(timestamp_from_callback);
- for (auto pair_key_vec : proc_state)
- if (not keyframe_from_callback->has(pair_key_vec.first))
- keyframe_from_callback->emplaceStateBlock(
- pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
- else
- keyframe_from_callback->getStateBlock(pair_key_vec.first)->setState(pair_key_vec.second);
-
- auto& capture_existing = last_ptr_;
+ getProblem()->emplaceStatesToFrame(keyframe_from_callback, proc_state);
// Find the capture acting as the buffer's origin
auto& capture_origin = origin_ptr_;
@@ -417,10 +402,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
+ splitBuffer(last_ptr_, timestamp_from_callback, capture_for_keyframe_callback);
- // create motion feature and add it to the capture
- // create motion factor and add it to the feature, and constrain to the other capture (origin)
+ // emplace motion features and factors
emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
// reset processor origin
@@ -433,24 +417,15 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- // Update state and time stamps
+ // Update time stamp
auto ts = getTimeStamp();
last_ptr_->setTimeStamp(ts);
- last_ptr_->getFrame()->setTimeStamp(ts);
- VectorComposite state_propa = getState(ts);
- for (auto pair_key_vec : state_propa)
- if (not last_ptr_->getFrame()->has(pair_key_vec.first))
- last_ptr_->getFrame()->emplaceStateBlock(
- pair_key_vec.first, getStateTypes().at(pair_key_vec.first), pair_key_vec.second, false);
- else
- last_ptr_->getFrame()->getStateBlock(pair_key_vec.first)->setState(pair_key_vec.second);
if (permittedKeyFrame() && voteForKeyFrame())
{
/*
* Legend:
* * : any keyframe
- * + : last frame
* x : any capture
* o : origin capture
* l : last capture -> we'll make a KF here
@@ -460,19 +435,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
*
* Trajectory before the KF
*
- * * ========= * ========= +
+ * * ========= *
* x ----------o ----------l i
* \____f____/
*
* Trajectory after creating KF at last
*
- * * ========= * ========= * = +
+ * * ========= * ========= *
* x ----------o ----------l --n
* \____f____/ \____f____/
*
* Trajectory after creating KF at last and reset
*
- * * ========= * ========= * = +
+ * * ========= * ========= *
* x ----------x ----------o --l
* \____f____/ \____f____/
*
@@ -481,19 +456,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Set the capture's calibration with best results available (in case origin_ptr_ received a solve())
setCalibration(last_ptr_, getCalibration(origin_ptr_));
- // Set the frame of last_ptr as key
- auto keyframe = last_ptr_->getFrame();
- keyframe->link(getProblem());
+ // Create a keyframe at last_ptr
+ auto keyframe =
+ getProblem()->emplaceFrame(last_ptr_->getTimeStamp(), this->getState(last_ptr_->getTimeStamp()));
- // create motion feature and add it to the key_capture
- // create motion factor and link it to parent feature and other frame (which is origin's frame)
+ // link last_ptr_ to the keyframe
+ last_ptr_->link(keyframe);
+
+ // emplace features and factors
emplaceFeaturesAndFactors(origin_ptr_, last_ptr_);
- // create a new frame
- auto frame_new =
- std::make_shared<FrameBase>(getTimeStamp(), getStateTypes(), getProblem()->getState(getStateKeys()));
// create a new capture
- auto capture_new = emplaceCapture(frame_new,
+ auto capture_new = emplaceCapture(nullptr, // no linked to any frame yet
getSensor(),
keyframe->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
@@ -508,11 +482,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
// Reset pointers
- origin_ptr_ = last_ptr_;
- last_ptr_ = capture_new;
- last_frame_ptr_ = frame_new;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = capture_new;
- // callback to other processors
+ // callback keyframe to other processors
getProblem()->keyFrameCallback(keyframe, shared_from_this());
}
@@ -546,8 +519,7 @@ VectorComposite ProcessorMotion::getState(StateKeys _keys) const
// We do not have any info of where to find a valid state
// Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
- if (origin_ptr_ == nullptr or origin_ptr_->isRemoving() or last_ptr_ == nullptr or
- last_ptr_->getFrame() == nullptr)
+ if (origin_ptr_ == nullptr or origin_ptr_->isRemoving() or last_ptr_ == nullptr)
{
WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
return VectorComposite(); // return empty state
@@ -559,7 +531,7 @@ VectorComposite ProcessorMotion::getState(StateKeys _keys) const
// this may happen when in the very first frame where the capture has no motion info --> empty buffer
if (last_ptr_->getBuffer().empty())
{
- return last_ptr_->getFrame()->getState(_keys);
+ return origin_ptr_->getFrame()->getState(_keys);
}
/* Doing this:
@@ -787,11 +759,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ptr_->getBuffer().push_back(motionZero(_origin_frame->getTimeStamp()));
// ---------- LAST ----------
- // Make non-key-frame for last Capture
- last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts, getStateTypes(), _origin_frame->getState(getStateKeys()));
-
- // emplace (emtpy) last Capture
- last_ptr_ = emplaceCapture(last_frame_ptr_,
+ // create last capture
+ last_ptr_ = emplaceCapture(nullptr, // no linked to any frame yet
getSensor(),
origin_ts,
Eigen::VectorXd::Zero(data_size_),
@@ -1131,8 +1100,7 @@ void ProcessorMotion::printHeader(int _depth,
<< " Frm" << getOrigin()->getFrame()->id() << std::endl;
if (getLast())
_stream << _tabs << " "
- << "l: Cap" << getLast()->id() << " - "
- << " Frm" << getLast()->getFrame()->id() << std::endl;
+ << "l: Cap" << getLast()->id() << std::endl;
if (getIncoming())
_stream << _tabs << " "
<< "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index d4b585dcd3a949b3ac831672118c53600ac5a988..b57fe809d014062ca5bf7edfd171b22c699bfe01 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -64,7 +64,7 @@ class ProcessorLoopClosureTest : public testing::Test
return CaptureBase::emplace<CaptureBase>(frame, "CaptureBase", frame->getTimeStamp(), sensor);
}
- CaptureBasePtr createCapture(TimeStamp ts)
+ CaptureBasePtr emplaceCapture(TimeStamp ts)
{
// new capture
return std::make_shared<CaptureBase>("CaptureBase", ts, sensor);
@@ -92,7 +92,7 @@ TEST_F(ProcessorLoopClosureTest, frame_stored)
TEST_F(ProcessorLoopClosureTest, capture_stored)
{
// new capture
- auto cap1 = createCapture(1);
+ auto cap1 = emplaceCapture(1);
// captureCallback
processor->captureCallback(cap1);
@@ -121,7 +121,7 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
auto frm1 = emplaceFrame(1, Vector3d::Zero());
// new capture
- auto cap1 = createCapture(1);
+ auto cap1 = emplaceCapture(1);
// keyframecallback
problem->keyFrameCallback(frm1, nullptr);
@@ -140,7 +140,7 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
// new frame
auto frm1 = emplaceFrame(1, Vector3d::Zero());
// new capture
- auto cap1 = createCapture(2);
+ auto cap1 = emplaceCapture(2);
// keyframecallback
problem->keyFrameCallback(frm1, nullptr);
@@ -173,7 +173,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
// new frame
auto frm1 = emplaceFrame(1, Vector3d::Zero());
// new capture
- auto cap1 = createCapture(1);
+ auto cap1 = emplaceCapture(1);
// captureCallback
processor->captureCallback(cap1);
@@ -192,7 +192,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
// new frame
auto frm1 = emplaceFrame(2, Vector3d::Zero());
// new capture
- auto cap1 = createCapture(1);
+ auto cap1 = emplaceCapture(1);
// captureCallback
processor->captureCallback(cap1);
@@ -211,7 +211,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
// new frame
auto frm1 = emplaceFrame(1, Vector3d::Zero());
// new capture
- auto cap1 = createCapture(2);
+ auto cap1 = emplaceCapture(2);
// captureCallback
processor->captureCallback(cap1);
@@ -234,7 +234,7 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
auto frm4 = emplaceFrame(4, Vector3d::Zero());
auto frm5 = emplaceFrame(5, Vector3d::Zero());
// new captures
- auto cap4 = createCapture(4);
+ auto cap4 = emplaceCapture(4);
// keyframecallback
problem->keyFrameCallback(frm1, nullptr);
@@ -264,11 +264,11 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
// new frame
auto frm2 = emplaceFrame(2, Vector3d::Zero());
// new captures
- auto cap1 = createCapture(1);
- auto cap2 = createCapture(2);
- auto cap3 = createCapture(3);
- auto cap4 = createCapture(4);
- auto cap5 = createCapture(5);
+ auto cap1 = emplaceCapture(1);
+ auto cap2 = emplaceCapture(2);
+ auto cap3 = emplaceCapture(3);
+ auto cap4 = emplaceCapture(4);
+ auto cap5 = emplaceCapture(5);
// captureCallback
processor->captureCallback(cap1);