WIP: Resolve "New factorRelativePose2DWithExtrinsics"

Closes #237 (closed)

include/core/factor/factor_odom_2D.h

@@ -8,25 +8,27 @@
 //#include "ceres/jet.h"
 
 namespace wolf {
-    
+
 WOLF_PTR_TYPEDEFS(FactorOdom2D);
 
 //class
-class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
+class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1, 2, 1>
 {
     public:
         FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
                          const FrameBasePtr& _frame_other_ptr,
                          const ProcessorBasePtr& _processor_ptr = nullptr,
                          bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
-             FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
+             FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1, 2, 1>("ODOM 2D",
                                                                  _frame_other_ptr, nullptr, nullptr, nullptr,
                                                                  _processor_ptr,
                                                                  _apply_loss_function, _status,
                                                                  _frame_other_ptr->getP(),
                                                                  _frame_other_ptr->getO(),
                                                                  _ftr_ptr->getFrame()->getP(),
-                                                                 _ftr_ptr->getFrame()->getO())
+                                                                 _ftr_ptr->getFrame()->getO(),
+                                                                 _ftr_ptr->getCapture()->getSensorP(),
+                                                                 _ftr_ptr->getCapture()->getSensorO())
         {
             //
         }
@@ -34,7 +36,7 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
         virtual ~FactorOdom2D() = default;
 
         template<typename T>
-        bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
+        bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, const T* const _sp, const T* const _so,
                          T* _residuals) const;
 
     public:
@@ -47,20 +49,23 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
 
 template<typename T>
 inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
-                                          const T* const _o2, T* _residuals) const
+                                          const T* const _o2, const T* const _sp, const T* const _so, T* _residuals) const
 {
 
     // MAPS
     Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
     Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
     Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
+    Eigen::Map<const Eigen::Matrix<T,2,1> > sp(_sp);
     T o1 = _o1[0];
     T o2 = _o2[0];
+    T so = _so[0];
+
     Eigen::Matrix<T, 3, 1> expected_measurement;
     Eigen::Matrix<T, 3, 1> er; // error
 
     // Expected measurement
-    expected_measurement.head(2) = Eigen::Rotation2D<T>(-o1) * (p2 - p1);
+    expected_measurement.head(2) = Eigen::Rotation2D<T>(-so)*(Eigen::Rotation2D<T>(-o1)*(p2 + Eigen::Rotation2D<T>(o2)*sp - p1) - sp);
     expected_measurement(2) = o2 - o1;
 
     // Error