diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_closeloop_icp.h index 9b78bd18dd2c51206deeb61582451f5a31ee4e8f..6d1e1970b71e94a0f9e9137bc0b2d499e2ae7249 100644 --- a/include/laser/processor/processor_closeloop_icp.h +++ b/include/laser/processor/processor_closeloop_icp.h @@ -45,7 +45,7 @@ class ProcessorCloseLoopICP : public ProcessorBase virtual void process(CaptureBasePtr _capture_ptr) override; - virtual bool voteForKeyFrame() override; + virtual bool voteForKeyFrame() const override; static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr); diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h index 138687cc2ac81d37d9516ba4f178fb1a3fcf38d7..4ca710c60d1c8148fb40f7e3859cca8386992184 100644 --- a/include/laser/processor/processor_odom_icp.h +++ b/include/laser/processor/processor_odom_icp.h @@ -98,7 +98,7 @@ class ProcessorOdomICP : public ProcessorTracker virtual unsigned int processKnown() override; virtual unsigned int processNew(const int& _max_features) override; - virtual bool voteForKeyFrame() override; + virtual bool voteForKeyFrame() const override; virtual void advanceDerived() override; virtual void resetDerived() override; @@ -108,10 +108,10 @@ class ProcessorOdomICP : public ProcessorTracker FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser); FactorBasePtr emplaceFactor(FeatureBasePtr _feature); - inline bool voteForKeyFrameDistance(); - inline bool voteForKeyFrameAngle(); - inline bool voteForKeyFrameTime(); - inline bool voteForKeyFrameMatchQuality(); + inline bool voteForKeyFrameDistance() const; + inline bool voteForKeyFrameAngle() const; + inline bool voteForKeyFrameTime() const; + inline bool voteForKeyFrameMatchQuality() const; diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h index d6804f771e9c78b718194cf78c417b442c39041a..289f82becb18b543d67bc139bb2de070a1547fb1 100644 --- a/include/laser/processor/processor_tracker_feature_polyline_2D.h +++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h @@ -132,7 +132,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature * * WARNING! This function only votes! It does not create KeyFrames! */ - virtual bool voteForKeyFrame() override; + virtual bool voteForKeyFrame() const override; /**\brief Process new Features * diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp index d4f410071df0de2110f27ad7a4878c8ee46ab519..1df5aed8431236bbada8f7723df16346aa846e20 100644 --- a/src/processor/processor_closeloop_icp.cpp +++ b/src/processor/processor_closeloop_icp.cpp @@ -35,7 +35,7 @@ void ProcessorCloseLoopICP::process(CaptureBasePtr _capture_ptr) // _origin_cpt = cpt_ptr; } -bool ProcessorCloseLoopICP::voteForKeyFrame() +bool ProcessorCloseLoopICP::voteForKeyFrame() const { return 0; } diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index 0e0102c4e628136731716b4e39be20763c08e15b..7c8071730a35d7842f281755dad5f22c1240a4fb 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/src/processor/processor_odom_icp.cpp @@ -82,7 +82,7 @@ unsigned int ProcessorOdomICP::processNew(const int& _max_features) return 0; } -bool ProcessorOdomICP::voteForKeyFrame() +bool ProcessorOdomICP::voteForKeyFrame() const { return (voteForKeyFrameDistance() || voteForKeyFrameAngle() || @@ -90,7 +90,7 @@ bool ProcessorOdomICP::voteForKeyFrame() voteForKeyFrameMatchQuality()); } -inline bool ProcessorOdomICP::voteForKeyFrameDistance() +inline bool ProcessorOdomICP::voteForKeyFrameDistance() const { bool vote = (trf_origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist); if (vote) @@ -101,7 +101,7 @@ inline bool ProcessorOdomICP::voteForKeyFrameDistance() return vote; } -inline bool ProcessorOdomICP::voteForKeyFrameAngle() +inline bool ProcessorOdomICP::voteForKeyFrameAngle() const { bool vote = (fabs(trf_origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle); if (vote) @@ -112,7 +112,7 @@ inline bool ProcessorOdomICP::voteForKeyFrameAngle() return vote; } -inline bool ProcessorOdomICP::voteForKeyFrameTime() +inline bool ProcessorOdomICP::voteForKeyFrameTime() const { Scalar secs = incoming_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(); @@ -126,7 +126,7 @@ inline bool ProcessorOdomICP::voteForKeyFrameTime() return vote; } -inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality() +inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality() const { bool vote = (trf_origin_incoming_.error > proc_params_->vfk_min_error || trf_origin_incoming_.nvalid < proc_params_->vfk_max_points); if (vote) diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index 404889898031bf5e548ef957625c071c0b8fe08f..8c2f96ab0fb7bdc880533aedeb1b71e34e05f632 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -285,7 +285,7 @@ bool ProcessorTrackerFeaturePolyline2D::tryCompletePartialMatch(LandmarkMatchPol return true; } -bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame() +bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame() const { //WOLF_DEBUG("PTFP ", getName(), "::voteForKeyFrame: "); // TODO