diff --git a/CMakeLists.txt b/CMakeLists.txt
index 246dffbc6f04c9054d91d8f09fb0daadf5052d98..72e9ec382ee49ea65b893c323e2a2f154ec649ad 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -216,19 +216,18 @@ SET(HDRS_CAPTURE
SET(HDRS_FACTOR
include/core/factor/factor_analytic.h
include/core/factor/factor_autodiff.h
- include/core/factor/factor_autodiff_distance_3d.h
include/core/factor/factor_base.h
include/core/factor/factor_block_absolute.h
include/core/factor/factor_block_difference.h
include/core/factor/factor_diff_drive.h
+ include/core/factor/factor_distance_3d.h
+ include/core/factor/factor_loopclosure_2d.h
include/core/factor/factor_odom_2d.h
- include/core/factor/factor_odom_2d_closeloop.h
- include/core/factor/factor_odom_2d_analytic.h
include/core/factor/factor_odom_3d.h
include/core/factor/factor_pose_2d.h
include/core/factor/factor_pose_3d.h
include/core/factor/factor_quaternion_absolute.h
- include/core/factor/factor_relative_2d_analytic.h
+ include/core/factor/factor_relative_pose_2d.h
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
)
SET(HDRS_FEATURE
diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
index 203c38ef635cf02d6ded7aa0a0491169ef67a04d..2504d0b10bf669ececd7ce33e6ab9b15928eb26b 100644
--- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -12,7 +12,6 @@
#include "ceres/numeric_diff_cost_function.h"
// Factors
-#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_base.h"
namespace wolf {
diff --git a/include/core/factor/factor_autodiff_distance_3d.h b/include/core/factor/factor_distance_3d.h
similarity index 84%
rename from include/core/factor/factor_autodiff_distance_3d.h
rename to include/core/factor/factor_distance_3d.h
index bd0808978214570e80c02dae7e19a4ec5b96129c..d3f4ef7099e2f16def682de4abe7119dab4b688a 100644
--- a/include/core/factor/factor_autodiff_distance_3d.h
+++ b/include/core/factor/factor_distance_3d.h
@@ -5,20 +5,20 @@
* \author: jsola
*/
-#ifndef FACTOR_AUTODIFF_DISTANCE_3d_H_
-#define FACTOR_AUTODIFF_DISTANCE_3d_H_
+#ifndef FACTOR_DISTANCE_3d_H_
+#define FACTOR_DISTANCE_3d_H_
#include "core/factor/factor_autodiff.h"
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorAutodiffDistance3d);
+WOLF_PTR_TYPEDEFS(FactorDistance3d);
-class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d, 1, 3, 3>
+class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
{
public:
- FactorAutodiffDistance3d(const FeatureBasePtr& _feat,
+ FactorDistance3d(const FeatureBasePtr& _feat,
const FrameBasePtr& _frm_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
@@ -37,7 +37,7 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
{
}
- ~FactorAutodiffDistance3d() override { /* nothing */ }
+ ~FactorDistance3d() override { /* nothing */ }
std::string getTopology() const override
{
@@ -74,4 +74,4 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
} /* namespace wolf */
-#endif /* FACTOR_AUTODIFF_DISTANCE_3d_H_ */
+#endif /* FACTOR_DISTANCE_3d_H_ */
diff --git a/include/core/factor/factor_odom_2d_analytic.h b/include/core/factor/factor_loopclosure_2d.h
similarity index 62%
rename from include/core/factor/factor_odom_2d_analytic.h
rename to include/core/factor/factor_loopclosure_2d.h
index c98a521dfdc638d250de0406c616b51469df6390..6a4c31a521e5ff95d81508f1ae53ece6e0fcca2d 100644
--- a/include/core/factor/factor_odom_2d_analytic.h
+++ b/include/core/factor/factor_loopclosure_2d.h
@@ -1,24 +1,24 @@
-#ifndef FACTOR_ODOM_2d_ANALYTIC_H_
-#define FACTOR_ODOM_2d_ANALYTIC_H_
+#ifndef FACTOR_LOOPCLOSURE_2d_H_
+#define FACTOR_LOOPCLOSURE_2d_H_
//Wolf includes
-#include "core/factor/factor_relative_2d_analytic.h"
+#include "core/factor/factor_relative_pose_2d.h"
#include <Eigen/StdVector>
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorOdom2dAnalytic);
-
+WOLF_PTR_TYPEDEFS(FactorLoopclosure2d);
+
//class
-class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
+class FactorLoopclosure2d : public FactorRelativePose2d
{
public:
- FactorOdom2dAnalytic(const FeatureBasePtr& _ftr_ptr,
+ FactorLoopclosure2d(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2dAnalytic("FactorOdom2dAnalytic",
+ FactorRelativePose2d("FactorLoopclosure2d",
_ftr_ptr,
_frame_ptr,
_processor_ptr,
@@ -28,11 +28,11 @@ class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
//
}
- ~FactorOdom2dAnalytic() override = default;
+ ~FactorLoopclosure2d() override = default;
std::string getTopology() const override
{
- return std::string("MOTION");
+ return std::string("LOOP");
}
};
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index 298cc35840163d92a353a450ed118360e2acf3d5..b59cc723c7f8218ee39b18e0a160ece3b6eda74b 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -1,35 +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_pose_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 FactorRelativePose2d
{
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",
- _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("FactorOdom2d",
+ _ftr_ptr,
+ _frame_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
@@ -41,65 +35,8 @@ 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)
-// {
-// 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_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
deleted file mode 100644
index 16f1d677024f4dd97968dbfda45c0e3aaca82723..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ /dev/null
@@ -1,104 +0,0 @@
-#ifndef FACTOR_ODOM_2d_CLOSELOOP_H_
-#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"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorOdom2dCloseloop);
-
-//class
-class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>
-{
- 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())
- {
- //
- }
-
- virtual ~FactorOdom2dCloseloop() = 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
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_pose_2d.h
similarity index 93%
rename from include/core/factor/factor_relative_2d_analytic.h
rename to include/core/factor/factor_relative_pose_2d.h
index dfb06c429b10e205ffe93146269f32a05d664a7d..72e77ea03eb9989058c597bc02a13a32ac9d9cc6 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -1,5 +1,5 @@
-#ifndef FACTOR_RELATIVE_2d_ANALYTIC_H_
-#define FACTOR_RELATIVE_2d_ANALYTIC_H_
+#ifndef FACTOR_RELATIVE_POSE_2d_H_
+#define FACTOR_RELATIVE_POSE_2d_H_
//Wolf includes
#include "core/factor/factor_analytic.h"
@@ -9,16 +9,16 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorRelative2dAnalytic);
+WOLF_PTR_TYPEDEFS(FactorRelativePose2d);
//class
-class FactorRelative2dAnalytic : public FactorAnalytic
+class FactorRelativePose2d : public FactorAnalytic
{
public:
/** \brief Constructor of category FAC_FRAME
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelativePose2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -43,7 +43,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelativePose2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -68,7 +68,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
- FactorRelative2dAnalytic(const std::string& _tp,
+ FactorRelativePose2d(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -96,7 +96,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
return std::string("GEOM");
}
- ~FactorRelative2dAnalytic() override = default;
+ ~FactorRelativePose2d() override = default;
/** \brief Returns the factor residual size
**/
@@ -139,7 +139,7 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/// IMPLEMENTATION ///
-inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
+inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
Eigen::VectorXd residual(3);
@@ -155,7 +155,7 @@ inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
return residual;
}
-inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+inline void FactorRelativePose2d::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
{
@@ -191,7 +191,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 FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
@@ -227,7 +227,7 @@ inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen:
}
}
-inline void FactorRelative2dAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+inline void FactorRelativePose2d::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 cb65fa06b01d72cb32d1209752b44d6826f0d76b..071976faf1419ea48fd00c37707d95bf344ec855 100644
--- a/include/core/feature/feature_odom_2d.h
+++ b/include/core/feature/feature_odom_2d.h
@@ -2,9 +2,8 @@
#define FEATURE_ODOM_2d_H_
//Wolf includes
+#include <core/factor/factor_odom_2d.h>
#include "core/feature/feature_base.h"
-#include "core/factor/factor_odom_2d.h"
-#include "core/factor/factor_odom_2d_analytic.h"
//std includes
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 2ad5b846b59e3c40d98351a0f8b30f75b3f71635..254eded5ccc644faf5b6c20e128f55da77c92b95 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -10,7 +10,6 @@
#include "core/processor/processor_motion.h"
#include "core/capture/capture_odom_2d.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/math/rotations.h"
#include "core/utils/params_server.h"
#include "core/math/SE2.h"
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index b586278c7e7f5a7cb949b78d2e90f58323031109..b89d49c8cac06092aaaada411598c20514fab574 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -9,13 +9,13 @@
#define TRUNK_SRC_SOLVER_QR_SOLVER_H_
//std includes
+#include <core/factor/factor_odom_2d.h>
#include <iostream>
#include <ctime>
//Wolf includes
#include "core/state_block/state_block.h"
#include "../factor_sparse.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_corner_2d.h"
#include "core/factor/factor_container.h"
#include "core/solver_suitesparse/sparse_utils.h"
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 2da7c330b59ffce0c7eceb11515e4f992a9eb3d9..d4f40c9ebea4250e313e2031650e33ef95f224ab 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -2,6 +2,7 @@
#include "core/sensor/sensor_odom_2d.h"
#include "core/math/covariance.h"
#include "core/state_block/state_composite.h"
+#include "core/factor/factor_odom_2d.h"
namespace wolf
{
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4363e38a4d4df67a73ce92086b7a3ae071c4a561..2e7d21c96e9061674a1d5108e45913f298994a3e 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -201,14 +201,14 @@ target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME})
+# FactorRelativePose2d class test
+wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
+target_link_libraries(gtest_factor_relative_pose_2d ${PLUGIN_NAME})
+
# FactorRelativePose2dWithExtrinsics class test
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 ${PLUGIN_NAME})
-# FactorRelative2dAnalytic class test
-wolf_add_gtest(gtest_factor_relative_2d_analytic gtest_factor_relative_2d_analytic.cpp)
-target_link_libraries(gtest_factor_relative_2d_analytic ${PLUGIN_NAME})
-
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
diff --git a/test/dummy/factor_odom_2d_autodiff.h b/test/dummy/factor_odom_2d_autodiff.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7cefbb0a94b2ea54b52f0b0de915d660020c1c6
--- /dev/null
+++ b/test/dummy/factor_odom_2d_autodiff.h
@@ -0,0 +1,105 @@
+#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",
+ _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())
+ {
+ //
+ }
+
+ ~FactorOdom2dAutodiff() override = default;
+
+ 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<FactorOdom2d>(_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 << "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 // FACTOR_ODOM_2d_AUTODIFF_H_
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index f4e3108d6bdf2e129358b851e82772dc5cffae3a..8c27c1e8a60eff512ac83ce511f7f0267f5198e8 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -5,16 +5,17 @@
* Author: jvallve
*/
-#include "core/utils/utils_gtest.h"
+#include "dummy/factor_odom_2d_autodiff.h"
+#include "dummy/factor_dummy_zero_1.h"
+#include "dummy/factor_dummy_zero_12.h"
+#include "core/factor/factor_odom_2d.h"
+#include "core/utils/utils_gtest.h"
#include "core/sensor/sensor_odom_2d.h"
#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_autodiff.h"
-#include "dummy/factor_dummy_zero_1.h"
-#include "dummy/factor_dummy_zero_12.h"
+
using namespace wolf;
using namespace Eigen;
@@ -369,7 +370,7 @@ TEST(FactorAutodiff, EmplaceOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
ASSERT_TRUE(factor_ptr->getFeature());
ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
@@ -405,7 +406,7 @@ TEST(FactorAutodiff, ResidualOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// EVALUATE
@@ -450,7 +451,7 @@ TEST(FactorAutodiff, JacobianOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
@@ -530,8 +531,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
const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index fa3d365424b3d62aa56eb6e6d186163251a594ad..54ce142a0dcb369522137e5ccf477728a609d4a8 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -5,8 +5,8 @@
* \author: jsola
*/
+#include <core/factor/factor_distance_3d.h>
#include "../include/core/ceres_wrapper/solver_ceres.h"
-#include "core/factor/factor_autodiff_distance_3d.h"
#include "core/problem/problem.h"
#include "core/math/rotations.h"
@@ -28,7 +28,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test
FrameBasePtr F1, F2;
CaptureBasePtr C2;
FeatureBasePtr f2;
- FactorAutodiffDistance3dPtr c2;
+ FactorDistance3dPtr c2;
Vector1d dist;
Matrix1d dist_cov;
@@ -68,7 +68,7 @@ TEST_F(FactorAutodiffDistance3d_Test, ground_truth)
double res;
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
c2->operator ()(pos1.data(), pos2.data(), &res);
ASSERT_NEAR(res, 0.0, 1e-5);
@@ -79,7 +79,7 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
double measurement = 1.400;
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
double res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
@@ -94,7 +94,7 @@ TEST_F(FactorAutodiffDistance3d_Test, solve)
double measurement = 1.400;
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2 = FactorBase::emplace<FactorDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET);
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
index 740933a67158634dfd7c6194c8b6a46bd926c063..5638239af4199356a877f186ca93a9e398b9f557 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d.cpp
@@ -1,7 +1,7 @@
+#include <core/factor/factor_odom_2d.h>
#include "../include/core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_pose_2d.cpp
similarity index 88%
rename from test/gtest_factor_relative_2d_analytic.cpp
rename to test/gtest_factor_relative_pose_2d.cpp
index 70cbc464716d50d60bb8ad05cfdb1a4bd6f79d95..8a46caebb25d6ffc0d50d779fcd7642c11140074 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -1,7 +1,7 @@
#include "../include/core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
-#include "core/factor/factor_relative_2d_analytic.h"
+#include "core/factor/factor_relative_pose_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
@@ -25,12 +25,12 @@ FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, Vector3d::Zero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-TEST(FactorRelative2dAnalytic, check_tree)
+TEST(FactorRelativePose2d, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorRelative2dAnalytic, fix_0_solve)
+TEST(FactorRelativePose2d, fix_0_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -54,7 +54,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -71,7 +71,7 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
}
}
-TEST(FactorRelative2dAnalytic, fix_1_solve)
+TEST(FactorRelativePose2d, fix_1_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -95,7 +95,7 @@ TEST(FactorRelative2dAnalytic, fix_1_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index ac405e6fa1e9f4bcdc7976d8f0adc2014cb79c5f..fc170fe3589bc43c5816b04af4419dcab510c671 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,13 +5,13 @@
* Author: jsola
*/
+#include <core/factor/factor_odom_2d.h>
#include "core/utils/utils_gtest.h"
#include "core/frame/frame_base.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/capture/capture_motion.h"
#include <iostream>
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index ab545f2eca3122493a20ac31024faab520f46c64..7f9afd039927db217e46dc4a2b1397e3529cf037 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -5,12 +5,12 @@
* \author: jsola
*/
+#include <core/factor/factor_odom_2d.h>
#include "core/utils/utils_gtest.h"
// Classes under test
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
-#include "core/factor/factor_odom_2d.h"
#include "core/capture/capture_odom_2d.h"
// Wolf includes