Skip to content
Snippets Groups Projects
Commit 038ffdd6 authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

[skip-ci] Merge branch 'devel' into 451-refactoring-wolf-installation-system

parents 86e309d5 6ddd8682
No related branches found
No related tags found
2 merge requests!451After cmake and const refactor,!445Resolve "Refactoring WOLF installation system"
This commit is part of merge request !445. Comments created here will be created in the context of that merge request.
...@@ -36,18 +36,28 @@ namespace wolf ...@@ -36,18 +36,28 @@ namespace wolf
using namespace Eigen; using namespace Eigen;
class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2> class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
{ {
public: public:
FactorBearing(const LandmarkBasePtr& _landmark_other_ptr, FactorBearing(const FeatureBasePtr& _feature_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr, const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, FactorStatus _status) : bool _apply_loss_function, FactorStatus _status) :
FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr, FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING",
_landmark_other_ptr, _processor_ptr, TOP_LMK,
_apply_loss_function, _status, _feature_ptr,
getCapture()->getFrame()->getP(), nullptr,
getCapture()->getFrame()->getO(), nullptr,
_landmark_other_ptr->getP()) 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> ...@@ -58,9 +68,11 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
} }
template<typename T> template<typename T>
bool operator ()(const T* const _p1, bool operator ()(const T* const _p_w_r,
const T* const _o1, const T* const _o_w_r,
const T* const _p2, const T* const _p_r_s, // sensor position
const T* const _o_r_s, // sensor orientation
const T* const _lmk,
T* _res) const; T* _res) const;
}; };
...@@ -73,24 +85,30 @@ namespace wolf ...@@ -73,24 +85,30 @@ namespace wolf
{ {
template<typename T> template<typename T>
inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, inline bool FactorBearing::operator ()( const T* const _p_w_r,
const T* const _p2, T* _res) const 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 // 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 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); Map<const Matrix<T, 1, 1>> res(_res);
// 2. Transform world point to robot-referenced point // 2. Transform world point to sensor-referenced point
Matrix<T, 2, 1> point_r = H_w_r.inverse() * point_w; Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w;
// 3. Get the expected bearing of the point // 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 // 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 // 5. Get the bearing error by comparing against the bearing measurement
T er = range_bearing(1) - bearing; T er = range_bearing(1) - bearing;
...@@ -100,7 +118,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, ...@@ -100,7 +118,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
er -= T(2*M_PI); er -= T(2*M_PI);
// 6. Compute the residual by scaling according to the standard deviation of the bearing part // 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; return true;
} }
......
...@@ -80,7 +80,7 @@ class MotionProvider ...@@ -80,7 +80,7 @@ class MotionProvider
void setStatePriority(int); void setStatePriority(int);
public: 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 setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
......
...@@ -136,7 +136,7 @@ class ProcessorTracker : public ProcessorBase ...@@ -136,7 +136,7 @@ class ProcessorTracker : public ProcessorBase
ParamsProcessorTrackerPtr _params_tracker); ParamsProcessorTrackerPtr _params_tracker);
~ProcessorTracker() override; ~ProcessorTracker() override;
StateStructure getStateStructure() const; const StateStructure& getStateStructure() const;
virtual CaptureBaseConstPtr getOrigin() const; virtual CaptureBaseConstPtr getOrigin() const;
virtual CaptureBasePtr getOrigin(); virtual CaptureBasePtr getOrigin();
...@@ -313,7 +313,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming() ...@@ -313,7 +313,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_; return new_features_incoming_;
} }
inline StateStructure ProcessorTracker::getStateStructure ( ) const inline const StateStructure& ProcessorTracker::getStateStructure ( ) const
{ {
return state_structure_; return state_structure_;
} }
......
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