diff --git a/include/vision/factor/factor_AHP.h b/include/vision/factor/factor_AHP.h
index 41a54be439c7878bca6ceabf5e0c6a3e05b97b76..d8e27a505481130602514c99b0de2b0142d42654 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 = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
+ const LandmarkAHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorAHP() = default;
@@ -55,34 +55,26 @@ 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())
@@ -112,11 +104,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;
@@ -169,11 +161,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;
@@ -188,18 +180,6 @@ 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 a12fe1c128f5c9085a463af2e0628e589a19b30d..d380de5183900dad28a9b811681090403ec7a203 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 = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
+ const FeatureBasePtr& _feature_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorEpipolar() = default;
@@ -35,11 +35,6 @@ 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
@@ -47,9 +42,10 @@ 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,
@@ -72,12 +68,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;
@@ -174,13 +170,6 @@ 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 18e7ee521ae8cba0736c494ea370084fb6faffa9..c9ee85ccb4c1e4c39070568dfb2d5dc1427a84e4 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 = nullptr,
- bool _apply_loss_function = false,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
virtual ~FactorPixelHP() = default;
@@ -54,14 +54,6 @@ 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,
@@ -70,18 +62,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";
@@ -184,18 +176,6 @@ 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 c249690b056d6a41539f244f97898fc772075874..627296b8bae01036c90c69a98436e9125b05fc7f 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());
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this(), params_->apply_loss_function);
}
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());
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this(), params_->apply_loss_function);
}
}
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 5b4f95d78b8d31cf5143b71b05e22e92d3943dce..8d8149ba240a2c0ac76eea576289a82c3b4612d7 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());
+ return FactorBase::emplace<FactorEpipolar>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this(), params_->apply_loss_function);
}
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 4201b4890ab62f12773b3160411ac8e82c1005c0..d170ec5313c9d16d7511c60242707720d8e70cdf 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(), false, FAC_ACTIVE);
+ auto ctr = FactorBase::emplace<FactorAutodiffTrifocal>(ftr_last, ftr_first, ftr_mid, ftr_last, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
}
}
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index 53ea677e7ae6d18da3595da605e6dca3205841f0..841d501c8ca9a62c5adec7dbdd1e808c8a7607f7 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(), true);
+ return FactorBase::emplace<FactorAHP>(_feature_ptr, _feature_ptr, landmark_ahp, shared_from_this(), params_->apply_loss_function);
}
}
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index 26607ca275268f2cdb664a0ca1c0409d9461be8a..63c589139ece552e2abb032af413293373423095 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*/));
- 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*/));
+ 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*/));
}
};
@@ -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);
+ auto fac0 = FactorBase::emplace<FactorPixelHP>(fea0, fea0, lmk_hp, proc, false);
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*/));
- 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*/));
+ 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*/));
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*/));
- 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*/));
+ 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*/));
- 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*/));
+ 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*/));
- 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*/));
+ 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*/));
ASSERT_TRUE(problem->check());