diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp index ccf1c5522c993d985be079f274283fec530e8d3f..151b58915918f67fc00dc36096821cf201b36d71 100644 --- a/demos/hello_wolf/processor_range_bearing.cpp +++ b/demos/hello_wolf/processor_range_bearing.cpp @@ -75,7 +75,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) // 4. create or recover landmark Landmark2dPtr lmk; - auto lmk_it = known_lmks.find(id); + auto lmk_it = known_lmks.find(id); if (lmk_it != known_lmks.end()) { // known landmarks : recover landmark diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 1ea0197caeed382762958dc1920453396a316128..da101c346b8a3dbf4a94342c76a76672dc09c056 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -56,7 +56,6 @@ class CaptureBase : public NodeStateBlocks void setProblem(ProblemPtr _problem) override final; public: - /** \brief Constructor * * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived type name diff --git a/include/core/composite/prior_composite.h b/include/core/composite/prior_composite.h index 4d78f83ea25d039a1527707f0937f1cbe0a3cf3d..63742f486632e7c3dba03dff4d29a16de152ad59 100644 --- a/include/core/composite/prior_composite.h +++ b/include/core/composite/prior_composite.h @@ -32,7 +32,7 @@ class Prior { protected: std::string prior_mode_; // Prior mode can be 'initial_guess', 'fix' or 'factor' - Eigen::VectorXd factor_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix + Eigen::VectorXd factor_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix // (ONLY IF prior_mode_ == 'factor') public: diff --git a/include/core/composite/vector_composite.h b/include/core/composite/vector_composite.h index 8ae285755ea11bfc5fc6910f116510c68b05005d..0f357cbcc154d93aca2c81e6bbd8a65383a6ccaf 100644 --- a/include/core/composite/vector_composite.h +++ b/include/core/composite/vector_composite.h @@ -25,7 +25,6 @@ namespace wolf { - class VectorComposite : public Composite<Eigen::VectorXd> { public: diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 0afb361739c83d2ca78486ae5d8045fa2a190e0c..96fa788f55d0b458a133384483188508c0a00919 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -54,7 +54,6 @@ class FrameBase : public NodeStateBlocks TimeStamp time_stamp_; ///< frame time stamp public: - /** \brief Constructor time stamp and specs composite * * Constructor with time stamp and specs (types, vectors, priors) diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index 401bcbe7df28ff0a28f25b39e77f3fd843d46792..2476497e8a379340ece2f6280b69510eef0285b1 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -116,8 +116,8 @@ class LandmarkBase : public NodeStateBlocks const TypeComposite& _state_types, const VectorComposite& _state_vectors, const PriorComposite& _state_priors, - const int _external_id = -1, - const int _external_type = -1); + const int _external_id = -1, + const int _external_type = -1); /** \brief Constructor with type, state types composite and a YAML node (to be used by the derived classes YAML *node constructors) diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp index 50fb08a149fef9e79c3867807f651b3a898af284..ee5c2a5042e51fef09afbc9bc4fec2b522215ae3 100644 --- a/src/capture/capture_diff_drive.cpp +++ b/src/capture/capture_diff_drive.cpp @@ -23,7 +23,6 @@ namespace wolf { - CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXd& _data, diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index d6631a7a8ecf4e05e353c0d21ab370a6263857fc..210a193e195ddb8d692b3102300adf79c75c41fc 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -22,7 +22,6 @@ namespace wolf { - CaptureMotion::CaptureMotion(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr, diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 2809a6f34e52a5c0813dc46af2903562a3374db6..0d5ba19a555975354182d0f073c3d295e6b03a89 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -292,11 +292,11 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) { auto node = node_w.lock(); WOLF_WARN_COND(node == nullptr, - "FactorBase::link: ", - getType(), - " id: ", - id(), - " node_state_blocks weak pointer is not valid"); + "FactorBase::link: ", + getType(), + " id: ", + id(), + " node_state_blocks weak pointer is not valid"); if (node != nullptr) { WOLF_DEBUG_COND(node->getProblem() == nullptr, diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 5267d43e9d5ef728cc43d441ab82c361445c44ec..2d9fb444fa8c13f50495b048f480acd71f4565b3 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -54,10 +54,10 @@ SensorBase::SensorBase(const std::string& _type, for (auto state_pair : getStateBlockMap()) { state_block_dynamic_.emplace(state_pair.first, dynamics.at(state_pair.first)); - + if (dynamics.at(state_pair.first) and drifts.count(state_pair.first)) setDriftStd(state_pair.first, drifts.at(state_pair.first)); - + WOLF_WARN_COND(not dynamics.at(state_pair.first) and drifts.count(state_pair.first), "Sensor of type ", _type, diff --git a/test/dummy/landmark_dummy.cpp b/test/dummy/landmark_dummy.cpp index a84df8024dd4f144f75cbec2c2aaf19fafe99b62..c9c06e04c1a9da9f4906db397a3e72aad524e63b 100644 --- a/test/dummy/landmark_dummy.cpp +++ b/test/dummy/landmark_dummy.cpp @@ -26,7 +26,10 @@ LandmarkDummy::LandmarkDummy(const VectorComposite& _state_vectors, const PriorComposite& _state_priors, const int& _int_param, const double& _double_param) - : LandmarkBase("LandmarkDummy", {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'A', "StateParams1"}}, _state_vectors, _state_priors), + : LandmarkBase("LandmarkDummy", + {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'A', "StateParams1"}}, + _state_vectors, + _state_priors), int_param_(_int_param), double_param_(_double_param) { diff --git a/test/gtest_composite.cpp b/test/gtest_composite.cpp index 534b0a0ac67cc2e83d67ab6bbfbfdc93f38a5f92..ccfea70b58d580a46387b8d9171df2322279bea0 100644 --- a/test/gtest_composite.cpp +++ b/test/gtest_composite.cpp @@ -75,8 +75,8 @@ TEST(CompositeTest, ConstructorList) WOLF_INFO(dc); } -TEST(CompositeTest, emplace) { - +TEST(CompositeTest, emplace) +{ auto dc = DummyComposite(); dc.emplace('a', 1); dc.emplace('b', 2); @@ -136,15 +136,15 @@ TEST(CompositeTest, toYaml) TEST(CompositeTest, ConstructorYamlPlain) { YAML::Node dc_yaml; - dc_yaml["a"] = 1; - dc_yaml["b"] = 2; - dc_yaml["c"] = 3; - dc_yaml["d"] = 4; - dc_yaml["e"] = 5; - dc_yaml["f"] = 6; - dc_yaml["g"] = 7; - - auto dc = DummyComposite{dc_yaml, "", true}; // no field + dc_yaml["a"] = 1; + dc_yaml["b"] = 2; + dc_yaml["c"] = 3; + dc_yaml["d"] = 4; + dc_yaml["e"] = 5; + dc_yaml["f"] = 6; + dc_yaml["g"] = 7; + + auto dc = DummyComposite{dc_yaml, "", true}; // no field // getKeys ASSERT_EQ(dc.getKeys(), "abcdefg"); @@ -171,21 +171,21 @@ TEST(CompositeTest, ConstructorYamlPlain) TEST(CompositeTest, ConstructorYamlField) { YAML::Node dc_yaml; - dc_yaml["a"]["dummy_field"] = 1; - dc_yaml["a"]["other_field"] = "whatever_1"; - dc_yaml["b"]["dummy_field"] = 2; - dc_yaml["b"]["other_field"] = "whatever_2"; - dc_yaml["c"]["dummy_field"] = 3; - dc_yaml["c"]["other_field"] = "whatever_3"; - dc_yaml["d"]["dummy_field"] = 4; - dc_yaml["d"]["other_field"] = "whatever_4"; - dc_yaml["e"]["dummy_field"] = 5; - dc_yaml["e"]["other_field"] = "whatever_5"; - dc_yaml["f"]["dummy_field"] = 6; - dc_yaml["f"]["other_field"] = "whatever_6"; - dc_yaml["g"]["dummy_field"] = 7; - dc_yaml["g"]["other_field"] = "whatever_7"; - + dc_yaml["a"]["dummy_field"] = 1; + dc_yaml["a"]["other_field"] = "whatever_1"; + dc_yaml["b"]["dummy_field"] = 2; + dc_yaml["b"]["other_field"] = "whatever_2"; + dc_yaml["c"]["dummy_field"] = 3; + dc_yaml["c"]["other_field"] = "whatever_3"; + dc_yaml["d"]["dummy_field"] = 4; + dc_yaml["d"]["other_field"] = "whatever_4"; + dc_yaml["e"]["dummy_field"] = 5; + dc_yaml["e"]["other_field"] = "whatever_5"; + dc_yaml["f"]["dummy_field"] = 6; + dc_yaml["f"]["other_field"] = "whatever_6"; + dc_yaml["g"]["dummy_field"] = 7; + dc_yaml["g"]["other_field"] = "whatever_7"; + auto dc = DummyComposite(dc_yaml, "dummy_field", true); // getKeys @@ -213,21 +213,21 @@ TEST(CompositeTest, ConstructorYamlField) TEST(CompositeTest, ConstructorYamlFieldOptional) { YAML::Node dc_yaml; - dc_yaml["a"]["dummy_field"] = 1; - dc_yaml["a"]["other_field"] = "whatever_1"; - //dc_yaml["b"]["dummy_field"] = 2; - dc_yaml["b"]["other_field"] = "whatever_2"; - dc_yaml["c"]["dummy_field"] = 3; - dc_yaml["c"]["other_field"] = "whatever_3"; - dc_yaml["d"]["dummy_field"] = 4; - dc_yaml["d"]["other_field"] = "whatever_4"; - dc_yaml["e"]["dummy_field"] = 5; - dc_yaml["e"]["other_field"] = "whatever_5"; - dc_yaml["f"]["dummy_field"] = 6; - dc_yaml["f"]["other_field"] = "whatever_6"; - dc_yaml["g"]["dummy_field"] = 7; - dc_yaml["g"]["other_field"] = "whatever_7"; - + dc_yaml["a"]["dummy_field"] = 1; + dc_yaml["a"]["other_field"] = "whatever_1"; + // dc_yaml["b"]["dummy_field"] = 2; + dc_yaml["b"]["other_field"] = "whatever_2"; + dc_yaml["c"]["dummy_field"] = 3; + dc_yaml["c"]["other_field"] = "whatever_3"; + dc_yaml["d"]["dummy_field"] = 4; + dc_yaml["d"]["other_field"] = "whatever_4"; + dc_yaml["e"]["dummy_field"] = 5; + dc_yaml["e"]["other_field"] = "whatever_5"; + dc_yaml["f"]["dummy_field"] = 6; + dc_yaml["f"]["other_field"] = "whatever_6"; + dc_yaml["g"]["dummy_field"] = 7; + dc_yaml["g"]["other_field"] = "whatever_7"; + auto dc = DummyComposite(dc_yaml, "dummy_field", false); // getKeys @@ -254,63 +254,63 @@ TEST(CompositeTest, ConstructorYamlFieldOptional) TEST(CompositeTest, ConstructorYaml_wrong) { YAML::Node dc_yaml; - dc_yaml["a"] = 1; - dc_yaml["a"] = "whatever_1"; - //dc_yaml["b"] = 2; - dc_yaml["b"] = "whatever_2"; - dc_yaml["c"] = 3; - dc_yaml["c"] = "whatever_3"; - dc_yaml["d"] = 4; - dc_yaml["d"] = "whatever_4"; - dc_yaml["e"] = 5; - dc_yaml["e"] = "whatever_5"; - dc_yaml["f"] = 6; - dc_yaml["f"] = "whatever_6"; - dc_yaml["g"] = 7; - dc_yaml["g"] = "whatever_7"; - + dc_yaml["a"] = 1; + dc_yaml["a"] = "whatever_1"; + // dc_yaml["b"] = 2; + dc_yaml["b"] = "whatever_2"; + dc_yaml["c"] = 3; + dc_yaml["c"] = "whatever_3"; + dc_yaml["d"] = 4; + dc_yaml["d"] = "whatever_4"; + dc_yaml["e"] = 5; + dc_yaml["e"] = "whatever_5"; + dc_yaml["f"] = 6; + dc_yaml["f"] = "whatever_6"; + dc_yaml["g"] = 7; + dc_yaml["g"] = "whatever_7"; + ASSERT_THROW(DummyComposite(dc_yaml, "", true), std::runtime_error); } TEST(CompositeTest, ConstructorYamlField_wrong) { YAML::Node dc_yaml; - dc_yaml["a"]["dummy_field"] = 1; - dc_yaml["a"]["other_field"] = "whatever_1"; - //dc_yaml["b"]["dummy_field"] = 2; - dc_yaml["b"]["other_field"] = "whatever_2"; - dc_yaml["c"]["dummy_field"] = 3; - dc_yaml["c"]["other_field"] = "whatever_3"; - dc_yaml["d"]["dummy_field"] = 4; - dc_yaml["d"]["other_field"] = "whatever_4"; - dc_yaml["e"]["dummy_field"] = 5; - dc_yaml["e"]["other_field"] = "whatever_5"; - dc_yaml["f"]["dummy_field"] = 6; - dc_yaml["f"]["other_field"] = "whatever_6"; - dc_yaml["g"]["dummy_field"] = 7; - dc_yaml["g"]["other_field"] = "whatever_7"; - + dc_yaml["a"]["dummy_field"] = 1; + dc_yaml["a"]["other_field"] = "whatever_1"; + // dc_yaml["b"]["dummy_field"] = 2; + dc_yaml["b"]["other_field"] = "whatever_2"; + dc_yaml["c"]["dummy_field"] = 3; + dc_yaml["c"]["other_field"] = "whatever_3"; + dc_yaml["d"]["dummy_field"] = 4; + dc_yaml["d"]["other_field"] = "whatever_4"; + dc_yaml["e"]["dummy_field"] = 5; + dc_yaml["e"]["other_field"] = "whatever_5"; + dc_yaml["f"]["dummy_field"] = 6; + dc_yaml["f"]["other_field"] = "whatever_6"; + dc_yaml["g"]["dummy_field"] = 7; + dc_yaml["g"]["other_field"] = "whatever_7"; + ASSERT_THROW(DummyComposite(dc_yaml, "dummy_field", true), std::runtime_error); } TEST(CompositeTest, ConstructorYamlFieldKeys) { YAML::Node dc_yaml; - dc_yaml["a"]["dummy_field"] = 1; - dc_yaml["a"]["other_field"] = "whatever_1"; - dc_yaml["b"]["dummy_field"] = 2; - dc_yaml["b"]["other_field"] = "whatever_2"; - dc_yaml["c"]["dummy_field"] = 3; - dc_yaml["c"]["other_field"] = "whatever_3"; - dc_yaml["d"]["dummy_field"] = 4; - dc_yaml["d"]["other_field"] = "whatever_4"; - dc_yaml["e"]["dummy_field"] = 5; - dc_yaml["e"]["other_field"] = "whatever_5"; - dc_yaml["f"]["dummy_field"] = 6; - dc_yaml["f"]["other_field"] = "whatever_6"; - dc_yaml["g"]["dummy_field"] = 7; - dc_yaml["g"]["other_field"] = "whatever_7"; - + dc_yaml["a"]["dummy_field"] = 1; + dc_yaml["a"]["other_field"] = "whatever_1"; + dc_yaml["b"]["dummy_field"] = 2; + dc_yaml["b"]["other_field"] = "whatever_2"; + dc_yaml["c"]["dummy_field"] = 3; + dc_yaml["c"]["other_field"] = "whatever_3"; + dc_yaml["d"]["dummy_field"] = 4; + dc_yaml["d"]["other_field"] = "whatever_4"; + dc_yaml["e"]["dummy_field"] = 5; + dc_yaml["e"]["other_field"] = "whatever_5"; + dc_yaml["f"]["dummy_field"] = 6; + dc_yaml["f"]["other_field"] = "whatever_6"; + dc_yaml["g"]["dummy_field"] = 7; + dc_yaml["g"]["other_field"] = "whatever_7"; + auto dc = DummyComposite{dc_yaml, "dummy_field", true, "aceg"}; // getKeys diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 23093bb6c45dac07ff270323c7b003e0a0d63a68..2471162fe159902408ba8a919bc7c51e81006fa0 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -52,8 +52,8 @@ TEST(Emplace, Landmark) ASSERT_EQ(P->getMap(), L->getMap()); ASSERT_EQ(P->getMap()->getLandmarkList().front(), L); - auto L2 = LandmarkBase::emplace<Landmark3d>( - P->getMap(), VectorComposite{{'P', Vector3d::Zero()}}, PriorComposite()); + auto L2 = + LandmarkBase::emplace<Landmark3d>(P->getMap(), VectorComposite{{'P', Vector3d::Zero()}}, PriorComposite()); ASSERT_EQ(P, L2->getProblem()); ASSERT_EQ(P->getMap(), L2->getMap()); @@ -64,7 +64,7 @@ TEST(Emplace, Sensor) { ProblemPtr P = Problem::create("POV", 3); - auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml"); + auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml"); auto S = SensorBase::emplace<SensorDummyPo3d>(P->getHardware(), params, {wolf_dir}); @@ -283,17 +283,19 @@ TEST(Emplace, ReturnDerived) ASSERT_NE(P->getTrajectory(), nullptr); - auto F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), - TimeStamp(0), - TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, - VectorComposite{{'P', Vector3d::Random()}, {'O', Vector4d::Random().normalized()}}, - PriorComposite{}); - - auto F = FrameBase::emplace<FrameBase>(P->getTrajectory(), - TimeStamp(1), - TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, - VectorComposite{{'P', Vector3d::Random()}, {'O', Vector4d::Random().normalized()}}, - PriorComposite{}); + auto F0 = FrameBase::emplace<FrameBase>( + P->getTrajectory(), + TimeStamp(0), + TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, + VectorComposite{{'P', Vector3d::Random()}, {'O', Vector4d::Random().normalized()}}, + PriorComposite{}); + + auto F = FrameBase::emplace<FrameBase>( + P->getTrajectory(), + TimeStamp(1), + TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, + VectorComposite{{'P', Vector3d::Random()}, {'O', Vector4d::Random().normalized()}}, + PriorComposite{}); auto C = CaptureBase::emplace<CaptureBase>(F, "Dummy", TimeStamp(1), nullptr); diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp index 3caf5690780c9e14a89e41b2d631c2d003a23bf7..abebef94873d7eff723cd6a8e869a26bd6f90da5 100644 --- a/test/gtest_factor_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp @@ -44,7 +44,7 @@ auto problem_ptr = Problem::create("PO", 3); auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); // Sensor -auto sensor_pose3d = problem_ptr->installSensor(wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); +auto sensor_pose3d = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm = problem_ptr->emplaceFrame(0, problem_ptr->stateZero()); diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index eefcff4041d2a345a5ae96a6cf1ee999953e0fbb..3da1a2e5c856fbffee568fe8b0b3864d0f551495 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -50,7 +50,7 @@ auto problem_ptr = Problem::create("PO", 2); auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); // Sensor -auto sensor = problem_ptr->installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero()); diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp index 3a2c58ddc66d909f12aa4184f3bcea67935c29ed..8920e50ad77fe2dbd8a29110bb19862c9c5c4af1 100644 --- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp @@ -47,7 +47,7 @@ auto problem_ptr = Problem::create("PO", 3); auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); // Sensor -auto sensor = problem_ptr->installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero()); diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp index 3380cd62c1c53051125e9918d7bcaa75eeaef060..8c10f8444057637b68dec599f9feb2781f53a88f 100644 --- a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp @@ -50,7 +50,7 @@ auto problem_ptr = Problem::create("PO", 2); auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); // Sensor -auto sensor = problem_ptr->installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // Frame FrameBasePtr frm = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero()); diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp index 1a45f83d52441ad58d019fe4c74c8e19b8a15bd9..68b39536c395756c700efcfdc13e947684662e11 100644 --- a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp @@ -50,7 +50,7 @@ auto problem_ptr = Problem::create("PO", 3); auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); // Sensor -auto sensor = problem_ptr->installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); // Frame FrameBasePtr frm = problem_ptr->emplaceFrame(0.0, problem_ptr->stateZero()); diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 0a212414a5d3a13ae6586038facc250482cc80c8..dc6f265da2480fa8f30a5d899ffbe2e5ad688e06 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -434,10 +434,11 @@ TEST(FrameBase, Frames) TEST(FrameBase, GetSetState) { // Create PQV_3d frame - FrameBase F(1, - TypeComposite{{'P', "StatePoint3d"}, {'V', "StateVector3d"}, {'O', "StateQuaternion"}}, - VectorComposite{{'P', Vector3d::Zero()}, {'V', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}}, - PriorComposite()); + FrameBase F( + 1, + TypeComposite{{'P', "StatePoint3d"}, {'V', "StateVector3d"}, {'O', "StateQuaternion"}}, + VectorComposite{{'P', Vector3d::Zero()}, {'V', Vector3d::Zero()}, {'O', Vector4d::Random().normalized()}}, + PriorComposite()); // Give values to vectors and vector blocks VectorXd x(10), x1(10), x2(10); diff --git a/test/gtest_map.cpp b/test/gtest_map.cpp index b5c423f2c8305ad33ce58c23e57d3c172185c471..30a09c2b12a268f1d5b676a2d98d01370d096bf7 100644 --- a/test/gtest_map.cpp +++ b/test/gtest_map.cpp @@ -130,8 +130,8 @@ TEST(MapYaml, save_3d) int_param, double_param); auto lmk4 = LandmarkBase::emplace<LandmarkDummy>(problem->getMap(), - VectorComposite{{'P', pdummy},// - {'O', odummy}, + VectorComposite{{'P', pdummy}, // + {'O', odummy}, {'A', Vector1d(double_param)}}, PriorComposite{{'P', Prior("initial_guess")}, // {'O', Prior("fix")}, @@ -259,7 +259,7 @@ TEST(MapYaml, load) ASSERT_EQ(lmk->getExternalId(), -1); ASSERT_EQ(lmk->getExternalType(), -1); } - else + else ASSERT_FALSE(true); } diff --git a/test/gtest_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp index f8e1917f14473ba3b7022bc064dafeba41a16120..35c627f242131c7eb62f29df96d55c023d77c2c4 100644 --- a/test/gtest_node_state_blocks.cpp +++ b/test/gtest_node_state_blocks.cpp @@ -103,9 +103,9 @@ TEST(NodeStateBlocksTest, constructor_type_vector_prior) TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'I', "StateParams5"}, {'A', "StateAngle"}}; auto vectors = VectorComposite{{'P', p_state}, {'O', o_state}, {'I', i_state}, {'A', a_state}}; auto priors = PriorComposite{{'P', Prior("fix")}, - {'O', Prior("factor", o_std)}, - {'I', Prior("initial_guess")}, - {'A', Prior("factor", a_std)}}; + {'O', Prior("factor", o_std)}, + {'I', Prior("initial_guess")}, + {'A', Prior("factor", a_std)}}; auto N = std::make_shared<NodeStateBlocksDummy>(types, vectors, priors); ASSERT_TRUE(N->has("POIA")); @@ -146,33 +146,28 @@ TEST(NodeStateBlocksTest, constructor_type_vector_prior) class NodeStateBlocksDerivedTest : public testing::Test { public: - ProblemPtr problem; - SensorBasePtr S0, S1; - FrameBasePtr F0, F1; - StateBlockPtr sbp0, sbo0, sbv0, sbp1, sbv1; - FactorBasePtr f0p; + ProblemPtr problem; + SensorBasePtr S0, S1; + FrameBasePtr F0, F1; + StateBlockPtr sbp0, sbo0, sbv0, sbp1, sbv1; + FactorBasePtr f0p; void SetUp() override { problem = Problem::create("POV", 3); // Two frames not linked to problem - F0 = FrameBase::emplace<FrameBase>(nullptr, - 0.0, - TypeComposite{{'P', "StatePoint3d"}, - {'O', "StateQuaternion"}}, - VectorComposite{{'P', Vector3d::Zero()}, - {'O', Vector4d(Quaterniond::Identity().coeffs())}}, - PriorComposite{{'P', Prior("factor", Vector3d::Ones())}}); - F1 = FrameBase::emplace<FrameBase>(nullptr, - 1.0, - TypeComposite(), - VectorComposite(), - PriorComposite()); + F0 = FrameBase::emplace<FrameBase>( + nullptr, + 0.0, + TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, + VectorComposite{{'P', Vector3d::Zero()}, {'O', Vector4d(Quaterniond::Identity().coeffs())}}, + PriorComposite{{'P', Prior("factor", Vector3d::Ones())}}); + F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, TypeComposite(), VectorComposite(), PriorComposite()); f0p = F0->getPriorFactor('P'); sbp0 = F0->getStateBlock('P'); sbo0 = F0->getStateBlock('O'); - + ASSERT_TRUE(f0p); ASSERT_TRUE(sbp0); ASSERT_TRUE(sbo0); @@ -273,7 +268,7 @@ TEST_F(NodeStateBlocksDerivedTest, notify_prior_factors) // create solver SolverManagerPtr solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); - + // update will consume notifications solver->update(); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 72abad4bcda1feb8eb4cc48eccd2a25a66837978..84f7db992717ce6d9c452e8f1de74a70e6d543d0 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -96,7 +96,7 @@ TEST(Problem, emplaceFirstFrame_PO_2d) TimeStamp t0(0); VectorComposite first_frame_values{{'P', Vector2d(1, 2)}, {'O', Vector1d(3)}}; PriorComposite first_frame_priors{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))}, - {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}}; + {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}}; P->emplaceFirstFrame(t0, {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, first_frame_values, first_frame_priors); WOLF_INFO("printing..."); @@ -150,7 +150,7 @@ TEST(Problem, emplaceFirstFrame_PO_3d) TimeStamp t0(0); VectorComposite first_frame_values{{'P', Vector3d(1, 2, 3)}, {'O', Vector4d(4, 5, 6, 7).normalized()}}; PriorComposite first_frame_priors{{'P', Prior("factor", Vector3d::Constant(sqrt(0.1)))}, - {'O', Prior("factor", Vector3d::Constant(sqrt(0.1)))}}; + {'O', Prior("factor", Vector3d::Constant(sqrt(0.1)))}}; P->emplaceFirstFrame( t0, {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, first_frame_values, first_frame_priors); diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 7e56bae0290a0f25d61799cea802091a2544a2b5..647bf7e15d226723c6fb5625ad0b8188f1831a93 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -83,12 +83,12 @@ TEST(ProcessorBase, KeyFrameCallback) // Sequence to test Key Frame creations (callback calls) // initialize - TimeStamp t(0.0); + TimeStamp t(0.0); problem->emplaceFirstFrame(t, - TypeComposite{{'P', "StatePoint2d"},{'O', "StateAngle"}}, - VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}}, - PriorComposite{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))}, {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}} - ); + TypeComposite{{'P', "StatePoint2d"}, {'O', "StateAngle"}}, + VectorComposite{{'P', Vector2d::Zero()}, {'O', Vector1d::Zero()}}, + PriorComposite{{'P', Prior("factor", Vector2d::Constant(sqrt(0.1)))}, + {'O', Prior("factor", Vector1d::Constant(sqrt(0.1)))}}); CaptureOdom2dPtr capt_odo = std::make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5, 0)); @@ -116,9 +116,7 @@ TEST(ProcessorBase, KeyFrameCallback) std::cout << "5\n"; // keyframe creation - if (ii == 5) - problem->emplaceFrame(t, - VectorComposite{{'P', Vector2d(0, 0)},{'O', Vector1d(0)}}); + if (ii == 5) problem->emplaceFrame(t, VectorComposite{{'P', Vector2d(0, 0)}, {'O', Vector1d(0)}}); problem->print(4, 1, 1, 0); std::cout << "6\n"; diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp index afe5e08d9438b8ac9333eab2633203ae1c4c1053..0c977002356ef6648bcfef7794533d2ea6b5f1c5 100644 --- a/test/gtest_processor_landmark_external.cpp +++ b/test/gtest_processor_landmark_external.cpp @@ -323,7 +323,7 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim, // process detections cap->process(); ASSERT_TRUE(problem->check()); - //problem->print(4, 1, 1, 1); + // problem->print(4, 1, 1, 1); // CHECKS fac_list.clear(); diff --git a/test/gtest_processor_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp index 17be901e4e834d27e005051e1826025877cd1c15..5c62eff0c749bd55f76c9f1282956ac1ca282e64 100644 --- a/test/gtest_processor_odom_2d.cpp +++ b/test/gtest_processor_odom_2d.cpp @@ -118,7 +118,7 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve) ASSERT_TRUE(KF->getProblem()); - problem->print(4,1,1,1); + problem->print(4, 1, 1, 1); problem->check(4); auto report = solver->solve(SolverManager::ReportVerbosity::FULL); @@ -315,8 +315,8 @@ TEST(ProcessorOdom2d, KF_callback) std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl; - VectorComposite x_split = processor_odom2d->getState(t_split, "PO"); - FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split); + VectorComposite x_split = processor_odom2d->getState(t_split, "PO"); + FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split); // there must be 2KF and one F ASSERT_EQ(problem->getTrajectory()->size(), 2); diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp index 98136b31d3c7295a593c5f86bd48248c4f5a8a48..ce980fcf7c9023b0f618ae31b83274beb5adab12 100644 --- a/test/gtest_processor_tracker_feature_dummy.cpp +++ b/test/gtest_processor_tracker_feature_dummy.cpp @@ -200,7 +200,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, establishFactors) processor->setLast(last_cap); // Put a capture on incoming_ptr_ - FrameBasePtr inc_frm = FrameBase::emplace<FrameBase>(nullptr, 1, problem->getFrameTypes(), problem->stateZero("P")); + FrameBasePtr inc_frm = + FrameBase::emplace<FrameBase>(nullptr, 1, problem->getFrameTypes(), problem->stateZero("P")); CaptureBasePtr inc_cap = CaptureBase::emplace<CaptureVoid>(inc_frm, 1, sensor); processor->setInc(inc_cap); diff --git a/test/gtest_vector_composite.cpp b/test/gtest_vector_composite.cpp index fd980fd8ef40f58f75712409329da318ee648be4..8866142c6d8b962b3f4e79a62a5ddb6ea57e374b 100644 --- a/test/gtest_vector_composite.cpp +++ b/test/gtest_vector_composite.cpp @@ -34,7 +34,7 @@ TEST(VectorComposite, constructor_empty) TEST(VectorComposite, constructor_list) { - VectorComposite u{{'a', Vector2d(1, 2)},{'b', Vector3d(3, 4, 5)}}; // std::map API + VectorComposite u{{'a', Vector2d(1, 2)}, {'b', Vector3d(3, 4, 5)}}; // std::map API ASSERT_EQ(u.getKeys(), "ab"); ASSERT_MATRIX_APPROX(u['a'], Vector2d(1, 2), 1e-20);