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

[skip ci] wip

parent 6ec8549f
No related branches found
No related tags found
1 merge request!49Draft: Resolve "Adapt to new sensor constructors in core"
Hide whitespace changes
Inline Side-by-side
test/gtest_imu_2d_static_init.cpp
+ 35
− 36
View file @ 623405ad
...@@ -40,8 +40,8 @@ std::string wolf_schema_dir = _WOLF_SCHEMA_DIR; ...@@ -40,8 +40,8 @@ std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
class ProcessorImu2dStaticInitTest : public testing::Test class ProcessorImu2dStaticInitTest : public testing::Test
{ {
public: // These can be accessed in fixtures public: // These can be accessed in fixtures
ProblemPtr problem_ptr_; ProblemPtr problem_;
SensorBasePtr sensor_ptr_; SensorBasePtr sensor_;
ProcessorMotionPtr processor_motion_; ProcessorMotionPtr processor_motion_;
TimeStamp t; TimeStamp t;
TimeStamp t0; TimeStamp t0;
...@@ -51,7 +51,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -51,7 +51,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
FrameBasePtr KF0_; FrameBasePtr KF0_;
FrameBasePtr first_frame_; FrameBasePtr first_frame_;
FrameBasePtr last_frame_; FrameBasePtr last_frame_;
SolverCeresPtr solver_; SolverManagerPtr solver_;
// a new of this will be created for each new test // a new of this will be created for each new test
void SetUp() override void SetUp() override
...@@ -59,11 +59,10 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -59,11 +59,10 @@ class ProcessorImu2dStaticInitTest : public testing::Test
WOLF_INFO("Doing setup..."); WOLF_INFO("Doing setup...");
// Wolf problem // Wolf problem
problem_ptr_ = Problem::create("POV", 2); problem_ = Problem::create("POV", 2);
sensor_ptr_ = sensor_ = problem_->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir});
problem_ptr_->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir}); processor_motion_ = std::static_pointer_cast<ProcessorMotion>(
processor_motion_ = std::static_pointer_cast<ProcessorMotion>(problem_ptr_->installProcessor( problem_->installProcessor(imu_dir + "/test/yaml/processor_imu_2d_static_init.yaml", {imu_dir}));
imu_dir + "/test/yaml/processor_imu_2d_static_init.yaml", {imu_dir}));
// Time and data variables // Time and data variables
dt = 0.1; dt = 0.1;
...@@ -75,26 +74,27 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -75,26 +74,27 @@ class ProcessorImu2dStaticInitTest : public testing::Test
first_frame_ = nullptr; first_frame_ = nullptr;
// Create the solver // 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 // Set the origin
// VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()}); // 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()}); // 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_->setPriorFix(x_origin, t0);
// KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0); // KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0);
SpecComposite problem_prior{ SpecComposite problem_prior{
{'P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))}, {'P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))},
{'O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.001))}, {'O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.001))},
{'V', SpecState("StateVector2d", Vector2d::Zero(), "factor", Vector2d::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, void TestStatic(const Vector3d& body_magnitudes,
const Vector3d& bias_groundtruth, const Vector3d& bias_groundtruth,
const Vector3d& bias_initial_guess, const Vector3d& bias_initial_guess,
const std::string& test_name, const std::string& test_name,
int testing_var, int testing_var,
bool small_tol) bool small_tol)
{ {
// Data // Data
data.head(2) = body_magnitudes.head(2); data.head(2) = body_magnitudes.head(2);
...@@ -103,7 +103,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -103,7 +103,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
data(5) += bias_groundtruth(2); data(5) += bias_groundtruth(2);
// Set bias initial guess // Set bias initial guess
sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); sensor_->getIntrinsic(t0)->setState(bias_initial_guess);
processor_motion_->setOrigin(KF0_); processor_motion_->setOrigin(KF0_);
#if WRITE_CSV_FILE #if WRITE_CSV_FILE
...@@ -124,11 +124,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -124,11 +124,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test
WOLF_INFO("\n------------------------------------------------------------------------"); WOLF_INFO("\n------------------------------------------------------------------------");
// Create and process capture // 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(); C->process();
auto state = problem_ptr_->getState(); auto state = problem_->getState();
auto bias_est = sensor_ptr_->getIntrinsic()->getState(); auto bias_est = sensor_->getIntrinsic()->getState();
auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
#if WRITE_CSV_FILE #if WRITE_CSV_FILE
...@@ -167,12 +167,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -167,12 +167,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO("Solver Report: ", report); // WOLF_INFO("Solver Report: ", report);
state = problem_ptr_->getState(); state = problem_->getState();
bias_est = sensor_ptr_->getIntrinsic()->getState(); bias_est = sensor_->getIntrinsic()->getState();
bias_preint = processor_motion_->getLast()->getCalibrationPreint(); bias_preint = processor_motion_->getLast()->getCalibrationPreint();
WOLF_INFO("The current problem is:"); WOLF_INFO("The current problem is:");
problem_ptr_->print(4); problem_->print(4);
#if WRITE_CSV_FILE #if WRITE_CSV_FILE
// post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result // 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 ...@@ -245,7 +245,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
data(5) += bias_groundtruth(2); data(5) += bias_groundtruth(2);
// Set bias initial guess // Set bias initial guess
sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); sensor_->getIntrinsic(t0)->setState(bias_initial_guess);
processor_motion_->setOrigin(KF0_); processor_motion_->setOrigin(KF0_);
#if WRITE_CSV_FILE #if WRITE_CSV_FILE
...@@ -266,11 +266,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -266,11 +266,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test
WOLF_INFO("\n------------------------------------------------------------------------"); WOLF_INFO("\n------------------------------------------------------------------------");
// Create and process capture // 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(); C->process();
auto state = problem_ptr_->getState(); auto state = problem_->getState();
auto bias_est = sensor_ptr_->getIntrinsic()->getState(); auto bias_est = sensor_->getIntrinsic()->getState();
auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
#if WRITE_CSV_FILE #if WRITE_CSV_FILE
...@@ -296,12 +296,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -296,12 +296,11 @@ class ProcessorImu2dStaticInitTest : public testing::Test
last_frame_ = processor_motion_->getOrigin()->getFrame(); last_frame_ = processor_motion_->getOrigin()->getFrame();
// impose zero odometry // impose zero odometry
processor_motion_->setOdometry( processor_motion_->setOdometry(sensor_->getProblem()->stateZero(processor_motion_->getStateKeys()));
sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateKeys()));
// add zero displacement and rotation capture & feature & factor with all previous frames // add zero displacement and rotation capture & feature & factor with all previous frames
assert(sensor_ptr_->getProblem()); assert(sensor_->getProblem());
for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap()) for (auto frm_pair : sensor_->getProblem()->getTrajectory()->getFrameMap())
{ {
if (frm_pair.second == last_frame_) break; if (frm_pair.second == last_frame_) break;
auto capture_zero = auto capture_zero =
...@@ -326,12 +325,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -326,12 +325,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO("Solver Report: ", report); // WOLF_INFO("Solver Report: ", report);
state = problem_ptr_->getState(); state = problem_->getState();
bias_est = sensor_ptr_->getIntrinsic()->getState(); bias_est = sensor_->getIntrinsic()->getState();
bias_preint = processor_motion_->getLast()->getCalibrationPreint(); bias_preint = processor_motion_->getLast()->getCalibrationPreint();
WOLF_INFO("The current problem is:"); WOLF_INFO("The current problem is:");
problem_ptr_->print(4); problem_->print(4);
#if WRITE_CSV_FILE #if WRITE_CSV_FILE
// post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment