Skip to content
Snippets Groups Projects
Commit 64c3f514 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

gtests ok

parent 31f3fe71
No related branches found
No related tags found
2 merge requests!440New tag,!416Resolve "New factor FactorKfLmkPose3dWithExtrinsicsPtr"
Pipeline #8613 passed
Hide whitespace changes
Inline Side-by-side
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+ 28
− 28
View file @ 64c3f514
...@@ -51,20 +51,20 @@ FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const Fea ...@@ -51,20 +51,20 @@ FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const Fea
const ProcessorBasePtr& _processor_ptr, const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, bool _apply_loss_function,
const FactorTopology& _top, const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status) :
FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics", FactorAutodiff("FactorRelativePose2dWithExtrinsics",
_top, _top,
_ftr_ptr, _ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr, _frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr, _processor_ptr,
_apply_loss_function, _apply_loss_function,
_status, _status,
_frame_other_ptr->getP(), _frame_other_ptr->getP(),
_frame_other_ptr->getO(), _frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getSensorP(), _ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO()) _ftr_ptr->getCapture()->getSensorO())
{ {
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!"); assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!"); assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
...@@ -76,20 +76,20 @@ FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const Fea ...@@ -76,20 +76,20 @@ FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const Fea
const ProcessorBasePtr& _processor_ptr, const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, bool _apply_loss_function,
const FactorTopology& _top, const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status) :
FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics", FactorAutodiff("FactorRelativePose2dWithExtrinsics",
_top, _top,
_ftr_ptr, _ftr_ptr,
_lmk_other_ptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, _lmk_other_ptr,
_processor_ptr, _processor_ptr,
_apply_loss_function, _apply_loss_function,
_status, _status,
_ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getO(),
_lmk_other_ptr->getP(), _lmk_other_ptr->getP(),
_lmk_other_ptr->getO(), _lmk_other_ptr->getO(),
_ftr_ptr->getCapture()->getSensorP(), _ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO()) _ftr_ptr->getCapture()->getSensorO())
{ {
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!"); assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!"); assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
......
include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+ 2
− 2
View file @ 64c3f514
...@@ -78,7 +78,7 @@ FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const Fea ...@@ -78,7 +78,7 @@ FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const Fea
bool _apply_loss_function, bool _apply_loss_function,
const FactorTopology& _top, const FactorTopology& _top,
FactorStatus _status) : FactorStatus _status) :
FactorAutodiff("FactorKfLmkPose3dWithExtrinsics", FactorAutodiff("FactorRelativePose3dWithExtrinsics",
_top, _top,
_feature_ptr, _feature_ptr,
_frame_other_ptr, _frame_other_ptr,
...@@ -103,7 +103,7 @@ FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const Fea ...@@ -103,7 +103,7 @@ FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const Fea
bool _apply_loss_function, bool _apply_loss_function,
const FactorTopology& _top, const FactorTopology& _top,
FactorStatus _status) : FactorStatus _status) :
FactorAutodiff("FactorKfLmkPose3dWithExtrinsics", FactorAutodiff("FactorRelativePose3dWithExtrinsics",
_top, _top,
_feature_ptr, _feature_ptr,
nullptr, nullptr,
......
test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+ 3
− 1
View file @ 64c3f514
...@@ -118,7 +118,7 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{ ...@@ -118,7 +118,7 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
meas_cov.bottomRightCorner(3,3) *= 1e-3; meas_cov.bottomRightCorner(3,3) *= 1e-3;
//emplace feature and landmark //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)); f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose", lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
std::make_shared<StateBlock>(pos_landmark), std::make_shared<StateBlock>(pos_landmark),
...@@ -142,6 +142,8 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor) ...@@ -142,6 +142,8 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
///////////////////////////////////////////// /////////////////////////////////////////////
TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree) TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
{ {
ASSERT_TRUE(problem->check(1));
auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1, auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
f1, f1,
lmk1, lmk1,
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment