diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h new file mode 100644 index 0000000000000000000000000000000000000000..59e73405f9ae75c3aa30795149c7e64f244b51bf --- /dev/null +++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h @@ -0,0 +1,119 @@ +#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