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

small changes in MatchLoopClosure struct

parent 8f05890c
No related branches found
No related tags found
No related merge requests found
Pipeline #7115 passed
......@@ -22,10 +22,10 @@ struct ParamsProcessorLoopClosure : public ParamsProcessorBase
std::string print() const override
{
return "\n" + ParamsProcessorBase::print()
+ "max_loops: " + std::to_string(max_loops) + "\n";
+ "max_loops: " + std::to_string(max_loops) + "\n";
}
};
WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
/** \brief Match between a capture and a capture
......@@ -35,9 +35,9 @@ WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
*/
struct MatchLoopClosure
{
CaptureBasePtr capture_reference_ptr_; ///< Capture reference
CaptureBasePtr capture_target_ptr_; ///< Capture target
double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
CaptureBasePtr capture_reference; ///< Capture reference
CaptureBasePtr capture_target; ///< Capture target
double normalized_score; ///< normalized similarity score (0 is bad, 1 is good)
};
/** \brief General loop closure processor
......@@ -54,55 +54,55 @@ struct MatchLoopClosure
*/
class ProcessorLoopClosure : public ProcessorBase
{
protected:
protected:
ParamsProcessorLoopClosurePtr params_loop_closure_;
ParamsProcessorLoopClosurePtr params_loop_closure_;
public:
public:
ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
~ProcessorLoopClosure() override = default;
void configure(SensorBasePtr _sensor) override { };
~ProcessorLoopClosure() override = default;
void configure(SensorBasePtr _sensor) override { };
protected:
protected:
/** \brief Process a capture (linked to a frame)
* If voteFindLoopClosures() returns true, findLoopClosures() is called.
* emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
*/
virtual void process(CaptureBasePtr);
/** \brief Process a capture (linked to a frame)
* If voteFindLoopClosures() returns true, findLoopClosures() is called.
* emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
*/
virtual void process(CaptureBasePtr);
/** \brief Returns if findLoopClosures() has to be called for the given capture
*/
virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
/** \brief Returns if findLoopClosures() has to be called for the given capture
*/
virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
/** \brief detects and emplaces all features of the given capture
*/
virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
/** \brief detects and emplaces all features of the given capture
*/
virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
/** \brief Find captures that correspond to loop closures with the given capture
*/
virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
/** \brief Find captures that correspond to loop closures with the given capture
*/
virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
/** \brief validates a loop closure
*/
virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
/** \brief validates a loop closure
*/
virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
/** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
*/
virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
/** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
*/
virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
void processCapture(CaptureBasePtr) override;
void processKeyFrame(FrameBasePtr, const double&) override;
void processCapture(CaptureBasePtr) override;
void processKeyFrame(FrameBasePtr, const double&) override;
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
bool storeCapture(CaptureBasePtr _cap) override { return false;};
bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
bool storeCapture(CaptureBasePtr _cap) override { return false;};
bool voteForKeyFrame() const override { return false;};
bool voteForKeyFrame() const override { return false;};
};
} // namespace wolf
......
......@@ -44,14 +44,14 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
(feat->getMeasurement() - _capture->getFeatureList().front()->getMeasurement()).norm() < 1e-3)
{
MatchLoopClosurePtr match = std::make_shared<MatchLoopClosure>();
match->capture_reference_ptr_ = cap;
match->capture_target_ptr_ = _capture;
match->normalized_score_ = 1;
match->capture_reference = cap;
match->capture_target = _capture;
match->normalized_score = 1;
while (match_lc_map.count(match->normalized_score_))
match->normalized_score_ -= 1e-9;
while (match_lc_map.count(match->normalized_score))
match->normalized_score -= 1e-9;
match_lc_map.emplace(match->normalized_score_, match);
match_lc_map.emplace(match->normalized_score, match);
}
old_frame = old_frame->getPreviousFrame();
......@@ -63,7 +63,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
void emplaceFactors(MatchLoopClosurePtr match) override
{
FeatureBasePtr feat_2;
for (auto feat : match->capture_target_ptr_->getFeatureList())
for (auto feat : match->capture_target->getFeatureList())
if (feat->getType() == "FeatureLoopClosureDummy")
{
feat_2 = feat;
......@@ -71,7 +71,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
}
FactorBase::emplace<FactorRelativePose2d>(feat_2, feat_2,
match->capture_reference_ptr_->getFrame(),
match->capture_reference->getFrame(),
shared_from_this(),
false,
TOP_LOOP);
......
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