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

ceres bug investigation

parent bac5a67e
No related branches found
No related tags found
1 merge request!234WIP: Gnss
Pipeline #2531 failed
......@@ -148,6 +148,13 @@ 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_1 gtest_constraint_gnss_fix_2D_1.cpp)
target_link_libraries(gtest_constraint_gnss_fix_2D_1 ${PROJECT_NAME})
# ConstraintGnssFix2D test
wolf_add_gtest(gtest_constraint_gnss_fix_2D_2 gtest_constraint_gnss_fix_2D_2.cpp)
target_link_libraries(gtest_constraint_gnss_fix_2D_2 ${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})
......
/**
* \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/capture/capture_motion.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;
using std::cout;
using std::endl;
class ConstraintGnssFix2DTest : public testing::Test
{
public:
// groundtruth transformations
Vector3s t_ecef_enu;
Matrix3s R_ecef_enu, R_enu_map, R_map_base;
Vector3s t_base_antena, t_ecef_antena;
Vector1s o_enu_map;
Vector3s t_enu_map;
Vector3s t_map_base;
Vector1s o_map_base;
// WOLF
ProblemPtr problem_ptr;
CeresManagerPtr ceres_mgr_ptr;
SensorGnssPtr gnss_sensor_ptr;
ConstraintGnssFix2DTest()
{
o_enu_map << 2.6;
t_enu_map << 12, -34, 4;
t_map_base << 32, -34, 0; // z=0
o_map_base << -0.4;
t_base_antena << 3,-2,8;// Antena extrinsics
t_ecef_enu << 100,30,50;
R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX())
*AngleAxis<Scalar>(-2.2, Vector3s::UnitY())
*AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
R_map_base = Matrix3s::Identity();
R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
t_ecef_antena = R_ecef_enu * R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_ecef_enu;
// ----------------------------------------------------
// Problem and solver
problem_ptr = Problem::create("PO 2D");
ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
// gnss sensor & processor
gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
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));
std::cout << "gnss sensor installed" << std::endl;
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);
std::cout << "gnss processor installed" << std::endl;
// set prior (FIXED)
Eigen::Vector3s frame_pose;
frame_pose << t_map_base.head(2), o_map_base;
FrameBasePtr prior_frame_ptr = problem_ptr->setPrior(frame_pose, Matrix3s::Identity(), TimeStamp(0), Scalar(0.1));
prior_frame_ptr->fix();
};
virtual void SetUp()
{
// reset grountruth parameters
R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
R_map_base = Matrix3s::Identity();
R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
// Reset antena extrinsics
gnss_sensor_ptr->getPPtr()->setState(t_base_antena);
// Reset ENU_ECEF
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
}
};
////////////////////////////////////////////////////////
TEST_F(ConstraintGnssFix2DTest, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
/*
* Test with one GNSS fix capture.
*
* Estimating: MAP_BASE position.
* Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
*/
TEST_F(ConstraintGnssFix2DTest, gnss_1_map_base_position)
{
// Create 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);
// fixing things
gnss_sensor_ptr->getEnuMapTranslationStatePtr()->fix();// enu-map yaw translation
gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();// enu-map yaw orientation
cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base); // frame orientation
cap_gnss_ptr->getFramePtr()->getOPtr()->fix();
// distort frm position
Vector3s frm_dist = cap_gnss_ptr->getFramePtr()->getState();
frm_dist(0) += 0.18;
frm_dist(1) += -0.58;
cap_gnss_ptr->getFramePtr()->setState(frm_dist);
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
std::cout << report << std::endl;
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();
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
/**
* \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/capture/capture_motion.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;
using std::cout;
using std::endl;
class ConstraintGnssFix2DTest : public testing::Test
{
public:
// groundtruth transformations
Vector3s t_ecef_enu;
Matrix3s R_ecef_enu, R_enu_map, R_map_base;
Vector3s t_base_antena, t_ecef_antena;
Vector1s o_enu_map;
Vector3s t_enu_map;
Vector3s t_map_base;
Vector1s o_map_base;
// WOLF
ProblemPtr problem_ptr;
CeresManagerPtr ceres_mgr_ptr;
SensorGnssPtr gnss_sensor_ptr;
ConstraintGnssFix2DTest()
{
o_enu_map << 2.6;
t_enu_map << 12, -34, 4;
t_map_base << 32, -34, 0; // z=0
o_map_base << -0.4;
t_base_antena << 3,-2,8;// Antena extrinsics
t_ecef_enu << 100,30,50;
R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX())
*AngleAxis<Scalar>(-2.2, Vector3s::UnitY())
*AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
R_map_base = Matrix3s::Identity();
R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
t_ecef_antena = R_ecef_enu * R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_ecef_enu;
// ----------------------------------------------------
// Problem and solver
problem_ptr = Problem::create("PO 2D");
ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
// gnss sensor & processor
gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
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));
std::cout << "gnss sensor installed" << std::endl;
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);
std::cout << "gnss processor installed" << std::endl;
// set prior (FIXED)
Eigen::Vector3s frame_pose;
frame_pose << t_map_base.head(2), o_map_base;
FrameBasePtr prior_frame_ptr = problem_ptr->setPrior(frame_pose, Matrix3s::Identity(), TimeStamp(0), Scalar(0.1));
prior_frame_ptr->fix();
};
virtual void SetUp()
{
// reset grountruth parameters
R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
R_map_base = Matrix3s::Identity();
R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
// Reset antena extrinsics
gnss_sensor_ptr->getPPtr()->setState(t_base_antena);
// Reset ENU_ECEF
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
}
};
////////////////////////////////////////////////////////
TEST_F(ConstraintGnssFix2DTest, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
/*
* Test with one GNSS fix capture.
*
* Estimating: MAP_BASE position.
* Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
*/
TEST_F(ConstraintGnssFix2DTest, gnss_1_map_base_position)
{
// Create 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);
// fixing things
gnss_sensor_ptr->getEnuMapTranslationStatePtr()->fix();// enu-map yaw translation
gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();// enu-map yaw orientation
cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base); // frame orientation
cap_gnss_ptr->getFramePtr()->getOPtr()->fix();
// distort frm position
Vector3s frm_dist = cap_gnss_ptr->getFramePtr()->getState();
frm_dist(0) += 0.18;
frm_dist(1) += -0.58;
cap_gnss_ptr->getFramePtr()->setState(frm_dist);
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
std::cout << report << std::endl;
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();
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
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