diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index db6c1c261d42ac79b1cafa24a9a09ce3ea1bbfa0..1142c38c997b40a674c04ffece7cc254a58f1d0d 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -5,7 +5,7 @@
* WOLF includes *
**************************/
#include "core/common/wolf.h"
-#include <core/processor/processor_base.h>
+#include <core/processor/processor_loop_closure.h>
#include <laser/capture/capture_laser_2d.h>
#include <core/factor/factor_relative_pose_2d_with_extrinsics.h>
@@ -22,10 +22,10 @@ namespace wolf
WOLF_PTR_TYPEDEFS(ProcessorLoopClosureIcp);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcp);
-struct ParamsProcessorLoopClosureIcp : public ParamsProcessorBase
+struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure
{
- int recent_key_frames_ignored;
- int key_frames_to_wait;
+ int recent_frames_ignored;
+ int frames_to_wait;
double max_error_threshold;
double min_points_percent;
@@ -34,14 +34,14 @@ struct ParamsProcessorLoopClosureIcp : public ParamsProcessorBase
ParamsProcessorLoopClosureIcp() = default;
ParamsProcessorLoopClosureIcp(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorBase(_unique_name, _server)
+ ParamsProcessorLoopClosure(_unique_name, _server)
{
- recent_key_frames_ignored = _server.getParam<int> (prefix + _unique_name + "/recent_key_frames_ignored");
- key_frames_to_wait = _server.getParam<int> (prefix + _unique_name + "/key_frames_to_wait");
+ recent_frames_ignored = _server.getParam<int> (prefix + _unique_name + "/recent_frames_ignored");
+ frames_to_wait = _server.getParam<int> (prefix + _unique_name + "/frames_to_wait");
max_error_threshold = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
min_points_percent = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
- icp_params.use_point_to_line_distance = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
+ icp_params.cov_factor = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
icp_params.use_point_to_line_distance = _server.getParam<int> (prefix + _unique_name + "/use_point_to_line_distance");
icp_params.max_correspondence_dist = _server.getParam<double> (prefix + _unique_name + "/max_correspondence_dist");
@@ -79,22 +79,21 @@ struct ParamsProcessorLoopClosureIcp : public ParamsProcessorBase
}
std::string print() const override
{
- return "\n" + ParamsProcessorBase::print()
- + "recent_key_frames_ignored: " + std::to_string(recent_key_frames_ignored) + "\n"
- + "key_frames_to_wait: " + std::to_string(key_frames_to_wait) + "\n"
- + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
- + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+ return "\n" + ParamsProcessorLoopClosure::print()
+ + "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n"
+ + "frames_to_wait: " + std::to_string(frames_to_wait) + "\n"
+ + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
+ + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
}
};
- struct CapturesAligned{
- CaptureBasePtr capture_own;
- CaptureBasePtr capture_other;
- laserscanutils::icpOutput align_result;
-
- };
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosureIcp);
+struct MatchLoopClosureIcp : public MatchLoopClosure
+{
+ laserscanutils::icpOutput align_result_;
+};
-class ProcessorLoopClosureIcp : public ProcessorBase
+class ProcessorLoopClosureIcp : public ProcessorLoopClosure
{
protected:
// Useful sensor stuff
@@ -103,7 +102,7 @@ class ProcessorLoopClosureIcp : public ProcessorBase
//Closeloop parameters
ParamsProcessorLoopClosureIcpPtr params_loop_closure_icp_;
- int key_frames_skipped_;
+ int frames_skipped_;
// ICP algorithm
std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
@@ -115,32 +114,29 @@ class ProcessorLoopClosureIcp : public ProcessorBase
void configure(SensorBasePtr _sensor) override;
protected:
- void processCapture(CaptureBasePtr) override;
-
- void processKeyFrame(FrameBasePtr _keyframe_ptr,
- const double& _time_tolerance) override;
-
- bool triggerInCapture(CaptureBasePtr) const override;
-
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
- const double& _time_tolerance) const override;
-
- bool storeKeyFrame(FrameBasePtr) override;
-
- bool storeCapture(CaptureBasePtr) override;
-
- bool voteForKeyFrame() const override;
- FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr,
- const double &_time_tolerance);
+ /** \brief Returns if findLoopClosures() has to be called for the given capture
+ * Since there is no enough information in the capture to decide if a loop can be closed, try always.
+ */
+ bool voteFindLoopClosures(CaptureBasePtr cap) override;
- std::map<double, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own,
- FrameBasePtrList& _keyframe_candidates);
+ /** \brief detects and emplaces all features of the given capture
+ * Since the loop closures of this processor are not based on any features, it is empty
+ */
+ void emplaceFeatures(CaptureBasePtr cap) override{};
- CapturesAligned bestCandidate(std::map<double, CapturesAligned>& _capture_candidates);
+ /** \brief Find captures that correspond to loop closures with the given capture
+ */
+ std::map<double, MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override;
- FactorBasePtr emplaceFeatureAndFactor(CapturesAligned& _captures_aligned);
+ /** \brief validates a loop closure
+ * Loops are validated just after calling ICP
+ */
+ bool validateLoopClosure(MatchLoopClosurePtr) override{ return true;};
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ void emplaceFactors(MatchLoopClosurePtr) override;
};
}
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index cd3e809f405ee5b40ee252d21a9420d33c06b42b..ad47fe887654a6b6cf50aef494bfbfd2d804ebd4 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -2,17 +2,15 @@
#include "laser/sensor/sensor_laser_2d.h"
#include "core/math/covariance.h"
-using namespace wolf;
-using namespace laserscanutils;
+namespace wolf
+{
-namespace wolf{
-// Constructor
ProcessorLoopClosureIcp::ProcessorLoopClosureIcp(ParamsProcessorLoopClosureIcpPtr _params) :
- ProcessorBase("ProcessorLoopClosureIcp", 2, _params),
+ ProcessorLoopClosure("ProcessorLoopClosureIcp", 2, _params),
params_loop_closure_icp_(_params),
- key_frames_skipped_(0)
+ frames_skipped_(0)
{
- icp_tools_ptr_ = std::make_shared<ICP>();
+ icp_tools_ptr_ = std::make_shared<laserscanutils::ICP>();
}
void ProcessorLoopClosureIcp::configure(SensorBasePtr _sensor)
@@ -21,218 +19,150 @@ void ProcessorLoopClosureIcp::configure(SensorBasePtr _sensor)
laser_scan_params_ = sensor_laser_->getScanParams();
}
-void ProcessorLoopClosureIcp::processCapture(CaptureBasePtr _capture_ptr)
-{
-}
-
-void ProcessorLoopClosureIcp::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+bool ProcessorLoopClosureIcp::voteFindLoopClosures(CaptureBasePtr cap)
{
- // TODO: iterate all stored KF
- // packs buffer_pack_kf_.selectFirstPackBefore(_keyframe_ptr->getTimeStamp(),_time_tolerance);
- // if not _keyframe_ptr->hasCaptureOf(getSensor())
- //{
- // link _keyframe_ptr to select(const TimeStamp& _time_stamp, const double& _time_tolerance) if not nullptr
- // else return
- //}
-
- // PROCESS
- if (_keyframe_ptr != nullptr) {
- WOLF_INFO("KF TS", _keyframe_ptr->getTimeStamp());
- } else
- WOLF_INFO("keyframe nullptr");
-
- auto kf_cptr = _keyframe_ptr->getCaptureOf(getSensor());
- auto cptr = buffer_capture_.select(_keyframe_ptr->getTimeStamp(), _time_tolerance);
-
- WOLF_INFO_COND(cptr != nullptr, "cptr timestamp ", cptr->getTimeStamp());
-
- if (kf_cptr != nullptr)
- {
- // WOLF_INFO("Linking capture");
- // kf_cptr->link(_keyframe_ptr);
- }
- else
+ // process 1 and skip 'frames_to_wait' frames
+ if (frames_skipped_ >= params_loop_closure_icp_->frames_to_wait)
{
- WOLF_INFO("Not Linking capture");
+ frames_skipped_=0;
+ return true;
}
-
- if(key_frames_skipped_ >= params_loop_closure_icp_->key_frames_to_wait)
- {
- key_frames_skipped_ = 0;
- FrameBasePtrList candidates = selectCandidates(_keyframe_ptr, _time_tolerance);
- WOLF_DEBUG("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% START EVALUATE CANDIDATES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%");
- auto candidates_map = evaluateCandidates(_keyframe_ptr, candidates);
- WOLF_DEBUG("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% END EVALUATE CANDIDATES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%");
- if (not candidates_map.empty()) {
- auto best = bestCandidate(candidates_map);
- emplaceFeatureAndFactor(best);
- // this->getProblem()->print(4,1,1,0);
-
- } else {
- WOLF_DEBUG("%%%%%%%%%%%%%%%% Candidates map is empty :(");
- }
- } else {
- key_frames_skipped_++;
- }
- }
-
-bool ProcessorLoopClosureIcp::triggerInCapture(CaptureBasePtr) const
-{
+ frames_skipped_++;
return false;
}
-bool ProcessorLoopClosureIcp::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const
+std::map<double, MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(CaptureBasePtr _capture)
{
- return true;
-}
-
-bool ProcessorLoopClosureIcp::storeKeyFrame(FrameBasePtr)
-{
- return false;
-}
+ assert(std::dynamic_pointer_cast<CaptureLaser2d>(_capture));
+ std::map<double, MatchLoopClosurePtr> match_map;
-bool ProcessorLoopClosureIcp::storeCapture(CaptureBasePtr)
-{
- return true;
-}
+ auto cap_own = std::static_pointer_cast<CaptureLaser2d>(_capture);
+ auto frame_own = _capture->getFrame();
+ auto frame_ref = frame_own->getPreviousFrame();
+ auto frame_distance = 1;
-bool ProcessorLoopClosureIcp::voteForKeyFrame() const
-{
- return false;
-}
-
-FrameBasePtrList ProcessorLoopClosureIcp::selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance)
-{
- FrameBasePtrList candidates;
- int key_frames_counter = 0;
- const auto &trajectory = getProblem()->getTrajectory();
- //Consider only key_frames from 1 to n - match_past_key_frame_
- // std::copy_if(frames.begin(), frames.end(), std::back_inserter(candidates), [&](FrameBasePtr _frame) { if(_frame->isKey()) {key_frames_counter++; return (key_frames_counter % match_past_key_frame_ == 0);
- // }else{ return false;};});
- for(auto it=trajectory->rbegin(); it != trajectory->rend(); it++)
- {
- // WOLF_DEBUG("TIMESTAMP KEY FRAME ", (*it)->id(), " ", (*it)->getTimeStamp());
- key_frames_counter++;
- if (key_frames_counter > params_loop_closure_icp_->recent_key_frames_ignored and
- (_keyframe_ptr->getP()->getState() - (*it)->getP()->getState()).norm() < laser_scan_params_.range_max_)
- candidates.push_back(*it);
- }
- WOLF_DEBUG("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size());
- return candidates;
-}
-std::map<double, CapturesAligned> ProcessorLoopClosureIcp::evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList &_keyframe_candidates)
-{
- std::map<double, CapturesAligned> captures_candidates;
- CaptureLaser2dPtr capture_current = std::static_pointer_cast<wolf::CaptureLaser2d>(_keyframe_own->getCaptureOf(this->getSensor()));
- for(const auto& capture_own : _keyframe_own->getCaptureList())
+ while (frame_ref)
{
- WOLF_DEBUG("CAPTURE TYPE OWN", capture_own->getType());
- if(capture_own->getType() == "CaptureLaser2d")
+ // not too recent frames
+ if (frame_distance > params_loop_closure_icp_->recent_frames_ignored)
{
- CaptureLaser2dPtr capture_laser_own = std::static_pointer_cast<wolf::CaptureLaser2d>(capture_own);
- WOLF_DEBUG("CANDIDATES SIZE ", _keyframe_candidates.size());
- for (const auto &_keyframe_other : _keyframe_candidates)
- for (const auto &capture_other : _keyframe_other->getCaptureList())
+ // search CaptureLaser2d
+ for (auto cap : frame_ref->getCaptureList())
+ {
+ auto cap_ref = std::static_pointer_cast<wolf::CaptureLaser2d>(cap);
+ // try to match if no too far (more than laser range)
+ if (cap_ref and
+ (frame_ref->getP()->getState() - frame_own->getP()->getState()).norm() < laser_scan_params_.range_max_)
{
- if (capture_other->getType() == "CaptureLaser2d")
+ // Initial guess for alignment
+ Eigen::Vector3d transform_guess;
+ Eigen::Isometry2d T_w_rown, T_w_rref, T_r_s, T_sref_sown;
+
+ // Transformations
+ T_w_rown = Eigen::Translation2d(frame_own->getP()->getState()) *
+ Eigen::Rotation2Dd(frame_own->getO()->getState()(0));
+ T_w_rref = Eigen::Translation2d(frame_ref->getP()->getState()) *
+ Eigen::Rotation2Dd(frame_ref->getO()->getState()(0));
+ T_r_s = Eigen::Translation2d(this->getSensor()->getP()->getState()) * Eigen::Rotation2Dd(this->getSensor()->getO()->getState()(0));
+ T_sref_sown = T_r_s.inverse() * T_w_rref.inverse() * T_w_rown * T_r_s;
+
+ // Fill up initial guess
+ transform_guess.head(2) = T_sref_sown.translation();
+ Eigen::Rotation2Dd R(T_sref_sown.rotation());
+ transform_guess(2) = R.angle();
+
+ SensorLaser2dPtr sensor_own, sensor_ref;
+ sensor_own = std::static_pointer_cast<wolf::SensorLaser2d>(cap_own->getSensor());
+ sensor_ref = std::static_pointer_cast<wolf::SensorLaser2d>(cap_ref->getSensor());
+
+ // WOLF_DEBUG("LOOP CLOSURE: Aligning key frames: ", frame_own->id(), " and ", frame_ref->id(), "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
+ // Finally align
+ // WOLF_DEBUG("Sensor own scan params\n");
+ // sensor_own->getScanParams().print();
+ // WOLF_DEBUG("Sensor ref scan params\n");
+ // sensor_ref->getScanParams().print();
+ // WOLF_DEBUG("Icp params\n");
+ // WOLF_DEBUG("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
+ // "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance,
+ // "\n max_iterations: ", this->icp_params_.max_iterations,
+ // "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
+ // "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order,
+ // "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
+ WOLF_DEBUG("DBG Attempting to close loop with ", frame_own->id(), " and ", frame_ref->id());
+ try
{
- CaptureLaser2dPtr capture_laser_other = std::static_pointer_cast<wolf::CaptureLaser2d>(capture_other);
-
- // Initial guess for alignment
- Eigen::Vector3d transform_guess;
- Eigen::Isometry2d T_w_rown, T_w_rother, T_r_s, T_sother_sown;
-
- // Transformations
- T_w_rown = Eigen::Translation2d(_keyframe_own->getP()->getState()) * Eigen::Rotation2Dd(_keyframe_own->getO()->getState()(0));
- T_w_rother =
- Eigen::Translation2d(_keyframe_other->getP()->getState()) * Eigen::Rotation2Dd(_keyframe_other->getO()->getState()(0));
- T_r_s = Eigen::Translation2d(this->getSensor()->getP()->getState()) * Eigen::Rotation2Dd(this->getSensor()->getO()->getState()(0));
- T_sother_sown = T_r_s.inverse() * T_w_rother.inverse() * T_w_rown * T_r_s;
-
- // Fill up initial guess
- transform_guess.head(2) = T_sother_sown.translation();
- Eigen::Rotation2Dd R(T_sother_sown.rotation());
- transform_guess(2) = R.angle();
-
- SensorLaser2dPtr sensor_own, sensor_other;
- sensor_own = std::static_pointer_cast<wolf::SensorLaser2d>(capture_laser_own->getSensor());
- sensor_other = std::static_pointer_cast<wolf::SensorLaser2d>(capture_laser_other->getSensor());
-
- // WOLF_DEBUG("LOOP CLOSURE: Aligning key frames: ", _keyframe_own->id(), " and ", _keyframe_other->id(), "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
- // Finally align
- // WOLF_DEBUG("Sensor own scan params\n");
- // sensor_own->getScanParams().print();
- // WOLF_DEBUG("Sensor other scan params\n");
- // sensor_other->getScanParams().print();
- // WOLF_DEBUG("Icp params\n");
- // WOLF_DEBUG("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
- // "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance,
- // "\n max_iterations: ", this->icp_params_.max_iterations,
- // "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
- // "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order,
- // "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
- WOLF_DEBUG("DBG Attempting to close loop with ", _keyframe_own->id(), " and ", _keyframe_other->id());
- try
+ laserscanutils::icpOutput trf =
+ icp_tools_ptr_->align(cap_own->getScan(),
+ cap_ref->getScan(),
+ sensor_own->getScanParams(),
+ sensor_ref->getScanParams(),
+ params_loop_closure_icp_->icp_params,
+ transform_guess);
+
+ double points_coeff_used = ((double)trf.nvalid / (double)fmin(cap_own->getScan().ranges_raw_.size(),
+ cap_ref->getScan().ranges_raw_.size()));
+ double mean_error = trf.error / trf.nvalid;
+ WOLF_DEBUG("DBG ------------------------------");
+ WOLF_DEBUG("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", frame_own->id(),
+ " ref_id: ", frame_ref->id());
+ WOLF_DEBUG("DBG own_POSE: ", frame_own->getState().vector("PO").transpose(), " ref_POSE: ", frame_ref->getState().vector("PO").transpose(),
+ " Icp_guess: ", transform_guess.transpose(), " Icp_trf: ", trf.res_transf.transpose());
+
+ // WOLF_DEBUG("Covariance \n", trf.res_covar);
+ if (trf.valid == 1 and
+ mean_error < params_loop_closure_icp_->max_error_threshold and
+ points_coeff_used * 100 > params_loop_closure_icp_->min_points_percent)
{
- laserscanutils::icpOutput trf =
- icp_tools_ptr_->align(capture_laser_own->getScan(),
- capture_laser_other->getScan(),
- sensor_own->getScanParams(),
- sensor_other->getScanParams(),
- params_loop_closure_icp_->icp_params,
- transform_guess);
-
- double points_coeff_used = ((double)trf.nvalid / (double)fmin(capture_laser_own->getScan().ranges_raw_.size(),
- capture_laser_other->getScan().ranges_raw_.size()));
- double mean_error = trf.error / trf.nvalid;
- WOLF_DEBUG("DBG ------------------------------");
- WOLF_DEBUG("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", _keyframe_own->id(),
- " other_id: ", _keyframe_other->id());
- WOLF_DEBUG("DBG own_POSE: ", _keyframe_own->getState().vector("PO").transpose(), " other_POSE: ", _keyframe_other->getState().vector("PO").transpose(),
- " Icp_guess: ", transform_guess.transpose(), " Icp_trf: ", trf.res_transf.transpose());
- // WOLF_DEBUG("Covariance \n", trf.res_covar);
- if (trf.valid == 1 and
- mean_error < params_loop_closure_icp_->max_error_threshold and
- points_coeff_used * 100 > params_loop_closure_icp_->min_points_percent)
- {
- WOLF_DEBUG("DBG ADDED ", _keyframe_own->id(), " and ", _keyframe_other->id());
- captures_candidates.emplace(mean_error, CapturesAligned({capture_laser_own, capture_laser_other, trf}));
- }
- else
- {
- WOLF_DEBUG("DBG DISCARDED ", _keyframe_own->id(), " and ", _keyframe_other->id());
- }
+ WOLF_DEBUG("DBG ADDED ", frame_own->id(), " and ", frame_ref->id());
+
+ auto match = std::make_shared<MatchLoopClosureIcp>();
+ match->capture_reference_ptr_ = cap_ref;
+ match->capture_target_ptr_ = cap_own;
+ match->normalized_score_ = 1 - mean_error / params_loop_closure_icp_->max_error_threshold;
+ match->align_result_ = trf;
+ match_map.emplace(match->normalized_score_, match);
}
- catch (std::exception &e)
+ else
{
- WOLF_WARN(e.what())
+ WOLF_DEBUG("DBG DISCARDED ", frame_own->id(), " and ", frame_ref->id());
}
+
+ }
+ catch (std::exception &e)
+ {
+ WOLF_WARN("ProcessorLoopClosureIcp: ICP failed with error: ", e.what());
}
}
+ }
}
+ frame_ref = frame_ref->getPreviousFrame();
+ frame_distance++;
}
- return captures_candidates;
-}
-CapturesAligned ProcessorLoopClosureIcp::bestCandidate(std::map<double, CapturesAligned> &_capture_candidates)
-{
- return _capture_candidates.begin()->second;
+
+ return match_map;
}
-FactorBasePtr ProcessorLoopClosureIcp::emplaceFeatureAndFactor(CapturesAligned &_captures_aligned)
+
+void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr _match)
{
- assert(_captures_aligned.align_result.valid == 1);
- if (not isCovariance(_captures_aligned.align_result.res_covar))
- _captures_aligned.align_result.res_covar = 1e-4 * Eigen::Matrix3d::Identity();
-
- auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own,
- _captures_aligned.align_result);
- return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr,
- ftr,
- _captures_aligned.capture_other->getFrame(),
- shared_from_this(),
- params_->apply_loss_function,
- TOP_LOOP);
+ auto match_icp = std::static_pointer_cast<MatchLoopClosureIcp>(_match);
+
+ assert(match_icp->align_result_.valid == 1);
+
+ if (not isCovariance(match_icp->align_result_.res_covar))
+ match_icp->align_result_.res_covar = 1e-4 * Eigen::Matrix3d::Identity();
+
+ auto ftr = FeatureBase::emplace<FeatureICPAlign>(match_icp->capture_target_ptr_,
+ match_icp->align_result_);
+
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(ftr,
+ ftr,
+ match_icp->capture_reference_ptr_->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_LOOP);
}
+
}
// Register in the FactorySensor
#include "core/processor/factory_processor.h"