Resolve "New factorRelativePose2DWithExtrinsics"

Closes #237 (closed)

include/core/factor/factor_relative_pose_2d_with_extrinsics.h

+#ifndef FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
+
+//class
+class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
+{
+    public:
+        FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+                         const FrameBasePtr& _frame_other_ptr,
+                         const ProcessorBasePtr& _processor_ptr = nullptr,
+                         bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+             FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+                                                                 _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->getCapture()->getSensorP(),
+                                                                 _ftr_ptr->getCapture()->getSensorO())
+        {
+            //
+        }
+
+        virtual ~FactorRelativePose2dWithExtrinsics() = default;
+
+        template<typename T>
+        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:
+        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
+        {
+            return std::make_shared<FactorRelativePose2dWithExtrinsics>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+        }
+
+};
+
+template<typename T>
+inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+                                          const T* const _o2, const T* const _ps, const T* const _os, 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> > ps(_ps);
+    T o1 = _o1[0];
+    T o2 = _o2[0];
+    T os = _os[0];
+
+    Eigen::Matrix<T, 3, 1> expected_measurement;
+    Eigen::Matrix<T, 3, 1> er; // error
+
+    // Expected measurement
+    // r1_p_r1_s1 = ps
+    // r2_p_r2_s2 = ps
+    // r1_R_s1 = R(os)
+    // r2_R_s2 = R(os)
+    // w_R_r1 = R(o1)
+    // w_R_r2 = R(o2)
+    // ----------------------------------------
+
+    // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
+    //              s1_R_r1*r1_R_w*w_p_r1_w +
+    //              s1_R_r1*r1_R_w*w_p_w_r2 +
+    //              s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
+
+    // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
+
+    expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
+    expected_measurement(2) = o2 - o1;
+
+    // Error
+    er = expected_measurement - getMeasurement().cast<T>();
+    while (er(2) > T( M_PI ))
+        er(2) -=   T( 2 * M_PI );
+    while (er(2) < T( -M_PI ))
+        er(2) +=   T( 2 * M_PI );
+
+    // 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::MatrixXs J(3,6);
+//    J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
+//    J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
+//    J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
+//    J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
+//    J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
+//    J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
+//    if (sizeof(er(0)) != sizeof(double))
+//    {
+//        std::cout << "FactorRelativePose2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
+//        std::cout << "FactorRelativePose2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+//        std::cout << "FactorRelativePose2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+//    }
+    ////////////////////////////////////////////////////////
+
+    return true;
+}
+
+} // namespace wolf
+
+#endif