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

gtests ok

parent 31f3fe71
Branches 402-new-factor-factorkflmkpose3dwithextrinsicsptr
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
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!");
......
include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+ 2
− 2
View file @ 64c3f514
......@@ -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,
......
test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+ 3
− 1
View file @ 64c3f514
......@@ -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,
......
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