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

finally found the BUG!

parent 3ed8ca85
No related branches found
No related tags found
1 merge request!234WIP: Gnss
Pipeline #2563 passed
This commit is part of merge request !234. Comments created here will be created in the context of that merge request.
......@@ -148,25 +148,6 @@ 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})
# ConstraintGnssFix2D test
wolf_add_gtest(gtest_constraint_gnss_fix_2D_3 gtest_constraint_gnss_fix_2D_3.cpp)
target_link_libraries(gtest_constraint_gnss_fix_2D_3 ${PROJECT_NAME})
# ConstraintGnssFix2D test
wolf_add_gtest(gtest_constraint_gnss_fix_2D_4 gtest_constraint_gnss_fix_2D_4.cpp)
target_link_libraries(gtest_constraint_gnss_fix_2D_4 ${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})
......
This diff is collapsed.
/**
* \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();
}
/**
* \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;
};
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)
{
// 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();
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(prior_frame_ptr->getState(), frame_pose, 1e-6);
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;
FrameBasePtr first_frame_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;
first_frame_ptr = problem_ptr->setPrior(frame_pose, Matrix3s::Identity(), TimeStamp(0), Scalar(0.1));
first_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));
}
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);
}
}
}
};
////////////////////////////////////////////////////////
TEST_F(ConstraintGnssFix2DTest, check_tree)
{
// --------------------------- 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, 0);
ASSERT_EQ(num_params_reduced, 0);
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, 0);
ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 0);
ASSERT_MATRIX_APPROX(first_frame_ptr->getState().head(2), t_map_base.head(2), 1e-6);
ASSERT_MATRIX_APPROX(first_frame_ptr->getState().tail(1), o_map_base, 1e-6);
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);
// --------------------------- 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);
// 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/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();
}
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