From 623405ad8112a342147fe141c970e77d824c58e7 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Tue, 25 Jul 2023 14:21:58 +0200
Subject: [PATCH] [skip ci] wip
---
test/gtest_imu_2d_static_init.cpp | 71 +++++++++++++++----------------
1 file changed, 35 insertions(+), 36 deletions(-)
diff --git a/test/gtest_imu_2d_static_init.cpp b/test/gtest_imu_2d_static_init.cpp
index 12cb3c697..86f4128b3 100644
--- a/test/gtest_imu_2d_static_init.cpp
+++ b/test/gtest_imu_2d_static_init.cpp
@@ -40,8 +40,8 @@ std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
class ProcessorImu2dStaticInitTest : public testing::Test
{
public: // These can be accessed in fixtures
- ProblemPtr problem_ptr_;
- SensorBasePtr sensor_ptr_;
+ ProblemPtr problem_;
+ SensorBasePtr sensor_;
ProcessorMotionPtr processor_motion_;
TimeStamp t;
TimeStamp t0;
@@ -51,7 +51,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
FrameBasePtr KF0_;
FrameBasePtr first_frame_;
FrameBasePtr last_frame_;
- SolverCeresPtr solver_;
+ SolverManagerPtr solver_;
// a new of this will be created for each new test
void SetUp() override
@@ -59,11 +59,10 @@ class ProcessorImu2dStaticInitTest : public testing::Test
WOLF_INFO("Doing setup...");
// Wolf problem
- problem_ptr_ = Problem::create("POV", 2);
- sensor_ptr_ =
- problem_ptr_->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir});
- processor_motion_ = std::static_pointer_cast<ProcessorMotion>(problem_ptr_->installProcessor(
- imu_dir + "/test/yaml/processor_imu_2d_static_init.yaml", {imu_dir}));
+ problem_ = Problem::create("POV", 2);
+ sensor_ = problem_->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir});
+ processor_motion_ = std::static_pointer_cast<ProcessorMotion>(
+ problem_->installProcessor(imu_dir + "/test/yaml/processor_imu_2d_static_init.yaml", {imu_dir}));
// Time and data variables
dt = 0.1;
@@ -75,26 +74,27 @@ class ProcessorImu2dStaticInitTest : public testing::Test
first_frame_ = nullptr;
// Create the solver
- solver_ = make_shared<SolverCeres>(problem_ptr_);
+ solver_ = FactorySolverFile::create(
+ "SolverCeres", problem_, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
// Set the origin
// VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
// VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
- // //KF0_ = problem_ptr_->setPriorFix(x_origin, t0);
- // KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0);
+ // //KF0_ = problem_->setPriorFix(x_origin, t0);
+ // KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0);
SpecComposite problem_prior{
{'P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))},
{'O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.001))},
{'V', SpecState("StateVector2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))}};
- KF0_ = problem_ptr_->setPrior(problem_prior, t0);
+ KF0_ = problem_->setPrior(problem_prior, t0);
}
- void TestStatic(const Vector3d& body_magnitudes,
- const Vector3d& bias_groundtruth,
- const Vector3d& bias_initial_guess,
- const std::string& test_name,
- int testing_var,
- bool small_tol)
+ void TestStatic(const Vector3d& body_magnitudes,
+ const Vector3d& bias_groundtruth,
+ const Vector3d& bias_initial_guess,
+ const std::string& test_name,
+ int testing_var,
+ bool small_tol)
{
// Data
data.head(2) = body_magnitudes.head(2);
@@ -103,7 +103,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
data(5) += bias_groundtruth(2);
// Set bias initial guess
- sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ sensor_->getIntrinsic(t0)->setState(bias_initial_guess);
processor_motion_->setOrigin(KF0_);
#if WRITE_CSV_FILE
@@ -124,11 +124,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test
WOLF_INFO("\n------------------------------------------------------------------------");
// Create and process capture
- auto C = make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ auto C = make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front());
C->process();
- auto state = problem_ptr_->getState();
- auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+ auto state = problem_->getState();
+ auto bias_est = sensor_->getIntrinsic()->getState();
auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
#if WRITE_CSV_FILE
@@ -167,12 +167,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO("Solver Report: ", report);
- state = problem_ptr_->getState();
- bias_est = sensor_ptr_->getIntrinsic()->getState();
+ state = problem_->getState();
+ bias_est = sensor_->getIntrinsic()->getState();
bias_preint = processor_motion_->getLast()->getCalibrationPreint();
WOLF_INFO("The current problem is:");
- problem_ptr_->print(4);
+ problem_->print(4);
#if WRITE_CSV_FILE
// post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
@@ -245,7 +245,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
data(5) += bias_groundtruth(2);
// Set bias initial guess
- sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ sensor_->getIntrinsic(t0)->setState(bias_initial_guess);
processor_motion_->setOrigin(KF0_);
#if WRITE_CSV_FILE
@@ -266,11 +266,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test
WOLF_INFO("\n------------------------------------------------------------------------");
// Create and process capture
- auto C = make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ auto C = make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front());
C->process();
- auto state = problem_ptr_->getState();
- auto bias_est = sensor_ptr_->getIntrinsic()->getState();
+ auto state = problem_->getState();
+ auto bias_est = sensor_->getIntrinsic()->getState();
auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
#if WRITE_CSV_FILE
@@ -296,12 +296,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test
last_frame_ = processor_motion_->getOrigin()->getFrame();
// impose zero odometry
- processor_motion_->setOdometry(
- sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateKeys()));
+ processor_motion_->setOdometry(sensor_->getProblem()->stateZero(processor_motion_->getStateKeys()));
// add zero displacement and rotation capture & feature & factor with all previous frames
- assert(sensor_ptr_->getProblem());
- for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
+ assert(sensor_->getProblem());
+ for (auto frm_pair : sensor_->getProblem()->getTrajectory()->getFrameMap())
{
if (frm_pair.second == last_frame_) break;
auto capture_zero =
@@ -326,12 +325,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO("Solver Report: ", report);
- state = problem_ptr_->getState();
- bias_est = sensor_ptr_->getIntrinsic()->getState();
+ state = problem_->getState();
+ bias_est = sensor_->getIntrinsic()->getState();
bias_preint = processor_motion_->getLast()->getCalibrationPreint();
WOLF_INFO("The current problem is:");
- problem_ptr_->print(4);
+ problem_->print(4);
#if WRITE_CSV_FILE
// post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
--
GitLab