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

removed debug lines

parent 14616cf9
No related branches found
No related tags found
1 merge request!243Constraint prior sensor params
...@@ -27,8 +27,7 @@ Vector10s pose9toPose10(Vector9s _pose9) ...@@ -27,8 +27,7 @@ Vector10s pose9toPose10(Vector9s _pose9)
// Input pose9 and covariance // Input pose9 and covariance
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()); Eigen::Matrix<wolf::Scalar, 9, 9> data_cov = 0.2 * Eigen::Matrix<Scalar,9,9>::Identity();
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);
...@@ -63,8 +62,6 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p) ...@@ -63,8 +62,6 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p)
// 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->getPPtr()->getState(), pose10.head<3>(), 1e-6); ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState(), pose10.head<3>(), 1e-6);
} }
...@@ -81,8 +78,6 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_tail2) ...@@ -81,8 +78,6 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_tail2)
// 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->getPPtr()->getState().tail<2>(), pose10.tail<2>(), 1e-6); ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState().tail<2>(), pose10.tail<2>(), 1e-6);
} }
...@@ -101,8 +96,6 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v) ...@@ -101,8 +96,6 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v)
// 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->getVPtr()->getState(), pose10.tail<3>(), 1e-6); ASSERT_MATRIX_APPROX(frm0->getVPtr()->getState(), pose10.tail<3>(), 1e-6);
} }
...@@ -121,8 +114,6 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o) ...@@ -121,8 +114,6 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o)
// 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->getOPtr()->getState(), pose10.segment<4>(3), 1e-6); ASSERT_MATRIX_APPROX(frm0->getOPtr()->getState(), 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