diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp index 60c94cbe85f33e44f202687008479731b79a22f1..1c048da6e7ed1860c94b98a667b7c3305cbc7cc7 100644 --- a/test/gtest_factor_gnss_fix_2d.cpp +++ b/test/gtest_factor_gnss_fix_2d.cpp @@ -63,10 +63,11 @@ void computeParamSizes(const SolverCeresPtr& solver_ceres, int& num_params_reduc std::vector<ceres::ResidualBlockId> residual_blocks; solver_ceres->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks); - if (!solver_ceres->getCeresProblem()->IsParameterBlockConstant(pb) && !residual_blocks.empty()) + if (not solver_ceres->getCeresProblem()->IsParameterBlockConstant(pb) and + not residual_blocks.empty()) { - num_param_blocks_reduced ++; - num_params_reduced += solver_ceres->getCeresProblem()->ParameterBlockLocalSize(pb); + num_param_blocks_reduced ++; + num_params_reduced += solver_ceres->getCeresProblem()->ParameterBlockLocalSize(pb); } } } @@ -120,8 +121,8 @@ TEST(FactorGnssFix2dTest, configure_tree) gnss_sensor_ptr->process(cap_gnss_ptr); // Checks - ASSERT_TRUE(problem_ptr->check(1)); - ASSERT_TRUE(frame_ptr->isKey()); + EXPECT_TRUE(problem_ptr->check(1)); + EXPECT_TRUE(frame_ptr->isKey()); } /* @@ -152,8 +153,8 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position) int num_params_reduced, num_param_blocks_reduced; computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); - ASSERT_EQ(num_param_blocks_reduced, 1); - ASSERT_EQ(num_params_reduced, 2); + EXPECT_EQ(num_param_blocks_reduced, 1); + EXPECT_EQ(num_params_reduced, 2); // --------------------------- solve std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL); @@ -161,13 +162,13 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position) //std::cout << report << std::endl; // --------------------------- check summary parameters & residuals - ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2); - ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); + EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2); + EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(frame_ptr->getState().at('P'), t_map_base.head(2), 1e-6); + EXPECT_MATRIX_APPROX(frame_ptr->getState().at('P'), t_map_base.head(2), 1e-6); } /* @@ -197,20 +198,20 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation) int num_params_reduced, num_param_blocks_reduced; computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); - ASSERT_EQ(num_param_blocks_reduced, 1); - ASSERT_EQ(num_params_reduced, 1); + EXPECT_EQ(num_param_blocks_reduced, 1); + EXPECT_EQ(num_params_reduced, 1); // --------------------------- solve std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); // --------------------------- check summary parameters & residuals - ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); + EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6); + EXPECT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6); } /* @@ -235,25 +236,28 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw) // --------------------------- update solver solver_ceres->update(); + EXPECT_TRUE(solver_ceres->check()); // --------------------------- check problem parameters int num_params_reduced, num_param_blocks_reduced; computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); - ASSERT_EQ(num_param_blocks_reduced, 1); - ASSERT_EQ(num_params_reduced, 1); + EXPECT_EQ(num_param_blocks_reduced, 1); + EXPECT_EQ(num_params_reduced, 1); // --------------------------- solve - std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL); // --------------------------- check summary parameters & residuals - ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); + EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6); + EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6); + + problem_ptr->print(4,1,1,1); } /* @@ -284,20 +288,20 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_position) int num_params_reduced, num_param_blocks_reduced; computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); - ASSERT_EQ(num_param_blocks_reduced, 1); - ASSERT_EQ(num_params_reduced, 3); + EXPECT_EQ(num_param_blocks_reduced, 1); + EXPECT_EQ(num_params_reduced, 3); // --------------------------- solve std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); // --------------------------- check summary parameters & residuals - ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3); - ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); + EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3); + EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6); + EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6); } /* @@ -328,20 +332,20 @@ TEST(FactorGnssFix2dTest, gnss_1_base_antena) int num_params_reduced, num_param_blocks_reduced; computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); - ASSERT_EQ(num_param_blocks_reduced, 1); - ASSERT_EQ(num_params_reduced, 3); + EXPECT_EQ(num_param_blocks_reduced, 1); + EXPECT_EQ(num_params_reduced, 3); // --------------------------- solve std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); // --------------------------- check summary parameters & residuals - ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3); - ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); + EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3); + EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6); + EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6); } int main(int argc, char **argv)