diff --git a/include/core/composite/vector_composite.h b/include/core/composite/vector_composite.h
index 84ae2785801172af3e6471015c65699b131c343c..89411fef396ca5ffa0b8d32bd7c1a4273916e59d 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 49d1651b3578155b90d0fe60f7fd50ad6d4d0468..6f860c6a48e97640fea9358d68d2f94f15a79978 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 c5fe7318e79c6ea48b68bf98ae6ebe9b2084f571..8bd73f046e2533d287e2a85115e152e73de6ca83 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 503a9357a14007afc17a438516c6b2612828800a..155085b2f8bb12c9e125f29d2f96c968cf02509d 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 68c13b4909330c717bd9c0ac5dead0805e6c2499..0a9789452ff859ebf991275d109114b7e560297d 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 04fd8b9ea1c2651eb9e022a3a9ac50bd0131f863..3bec18875e33a642696e754d83847913ef03073b 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 b13eae3e02196247bf1919546251b4d3a67e550c..cff10a30e7ac24b76a5a81ff2cee6249e75b39b8 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 adcb6aac985eeeef4160bf429097cedda855bb7d..ff7ea2052b055e8ceeb38502b4d8f9f6442bec89 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 cf65d83ddc42d4266b57e5c83dd4829a56518445..4e804716769fb2ed4b4cf6a9f8e9bb37a6ddffc3 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 173f95e28be1d26c598ee55340d74628b02cdd30..3ccae0ffd86f08c883b75680e6fc599446118343 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 4168da056bcc63e5ef4f27279c43a5a1ee4d019f..ac9481c820166dcacf9f2e9b932ce0b30c4520b3 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 5a33644c561d01615b09d797538c8d002d4621fb..98d2cdd42a417ad5bc9bb55496a8cde9957cd147 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 bf2338a895c3df5ca3a7ee816361e80ca44d0a58..ad245cf58afe5d46c3faf84095ce173f5af33dab 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();
}