Make FactorOdom2dCloseloop inherit from FactorRelativePose2d

include/core/factor/factor_odom_2d_closeloop.h

@@ -2,103 +2,41 @@
 #define FACTOR_ODOM_2d_CLOSELOOP_H_
 
 //Wolf includes
-#include "core/factor/factor_autodiff.h"
-#include "core/frame/frame_base.h"
-#include "core/math/rotations.h"
-
-//#include "ceres/jet.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include <Eigen/StdVector>
 
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(FactorOdom2dCloseloop);
 
 //class
-class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>
+class FactorOdom2dCloseloop : public FactorRelativePose2d
 {
     public:
         FactorOdom2dCloseloop(const FeatureBasePtr& _ftr_ptr,
-                         const FrameBasePtr& _frame_other_ptr,
-                         const ProcessorBasePtr& _processor_ptr = nullptr,
-                         bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
-             FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>("FactorOdom2dCloseloop",
-                                                                  _ftr_ptr,
-                                                                  _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())
+                             const FrameBasePtr& _frame_ptr,
+                             const ProcessorBasePtr& _processor_ptr,
+                             bool _apply_loss_function,
+                             FactorStatus _status = FAC_ACTIVE) :
+            FactorRelativePose2d("FactorOdom2dCloseloop",
+                                     _ftr_ptr,
+                                     _frame_ptr,
+                                     _processor_ptr,
+                                     _apply_loss_function,
+                                     _status)
         {
             //
         }
 
-        virtual ~FactorOdom2dCloseloop() = default;
+        ~FactorOdom2dCloseloop() override = default;
 
         std::string getTopology() const override
         {
             return std::string("LOOP");
         }
 
-
-        template<typename T>
-        bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
-                         T* _residuals) const;
-
-    public:
-        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
-        {
-            return std::make_shared<FactorOdom2dCloseloop>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
-        }
-
 };
 
-template<typename T>
-inline bool FactorOdom2dCloseloop::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
-                                          const T* const _o2, 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);
-    T o1 = _o1[0];
-    T o2 = _o2[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(2) = o2 - o1;
-
-    // Error
-    er = expected_measurement - getMeasurement().cast<T>();
-    er(2)=pi2pi(er(2));
-
-    // Residuals
-    res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
-
-    ////////////////////////////////////////////////////////
-    // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
-//    using ceres::Jet;
-//    Eigen::MatrixXd J(3,6);
-//    J.row(0) = ((Jet<double, 6>)(er(0))).v;
-//    J.row(1) = ((Jet<double, 6>)(er(1))).v;
-//    J.row(2) = ((Jet<double, 6>)(er(2))).v;
-//    J.row(0) = ((Jet<double, 6>)(res(0))).v;
-//    J.row(1) = ((Jet<double, 6>)(res(1))).v;
-//    J.row(2) = ((Jet<double, 6>)(res(2))).v;
-//    if (sizeof(er(0)) != sizeof(double))
-//    {
-//        std::cout << "FactorOdom2dCloseloop::Jacobian(c" << id() << ") = \n " << J << std::endl;
-//        std::cout << "FactorOdom2dCloseloop::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-//        std::cout << "FactorOdom2dCloseloop::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
-//    }
-    ////////////////////////////////////////////////////////
-
-    return true;
-}
-
 } // namespace wolf
 
 #endif