From 9b59f5d54f848710b8d7d0f3307e824546cd61fd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 15 Jan 2021 21:42:14 +0100
Subject: [PATCH] Make FactorOdom2dCloseloop inherit from FactorRelativePose2d
---
.../core/factor/factor_odom_2d_closeloop.h | 90 +++----------------
1 file changed, 14 insertions(+), 76 deletions(-)
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
index 16f1d6770..bee96e573 100644
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ b/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
--
GitLab