diff --git a/src/processor/processor_compass.cpp b/src/processor/processor_compass.cpp index 54c16bc7b571fd75e5060dac6bd4173aa0eed805..53bc48b420f41b2d94990ec4a2ba718796ef66f3 100644 --- a/src/processor/processor_compass.cpp +++ b/src/processor/processor_compass.cpp @@ -22,7 +22,7 @@ namespace wolf { -ProcessorCompass::ProcessorCompass(const YAML::Node &_params) : ProcessorBase("ProcessorCompass", 0, _params) +ProcessorCompass::ProcessorCompass(const YAML::Node &_params) : ProcessorBase("ProcessorCompass", {{'O', "StateQuaternion"}}, _params) { // } diff --git a/src/processor/processor_imu_2d.cpp b/src/processor/processor_imu_2d.cpp index 9f2a42b69a7b9d13faa3bca681fd9fdca5e9eedf..879e508a9ebac6fd530b572a5a7452c064c79c7d 100644 --- a/src/processor/processor_imu_2d.cpp +++ b/src/processor/processor_imu_2d.cpp @@ -32,7 +32,6 @@ namespace wolf ProcessorImu2d::ProcessorImu2d(const YAML::Node &_params) : ProcessorMotion("ProcessorImu2d", {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'V', "StateVector2d"}}, - 2, 5, 5, 5, diff --git a/src/processor/processor_imu_3d.cpp b/src/processor/processor_imu_3d.cpp index 53ec5d6831a240448e259b48b394df27fabd587e..a70c9394b17634220b0fac1702c7dd5c5c6f1553 100644 --- a/src/processor/processor_imu_3d.cpp +++ b/src/processor/processor_imu_3d.cpp @@ -32,7 +32,6 @@ namespace wolf ProcessorImu3d::ProcessorImu3d(const YAML::Node &_params) : ProcessorMotion("ProcessorImu3d", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'V', "StateVector3d"}}, - 3, 10, 10, 9, diff --git a/test/gtest_feature_imu_3d.cpp b/test/gtest_feature_imu_3d.cpp index 5c2aad3e470f7799fde3e7a9f2a8a16a14b09cbd..7b07b13bcd9584fe804c65f7d7e31996dc0695bf 100644 --- a/test/gtest_feature_imu_3d.cpp +++ b/test/gtest_feature_imu_3d.cpp @@ -84,7 +84,7 @@ class FeatureImu3d_test : public testing::Test // emplace Frame ts = problem->getTimeStamp(); - state_vec = problem->getState().vector("POV"); + state_vec = problem->getState("POV").vector("POV"); last_frame = problem->emplaceFrame(ts, "POV", state_vec); // emplace a feature diff --git a/test/gtest_imu_2d_static_init.cpp b/test/gtest_imu_2d_static_init.cpp index e682d3568d2f8a7b5c000f9944191b1a75bbd45b..9497aba141eb9c982f512360c976ed9d115a1902 100644 --- a/test/gtest_imu_2d_static_init.cpp +++ b/test/gtest_imu_2d_static_init.cpp @@ -121,7 +121,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test auto C = make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); C->process(); - auto state = problem_->getState(); + auto state = problem_->getState("POV"); auto bias_est = sensor_->getIntrinsic()->getState(); auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); @@ -161,7 +161,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); // WOLF_INFO("Solver Report: ", report); - state = problem_->getState(); + state = problem_->getState("POV"); bias_est = sensor_->getIntrinsic()->getState(); bias_preint = processor_motion_->getLast()->getCalibrationPreint(); @@ -266,7 +266,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test auto C = make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); C->process(); - auto state = problem_->getState(); + auto state = problem_->getState("POV"); auto bias_est = sensor_->getIntrinsic()->getState(); auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); @@ -329,7 +329,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); // WOLF_INFO("Solver Report: ", report); - state = problem_->getState(); + state = problem_->getState("POV"); bias_est = sensor_->getIntrinsic()->getState(); bias_preint = processor_motion_->getLast()->getCalibrationPreint(); diff --git a/test/gtest_imu_3d.cpp b/test/gtest_imu_3d.cpp index b30578252232279f685284bc1b3742329f38bb15..422dea8a4486db9e0f91931efd516f894513d57d 100644 --- a/test/gtest_imu_3d.cpp +++ b/test/gtest_imu_3d.cpp @@ -1516,7 +1516,7 @@ TEST_F(Process_Factor_Imu_ODO, MatrixXd Trj_x_optim_prc(10, Trj_D_preint_prc.cols()); for (int i = 0; i < Trj_x_optim_prc.cols(); i++) { - Trj_x_optim_prc.col(i) = problem->getState(t).vector("POV"); + Trj_x_optim_prc.col(i) = problem->getState(t, "POV").vector("POV"); t += dt; } WOLF_INFO("optimal trajectory get"); diff --git a/test/gtest_imu_3d_static_init.cpp b/test/gtest_imu_3d_static_init.cpp index cf7686ad009ee3206d895e3d656e535542c6856a..0d8511bb967b0e4f30697cd15d6437a7de7a31f1 100644 --- a/test/gtest_imu_3d_static_init.cpp +++ b/test/gtest_imu_3d_static_init.cpp @@ -125,7 +125,7 @@ class ProcessorImuStaticInitTest : public testing::Test auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); C->process(); - auto state = problem_->getState(); + auto state = problem_->getState("POV"); auto bias_est = sensor_->getIntrinsic()->getState(); auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); @@ -165,7 +165,7 @@ class ProcessorImuStaticInitTest : public testing::Test std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); // WOLF_INFO("Solver Report: ", report); - state = problem_->getState(); + state = problem_->getState("POV"); bias_est = sensor_->getIntrinsic()->getState(); bias_preint = processor_motion_->getLast()->getCalibrationPreint(); @@ -275,7 +275,7 @@ class ProcessorImuStaticInitTest : public testing::Test auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); C->process(); - auto state = problem_->getState(); + auto state = problem_->getState("POV"); auto bias_est = sensor_->getIntrinsic()->getState(); auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); @@ -338,7 +338,7 @@ class ProcessorImuStaticInitTest : public testing::Test std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); // WOLF_INFO("Solver Report: ", report); - state = problem_->getState(); + state = problem_->getState("POV"); bias_est = sensor_->getIntrinsic()->getState(); bias_preint = processor_motion_->getLast()->getCalibrationPreint(); diff --git a/test/gtest_processor_imu_2d.cpp b/test/gtest_processor_imu_2d.cpp index 4f95eaec2565341dc2cc193ae302e135e9cc63bb..418d32ec750ab8e8944ba79003ff27582b5624cf 100644 --- a/test/gtest_processor_imu_2d.cpp +++ b/test/gtest_processor_imu_2d.cpp @@ -93,16 +93,16 @@ TEST_F(ProcessorImu2dTest, MRUA) data_cov *= 1e-3; // problem->print(4,1,1,1); - WOLF_INFO("Current State: ", problem->getState()); + WOLF_INFO("Current State: ", problem->getState("POV")); for (double t = 0; t <= 1.0 + dt / 2; t += dt) { capture_imu->setTimeStamp(t); capture_imu->setData(data); capture_imu->setDataCovariance(data_cov); capture_imu->process(); - WOLF_INFO("Current State: ", problem->getState()['P'].transpose()); + WOLF_INFO("Current State: ", problem->getState("P")['P'].transpose()); WOLF_INFO((p_0 + v_0 * t + 0.5 * data.head(2) * std::pow(t, 2)).transpose()); - ASSERT_MATRIX_APPROX(p_0 + v_0 * t + 0.5 * data.head(2) * std::pow(t, 2), problem->getState()['P'], 1e-9); + ASSERT_MATRIX_APPROX(p_0 + v_0 * t + 0.5 * data.head(2) * std::pow(t, 2), problem->getState("P")['P'], 1e-9); } } @@ -124,11 +124,11 @@ TEST_F(ProcessorImu2dTest, parabola) capture_imu->setData(data); capture_imu->setDataCovariance(data_cov); capture_imu->process(); - // WOLF_DEBUG("Current State: ", problem->getState()); + // WOLF_DEBUG("Current State: ", problem->getState("POV")); pos << v0x * t, 0.5 * a_y * std::pow(t, 2); // WOLF_DEBUG("Calculated State: ", pos.transpose()); - EXPECT_MATRIX_APPROX(pos, problem->getState()['P'], 1e-9); - EXPECT_MATRIX_APPROX(Vector2d(v0x, a_y * t), problem->getState()['V'], 1e-9); + EXPECT_MATRIX_APPROX(pos, problem->getState("P")['P'], 1e-9); + EXPECT_MATRIX_APPROX(Vector2d(v0x, a_y * t), problem->getState("V")['V'], 1e-9); } } @@ -150,11 +150,11 @@ TEST_F(ProcessorImu2dTest, parabola_xy) capture_imu->setData(data); capture_imu->setDataCovariance(data_cov); capture_imu->process(); - // WOLF_DEBUG("Current State: ", problem->getState()); + // WOLF_DEBUG("Current State: ", problem->getState("POV")); pos = v0 * t + 0.5 * a * std::pow(t, 2); // WOLF_DEBUG("Calculated State: ", pos.transpose()); - EXPECT_MATRIX_APPROX(pos, problem->getState()['P'], 1e-9); - EXPECT_MATRIX_APPROX(v0 + a * t, problem->getState()['V'], 1e-9); + EXPECT_MATRIX_APPROX(pos, problem->getState("P")['P'], 1e-9); + EXPECT_MATRIX_APPROX(v0 + a * t, problem->getState("V")['V'], 1e-9); } } @@ -172,7 +172,7 @@ TEST_F(ProcessorImu2dTest, Circular_move) problem->getLastFrame()->getStateBlock('O')->setState(Vector1d(alpha)); problem->getLastFrame()->getStateBlock('V')->setState(Vector2d(0, w * r)); - WOLF_DEBUG("Current State: ", problem->getState()); + WOLF_DEBUG("Current State: ", problem->getState("POV")); for (double t = dt; t <= 0.5 + dt / 2; t += dt) { capture_imu->setTimeStamp(t); @@ -180,10 +180,10 @@ TEST_F(ProcessorImu2dTest, Circular_move) capture_imu->setData(data); capture_imu->setDataCovariance(data_cov); capture_imu->process(); - WOLF_DEBUG("Current State: ", problem->getState()); + WOLF_DEBUG("Current State: ", problem->getState("POV")); pos << r * std::cos(w * t), r * std::sin(w * t); WOLF_DEBUG("Calculated State: ", pos.transpose()); - EXPECT_MATRIX_APPROX(pos, problem->getState()['P'], 1e-9); + EXPECT_MATRIX_APPROX(pos, problem->getState("P")['P'], 1e-9); } } diff --git a/test/gtest_processor_imu_2d_with_gravity.cpp b/test/gtest_processor_imu_2d_with_gravity.cpp index bb4acc3236446e96b40bbd40d12e96f653eb2fd6..adb947e2865ac75e90bcad10a7ea67c778e1759d 100644 --- a/test/gtest_processor_imu_2d_with_gravity.cpp +++ b/test/gtest_processor_imu_2d_with_gravity.cpp @@ -86,18 +86,18 @@ TEST_F(ProcessorImu2dTest, MUA_Only_G) g << 6, 7; sensor->getStateBlock('G')->setState(g); - // WOLF_DEBUG("Current State: ", problem->getState()); + // WOLF_DEBUG("Current State: ", problem->getState("POV")); for (double t = 0 + dt; t <= 1.0 + dt / 2; t += dt) { capture_imu->setTimeStamp(t); capture_imu->setData(data); capture_imu->setDataCovariance(data_cov); capture_imu->process(); - // WOLF_INFO("Current State: ", problem->getState()['P'].transpose()); + // WOLF_INFO("Current State: ", problem->getState("P")['P'].transpose()); // WOLF_INFO((p_0 + v_0*(t-0) + 0.5*data.head(2)*std::pow(t-0, // 2)).transpose()); - ASSERT_MATRIX_APPROX(p_0 + v_0 * (t - 0) + 0.5 * g * std::pow(t - 0, 2), problem->getState()['P'], 1e-9); - EXPECT_MATRIX_APPROX(v_0 + g * (t - 0), problem->getState()['V'], 1e-9); + ASSERT_MATRIX_APPROX(p_0 + v_0 * (t - 0) + 0.5 * g * std::pow(t - 0, 2), problem->getState("P")['P'], 1e-9); + EXPECT_MATRIX_APPROX(v_0 + g * (t - 0), problem->getState("V")['V'], 1e-9); } } @@ -110,19 +110,19 @@ TEST_F(ProcessorImu2dTest, MUA) g << 6, 7; sensor->getStateBlock('G')->setState(g); - // WOLF_DEBUG("Current State: ", problem->getState()); + // WOLF_DEBUG("Current State: ", problem->getState("POV")); for (double t = 0 + dt; t <= 1.0 + dt / 2; t += dt) { capture_imu->setTimeStamp(t); capture_imu->setData(data); capture_imu->setDataCovariance(data_cov); capture_imu->process(); - // WOLF_INFO("Current State: ", problem->getState()['P'].transpose()); + // WOLF_INFO("Current State: ", problem->getState("P")['P'].transpose()); // WOLF_INFO((p_0 + v_0*(t-0) + 0.5*data.head(2)*std::pow(t-0, // 2)).transpose()); ASSERT_MATRIX_APPROX( - p_0 + v_0 * (t - 0) + 0.5 * (data.head(2) + g) * std::pow(t - 0, 2), problem->getState()['P'], 1e-9); - EXPECT_MATRIX_APPROX(v_0 + (data.head(2) + g) * (t - 0), problem->getState()['V'], 1e-9); + p_0 + v_0 * (t - 0) + 0.5 * (data.head(2) + g) * std::pow(t - 0, 2), problem->getState("P")['P'], 1e-9); + EXPECT_MATRIX_APPROX(v_0 + (data.head(2) + g) * (t - 0), problem->getState("V")['V'], 1e-9); } } @@ -136,19 +136,19 @@ TEST_F(ProcessorImu2dTest, Spinning) g << 6, 7; sensor->getStateBlock('G')->setState(g); - // WOLF_DEBUG("Current State: ", problem->getState()); + // WOLF_DEBUG("Current State: ", problem->getState("POV")); for (double t = 0 + dt; t <= 1.0 + dt / 2; t += dt) { capture_imu->setTimeStamp(t); capture_imu->setData(data); capture_imu->setDataCovariance(data_cov); capture_imu->process(); - // WOLF_INFO("Current State: ", problem->getState()['P'].transpose()); + // WOLF_INFO("Current State: ", problem->getState("P")['P'].transpose()); // WOLF_INFO((p_0 + v_0*(t-0) + 0.5*data.head(2)*std::pow(t-0, // 2)).transpose()); - ASSERT_MATRIX_APPROX(p_0 + v_0 * (t - 0) + 0.5 * g * std::pow(t - 0, 2), problem->getState()['P'], 1e-9); - EXPECT_MATRIX_APPROX(v_0 + g * (t - 0), problem->getState()['V'], 1e-9); - EXPECT_NEAR(o_0(0) + w * (t - 0), problem->getState()['O'](0), 1e-9); + ASSERT_MATRIX_APPROX(p_0 + v_0 * (t - 0) + 0.5 * g * std::pow(t - 0, 2), problem->getState("P")['P'], 1e-9); + EXPECT_MATRIX_APPROX(v_0 + g * (t - 0), problem->getState("V")['V'], 1e-9); + EXPECT_NEAR(o_0(0) + w * (t - 0), problem->getState("O")['O'](0), 1e-9); } } @@ -172,7 +172,7 @@ TEST_F(ProcessorImu2dTest, Spinning) // g << 0.1, 0; // sensor->getStateBlock('G')->setState(g); // -// WOLF_DEBUG("Current State: ", problem->getState()); +// WOLF_DEBUG("Current State: ", problem->getState("POV")); // dt = 0.001; // for(t = 0+dt; t <= 0 + 0.5 + dt/2; t+=dt){ // capture_imu->setTimeStamp(t); @@ -181,12 +181,12 @@ TEST_F(ProcessorImu2dTest, Spinning) // capture_imu->setData(data); // capture_imu->setDataCovariance(data_cov); // capture_imu->process(); -// WOLF_INFO("Current State: ", problem->getState()); +// WOLF_INFO("Current State: ", problem->getState("POV")); // pos << r*std::cos( w*(t-0) ), // r*std::sin( w*(t-0) ); // WOLF_INFO("Calculated State: ", pos.transpose()); // EXPECT_MATRIX_APPROX(Eigen::Rotation2Dd(w*(t-0))*v_0 , -// problem->getState()['V'], 1e-3); +// problem->getState("V")['V'], 1e-3); // } //} diff --git a/test/gtest_processor_imu_3d.cpp b/test/gtest_processor_imu_3d.cpp index 80b453844671a6a26d19d19f7a002c728e26c80a..0b44b0c90cf29be44e4505383fd74c1b772f5f3d 100644 --- a/test/gtest_processor_imu_3d.cpp +++ b/test/gtest_processor_imu_3d.cpp @@ -156,7 +156,7 @@ TEST_F(ProcessorImu3dTest, acc_x) Vector6d b; b << 0, 0, 0, 0, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS_SMALL); } @@ -185,7 +185,7 @@ TEST_F(ProcessorImu3dTest, acc_y) Vector6d b; b << 0, 0, 0, 0, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS_SMALL); @@ -199,7 +199,7 @@ TEST_F(ProcessorImu3dTest, acc_y) // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20 x << 0, 10, 0, 0, 0, 0, 1, 0, 20, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS); ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS); } @@ -225,7 +225,7 @@ TEST_F(ProcessorImu3dTest, acc_z) Vector6d b; b << 0, 0, 0, 0, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV").head(10), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV").head(10), x, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS_SMALL); @@ -239,7 +239,7 @@ TEST_F(ProcessorImu3dTest, acc_z) // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10 x << 0, 0, 25, 0, 0, 0, 1, 0, 0, 10; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); ASSERT_MATRIX_APPROX(processor->getLast()->getSensorIntrinsic()->getState(), b, Constants::EPS); ASSERT_MATRIX_APPROX(processor->getLast()->getCalibrationPreint(), b, Constants::EPS); } @@ -291,7 +291,7 @@ TEST_F(ProcessorImu3dTest, gyro_x) x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); // do so for 5s const unsigned int iter = 1000; // how many ms @@ -300,7 +300,7 @@ TEST_F(ProcessorImu3dTest, gyro_x) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()); capture_imu->setTimeStamp(i * dt + dt); // first one will be 0.002 and last will be 1.000 @@ -309,7 +309,7 @@ TEST_F(ProcessorImu3dTest, gyro_x) } x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_x_biasedAbx) @@ -347,7 +347,7 @@ TEST_F(ProcessorImu3dTest, gyro_x_biasedAbx) 0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 VectorXd x_est(10); - x_est = problem->getState().vector("POV"); + x_est = problem->getState("POV").vector("POV"); ASSERT_MATRIX_APPROX(x_est.transpose(), x_true.transpose(), Constants::EPS); @@ -358,7 +358,7 @@ TEST_F(ProcessorImu3dTest, gyro_x_biasedAbx) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()) + acc_bias; capture_imu->setTimeStamp(i * dt + dt); // first one will be 0.002 and last will be 1.000 @@ -367,7 +367,7 @@ TEST_F(ProcessorImu3dTest, gyro_x_biasedAbx) } x_true << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x_true, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x_true, Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_xy_biasedAbxy) @@ -401,10 +401,10 @@ TEST_F(ProcessorImu3dTest, gyro_xy_biasedAbxy) x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; //, abx,aby,0, 0,0,0; ASSERT_MATRIX_APPROX( - problem->getState().vector("POV"), + problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); // << "expected state : " << - // problem->getState().vector(problem->getFrameStructure()).transpose() + // problem->getState("POV").vector(problem->getFrameStructure()).transpose() // << "\n estimated state : " << x.transpose(); // do so for 5s @@ -414,7 +414,7 @@ TEST_F(ProcessorImu3dTest, gyro_xy_biasedAbxy) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()) + acc_bias; capture_imu->setTimeStamp(i * dt + dt); // first one will be 0.002 and last will be 1.000 @@ -423,10 +423,10 @@ TEST_F(ProcessorImu3dTest, gyro_xy_biasedAbxy) } x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); // << "estimated state is : \n" << - // problem->getState().vector(problem->getFrameStructure()).transpose() + // problem->getState("POV").vector(problem->getFrameStructure()).transpose() // << // "\n expected state is : \n" << x.transpose() << std::endl; } @@ -454,7 +454,7 @@ TEST_F(ProcessorImu3dTest, gyro_z) x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); // do so for 5s const unsigned int iter = 1000; // how many ms @@ -468,7 +468,7 @@ TEST_F(ProcessorImu3dTest, gyro_z) } x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_xyz) @@ -516,7 +516,7 @@ TEST_F(ProcessorImu3dTest, gyro_xyz) quat_comp = quat_comp * v2q(tmp_vec * dt); R0 = R0 * v2R(tmp_vec * dt); // use processorImu - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()); // gravity measured data.tail(3) = tmp_vec; @@ -548,18 +548,18 @@ TEST_F(ProcessorImu3dTest, gyro_xyz) VectorXd x(10); x << 0, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0; - Quaterniond result_quat(problem->getState().vector("O").data()); + Quaterniond result_quat(problem->getState("O").vector("O").data()); // std::cout << "final orientation : " << q2v(result_quat).transpose() << // std::endl; // check position part - ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("P").vector("P"), x.head(3), Constants::EPS); // check orientation part - ASSERT_MATRIX_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("O").vector("O"), x.segment(3, 4), Constants::EPS); // check velocity and bias parts - ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("V").vector("V"), x.segment(7, 3), Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_z_ConstVelocity) @@ -588,7 +588,7 @@ TEST_F(ProcessorImu3dTest, gyro_z_ConstVelocity) VectorXd x(10); x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); // do so for 1s const unsigned int iter = 1000; // how many ms @@ -603,9 +603,9 @@ TEST_F(ProcessorImu3dTest, gyro_z_ConstVelocity) // check states x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS); - ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS); - ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("P").vector("P"), x.head(3), Constants::EPS); + ASSERT_QUATERNION_VECTOR_APPROX(problem->getState("O").vector("O"), x.segment(3, 4), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("V").vector("V"), x.segment(7, 3), Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_x_ConstVelocity) @@ -634,7 +634,7 @@ TEST_F(ProcessorImu3dTest, gyro_x_ConstVelocity) VectorXd x(10); x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); // do so for 1s const unsigned int iter = 1000; // how many ms @@ -643,7 +643,7 @@ TEST_F(ProcessorImu3dTest, gyro_x_ConstVelocity) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()); capture_imu->setTimeStamp(i * dt + dt); // first one will be 0.002 and last will be 1.000 @@ -653,9 +653,9 @@ TEST_F(ProcessorImu3dTest, gyro_x_ConstVelocity) // check states x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS); - ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS); - ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("P").vector("P"), x.head(3), Constants::EPS); + ASSERT_QUATERNION_VECTOR_APPROX(problem->getState("O").vector("O"), x.segment(3, 4), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("V").vector("V"), x.segment(7, 3), Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_xy_ConstVelocity) @@ -684,7 +684,7 @@ TEST_F(ProcessorImu3dTest, gyro_xy_ConstVelocity) VectorXd x(10); x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); // do so for 1s const unsigned int iter = 1000; // how many ms @@ -693,7 +693,7 @@ TEST_F(ProcessorImu3dTest, gyro_xy_ConstVelocity) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()); capture_imu->setTimeStamp(i * dt + dt); // first one will be 0.002 and last will be 1.000 @@ -703,9 +703,9 @@ TEST_F(ProcessorImu3dTest, gyro_xy_ConstVelocity) // check states x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS); - ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS); - ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("P").vector("P"), x.head(3), Constants::EPS); + ASSERT_QUATERNION_VECTOR_APPROX(problem->getState("O").vector("O"), x.segment(3, 4), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("V").vector("V"), x.segment(7, 3), Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_y_ConstVelocity) @@ -734,7 +734,7 @@ TEST_F(ProcessorImu3dTest, gyro_y_ConstVelocity) VectorXd x(10); x << v * dt, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS_SMALL); // do so for 1s const unsigned int iter = 1000; // how many ms @@ -743,7 +743,7 @@ TEST_F(ProcessorImu3dTest, gyro_y_ConstVelocity) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()); capture_imu->setTimeStamp(i * dt + dt); // first one will be 0.002 and last will be 1.000 @@ -753,9 +753,9 @@ TEST_F(ProcessorImu3dTest, gyro_y_ConstVelocity) // check states x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS); - ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS); - ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("P").vector("P"), x.head(3), Constants::EPS); + ASSERT_QUATERNION_VECTOR_APPROX(problem->getState("O").vector("O"), x.segment(3, 4), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("V").vector("V"), x.segment(7, 3), Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_xyz_ConstVelocity) @@ -807,7 +807,7 @@ TEST_F(ProcessorImu3dTest, gyro_xyz_ConstVelocity) quat_comp = quat_comp * v2q(tmp_vec * dt); R0 = R0 * v2R(tmp_vec * dt); // use processorImu - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()); // gravity measured data.tail(3) = tmp_vec; @@ -840,14 +840,14 @@ TEST_F(ProcessorImu3dTest, gyro_xyz_ConstVelocity) // rotation + translation due to initial velocity x << v * dt * iter, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), v; - Quaterniond result_quat(problem->getState().vector("O").data()); + Quaterniond result_quat(problem->getState("O").vector("O").data()); // std::cout << "final orientation : " << q2v(result_quat).transpose() << // std::endl; // check states - ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS); - ASSERT_QUATERNION_VECTOR_APPROX(problem->getState().vector("O"), x.segment(3, 4), Constants::EPS); - ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7, 3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("P").vector("P"), x.head(3), Constants::EPS); + ASSERT_QUATERNION_VECTOR_APPROX(problem->getState("O").vector("O"), x.segment(3, 4), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("V").vector("V"), x.segment(7, 3), Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_x_acc_x) @@ -876,13 +876,9 @@ TEST_F(ProcessorImu3dTest, gyro_x_acc_x) // 0.001 x << 0.0000005, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0.001, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, - Constants::EPS); // << "1. current state is : \n" << - // problem->getState().vector(problem->getFrameStructure()).transpose() - // << - // "\n current x is : \n" << - // x.transpose() << std::endl; + Constants::EPS); // do so for 1s const unsigned int iter = 1000; // how many ms @@ -891,7 +887,7 @@ TEST_F(ProcessorImu3dTest, gyro_x_acc_x) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()) + (Vector3d() << 1, 0, 0).finished(); capture_imu->setTimeStamp(i * dt); // first one will be 0.002 and last will be 1.000 @@ -902,7 +898,7 @@ TEST_F(ProcessorImu3dTest, gyro_x_acc_x) // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p // = 0.5*a*dt + v*dt = 0.5 on x axis v = a*dt = 1 x << 0.5, 0, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 1, 0, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_y_acc_y) @@ -931,13 +927,9 @@ TEST_F(ProcessorImu3dTest, gyro_y_acc_y) // 0.001 x << 0, 0.0000005, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0.001, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, - Constants::EPS); // << "1. current state is : \n" << - // problem->getState().vector(problem->getFrameStructure()).transpose() - // << - // "\n current x is : \n" << - // x.transpose() << std::endl; + Constants::EPS); // do so for 1s const unsigned int iter = 1000; // how many ms @@ -946,7 +938,7 @@ TEST_F(ProcessorImu3dTest, gyro_y_acc_y) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()) + (Vector3d() << 0, 1, 0).finished(); capture_imu->setTimeStamp(i * dt); // first one will be 0.002 and last will be 1.000 @@ -957,7 +949,7 @@ TEST_F(ProcessorImu3dTest, gyro_y_acc_y) // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p // = 0.5*a*dt + v*dt = 0.5 on y axis v = a*dt = 1 x << 0, 0.5, 0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 1, 0; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); } TEST_F(ProcessorImu3dTest, gyro_z_acc_z) @@ -986,7 +978,7 @@ TEST_F(ProcessorImu3dTest, gyro_z_acc_z) // 0.001 x << 0, 0, 0.0000005, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 0.001; - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); // do so for 1s const unsigned int iter = 1000; // how many ms @@ -995,7 +987,7 @@ TEST_F(ProcessorImu3dTest, gyro_z_acc_z) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3) * dt); - Quaterniond rot(problem->getState().vector("O").data()); + Quaterniond rot(problem->getState("O").vector("O").data()); data.head(3) = rot.conjugate() * (-gravity()) + (Vector3d() << 0, 0, 1).finished(); capture_imu->setTimeStamp(i * dt); // first one will be 0.002 and last will be 1.000 @@ -1006,8 +998,8 @@ TEST_F(ProcessorImu3dTest, gyro_z_acc_z) // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p // = 0.5*a*dt + v*dt = 0.5 on z axis v = a*dt = 1 x << 0, 0, 0.5, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0, 0, 1; - WOLF_DEBUG(problem->getState().vector("POV").transpose()); - ASSERT_MATRIX_APPROX(problem->getState().vector("POV"), x, Constants::EPS); + WOLF_DEBUG(problem->getState("POV").vector("POV").transpose()); + ASSERT_MATRIX_APPROX(problem->getState("POV").vector("POV"), x, Constants::EPS); } int main(int argc, char **argv)