new gtest for gnss fix 2D debugging strange ceres bug

test/CMakeLists.txt

@@ -148,6 +148,10 @@ ENDIF(vision_utils_FOUND)
 wolf_add_gtest(gtest_constraint_gnss_fix_2D gtest_constraint_gnss_fix_2D.cpp)
 target_link_libraries(gtest_constraint_gnss_fix_2D ${PROJECT_NAME})
 
+# ConstraintGnssFix2D test
+wolf_add_gtest(gtest_constraint_gnss_fix_2D_simple gtest_constraint_gnss_fix_2D_simple.cpp)
+target_link_libraries(gtest_constraint_gnss_fix_2D_simple ${PROJECT_NAME})
+
 # ConstraintGnssSingleDiff2D test
 wolf_add_gtest(gtest_constraint_gnss_single_diff_2D gtest_constraint_gnss_single_diff_2D.cpp)
 target_link_libraries(gtest_constraint_gnss_single_diff_2D ${PROJECT_NAME})

test/gtest_constraint_gnss_fix_2D_simple.cpp

+/**
+ * \file gtest_constraint_gnss_fix_2D.cpp
+ *
+ *  Created on: Aug 1, 2018
+ *      \author: jvallve
+ */
+
+
+#include "base/constraint/constraint_gnss_fix_2D.h"
+#include "utils_gtest.h"
+
+#include "base/problem.h"
+#include "base/sensor/sensor_gnss.h"
+#include "base/processor/processor_gnss_fix.h"
+
+#include "base/ceres_wrapper/ceres_manager.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
+                 const Vector1s& o_enu_map,
+                 const Vector3s& t_base_antena,
+                 const Vector3s& t_enu_map,
+                 const Vector3s& t_map_base,
+                 const Vector1s& o_map_base)
+{
+    // ENU-MAP
+    gnss_sensor_ptr->getEnuMapRollStatePtr()->setState(Eigen::Vector1s::Zero());
+    gnss_sensor_ptr->getEnuMapPitchStatePtr()->setState(Eigen::Vector1s::Zero());
+    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map);
+    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map);
+    // Antena
+    gnss_sensor_ptr->getPPtr()->setState(t_base_antena);
+    // Frame
+    frame_ptr->getPPtr()->setState(t_map_base.head(2));
+    frame_ptr->getOPtr()->setState(o_map_base);
+}
+
+void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr)
+{
+    // ENU-MAP
+    gnss_sensor_ptr->getEnuMapRollStatePtr()->fix();
+    gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix();
+    gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();
+    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->fix();
+    // Antena
+    gnss_sensor_ptr->getPPtr()->fix();
+    // Frame
+    frame_ptr->getPPtr()->fix();
+    frame_ptr->getOPtr()->fix();
+ }
+
+void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_reduced, int& num_param_blocks_reduced )
+{
+    num_param_blocks_reduced = 0;
+    num_params_reduced = 0;
+
+    std::vector<Scalar*> param_blocks;
+    ceres_mgr_ptr->getCeresProblemPtr()->GetParameterBlocks(&param_blocks);
+
+    for (auto pb : param_blocks)
+    {
+        std::vector<ceres::ResidualBlockId> residual_blocks;
+        ceres_mgr_ptr->getCeresProblemPtr()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
+
+        if (!ceres_mgr_ptr->getCeresProblemPtr()->IsParameterBlockConstant(pb) && !residual_blocks.empty())
+        {
+            num_param_blocks_reduced ++;
+            num_params_reduced += ceres_mgr_ptr->getCeresProblemPtr()->ParameterBlockLocalSize(pb);
+        }
+    }
+}
+
+// groundtruth transformations
+Vector1s o_enu_map = (Vector1s() << 2.6).finished();
+Vector3s t_enu_map = (Vector3s() << 12, -34, 4).finished();
+Vector3s t_map_base = (Vector3s() << 32, -34, 0).finished(); // z=0
+Vector1s o_map_base = (Vector1s() << -0.4).finished();
+Vector3s t_base_antena = (Vector3s() << 3,-2,8).finished();// Antena extrinsics
+Vector3s t_ecef_enu = (Vector3s() << 100,30,50).finished();
+Matrix3s R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX()) *
+                       AngleAxis<Scalar>(-2.2, Vector3s::UnitY()) *
+                       AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
+Matrix3s R_enu_map  = AngleAxis<Scalar>(o_enu_map(0),  Vector3s::UnitZ()).toRotationMatrix();
+Matrix3s R_map_base = AngleAxis<Scalar>(o_map_base(0), Vector3s::UnitZ()).toRotationMatrix();
+Vector3s t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+
+// WOLF
+ProblemPtr problem_ptr = Problem::create("PO 2D");
+CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
+SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
+FrameBasePtr frame_ptr;
+
+////////////////////////////////////////////////////////
+
+TEST(ConstraintGnssFix2DTest, configure_tree)
+{
+    ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
+
+    // Configure sensor and processor
+    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map);
+    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map);
+    gnss_sensor_ptr->getEnuMapRollStatePtr()->fix();
+    gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix();
+    gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
+
+    ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
+    gnss_params_ptr->time_tolerance = 1.0;
+    gnss_params_ptr->voting_active = true;
+    problem_ptr->installProcessor("GNSS FIX", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
+
+    // Emplace a frame (FIXED)
+    Vector3s frame_pose = (Vector3s() << t_map_base(0), t_map_base(1), o_map_base(0)).finished();
+    frame_ptr = problem_ptr->emplaceFrame(KEY_FRAME, frame_pose, TimeStamp(0));
+    problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0);
+
+    // Create & process GNSS Fix capture
+    CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
+    gnss_sensor_ptr->process(cap_gnss_ptr);
+
+    // Checks
+    ASSERT_TRUE(problem_ptr->check(0));
+    ASSERT_TRUE(frame_ptr->isKey());
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE position.
+ * Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
+ */
+TEST(ConstraintGnssFix2DTest, gnss_1_map_base_position)
+{
+    // --------------------------- Reset states
+    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+
+    // --------------------------- fixing/unfixing states
+    fixAllStates(gnss_sensor_ptr, frame_ptr);
+    frame_ptr->getPPtr()->unfix();
+
+    // --------------------------- distort: map base position
+    Vector3s frm_dist = frame_ptr->getState();
+    frm_dist(0) += 0.18;
+    frm_dist(1) += -0.58;
+    frame_ptr->setState(frm_dist);
+
+    // --------------------------- update solver
+    ceres_mgr_ptr->update();
+
+    // --------------------------- check problem parameters
+    int num_params_reduced, num_param_blocks_reduced;
+    computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+
+    ASSERT_EQ(num_param_blocks_reduced, 1);
+    ASSERT_EQ(num_params_reduced, 2);
+
+    // --------------------------- check residuals
+//    int num_residual_blocks_reduced(0);
+//    int num_residual_reduced(0);
+//    ceres_mgr_ptr->getCeresProblem().GetResidualBlocks(&residual_blocks);
+//    for (auto rb : residual_blocks)
+//        if (!ceres_mgr_ptr->getCeresProblem().IsParameterBlockConstant(rb) && ceres_mgr_ptr->getCeresProblem().GetResidualBlocksForParameterBlock(pg))
+//        {
+//            num_param_blocks_reduced ++;
+//            num_params_reduced += ceres_mgr_ptr->getCeresProblem().ParameterBlockLocalSize(rb);
+//        }
+//
+//    ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
+//    ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+
+    // --------------------------- solve
+    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
+
+    std::cout << report << std::endl;
+
+    // --------------------------- check summary parameters & residuals
+    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);
+
+    // --------------------------- check solver solution
+    ASSERT_MATRIX_APPROX(frame_ptr->getState().head(2), t_map_base.head(2), 1e-6);
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE orientation.
+ * Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
+ */
+TEST(ConstraintGnssFix2DTest, gnss_1_map_base_orientation)
+{
+    // --------------------------- Reset states
+    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+
+    // --------------------------- fixing/unfixing states
+    fixAllStates(gnss_sensor_ptr, frame_ptr);
+    frame_ptr->getOPtr()->unfix();
+
+    // --------------------------- distort: map base orientation
+    Vector1s frm_dist = frame_ptr->getOPtr()->getState();
+    frm_dist(0) += 0.18;
+    frame_ptr->getOPtr()->setState(frm_dist);
+
+    // --------------------------- update solver
+    ceres_mgr_ptr->update();
+
+    // --------------------------- check problem parameters
+    int num_params_reduced, num_param_blocks_reduced;
+    computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+
+    ASSERT_EQ(num_param_blocks_reduced, 1);
+    ASSERT_EQ(num_params_reduced, 1);
+
+    // --------------------------- solve
+    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+
+    // --------------------------- check summary parameters & residuals
+    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);
+
+    // --------------------------- check solver solution
+    ASSERT_MATRIX_APPROX(frame_ptr->getOPtr()->getState(), o_map_base, 1e-6);
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP yaw.
+ * Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
+ */
+TEST(ConstraintGnssFix2DTest, gnss_1_enu_map_yaw)
+{
+    // --------------------------- Reset states
+    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+
+    // --------------------------- fixing/unfixing states
+    fixAllStates(gnss_sensor_ptr, frame_ptr);
+    gnss_sensor_ptr->getEnuMapYawStatePtr()->unfix();
+
+    // --------------------------- distort: yaw enu_map
+    Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawStatePtr()->getState();
+    o_enu_map_dist(0) += 0.18;
+    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map_dist);
+
+    // --------------------------- update solver
+    ceres_mgr_ptr->update();
+
+    // --------------------------- check problem parameters
+    int num_params_reduced, num_param_blocks_reduced;
+    computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+
+    ASSERT_EQ(num_param_blocks_reduced, 1);
+    ASSERT_EQ(num_params_reduced, 1);
+
+    // --------------------------- solve
+    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+
+    // --------------------------- check summary parameters & residuals
+    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);
+
+    // --------------------------- check solver solution
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(), o_enu_map, 1e-6);
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: ENU_MAP position.
+ * Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
+ */
+TEST(ConstraintGnssFix2DTest, gnss_1_enu_map_position)
+{
+    // --------------------------- Reset states
+    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+
+    // --------------------------- fixing/unfixing states
+    fixAllStates(gnss_sensor_ptr, frame_ptr);
+    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->unfix();// enu-map yaw translation
+
+    // --------------------------- distort: position enu_map
+    Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslationStatePtr()->getState();
+    t_enu_map_dist(0) += 0.86;
+    t_enu_map_dist(1) += -0.34;
+    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map_dist);
+
+    // --------------------------- update solver
+    ceres_mgr_ptr->update();
+
+    // --------------------------- check problem parameters
+    int num_params_reduced, num_param_blocks_reduced;
+    computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+
+    ASSERT_EQ(num_param_blocks_reduced, 1);
+    ASSERT_EQ(num_params_reduced, 3);
+
+    // --------------------------- solve
+    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+
+    // --------------------------- check summary parameters & residuals
+    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);
+
+    // --------------------------- check solver solution
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslationStatePtr()->getState().head(2), t_enu_map.head(2), 1e-6);
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: BASE_ANTENA (2 first components that are observable).
+ * Fixed: ENU_MAP, MAP_BASE.
+ */
+TEST(ConstraintGnssFix2DTest, gnss_1_base_antena)
+{
+    // --------------------------- Reset states
+    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+
+    // --------------------------- fixing/unfixing states
+    fixAllStates(gnss_sensor_ptr, frame_ptr);
+    gnss_sensor_ptr->getPPtr()->unfix();
+
+    // --------------------------- distort: base_antena
+    Vector3s base_antena_dist = gnss_sensor_ptr->getPPtr()->getState();
+    base_antena_dist(0) += 1.68;
+    base_antena_dist(0) += -0.48;
+    gnss_sensor_ptr->getPPtr()->setState(base_antena_dist);
+
+    // --------------------------- update solver
+    ceres_mgr_ptr->update();
+
+    // --------------------------- check problem parameters
+    int num_params_reduced, num_param_blocks_reduced;
+    computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+
+    ASSERT_EQ(num_param_blocks_reduced, 1);
+    ASSERT_EQ(num_params_reduced, 3);
+
+    // --------------------------- solve
+    std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+
+    // --------------------------- check summary parameters & residuals
+    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);
+
+    // --------------------------- check solver solution
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getPPtr()->getState().head(2), t_base_antena.head(2), 1e-6);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+