diff --git a/src/processor_IMU.cpp b/src/processor_IMU.cpp index bb0ae86bffcb26e3b9d913bb57e65c23b6539231..4c944933433a540fc9d4d873d63fc97bce812351 100644 --- a/src/processor_IMU.cpp +++ b/src/processor_IMU.cpp @@ -8,13 +8,14 @@ ProcessorIMU::ProcessorIMU(const ProcessorParamsIMU& _params) : max_time_span_ (_params.max_time_span ), max_buff_length_(_params.max_buff_length ), dist_traveled_ (_params.dist_traveled ), - angle_turned_ (_params.angle_turned ), - voting_active_ (_params.voting_active ) + angle_turned_ (_params.angle_turned ) { // Set constant parts of Jacobians jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros jacobian_delta_.setIdentity(9,9); // jacobian_calib_.setZero(9,6); + + setVotingActive(_params.voting_active ); } ProcessorIMU::~ProcessorIMU() @@ -44,7 +45,7 @@ ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const Pro bool ProcessorIMU::voteForKeyFrame() { - if(!voting_active_) + if(!isVotingActive()) return false; // time span if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > max_time_span_) diff --git a/src/processor_IMU.h b/src/processor_IMU.h index 911d50f8cb26f230f1ba8e7e9802bfb1ec820a99..3954d5ac18b600a5ef4a5f40aaf6fa46115f81d3 100644 --- a/src/processor_IMU.h +++ b/src/processor_IMU.h @@ -16,7 +16,6 @@ struct ProcessorParamsIMU : public ProcessorParamsBase Size max_buff_length = 10; Scalar dist_traveled = 5; Scalar angle_turned = 0.5; - bool voting_active = false; //IMU will not vote for key Frames to be created }; WOLF_PTR_TYPEDEFS(ProcessorIMU); @@ -70,7 +69,6 @@ class ProcessorIMU : public ProcessorMotion{ Size max_buff_length_;// maximum buffer size before keyframe Scalar dist_traveled_; // maximum linear motion between keyframes Scalar angle_turned_; // maximum rotation between keyframes - bool voting_active_; // IMU will be voting for KeyFrame only if this is true public: diff --git a/src/processor_base.cpp b/src/processor_base.cpp index 2cc289cdd55bec6a315936749db2d3cffe86cdb3..c6a1e452976a9fa65f9b30600265ae203ec26a7e 100644 --- a/src/processor_base.cpp +++ b/src/processor_base.cpp @@ -12,7 +12,8 @@ ProcessorBase::ProcessorBase(const std::string& _type, const Scalar& _time_toler processor_id_(++processor_id_count_), time_tolerance_(_time_tolerance), sensor_ptr_(), - is_removing_(false) + is_removing_(false), + voting_active_(true) { // WOLF_DEBUG("constructed +p" , id()); } @@ -24,7 +25,7 @@ ProcessorBase::~ProcessorBase() bool ProcessorBase::permittedKeyFrame() { - return getProblem()->permitKeyFrame(shared_from_this()); + return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this()); } FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr) diff --git a/src/processor_base.h b/src/processor_base.h index 7d7fe4226957be55b61cb601f57bc9cf08e7e416..de6ecd94b37291e0c85f9b067fa0c35a2eb9b4ab 100644 --- a/src/processor_base.h +++ b/src/processor_base.h @@ -111,6 +111,7 @@ struct ProcessorParamsBase std::string type; std::string name; Scalar time_tolerance; ///< maximum time difference between a Keyframe time stamp and a particular Capture of this processor to allow assigning this Capture to the Keyframe. + bool voting_active; }; //class ProcessorBase @@ -125,6 +126,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce SensorBaseWPtr sensor_ptr_; bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove(). + bool voting_active_; ///< A flag for allowing the processor to vote for KF or not. + static unsigned int processor_id_count_; public: @@ -176,8 +179,21 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce void setTimeTolerance(Scalar _time_tolerance); + bool isVotingActive() const; + + void setVotingActive(bool _voting_active = true); }; +inline bool ProcessorBase::isVotingActive() const +{ + return voting_active_; +} + +inline void ProcessorBase::setVotingActive(bool _voting_active) +{ + voting_active_ = _voting_active; +} + } #include "sensor_base.h" diff --git a/src/processors/processor_diff_drive.cpp b/src/processors/processor_diff_drive.cpp index 8031040ce0b184d9c300143897925141140e5e3a..b6d57d918fc0c01cafc646146fc401d63d48d0d3 100644 --- a/src/processors/processor_diff_drive.cpp +++ b/src/processors/processor_diff_drive.cpp @@ -13,7 +13,7 @@ namespace wolf { ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrive ¶ms) : - ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, 3, 0.15), + ProcessorMotion("DIFF DRIVE", 0.15, 3, 3, 3, 2, 3), unmeasured_perturbation_cov_(Matrix3s::Identity()* params.unmeasured_perturbation_std_* params.unmeasured_perturbation_std_),