diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 7483ef45e2d3cd122ff094d072c9a359a34d8f8c..d8ea81ae0bbe3ec3fa4547961ab715eb810662f7 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -87,10 +87,10 @@ class HasStateBlocks VectorComposite getState(const StateStructure& structure="") const; void setState(const VectorComposite& _state, const bool _notify = true); void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true); - void setState(const Eigen::VectorXd& _state, const StateStructure& _structure="", const bool _notify = true); + void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify = true); void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true); - VectorXd getStateVector(const StateStructure& structure="") const; + VectorXd getStateVector(const StateStructure& structure) const; unsigned int getSize(const StateStructure& _structure="") const; unsigned int getLocalSize(const StateStructure& _structure="") const; diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index abd76885b291b4af5a3315f773da2ddbf028047b..b8fab49284566748ce110c2d6300da7578db8bea 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -33,10 +33,10 @@ namespace wolf { unsigned int SensorBase::sensor_id_count_ = 0; -SpecStates specs_po_2d({{'P',SpecState("StateBlock",2,"Sensor extrinsics in 2D: position (x, y) of the sensor in robot coordinates [m].")}, - {'O',SpecState("StateAngle",1,"Sensor extrinsics in 2D: orientation (yaw) of the sensor in robot coordinates [rad].")}}); -SpecStates specs_po_3d({{'P',SpecState("StateBlock",3,"Sensor extrinsics in 2D: position (x, y, z) of the sensor in robot coordinates [m].")}, - {'O',SpecState("StateQuaternion",4,"Sensor extrinsics in 2D: orientation (quaternion) of the sensor in robot coordinates.")}}); +SpecState spec_p_2d = SpecState("StateBlock", 2,"Sensor extrinsics in 2D: position (x, y) of the sensor in robot coordinates [m]."); +SpecState spec_p_3d = SpecState("StateBlock", 3,"Sensor extrinsics in 3D: position (x, y, z) of the sensor in robot coordinates [m]."); +SpecState spec_o_2d = SpecState("StateAngle", 1,"Sensor extrinsics in 2D: orientation (yaw) of the sensor in robot coordinates [rad]."); +SpecState spec_o_3d = SpecState("StateQuaternion",4,"Sensor extrinsics in 3D: orientation (quaternion) of the sensor in robot coordinates."); SensorBase::SensorBase(const std::string& _type, const std::string& _unique_name, @@ -45,7 +45,7 @@ SensorBase::SensorBase(const std::string& _type, const Priors& _priors, SpecStates _state_specs) : NodeBase("SENSOR", _type, _unique_name), - HasStateBlocks(""), + HasStateBlocks("PO"), hardware_ptr_(), sensor_id_(++sensor_id_count_), // simple ID factory params_(_params), @@ -54,19 +54,16 @@ SensorBase::SensorBase(const std::string& _type, last_capture_(nullptr) { assert(_dim == 2 or _dim == 3); - assert(_state_specs.count('P') == 0 and - _state_specs.count('O') == 0 and - "SensorBase: '_state_specs' should not contain 'P' or 'O' keys"); // check params valid if (not params_) throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null"); - // append P, O to _state_specs - if (_dim == 2) - _state_specs.insert(specs_po_2d.begin(),specs_po_2d.end()); - else - _state_specs.insert(specs_po_3d.begin(),specs_po_3d.end()); + // append default P and O specs to _state_specs (if missing) + if (_state_specs.count('P') == 0) + _state_specs.emplace('P',_dim == 2 ? spec_p_2d : spec_p_3d); + if (_state_specs.count('O') == 0) + _state_specs.emplace('O', _dim == 2 ? spec_o_2d : spec_o_3d); // load priors loadPriors(_priors, _dim, _state_specs); @@ -79,7 +76,7 @@ SensorBase::SensorBase(const std::string& _type, const ParamsServer& _server, SpecStates _state_specs) : NodeBase("SENSOR", _type, _unique_name), - HasStateBlocks(""), + HasStateBlocks("PO"), hardware_ptr_(), sensor_id_(++sensor_id_count_), // simple ID factory params_(_params), @@ -88,19 +85,16 @@ SensorBase::SensorBase(const std::string& _type, last_capture_(nullptr) { assert(_dim == 2 or _dim == 3); - assert(_state_specs.count('P') == 0 and - _state_specs.count('O') == 0 and - "SensorBase: _state_specs should not contain 'P' or 'O' keys"); // check params valid if (not params_) throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null"); - // append P, O to _state_specs - if (_dim == 2) - _state_specs.insert(specs_po_2d.begin(),specs_po_2d.end()); - else - _state_specs.insert(specs_po_3d.begin(),specs_po_3d.end()); + // append default P and O specs to _state_specs (if missing) + if (_state_specs.count('P') == 0) + _state_specs.emplace('P',_dim == 2 ? spec_p_2d : spec_p_3d); + if (_state_specs.count('O') == 0) + _state_specs.emplace('O', _dim == 2 ? spec_o_2d : spec_o_3d); // create priors Priors priors; @@ -676,7 +670,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st else if (_metric) { _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") << ",\t x = ( " << std::setprecision(3) - << getStateVector().transpose() << " )" << std::endl; + << getStateVector(getStructure()).transpose() << " )" << std::endl; } else if (_state_blocks) { diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp index 181188658a4776bf657ba7acb6c86f88775c92e4..d152ebcc0b0361d6a222e5679b863edcb1621e2c 100644 --- a/src/state_block/has_state_blocks.cpp +++ b/src/state_block/has_state_blocks.cpp @@ -108,7 +108,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& else if (_metric) { _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") - << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )" + << ",\t x = ( " << std::setprecision(3) << getStateVector(getStructure()).transpose() << " )" << std::endl; } else if (_state_blocks) diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp index 2a98cc7a39d0d0b3ce8b6ab6de18bb01ae737eab..4d7137a2028c90a15ca8817f09a11962e6d22764 100644 --- a/test/gtest_factor_absolute.cpp +++ b/test/gtest_factor_absolute.cpp @@ -77,7 +77,7 @@ TEST(FactorBlockAbs, fac_block_abs_p) // Unfix frame 0, perturb frm0 frm0->unfix(); - frm0->setState(x0); + frm0->setState(x0,"POV"); // solve for frm0 std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); @@ -93,7 +93,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2) // Unfix frame 0, perturb frm0 frm0->unfix(); - frm0->setState(x0); + frm0->setState(x0,"POV"); // solve for frm0 std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); @@ -111,7 +111,7 @@ TEST(FactorBlockAbs, fac_block_abs_v) // Unfix frame 0, perturb frm0 frm0->unfix(); - frm0->setState(x0); + frm0->setState(x0,"POV"); // solve for frm0 std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); @@ -129,7 +129,7 @@ TEST(FactorQuatAbs, fac_block_abs_o) // Unfix frame 0, perturb frm0 frm0->unfix(); - frm0->setState(x0); + frm0->setState(x0,"POV"); // solve for frm0 std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF); diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index b8266a980592cf6f7b1ee31c15162c914d044708..0d03e872da2f6aaa9905e576b85385f8242b08e9 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -318,7 +318,7 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg) processor->getCalibration(C0), Matrix3d::Identity()); // TODO Jacobian? - F1->setState(Vector3d(1.5, -1.5, -M_PI_2)); + F1->setState(Vector3d(1.5, -1.5, -M_PI_2),"PO"); // wolf: emplace c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false); @@ -353,7 +353,7 @@ TEST_F(FactorDiffDriveTest, solve_F1) processor->getCalibration(C0), Matrix3d::Identity()); // TODO Jacobian? - F1->setState(Vector3d(1.5, -1.5, -M_PI_2)); + F1->setState(Vector3d(1.5, -1.5, -M_PI_2),"PO"); // wolf: emplace c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false); @@ -370,7 +370,7 @@ TEST_F(FactorDiffDriveTest, solve_F1) // WOLF_TRACE("\n", report); problem->print(1,0,1,1); - ASSERT_MATRIX_APPROX(F1->getStateVector(), delta, 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), delta, 1e-6); } @@ -399,7 +399,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics) processor->getCalibration(C0), Matrix3d::Identity()); // TODO Jacobian? - F1->setState(Vector3d(1.5, -1.5, -M_PI_2)); + F1->setState(Vector3d(1.5, -1.5, -M_PI_2),"PO"); // wolf: emplace c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false); @@ -542,7 +542,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) WOLF_TRACE("\n", report); WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose()); - WOLF_TRACE("x2 : ", F2->getStateVector().transpose()); + WOLF_TRACE("x2 : ", F2->getStateVector("PO").transpose()); WOLF_TRACE("calib_preint : ", calib_preint.transpose()); WOLF_TRACE("calib_pert : ", calib_pert.transpose()); WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose()); @@ -680,7 +680,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) WOLF_TRACE("\n", report); WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose()); - WOLF_TRACE("x2 : ", F2->getStateVector().transpose()); + WOLF_TRACE("x2 : ", F2->getStateVector("PO").transpose()); WOLF_TRACE("calib_preint : ", calib_preint.transpose()); WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose()); WOLF_TRACE("calib_GT : ", calib_gt.transpose()); diff --git a/test/gtest_factor_odom_2d_autodiff.cpp b/test/gtest_factor_odom_2d_autodiff.cpp index fd8aebb18f89a0d5276396d2940591ab7b9dcdcd..a8eed809e2800acac9d6e66c97f0b1b9e9f9ea1f 100644 --- a/test/gtest_factor_odom_2d_autodiff.cpp +++ b/test/gtest_factor_odom_2d_autodiff.cpp @@ -69,8 +69,8 @@ TEST(FactorOdom2dAutodiff, fix_0_solve) x1(2) = pi2pi(x0(2) + delta(2)); // Set states - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); // feature & factor with delta measurement @@ -85,7 +85,7 @@ TEST(FactorOdom2dAutodiff, fix_0_solve) // solve for frm1 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); + ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); @@ -110,8 +110,8 @@ TEST(FactorOdom2dAutodiff, fix_1_solve) x1(2) = pi2pi(x0(2) + delta(2)); // Set states - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); // feature & factor with delta measurement @@ -126,7 +126,7 @@ TEST(FactorOdom2dAutodiff, fix_1_solve) // solve for frm0 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); + ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp index 498df49f23cfea49b7cb80e223b9e2ccfbe65692..a352ae12ebf13b2d312a6b4a7333f47b644a37f3 100644 --- a/test/gtest_factor_pose_2d.cpp +++ b/test/gtest_factor_pose_2d.cpp @@ -73,12 +73,12 @@ TEST(FactorPose2d, solve) // Fix frame 0, perturb frm1 frm0->unfix(); - frm0->setState(x0); + frm0->setState(x0,"PO"); // solve for frm0 std::string report = solver.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose, 1e-6); + ASSERT_MATRIX_APPROX(frm0->getStateVector("PO"), pose, 1e-6); } diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp index 544ed512744a9b7d6b6e1e2ca99cf39cbc5ed164..c3548e02dc394eb80b90f0e7a04d7afd3bda3ed8 100644 --- a/test/gtest_factor_pose_3d.cpp +++ b/test/gtest_factor_pose_3d.cpp @@ -80,12 +80,12 @@ TEST(FactorPose3d, solve) // Fix frame 0, perturb frm1 frm0->unfix(); - frm0->setState(x0); + frm0->setState(x0,"PO"); // solve for frm0 std::string brief_report = solver.solve(SolverManager::ReportVerbosity::FULL); - ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose7, 1e-6); + ASSERT_MATRIX_APPROX(frm0->getStateVector("PO"), pose7, 1e-6); } diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp index cc4030a9e51617399f2b256047f24eb30a93f5df..63eed7c281b4bfb8c5ae3ba213c2fc49726a151b 100644 --- a/test/gtest_factor_relative_pose_2d.cpp +++ b/test/gtest_factor_relative_pose_2d.cpp @@ -69,8 +69,8 @@ TEST(FactorRelativePose2d, fix_0_solve) x1(2) = pi2pi(x0(2) + delta(2)); // Set states and measurement - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); // feature & factor with delta measurement @@ -85,7 +85,7 @@ TEST(FactorRelativePose2d, fix_0_solve) // solve for frm1 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); + ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); @@ -110,8 +110,8 @@ TEST(FactorRelativePose2d, fix_1_solve) x1(2) = pi2pi(x0(2) + delta(2)); // Set states and measurement - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); // feature & factor with delta measurement @@ -126,7 +126,7 @@ TEST(FactorRelativePose2d, fix_1_solve) // solve for frm0 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); + ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index ca8aba7d3179a8d0596dc45fd570f416f83b1bf6..86578050a1fb1375fe9e18aa1fb518db07bdb4b4 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -28,7 +28,6 @@ #include "core/processor/processor_odom_2d.h" #include "core/math/rotations.h" - using namespace Eigen; using namespace wolf; using std::cout; @@ -81,10 +80,10 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve) x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>(); // Set states - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); - sensor_odom2d->setState(xs); + sensor_odom2d->setState(xs, "PO"); // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); @@ -100,7 +99,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve) // solve for frm1 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); + ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); @@ -129,10 +128,10 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve) x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>(); // Set states - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); - sensor_odom2d->setState(xs); + sensor_odom2d->setState(xs,"PO"); // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); @@ -148,7 +147,7 @@ TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve) // solve for frm0 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); + ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); @@ -177,10 +176,10 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve) x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>(); // Set states - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); - sensor_odom2d->setState(xs); + sensor_odom2d->setState(xs,"PO"); // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); @@ -196,7 +195,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve) // solve for frm0 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6); + ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector("PO"), xs, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); @@ -225,10 +224,10 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve) x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>(); // Set states - frm0->setState(x0); - frm1->setState(x1); + frm0->setState(x0,"PO"); + frm1->setState(x1,"PO"); cap1->setData(delta); - sensor_odom2d->setState(xs); + sensor_odom2d->setState(xs,"PO"); // feature & factor with delta measurement auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov); @@ -246,7 +245,7 @@ TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve) // solve for frm0 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6); + ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector("PO"), xs, 1e-6); // remove feature (and factor) for the next loop fea1->remove(); diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp index 9369c090efecad4b74435214230fca2b3b01f959..0588ab5b0b63c15e09d6137ccc52d06d4bf89b16 100644 --- a/test/gtest_factor_relative_pose_3d.cpp +++ b/test/gtest_factor_relative_pose_3d.cpp @@ -84,12 +84,12 @@ TEST(FactorRelativePose3d, fix_0_solve) // Fix frame 0, perturb frm1 frm0->fix(); frm1->unfix(); - frm1->setState(x1); + frm1->setState(x1,"PO"); // solve for frm1 std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6); + ASSERT_MATRIX_APPROX(frm1->getStateVector("PO"), delta, 1e-6); } @@ -98,12 +98,12 @@ TEST(FactorRelativePose3d, fix_1_solve) // Fix frame 1, perturb frm0 frm0->unfix(); frm1->fix(); - frm0->setState(x0); + frm0->setState(x0,"PO"); // solve for frm0 std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF); - ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6); + ASSERT_MATRIX_APPROX(frm0->getStateVector("PO"), problem_ptr->stateZero().vector("PO"), 1e-6); } int main(int argc, char **argv) diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp index ee0ff88d7b8ddbf8301616920c753e1039c2e6a8..3c35668b0aaa1bd4a96a359e21158834338f58c8 100644 --- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp @@ -180,7 +180,7 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated) F1->getP()->perturb(); std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6); } TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated) @@ -196,7 +196,7 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated) F1->getO()->perturb(); std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6); } TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization) @@ -207,9 +207,9 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization) nullptr, false); - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6); ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6); } @@ -226,8 +226,8 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated) lmk1->getP()->perturb(); std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6); } TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated) @@ -243,8 +243,8 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated) lmk1->getO()->perturb(); std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), pose_robot, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), pose_landmark, 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6); } @@ -298,16 +298,16 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated) Vector7d t_w_r, t_w_l; t_w_r << p_w_r, q_w_r.coeffs(); t_w_l << p_w_l, q_w_l.coeffs(); - ASSERT_MATRIX_APPROX(F1->getState().vector("PO"), t_w_r, 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO"), t_w_l, 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), t_w_r, 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), t_w_l, 1e-6); // unfix LMK, perturbate state lmk1->unfix(); lmk1->perturb(); std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport - ASSERT_MATRIX_APPROX(F1->getState().vector("PO").transpose(), t_w_r.transpose(), 1e-6); - ASSERT_MATRIX_APPROX(lmk1->getState().vector("PO").transpose(), t_w_l.transpose(), 1e-6); + ASSERT_MATRIX_APPROX(F1->getStateVector("PO").transpose(), t_w_r.transpose(), 1e-6); + ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO").transpose(), t_w_l.transpose(), 1e-6); } diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 2b2da613e168a941d58b2de6fe2d4af0bdf7ea9e..6cab028bb08b6eb4b70f7056684aca44ba4bced3 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -410,13 +410,13 @@ TEST(FrameBase, GetSetState) x << p, q, v; // Set the state, check that state blocks hold the current states - F.setState(x); + F.setState(x,"POV"); ASSERT_MATRIX_APPROX(p, F.getP()->getState(), Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(q, F.getO()->getState(), Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(v, F.getV()->getState(), Constants::EPS_SMALL); // get the state, form 2 by return value - x2 = F.getStateVector(); + x2 = F.getStateVector("POV"); ASSERT_MATRIX_APPROX(x2, x, Constants::EPS_SMALL); } diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index 8d5caccb0533e14ced1993fa733717362d481485..eaaaef6ff3378a6dfc5f0ad5e020e1b9cd7147bb 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -112,7 +112,7 @@ void show(const ProblemPtr& problem) << F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl; } } - cout << " state: \n" << F->getStateVector().transpose() << endl; + cout << " state: \n" << F->getStateVector("PO").transpose() << endl; Eigen::MatrixXd cov; problem->getFrameCovariance(F,cov); cout << " covariance: \n" << cov << endl; @@ -166,9 +166,9 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) ASSERT_TRUE(Pr->check(0)); // cout << "===== full ====" << endl; - F0->setState(Vector3d(1,2,3)); - F1->setState(Vector3d(2,3,1)); - F2->setState(Vector3d(3,1,2)); + F0->setState(Vector3d(1,2,3),"PO"); + F1->setState(Vector3d(2,3,1),"PO"); + F2->setState(Vector3d(3,1,2),"PO"); std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); // std::cout << report << std::endl; @@ -189,12 +189,12 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver)); ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver)); - ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6); + ASSERT_POSE2d_APPROX(F0->getStateVector("PO"), Vector3d(0,0,0), 1e-6); auto s0_vector = s0.vector("PO"); ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((s0_vector.array() * s0_vector.array()).matrix().asDiagonal()), 1e-6); - ASSERT_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6); + ASSERT_POSE2d_APPROX(F1->getStateVector("PO"), Vector3d(2,0,0), 1e-6); ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6); - ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6); + ASSERT_POSE2d_APPROX(F2->getStateVector("PO"), Vector3d(4,0,0), 1e-6); ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6); } @@ -464,7 +464,7 @@ TEST(Odom2d, KF_callback) // std::cout << report << std::endl; solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6); + ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO") , integrated_pose_vector[n_split], 1e-6); MatrixXd computed_cov; ASSERT_TRUE(problem->getLastFrameCovariance(computed_cov)); ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6); @@ -493,9 +493,9 @@ TEST(Odom2d, KF_callback) // solve // cout << "===== full ====" << endl; - keyframe_0->setState(Vector3d(1,2,3)); - keyframe_1->setState(Vector3d(2,3,1)); - keyframe_2->setState(Vector3d(3,1,2)); + keyframe_0->setState(Vector3d(1,2,3),"PO"); + keyframe_1->setState(Vector3d(2,3,1),"PO"); + keyframe_2->setState(Vector3d(3,1,2),"PO"); report = solver.solve(SolverManager::ReportVerbosity::BRIEF); solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); @@ -505,13 +505,13 @@ TEST(Odom2d, KF_callback) // check the split KF MatrixXd KF1_cov; ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov)); - ASSERT_POSE2d_APPROX(keyframe_1->getStateVector(), integrated_pose_vector[m_split], 1e-6); + ASSERT_POSE2d_APPROX(keyframe_1->getStateVector("PO"), integrated_pose_vector[m_split], 1e-6); ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6); // check other KF in the future of the split KF MatrixXd KF2_cov; ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov)); - ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6); + ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO"), integrated_pose_vector[n_split], 1e-6); ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6); // Check full trajectory diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index c4b845ed66d76fbfdf7c7302257bc21eb75db318..a5808a865a0fb02bb523df1a2e60e2c86208852a 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -264,9 +264,12 @@ TEST(Problem, emplaceFrame_factory) // check that all frames are effectively in the trajectory ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 3); - ASSERT_EQ(f0->getStateVector().size(), 3 ); - ASSERT_EQ(f1->getStateVector().size(), 7 ); - ASSERT_EQ(f2->getStateVector().size(), 10); + ASSERT_EQ(f0->getState().size(), 2 ); + ASSERT_EQ(f1->getState().size(), 2 ); + ASSERT_EQ(f2->getState().size(), 3 ); + ASSERT_EQ(f0->getStateVector("PO").size(), 3 ); + ASSERT_EQ(f1->getStateVector("PO").size(), 7 ); + ASSERT_EQ(f2->getStateVector("POV").size(), 10 ); // check that all frames are linked to Problem ASSERT_EQ(f0->getProblem(), P); diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp index b957ef47a3c55d228291f9081babbe792d840395..d71d2daaca4676ef2a4bc9a9e565ca94aa1fc104 100644 --- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp +++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp @@ -296,8 +296,8 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve) std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_); - ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6); - ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6); + ASSERT_MATRIX_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6); + ASSERT_MATRIX_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6); ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6); ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6); } @@ -314,8 +314,8 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_); - ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6); - ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6); + ASSERT_MATRIX_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6); + ASSERT_MATRIX_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6); ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6); ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6); @@ -333,8 +333,8 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve) std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_); - ASSERT_MATRIX_APPROX(KF1_->getStateVector(), pose1_, 1e-6); - ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6); + ASSERT_MATRIX_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6); + ASSERT_MATRIX_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6); ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6); ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6); } diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index 0f03f56523e12849d7757c01b0f4a1b7eeecef2e..fd252e8001af0a0c9864a234a5901a887ecb245a 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/test/gtest_tree_manager_sliding_window.cpp @@ -167,7 +167,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) auto F1 = P->getTrajectory()->getLastFrame(); EXPECT_TRUE(F1 != nullptr); - Vector7d state = F1->getStateVector(); + Vector7d state = F1->getStateVector("PO"); Vector7d zero_disp(state); Matrix6d cov = Matrix6d::Identity(); @@ -276,7 +276,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) auto F1 = P->getTrajectory()->getLastFrame(); EXPECT_TRUE(F1 != nullptr); - Vector7d state = F1->getStateVector(); + Vector7d state = F1->getStateVector("PO"); Vector7d zero_disp(state); Matrix6d cov = Matrix6d::Identity(); diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp index 6f827c5921c011802f5ce1434b09a6e10fa42b1e..3609485ec09f2ac328b1f2f693eca7adffe96b16 100644 --- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp +++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp @@ -184,7 +184,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral) ASSERT_FALSE(f1->getFactorList().empty()); auto c1 = f1->getFactorList().front(); - Vector7d state = F1->getStateVector(); + Vector7d state = F1->getStateVector("PO"); Vector7d zero_disp(state); Matrix6d cov = Matrix6d::Identity(); @@ -659,7 +659,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral) ASSERT_FALSE(f1->getFactorList().empty()); auto c1 = f1->getFactorList().front(); - Vector7d state = F1->getStateVector(); + Vector7d state = F1->getStateVector("PO"); Vector7d zero_disp(state); Matrix6d cov = Matrix6d::Identity();