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 54a6acbc8f00b24217168344b4b95302798607b3..54d7b50e007735e04a07352fdfba1a41b87b4cbe 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -51,20 +51,20 @@ FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const Fea
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,
- _apply_loss_function,
- _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getCapture()->getSensorP(),
- _ftr_ptr->getCapture()->getSensorO())
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePose2dWithExtrinsics",
+ _top,
+ _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(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
@@ -76,20 +76,20 @@ FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const Fea
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,
- _lmk_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _lmk_other_ptr->getP(),
- _lmk_other_ptr->getO(),
- _ftr_ptr->getCapture()->getSensorP(),
- _ftr_ptr->getCapture()->getSensorO())
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePose2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, _lmk_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_other_ptr->getP(),
+ _lmk_other_ptr->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
index dec430a9547d8c15edd32f06f7ef38f7c86aa89b..5f065f0bae985d849cdd6702ce823ef80bfc31d3 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -78,7 +78,7 @@ FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const Fea
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
- FactorAutodiff("FactorKfLmkPose3dWithExtrinsics",
+ FactorAutodiff("FactorRelativePose3dWithExtrinsics",
_top,
_feature_ptr,
_frame_other_ptr,
@@ -103,7 +103,7 @@ FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const Fea
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
- FactorAutodiff("FactorKfLmkPose3dWithExtrinsics",
+ FactorAutodiff("FactorRelativePose3dWithExtrinsics",
_top,
_feature_ptr,
nullptr,
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 84a4e373be68d3d1b6fe35f2f4f3e4d908564e4f..3b57c7b8b71d69e3454fe84a4ca5ca0a5b17076a 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -118,7 +118,7 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
meas_cov.bottomRightCorner(3,3) *= 1e-3;
//emplace feature and landmark
- c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, nullptr, pose_landmark, meas_cov));
+ c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
std::make_shared<StateBlock>(pos_landmark),
@@ -142,6 +142,8 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
/////////////////////////////////////////////
TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
{
+ ASSERT_TRUE(problem->check(1));
+
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
f1,
lmk1,