diff --git a/test/gtest_constraint_gnss_fix_2D.cpp b/test/gtest_constraint_gnss_fix_2D.cpp index c525ec9326143592d848250b1c679dfdd7951f1a..fe1f6567144beb725c994215e564835f0ed0c1f4 100644 --- a/test/gtest_constraint_gnss_fix_2D.cpp +++ b/test/gtest_constraint_gnss_fix_2D.cpp @@ -153,10 +153,15 @@ TEST_F(ConstraintGnssFix2DTest, gnss_1_map_base_position) std::cout << report << std::endl; - ASSERT_TRUE(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced == 1); - ASSERT_TRUE(ceres_mgr_ptr->getSummary().num_parameters == 2); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 2); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getState().head(2), t_map_base.head(2), 1e-6); + + cap_gnss_ptr->getFramePtr()->remove(); } /* @@ -185,8 +190,15 @@ TEST_F(ConstraintGnssFix2DTest, gnss_1_map_base_orientation) // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getOPtr()->getState(), o_map_base, 1e-6); + + cap_gnss_ptr->getFramePtr()->remove(); } /* @@ -219,8 +231,15 @@ TEST_F(ConstraintGnssFix2DTest, gnss_1_enu_map_yaw) // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(), o_enu_map, 1e-6); + + cap_gnss_ptr->getFramePtr()->remove(); } /* @@ -254,8 +273,15 @@ TEST_F(ConstraintGnssFix2DTest, gnss_1_enu_map_position) // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslationStatePtr()->getState().head(2), t_enu_map.head(2), 1e-6); + + cap_gnss_ptr->getFramePtr()->remove(); } /* @@ -289,8 +315,15 @@ TEST_F(ConstraintGnssFix2DTest, gnss_1_base_antena) // solve for frm std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey()); ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getPPtr()->getState().head(2), t_base_antena.head(2), 1e-6); + + cap_gnss_ptr->getFramePtr()->remove(); } int main(int argc, char **argv)