Skip to content
Snippets Groups Projects
Commit 9be9bce9 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Simplify code

parent 9b1ebf16
No related branches found
No related tags found
1 merge request!144Imu improvements
...@@ -70,7 +70,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) ...@@ -70,7 +70,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
status_ = RUNNING; status_ = RUNNING;
} }
incoming_ptr_ = getIncomingCaptureMotion(_incoming_ptr); incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
/// @todo Anything else to do ? /// @todo Anything else to do ?
if (incoming_ptr_ == nullptr) return; if (incoming_ptr_ == nullptr) return;
...@@ -402,11 +402,6 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) ...@@ -402,11 +402,6 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
} }
} }
CaptureMotionPtr ProcessorMotion::getIncomingCaptureMotion(CaptureBasePtr& _incoming_ptr)
{
return std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
}
CaptureMotionPtr ProcessorMotion::getCaptureMotionContainingTimeStamp(const TimeStamp& _ts) CaptureMotionPtr ProcessorMotion::getCaptureMotionContainingTimeStamp(const TimeStamp& _ts)
{ {
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
......
...@@ -131,17 +131,13 @@ class ProcessorMotion : public ProcessorBase ...@@ -131,17 +131,13 @@ class ProcessorMotion : public ProcessorBase
* \param _x the returned state vector * \param _x the returned state vector
*/ */
void getCurrentState(Eigen::VectorXs& _x); void getCurrentState(Eigen::VectorXs& _x);
void getCurrentTimeStamp(TimeStamp& _ts){ _ts = getBuffer().get().back().ts_; }
/** \brief Get a constant reference to the state integrated so far /** \brief Get the state integrated so far
* \return the state vector * \return the state vector
*/ */
Eigen::VectorXs getCurrentState(); Eigen::VectorXs getCurrentState();
wolf::TimeStamp getCurrentTimeStamp();
/** \brief Fill a reference to the state integrated so far and its stamp
* \param _x the returned state vector
* \param _ts the returned stamp
*/
void getCurrentStateAndStamp(Eigen::VectorXs& _x, TimeStamp& _ts);
/** \brief Fill the state corresponding to the provided time-stamp /** \brief Fill the state corresponding to the provided time-stamp
* \param _ts the time stamp * \param _ts the time stamp
...@@ -225,14 +221,6 @@ class ProcessorMotion : public ProcessorBase ...@@ -225,14 +221,6 @@ class ProcessorMotion : public ProcessorBase
*/ */
virtual void postProcess() { }; virtual void postProcess() { };
/**
* @brief Get the incoming CaptureBasePtr and returns a CaptureMotionPtr out of it.
* If not overloaded, the base class calls
* std::static_pointer_cast<CaptureMotion>(_incoming_ptr)
* @return CaptureMotionPtr.
*/
virtual CaptureMotionPtr getIncomingCaptureMotion(CaptureBasePtr& _incoming_ptr);
// These are the pure virtual functions doing the mathematics // These are the pure virtual functions doing the mathematics
protected: protected:
...@@ -433,8 +421,6 @@ class ProcessorMotion : public ProcessorBase ...@@ -433,8 +421,6 @@ class ProcessorMotion : public ProcessorBase
Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
private:
wolf::TimeStamp getCurrentTimeStamp();
}; };
} }
...@@ -509,12 +495,6 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x) ...@@ -509,12 +495,6 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
statePlusDelta(origin_ptr_->getFramePtr()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x); statePlusDelta(origin_ptr_->getFramePtr()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
} }
inline void ProcessorMotion::getCurrentStateAndStamp(Eigen::VectorXs& _x, TimeStamp& _ts)
{
getCurrentState(_x);
_ts = getCurrentTimeStamp();
}
inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov() inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov()
{ {
return getBuffer().get().back().delta_integr_cov_; return getBuffer().get().back().delta_integr_cov_;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment