wip

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,7 +49,7 @@ 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

include/core/feature/feature_odom_2D.h

@@ -3,15 +3,13 @@
 
 //Wolf includes
 #include "core/feature/feature_base.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/factor/factor_odom_2D_analytic.h"
 
 //std includes
 
 namespace wolf {
 
 WOLF_PTR_TYPEDEFS(FeatureOdom2D);
-    
+
 //class FeatureOdom2D
 class FeatureOdom2D : public FeatureBase
 {
@@ -28,13 +26,13 @@ class FeatureOdom2D : public FeatureBase
         virtual ~FeatureOdom2D();
 
         /** \brief Generic interface to find factors
-         * 
+         *
          * TBD
          * Generic interface to find factors between this feature and a map (static/slam) or a previous feature
          *
          **/
         virtual void findFactors();
-        
+
 };
 
 } // namespace wolf