From 05c10a2047e06218ae03fc4a3edbc9c2b85ccfc0 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 8 Feb 2021 14:56:36 +0100
Subject: [PATCH] topology added to FactorBase and core adapted
---
demos/hello_wolf/factor_bearing.h | 16 ++--
demos/hello_wolf/factor_range_bearing.h | 14 ++-
include/core/factor/factor_analytic.h | 1 +
include/core/factor/factor_autodiff.h | 36 +++++---
include/core/factor/factor_base.h | 35 +++++---
include/core/factor/factor_block_absolute.h | 7 +-
include/core/factor/factor_block_difference.h | 6 +-
include/core/factor/factor_diff_drive.h | 7 +-
include/core/factor/factor_distance_3d.h | 6 +-
include/core/factor/factor_loopclosure_2d.h | 42 ---------
include/core/factor/factor_odom_2d.h | 42 ---------
include/core/factor/factor_pose_2d.h | 6 +-
include/core/factor/factor_pose_3d.h | 6 +-
.../core/factor/factor_quaternion_absolute.h | 6 +-
include/core/factor/factor_relative_pose_2d.h | 50 +++++------
.../factor_relative_pose_2d_with_extrinsics.h | 7 +-
...or_odom_3d.h => factor_relative_pose_3d.h} | 88 ++++++++-----------
include/core/feature/feature_odom_2d.h | 10 ---
include/core/processor/processor_odom_3d.h | 2 +-
src/factor/factor_analytic.cpp | 3 +-
src/factor/factor_base.cpp | 4 +
src/feature/feature_odom_2d.cpp | 5 --
src/processor/processor_odom_2d.cpp | 13 +--
src/processor/processor_odom_3d.cpp | 11 +--
test/CMakeLists.txt | 16 ++--
test/dummy/factor_dummy_zero_1.h | 6 +-
test/dummy/factor_dummy_zero_12.h | 6 +-
test/dummy/factor_feature_dummy.h | 6 +-
test/dummy/factor_landmark_dummy.h | 6 +-
test/dummy/factor_odom_2d_autodiff.h | 23 ++---
test/gtest_emplace.cpp | 7 +-
test/gtest_factor_absolute.cpp | 2 +-
test/gtest_factor_autodiff.cpp | 4 +-
test/gtest_factor_autodiff_distance_3d.cpp | 4 +-
test/gtest_factor_base.cpp | 5 +-
test/gtest_factor_block_difference.cpp | 4 +-
test/gtest_factor_diff_drive.cpp | 7 +-
....cpp => gtest_factor_odom_2d_autodiff.cpp} | 14 +--
test/gtest_factor_pose_2d.cpp | 4 +-
test/gtest_factor_pose_3d.cpp | 5 +-
test/gtest_factor_relative_pose_2d.cpp | 6 +-
...actor_relative_pose_2d_with_extrinsics.cpp | 10 +--
....cpp => gtest_factor_relative_pose_3d.cpp} | 16 ++--
test/gtest_factor_sparse.cpp | 52 -----------
test/gtest_factory_state_block.cpp | 3 +-
test/gtest_frame_base.cpp | 5 +-
test/gtest_has_state_blocks.cpp | 2 +-
test/gtest_odom_2d.cpp | 8 +-
test/gtest_param_prior.cpp | 3 +-
test/gtest_problem.cpp | 3 -
test/gtest_tree_manager.cpp | 1 -
test/gtest_tree_manager_sliding_window.cpp | 19 ++--
..._tree_manager_sliding_window_dual_rate.cpp | 31 ++++---
53 files changed, 249 insertions(+), 452 deletions(-)
delete mode 100644 include/core/factor/factor_loopclosure_2d.h
delete mode 100644 include/core/factor/factor_odom_2d.h
rename include/core/factor/{factor_odom_3d.h => factor_relative_pose_3d.h} (69%)
rename test/{gtest_factor_odom_2d.cpp => gtest_factor_odom_2d_autodiff.cpp} (88%)
rename test/{gtest_factor_odom_3d.cpp => gtest_factor_relative_pose_3d.cpp} (82%)
delete mode 100644 test/gtest_factor_sparse.cpp
diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
index f5c5dd958..d3902daad 100644
--- a/demos/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
@@ -19,14 +19,14 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
{
public:
FactorBearing(const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", nullptr, nullptr, nullptr,
- _landmark_other_ptr, _processor_ptr,
- _apply_loss_function, _status,
- getCapture()->getFrame()->getP(),
- getCapture()->getFrame()->getO(),
- _landmark_other_ptr->getP())
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, FactorStatus _status) :
+ FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr,
+ _landmark_other_ptr, _processor_ptr,
+ _apply_loss_function, _status,
+ getCapture()->getFrame()->getP(),
+ getCapture()->getFrame()->getO(),
+ _landmark_other_ptr->getP())
{
//
}
diff --git a/demos/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
index 978f88846..596278b2f 100644
--- a/demos/hello_wolf/factor_range_bearing.h
+++ b/demos/hello_wolf/factor_range_bearing.h
@@ -22,12 +22,13 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
public:
FactorRangeBearing(const CaptureBasePtr& _capture_own, // own capture's pointer
const FeatureBasePtr& _feature_ptr,
- const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
- const ProcessorBasePtr& _processor_ptr, // processor having created this
- bool _apply_loss_function, // apply loss function to residual?
- FactorStatus _status) : // active factor?
+ const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
+ const ProcessorBasePtr& _processor_ptr, // processor having created this
+ bool _apply_loss_function, // apply loss function to residual?
+ FactorStatus _status) : // active factor?
FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
"RANGE BEARING", // factor type enum (see wolf.h)
+ TOP_LMK, // factor topology (see factor_base.h)
_feature_ptr,
nullptr, // other frame's pointer
nullptr, // other capture's pointer
@@ -50,11 +51,6 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
//
}
- std::string getTopology() const override
- {
- return "LMK";
- }
-
template<typename T>
bool operator ()(const T* const _p_w_r, // robot position
const T* const _o_w_r, // robot orientation
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index c05b1a2e1..93daadcee 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -19,6 +19,7 @@ class FactorAnalytic: public FactorBase
public:
FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 8b3ab6f9c..68c820c6f 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -64,6 +64,7 @@ class FactorAutodiff : public FactorBase
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
**/
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -84,7 +85,7 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state9Ptr,
StateBlockPtr _state10Ptr,
StateBlockPtr _state11Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
{
// asserts for null states
@@ -362,6 +363,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -381,7 +383,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
StateBlockPtr _state8Ptr,
StateBlockPtr _state9Ptr,
StateBlockPtr _state10Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
{
// asserts for null states
@@ -626,6 +628,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -644,7 +647,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr,
StateBlockPtr _state9Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
{
// asserts for null states
@@ -880,6 +883,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -897,7 +901,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
{
// asserts for null states
@@ -1124,6 +1128,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -1140,7 +1145,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
StateBlockPtr _state5Ptr,
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
{
// asserts for null states
@@ -1358,6 +1363,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -1373,7 +1379,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr,
StateBlockPtr _state6Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
{
// asserts for null states
@@ -1582,6 +1588,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -1596,7 +1603,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state3Ptr,
StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
{
// asserts for null states
@@ -1791,6 +1798,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -1804,7 +1812,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state2Ptr,
StateBlockPtr _state3Ptr,
StateBlockPtr _state4Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
{
// asserts for null states
@@ -1990,6 +1998,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -2002,7 +2011,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr,
StateBlockPtr _state3Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
{
// asserts for null states
@@ -2183,6 +2192,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -2194,7 +2204,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
{
// asserts for null states
@@ -2366,6 +2376,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -2376,7 +2387,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
FactorStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr})
{
// asserts for null states
@@ -2539,6 +2550,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
public:
FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -2548,7 +2560,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
bool _apply_loss_function,
FactorStatus _status,
StateBlockPtr _state0Ptr) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr})
{
// asserts for null states
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index c600f8a24..e3dd39803 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -26,6 +26,20 @@ typedef enum
FAC_ACTIVE = 1 ///< Normal active factor, playing its role in the solution.
} FactorStatus;
+/** \brief Enumeration of factor topologies
+ *
+ * You may add items to this list as needed. Be concise with names, and document your entries.
+ */
+typedef enum
+{
+ TOP_ABS, ///< absolute factor
+ TOP_MOTION, ///<motion factor, e.g. odometry, IMU
+ TOP_LOOP, ///<loop closure factor
+ TOP_LMK, ///<landmark observation factor
+ TOP_GEOM, ///<some geometric relation, e.g. distance
+ TOP_OTHER ///<other topologies
+} FactorTopology;
+
/** \brief Enumeration of jacobian computation method
*
* You may add items to this list as needed. Be concise with names, and document your entries.
@@ -48,6 +62,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
protected:
unsigned int factor_id_;
+ FactorTopology topology_; ///< topology of factor
FactorStatus status_; ///< status of factor
bool apply_loss_function_; ///< flag for applying loss function to this factor
FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
@@ -68,6 +83,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
* that does not located in the same branch.
**/
FactorBase(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -78,6 +94,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
FactorStatus _status = FAC_ACTIVE);
FactorBase(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
@@ -96,19 +113,11 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
unsigned int id() const;
/** \brief get the main topological characteristic
- * Possible outputs:
- * - "ABS" absolute factor
- * - "MOTION" motion factor, e.g. odometry, IMU
- * - "LOOP" loop closure factor
- * - "LMK" landmark observation factor
- * - "GEOM" some geometric relation, e.g. distance
- * - "UNDEFINED" undefined topology
- * - "OTHER" other topologies
- * You can add you own return strings if you wish. But the strings above may already have a function in WOLF.
- *
- * This function needs to be implemented in all derived classes
- */
- virtual std::string getTopology() const = 0;
+ **/
+ virtual FactorTopology getTopology() const final
+ {
+ return topology_;
+ }
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 21e2074e2..58d0570dd 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -39,6 +39,7 @@ class FactorBlockAbsolute : public FactorAnalytic
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
+ TOP_ABS,
_feature_ptr,
nullptr,
nullptr,
@@ -73,6 +74,7 @@ class FactorBlockAbsolute : public FactorAnalytic
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
+ TOP_ABS,
_feature_ptr,
nullptr,
nullptr,
@@ -100,11 +102,6 @@ class FactorBlockAbsolute : public FactorAnalytic
~FactorBlockAbsolute() override = default;
- std::string getTopology() const override
- {
- return std::string("ABS");
- }
-
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index d632a7987..bdf76c063 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -51,6 +51,7 @@ class FactorBlockDifference : public FactorAnalytic
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockDifference",
+ TOP_GEOM,
_feature_ptr,
_frame_other,
_cap_other,
@@ -83,11 +84,6 @@ class FactorBlockDifference : public FactorAnalytic
~FactorBlockDifference() override = default;
- std::string getTopology() const override
- {
- return std::string("DIFF");
- }
-
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index aa28c9eee..726771964 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -55,6 +55,7 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
const bool _apply_loss_function,
const FactorStatus _status = FAC_ACTIVE) :
Base("FactorDiffDrive",
+ TOP_MOTION,
_ftr_ptr,
_capture_origin_ptr->getFrame(),
_capture_origin_ptr,
@@ -82,12 +83,6 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
**/
~FactorDiffDrive() 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, const T* const _c, T* _residuals) const;
diff --git a/include/core/factor/factor_distance_3d.h b/include/core/factor/factor_distance_3d.h
index d3f4ef709..61f6767a2 100644
--- a/include/core/factor/factor_distance_3d.h
+++ b/include/core/factor/factor_distance_3d.h
@@ -24,6 +24,7 @@ class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorAutodiffDistance3d",
+ TOP_GEOM,
_feat,
_frm_other,
nullptr,
@@ -39,11 +40,6 @@ class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
~FactorDistance3d() override { /* nothing */ }
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
template<typename T>
bool operator () (const T* const _pos1,
const T* const _pos2,
diff --git a/include/core/factor/factor_loopclosure_2d.h b/include/core/factor/factor_loopclosure_2d.h
deleted file mode 100644
index 6a4c31a52..000000000
--- a/include/core/factor/factor_loopclosure_2d.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef FACTOR_LOOPCLOSURE_2d_H_
-#define FACTOR_LOOPCLOSURE_2d_H_
-
-//Wolf includes
-#include "core/factor/factor_relative_pose_2d.h"
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorLoopclosure2d);
-
-//class
-class FactorLoopclosure2d : public FactorRelativePose2d
-{
- public:
- FactorLoopclosure2d(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorRelativePose2d("FactorLoopclosure2d",
- _ftr_ptr,
- _frame_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status)
- {
- //
- }
-
- ~FactorLoopclosure2d() override = default;
-
- std::string getTopology() const override
- {
- return std::string("LOOP");
- }
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
deleted file mode 100644
index b59cc723c..000000000
--- a/include/core/factor/factor_odom_2d.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef FACTOR_ODOM_2d_H_
-#define FACTOR_ODOM_2d_H_
-
-//Wolf includes
-#include "core/factor/factor_relative_pose_2d.h"
-#include <Eigen/StdVector>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorOdom2d);
-
-//class
-class FactorOdom2d : public FactorRelativePose2d
-{
- public:
- FactorOdom2d(const FeatureBasePtr& _ftr_ptr,
- 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)
- {
- //
- }
-
- ~FactorOdom2d() override = default;
-
- std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index 6536dff68..4357aac83 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -22,6 +22,7 @@ class FactorPose2d: public FactorAutodiff<FactorPose2d,3,2,1>
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose2d, 3, 2, 1>("FactorPose2d",
+ TOP_ABS,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
@@ -34,11 +35,6 @@ class FactorPose2d: public FactorAutodiff<FactorPose2d,3,2,1>
~FactorPose2d() override = default;
- std::string getTopology() const override
- {
- return std::string("ABS");
- }
-
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
diff --git a/include/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h
index dfc8a1754..139b87b65 100644
--- a/include/core/factor/factor_pose_3d.h
+++ b/include/core/factor/factor_pose_3d.h
@@ -21,6 +21,7 @@ class FactorPose3d: public FactorAutodiff<FactorPose3d,6,3,4>
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose3d,6,3,4>("FactorPose3d",
+ TOP_ABS,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
@@ -33,11 +34,6 @@ class FactorPose3d: public FactorAutodiff<FactorPose3d,6,3,4>
~FactorPose3d() override = default;
- std::string getTopology() const override
- {
- return std::string("ABS");
- }
-
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index a897c97bf..3122e8097 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -29,6 +29,7 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
+ TOP_ABS,
_ftr_ptr,
nullptr,
nullptr,
@@ -44,11 +45,6 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
~FactorQuaternionAbsolute() override = default;
- std::string getTopology() const override
- {
- return std::string("ABS");
- }
-
template<typename T>
bool operator ()(const T* const _o, T* _residuals) const;
diff --git a/include/core/factor/factor_relative_pose_2d.h b/include/core/factor/factor_relative_pose_2d.h
index 72e77ea03..a51c9a7fc 100644
--- a/include/core/factor/factor_relative_pose_2d.h
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -18,13 +18,14 @@ class FactorRelativePose2d : public FactorAnalytic
/** \brief Constructor of category FAC_FRAME
**/
- FactorRelativePose2d(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp,
+ FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePose2d",
+ _top,
_ftr_ptr,
_frame_other_ptr,
nullptr,
@@ -43,13 +44,14 @@ class FactorRelativePose2d : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
- FactorRelativePose2d(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FeatureBasePtr& _ftr_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp,
+ FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePose2d",
+ _top,
_ftr_ptr,
nullptr,
nullptr,
@@ -68,13 +70,14 @@ class FactorRelativePose2d : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
- FactorRelativePose2d(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp,
+ FactorRelativePose2d(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePose2d",
+ _top,
_ftr_ptr,
nullptr,
nullptr,
@@ -91,11 +94,6 @@ class FactorRelativePose2d : public FactorAnalytic
//
}
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
~FactorRelativePose2d() override = default;
/** \brief Returns the factor residual size
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index 1ce6c842b..1d7e85804 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -20,8 +20,10 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
+ const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _top,
_ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
@@ -41,11 +43,6 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
~FactorRelativePose2dWithExtrinsics() override = default;
- std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
template<typename T>
bool operator ()(const T* const _p1,
const T* const _o1,
diff --git a/include/core/factor/factor_odom_3d.h b/include/core/factor/factor_relative_pose_3d.h
similarity index 69%
rename from include/core/factor/factor_odom_3d.h
rename to include/core/factor/factor_relative_pose_3d.h
index 94b117f21..67d98f7e2 100644
--- a/include/core/factor/factor_odom_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -1,12 +1,12 @@
/*
- * factor_odom_3d.h
+ * factor_relative_pose_3d.h
*
* Created on: Oct 7, 2016
* Author: jsola
*/
-#ifndef FACTOR_ODOM_3d_H_
-#define FACTOR_ODOM_3d_H_
+#ifndef FACTOR_RELATIVE_POSE_3D_H_
+#define FACTOR_RELATIVE_POSE_3D_H_
#include "core/factor/factor_autodiff.h"
#include "core/math/rotations.h"
@@ -14,25 +14,20 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorOdom3d);
+WOLF_PTR_TYPEDEFS(FactorRelativePose3d);
//class
-class FactorOdom3d : public FactorAutodiff<FactorOdom3d,6,3,4,3,4>
+class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,4>
{
public:
- FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorOdom3d() override = default;
-
- std::string getTopology() const override
- {
- return std::string("MOTION");
- }
+ FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+ ~FactorRelativePose3d() override = default;
template<typename T>
bool operator ()(const T* const _p_current,
@@ -54,11 +49,12 @@ class FactorOdom3d : public FactorAutodiff<FactorOdom3d,6,3,4,3,4>
private:
template<typename T>
void printRes(const Eigen::Matrix<T, 6, 1>& r) const;
+ std::string topology_;
};
template<typename T>
-inline void FactorOdom3d::printRes(const Eigen::Matrix<T, 6, 1>& r) const
+inline void FactorRelativePose3d::printRes(const Eigen::Matrix<T, 6, 1>& r) const
{
std::cout << "Jet residual = " << std::endl;
std::cout << r(0) << std::endl;
@@ -70,43 +66,37 @@ inline void FactorOdom3d::printRes(const Eigen::Matrix<T, 6, 1>& r) const
}
template<>
-inline void FactorOdom3d::printRes (const Eigen::Matrix<double,6,1> & r) const
+inline void FactorRelativePose3d::printRes (const Eigen::Matrix<double,6,1> & r) const
{
std::cout << "Scalar residual = " << std::endl;
std::cout << r.transpose() << std::endl;
}
-inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorOdom3d, 6, 3, 4, 3, 4>("FactorOdom3d", // type
- _ftr_current_ptr, // feature
- _frame_past_ptr, // frame other
- nullptr, // capture other
- nullptr, // feature other
- nullptr, // landmark other
- _processor_ptr, // processor
- _apply_loss_function,
- _status,
- _ftr_current_ptr->getFrame()->getP(), // current frame P
- _ftr_current_ptr->getFrame()->getO(), // current frame Q
- _frame_past_ptr->getP(), // past frame P
- _frame_past_ptr->getO()) // past frame Q
+inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // type
+ _top, // topology
+ _ftr_current_ptr, // feature
+ _frame_past_ptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO(), // current frame Q
+ _frame_past_ptr->getP(), // past frame P
+ _frame_past_ptr->getO()) // past frame Q
{
-// WOLF_TRACE("Constr ODOM 3d (f", _ftr_current_ptr->id(),
-// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
-// ") (Fo", _frame_past_ptr->id(), ")");
-//
-// WOLF_TRACE("delta preint: ", _ftr_current_ptr->getMeasurement().transpose());
-//
-// WOLF_TRACE("Omega_delta.sqrt: \n", _ftr_current_ptr->getMeasurementSquareRootInformationUpper());
- //
}
template<typename T>
-inline bool FactorOdom3d::expectation(const T* const _p_current,
+inline bool FactorRelativePose3d::expectation(const T* const _p_current,
const T* const _q_current,
const T* const _p_past,
const T* const _q_past,
@@ -150,7 +140,7 @@ inline bool FactorOdom3d::expectation(const T* const _p_current,
return true;
}
-inline Eigen::VectorXd FactorOdom3d::expectation() const
+inline Eigen::VectorXd FactorRelativePose3d::expectation() const
{
Eigen::VectorXd exp(7);
FrameBasePtr frm_current = getFeature()->getFrame();
@@ -173,7 +163,7 @@ inline Eigen::VectorXd FactorOdom3d::expectation() const
}
template<typename T>
-inline bool FactorOdom3d::operator ()(const T* const _p_current,
+inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
const T* const _q_current,
const T* const _p_past,
const T* const _q_past,
@@ -243,4 +233,4 @@ inline bool FactorOdom3d::operator ()(const T* const _p_current,
} /* namespace wolf */
-#endif /* FACTOR_ODOM_3d_H_ */
+#endif /* FACTOR_RELATIVE_POSE_3D_H_ */
diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h
index 071976faf..91016cbfc 100644
--- a/include/core/feature/feature_odom_2d.h
+++ b/include/core/feature/feature_odom_2d.h
@@ -2,7 +2,6 @@
#define FEATURE_ODOM_2d_H_
//Wolf includes
-#include <core/factor/factor_odom_2d.h>
#include "core/feature/feature_base.h"
//std includes
@@ -25,15 +24,6 @@ class FeatureOdom2d : public FeatureBase
FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeatureOdom2d() override;
-
- /** \brief Generic interface to find factors
- *
- * TBD
- * Generic interface to find factors between this feature and a map (static/slam) or a previous feature
- *
- **/
- virtual void findFactors();
-
};
} // namespace wolf
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 438dcc33b..95035e56a 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -11,8 +11,8 @@
#include "core/processor/processor_motion.h"
#include "core/sensor/sensor_odom_3d.h"
#include "core/capture/capture_odom_3d.h"
-#include "core/factor/factor_odom_3d.h"
#include "core/math/rotations.h"
+#include "core/factor/factor_relative_pose_3d.h"
#include <cmath>
namespace wolf {
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index 6d1467060..f3c300baa 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -4,6 +4,7 @@
namespace wolf {
FactorAnalytic::FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -13,7 +14,7 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
bool _apply_loss_function, FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index c03fe00ad..5e3fef35e 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -7,6 +7,7 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
FactorBase::FactorBase(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
@@ -18,6 +19,7 @@ FactorBase::FactorBase(const std::string& _tp,
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
+ topology_(_top),
status_(_status),
apply_loss_function_(_apply_loss_function),
frame_other_list_(),
@@ -41,6 +43,7 @@ FactorBase::FactorBase(const std::string& _tp,
}
FactorBase::FactorBase(const std::string& _tp,
+ const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
@@ -52,6 +55,7 @@ FactorBase::FactorBase(const std::string& _tp,
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
+ topology_(_top),
status_(_status),
apply_loss_function_(_apply_loss_function),
frame_other_list_(),
diff --git a/src/feature/feature_odom_2d.cpp b/src/feature/feature_odom_2d.cpp
index 169031037..a4d2571b9 100644
--- a/src/feature/feature_odom_2d.cpp
+++ b/src/feature/feature_odom_2d.cpp
@@ -13,9 +13,4 @@ FeatureOdom2d::~FeatureOdom2d()
//
}
-void FeatureOdom2d::findFactors()
-{
-
-}
-
} // namespace wolf
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index d4f40c9eb..573406efe 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -2,7 +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"
+#include "core/factor/factor_relative_pose_2d.h"
namespace wolf
{
@@ -144,11 +144,12 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom2d>(_feature,
- _feature,
- _capture_origin->getFrame(),
- shared_from_this(),
- params_->apply_loss_function);
+ auto fac_odom = FactorBase::emplace<FactorRelativePose2d>(_feature,
+ _feature,
+ _capture_origin->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
return fac_odom;
}
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 8d8b190ee..e57ba32bd 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -223,11 +223,12 @@ Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_prei
FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom3d>(_feature_motion,
- _feature_motion,
- _capture_origin->getFrame(),
- shared_from_this(),
- params_->apply_loss_function);
+ auto fac_odom = FactorBase::emplace<FactorRelativePose3d>(_feature_motion,
+ _feature_motion,
+ _capture_origin->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
return fac_odom;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2e7d21c96..8deec25db 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -181,17 +181,13 @@ target_link_libraries(gtest_factor_autodiff_distance_3d ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME})
-# FactorOdom3d class test
+# FactorDiffDrive class test
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
-# FactorOdom2d class test
-wolf_add_gtest(gtest_factor_odom_2d gtest_factor_odom_2d.cpp)
-target_link_libraries(gtest_factor_odom_2d ${PLUGIN_NAME})
-
-# FactorOdom3d class test
-wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp)
-target_link_libraries(gtest_factor_odom_3d ${PLUGIN_NAME})
+# FactorOdom2dAutodiff class test
+wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
+target_link_libraries(gtest_factor_odom_2d_autodiff ${PLUGIN_NAME})
# FactorPose2d class test
wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
@@ -209,6 +205,10 @@ target_link_libraries(gtest_factor_relative_pose_2d ${PLUGIN_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 ${PLUGIN_NAME})
+# FactorRelativePose3d class test
+wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
+target_link_libraries(gtest_factor_relative_pose_3d ${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_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
index e0d5b37f3..f2d174702 100644
--- a/test/dummy/factor_dummy_zero_1.h
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -18,6 +18,7 @@ class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
FactorDummyZero1(const FeatureBasePtr& _ftr_ptr,
const StateBlockPtr& _sb_ptr) :
Base("FactorDummy1",
+ TOP_OTHER,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
nullptr,
@@ -30,11 +31,6 @@ class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
~FactorDummyZero1() override = default;
- std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
template<typename T>
bool operator ()(const T* const _st1,
T* _residuals) const
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_12.h
index fc38a80ac..978592b61 100644
--- a/test/dummy/factor_dummy_zero_12.h
+++ b/test/dummy/factor_dummy_zero_12.h
@@ -29,6 +29,7 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
const StateBlockPtr& _sb11_ptr,
const StateBlockPtr& _sb12_ptr) :
Base("FactorDummy12",
+ TOP_OTHER,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
nullptr,
@@ -52,11 +53,6 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
~FactorDummyZero12() override = default;
- std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
template<typename T>
bool operator ()(const T* const _st1,
const T* const _st2,
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index e4e8dd9de..cf81f3025 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -18,11 +18,6 @@ class FactorFeatureDummy : public FactorBase
~FactorFeatureDummy() override = default;
- std::string getTopology() const override
- {
- return "DUMMY";
- }
-
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override {return true;};
@@ -60,6 +55,7 @@ inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr
bool _apply_loss_function,
FactorStatus _status) :
FactorBase("FactorFeatureDummy",
+ TOP_OTHER,
_feature_ptr,
nullptr,
nullptr,
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index fe48f2628..5f6f21124 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -18,11 +18,6 @@ class FactorLandmarkDummy : public FactorBase
~FactorLandmarkDummy() override = default;
- std::string getTopology() const override
- {
- return "DUMMY";
- }
-
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
bool evaluate(double const* const* parameters,
@@ -70,6 +65,7 @@ inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_p
bool _apply_loss_function,
FactorStatus _status) :
FactorBase("FactorFeatureDummy",
+ TOP_OTHER,
_feature_ptr,
nullptr,
nullptr,
diff --git a/test/dummy/factor_odom_2d_autodiff.h b/test/dummy/factor_odom_2d_autodiff.h
index f7cefbb0a..e765ed760 100644
--- a/test/dummy/factor_odom_2d_autodiff.h
+++ b/test/dummy/factor_odom_2d_autodiff.h
@@ -22,26 +22,21 @@ class FactorOdom2dAutodiff : public FactorAutodiff<FactorOdom2dAutodiff, 3, 2, 1
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())
+ TOP_OTHER,
+ _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;
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 21ed31670..f790bc57f 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -15,6 +15,7 @@
#include "core/processor/processor_odom_3d.h"
#include "core/processor/processor_odom_2d.h"
#include "core/feature/feature_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include <iostream>
@@ -111,7 +112,7 @@ TEST(Emplace, Factor)
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
- auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
+ auto cnt = FactorBase::emplace<FactorRelativePose2d>(ftr, ftr, frm, nullptr, false, TOP_MOTION);
ASSERT_NE(nullptr, ftr->getFactorList().front().get());
}
@@ -145,9 +146,9 @@ TEST(Emplace, ReturnDerived)
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
- auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
+ auto cnt = FactorBase::emplace<FactorRelativePose2d>(ftr, ftr, frm, nullptr, false, TOP_MOTION);
- FactorOdom2dPtr fac = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
+ FactorRelativePose2dPtr fac = FactorBase::emplace<FactorRelativePose2d>(ftr, ftr, frm, nullptr, false, TOP_MOTION);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index dc3551d2c..e67a2f740 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -5,7 +5,7 @@
* \author: datchuth
*/
-#include "../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 8c27c1e8a..c0cbacdec 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -9,7 +9,7 @@
#include "dummy/factor_dummy_zero_1.h"
#include "dummy/factor_dummy_zero_12.h"
-#include "core/factor/factor_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
#include "core/utils/utils_gtest.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/capture/capture_void.h"
@@ -532,7 +532,7 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
// FACTOR
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);
+ auto fac_analytic_ptr = FactorBase::emplace<FactorRelativePose2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false, TOP_MOTION);
// 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 54ce142a0..477520c0f 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_distance_3d.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/problem/problem.h"
#include "core/math/rotations.h"
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 79663676a..f861114e7 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -7,8 +7,6 @@
#include "core/utils/utils_gtest.h"
-
-
#include "core/factor/factor_base.h"
using namespace wolf;
@@ -45,6 +43,7 @@ class FactorDummy : public FactorBase
const FeatureBasePtr& _feature_other,
const LandmarkBasePtr& _landmark_other) :
FactorBase("Dummy",
+ TOP_OTHER,
_feature,
_frame_other,
_capture_other,
@@ -61,6 +60,7 @@ class FactorDummy : public FactorBase
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list) :
FactorBase("Dummy",
+ TOP_OTHER,
_feature,
_frame_other_list,
_capture_other_list,
@@ -73,7 +73,6 @@ class FactorDummy : public FactorBase
}
~FactorDummy() override = default;
- std::string getTopology() const override {return "DUMMY";}
bool evaluate(double const* const* _parameters,
double* _residuals,
double** _jacobians) const override {return true;}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 1dbf21b97..c314c74ce 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -7,9 +7,7 @@
#include "core/utils/utils_gtest.h"
-#include <Eigen/Dense>
-
-#include "../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/problem/problem.h"
#include "core/frame/frame_base.h"
#include "core/capture/capture_base.h"
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 4f3ece70d..a2e7783be 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -5,13 +5,12 @@
* \author: jsola
*/
-#include "../include/core/ceres_wrapper/solver_ceres.h"
-#include "core/processor/processor_diff_drive.h"
-#include "core/sensor/sensor_diff_drive.h"
#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/processor/processor_diff_drive.h"
+#include "core/sensor/sensor_diff_drive.h"
#include "core/factor/factor_diff_drive.h"
-
#include "core/frame/frame_base.h"
#include "core/capture/capture_motion.h"
#include "core/feature/feature_motion.h"
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d_autodiff.cpp
similarity index 88%
rename from test/gtest_factor_odom_2d.cpp
rename to test/gtest_factor_odom_2d_autodiff.cpp
index 5638239af..45315ddf2 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d_autodiff.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 "dummy/factor_odom_2d_autodiff.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
@@ -25,12 +25,12 @@ FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector3d::Zero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-TEST(FactorOdom2d, check_tree)
+TEST(FactorOdom2dAutodiff, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorOdom2d, fix_0_solve)
+TEST(FactorOdom2dAutodiff, fix_0_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -54,7 +54,7 @@ TEST(FactorOdom2d, fix_0_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -71,7 +71,7 @@ TEST(FactorOdom2d, fix_0_solve)
}
}
-TEST(FactorOdom2d, fix_1_solve)
+TEST(FactorOdom2dAutodiff, fix_1_solve)
{
for (int i = 0; i < 1e3; i++)
{
@@ -95,7 +95,7 @@ TEST(FactorOdom2d, fix_1_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorOdom2dAutodiff>(fea1, fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index 7ecc6ab08..d38ec5284 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -5,13 +5,11 @@
* \author: jsola
*/
-#include "../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_pose_2d.h"
#include "core/utils/utils_gtest.h"
-
#include "core/capture/capture_motion.h"
-
using namespace Eigen;
using namespace wolf;
using std::cout;
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index c44374caa..8bf5b82ed 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -5,10 +5,9 @@
* \author: jsola
*/
-#include "../include/core/ceres_wrapper/solver_ceres.h"
-#include "core/factor/factor_pose_3d.h"
#include "core/utils/utils_gtest.h"
-
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_pose_3d.h"
#include "core/capture/capture_motion.h"
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index 8a46caebb..b4c11c8bb 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -1,6 +1,6 @@
-#include "../include/core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
@@ -54,7 +54,7 @@ TEST(FactorRelativePose2d, fix_0_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, "odom2d", fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
// Fix frm0, perturb frm1
frm0->fix();
@@ -95,7 +95,7 @@ TEST(FactorRelativePose2d, fix_1_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, "odom2d", fea1, frm0, nullptr, false);
+ FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
// Fix frm1, perturb frm0
frm1->fix();
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index f6e8e18f1..eb1f5eb4f 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -1,6 +1,6 @@
-#include "../include/core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/capture/capture_odom_2d.h"
#include "core/sensor/sensor_odom_2d.h"
@@ -67,7 +67,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
// Perturb frm1, fix the rest
frm0->fix();
@@ -115,7 +115,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
// Perturb frm0, fix the rest
frm1->fix();
@@ -163,7 +163,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
// Perturb sensor P, fix the rest
frm1->fix();
@@ -211,7 +211,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
// feature & factor with delta measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
// Perturb sensor O, fix the rest
frm1->fix();
diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
similarity index 82%
rename from test/gtest_factor_odom_3d.cpp
rename to test/gtest_factor_relative_pose_3d.cpp
index 7a57f0354..5771b1246 100644
--- a/test/gtest_factor_odom_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -1,14 +1,14 @@
/**
- * \file gtest_factor_odom_3d.cpp
+ * \file gtest_factor_relative_pose_3d.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
*/
-#include "../include/core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
-#include "core/factor/factor_odom_3d.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_pose_3d.h"
#include "core/capture/capture_motion.h"
@@ -43,21 +43,21 @@ FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), delta);
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
-FactorOdom3dPtr ctr1 = FactorBase::emplace<FactorOdom3d>(fea1, fea1, frm0, nullptr, false); // create and add
+FactorRelativePose3dPtr ctr1 = FactorBase::emplace<FactorRelativePose3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
////////////////////////////////////////////////////////
-TEST(FactorOdom3d, check_tree)
+TEST(FactorRelativePose3d, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorOdom3d, expectation)
+TEST(FactorRelativePose3d, expectation)
{
ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
}
-TEST(FactorOdom3d, fix_0_solve)
+TEST(FactorRelativePose3d, fix_0_solve)
{
// Fix frame 0, perturb frm1
@@ -72,7 +72,7 @@ TEST(FactorOdom3d, fix_0_solve)
}
-TEST(FactorOdom3d, fix_1_solve)
+TEST(FactorRelativePose3d, fix_1_solve)
{
// Fix frame 1, perturb frm0
frm0->unfix();
diff --git a/test/gtest_factor_sparse.cpp b/test/gtest_factor_sparse.cpp
deleted file mode 100644
index 53d8b91a1..000000000
--- a/test/gtest_factor_sparse.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/**
- * \file gtest_factor_sparse.cpp
- *
- * Created on: Apr 25, 2017
- * \author: jsola
- */
-
-#include "core/utils/utils_gtest.h"
-
-#include "factor_sparse.h"
-
-using namespace wolf;
-
-// Dummy class for avoiding the pure virtual compilation error
-template <JacobianMethod J>
-class FactorSparseObject : public FactorSparse<1, 2, 1>
-{
- public:
- FactorSparseObject(FactorType _tp, bool _apply_loss_function, FactorStatus _status, StateBlockPtr _xy, StateBlockPtr _theta) :
- FactorSparse<1, 2, 1>(_tp, _apply_loss_function, _status, _xy, _theta)
- {
- //
- }
- virtual ~FactorSparseObject(){}
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return J;
- }
-};
-
-TEST(FactorSparseAnalytic, Constructor)
-{
- FactorSparseObject<JAC_ANALYTIC> fac_analytic(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(fac_analytic.getJacobianMethod(), JAC_ANALYTIC);
- ASSERT_EQ(fac_analytic.getApplyLossFunction(), false);
- ASSERT_EQ(fac_analytic.getStatus(), FAC_ACTIVE);
- ASSERT_EQ(fac_analytic.getSize(), 1);
-
- FactorSparseObject<JAC_AUTO> fac_auto(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(fac_auto.getJacobianMethod(), JAC_AUTO);
-
- FactorSparseObject<JAC_NUMERIC> fac_numeric(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(fac_numeric.getJacobianMethod(), JAC_NUMERIC);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 7edff3ed1..96545d1be 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -5,11 +5,12 @@
* Author: jsola
*/
+#include "core/utils/utils_gtest.h"
+
#include "core/common/wolf.h"
#include "core/state_block/factory_state_block.h"
#include "core/sensor/factory_sensor.h"
-#include "core/utils/utils_gtest.h"
using namespace wolf;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 790ac4b32..b37ce9708 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -5,10 +5,9 @@
* Author: jsola
*/
-#include <core/factor/factor_odom_2d.h>
#include "core/utils/utils_gtest.h"
-
+#include "core/factor/factor_relative_pose_2d.h"
#include "core/frame/frame_base.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
@@ -75,7 +74,7 @@ TEST(FrameBase, LinksToTree)
//auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
- auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false);
+ auto c = FactorBase::emplace<FactorRelativePose2d>(f, f, F2, p, false, TOP_MOTION);
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
ASSERT_FALSE(F2->getConstrainedByList().empty());
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index d9eb8a1be..71e271a92 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -6,9 +6,9 @@
*/
-#include "../include/core/ceres_wrapper/solver_ceres.h"
#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include "core/frame/frame_base.h"
#include "core/sensor/sensor_base.h"
#include "core/landmark/landmark_base.h"
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 7f9afd039..3aad59381 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -5,18 +5,19 @@
* \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/capture/capture_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
// Wolf includes
#include "core/state_block/state_block.h"
#include "core/common/wolf.h"
#include "core/capture/capture_pose.h"
+#include "core/ceres_wrapper/solver_ceres.h"
// STL includes
#include <map>
@@ -27,7 +28,6 @@
// General includes
#include <iostream>
#include <iomanip> // std::setprecision
-#include "../include/core/ceres_wrapper/solver_ceres.h"
using namespace wolf;
using namespace Eigen;
@@ -132,14 +132,14 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
FrameBasePtr F1 = Pr->emplaceFrame(t, Vector3d::Zero());
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
- auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false);
+ auto c1 = FactorBase::emplace<FactorRelativePose2d>(f1, f1, F0, nullptr, false, TOP_MOTION);
// KF2 and motion from KF1
t += dt;
FrameBasePtr F2 = Pr->emplaceFrame(t, Vector3d::Zero());
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
- auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false);
+ auto c2 = FactorBase::emplace<FactorRelativePose2d>(f2, f2, F1, nullptr, false, TOP_MOTION);
ASSERT_TRUE(Pr->check(0));
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 8faac803c..89670c622 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -7,12 +7,11 @@
#include "core/utils/utils_gtest.h"
-
#include "core/problem/problem.h"
#include "core/sensor/sensor_odom_3d.h"
+#include "core/ceres_wrapper/solver_ceres.h"
#include <iostream>
-#include "../include/core/ceres_wrapper/solver_ceres.h"
using namespace wolf;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 181fb1ce4..6d0e7014d 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -17,13 +17,10 @@
#include "core/solver/solver_manager.h"
#include "dummy/solver_manager_dummy.h"
#include "core/yaml/parser_yaml.h"
-
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
#include "core/capture/capture_diff_drive.h"
-//#include "core/feature/feature_diff_drive.h"
#include "core/factor/factor_diff_drive.h"
-
#include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 9c684c7bc..b8b86469e 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -1,6 +1,5 @@
#include "core/utils/utils_gtest.h"
-
#include "core/problem/problem.h"
#include "dummy/tree_manager_dummy.h"
#include "core/yaml/parser_yaml.h"
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 486ef3df9..5ec1cbe4d 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -1,12 +1,11 @@
#include "core/utils/utils_gtest.h"
-
+#include "core/factor/factor_relative_pose_3d.h"
#include "core/problem/problem.h"
#include "core/tree_manager/tree_manager_sliding_window.h"
#include "core/yaml/parser_yaml.h"
#include "core/capture/capture_void.h"
#include "core/feature/feature_base.h"
-#include "core/factor/factor_odom_3d.h"
#include "core/factor/factor_pose_3d.h"
using namespace wolf;
@@ -100,7 +99,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// displacement
auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
- auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -119,7 +118,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// displacement
auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
- auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -139,7 +138,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// displacement
auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
- auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
// Check F1 (virally) removed
EXPECT_TRUE(F1->isRemoving());
@@ -162,7 +161,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// displacement
auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
- auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
// Check F1 and F2 (virally) removed
EXPECT_TRUE(F1->isRemoving());
@@ -209,7 +208,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// displacement
auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
- auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -228,7 +227,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// displacement
auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
- auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -248,7 +247,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// displacement
auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
- auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
// Checks
EXPECT_TRUE(F1->isRemoving());
@@ -271,7 +270,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// displacement
auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
- auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
// Checks
EXPECT_TRUE(F1->isRemoving());
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index dbc9f67c3..65092ed3b 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -1,13 +1,12 @@
#include "core/utils/utils_gtest.h"
-
+#include "core/factor/factor_relative_pose_3d.h"
#include "core/problem/problem.h"
#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
#include "core/yaml/parser_yaml.h"
#include "core/capture/capture_void.h"
#include "core/capture/capture_odom_3d.h"
#include "core/feature/feature_base.h"
-#include "core/factor/factor_odom_3d.h"
#include "core/factor/factor_pose_3d.h"
#include "core/solver/factory_solver.h"
@@ -130,7 +129,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
// displacement
auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
- auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -165,7 +164,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
// displacement
auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
- auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -210,7 +209,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
// displacement
auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
- auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -264,7 +263,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
// displacement
auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
- auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -326,7 +325,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
// displacement
auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
- auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false);
+ auto c56 = FactorBase::emplace<FactorRelativePose3d>(f56, f56, F5, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -397,7 +396,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
// displacement
auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
- auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false);
+ auto c67 = FactorBase::emplace<FactorRelativePose3d>(f67, f67, F6, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -476,7 +475,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
// displacement
auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
- auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false);
+ auto c78 = FactorBase::emplace<FactorRelativePose3d>(f78, f78, F7, nullptr, false, TOP_MOTION);
// Checks
EXPECT_TRUE(F1->isRemoving()); // First frame removed
@@ -604,7 +603,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
// displacement
auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
- auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+ auto c12 = FactorBase::emplace<FactorRelativePose3d>(f12, f12, F1, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -638,7 +637,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
// displacement
auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
- auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+ auto c23 = FactorBase::emplace<FactorRelativePose3d>(f23, f23, F2, nullptr, false, TOP_MOTION);
// Check no frame removed
EXPECT_FALSE(F1->isRemoving());
@@ -681,7 +680,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
// displacement
auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
- auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+ auto c34 = FactorBase::emplace<FactorRelativePose3d>(f34, f34, F3, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -733,7 +732,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
// displacement
auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
- auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+ auto c45 = FactorBase::emplace<FactorRelativePose3d>(f45, f45, F4, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -793,7 +792,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
// displacement
auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov);
- auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false);
+ auto c56 = FactorBase::emplace<FactorRelativePose3d>(f56, f56, F5, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -862,7 +861,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
// displacement
auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov);
- auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false);
+ auto c67 = FactorBase::emplace<FactorRelativePose3d>(f67, f67, F6, nullptr, false, TOP_MOTION);
// Checks
EXPECT_FALSE(F1->isRemoving());
@@ -939,7 +938,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
// displacement
auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov);
- auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false);
+ auto c78 = FactorBase::emplace<FactorRelativePose3d>(f78, f78, F7, nullptr, false, TOP_MOTION);
// Checks
EXPECT_TRUE(F1->isRemoving()); // First frame removed
--
GitLab