diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h index d06ace3a796002e87d33a2e9546ad280bea49416..f790a24839bb6c63f1a09703fe283a6f48c11949 100644 --- a/demos/hello_wolf/factor_bearing.h +++ b/demos/hello_wolf/factor_bearing.h @@ -36,18 +36,28 @@ namespace wolf using namespace Eigen; -class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2> +class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2> { public: - FactorBearing(const LandmarkBasePtr& _landmark_other_ptr, + FactorBearing(const FeatureBasePtr& _feature_ptr, + const LandmarkBasePtr& _landmark_other_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status) : - FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr, - _landmark_other_ptr, _processor_ptr, - _apply_loss_function, _status, - getCapture()->getFrame()->getP(), - getCapture()->getFrame()->getO(), - _landmark_other_ptr->getP()) + FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING", + TOP_LMK, + _feature_ptr, + nullptr, + nullptr, + nullptr, + _landmark_other_ptr, + _processor_ptr, + _apply_loss_function, + _status, + _feature_ptr->getCapture()->getFrame()->getP(), + _feature_ptr->getCapture()->getFrame()->getO(), + _feature_ptr->getCapture()->getSensor()->getP(), + _feature_ptr->getCapture()->getSensor()->getO(), + _landmark_other_ptr->getP()) { // } @@ -58,9 +68,11 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2> } template<typename T> - bool operator ()(const T* const _p1, - const T* const _o1, - const T* const _p2, + bool operator ()(const T* const _p_w_r, + const T* const _o_w_r, + const T* const _p_r_s, // sensor position + const T* const _o_r_s, // sensor orientation + const T* const _lmk, T* _res) const; }; @@ -73,24 +85,30 @@ namespace wolf { template<typename T> -inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, - const T* const _p2, T* _res) const +inline bool FactorBearing::operator ()( const T* const _p_w_r, + const T* const _o_w_r, + const T* const _p_r_s, // sensor position + const T* const _o_r_s, // sensor orientation + const T* const _lmk, + T* _res) const { // 1. produce a transformation matrix to transform from robot frame to world frame - Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform + Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform + Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform + // Map input pointers into meaningful Eigen elements - Map<const Matrix<T, 2, 1>> point_w(_p2); + Map<const Matrix<T, 2, 1>> point_w(_lmk); Map<const Matrix<T, 1, 1>> res(_res); - // 2. Transform world point to robot-referenced point - Matrix<T, 2, 1> point_r = H_w_r.inverse() * point_w; + // 2. Transform world point to sensor-referenced point + Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w; // 3. Get the expected bearing of the point - T bearing = atan2(point_r(1), point_r(0)); + T bearing = atan2(point_s(1), point_s(0)); // 4. Get the measured range-and-bearing to the point - Matrix<T, 2, 1> range_bearing = getMeasurement(); + Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>(); // 5. Get the bearing error by comparing against the bearing measurement T er = range_bearing(1) - bearing; @@ -100,7 +118,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, er -= T(2*M_PI); // 6. Compute the residual by scaling according to the standard deviation of the bearing part - *_res = er * T(getMeasurementSquareRootInformationUpper()(1,1)); + *_res = er * getMeasurementSquareRootInformationUpper()(1,1); return true; } diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h index 9a6e84d9c8949b4c9d5c5e69cdf3ec071d66fa57..d7bd26d00df45c7f14d0ff5ae01966e271af03ef 100644 --- a/include/core/processor/motion_provider.h +++ b/include/core/processor/motion_provider.h @@ -80,7 +80,7 @@ class MotionProvider void setStatePriority(int); public: - const StateStructure& getStateStructure ( ) { return state_structure_; }; + const StateStructure& getStateStructure ( ) const { return state_structure_; }; void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; }; void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index d93c9baa367b8ba65374a4b64d21bab194ce1d2a..b0683aba04b3e5d8fe041775199823436c728655 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -136,7 +136,7 @@ class ProcessorTracker : public ProcessorBase ParamsProcessorTrackerPtr _params_tracker); ~ProcessorTracker() override; - StateStructure getStateStructure() const; + const StateStructure& getStateStructure() const; virtual CaptureBaseConstPtr getOrigin() const; virtual CaptureBasePtr getOrigin(); @@ -313,7 +313,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming() return new_features_incoming_; } -inline StateStructure ProcessorTracker::getStateStructure ( ) const +inline const StateStructure& ProcessorTracker::getStateStructure ( ) const { return state_structure_; }