From eca1a318a9b74f37d8ee0ade17bdeeca2e4464d6 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 28 Jul 2023 15:02:34 +0200
Subject: [PATCH] PB checks for timetolerance and PM dealing keys
---
include/core/composite/vector_composite.h | 2 +
include/core/processor/motion_provider.h | 6 +-
include/core/processor/processor_base.h | 33 +++--
include/core/processor/processor_motion.h | 12 +-
src/composite/vector_composite.cpp | 16 +++
src/problem/problem.cpp | 51 -------
src/processor/processor_base.cpp | 27 ++++
src/processor/processor_motion.cpp | 129 +++++++++++-------
.../dummy/processor_motion_provider_dummy.cpp | 4 +-
test/dummy/processor_motion_provider_dummy.h | 4 +-
.../processor_motion_provider_dummy_pov.cpp | 4 +-
.../processor_motion_provider_dummy_pov.h | 4 +-
test/gtest_processor_motion.cpp | 1 +
13 files changed, 163 insertions(+), 130 deletions(-)
diff --git a/include/core/composite/vector_composite.h b/include/core/composite/vector_composite.h
index 84ae27858..89411fef3 100644
--- a/include/core/composite/vector_composite.h
+++ b/include/core/composite/vector_composite.h
@@ -76,6 +76,8 @@ class VectorComposite : public Composite<Eigen::VectorXd>
void setZero();
+ VectorComposite operator()(const std::string& _keys) const;
+
friend std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x);
};
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index 49d1651b3..6f860c6a4 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -38,9 +38,9 @@ class MotionProvider
virtual ~MotionProvider();
// Queries to the processor:
- virtual TimeStamp getTimeStamp() const = 0;
- virtual VectorComposite getState(const StateKeys& _structure = "") const = 0;
- virtual VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const = 0;
+ virtual TimeStamp getTimeStamp() const = 0;
+ virtual VectorComposite getState(StateKeys _structure = "") const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const = 0;
VectorComposite getOdometry() const;
void setOdometry(const VectorComposite&);
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c5fe7318e..8bd73f046 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -244,7 +244,26 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
SensorBaseWPtr sensor_ptr_;
static unsigned int processor_id_count_;
+ // CHECKING captures period and time_tolerance
+ double capture_period_max_, capture_period_min_, capture_period_mean_;
+ TimeStamp prev_capture_stamp_;
+
public:
+ // PROFILING
+ unsigned int n_capture_callback_;
+ unsigned int n_kf_callback_;
+ std::chrono::microseconds acc_duration_capture_;
+ std::chrono::microseconds acc_duration_kf_;
+ std::chrono::microseconds max_duration_capture_;
+ std::chrono::microseconds max_duration_kf_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_;
+ std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_;
+ void startCaptureProfiling();
+ void stopCaptureProfiling();
+ void startKFProfiling();
+ void stopKFProfiling();
+ void printProfiling(std::ostream& stream = std::cout) const;
+
/** \brief constructor
*
* \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: type name
@@ -260,20 +279,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
unsigned int id() const;
- // PROFILING
- unsigned int n_capture_callback_;
- unsigned int n_kf_callback_;
- std::chrono::microseconds acc_duration_capture_;
- std::chrono::microseconds acc_duration_kf_;
- std::chrono::microseconds max_duration_capture_;
- std::chrono::microseconds max_duration_kf_;
- std::chrono::time_point<std::chrono::high_resolution_clock> start_capture_;
- std::chrono::time_point<std::chrono::high_resolution_clock> start_kf_;
- void startCaptureProfiling();
- void stopCaptureProfiling();
- void startKFProfiling();
- void stopKFProfiling();
- void printProfiling(std::ostream& stream = std::cout) const;
protected:
/** \brief process an incoming capture
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 503a9357a..155085b2f 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -127,7 +127,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
ProcessingStep processing_step_; ///< State machine controlling the processing step
bool bootstrapping_; ///< processor is bootstrapping
- // This is the main public interface
+ // This is the main public interface
public:
ProcessorMotion(const std::string& _type,
TypeComposite _state_types,
@@ -145,8 +145,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
// Queries to the processor:
TimeStamp getTimeStamp() const override;
- VectorComposite getState(const StateKeys& _structure = "") const override;
- VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override;
+ VectorComposite getState(StateKeys _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
@@ -522,7 +522,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
mutable Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
mutable Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
- Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
+ Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
} // namespace wolf
@@ -579,8 +579,8 @@ inline MotionBuffer& ProcessorMotion::getBuffer()
inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
{
return Motion(_ts,
- VectorXd::Zero(data_size_), // data
- Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
+ VectorXd::Zero(data_size_), // data
+ Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
deltaZero(),
Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
deltaZero(),
diff --git a/src/composite/vector_composite.cpp b/src/composite/vector_composite.cpp
index 68c13b490..0a9789452 100644
--- a/src/composite/vector_composite.cpp
+++ b/src/composite/vector_composite.cpp
@@ -80,6 +80,22 @@ Eigen::VectorXd VectorComposite::vector(const StateKeys& _keys) const
return x;
}
+VectorComposite VectorComposite::operator()(const std::string& _keys) const
+{
+ if (not this->has(_keys))
+ {
+ throw std::runtime_error("VectorComposite::operator() required keys " + _keys + " are not available, only have " +
+ getKeys());
+ }
+
+ VectorComposite output;
+ for (auto key : _keys)
+ {
+ output.emplace(key, this->at(key));
+ }
+ return output;
+}
+
std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x)
{
for (auto&& pair : _x)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 04fd8b9ea..3bec18875 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1298,57 +1298,6 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
FrameBasePtr first_frame = emplaceFrame(_ts, prior_options_);
first_frame->emplacePriors(prior_options_);
- // CaptureBasePtr prior_cap(nullptr);
-
- // for (auto prior_pair : prior_options_)
- // {
- // auto key = prior_pair.first;
- // auto sb = first_frame->getStateBlock(key);
-
- // // Fix
- // if (prior_pair.second.isFixed())
- // {
- // sb->fix();
- // }
-
- // // Factor
- // else if (prior_pair.second.isFactor())
- // {
- // // Emplace a capture (if not done already)
- // if (not prior_cap)
- // prior_cap = CaptureBase::emplace<CaptureVoid>(first_frame, _ts, nullptr);
-
- // // Emplace a feature
- // Eigen::MatrixXd cov = prior_pair.second.getNoiseStd().cwiseAbs2().asDiagonal();
- // if(cov.rows() != sb->getLocalSize())
- // {
- // throw std::runtime_error("Problem::applyPriorOptions: covariance matrix has wrong size");
- // }
- // auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap,
- // "Prior " + std::string(1,key),
- // prior_pair.second.getState(),
- // cov);
-
- // // Emplace a factor
- // if (sb->hasLocalParametrization())
- // {
- // if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
- // auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, prior_fea, sb,
- // nullptr, false);
- // else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
- // auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr,
- // false);
- // else
- // throw std::runtime_error("Absolute factor not implemented for state of type " +
- // prior_pair.second.getType());
- // }
- // else
- // {
- // auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
- // }
- // }
- // }
-
// notify all processors
keyFrameCallback(first_frame, nullptr);
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index b13eae3e0..cff10a30e 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -33,6 +33,10 @@ ProcessorBase::ProcessorBase(const std::string& _type, int _dim_compatible, cons
params_(Clone(_params)),
dim_compatible_(_dim_compatible),
sensor_ptr_(),
+ capture_period_max_(0),
+ capture_period_min_(1e12),
+ capture_period_mean_(0),
+ prev_capture_stamp_(TimeStamp::Invalid()),
n_capture_callback_(0),
n_kf_callback_(0),
acc_duration_capture_(0),
@@ -94,6 +98,29 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture)
n_capture_callback_++;
startCaptureProfiling();
+ // CHECKING captures period and time_tolerance
+ if (n_capture_callback_ > 1)
+ {
+ assert(prev_capture_stamp_.ok());
+
+ double dt = _capture->getTimeStamp() - prev_capture_stamp_;
+ capture_period_mean_ = ((n_capture_callback_ - 2) * capture_period_mean_ + dt) / (n_capture_callback_ - 1);
+ capture_period_min_ = std::min(dt, capture_period_min_);
+ capture_period_max_ = std::max(dt, capture_period_max_);
+
+ WOLF_ERROR_COND(capture_period_mean_ < getTimeTolerance() * 2,
+ "Processor '",
+ getName(),
+ "' (",
+ getType(),
+ ") has a wrong 'time_tolerance' value: ",
+ getTimeTolerance(),
+ "s! It should be <= ",
+ capture_period_mean_ / 2,
+ "(half of the sensor period)");
+ }
+ prev_capture_stamp_ = _capture->getTimeStamp();
+
// apply prior in problem if not done (very first capture)
if (getProblem() and not getProblem()->isPriorApplied()) getProblem()->applyPriorOptions(_capture->getTimeStamp());
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index adcb6aac9..ff7ea2052 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -345,7 +345,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// will be emplaced
capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
- assert(capture_existing->getFeatureList().empty()); // there was only one feature!
+ assert(capture_existing->getFeatureList().empty()); // there was only one feature!
emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing);
}
@@ -494,7 +494,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
emplaceFeaturesAndFactors(origin_ptr_, last_ptr_);
// create a new frame
- auto frame_new = std::make_shared<FrameBase>(getTimeStamp(), getStateTypes(), getProblem()->getState());
+ auto frame_new =
+ std::make_shared<FrameBase>(getTimeStamp(), getStateTypes(), getProblem()->getState(getStateKeys()));
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
@@ -525,14 +526,32 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
-VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
+VectorComposite ProcessorMotion::getState(StateKeys _keys) const
{
- const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys);
+ // Check that we have all requested keys
+ if (not getStateTypes().has(_keys))
+ {
+ std::string has_keys("");
+ for (auto key : _keys)
+ if (getStateTypes().has(key)) has_keys.push_back(key);
+
+ WOLF_DEBUG("Processor has no state for all keys '",
+ _keys,
+ "'. Returning a VectorComposite only with the available ones: '",
+ has_keys,
+ "'.");
+
+ // override _keys without the ones that are missing
+ _keys = has_keys;
+
+ // check if no keys left
+ if (_keys.empty()) return VectorComposite(); // return empty state
+ }
+ // We do not have any info of where to find a valid state
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
if (origin_ptr_ == nullptr or origin_ptr_->isRemoving() or last_ptr_ == nullptr or
- last_ptr_->getFrame() ==
- nullptr) // We do not have any info of where to find a valid state
- // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
+ last_ptr_->getFrame() == nullptr)
{
WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
return VectorComposite(); // return empty state
@@ -544,7 +563,7 @@ VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
// this may happen when in the very first frame where the capture has no motion info --> empty buffer
if (last_ptr_->getBuffer().empty())
{
- return last_ptr_->getFrame()->getState(keys);
+ return last_ptr_->getFrame()->getState(_keys);
}
/* Doing this:
@@ -595,31 +614,38 @@ VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
}
+ // Return the requested keys
if (_keys == "")
- return state;
-
- else
{
- // remove states not requested by structure
- auto pair_key_vec_it = state.begin();
- while (pair_key_vec_it != state.end())
- {
- if (_keys.find(pair_key_vec_it->first) == std::string::npos)
- pair_key_vec_it = state.erase(pair_key_vec_it);
-
- else
- pair_key_vec_it++;
- }
return state;
}
+ return state(_keys);
}
// _x needs to have the size of the processor state
-VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys& _keys) const
+VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, StateKeys _keys) const
{
assert(_ts.ok());
- const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys);
+ // Check that we have all requested keys
+ if (not getStateTypes().has(_keys))
+ {
+ std::string has_keys("");
+ for (auto key : _keys)
+ if (getStateTypes().has(key)) has_keys.push_back(key);
+
+ WOLF_DEBUG("Processor has no state for all keys '",
+ _keys,
+ "'. Returning a VectorComposite only with the available ones: '",
+ has_keys,
+ "'.");
+
+ // override _keys without the ones that are missing
+ _keys = has_keys;
+
+ // check if no keys left
+ if (_keys.empty()) return VectorComposite(); // return empty state
+ }
// We need to search for the capture containing a motion buffer with the queried time stamp
auto capture_motion = findCaptureContainingTimeStamp(_ts);
@@ -632,11 +658,23 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
else // We found a CaptureMotion whose buffer contains the time stamp
{
+ assert(capture_motion->getOriginCapture() && "ProcessorMotion::getState null origin capture!");
+ assert(capture_motion->getOriginCapture()->getFrame() &&
+ "ProcessorMotion::getState null origin capture frame!");
+ assert(capture_motion->getOriginCapture()->getFrame()->has(getStateKeys()) &&
+ "ProcessorMotion::getState origin capture frame doesn't have necessary keys!");
+
// if buffer is empty --> we did not advance from origin!
// this may happen when in the very first frame where the capture has no motion info --> empty buffer
if (capture_motion->getBuffer().empty())
{
- return capture_motion->getFrame()->getState(keys);
+ return capture_motion->getOriginCapture()->getFrame()->getState(_keys);
+ }
+
+ // if _ts is at origin capture's stamp (+/- time tolerance), return the state of its frame
+ if (checkTimeTolerance(_ts, capture_motion->getOriginCapture()->getTimeStamp()))
+ {
+ return capture_motion->getOriginCapture()->getFrame()->getState(_keys);
}
/* Doing this:
@@ -652,7 +690,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
* (+) : block-wise plus
*/
- // Get state of origin
+ // Get state of origin (all processor keys to be able to integrate)
auto cap_orig = capture_motion->getOriginCapture();
auto x_origin = cap_orig->getFrame()->getState(getStateKeys());
@@ -663,13 +701,12 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
const auto& delta_preint = motion.delta_integr_;
// Get calibration preint -- stored in last capture
+ auto calib_preint = capture_motion->getCalibrationPreint();
// dt
double dt = _ts - cap_orig->getTimeStamp();
if (dt < 0)
{
- dt = 0;
- // ERROR POSSIBLE WRONG time_tolerance
WOLF_ERROR("ProcessorMotion::getState: negative dt!",
"\ttimestamp requested: ",
_ts,
@@ -681,11 +718,8 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
cap_orig->id(),
" ts = ",
cap_orig->getTimeStamp());
- WOLF_ERROR("THIS ERROR MAY BE CAUSED BY A WRONG 'time_tolerance' VALUE OF THIS PROCESSOR: ",
- getName(),
- "\nIt should be lower than 1/2 of the period of the captures arribal.");
+ throw std::runtime_error("ProcessorMotion::getState: negative dt!");
}
- auto calib_preint = capture_motion->getCalibrationPreint();
VectorComposite state;
@@ -716,27 +750,15 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys&
{
return state;
}
- else
- {
- // remove states not requested by structure
- auto pair_key_vec_it = state.begin();
- while (pair_key_vec_it != state.end())
- {
- if (_keys.find(pair_key_vec_it->first) == std::string::npos)
- pair_key_vec_it = state.erase(pair_key_vec_it);
-
- else
- pair_key_vec_it++;
- }
- return state;
- }
+ return state(_keys);
}
}
FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr =
- FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(), _ts_origin, getStateTypes(), _x_origin);
+ assert(_x_origin.has(getStateKeys()));
+ FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(
+ getProblem()->getTrajectory(), _ts_origin, getStateTypes(), _x_origin(getStateKeys()));
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -748,6 +770,17 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
assert(_origin_frame->getTrajectory() != nullptr &&
"ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
+ // add states to origin_frame if missing
+ if (not _origin_frame->has(getStateKeys()))
+ {
+ std::string missing_keys = "";
+ for (auto key : getStateKeys())
+ if (not _origin_frame->has(key)) missing_keys.push_back(key);
+
+ // add zero state unfixed for each missing key
+ _origin_frame->emplaceStateBlocks(getStateTypes()(missing_keys), getProblem()->stateZero(missing_keys), false);
+ }
+
TimeStamp origin_ts = _origin_frame->getTimeStamp();
// -------- ORIGIN ---------
@@ -766,7 +799,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts, getStateTypes(), _origin_frame->getState());
+ last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts, getStateTypes(), _origin_frame->getState(getStateKeys()));
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(last_frame_ptr_,
diff --git a/test/dummy/processor_motion_provider_dummy.cpp b/test/dummy/processor_motion_provider_dummy.cpp
index cf65d83dd..4e8047167 100644
--- a/test/dummy/processor_motion_provider_dummy.cpp
+++ b/test/dummy/processor_motion_provider_dummy.cpp
@@ -57,12 +57,12 @@ TimeStamp ProcessorMotionProviderDummy::getTimeStamp() const
return TimeStamp(0);
};
-VectorComposite ProcessorMotionProviderDummy::getState(const StateKeys& _structure) const
+VectorComposite ProcessorMotionProviderDummy::getState(StateKeys _structure) const
{
return getOdometry();
};
-VectorComposite ProcessorMotionProviderDummy::getState(const TimeStamp& _ts, const StateKeys& _structure) const
+VectorComposite ProcessorMotionProviderDummy::getState(const TimeStamp& _ts, StateKeys _structure) const
{
return getOdometry();
};
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index 173f95e28..3ccae0ffd 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -46,9 +46,9 @@ class ProcessorMotionProviderDummy : public ProcessorBase, public MotionProvider
bool voteForKeyFrame() const override;
TimeStamp getTimeStamp() const override;
- VectorComposite getState(const StateKeys& _structure = "") const override;
+ VectorComposite getState(StateKeys _structure = "") const override;
- VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const override;
};
} /* namespace wolf */
\ No newline at end of file
diff --git a/test/dummy/processor_motion_provider_dummy_pov.cpp b/test/dummy/processor_motion_provider_dummy_pov.cpp
index 4168da056..ac9481c82 100644
--- a/test/dummy/processor_motion_provider_dummy_pov.cpp
+++ b/test/dummy/processor_motion_provider_dummy_pov.cpp
@@ -57,12 +57,12 @@ TimeStamp ProcessorMotionProviderDummyPOV::getTimeStamp() const
return TimeStamp(0);
};
-VectorComposite ProcessorMotionProviderDummyPOV::getState(const StateKeys& _structure) const
+VectorComposite ProcessorMotionProviderDummyPOV::getState(StateKeys _structure) const
{
return getOdometry();
};
-VectorComposite ProcessorMotionProviderDummyPOV::getState(const TimeStamp& _ts, const StateKeys& _structure) const
+VectorComposite ProcessorMotionProviderDummyPOV::getState(const TimeStamp& _ts, StateKeys _structure) const
{
return getOdometry();
};
diff --git a/test/dummy/processor_motion_provider_dummy_pov.h b/test/dummy/processor_motion_provider_dummy_pov.h
index 5a33644c5..98d2cdd42 100644
--- a/test/dummy/processor_motion_provider_dummy_pov.h
+++ b/test/dummy/processor_motion_provider_dummy_pov.h
@@ -46,9 +46,9 @@ class ProcessorMotionProviderDummyPOV : public ProcessorBase, public MotionProvi
bool voteForKeyFrame() const override;
TimeStamp getTimeStamp() const override;
- VectorComposite getState(const StateKeys& _structure = "") const override;
+ VectorComposite getState(StateKeys _structure = "") const override;
- VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override;
+ VectorComposite getState(const TimeStamp& _ts, StateKeys _structure = "") const override;
};
} /* namespace wolf */
\ No newline at end of file
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index bf2338a89..ad245cf58 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -524,5 +524,6 @@ TEST_F(ProcessorMotion_test, integrateOdometry)
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
+ //testing::GTEST_FLAG(filter) = "ProcessorMotion_test.getState_time_structure";
return RUN_ALL_TESTS();
}
--
GitLab