diff --git a/include/vision/factor/factor_AHP.h b/include/vision/factor/factor_AHP.h
index d8e27a505481130602514c99b0de2b0142d42654..41a54be439c7878bca6ceabf5e0c6a3e05b97b76 100644
--- a/include/vision/factor/factor_AHP.h
+++ b/include/vision/factor/factor_AHP.h
@@ -26,10 +26,10 @@ class FactorAHP : public FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>
public:
FactorAHP(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
+ const LandmarkAHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorAHP() = default;
@@ -55,26 +55,34 @@ class FactorAHP : public FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>
const T* const _anchor_frame_o,
const T* const _lmk_hmg,
T* _residuals) const;
+
+ // Static creator method
+ static FactorAHPPtr create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkAHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
};
inline FactorAHP::FactorAHP(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
+ const LandmarkAHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>("AHP",
- _landmark_ptr->getAnchorFrame(),
- nullptr,
- nullptr,
- _landmark_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getCapture()->getFrame()->getP(),
- _ftr_ptr->getCapture()->getFrame()->getO(),
- _landmark_ptr->getAnchorFrame()->getP(),
- _landmark_ptr->getAnchorFrame()->getO(),
- _landmark_ptr->getP()),
+ _landmark_ptr->getAnchorFrame(),
+ nullptr,
+ nullptr,
+ _landmark_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getCapture()->getFrame()->getP(),
+ _ftr_ptr->getCapture()->getFrame()->getO(),
+ _landmark_ptr->getAnchorFrame()->getP(),
+ _landmark_ptr->getAnchorFrame()->getO(),
+ _landmark_ptr->getP()),
anchor_sensor_extrinsics_p_(_ftr_ptr->getCapture()->getSensorP()->getState()),
anchor_sensor_extrinsics_o_(_ftr_ptr->getCapture()->getSensorO()->getState()),
intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState())
@@ -104,11 +112,11 @@ inline Eigen::VectorXd FactorAHP::expectation() const
template<typename T>
inline void FactorAHP::expectation(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _expectation) const
+ const T* const _current_frame_o,
+ const T* const _anchor_frame_p,
+ const T* const _anchor_frame_o,
+ const T* const _lmk_hmg,
+ T* _expectation) const
{
using namespace Eigen;
@@ -161,11 +169,11 @@ inline void FactorAHP::expectation(const T* const _current_frame_p,
template<typename T>
inline bool FactorAHP::operator ()(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _residuals) const
+ const T* const _current_frame_o,
+ const T* const _anchor_frame_p,
+ const T* const _anchor_frame_o,
+ const T* const _lmk_hmg,
+ T* _residuals) const
{
// expected
Eigen::Matrix<T, 2, 1> expected;
@@ -180,6 +188,18 @@ inline bool FactorAHP::operator ()(const T* const _current_frame_p,
return true;
}
+inline FactorAHPPtr FactorAHP::create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkAHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+{
+ // construct factor
+ FactorAHPPtr fac_ahp = std::make_shared<FactorAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
+
+ return fac_ahp;
+}
+
} // namespace wolf
#endif // FACTOR_AHP_H
diff --git a/include/vision/factor/factor_epipolar.h b/include/vision/factor/factor_epipolar.h
index d380de5183900dad28a9b811681090403ec7a203..a12fe1c128f5c9085a463af2e0628e589a19b30d 100644
--- a/include/vision/factor/factor_epipolar.h
+++ b/include/vision/factor/factor_epipolar.h
@@ -14,10 +14,10 @@ class FactorEpipolar : public FactorAutodiff<FactorEpipolar, 1, 3, 4, 3, 4, 3, 4
{
public:
FactorEpipolar(const FeatureBasePtr& _feature_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
+ const FeatureBasePtr& _feature_other_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorEpipolar() = default;
@@ -35,6 +35,11 @@ class FactorEpipolar : public FactorAutodiff<FactorEpipolar, 1, 3, 4, 3, 4, 3, 4
const T* const _sensor_o,
T* _residuals) const;
+ public:
+ static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr);
+
private:
SensorCameraPtr camera_;
Eigen::Matrix3d K_inv_; ///< Intrinsic matrix and its inverse
@@ -42,10 +47,9 @@ class FactorEpipolar : public FactorAutodiff<FactorEpipolar, 1, 3, 4, 3, 4, 3, 4
};
inline FactorEpipolar::FactorEpipolar(const FeatureBasePtr& _feature_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
+ const FeatureBasePtr& _feature_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, FactorStatus _status) :
FactorAutodiff<FactorEpipolar, 1, 3, 4, 3, 4, 3, 4>("FEATURE EPIPOLAR",
nullptr,
nullptr,
@@ -68,12 +72,12 @@ inline FactorEpipolar::FactorEpipolar(const FeatureBasePtr& _feature_ptr,
template<typename T>
inline bool FactorEpipolar::operator ()(const T* const _frame_own_p,
- const T* const _frame_own_o,
- const T* const _frame_other_p,
- const T* const _frame_other_o,
- const T* const _sensor_p,
- const T* const _sensor_o,
- T* _residuals) const
+ const T* const _frame_own_o,
+ const T* const _frame_other_p,
+ const T* const _frame_other_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ T* _residuals) const
{
using namespace Eigen;
@@ -170,6 +174,13 @@ inline bool FactorEpipolar::operator ()(const T* const _frame_own_p,
return true;
}
+inline FactorBasePtr FactorEpipolar::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr)
+{
+ return std::make_shared<FactorEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
+}
+
} // namespace wolf
#endif // FACTOR_EPIPOLAR_H
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
index c9ee85ccb4c1e4c39070568dfb2d5dc1427a84e4..18e7ee521ae8cba0736c494ea370084fb6faffa9 100644
--- a/include/vision/factor/factor_pixelHP.h
+++ b/include/vision/factor/factor_pixelHP.h
@@ -26,8 +26,8 @@ class FactorPixelHP : public FactorAutodiff<FactorPixelHP, 2, 3, 4, 3, 4, 4>
FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
const LandmarkHPPtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE);
virtual ~FactorPixelHP() = default;
@@ -54,6 +54,14 @@ class FactorPixelHP : public FactorAutodiff<FactorPixelHP, 2, 3, 4, 3, 4, 4>
const T* const _sensor_o,
const T* const _lmk_hmg,
T* _residuals) const;
+
+ // Static creator method
+ static FactorPixelHPPtr create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
};
inline FactorPixelHP::FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
@@ -62,18 +70,18 @@ inline FactorPixelHP::FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff<FactorPixelHP, 2, 3, 4, 3, 4, 4>("PIXELHP",
- nullptr,
- nullptr,
- nullptr,
- _landmark_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getCapture()->getFrame()->getP(),
- _ftr_ptr->getCapture()->getFrame()->getO(),
- _ftr_ptr->getCapture()->getSensorP(),
- _ftr_ptr->getCapture()->getSensorO(),
- _landmark_ptr->getP()),
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getCapture()->getFrame()->getP(),
+ _ftr_ptr->getCapture()->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO(),
+ _landmark_ptr->getP()),
intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState()) //TODO: intrinsic
{
// std::cout << "FactorPixelHP::Constructor\n";
@@ -176,6 +184,18 @@ inline bool FactorPixelHP::operator ()(const T* const _frame_p,
return true;
}
+inline FactorPixelHPPtr FactorPixelHP::create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+{
+ // construct factor
+ FactorPixelHPPtr fac_ahp = std::make_shared<FactorPixelHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
+
+ return fac_ahp;
+}
+
} // namespace wolf
#endif // FACTOR_AHP_H
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index 627296b8bae01036c90c69a98436e9125b05fc7f..c249690b056d6a41539f244f97898fc772075874 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -759,7 +759,7 @@ void ProcessorBundleAdjustment::establishFactors()
lmk_track_map_[trkid] = lmk;
//emplace a factor
- FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this(), params_->apply_loss_function);
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this());
}
else
@@ -769,7 +769,7 @@ void ProcessorBundleAdjustment::establishFactors()
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
//emplace a factor
- FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this(), params_->apply_loss_function);
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this());
}
}
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 8d8149ba240a2c0ac76eea576289a82c3b4612d7..5b4f95d78b8d31cf5143b71b05e22e92d3943dce 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -262,7 +262,7 @@ double ProcessorTrackerFeatureImage::match(cv::Mat _target_descriptor, cv::Mat _
FactorBasePtr ProcessorTrackerFeatureImage::emplaceFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- return FactorBase::emplace<FactorEpipolar>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this(), params_->apply_loss_function);
+ return FactorBase::emplace<FactorEpipolar>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this());
}
unsigned int ProcessorTrackerFeatureImage::detect(cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index d170ec5313c9d16d7511c60242707720d8e70cdf..4201b4890ab62f12773b3160411ac8e82c1005c0 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -432,7 +432,7 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
assert(ftr_mid != ftr_last && "Last and middle features are the same!");
// emplace factor
- auto ctr = FactorBase::emplace<FactorAutodiffTrifocal>(ftr_last, ftr_first, ftr_mid, ftr_last, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
+ auto ctr = FactorBase::emplace<FactorAutodiffTrifocal>(ftr_last, ftr_first, ftr_mid, ftr_last, shared_from_this(), false, FAC_ACTIVE);
}
}
}
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index 841d501c8ca9a62c5adec7dbdd1e808c8a7607f7..53ea677e7ae6d18da3595da605e6dca3205841f0 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -273,7 +273,7 @@ FactorBasePtr ProcessorTrackerLandmarkImage::emplaceFactor(FeatureBasePtr _featu
LandmarkAHPPtr landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
- return FactorBase::emplace<FactorAHP>(_feature_ptr, _feature_ptr, landmark_ahp, shared_from_this(), params_->apply_loss_function);
+ return FactorBase::emplace<FactorAHP>(_feature_ptr, _feature_ptr, landmark_ahp, shared_from_this(), true);
}
}
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index 63c589139ece552e2abb032af413293373423095..26607ca275268f2cdb664a0ca1c0409d9461be8a 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -188,9 +188,9 @@ class FactorPixelHPTest : public testing::Test{
L1 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP1, camera, des));
// factors
- c11 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f11, f11, L1, nullptr /*proc*/, false /*use loss function*/));
- c21 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f21, f21, L1, nullptr /*proc*/, false /*use loss function*/));
- c31 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f31, f31, L1, nullptr /*proc*/, false /*use loss function*/));
+ c11 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f11, f11, L1, nullptr /*proc*/));
+ c21 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f21, f21, L1, nullptr /*proc*/));
+ c31 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f31, f31, L1, nullptr /*proc*/));
}
};
@@ -238,7 +238,7 @@ TEST(ProcessorFactorPixelHP, testZeroResidual)
LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
// Factor
- auto fac0 = FactorBase::emplace<FactorPixelHP>(fea0, fea0, lmk_hp, proc, false);
+ auto fac0 = FactorBase::emplace<FactorPixelHP>(fea0, fea0, lmk_hp, proc);
auto fac_ptr = std::static_pointer_cast<FactorPixelHP>(fac0);
ASSERT_TRUE(problem_ptr->check(0));
@@ -382,9 +382,9 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition)
L4 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP4, camera, des));
//create factors
- c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/, false /*use loss function*/));
- c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/, false /*use loss function*/));
- c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/, false /*use loss function*/));
+ c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/));
+ c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/));
+ c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/));
ASSERT_TRUE(problem->check());
@@ -528,17 +528,17 @@ TEST_F(FactorPixelHPTest, testSolveBundleAdjustment)
L4 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP4, camera, des));
//create factors
- c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/, false /*use loss function*/));
- c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/, false /*use loss function*/));
- c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/, false /*use loss function*/));
+ c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/));
+ c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/));
+ c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/));
- c22 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f22, f22, L2, nullptr /*proc*/, false /*use loss function*/));
- c23 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f23, f23, L3, nullptr /*proc*/, false /*use loss function*/));
- c24 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f24, f24, L4, nullptr /*proc*/, false /*use loss function*/));
+ c22 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f22, f22, L2, nullptr /*proc*/));
+ c23 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f23, f23, L3, nullptr /*proc*/));
+ c24 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f24, f24, L4, nullptr /*proc*/));
- c32 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f32, f32, L2, nullptr /*proc*/, false /*use loss function*/));
- c33 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f33, f33, L3, nullptr /*proc*/, false /*use loss function*/));
- c34 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f34, f34, L4, nullptr /*proc*/, false /*use loss function*/));
+ c32 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f32, f32, L2, nullptr /*proc*/));
+ c33 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f33, f33, L3, nullptr /*proc*/));
+ c34 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f34, f34, L4, nullptr /*proc*/));
ASSERT_TRUE(problem->check());