From 9c3b4facbb5aeee39da7fecb4f1c75c080814bd9 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 6 May 2020 15:45:24 +0200
Subject: [PATCH] Refactor factor_odom_2d
---
demos/demo_analytic_autodiff_factor.cpp | 2 +-
include/core/factor/factor_odom_2d.h | 97 ++++------------
include/core/factor/factor_odom_2d_analytic.h | 51 ---------
...ive_2d_analytic.h => factor_relative_2d.h} | 24 ++--
include/core/feature/feature_odom_2d.h | 2 +-
test/CMakeLists.txt | 2 +-
test/dummy/factor_odom_2d_autodiff.h | 104 ++++++++++++++++++
test/gtest_factor_autodiff.cpp | 7 +-
test/gtest_factor_relative_2d_analytic.cpp | 10 +-
9 files changed, 150 insertions(+), 149 deletions(-)
delete mode 100644 include/core/factor/factor_odom_2d_analytic.h
rename include/core/factor/{factor_relative_2d_analytic.h => factor_relative_2d.h} (92%)
create mode 100644 test/dummy/factor_odom_2d_autodiff.h
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 6325f785b..f6467738b 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -255,7 +255,7 @@ int main(int argc, char** argv)
FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
capture_ptr_analytic->addFeature(feature_ptr_analytic);
- FactorOdom2dAnalytic* factor_ptr_analytic = new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
+ FactorOdom2d* factor_ptr_analytic = new FactorOdom2d(feature_ptr_analytic, frame_old_ptr_analytic);
feature_ptr_analytic->addFactor(factor_ptr_analytic);
//std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
//std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index 1ce2a375c..a4bbef28b 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -1,34 +1,29 @@
-#ifndef FACTOR_ODOM_2d_THETA_H_
-#define FACTOR_ODOM_2d_THETA_H_
+#ifndef FACTOR_ODOM_2d_H_
+#define FACTOR_ODOM_2d_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_2d.h"
+#include <Eigen/StdVector>
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorOdom2d);
-
+
//class
-class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
+class FactorOdom2d : public FactorRelative2d
{
public:
FactorOdom2d(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>("FactorOdom2d",
- _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) :
+ FactorRelative2d("FactorOdom2d",
+ _ftr_ptr,
+ _frame_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
@@ -40,65 +35,17 @@ class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
return std::string("MOTION");
}
-
- 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)
+ public:
+// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
+// const NodeBasePtr& _correspondant_ptr,
+// const ProcessorBasePtr& _processor_ptr = nullptr)
// {
-// return std::make_shared<FactorOdom2d>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+// return std::make_shared<FactorOdom2d>(_feature_ptr,
+// std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
// }
};
-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
-{
-
- // 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();
- er(2) = pi2pi(er(2));
-
- // Residuals
- res = getMeasurementSquareRootInformationUpper() * 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 << "FactorOdom2d::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2d::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2d::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
-// }
- ////////////////////////////////////////////////////////
-
- return true;
-}
-
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_odom_2d_analytic.h b/include/core/factor/factor_odom_2d_analytic.h
deleted file mode 100644
index 52ea04ccd..000000000
--- a/include/core/factor/factor_odom_2d_analytic.h
+++ /dev/null
@@ -1,51 +0,0 @@
-#ifndef FACTOR_ODOM_2d_ANALYTIC_H_
-#define FACTOR_ODOM_2d_ANALYTIC_H_
-
-//Wolf includes
-#include "core/factor/factor_relative_2d_analytic.h"
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorOdom2dAnalytic);
-
-//class
-class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
-{
- public:
- FactorOdom2dAnalytic(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2dAnalytic("FactorOdom2dAnalytic",
- _ftr_ptr,
- _frame_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status)
- {
- //
- }
-
- virtual ~FactorOdom2dAnalytic() = default;
-
- virtual std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
- public:
-// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
-// const NodeBasePtr& _correspondant_ptr,
-// const ProcessorBasePtr& _processor_ptr = nullptr)
-// {
-// return std::make_shared<FactorOdom2dAnalytic>(_feature_ptr,
-// std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
-// }
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_2d.h
similarity index 92%
rename from include/core/factor/factor_relative_2d_analytic.h
rename to include/core/factor/factor_relative_2d.h
index 2dba90bc6..08a7ae86e 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_2d.h
@@ -1,5 +1,5 @@
-#ifndef FACTOR_RELATIVE_2d_ANALYTIC_H_
-#define FACTOR_RELATIVE_2d_ANALYTIC_H_
+#ifndef FACTOR_RELATIVE_2d_H_
+#define FACTOR_RELATIVE_2d_H_
//Wolf includes
#include "core/factor/factor_analytic.h"
@@ -9,16 +9,16 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorRelative2dAnalytic);
+WOLF_PTR_TYPEDEFS(FactorRelative2d);
//class
-class FactorRelative2dAnalytic : public FactorAnalytic
+class FactorRelative2d : public FactorAnalytic
{
public:
/** \brief Constructor of category FAC_FRAME
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelative2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -31,7 +31,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelative2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -44,7 +44,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelative2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -60,7 +60,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
return std::string("GEOM");
}
- virtual ~FactorRelative2dAnalytic() = default;
+ virtual ~FactorRelative2d() = default;
/** \brief Returns the factor residual size
**/
@@ -103,7 +103,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/// IMPLEMENTATION ///
-inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
+inline Eigen::VectorXd FactorRelative2d::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
Eigen::VectorXd residual(3);
@@ -119,7 +119,7 @@ inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
return residual;
}
-inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+inline void FactorRelative2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
@@ -155,7 +155,7 @@ inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen:
}
}
-inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+inline void FactorRelative2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
@@ -191,7 +191,7 @@ inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen:
}
}
-inline void FactorRelative2dAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+inline void FactorRelative2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 4);
diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h
index 9ef3059cd..764e9ca7b 100644
--- a/include/core/feature/feature_odom_2d.h
+++ b/include/core/feature/feature_odom_2d.h
@@ -4,7 +4,7 @@
//Wolf includes
#include "core/feature/feature_base.h"
#include "core/factor/factor_odom_2d.h"
-#include "core/factor/factor_odom_2d_analytic.h"
+#include "core/factor/factor_odom_2d.h"
//std includes
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index b0309585c..eea3f9274 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -201,7 +201,7 @@ target_link_libraries(gtest_factor_pose_3d ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PROJECT_NAME})
-# FactorRelative2dAnalytic class test
+# FactorRelative2d class test
wolf_add_gtest(gtest_factor_relative_2d_analytic gtest_factor_relative_2d_analytic.cpp)
target_link_libraries(gtest_factor_relative_2d_analytic ${PROJECT_NAME})
diff --git a/test/dummy/factor_odom_2d_autodiff.h b/test/dummy/factor_odom_2d_autodiff.h
new file mode 100644
index 000000000..0c5f87264
--- /dev/null
+++ b/test/dummy/factor_odom_2d_autodiff.h
@@ -0,0 +1,104 @@
+#ifndef FACTOR_ODOM_2d_autodiff_H_
+#define FACTOR_ODOM_2d_autodiff_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorOdom2dAutodiff);
+
+//class
+class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1>
+{
+ public:
+ FactorOdom2dAutodiff(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1, 2, 1>("FactorOdom2dAutodiff",
+ _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())
+ {
+ //
+ }
+
+ virtual ~FactorOdom2dAutodiff() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("MOTION");
+ }
+
+
+ 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<FactorOdom2dAutodiff>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+// }
+
+};
+
+template<typename T>
+inline bool FactorOdom2dAutodiff::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();
+ er(2) = pi2pi(er(2));
+
+ // Residuals
+ res = getMeasurementSquareRootInformationUpper() * 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 << "FactorOdom2dAutodiff::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2dAutodiff::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2dAutodiff::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index b2b28b32d..589488e1f 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -11,10 +11,11 @@
#include "core/capture/capture_void.h"
#include "core/feature/feature_odom_2d.h"
#include "core/factor/factor_odom_2d.h"
-#include "core/factor/factor_odom_2d_analytic.h"
+#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_autodiff.h"
#include "dummy/factor_dummy_zero_1.h"
#include "dummy/factor_dummy_zero_12.h"
+#include "dummy/factor_odom_2d_autodiff.h"
using namespace wolf;
using namespace Eigen;
@@ -519,8 +520,8 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
- auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2dAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
index fd8e2296c..97e1ab832 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -1,6 +1,6 @@
#include "core/utils/utils_gtest.h"
-#include "core/factor/factor_relative_2d_analytic.h"
+#include "core/factor/factor_relative_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
@@ -25,14 +25,14 @@ FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
-auto fac1 = FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false); // create and add
+auto fac1 = FactorBase::emplace<FactorRelative2d>(fea1, "odom2d", fea1, frm0, nullptr, false); // create and add
-TEST(FactorRelative2dAnalytic, check_tree)
+TEST(FactorRelative2d, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorRelative2dAnalytic, fix_0_solve)
+TEST(FactorRelative2d, fix_0_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -67,7 +67,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
}
}
-TEST(FactorRelative2dAnalytic, fix_1_solve)
+TEST(FactorRelative2d, fix_1_solve)
{
for (int i = 0; i < 1e3; i++)
{
--
GitLab