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

More tests in gtest_constraint_absolute

parent 8b800d41
No related branches found
No related tags found
1 merge request!243Constraint prior sensor params
...@@ -28,7 +28,7 @@ Vector10s pose9toPose10(Vector9s _pose9) ...@@ -28,7 +28,7 @@ Vector10s pose9toPose10(Vector9s _pose9)
Vector9s pose(Vector9s::Random()); Vector9s pose(Vector9s::Random());
Vector10s pose10 = pose9toPose10(pose); Vector10s pose10 = pose9toPose10(pose);
Vector9s data_var((Vector9s() << 0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2).finished()); Vector9s data_var((Vector9s() << 0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2).finished());
Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = data_var.asDiagonal(); Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = 0.01*Eigen::Matrix<Scalar,9,9>::Identity();//data_var.asDiagonal();
// perturbated priors // perturbated priors
Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25); Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
...@@ -61,9 +61,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_check) ...@@ -61,9 +61,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_check)
TEST(ConstraintBlockAbs, ctr_block_abs_p_solve) TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
{ {
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>())); FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()));
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
);
// Unfix frame 0, perturb frm0 // Unfix frame 0, perturb frm0
frm0->unfix(); frm0->unfix();
...@@ -72,25 +70,42 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_solve) ...@@ -72,25 +70,42 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
// solve for frm0 // solve for frm0
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
WOLF_INFO("SOLVER REPORT:\n",brief_report);
//only orientation is constrained //only orientation is constrained
ASSERT_MATRIX_APPROX(frm0->getState().head<3>(), pose10.head<3>(), 1e-6); ASSERT_MATRIX_APPROX(frm0->getState().head<3>(), pose10.head<3>(), 1e-6);
} }
TEST(ConstraintBlockAbs, ctr_block_abs_p_tail2_solve)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.tail<2>(), data_cov.bottomRightCorner<2,2>()));
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr(),1,2));
// Unfix frame 0, perturb frm0
frm0->unfix();
frm0->setState(x0);
// solve for frm0
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
WOLF_INFO("SOLVER REPORT:\n",brief_report);
//only orientation is constrained
ASSERT_MATRIX_APPROX(frm0->getState().tail<2>(), pose10.tail<2>(), 1e-6);
}
TEST(ConstraintBlockAbs, ctr_block_abs_v_check) TEST(ConstraintBlockAbs, ctr_block_abs_v_check)
{ {
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()));
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
} }
TEST(ConstraintBlockAbs, ctr_block_abs_v_solve) TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
{ {
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>())); FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()));
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
);
// Unfix frame 0, perturb frm0 // Unfix frame 0, perturb frm0
frm0->unfix(); frm0->unfix();
...@@ -99,6 +114,8 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve) ...@@ -99,6 +114,8 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
// solve for frm0 // solve for frm0
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
WOLF_INFO("SOLVER REPORT:\n",brief_report);
//only velocity is constrained //only velocity is constrained
ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6); ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6);
} }
...@@ -106,18 +123,15 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve) ...@@ -106,18 +123,15 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
TEST(ConstraintQuatAbs, ctr_block_abs_o_check) TEST(ConstraintQuatAbs, ctr_block_abs_o_check)
{ {
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3))); FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()));
fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
);
ASSERT_TRUE(problem->check(0)); ASSERT_TRUE(problem->check(0));
} }
TEST(ConstraintQuatAbs, ctr_block_abs_o_solve) TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
{ {
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3))); FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>( fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()));
fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
);
// Unfix frame 0, perturb frm0 // Unfix frame 0, perturb frm0
frm0->unfix(); frm0->unfix();
...@@ -126,6 +140,8 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o_solve) ...@@ -126,6 +140,8 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
// solve for frm0 // solve for frm0
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
WOLF_INFO("SOLVER REPORT:\n",brief_report);
//only velocity is constrained //only velocity is constrained
ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(3), 1e-6); ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(3), 1e-6);
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment