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

Removed wrong addConstrainedBy calls

parent 82589bfc
No related branches found
No related tags found
2 merge requests!258Apriltag: some improvements/changes,!233WIP: Apriltag
Hide whitespace changes
Inline Side-by-side
test/gtest_factor_autodiff_apriltag.cpp
+ 7
− 23
View file @ 372e763c
...@@ -215,12 +215,8 @@ TEST_F(FactorAutodiffApriltag_class, Check_tree) ...@@ -215,12 +215,8 @@ TEST_F(FactorAutodiffApriltag_class, Check_tree)
CTR_ACTIVE CTR_ACTIVE
); );
FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint)); f1->addFactor(constraint);
lmk1->addConstrainedBy(constraint); lmk1->addConstrainedBy(constraint);
//check is returning true even without the lines below....
WOLF_WARN("I think the lines below are needed... to be checked !")
F1->addConstrainedBy(constraint);
f1->addConstrainedBy(constraint);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
} }
...@@ -236,10 +232,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_F1_P_perturbated) ...@@ -236,10 +232,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_F1_P_perturbated)
CTR_ACTIVE CTR_ACTIVE
); );
FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint)); f1->addFactor(constraint);
lmk1->addConstrainedBy(constraint); lmk1->addConstrainedBy(constraint);
F1->addConstrainedBy(constraint);
f1->addConstrainedBy(constraint);
// unfix F1, perturbate state // unfix F1, perturbate state
F1->unfix(); F1->unfix();
...@@ -274,10 +268,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_F1_O_perturbated) ...@@ -274,10 +268,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_F1_O_perturbated)
CTR_ACTIVE CTR_ACTIVE
); );
FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint)); f1->addFactor(constraint);
lmk1->addConstrainedBy(constraint); lmk1->addConstrainedBy(constraint);
F1->addConstrainedBy(constraint);
f1->addConstrainedBy(constraint);
// unfix F1, perturbate state // unfix F1, perturbate state
F1->unfix(); F1->unfix();
...@@ -314,10 +306,8 @@ TEST_F(FactorAutodiffApriltag_class, Check_initialization) ...@@ -314,10 +306,8 @@ TEST_F(FactorAutodiffApriltag_class, Check_initialization)
CTR_ACTIVE CTR_ACTIVE
); );
FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint)); f1->addFactor(constraint);
lmk1->addConstrainedBy(constraint); lmk1->addConstrainedBy(constraint);
F1->addConstrainedBy(constraint);
f1->addConstrainedBy(constraint);
ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6); ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6); ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
...@@ -336,10 +326,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_P_perturbated) ...@@ -336,10 +326,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_P_perturbated)
CTR_ACTIVE CTR_ACTIVE
); );
FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint)); f1->addFactor(constraint);
lmk1->addConstrainedBy(constraint); lmk1->addConstrainedBy(constraint);
F1->addConstrainedBy(constraint);
f1->addConstrainedBy(constraint);
// unfix lmk1, perturbate state // unfix lmk1, perturbate state
...@@ -375,10 +363,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_O_perturbated) ...@@ -375,10 +363,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_O_perturbated)
CTR_ACTIVE CTR_ACTIVE
); );
FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint)); f1->addFactor(constraint);
lmk1->addConstrainedBy(constraint); lmk1->addConstrainedBy(constraint);
F1->addConstrainedBy(constraint);
f1->addConstrainedBy(constraint);
// unfix F1, perturbate state // unfix F1, perturbate state
lmk1->unfix(); lmk1->unfix();
...@@ -414,10 +400,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_PO_perturbated) ...@@ -414,10 +400,8 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_PO_perturbated)
CTR_ACTIVE CTR_ACTIVE
); );
FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint)); f1->addFactor(constraint);
lmk1->addConstrainedBy(constraint); lmk1->addConstrainedBy(constraint);
F1->addConstrainedBy(constraint);
f1->addConstrainedBy(constraint);
// Change setup // Change setup
Vector3s p_w_r, p_r_c, p_c_l, p_w_l; Vector3s p_w_r, p_r_c, p_c_l, p_w_l;
......
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