Make factor_bearing accept sensor extrinsics

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;
 }