Skip to content
Snippets Groups Projects
Commit d5501da0 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

WIP: Method PM::emplaceFeaturesAndFactors()

parent c4bfa85c
No related branches found
No related tags found
2 merge requests!466devel->main,!459Resolve "Allow ProcessorMotion to produce more than one Feature and Factor"
Pipeline #12746 failed
...@@ -488,6 +488,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider ...@@ -488,6 +488,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
*/ */
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0; virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) {}; // TODO make pure virtual
virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0; virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
Motion motionZero(const TimeStamp& _ts) const; Motion motionZero(const TimeStamp& _ts) const;
......
...@@ -119,8 +119,10 @@ void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev, ...@@ -119,8 +119,10 @@ void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
// emplace new feature and factor (if origin has frame) // emplace new feature and factor (if origin has frame)
if (cap_prev->getOriginCapture() and cap_prev->getOriginCapture()->getFrame()) if (cap_prev->getOriginCapture() and cap_prev->getOriginCapture()->getFrame())
{ {
auto new_feature = emplaceFeature(cap_target); // auto new_feature = emplaceFeature(cap_target);
emplaceFactor(new_feature, cap_prev->getOriginCapture()); // emplaceFactor(new_feature, cap_prev->getOriginCapture());
emplaceFeaturesAndFactors(cap_prev->getOriginCapture(), cap_target);
} }
} }
...@@ -312,11 +314,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) ...@@ -312,11 +314,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// and give the part of the buffer before the new keyframe to the capture for the KF callback // 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(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture // // create motion feature and add it to the capture
auto feature_new = emplaceFeature(capture_for_keyframe_callback); // auto feature_new = emplaceFeature(capture_for_keyframe_callback);
// // create motion factor and add it to the feature, and constrain to the other capture (origin)
// emplaceFactor(feature_new, capture_origin );
// create motion factor and add it to the feature, and constrain to the other capture (origin) emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
emplaceFactor(feature_new, capture_origin );
// modify existing feature and factor (if they exist in the existing capture) // modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty()) if (!capture_existing->getFeatureList().empty())
...@@ -326,9 +330,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) ...@@ -326,9 +330,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
assert(capture_existing->getFeatureList().empty());// there was only one feature! assert(capture_existing->getFeatureList().empty());// there was only one feature!
auto new_feature_existing = emplaceFeature(capture_existing); // auto new_feature_existing = emplaceFeature(capture_existing);
emplaceFactor(new_feature_existing, capture_for_keyframe_callback); // emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing);
} }
break; break;
...@@ -405,11 +411,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) ...@@ -405,11 +411,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// and give the part of the buffer before the new keyframe to the capture for the KF callback // 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(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture // // create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback); // auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
// // create motion factor and add it to the feature, and constrain to the other capture (origin)
// emplaceFactor(feature_for_keyframe_callback, capture_origin );
// create motion factor and add it to the feature, and constrain to the other capture (origin) emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
emplaceFactor(feature_for_keyframe_callback, capture_origin );
// reset processor origin // reset processor origin
origin_ptr_ = capture_for_keyframe_callback; origin_ptr_ = capture_for_keyframe_callback;
...@@ -476,11 +484,15 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) ...@@ -476,11 +484,15 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto keyframe = last_ptr_->getFrame(); auto keyframe = last_ptr_->getFrame();
keyframe ->link(getProblem()); keyframe ->link(getProblem());
// create motion feature and add it to the key_capture // // create motion feature and add it to the key_capture
auto key_feature = emplaceFeature(last_ptr_); // auto key_feature = emplaceFeature(last_ptr_);
// // create motion factor and link it to parent feature and other frame (which is origin's frame)
// auto factor = emplaceFactor(key_feature, origin_ptr_);
// 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) // create motion factor and link it to parent feature and other frame (which is origin's frame)
auto factor = emplaceFactor(key_feature, origin_ptr_); emplaceFeaturesAndFactors(origin_ptr_, last_ptr_);
// create a new frame // create a new frame
auto frame_new = std::make_shared<FrameBase>(getTimeStamp(), auto frame_new = std::make_shared<FrameBase>(getTimeStamp(),
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment