diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index f1f9f02e617be2046471b7cecb26959bfe610143..34fd699a7b1461f9ab147c9d5d9f6ef2496b2f97 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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 03fc5fbb407c1f2c6914f871ede6834701af2bab..dce0203c894704953ff3001732f418b2636343a3 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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -83,7 +84,7 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state9Ptr,
StateBlockPtr _state10Ptr,
StateBlockPtr _state11Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -361,6 +362,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -379,7 +381,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -624,6 +626,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -641,7 +644,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -877,6 +880,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -893,7 +897,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -1120,6 +1124,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1135,7 +1140,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -1353,6 +1358,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1367,7 +1373,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -1576,6 +1582,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1589,7 +1596,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -1784,6 +1791,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1796,7 +1804,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -1982,6 +1990,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1993,7 +2002,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -2174,6 +2183,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2184,7 +2194,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -2356,6 +2366,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2365,7 +2376,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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
@@ -2528,6 +2539,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2536,7 +2548,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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_autodiff_distance_3d.h b/include/core/factor/factor_autodiff_distance_3d.h
index 1ef90ba493d3d3922685f7f42ff2b14402d7a546..320f7ac270cdd151f078c520561e509f108296d8 100644
--- a/include/core/factor/factor_autodiff_distance_3d.h
+++ b/include/core/factor/factor_autodiff_distance_3d.h
@@ -24,7 +24,8 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorAutodiffDistance3d",
- _frm_other,
+ _feat,
+ _frm_other,
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 82feb706d6012dbd35ff78aa09206ff3b5458f49..acbe45ec5d827c247376a130d0f5de5591c1bed3 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -66,6 +66,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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -75,6 +76,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
FactorStatus _status = FAC_ACTIVE);
FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
@@ -134,10 +136,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
**/
virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
- /** \brief Update the measurement and its square root information according to the feature
- **/
- virtual void updateMeasurementAndSquareRootInformationUpper();
-
/** \brief Returns a pointer to the feature constrained from
**/
FeatureBasePtr getFeature() const;
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 4ff0a96e89ee4bb9b6b5a77534e6656da24d7269..143308af2988a74d754f865c305e1ba089f4f778 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -33,11 +33,13 @@ class FactorBlockAbsolute : public FactorAnalytic
* \param _sb_ptr the constrained state block
*
*/
- FactorBlockAbsolute(StateBlockPtr _sb_ptr,
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
+ _feature_ptr,
nullptr,
nullptr,
nullptr,
@@ -63,13 +65,15 @@ class FactorBlockAbsolute : public FactorAnalytic
* \param _start_idx the size of the state segment that is constrained, -1 = all the
*
*/
- FactorBlockAbsolute(StateBlockPtr _sb_ptr,
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
unsigned int _start_idx,
int _size,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
+ _feature_ptr,
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 2c089128b6aa6e0190786accdf3130d73ac8e517..ebaf8bc7be42c444f74588599e5b18d7135a5a88 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -36,21 +36,22 @@ class FactorBlockDifference : public FactorAnalytic
* \param _sb_ptr the constrained state block
*
*/
- FactorBlockDifference(
- const StateBlockPtr& _sb1_ptr,
- const StateBlockPtr& _sb2_ptr,
- const FrameBasePtr& _frame_other = nullptr,
- const CaptureBasePtr& _cap_other = nullptr,
- const FeatureBasePtr& _feat_other = nullptr,
- const LandmarkBasePtr& _lmk_other = nullptr,
- unsigned int _start_idx1 = 0,
- int _size1 = -1,
- unsigned int _start_idx2 = 0,
- int _size2 = -1,
- ProcessorBasePtr _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
+ FactorBlockDifference(const FeatureBasePtr& _feature_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
+ unsigned int _start_idx1 = 0,
+ int _size1 = -1,
+ unsigned int _start_idx2 = 0,
+ int _size2 = -1,
+ ProcessorBasePtr _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockDifference",
+ _feature_ptr,
_frame_other,
_cap_other,
_feat_other,
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 7b189e4424b419b655833020ff9a94c71952748e..e267fb507ebfd811a0c17b1b6446267223510198 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -32,20 +32,20 @@ namespace wolf
WOLF_PTR_TYPEDEFS(FactorDiffDrive);
class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
- RESIDUAL_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- INTRINSICS_SIZE>
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>
{
using Base = FactorAutodiff<FactorDiffDrive, // @suppress("Type cannot be resolved")
- RESIDUAL_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- INTRINSICS_SIZE>;
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>;
public:
@@ -55,6 +55,7 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
const bool _apply_loss_function,
const FactorStatus _status = FAC_ACTIVE) :
Base("FactorDiffDrive",
+ _ftr_ptr,
_capture_origin_ptr->getFrame(),
_capture_origin_ptr,
nullptr,
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index 1ce2a375c131cea5234a5e603cdf2808764bc4cc..d8cfa08a9a9bf70c8b580f407f0983f60e791566 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -17,18 +17,19 @@ class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
{
public:
FactorOdom2d(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>("FactorOdom2d",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ _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())
{
//
}
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
index 85a1785440a03a15a899117b619d3d07aaa3eb39..a4ea7b1dff8484a89885edd424a6ac899331d3d7 100644
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ b/include/core/factor/factor_odom_2d_closeloop.h
@@ -21,13 +21,14 @@ class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>("FactorOdom2dCloseloop",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ _ftr_ptr,
+ _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())
{
//
}
diff --git a/include/core/factor/factor_odom_3d.h b/include/core/factor/factor_odom_3d.h
index 2e49daf216906b4e96d92e9c195cbff579fd6545..4e32efe4067d52c23bfe978cea7ef792214e00c9 100644
--- a/include/core/factor/factor_odom_3d.h
+++ b/include/core/factor/factor_odom_3d.h
@@ -21,10 +21,10 @@ class FactorOdom3d : public FactorAutodiff<FactorOdom3d,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);
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorOdom3d() = default;
@@ -81,18 +81,19 @@ inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorOdom3d, 6, 3, 4, 3, 4>("FactorOdom3d", // type
- _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
+ 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
{
// WOLF_TRACE("Constr ODOM 3d (f", _ftr_current_ptr->id(),
// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
@@ -105,8 +106,12 @@ inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
}
template<typename T>
-inline bool FactorOdom3d::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
- const T* const _q_past, T* _expectation_dp, T* _expectation_dq) const
+inline bool FactorOdom3d::expectation(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _expectation_dp,
+ T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
@@ -168,8 +173,11 @@ inline Eigen::VectorXd FactorOdom3d::expectation() const
}
template<typename T>
-inline bool FactorOdom3d::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
- const T* const _q_past, T* _residuals) const
+inline bool FactorOdom3d::operator ()(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _residuals) const
{
/* Residual expression
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index f11b674e7775c78c0f0f306be69c85c832e328de..6ccfb963d4384c35acc10294e99baa482eaab935 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",
+ _ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
diff --git a/include/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h
index 9a46cbab24101a69faaad78ea190f1823406dd35..e696f396bb0cf9adc21b0a91107ea2c862ccdb50 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",
+ _ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index aca1f7f966aa33e3f970e630b984b7fe145781ed..b9b6a75c3372f10f26a6bfb13724b9792943d95e 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -23,11 +23,13 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
{
public:
- FactorQuaternionAbsolute(StateBlockPtr _sb_ptr,
+ FactorQuaternionAbsolute(FeatureBasePtr _ftr_ptr,
+ StateBlockPtr _sb_ptr,
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
+ _ftr_ptr,
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_2d_analytic.h
index 2dba90bc6d4ead00634f601bf232d4471860c418..e4ccf958c00e0e6185e0cf97a20c8072517c2756 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_2d_analytic.h
@@ -19,12 +19,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FRAME
**/
FactorRelative2dAnalytic(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, _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 FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _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())
{
//
}
@@ -32,12 +44,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
FactorRelative2dAnalytic(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, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_other_ptr->getFrame()->getP(), _ftr_other_ptr->getFrame()->getO() )
+ const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ _ftr_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_other_ptr->getFrame()->getP(),
+ _ftr_other_ptr->getFrame()->getO() )
{
//
}
@@ -45,12 +69,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
FactorRelative2dAnalytic(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, nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_other_ptr->getP(), _landmark_other_ptr->getO())
+ const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP(),
+ _landmark_other_ptr->getO())
{
//
}
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 be13135c9c7e3e0899ac2bb9043f4dd1b7b60ef2..704cc3e7bc906a84a6702b6e8fdcad38766562ee 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -22,6 +22,7 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
@@ -46,13 +47,23 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
}
template<typename T>
- bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, const T* const _sp, const T* const _so,
+ bool operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _sp,
+ const T* const _so,
T* _residuals) const;
};
template<typename T>
-inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
- const T* const _o2, const T* const _ps, const T* const _os, T* _residuals) const
+inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _ps,
+ const T* const _os,
+ T* _residuals) const
{
// MAPS
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index c343dfe94748a3ba33963ad7fca306ba1d54e4ec..f412fd3688e114e6116cd17db7de724e0fb7fa85 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -76,9 +76,17 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
*/
double getMeasurement(unsigned int _ii) const;
const Eigen::VectorXd& getMeasurement() const;
+
+ private:
+ /* \brief Set the measurement and its noise
+ *
+ * WARNING: To be used once in the constructor only.
+ */
void setMeasurement(const Eigen::VectorXd& _meas);
void setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov);
void setMeasurementInformation(const Eigen::MatrixXd & _meas_info);
+
+ public:
const Eigen::MatrixXd& getMeasurementCovariance() const;
Eigen::MatrixXd getMeasurementInformation() const;
const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index 73bc391c2c038ec27da7e8b2fc936e054795a415..6d146706065f2ecc0d1ce51bfe1ac290a2f1d2bd 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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -12,7 +13,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, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _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 f2be4153e8c5c0d722fb2f3e156ad5f971e89944..4e18cb47d8485a7b9ee83853e863ddf20bb7e78a 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 FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -15,7 +16,7 @@ FactorBase::FactorBase(const std::string& _tp,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
- feature_ptr_(),
+ feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
@@ -33,9 +34,14 @@ FactorBase::FactorBase(const std::string& _tp,
feature_other_list_.push_back(_feature_other_ptr);
if (_landmark_other_ptr)
landmark_other_list_.push_back(_landmark_other_ptr);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
FactorBase::FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
@@ -44,7 +50,7 @@ FactorBase::FactorBase(const std::string& _tp,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
- feature_ptr_(),
+ feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
@@ -62,6 +68,10 @@ FactorBase::FactorBase(const std::string& _tp,
feature_other_list_.push_back(fo);
for (auto& Lo : landmark_other_list_)
landmark_other_list_.push_back(Lo);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
@@ -97,7 +107,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (frm_o)
{
frm_o->removeConstrainedBy(this_fac);
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
+ if (viral_remove_empty_parent && frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
frm_o->remove(viral_remove_empty_parent);
}
}
@@ -108,7 +118,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (cap_o)
{
cap_o->removeConstrainedBy(this_fac);
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ if (viral_remove_empty_parent && cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
cap_o->remove(viral_remove_empty_parent);
}
}
@@ -119,7 +129,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (ftr_o)
{
ftr_o->removeConstrainedBy(this_fac);
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
+ if (viral_remove_empty_parent && ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove(viral_remove_empty_parent);
}
}
@@ -130,7 +140,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (lmk_o)
{
lmk_o->removeConstrainedBy(this_fac);
- if (lmk_o->getConstrainedByList().empty())
+ if (viral_remove_empty_parent && lmk_o->getConstrainedByList().empty())
lmk_o->remove(viral_remove_empty_parent);
}
}
@@ -139,15 +149,6 @@ void FactorBase::remove(bool viral_remove_empty_parent)
}
}
-void FactorBase::updateMeasurementAndSquareRootInformationUpper()
-{
- if (getFeature())
- {
- measurement_ = getFeature()->getMeasurement();
- measurement_sqrt_information_upper_ = getFeature()->getMeasurementSquareRootInformationUpper();
- }
-}
-
CaptureBasePtr FactorBase::getCapture() const
{
assert(getFeature() != nullptr && "calling getCapture before linking with a feature");
@@ -256,9 +257,6 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
- // update measurement and info according to the feature
- updateMeasurementAndSquareRootInformationUpper();
-
// if frame, should be key
if (getCapture() and getFrame())
assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index fa7180f2de59a1263d937bee6a0c7b2e1ecc8c36..69133345e0f57c28e3135aa3b50e0d99e179be50 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -131,11 +131,6 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov)
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
-
- // call all factors to update their measurements and square root info
- WOLF_INFO_COND(!factor_list_.empty(), "Calling getMeasurementSquareRootInformationUpper() for all factors of this feature");
- for (auto fac : factor_list_)
- fac->updateMeasurementAndSquareRootInformationUpper();
}
void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info)
@@ -150,21 +145,11 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info)
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
-
- // call all factors to update their measurements and square root info
- WOLF_INFO_COND(!factor_list_.empty(), "Calling getMeasurementSquareRootInformationUpper() for all factors of this feature");
- for (auto fac : factor_list_)
- fac->updateMeasurementAndSquareRootInformationUpper();
}
void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
{
measurement_ = _meas;
-
- // call all factors to update their measurements and square root info
- WOLF_INFO_COND(!factor_list_.empty(), "Calling getMeasurementSquareRootInformationUpper() for all factors of this feature");
- for (auto fac : factor_list_)
- fac->updateMeasurementAndSquareRootInformationUpper();
}
void FeatureBase::setProblem(ProblemPtr _problem)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 6d3c459bb2f01e1f9f5acaa8914042fbc1304526..3cd90269823bcbe44202cc0b1a283a8ace2d8d98 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1004,15 +1004,15 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (sb->hasLocalParametrization())
{
if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
- auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, sb, nullptr, false);
+ auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
- auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
else
throw std::runtime_error("not implemented...!");
}
else
{
- auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
}
state_idx += sb->getSize();
cov_idx += sb->getLocalSize();
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index a6cddeff1fc847cdd792106b5a4039ca092410cc..d755b8f4d1485a4a449dca0295661b7860429ffe 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -201,17 +201,24 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty())
{
- auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
-
- // Modify existing feature --------
- feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
- feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
-
- // Modify existing factor --------
- // Instead of modifying, we remove one ctr, and create a new one.
- auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
- auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
- fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ // setMeasurement is not allowed: feature (and factor) will be removed and a new feature and factor will be emplaced
+ capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
+
+ assert(capture_existing->getFeatureList().empty());// there was only one feature!
+ auto new_feature_existing = emplaceFeature(capture_existing);
+ emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
+
+// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
+//
+// // Modify existing feature --------
+// feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
+// feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
+//
+// // Modify existing factor --------
+// // Instead of modifying, we remove one ctr, and create a new one.
+// auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
+// auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
+// fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
break;
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 2ae05abfde3dd647d6fca8ce9508f56607b29785..f8c3c76c7e5865d241846516ce9989226d9985cd 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -184,9 +184,9 @@ void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorX
// create & add factor absolute
if (is_quaternion)
- FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb, nullptr, false);
+ FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, ftr_prior, _sb, nullptr, false);
else
- FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size, nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _size, nullptr, false);
// store feature in params_prior_map_
params_prior_map_[_key] = ftr_prior;
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
index ffe372f0cb9f5671cc491f295d38ec24b305da3b..1ebe1b8fe2e4eba79fe2c9fbf236da5844327189 100644
--- a/test/dummy/factor_dummy_zero_1.h
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -13,13 +13,17 @@ WOLF_PTR_TYPEDEFS(FactorDummyZero1);
//class
class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
{
+ using Base = FactorAutodiff<FactorDummyZero1, 1, 1>;
public:
- FactorDummyZero1(const StateBlockPtr& _sb_ptr) :
- FactorAutodiff<FactorDummyZero1, 1, 1>("FactorDummy1",
- nullptr, nullptr, nullptr, nullptr,
- nullptr,
- false, FAC_ACTIVE,
- _sb_ptr)
+ FactorDummyZero1(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb_ptr) :
+ Base("FactorDummy1",
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb_ptr)
{
//
}
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_12.h
index 3a57aa386d20524fbbec30aa154f555af0fbb9a1..c60b9ce3232aa5c18807e80fbfb766cde275c953 100644
--- a/test/dummy/factor_dummy_zero_12.h
+++ b/test/dummy/factor_dummy_zero_12.h
@@ -12,9 +12,11 @@ namespace wolf {
template <unsigned int ID>
class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
{
+ using Base = FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>;
static const unsigned int id = ID;
public:
- FactorDummyZero12(const StateBlockPtr& _sb1_ptr,
+ FactorDummyZero12(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb1_ptr,
const StateBlockPtr& _sb2_ptr,
const StateBlockPtr& _sb3_ptr,
const StateBlockPtr& _sb4_ptr,
@@ -26,22 +28,24 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
const StateBlockPtr& _sb10_ptr,
const StateBlockPtr& _sb11_ptr,
const StateBlockPtr& _sb12_ptr) :
- FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>("FactorDummy12",
- nullptr, nullptr, nullptr, nullptr,
- nullptr,
- false, FAC_ACTIVE,
- _sb1_ptr,
- _sb2_ptr,
- _sb3_ptr,
- _sb4_ptr,
- _sb5_ptr,
- _sb6_ptr,
- _sb7_ptr,
- _sb8_ptr,
- _sb9_ptr,
- _sb10_ptr,
- _sb11_ptr,
- _sb12_ptr)
+ Base("FactorDummy12",
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb1_ptr,
+ _sb2_ptr,
+ _sb3_ptr,
+ _sb4_ptr,
+ _sb5_ptr,
+ _sb6_ptr,
+ _sb7_ptr,
+ _sb8_ptr,
+ _sb9_ptr,
+ _sb10_ptr,
+ _sb11_ptr,
+ _sb12_ptr)
{
assert(id > 0 && id <= 12);
}
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 6b31ffa63a398dc166a4e51fc552352f05a28390..48c7d66ff9482ed55df9f166374948a0d07ef672 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -49,23 +49,36 @@ class FactorFeatureDummy : public FactorBase
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr);
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr);
};
-inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
+inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FactorFeatureDummy", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ _feature_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
-inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr)
+inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorFeatureDummy>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorFeatureDummy>(_feature_ptr,
+ std::static_pointer_cast<FeatureBase>(_correspondant_ptr),
+ _processor_ptr);
}
} // namespace wolf
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index b4db44b7890f73289d54cb2708c85edb8a1b8b46..8adcfff4ec7891cee87da0fe0b70fd0bb15e0065 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -25,11 +25,15 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
- virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override {return true;};
+ virtual bool evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override {return true;};
/** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+ virtual void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& residual_,
+ std::vector<Eigen::MatrixXd>& jacobians_) const override {};
/** \brief Returns the jacobians computation method
**/
@@ -37,7 +41,10 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockPtr>(0);
+ }
/** \brief Returns the factor residual size
**/
@@ -45,7 +52,10 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns the factor states sizes
**/
- virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});}
+ virtual std::vector<unsigned int> getStateSizes() const override
+ {
+ return std::vector<unsigned int>({1});
+ }
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
@@ -54,18 +64,31 @@ class FactorLandmarkDummy : public FactorBase
};
-inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& /*_feature_ptr*/, const LandmarkBasePtr& _landmark_other_ptr,
+inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FactorFeatureDummy", nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status)
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
-inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorLandmarkDummy>(_feature_ptr, std::static_pointer_cast<LandmarkBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorLandmarkDummy>(_feature_ptr,
+ std::static_pointer_cast<LandmarkBase>(_correspondant_ptr),
+ _processor_ptr);
}
} // namespace wolf
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 0aa78e0ea31b11c742178ed491a016b421f70b93..bb7e299fc1a2e9462287516699756c8173792dfb 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -549,8 +549,8 @@ TEST(CeresManager, FactorsRemoveLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -591,8 +591,8 @@ TEST(CeresManager, FactorsUpdateLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 14dea0d458afbb8fa6223f0bfc8d286bc2a473e6..90b5f3381a9d20594f19623fb26e040fb36aa0b6 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -37,7 +37,7 @@ CeresManager ceres_mgr(problem_ptr);
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
-// Capture, feature and factor
+// Capture
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
////////////////////////////////////////////////////////
@@ -49,7 +49,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos
TEST(FactorBlockAbs, fac_block_abs_p)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(), nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -67,7 +67,7 @@ TEST(FactorBlockAbs, fac_block_abs_p)
TEST(FactorBlockAbs, fac_block_abs_p_tail2)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -83,7 +83,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
TEST(FactorBlockAbs, fac_block_abs_v)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureVelocity", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV(), nullptr, false);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getV(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -101,7 +101,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
TEST(FactorQuatAbs, fac_block_abs_o)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureQuaternion", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
- FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO(), nullptr, false);
+ FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0, fea0->getFrame()->getO(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 68712f969a298f5986b93378480f1a66e03cafe1..0acd77891bf1bae60fe41575edec629e9864999b 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -23,8 +23,9 @@ using namespace Eigen;
TEST(FactorAutodiff, AutodiffDummy1)
{
StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
- auto fac = std::make_shared<FactorDummyZero1>(sb);
+ auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
// COMPUTE JACOBIANS
std::vector<const double*> states_ptr({sb->getStateData()});
@@ -56,9 +57,10 @@ TEST(FactorAutodiff, AutodiffDummy12)
Eigen::VectorXd residuals;
unsigned int i;
FactorBasePtr fac = nullptr;
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
// 1 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<1>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -82,7 +84,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 2 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<2>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -106,7 +108,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 3 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<3>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -130,7 +132,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 4 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<4>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -154,7 +156,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 5 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<5>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -178,7 +180,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 6 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<6>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -202,7 +204,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 7 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<7>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -226,7 +228,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 8 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<8>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -250,7 +252,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 9 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<9>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -274,7 +276,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 10 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<10>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -298,7 +300,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 11 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<11>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -322,7 +324,7 @@ TEST(FactorAutodiff, AutodiffDummy12)
}
// 12 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<12>>(sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ fac = std::make_shared<FactorDummyZero12<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
i = fac->getSize();
// Jacobian
@@ -523,7 +525,6 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2dAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
-
const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index 985e25a23de5f1c6b137d1b477bb9a22b219fb2a..16202ca58bc8a85e86fd5dd7106ab805f6a23c55 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -59,9 +59,6 @@ class FactorAutodiffDistance3d_Test : public testing::Test
F2 = problem->emplaceFrame (KEY, pose2, 2.0);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
- f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
-
}
};
@@ -70,6 +67,8 @@ 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->operator ()(pos1.data(), pos2.data(), &res);
ASSERT_NEAR(res, 0.0, 1e-5);
@@ -79,7 +78,8 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1d(measurement));
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
double res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
@@ -92,7 +92,9 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
TEST_F(FactorAutodiffDistance3d_Test, solve)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1d(measurement));
+
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 9e4f2ef10e8bf2fc1fb9a47d48468e254e46d0af..5f30e7e55af5c4db939d7d242c96e542ad62cbaa 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -39,11 +39,13 @@ class FactorBaseTest : public testing::Test
class FactorDummy : public FactorBase
{
public:
- FactorDummy(const FrameBasePtr& _frame_other,
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtr& _frame_other,
const CaptureBasePtr& _capture_other,
const FeatureBasePtr& _feature_other,
const LandmarkBasePtr& _landmark_other) :
FactorBase("Dummy",
+ _feature,
_frame_other,
_capture_other,
_feature_other,
@@ -53,11 +55,13 @@ class FactorDummy : public FactorBase
{
//
}
- FactorDummy(const FrameBasePtrList& _frame_other_list,
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list) :
FactorBase("Dummy",
+ _feature,
_frame_other_list,
_capture_other_list,
_feature_other_list,
@@ -70,8 +74,12 @@ class FactorDummy : public FactorBase
virtual ~FactorDummy() = default;
virtual std::string getTopology() const override {return "DUMMY";}
- virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const override {return true;}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const override {}
+ virtual bool evaluate(double const* const* _parameters,
+ double* _residuals,
+ double** _jacobians) const override {return true;}
+ virtual void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& _residual,
+ std::vector<Eigen::MatrixXd>& _jacobians) const override {}
virtual JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
virtual std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
@@ -81,7 +89,7 @@ class FactorDummy : public FactorBase
TEST_F(FactorBaseTest, constructor_from_pointers)
{
- FactorDummy fac(nullptr,C0,f0,nullptr);
+ FactorDummy fac(f0,nullptr,C0,f1,nullptr);
ASSERT_TRUE(fac.getFrameOtherList().empty());
@@ -90,11 +98,15 @@ TEST_F(FactorBaseTest, constructor_from_pointers)
ASSERT_EQ(fac.getFeatureOtherList().size(), 1);
ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
}
TEST_F(FactorBaseTest, constructor_from_lists)
{
- FactorDummy fac({},{C0},{f0,f1},{});
+ FactorDummy fac(f0,{},{C0},{f0,f1},{});
ASSERT_TRUE(fac.getFrameOtherList().empty());
@@ -103,6 +115,10 @@ TEST_F(FactorBaseTest, constructor_from_lists)
ASSERT_EQ(fac.getFeatureOtherList().size(), 2);
ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 594eb169754b47fb3304536742ea445051e4da85..d9f5f7bf77869f32cb01237ba3002b08a5731f6c 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -24,7 +24,7 @@ using namespace wolf;
const Vector6d zero6 = Vector6d::Zero();
const Vector3d zero3 = zero6.head<3>();
-
+const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
class FixtureFactorBlockDifference : public testing::Test
{
@@ -35,6 +35,7 @@ class FixtureFactorBlockDifference : public testing::Test
FrameBasePtr KF1_;
CaptureBasePtr Cap_;
FeatureBasePtr Feat_;
+ FactorBlockDifferencePtr Fac_;
protected:
@@ -49,18 +50,9 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t1(1);
Vector10d x_origin = problem_->zeroState();
- Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
- KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0, 0.1);
-
- //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
- //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
- //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
-
+ KF0_ = problem_->setPriorFactor(x_origin, 1e-3 * Eigen::Matrix9d::Identity(), t0, 0.1);
KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
-
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
- Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
- Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
}
virtual void TearDown() override {}
@@ -68,20 +60,16 @@ class FixtureFactorBlockDifference : public testing::Test
TEST_F(FixtureFactorBlockDifference, CheckFactorType)
{
- // Feat_->setMeasurement()
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP()
- );
- ASSERT_EQ(Fac->getType(), "FactorBlockDifference");
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
+ ASSERT_EQ(Fac_->getType(), "FactorBlockDifference");
}
TEST_F(FixtureFactorBlockDifference, EqualP)
{
- // Feat_->setMeasurement()
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP()
- );
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -90,14 +78,12 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
-
}
TEST_F(FixtureFactorBlockDifference, EqualV)
{
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getV(), KF1_->getV()
- );
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
// perturbate
KF0_->getV()->setState(Vector3d::Random());
@@ -106,18 +92,14 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8);
-
}
-
-
TEST_F(FixtureFactorBlockDifference, DiffP)
{
Vector3d diff = Vector3d::Random();
- Feat_->setMeasurement(diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP()
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -126,18 +108,14 @@ TEST_F(FixtureFactorBlockDifference, DiffP)
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diff, 1e-8);
-
}
-
-
TEST_F(FixtureFactorBlockDifference, DiffV)
{
Vector3d diff = Vector3d::Random();
- Feat_->setMeasurement(diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getV(), KF1_->getV()
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
// perturbate
KF0_->getV()->setState(Vector3d::Random());
@@ -154,13 +132,11 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
// Vector3d diff = Vector3d::Random();
Vector1d diff; diff << 1; // measurement still of the same size as the constrained state
Matrix1d cov_diff = 1e-4 * Matrix1d::Identity(); // measurement still of the same size as the constrained state
- Feat_->setMeasurement(diff);
- Feat_->setMeasurementCovariance(cov_diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP(),
- nullptr, nullptr, nullptr, nullptr,
- 0, 1, 0, 1
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 1, 0, 1);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -171,21 +147,16 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<1>() - KF0_->getP()->getState().head<1>(), diff.head<1>(), 1e-8);
}
-
-
-
TEST_F(FixtureFactorBlockDifference, DiffPxy)
{
// Vector3d diff = Vector3d::Random();
Vector2d diff; diff << 1, 2; // measurement still of the same size as the constrained state
Matrix2d cov_diff = 1e-4 * Matrix2d::Identity(); // measurement still of the same size as the constrained state
- Feat_->setMeasurement(diff);
- Feat_->setMeasurementCovariance(cov_diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP(),
- nullptr, nullptr, nullptr, nullptr,
- 0, 2, 0, 2
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 2, 0, 2);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -201,13 +172,11 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
// Vector3d diff = Vector3d::Random();
Vector2d diff; diff << 1, 2; // measurement still of the same size as the constrained state
Matrix2d cov_diff = 1e-4 * Matrix2d::Identity(); // measurement still of the same size as the constrained state
- Feat_->setMeasurement(diff);
- Feat_->setMeasurementCovariance(cov_diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP(),
- nullptr, nullptr, nullptr, nullptr,
- 1, 2, 1, 2
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 1, 2, 1, 2);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index fd82dd83817fc2fe6991f0e8ba6bea09bf8c431e..b6804dde8458a048f4bd2a791b9f292452a830dc 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -290,7 +290,15 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ C0->getCalibration(),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -317,7 +325,15 @@ TEST_F(FactorDiffDriveTest, solve_F1)
delta1 .setZero();
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ C0->getCalibration(),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -355,7 +371,15 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
delta1 .setZero();
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ C0->getCalibration(),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
index 76d559a767f962ef7e82c41bffdded012dd4d04e..6573c73b3b8330169035e7117417c2919efbd110 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d.cpp
@@ -22,10 +22,8 @@ CeresManager ceres_mgr(problem_ptr);
FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
-// Capture, feature and factor from frm1 to frm0
+// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
-auto fac1 = FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); // create and add
TEST(FactorOdom2d, check_tree)
{
@@ -49,11 +47,14 @@ TEST(FactorOdom2d, fix_0_solve)
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
x1(2) = pi2pi(x0(2) + delta(2));
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -64,6 +65,9 @@ TEST(FactorOdom2d, fix_0_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -84,11 +88,14 @@ TEST(FactorOdom2d, fix_1_solve)
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
x1(2) = pi2pi(x0(2) + delta(2));
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
@@ -99,6 +106,9 @@ TEST(FactorOdom2d, fix_1_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
index fd8e2296c887aeb7ce6a07579eec939964d29a92..58b42ed5fb0ff9a47b8c85d3777f3742f6cfa9c6 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -22,10 +22,8 @@ CeresManager ceres_mgr(problem_ptr);
FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
-// Capture, feature and factor from frm1 to frm0
+// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
-auto fac1 = FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false); // create and add
TEST(FactorRelative2dAnalytic, check_tree)
{
@@ -53,7 +51,10 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm0, perturb frm1
frm0->fix();
@@ -64,6 +65,9 @@ TEST(FactorRelative2dAnalytic, fix_0_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -88,7 +92,10 @@ TEST(FactorRelative2dAnalytic, fix_1_solve)
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
// Fix frm1, perturb frm0
frm1->fix();
@@ -99,6 +106,9 @@ TEST(FactorRelative2dAnalytic, fix_1_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index 32cff2bd79a00fdcf18850c5f871493584e27865..ed37ce432e0d7ff2eb3454937418292d81d2bbfc 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -30,10 +30,8 @@ auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom",
FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(0));
FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
-// Capture, feature and factor from frm1 to frm0
+// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
-auto fac1 = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
TEST(FactorRelativePose2dWithExtrinsics, check_tree)
{
@@ -61,13 +59,16 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
+ // 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
+
// Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
@@ -79,6 +80,9 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm1->getState(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -103,13 +107,16 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
+ // 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
+
// Perturb frm0, fix the rest
frm1->fix();
frm0->unfix();
@@ -121,6 +128,9 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm0->getState(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -145,13 +155,16 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
+ // 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
+
// Perturb sensor P, fix the rest
frm1->fix();
frm0->fix();
@@ -163,6 +176,9 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
@@ -187,14 +203,15 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
x1(2) = pi2pi(x0(2) + delta(2));
x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
- // Set states and measurement
+ // Set states
frm0->setState(x0);
frm1->setState(x1);
cap1->setData(delta);
- fea1->setMeasurement(delta);
sensor_odom2d->setState(xs);
- //std::cout << "Sensor O: " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+ // 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
// Perturb sensor O, fix the rest
frm1->fix();
@@ -209,6 +226,9 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(sensor_odom2d->getState(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
}
}
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 2824fe179096eaca65136b5fadb31b4053bd99cc..bbb978af9c69260bb409ee28e0c9da88214609f5 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -12,7 +12,7 @@
using namespace wolf;
using namespace Eigen;
-TEST(FeatureBase, Constructor)
+TEST(FeatureBase, ConstructorCov)
{
Vector3d m; m << 1,2,3;
Matrix3d M; M.setRandom(); M = M*M.transpose();
@@ -29,7 +29,7 @@ TEST(FeatureBase, Constructor)
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
}
-TEST(FeatureBase, SetMeasurement)
+TEST(FeatureBase, ConstructorInfo)
{
Vector3d m; m << 1,2,3;
Matrix3d M; M.setRandom(); M = M*M.transpose();
@@ -38,26 +38,9 @@ TEST(FeatureBase, SetMeasurement)
Eigen::MatrixXd U = llt_of_info.matrixU();
Eigen::MatrixXd L = llt_of_info.matrixL();
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
-
- f->setMeasurement(m);
+ FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, I, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6);
-}
-
-TEST(FeatureBase, SetMeasurementCovariance)
-{
- Vector3d m; m << 1,2,3;
- Matrix3d M; M.setRandom(); M = M*M.transpose();
- Matrix3d I = M.inverse();
- Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXd U = llt_of_info.matrixU();
- Eigen::MatrixXd L = llt_of_info.matrixL();
-
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
-
- f->setMeasurementCovariance(M);
-
ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);