diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index 2b68ab2ac01bdc47a239dc2541713840f8b3358d..f5dfde2389fe74c81a949592688a052af275f36a 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -110,16 +110,16 @@ int main() // Wolf problem and solver ProblemPtr problem = Problem::create("PO", 2); YAML::Node params_solver; - params_solver["period"] = 1; - params_solver["verbose"] = 2; + params_solver["period"] = 1; + params_solver["verbose"] = 2; params_solver["interrupt_on_problem_change"] = false; - params_solver["max_num_iterations"] = 1000; - params_solver["n_threads"] = 2; - params_solver["function_tolerance"] = 0.000001; - params_solver["gradient_tolerance"] = 0.0000000001; - params_solver["minimizer"] = "LEVENBERG_MARQUARDT"; - params_solver["use_nonmonotonic_steps"] = false; - params_solver["compute_cov"] = false; + params_solver["max_num_iterations"] = 1000; + params_solver["n_threads"] = 2; + params_solver["function_tolerance"] = 0.000001; + params_solver["gradient_tolerance"] = 0.0000000001; + params_solver["minimizer"] = "LEVENBERG_MARQUARDT"; + params_solver["use_nonmonotonic_steps"] = false; + params_solver["compute_cov"] = false; SolverCeresPtr ceres = std::static_pointer_cast<SolverCeres>(SolverCeres::create(problem, params_solver)); // sensor odometer 2d diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index 1399367da99229aa5c16f639c8b02292080ff21f..d1a65b2d5217798e66d4831f5c766523274b9398 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -125,7 +125,8 @@ int main() problem->print(4, 0, 1, 0); // Solver. Configure a Ceres solver - SolverManagerPtr ceres = FactorySolverNode::create("SolverCeres", problem, server.getNode()["solver"],{wolf_path}); + SolverManagerPtr ceres = + FactorySolverNode::create("SolverCeres", problem, server.getNode()["solver"], {wolf_path}); // recover sensor pointers and other stuff for later use (access by sensor name) SensorBasePtr sensor_odo = problem->findSensor("sen odom"); diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index a1d0a53083e2e488143120c46c690a4c4e840d21..44c6bbccc6d3e807b9fb94c9de09922221127e26 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -95,7 +95,7 @@ namespace wolf { \ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \ server.setYaml(_input_node); \ - if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \ + if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \ { \ throw std::runtime_error(server.getLog()); \ } \ @@ -106,7 +106,7 @@ namespace wolf const std::vector<std::string>& _folders_schema) \ { \ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ - if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \ + if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \ { \ throw std::runtime_error(server.getLog()); \ } \ diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 94ed373177c0340daa7e45d0dfdc3aa0d9dc6cae..2ec64bcb723c6aefe60f8586603134bdf7933059 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -89,7 +89,7 @@ namespace wolf { \ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \ server.setYaml(_input_node); \ - if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \ + if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \ { \ throw std::runtime_error(server.getLog()); \ } \ @@ -101,7 +101,7 @@ namespace wolf static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \ { \ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ - if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \ + if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \ { \ throw std::runtime_error(server.getLog()); \ } \ diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h index 9b95310e4382bf5d8e8197a8ba4b21bfc4639f99..fc77e1374dc82a9aaf1e97f8e00700306549488d 100644 --- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h +++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h @@ -38,7 +38,7 @@ class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow void keyFrameCallback(FrameBasePtr _frame) override; protected: - unsigned int count_frames_; ///< counter of frames + unsigned int count_frames_; ///< counter of frames // PARAMS unsigned int n_frames_recent_; diff --git a/include/core/utils/string_utils.h b/include/core/utils/string_utils.h index 66d397985b842f9ba2e06da37412d8a6a0d7a85d..a5ad22f67d5e8d138ee952901a23649dc194ae99 100644 --- a/include/core/utils/string_utils.h +++ b/include/core/utils/string_utils.h @@ -28,7 +28,6 @@ namespace wolf { - // date now in string std::string dateTimeNow(); @@ -44,7 +43,7 @@ std::string toString(float _arg); std::string toString(double _arg); std::string toString(long double _arg); -template<typename Derived> +template <typename Derived> std::string toString(const Eigen::DenseBase<Derived>& mat) { std::stringstream ss; diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index c75b397249b115f51485abbedfb6c95b32807b4d..dc00901999032bc39d1bf4b9cc167ac69d46e07e 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -29,7 +29,6 @@ namespace wolf { - SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _params) : SolverManager(_wolf_problem, _params), n_iter_acc_(0), diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp index 4eaa9fb93c9faf5b9f7235e120bd036d6e0cb844..8d3bf82a6c887e5671a47a8ab92c76201c3449be 100644 --- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp +++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp @@ -29,13 +29,13 @@ TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(const YAML::N { NodeBase::setType("TreeManagerSlidingWindowDualRate"); - n_frames_recent_ = _params["n_frames_recent"].as<unsigned int>(); - rate_old_frames_ = _params["rate_old_frames"].as<unsigned int>(); - if (n_frames_recent_ >= n_frames_) - { - throw std::runtime_error( - "ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'"); - } + n_frames_recent_ = _params["n_frames_recent"].as<unsigned int>(); + rate_old_frames_ = _params["rate_old_frames"].as<unsigned int>(); + if (n_frames_recent_ >= n_frames_) + { + throw std::runtime_error( + "ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'"); + } } void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame) diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h index ba43cf0dc479981029f467ee88d15cee41f292e7..49b3d8a2561712316bf091ff8ccd25e216fa5177 100644 --- a/test/dummy/tree_manager_dummy.h +++ b/test/dummy/tree_manager_dummy.h @@ -24,7 +24,6 @@ namespace wolf { - WOLF_PTR_TYPEDEFS(TreeManagerDummy) class TreeManagerDummy : public TreeManagerBase @@ -43,7 +42,7 @@ class TreeManagerDummy : public TreeManagerBase n_KF_++; }; - int n_KF_; + int n_KF_; double toy_param_; }; diff --git a/test/gtest_factor_distance_3d.cpp b/test/gtest_factor_distance_3d.cpp index a8cf2891935e7edb4aa90d0491808753a04f80ff..3fb3794742c80d96377e48255b1e9be6b80f4164 100644 --- a/test/gtest_factor_distance_3d.cpp +++ b/test/gtest_factor_distance_3d.cpp @@ -98,7 +98,7 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual) double res_expected = (measurement - (pos2 - pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0, 0); double res; - c2->operator()(pos1.data(), pos2.data(), &res); + c2-> operator()(pos1.data(), pos2.data(), &res); ASSERT_NEAR(res, res_expected, 1e-5); } diff --git a/test/gtest_factor_pose_2d_with_extrinsics.cpp b/test/gtest_factor_pose_2d_with_extrinsics.cpp index 1a74f1acf81ac080070c2f312d8e70a649c394a1..ce1b772428d84413be23dc0e508c0662e1e711cc 100644 --- a/test/gtest_factor_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_pose_2d_with_extrinsics.cpp @@ -45,7 +45,7 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve // Sensor auto sensor_pose2d = - problem_ptr->installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir}); + problem_ptr -> installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero()); diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp index 5312d5a49fd6a13c72d1d406fc6883cbe535c7bb..7993a7b08251a15b34d66efba62f512316e3a48e 100644 --- a/test/gtest_factor_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp @@ -45,7 +45,7 @@ auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solve // Sensor auto sensor_pose3d = - problem_ptr->installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); + problem_ptr -> installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index be8620ebac35a3dfd4394483eb9bbc3ee4bc691a..3c55d0b50c4dd276fd25c1a34fa971c50beae47e 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -49,7 +49,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("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero()); diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp index f6c4c1a67200ac18cd486171308c03209b6b1dba..db1e95c723307a7588424a2bc777fdb5538dc4c0 100644 --- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp @@ -46,7 +46,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("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir}); // Two frames FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp index 668ee7267fa77c36130d02384f76dc8c3fe48951..08f34a8e80a0d8a9dd9d909f2613d7ae7bcc1866 100644 --- a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp @@ -49,7 +49,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("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); +auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir}); // Frame FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero()); diff --git a/test/gtest_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp index 0f58cadd22c259e16341ccc07ed6af6cfcfe614f..24329ff6e48323b8a09fcc92206a482e2c709bc2 100644 --- a/test/gtest_node_state_blocks.cpp +++ b/test/gtest_node_state_blocks.cpp @@ -199,15 +199,15 @@ class NodeStateBlocksDerivedTest : public testing::Test { problem = Problem::create("POV", 3); - sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3 + sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3 sbo0 = std::make_shared<StateQuaternion>(); sbv0 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3 sbp1 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3 sbo1 = std::make_shared<StateQuaternion>(); sbv1 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3 - F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF - F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF + F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF + F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF } void TearDown() override {} }; diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index dd0fddbc02c857f368de6086f03742d0c38686d1..c5bab34b3f523a624726e93de72757a4f8797dd7 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -324,7 +324,7 @@ TEST(Problem, StateBlocks) // consume notifications SolverManagerPtr SM = SolverDummy::create(P, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir}); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); - SM->update(); // calls P->consumeNotifications(); + SM->update(); // calls P->consumeNotifications(); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(0)); // consumeNotifications empties the notification map diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp index e4808eb359cf8bafd0b6164cae56867080f50514..7292ac09ee5d05a929c911852e44572bca8db13c 100644 --- a/test/gtest_processor_landmark_external.cpp +++ b/test/gtest_processor_landmark_external.cpp @@ -101,7 +101,7 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim, // Sensors sensor = problem->installSensor("SensorOdom" + toString(dim) + "d", wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml", - {wolf_dir}); + {wolf_dir}); sensor_odom = problem->installSensor("SensorOdom" + toString(dim) + "d", wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml", {wolf_dir}); diff --git a/test/gtest_processor_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp index 85296e9d21df2c2558393e91a481dd812e2c9ae1..ebd0973f461e47405a53fe6b33fa23c5e98b00cb 100644 --- a/test/gtest_processor_odom_2d.cpp +++ b/test/gtest_processor_odom_2d.cpp @@ -92,7 +92,7 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve) // motion data VectorXd data(Vector2d(1, M_PI_4)); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01; - int N = 7; // number of process() steps + int N = 7; // number of process() steps // Create Wolf tree nodes ProblemPtr problem = Problem::create("PO", 2); @@ -221,7 +221,7 @@ TEST(ProcessorOdom2d, KF_callback) double dt = .01; VectorXd data(Vector2d(1, M_PI_4)); // advance 1m Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01; - int N = 8; // number of process() steps + int N = 8; // number of process() steps // NOTE: We integrate and create KFs as follows: // n = 0 1 2 3 4 5 6 7 8 @@ -239,7 +239,7 @@ TEST(ProcessorOdom2d, KF_callback) "ProcessorOdom2d", sensor_odom2d, wolf_dir + "/test/yaml/processor_odom_2d_inactive.yaml", {wolf_dir})); // Ceres wrapper - auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); + auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir}); // Origin Key Frame SpecStateComposite prior; diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index 6853b1c4658efd28beba1885b5d6e1d6b6a6f41a..4d315e0d850fe7f930c357fdec9ed399f2f169b2 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -229,8 +229,8 @@ TEST(SensorBase, sensor_dummy) // factory file ------------------------------------------------------------------------ try { - auto S4 = FactorySensorFile::create( - "SensorDummy" + toString(dim) + "d", yaml_file, {wolf_dir}); + auto S4 = + FactorySensorFile::create("SensorDummy" + toString(dim) + "d", yaml_file, {wolf_dir}); // noise ASSERT_MATRIX_APPROX(S4->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index 816fa1cf692f60c1f0478cd8cbcfa8fd3ac81cef..d8c4055c749754892e7b5daddb720a82d2b095f9 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -52,20 +52,20 @@ TEST(TrajectoryBase, ClosestKeyFrame) FrameBasePtr F4 = std::make_shared<FrameBase>( 4, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()})); - FrameBasePtr KF; // closest key-frame queried + FrameBasePtr KF; // closest key-frame queried KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1 ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1 - ASSERT_EQ(KF->id(), F1->id()); // same id! + KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1 + ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! + KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! KF = T->closestFrameToTimeStamp( 2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2 - ASSERT_EQ(KF->id(), F2->id()); // same id! + ASSERT_EQ(KF->id(), F2->id()); // same id! KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index 533634a86cb26c5bb280089f7714747bef0c2afa..0abcfdf232ffc716cc88f892fc868d7b2b4c8c9f 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/test/gtest_tree_manager_sliding_window.cpp @@ -74,7 +74,7 @@ TEST(TreeManager, FactoryNode) ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindow")); - auto TM = FactoryTreeManagerNode::create("TreeManagerSlidingWindow", server.getNode(),{}); + auto TM = FactoryTreeManagerNode::create("TreeManagerSlidingWindow", server.getNode(), {}); EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);