diff --git a/.clang-format b/.clang-format index 88156faef..383002fbf 100644 --- a/.clang-format +++ b/.clang-format @@ -15,6 +15,7 @@ NamespaceIndentation: None ContinuationIndentWidth: 4 IndentCaseLabels: true IndentFunctionDeclarationAfterType: false +QualifierAlignment: Right AlignEscapedNewlinesLeft: false AlignTrailingComments: true diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3cf1717ab..2bf56247f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -19,7 +19,7 @@ permissions: jobs: build-ws: name: Build colcon workspace - runs-on: ubuntu-24.04 + runs-on: ubuntu-22.04 steps: - name: Checkout source uses: actions/checkout@v4 @@ -54,7 +54,7 @@ jobs: needs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws - runs-on: ubuntu-24.04 + runs-on: ubuntu-22.04 container: # Run on the Docker image we tagged and pushed to a private repo in the job above image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} @@ -79,7 +79,7 @@ jobs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws name: clang-tidy - runs-on: ubuntu-24.04 + runs-on: ubuntu-22.04 container: # Run on the Docker image we tagged and pushed to a private repo in the job above image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} @@ -93,7 +93,8 @@ jobs: **.cpp **.hpp - if: ${{ steps.changed-cpp-files.outputs.all_changed_files != '' }} - run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config "$(cat src/fuse/.clang-tidy)" ${{ steps.changed-cpp-files.outputs.all_changed_files }} + # https://stackoverflow.com/questions/48404289/using-clang-tidy-to-check-c17-code + run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config "$(cat src/fuse/.clang-tidy)" -extra-arg=-std=c++17 ${{ steps.changed-cpp-files.outputs.all_changed_files }} working-directory: /colcon_ws - uses: asarium/clang-tidy-action@v1.0 with: diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index e55c1faa7..09a1197f8 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -12,14 +12,12 @@ on: jobs: pre-commit: name: Format - runs-on: ubuntu-24.04 + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 - uses: actions/setup-python@v5 with: python-version: '3.10' - - name: Install clang-format-18 - run: sudo apt-get install clang-format-18 - uses: pre-commit/action@v3.0.1 id: precommit - name: Upload pre-commit changes diff --git a/README.md b/README.md index de613922d..ba29500da 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ Welcome to PickNik Robotics's fork of fuse! +This branch is for ROS Humble. + ## Getting Started Using the Dockerfile is the easiest way to get started. Once inside, simply `rosdep update` and `colcon build` and you're ready to get started! Try `ros2 launch fuse_tutorials fuse_3d_tutorial.launch.py` and look at its pertinent code for a crash course in fuse and its capabilities. @@ -40,7 +42,7 @@ optimizer will cache the constraints and process them in small batches on some s require considerable processing time, introducing a delay between the completion of the optimization cycle and the publishing of data to the ROS topic. -![fuse sequence diagram](doc/fuse_sequence_diagram.png) +![fuse sequence diagram](doc/images/fuse_sequence_diagram.png) ## Example @@ -74,14 +76,14 @@ time. This is enough to construct our first `fuse` system. Below is the constrai system. The large circles represent state variables at a given time, while the small squares represent measurements. The graph connectivity indicates which variables are involved in what measurements. -![fuse graph](doc/fuse_graph_1.png) +![fuse graph](doc/images/fuse_graph_1.png) The two sensor models are configured as plugins to an optimizer implementation. The optimizer performs the required computation to generate the optimal state variable values based on the provided sensor constraints. We will never be able to exactly satisfy both the wheel encoder constraints and the laserscan constraints. Instead we minimize the error of all the constraints using nonlinear least squares optimization. -![fuse optimizer](doc/fuse_optimizer_1.png) +![fuse optimizer](doc/images/fuse_optimizer_1.png) While our `fuse` system is optimizing constraints from two different sensors, it is not yet publishing any data back out to ROS. In order to publish data to ROS, we derive a `fuse_core::Publisher` class and add it to the @@ -90,7 +92,7 @@ implementation determines what type of messages are published and at what freque we would like visualize the current pose of the robot in RViz, so we create a `fuse` publisher that finds the most recent pose and converts it into a `geometry_msgs::msg::PoseStamped` message, then publishes the message to a topic. -![fuse optimizer](doc/fuse_optimizer_2.png) +![fuse optimizer](doc/images/fuse_optimizer_2.png) We finally have something that is starting to be useful. @@ -100,7 +102,7 @@ Typically the laser measurements and the wheel encoder measurements are not sync sampled faster than the laser, and are sampled at different times using a different clock. If we do not do anything different in this situation, the constraint graph becomes disconnected. -![fuse graph](doc/fuse_graph_2.png) +![fuse graph](doc/images/fuse_graph_2.png) This is where motion models come into play. A motion model differs from a sensor model in that constraints can be generated between any two requested timestamps. Motion model constraints are generated upon request, not due to their @@ -108,12 +110,12 @@ own internal clock. We use the motion model to connect the states introduced by derive a class from the `fuse_core::MotionModel` base class and implement a differential drive kinematic constraint for our robot. -![fuse optimizer](doc/fuse_optimizer_3.png) +![fuse optimizer](doc/images/fuse_optimizer_3.png) The motion models are also configured as plugins to the optimizer. The optimizer requests motion models constraints from the configured plugins whenever new states are created by the sensor models. -![fuse graph](doc/fuse_graph_3.png) +![fuse graph](doc/images/fuse_graph_3.png) ### Adaptation #2: Full path publishing @@ -121,7 +123,7 @@ Nothing about the `fuse` framework limits you to having a single publisher. What robot trajectory, instead of just the most recent pose? Well, we can create a new derived `fuse_core::Publisher` class that publishes all of the robot poses using a `nav_msgs::msg::Path` message. -![fuse optimizer](doc/fuse_optimizer_4.png) +![fuse optimizer](doc/images/fuse_optimizer_4.png) ### Adaptation #3: Changing kinematics @@ -129,9 +131,9 @@ In your spare time, you also build [autonomous power wheels racers](http://www.p don't use differential drive; you need a different motion model. Easy enough. We simply derive a new `fuse_core::MotionModel` class that implements an Ackermann steering model. Everything else can be reused. -![fuse optimizer](doc/fuse_optimizer_5.png) +![fuse optimizer](doc/images/fuse_optimizer_5.png) -![fuse graph](doc/fuse_graph_4.png) +![fuse graph](doc/images/fuse_graph_4.png) ### Adaptation #4: Online calibration @@ -146,9 +148,9 @@ how the wheel diameter is expected to change over time. Maybe some sort of expon derive a new publisher plugin from `fuse_core::Publisher` that publishes the current wheel diameter. This allows us to plot how the wheel diameter changes over the length of the race. -![fuse optimizer](doc/fuse_optimizer_6.png) +![fuse optimizer](doc/images/fuse_optimizer_6.png) -![fuse graph](doc/fuse_graph_5.png) +![fuse graph](doc/images/fuse_graph_5.png) Now our system estimates the wheel diameters at each time step as well as the robot's pose. @@ -167,11 +169,27 @@ end users to concentrate on modeling the robot, sensor, system, etc. and spend l sensor models together into runable code. And since all of the models are implemented as plugins, separate plugin libraries can be shared or kept private at the discretion of their authors. -## API Concepts +## Documentation + +### API Concepts -* [Variables](doc/Variables.md) -* [Constraints](doc/Constraints.md) +* Optimizers -- coming soon +* [Variables](doc/concepts/Variables.md) +* Graphs -- coming soon * Sensor Models -- coming soon * Motion Models -- coming soon * Publishers -- coming soon -* Optimizers -- coming soon +* [Constraints](doc/concepts/Constraints.md) + +### Implementation Documentation + +This is documentation of specific implementations of the above concepts. + +Documentation in progress, see [this issue](https://github.com/PickNikRobotics/fuse/issues/23). + +* [Optimizers](./fuse_optimizers/doc/optimizers.md) +* [Variables](./fuse_variables/doc/variables.md) +* [Graphs](./fuse_graphs/doc/graphs.md) +* [Motion Models](./fuse_models/doc/motion_models/motion_models.md) +* [Sensor Models](./fuse_models/doc/sensor_models/sensor_models.md) +* [Constraints](./fuse_constraints/doc/constraints.md) diff --git a/doc/Constraints.md b/doc/concepts/Constraints.md similarity index 100% rename from doc/Constraints.md rename to doc/concepts/Constraints.md diff --git a/doc/Variables.md b/doc/concepts/Variables.md similarity index 98% rename from doc/Variables.md rename to doc/concepts/Variables.md index d6c067b88..53940b91c 100644 --- a/doc/Variables.md +++ b/doc/concepts/Variables.md @@ -17,16 +17,16 @@ the system. There are several reasons multiple identities of a Variable will be * The Variable may represent a property common to multiple entity types. Both a robot and a visual landmark have a position in space. Different identities of the same Variable may be used to describe these different entities. - ![common property](variables-common_property.png) + ![common property](../images/variables-common_property.png) * Similarly, there may be multiple occurrences of the same entity type within the system. A multi-robot configuration will want to track the pose of each robot, and thus different identities of a Variable will be used for each robot in the system. - ![multiple occurrences](variables-multiple_occurrences.png) + ![multiple occurrences](../images/variables-multiple_occurrences.png) * Most commonly, a Variable will represent a time-varying process. A different identity will be required for each time instant for which the process value is to be estimated. For example, the pose of the robot will change over time, so we need a unique identity representing the robot pose at time `t1` **and** another unique identity representing the robot pose at time `t2`. Any time-varying process must be discretized within the fuse stack. - ![time series](variables-time_series.png) + ![time series](../images/variables-time_series.png) The identity takes the form of a UUID or hash, and is generally derived from a set of additional properties that describe what makes each occurrence unique from other occurrences. In the case of a time-varying process, this will diff --git a/doc/fuse_graph_1.png b/doc/images/fuse_graph_1.png similarity index 100% rename from doc/fuse_graph_1.png rename to doc/images/fuse_graph_1.png diff --git a/doc/fuse_graph_2.png b/doc/images/fuse_graph_2.png similarity index 100% rename from doc/fuse_graph_2.png rename to doc/images/fuse_graph_2.png diff --git a/doc/fuse_graph_3.png b/doc/images/fuse_graph_3.png similarity index 100% rename from doc/fuse_graph_3.png rename to doc/images/fuse_graph_3.png diff --git a/doc/fuse_graph_4.png b/doc/images/fuse_graph_4.png similarity index 100% rename from doc/fuse_graph_4.png rename to doc/images/fuse_graph_4.png diff --git a/doc/fuse_graph_5.png b/doc/images/fuse_graph_5.png similarity index 100% rename from doc/fuse_graph_5.png rename to doc/images/fuse_graph_5.png diff --git a/doc/fuse_lightning_talk.pdf b/doc/images/fuse_lightning_talk.pdf similarity index 100% rename from doc/fuse_lightning_talk.pdf rename to doc/images/fuse_lightning_talk.pdf diff --git a/doc/fuse_optimizer_1.png b/doc/images/fuse_optimizer_1.png similarity index 100% rename from doc/fuse_optimizer_1.png rename to doc/images/fuse_optimizer_1.png diff --git a/doc/fuse_optimizer_2.png b/doc/images/fuse_optimizer_2.png similarity index 100% rename from doc/fuse_optimizer_2.png rename to doc/images/fuse_optimizer_2.png diff --git a/doc/fuse_optimizer_3.png b/doc/images/fuse_optimizer_3.png similarity index 100% rename from doc/fuse_optimizer_3.png rename to doc/images/fuse_optimizer_3.png diff --git a/doc/fuse_optimizer_4.png b/doc/images/fuse_optimizer_4.png similarity index 100% rename from doc/fuse_optimizer_4.png rename to doc/images/fuse_optimizer_4.png diff --git a/doc/fuse_optimizer_5.png b/doc/images/fuse_optimizer_5.png similarity index 100% rename from doc/fuse_optimizer_5.png rename to doc/images/fuse_optimizer_5.png diff --git a/doc/fuse_optimizer_6.png b/doc/images/fuse_optimizer_6.png similarity index 100% rename from doc/fuse_optimizer_6.png rename to doc/images/fuse_optimizer_6.png diff --git a/doc/fuse_sequence_diagram.png b/doc/images/fuse_sequence_diagram.png similarity index 100% rename from doc/fuse_sequence_diagram.png rename to doc/images/fuse_sequence_diagram.png diff --git a/doc/variables-common_property.png b/doc/images/variables-common_property.png similarity index 100% rename from doc/variables-common_property.png rename to doc/images/variables-common_property.png diff --git a/doc/variables-multiple_occurrences.png b/doc/images/variables-multiple_occurrences.png similarity index 100% rename from doc/variables-multiple_occurrences.png rename to doc/images/variables-multiple_occurrences.png diff --git a/doc/variables-time_series.png b/doc/images/variables-time_series.png similarity index 100% rename from doc/variables-time_series.png rename to doc/images/variables-time_series.png diff --git a/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp b/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp index 7e489ef1b..ff4d09768 100644 --- a/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp +++ b/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp @@ -57,12 +57,12 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture static const fuse_core::Matrix3d sqrt_information; // Parameters - static const double* parameters[]; + static double const* parameters[]; // Residuals fuse_core::Vector3d residuals; - static const std::vector& block_sizes; + static std::vector const& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; @@ -72,40 +72,40 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture private: // Cost function covariance - static const double covariance_diagonal[]; + static double const covariance_diagonal[]; static const fuse_core::Matrix3d covariance; // Parameter blocks - static const double position1[]; - static const double orientation1[]; - static const double position2[]; - static const double orientation2[]; + static double const position1[]; + static double const orientation1[]; + static double const position2[]; + static double const orientation2[]; // Jacobian matrices std::vector J; }; // Cost function covariance -const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; +double const NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::covariance = fuse_core::Vector3d(covariance_diagonal).asDiagonal(); // Parameter blocks -const double NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 }; -const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = { 0.5 }; -const double NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 }; -const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = { 1.5 }; +double const NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 }; +double const NormalDeltaPose2DBenchmarkFixture::orientation1[] = { 0.5 }; +double const NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 }; +double const NormalDeltaPose2DBenchmarkFixture::orientation2[] = { 1.5 }; // Delta and sqrt information matrix const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{ 1.0, 2.0, 3.0 }; const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU()); // Parameters -const double* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 }; +double const* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 }; -const std::vector& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 }; +std::vector const& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 }; const size_t NormalDeltaPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size(); const size_t NormalDeltaPose2DBenchmarkFixture::num_residuals = 3; @@ -126,7 +126,7 @@ BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2 BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor - const auto partial_sqrt_information = sqrt_information.topRows(state.range(0)); + auto const partial_sqrt_information = sqrt_information.topRows(state.range(0)); const ceres::AutoDiffCostFunction cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, delta), partial_sqrt_information.rows()); diff --git a/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp b/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp index c550a82cf..4c23a57af 100644 --- a/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp +++ b/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp @@ -57,12 +57,12 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture static const fuse_core::Matrix3d sqrt_information; // Parameters - static const double* parameters[]; + static double const* parameters[]; // Residuals fuse_core::Vector3d residuals; - static const std::vector& block_sizes; + static std::vector const& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; @@ -72,36 +72,36 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture private: // Cost function covariance - static const double covariance_diagonal[]; + static double const covariance_diagonal[]; static const fuse_core::Matrix3d covariance; // Parameter blocks - static const double position[]; - static const double orientation[]; + static double const position[]; + static double const orientation[]; // Jacobian matrices std::vector J; }; // Cost function covariance -const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; +double const NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::covariance = fuse_core::Vector3d(covariance_diagonal).asDiagonal(); // Parameter blocks -const double NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 }; -const double NormalPriorPose2DBenchmarkFixture::orientation[] = { 0.0 }; +double const NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 }; +double const NormalPriorPose2DBenchmarkFixture::orientation[] = { 0.0 }; // Mean and sqrt information matrix const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{ 1.0, 2.0, 3.0 }; const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU()); // Parameters -const double* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation }; +double const* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation }; -const std::vector& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 }; +std::vector const& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 }; const size_t NormalPriorPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size(); const size_t NormalPriorPose2DBenchmarkFixture::num_residuals = 3; @@ -122,7 +122,7 @@ BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2 BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor - const auto partial_sqrt_information = sqrt_information.topRows(state.range(0)); + auto const partial_sqrt_information = sqrt_information.topRows(state.range(0)); const ceres::AutoDiffCostFunction cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, mean), partial_sqrt_information.rows()); diff --git a/fuse_constraints/doc/constraints.md b/fuse_constraints/doc/constraints.md new file mode 100644 index 000000000..154b40152 --- /dev/null +++ b/fuse_constraints/doc/constraints.md @@ -0,0 +1,3 @@ +# Constraints + +[Coming soon](https://github.com/PickNikRobotics/fuse/issues/23) diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index e4b90b549..2ecb1a36c 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -93,8 +93,8 @@ class AbsoluteConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior value of all variable dimensions * @param[in] covariance The measurement/prior uncertainty of all variable dimensions */ - AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& mean, - const fuse_core::MatrixXd& covariance); + AbsoluteConstraint(std::string const& source, Variable const& variable, fuse_core::VectorXd const& mean, + fuse_core::MatrixXd const& covariance); /** * @brief Create a constraint using a measurement/prior of only a partial set of dimensions of the @@ -109,8 +109,8 @@ class AbsoluteConstraint : public fuse_core::Constraint * by \p indices. * @param[in] indices The set of indices corresponding to the measured dimensions */ - AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& partial_mean, - const fuse_core::MatrixXd& partial_covariance, const std::vector& indices); + AbsoluteConstraint(std::string const& source, Variable const& variable, fuse_core::VectorXd const& partial_mean, + fuse_core::MatrixXd const& partial_covariance, std::vector const& indices); /** * @brief Destructor @@ -124,7 +124,7 @@ class AbsoluteConstraint : public fuse_core::Constraint * are in the order defined by the variable, not the order defined by the \p indices parameter. * All unmeasured variable dimensions are set to zero. */ - const fuse_core::VectorXd& mean() const + fuse_core::VectorXd const& mean() const { return mean_; } @@ -137,7 +137,7 @@ class AbsoluteConstraint : public fuse_core::Constraint * variable_dimensions. If only a partial set of dimensions are measured, then this matrix will * not be square. */ - const fuse_core::MatrixXd& sqrtInformation() const + fuse_core::MatrixXd const& sqrtInformation() const { return sqrt_information_; } @@ -187,7 +187,7 @@ class AbsoluteConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& mean_; diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index d46e41927..8d020de7a 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -46,8 +46,8 @@ namespace fuse_constraints { template -AbsoluteConstraint::AbsoluteConstraint(const std::string& source, const Variable& variable, - const fuse_core::VectorXd& mean, const fuse_core::MatrixXd& covariance) +AbsoluteConstraint::AbsoluteConstraint(std::string const& source, Variable const& variable, + fuse_core::VectorXd const& mean, fuse_core::MatrixXd const& covariance) : fuse_core::Constraint(source, { variable.uuid() }) , // NOLINT(whitespace/braces) mean_(mean) @@ -59,10 +59,10 @@ AbsoluteConstraint::AbsoluteConstraint(const std::string& source, cons } template -AbsoluteConstraint::AbsoluteConstraint(const std::string& source, const Variable& variable, - const fuse_core::VectorXd& partial_mean, - const fuse_core::MatrixXd& partial_covariance, - const std::vector& indices) +AbsoluteConstraint::AbsoluteConstraint(std::string const& source, Variable const& variable, + fuse_core::VectorXd const& partial_mean, + fuse_core::MatrixXd const& partial_covariance, + std::vector const& indices) : fuse_core::Constraint(source, { variable.uuid() }) // NOLINT(whitespace/braces) { assert(partial_mean.rows() == static_cast(indices.size())); diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp index d3f7b02c0..d6b6794d7 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp @@ -79,9 +79,9 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as a quaternion (4x1 vector: w, x, y, z) * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint(const std::string& source, - const fuse_variables::Orientation3DStamped& orientation, - const fuse_core::Vector4d& mean, const fuse_core::Matrix3d& covariance); + AbsoluteOrientation3DStampedConstraint(std::string const& source, + fuse_variables::Orientation3DStamped const& orientation, + fuse_core::Vector4d const& mean, fuse_core::Matrix3d const& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -91,9 +91,9 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as an Eigen quaternion * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint(const std::string& source, - const fuse_variables::Orientation3DStamped& orientation, - const Eigen::Quaterniond& mean, const fuse_core::Matrix3d& covariance); + AbsoluteOrientation3DStampedConstraint(std::string const& source, + fuse_variables::Orientation3DStamped const& orientation, + Eigen::Quaterniond const& mean, fuse_core::Matrix3d const& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -103,10 +103,10 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as a ROS quaternion message * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint(const std::string& source, - const fuse_variables::Orientation3DStamped& orientation, - const geometry_msgs::msg::Quaternion& mean, - const std::array& covariance); + AbsoluteOrientation3DStampedConstraint(std::string const& source, + fuse_variables::Orientation3DStamped const& orientation, + geometry_msgs::msg::Quaternion const& mean, + std::array const& covariance); /** * @brief Destructor @@ -118,7 +118,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (w, x, y, z) */ - const fuse_core::Vector4d& mean() const + fuse_core::Vector4d const& mean() const { return mean_; } @@ -128,7 +128,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z) */ - const fuse_core::Matrix3d& sqrtInformation() const + fuse_core::Matrix3d const& sqrtInformation() const { return sqrt_information_; } @@ -165,21 +165,21 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion - The input Eigen quaternion * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const Eigen::Quaterniond& quaternion); + static fuse_core::Vector4d toEigen(Eigen::Quaterniond const& quaternion); /** * @brief Utility method to convert an ROS quaternion message to an Eigen Vector4d * @param[in] quaternion - The input ROS quaternion message * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion& quaternion); + static fuse_core::Vector4d toEigen(geometry_msgs::msg::Quaternion const& quaternion); /** * @brief Utility method to convert a flat 1D array to a 3x3 Eigen matrix * @param[in] covariance - The input covariance array * @return The \p covariance, converted to an Eigen Matrix3d */ - static fuse_core::Matrix3d toEigen(const std::array& covariance); + static fuse_core::Matrix3d toEigen(std::array const& covariance); fuse_core::Vector4d mean_; //!< The measured/prior mean vector for this variable fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix @@ -196,7 +196,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& mean_; diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp index bcf527abc..bb819c410 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp @@ -86,10 +86,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] axes Used to specify which of the Euler axes they want to include in the * constraint, e.g. "{ Euler::ROLL, EULER::YAW }" */ - AbsoluteOrientation3DStampedEulerConstraint(const std::string& source, - const fuse_variables::Orientation3DStamped& orientation, - const fuse_core::VectorXd& mean, const fuse_core::MatrixXd& covariance, - const std::vector& axes); + AbsoluteOrientation3DStampedEulerConstraint(std::string const& source, + fuse_variables::Orientation3DStamped const& orientation, + fuse_core::VectorXd const& mean, fuse_core::MatrixXd const& covariance, + std::vector const& axes); /** * @brief Destructor @@ -112,7 +112,7 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * other currently implemented constraints in that the order does _not_ match the order defined in * the variable. */ - const fuse_core::VectorXd& mean() const + fuse_core::VectorXd const& mean() const { return mean_; } @@ -124,7 +124,7 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * from all other currently implemented constraints in that the order does _not_ match the order * defined in the variable. */ - const fuse_core::MatrixXd& sqrtInformation() const + fuse_core::MatrixXd const& sqrtInformation() const { return sqrt_information_; } @@ -174,7 +174,7 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& mean_; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp index 00623124b..675a21618 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp @@ -102,12 +102,12 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * dimensions e.g. "{fuse_variables::Orientation2DStamped::Yaw}" */ AbsolutePose2DStampedConstraint( - const std::string& source, const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation, const fuse_core::VectorXd& partial_mean, - const fuse_core::MatrixXd& partial_covariance, - const std::vector& linear_indices = { fuse_variables::Position2DStamped::X, + std::string const& source, fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation, fuse_core::VectorXd const& partial_mean, + fuse_core::MatrixXd const& partial_covariance, + std::vector const& linear_indices = { fuse_variables::Position2DStamped::X, fuse_variables::Position2DStamped::Y }, // NOLINT - const std::vector& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT + std::vector const& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT /** * @brief Destructor @@ -120,7 +120,7 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * Order is (x, y, yaw). Note that the returned vector will be full sized (3x1) and in the stated * order. */ - const fuse_core::Vector3d& mean() const + fuse_core::Vector3d const& mean() const { return mean_; } @@ -131,7 +131,7 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * If only a partial covariance matrix was provided in the constructor, this covariance matrix * will not be square. */ - const fuse_core::MatrixXd& sqrtInformation() const + fuse_core::MatrixXd const& sqrtInformation() const { return sqrt_information_; } @@ -180,7 +180,7 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& mean_; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp index 8fcc4925c..1b99856f8 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp @@ -87,9 +87,9 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * (7x1 vector: x, y, z, qw, qx, qy, qz) * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, qx, qy, qz) */ - AbsolutePose3DStampedConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, - const fuse_variables::Orientation3DStamped& orientation, - const fuse_core::Vector7d& mean, const fuse_core::Matrix6d& covariance); + AbsolutePose3DStampedConstraint(std::string const& source, fuse_variables::Position3DStamped const& position, + fuse_variables::Orientation3DStamped const& orientation, + fuse_core::Vector7d const& mean, fuse_core::Matrix6d const& covariance); /** * @brief Destructor @@ -101,7 +101,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z, qw, qx, qy, qz) */ - const fuse_core::Vector7d& mean() const + fuse_core::Vector7d const& mean() const { return mean_; } @@ -111,7 +111,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z, qx, qy, qz) */ - const fuse_core::Matrix6d& sqrtInformation() const + fuse_core::Matrix6d const& sqrtInformation() const { return sqrt_information_; } @@ -161,7 +161,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& mean_; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp index e367e1742..a98516b11 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -90,9 +90,9 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: x, y, z, roll, pitch, yaw) * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) */ - AbsolutePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, - const fuse_variables::Orientation3DStamped& orientation, - const fuse_core::Vector6d& mean, const fuse_core::Matrix6d& covariance); + AbsolutePose3DStampedEulerConstraint(std::string const& source, fuse_variables::Position3DStamped const& position, + fuse_variables::Orientation3DStamped const& orientation, + fuse_core::Vector6d const& mean, fuse_core::Matrix6d const& covariance); /** * @brief Create a constraint using a partial measurement/prior of the 3D pose @@ -105,11 +105,11 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] partial_covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) * @param[in] variable_indices The indices of the measured variables */ - AbsolutePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, - const fuse_variables::Orientation3DStamped& orientation, - const fuse_core::Vector6d& partial_mean, - const fuse_core::MatrixXd& partial_covariance, - const std::vector& variable_indices); + AbsolutePose3DStampedEulerConstraint(std::string const& source, fuse_variables::Position3DStamped const& position, + fuse_variables::Orientation3DStamped const& orientation, + fuse_core::Vector6d const& partial_mean, + fuse_core::MatrixXd const& partial_covariance, + std::vector const& variable_indices); /** * @brief Destructor @@ -121,7 +121,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * Order is (x, y, z, roll, pitch, yaw) */ - const fuse_core::Vector6d& mean() const + fuse_core::Vector6d const& mean() const { return mean_; } @@ -131,7 +131,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * Order is (x, y, z, roll, pitch, yaw) */ - const fuse_core::MatrixXd& sqrtInformation() const + fuse_core::MatrixXd const& sqrtInformation() const { return sqrt_information_; } @@ -178,7 +178,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& mean_; diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp index f9c8ed372..e12d41c3c 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp @@ -103,8 +103,8 @@ class MarginalConstraint : public fuse_core::Constraint * @param[in] b The b vector of the marginal cost (of the form A*(x - x_bar) + b) */ template - MarginalConstraint(const std::string& source, VariableIterator first_variable, VariableIterator last_variable, - MatrixIterator first_A, MatrixIterator last_A, const fuse_core::VectorXd& b); + MarginalConstraint(std::string const& source, VariableIterator first_variable, VariableIterator last_variable, + MatrixIterator first_A, MatrixIterator last_A, fuse_core::VectorXd const& b); /** * @brief Destructor @@ -114,7 +114,7 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the A matrices of the marginal constraint */ - const std::vector& A() const + std::vector const& A() const { return A_; } @@ -122,7 +122,7 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the b vector of the marginal constraint */ - const fuse_core::VectorXd& b() const + fuse_core::VectorXd const& b() const { return b_; } @@ -130,7 +130,7 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the variable linearization points, x_bar */ - const std::vector& x_bar() const + std::vector const& x_bar() const { return x_bar_; } @@ -139,7 +139,7 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the variable local parameterizations */ - const std::vector& localParameterizations() const + std::vector const& localParameterizations() const { return local_parameterizations_; } @@ -147,7 +147,7 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the variable manifolds */ - const std::vector& manifolds() const + std::vector const& manifolds() const { return manifolds_; } @@ -194,7 +194,7 @@ class MarginalConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being written. */ template - void save(Archive& archive, const unsigned int /* version */) const + void save(Archive& archive, unsigned int const /* version */) const { archive << boost::serialization::base_object(*this); archive << A_; @@ -214,7 +214,7 @@ class MarginalConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read. */ template - void load(Archive& archive, const unsigned int version) + void load(Archive& archive, unsigned int const version) { archive >> boost::serialization::base_object(*this); archive >> A_; @@ -265,7 +265,7 @@ namespace detail /** * @brief Return the UUID of the provided variable */ -inline const fuse_core::UUID getUuid(const fuse_core::Variable& variable) +inline const fuse_core::UUID getUuid(fuse_core::Variable const& variable) { return variable.uuid(); } @@ -273,7 +273,7 @@ inline const fuse_core::UUID getUuid(const fuse_core::Variable& variable) /** * @brief Return the current value of the provided variable */ -inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable& variable) +inline const fuse_core::VectorXd getCurrentValue(fuse_core::Variable const& variable) { return Eigen::Map(variable.data(), variable.size()); } @@ -282,7 +282,7 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable& vari /** * @brief Return the local parameterization of the provided variable */ -inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(const fuse_core::Variable& variable) +inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(fuse_core::Variable const& variable) { return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization()); } @@ -290,7 +290,7 @@ inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(cons /** * @brief Return the manifold of the provided variable */ -inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable& variable) +inline fuse_core::Manifold::SharedPtr getManifold(fuse_core::Variable const& variable) { return fuse_core::Manifold::SharedPtr(variable.manifold()); } @@ -299,9 +299,9 @@ inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable& var } // namespace detail template -MarginalConstraint::MarginalConstraint(const std::string& source, VariableIterator first_variable, +MarginalConstraint::MarginalConstraint(std::string const& source, VariableIterator first_variable, VariableIterator last_variable, MatrixIterator first_A, MatrixIterator last_A, - const fuse_core::VectorXd& b) + fuse_core::VectorXd const& b) : Constraint(source, boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getUuid), boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)) , A_(first_A, last_A) @@ -328,10 +328,10 @@ MarginalConstraint::MarginalConstraint(const std::string& source, VariableIterat assert(A_.size() == manifolds_.size()); #endif assert(b_.rows() > 0); - assert(std::all_of(A_.begin(), A_.end(), [this](const auto& A) { return A.rows() == this->b_.rows(); })); // NOLINT + assert(std::all_of(A_.begin(), A_.end(), [this](auto const& A) { return A.rows() == this->b_.rows(); })); // NOLINT assert(std::all_of(boost::make_zip_iterator(boost::make_tuple(A_.begin(), first_variable)), boost::make_zip_iterator(boost::make_tuple(A_.end(), last_variable)), - [](const boost::tuple& tuple) // NOLINT + [](boost::tuple const& tuple) // NOLINT { return static_cast(tuple.get<0>().cols()) == tuple.get<1>().localSize(); })); // NOLINT } diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp index c8a734221..f39dae102 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp @@ -78,9 +78,9 @@ class MarginalCostFunction : public ceres::CostFunction * @param[in] x_bar The linearization point of the involved variables * @param[in] local_parameterizations The local parameterization associated with the variable */ - MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, - const std::vector& x_bar, - const std::vector& local_parameterizations); + MarginalCostFunction(std::vector const& A, fuse_core::VectorXd const& b, + std::vector const& x_bar, + std::vector const& local_parameterizations); #else /** * @brief Construct a cost function instance @@ -90,9 +90,9 @@ class MarginalCostFunction : public ceres::CostFunction * @param[in] x_bar The linearization point of the involved variables * @param[in] manifolds The manifold associated with the variable */ - MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, - const std::vector& x_bar, - const std::vector& manifolds); + MarginalCostFunction(std::vector const& A, fuse_core::VectorXd const& b, + std::vector const& x_bar, + std::vector const& manifolds); #endif /** @@ -107,15 +107,15 @@ class MarginalCostFunction : public ceres::CostFunction bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override; private: - const std::vector& A_; //!< The A matrices of the marginal cost - const fuse_core::VectorXd& b_; //!< The b vector of the marginal cost + std::vector const& A_; //!< The A matrices of the marginal cost + fuse_core::VectorXd const& b_; //!< The b vector of the marginal cost #if !CERES_SUPPORTS_MANIFOLDS //!< Parameterizations - const std::vector& local_parameterizations_; + std::vector const& local_parameterizations_; #else - const std::vector& manifolds_; //!< Manifolds + std::vector const& manifolds_; //!< Manifolds #endif - const std::vector& x_bar_; //!< The linearization point of each variable + std::vector const& x_bar_; //!< The linearization point of each variable }; } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp index f69c9aad4..d151945e4 100644 --- a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp +++ b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp @@ -71,8 +71,8 @@ namespace fuse_constraints * least one marginalized variable * @return The mapping from variable UUID to the computed elimination order */ -UuidOrdering computeEliminationOrder(const std::vector& marginalized_variables, - const fuse_core::Graph& graph); +UuidOrdering computeEliminationOrder(std::vector const& marginalized_variables, + fuse_core::Graph const& graph); /** * @brief Generate a transaction that, when applied to the graph, will marginalize out the requested @@ -94,9 +94,9 @@ UuidOrdering computeEliminationOrder(const std::vector& margina * @return A transaction object containing the computed marginal constraints to be added, as well as * the set of variables and constraints to be removed. */ -fuse_core::Transaction marginalizeVariables(const std::string& source, - const std::vector& marginalized_variables, - const fuse_core::Graph& graph); +fuse_core::Transaction marginalizeVariables(std::string const& source, + std::vector const& marginalized_variables, + fuse_core::Graph const& graph); /** * @brief Generate a transaction that, when applied to the graph, will marginalize out the requested @@ -120,10 +120,10 @@ fuse_core::Transaction marginalizeVariables(const std::string& source, * @return A transaction object containing the computed marginal constraints to be added, as well as * the set of variables and constraints to be removed. */ -fuse_core::Transaction marginalizeVariables(const std::string& source, - const std::vector& marginalized_variables, - const fuse_core::Graph& graph, - const fuse_constraints::UuidOrdering& elimination_order); +fuse_core::Transaction marginalizeVariables(std::string const& source, + std::vector const& marginalized_variables, + fuse_core::Graph const& graph, + fuse_constraints::UuidOrdering const& elimination_order); namespace detail { @@ -153,8 +153,8 @@ struct LinearTerm * @return A LinearTerm consisting of Jacobian blocks associated with each involved variable in * elimination order */ -LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::Graph& graph, - const UuidOrdering& elimination_order); +LinearTerm linearize(fuse_core::Constraint const& constraint, fuse_core::Graph const& graph, + UuidOrdering const& elimination_order); /** * @brief Marginalize out the lowest-ordered variable from the provided set of linear terms @@ -166,7 +166,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G * index * @return A LinearTerm object containing the information on the remaining variables */ -LinearTerm marginalizeNext(const std::vector& linear_terms); +LinearTerm marginalizeNext(std::vector const& linear_terms); /** * @brief Convert the provided linear term into a MarginalConstraint @@ -178,9 +178,9 @@ LinearTerm marginalizeNext(const std::vector& linear_terms); * @param[in] elimination_order The mapping from variable UUID to LinearTerm variable index * @return An equivalent MarginalConstraint object */ -MarginalConstraint::SharedPtr createMarginalConstraint(const std::string& source, const LinearTerm& linear_term, - const fuse_core::Graph& graph, - const UuidOrdering& elimination_order); +MarginalConstraint::SharedPtr createMarginalConstraint(std::string const& source, LinearTerm const& linear_term, + fuse_core::Graph const& graph, + UuidOrdering const& elimination_order); } // namespace detail } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/normal_delta.hpp b/fuse_constraints/include/fuse_constraints/normal_delta.hpp index 8cf4ebd85..4c0deb0ef 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta.hpp @@ -72,7 +72,7 @@ class NormalDelta : public ceres::CostFunction * these are the same type of variable. At a minimum, they must have the same * dimensions and the per-element subtraction operator must be valid. */ - NormalDelta(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b); + NormalDelta(fuse_core::MatrixXd const& A, fuse_core::VectorXd const& b); /** * @brief Destructor diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp index 5864f1962..909a3704f 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp @@ -68,7 +68,7 @@ class NormalDeltaOrientation2D : public ceres::SizedCostFunction<1, 1, 1> * these are the same type of variable. At a minimum, they must have the same * dimensions and the per-element subtraction operator must be valid. */ - NormalDeltaOrientation2D(const double A, const double b); + NormalDeltaOrientation2D(double const A, double const b); /** * @brief Destructor diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp index dcdebdba9..8f5ef8a74 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp @@ -79,7 +79,7 @@ class NormalDeltaOrientation3DCostFunctor * order (x, y, z) * @param[in] b The measured change between the two orientation variables */ - NormalDeltaOrientation3DCostFunctor(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) : A_(A), b_(b) + NormalDeltaOrientation3DCostFunctor(fuse_core::Matrix3d const& A, fuse_core::Vector4d const& b) : A_(A), b_(b) { } diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp index 5f41b980a..52ffcc888 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp @@ -82,7 +82,7 @@ class NormalDeltaPose2D : public ceres::SizedCostFunction()) // Orientation residuals will // not be scaled { diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp index 4c1201650..506d31797 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -75,7 +75,7 @@ class NormalDeltaPose3DEulerCostFunctor * order (dx, dy, dz, droll, dpitch, dyaw) * @param[in] b The exposed pose difference in order (dx, dy, dz, droll, dpitch, dyaw) */ - NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b); + NormalDeltaPose3DEulerCostFunctor(fuse_core::MatrixXd const& A, fuse_core::Vector6d const& b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values @@ -89,8 +89,8 @@ class NormalDeltaPose3DEulerCostFunctor fuse_core::Vector6d b_; }; -NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, - const fuse_core::Vector6d& b) +NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor(fuse_core::MatrixXd const& A, + fuse_core::Vector6d const& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp index a870c59d9..819c40865 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp @@ -70,7 +70,7 @@ class NormalPriorOrientation2D : public ceres::SizedCostFunction<1, 1> * these are the same type of variable. At a minimum, they must have the same * dimensions and the per-element subtraction operator must be valid. */ - NormalPriorOrientation2D(const double A, const double b); + NormalPriorOrientation2D(double const A, double const b); /** * @brief Destructor diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp index bcb2ddb8f..863e62bae 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -72,7 +72,7 @@ class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> * order (qx, qy, qz) * @param[in] b The orientation measurement or prior in order (qw, qx, qy, qz) */ - NormalPriorOrientation3D(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b); + NormalPriorOrientation3D(fuse_core::Matrix3d const& A, fuse_core::Vector4d const& b); /** * @brief Destructor diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp index 1ae1ad7c6..ce250a11f 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp @@ -78,7 +78,7 @@ class NormalPriorOrientation3DCostFunctor * order (quaternion_x, quaternion_y, quaternion_z) * @param[in] b The orientation measurement or prior in order (w, x, y, z) */ - NormalPriorOrientation3DCostFunctor(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) : A_(A), b_(b) + NormalPriorOrientation3DCostFunctor(fuse_core::Matrix3d const& A, fuse_core::Vector4d const& b) : A_(A), b_(b) { } diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp index 851d65a58..3fe9ae968 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp @@ -85,8 +85,8 @@ class NormalPriorOrientation3DEulerCostFunctor * @param[in] b The orientation measurement or prior. Its order must match the values in \p axes. * @param[in] axes The Euler angle axes for which we want to compute errors. Defaults to all axes. */ - NormalPriorOrientation3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b, - const std::vector& axes = { Euler::ROLL, Euler::PITCH, Euler::YAW }) + NormalPriorOrientation3DEulerCostFunctor(fuse_core::MatrixXd const& A, fuse_core::VectorXd const& b, + std::vector const& axes = { Euler::ROLL, Euler::PITCH, Euler::YAW }) : // NOLINT A_(A) , b_(b) diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp index c8f262460..eef41de51 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp @@ -77,7 +77,7 @@ class NormalPriorPose2D : public ceres::SizedCostFunction * order (x, y, yaw) * @param[in] b The pose measurement or prior in order (x, y, yaw) */ - NormalPriorPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); + NormalPriorPose2D(fuse_core::MatrixXd const& A, fuse_core::Vector3d const& b); /** * @brief Destructor diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp index 0d24bdb8f..e1c30815c 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp @@ -79,7 +79,7 @@ class NormalPriorPose2DCostFunctor * order (x, y, yaw) * @param[in] b The pose measurement or prior in order (x, y, yaw) */ - NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); + NormalPriorPose2DCostFunctor(fuse_core::MatrixXd const& A, fuse_core::Vector3d const& b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -93,7 +93,7 @@ class NormalPriorPose2DCostFunctor fuse_core::Vector3d b_; //!< The measured 2D pose value }; -NormalPriorPose2DCostFunctor::NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) +NormalPriorPose2DCostFunctor::NormalPriorPose2DCostFunctor(fuse_core::MatrixXd const& A, fuse_core::Vector3d const& b) : A_(A), b_(b) { } diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp index 8088539d7..be07291c7 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -72,7 +72,7 @@ class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> * order (x, y, z, roll, pitch, yaw) * @param[in] b The pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3D(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); + NormalPriorPose3D(fuse_core::Matrix6d const& A, fuse_core::Vector7d const& b); /** * @brief Destructor diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp index 75bcfce05..1d66cd7ec 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp @@ -78,7 +78,7 @@ class NormalPriorPose3DCostFunctor * order (x, y, z, qx, qy, qz) * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); + NormalPriorPose3DCostFunctor(fuse_core::Matrix6d const& A, fuse_core::Vector7d const& b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -93,7 +93,7 @@ class NormalPriorPose3DCostFunctor NormalPriorOrientation3DCostFunctor orientation_functor_; }; -NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) +NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor(fuse_core::Matrix6d const& A, fuse_core::Vector7d const& b) : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled { } diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp index 723fc3fbd..dab6a9eda 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -78,7 +78,7 @@ class NormalPriorPose3DEuler : public ceres::SizedCostFunction& indices); + RelativeConstraint(std::string const& source, Variable const& variable1, Variable const& variable2, + fuse_core::VectorXd const& delta, fuse_core::MatrixXd const& covariance, + std::vector const& indices); /** * @brief Destructor @@ -127,7 +127,7 @@ class RelativeConstraint : public fuse_core::Constraint * are in the order defined by the variable, not the order defined by the \p indices parameter. * All unmeasured variable dimensions are set to zero. */ - const fuse_core::VectorXd& delta() const + fuse_core::VectorXd const& delta() const { return delta_; } @@ -140,7 +140,7 @@ class RelativeConstraint : public fuse_core::Constraint * variable_dimensions. If only a partial set of dimensions are measured, then this matrix will * not be square. */ - const fuse_core::MatrixXd& sqrtInformation() const + fuse_core::MatrixXd const& sqrtInformation() const { return sqrt_information_; } @@ -190,7 +190,7 @@ class RelativeConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& delta_; diff --git a/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp index b39b83535..9911ef4cd 100644 --- a/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp @@ -46,9 +46,9 @@ namespace fuse_constraints { template -RelativeConstraint::RelativeConstraint(const std::string& source, const Variable& variable1, - const Variable& variable2, const fuse_core::VectorXd& delta, - const fuse_core::MatrixXd& covariance) +RelativeConstraint::RelativeConstraint(std::string const& source, Variable const& variable1, + Variable const& variable2, fuse_core::VectorXd const& delta, + fuse_core::MatrixXd const& covariance) : fuse_core::Constraint(source, { variable1.uuid(), variable2.uuid() }) , // NOLINT(whitespace/braces) delta_(delta) @@ -61,10 +61,10 @@ RelativeConstraint::RelativeConstraint(const std::string& source, cons } template -RelativeConstraint::RelativeConstraint(const std::string& source, const Variable& variable1, - const Variable& variable2, const fuse_core::VectorXd& partial_delta, - const fuse_core::MatrixXd& partial_covariance, - const std::vector& indices) +RelativeConstraint::RelativeConstraint(std::string const& source, Variable const& variable1, + Variable const& variable2, fuse_core::VectorXd const& partial_delta, + fuse_core::MatrixXd const& partial_covariance, + std::vector const& indices) : fuse_core::Constraint(source, { variable1.uuid(), variable2.uuid() }) // NOLINT(whitespace/braces) { assert(variable1.size() == variable2.size()); diff --git a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp index ba5c32f8f..36c5545e2 100644 --- a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp @@ -82,10 +82,10 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * (4x1 vector: w, x, y, z) * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint(const std::string& source, - const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Orientation3DStamped& orientation2, - const fuse_core::Vector4d& delta, const fuse_core::Matrix3d& covariance); + RelativeOrientation3DStampedConstraint(std::string const& source, + fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Orientation3DStamped const& orientation2, + fuse_core::Vector4d const& delta, fuse_core::Matrix3d const& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -96,10 +96,10 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] delta The measured orientation change as an Eigen quaternion * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint(const std::string& source, - const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Orientation3DStamped& orientation2, - const Eigen::Quaterniond& delta, const fuse_core::Matrix3d& covariance); + RelativeOrientation3DStampedConstraint(std::string const& source, + fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Orientation3DStamped const& orientation2, + Eigen::Quaterniond const& delta, fuse_core::Matrix3d const& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -110,11 +110,11 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] delta The measured orientation change as a ROS quaternion message * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint(const std::string& source, - const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Orientation3DStamped& orientation2, - const geometry_msgs::msg::Quaternion& delta, - const std::array& covariance); + RelativeOrientation3DStampedConstraint(std::string const& source, + fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Orientation3DStamped const& orientation2, + geometry_msgs::msg::Quaternion const& delta, + std::array const& covariance); /** * @brief Destructor @@ -126,7 +126,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (w, x, y, z) */ - const fuse_core::Vector4d& delta() const + fuse_core::Vector4d const& delta() const { return delta_; } @@ -136,7 +136,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z) */ - const fuse_core::Matrix3d& sqrtInformation() const + fuse_core::Matrix3d const& sqrtInformation() const { return sqrt_information_; } @@ -174,7 +174,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion The input Eigen quaternion * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const Eigen::Quaterniond& quaternion); + static fuse_core::Vector4d toEigen(Eigen::Quaterniond const& quaternion); /** * @brief Utility method to convert an ROS quaternion message to an Eigen Vector4d @@ -182,7 +182,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion The input ROS quaternion message * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion& quaternion); + static fuse_core::Vector4d toEigen(geometry_msgs::msg::Quaternion const& quaternion); /** * @brief Utility method to convert a flat 1D array to a 3x3 Eigen matrix @@ -190,7 +190,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] covariance The input covariance array * @return The \p covariance, converted to an Eigen Matrix3d */ - static fuse_core::Matrix3d toEigen(const std::array& covariance); + static fuse_core::Matrix3d toEigen(std::array const& covariance); fuse_core::Vector4d delta_; //!< The measured/prior mean vector for this variable fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix @@ -207,7 +207,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& delta_; diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp index 54113de52..39eed9a99 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp @@ -105,13 +105,13 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * dimensions e.g., "{fuse_variables::Orientation2DStamped::Yaw}" */ RelativePose2DStampedConstraint( - const std::string& source, const fuse_variables::Position2DStamped& position1, - const fuse_variables::Orientation2DStamped& orientation1, const fuse_variables::Position2DStamped& position2, - const fuse_variables::Orientation2DStamped& orientation2, const fuse_core::VectorXd& partial_delta, - const fuse_core::MatrixXd& partial_covariance, - const std::vector& linear_indices = { fuse_variables::Position2DStamped::X, + std::string const& source, fuse_variables::Position2DStamped const& position1, + fuse_variables::Orientation2DStamped const& orientation1, fuse_variables::Position2DStamped const& position2, + fuse_variables::Orientation2DStamped const& orientation2, fuse_core::VectorXd const& partial_delta, + fuse_core::MatrixXd const& partial_covariance, + std::vector const& linear_indices = { fuse_variables::Position2DStamped::X, fuse_variables::Position2DStamped::Y }, // NOLINT - const std::vector& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT + std::vector const& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT /** * @brief Destructor @@ -124,7 +124,7 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * Order is (dx, dy, dyaw). Note that the returned vector will be full sized (3x1) and in the * stated order. */ - const fuse_core::Vector3d& delta() const + fuse_core::Vector3d const& delta() const { return delta_; } @@ -135,7 +135,7 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * If only a partial covariance matrix was provided in the constructor, this covariance matrix * will not be square. */ - const fuse_core::MatrixXd& sqrtInformation() const + fuse_core::MatrixXd const& sqrtInformation() const { return sqrt_information_; } @@ -185,7 +185,7 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& delta_; diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp index c0a5ff72e..0c98dce0a 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp @@ -84,11 +84,11 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) */ - RelativePose3DStampedConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, - const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Position3DStamped& position2, - const fuse_variables::Orientation3DStamped& orientation2, - const fuse_core::Vector7d& delta, const fuse_core::Matrix6d& covariance); + RelativePose3DStampedConstraint(std::string const& source, fuse_variables::Position3DStamped const& position1, + fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Position3DStamped const& position2, + fuse_variables::Orientation3DStamped const& orientation2, + fuse_core::Vector7d const& delta, fuse_core::Matrix6d const& covariance); /** * @brief Destructor @@ -98,7 +98,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Read-only access to the measured pose change. */ - const fuse_core::Vector7d& delta() const + fuse_core::Vector7d const& delta() const { return delta_; } @@ -106,7 +106,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::Matrix6d& sqrtInformation() const + fuse_core::Matrix6d const& sqrtInformation() const { return sqrt_information_; } @@ -155,7 +155,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& delta_; diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp index ab59f7915..0a0fcc25d 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -85,11 +85,11 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, droll, dpitch, dyaw) */ - RelativePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, - const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Position3DStamped& position2, - const fuse_variables::Orientation3DStamped& orientation2, - const fuse_core::Vector6d& delta, const fuse_core::Matrix6d& covariance); + RelativePose3DStampedEulerConstraint(std::string const& source, fuse_variables::Position3DStamped const& position1, + fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Position3DStamped const& position2, + fuse_variables::Orientation3DStamped const& orientation2, + fuse_core::Vector6d const& delta, fuse_core::Matrix6d const& covariance); /** * @brief Create a constraint using a measurement/prior of the relative 3D pose @@ -104,13 +104,13 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, droll, dpitch, dyaw) * @param[in] variable_indices The indices of the measured variables */ - RelativePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, - const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Position3DStamped& position2, - const fuse_variables::Orientation3DStamped& orientation2, - const fuse_core::Vector6d& partial_delta, - const fuse_core::MatrixXd& partial_covariance, - const std::vector& variable_indices); + RelativePose3DStampedEulerConstraint(std::string const& source, fuse_variables::Position3DStamped const& position1, + fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Position3DStamped const& position2, + fuse_variables::Orientation3DStamped const& orientation2, + fuse_core::Vector6d const& partial_delta, + fuse_core::MatrixXd const& partial_covariance, + std::vector const& variable_indices); /** * @brief Destructor @@ -120,7 +120,7 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint /** * @brief Read-only access to the measured pose change. */ - const fuse_core::Vector6d& delta() const + fuse_core::Vector6d const& delta() const { return delta_; } @@ -128,7 +128,7 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::MatrixXd& sqrtInformation() const + fuse_core::MatrixXd const& sqrtInformation() const { return sqrt_information_; } @@ -174,7 +174,7 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& delta_; diff --git a/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp b/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp index 1140c3884..221d7e5c5 100644 --- a/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp +++ b/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp @@ -103,12 +103,12 @@ class UuidOrdering /** * @brief Return true if the index exists in the ordering */ - bool exists(const unsigned int index) const; + bool exists(unsigned int const index) const; /** * @brief Return true if the UUID exists in the ordering */ - bool exists(const fuse_core::UUID& uuid) const; + bool exists(fuse_core::UUID const& uuid) const; /** * @brief Add a new UUID to the back of the ordering @@ -118,35 +118,35 @@ class UuidOrdering * @param[in] uuid The UUID to insert * @return True if the UUID was inserted, false if the UUID already existed */ - bool push_back(const fuse_core::UUID& uuid); + bool push_back(fuse_core::UUID const& uuid); /** * @brief Access the UUID stored at the provided index * * Accessing an index that does not exist results in undefined behavior */ - const fuse_core::UUID& operator[](const unsigned int index) const; + fuse_core::UUID const& operator[](unsigned int const index) const; /** * @brief Access the index associated with the provided UUID * * Accessing a UUID that does not exist results in the provided UUID being added to the ordering */ - unsigned int operator[](const fuse_core::UUID& uuid); + unsigned int operator[](fuse_core::UUID const& uuid); /** * @brief Access the UUID stored at the provided index * * If the requested index does not exist, an out_of_range exception will be thrown. */ - const fuse_core::UUID& at(const unsigned int index) const; + fuse_core::UUID const& at(unsigned int const index) const; /** * @brief Access the index associated with the provided UUID * * If the requested UUID does not exist, an out_of_range exception will be thrown. */ - unsigned int at(const fuse_core::UUID& uuid) const; + unsigned int at(fuse_core::UUID const& uuid) const; private: using UuidOrderMapping = diff --git a/fuse_constraints/include/fuse_constraints/variable_constraints.hpp b/fuse_constraints/include/fuse_constraints/variable_constraints.hpp index c83d66708..6f65faa49 100644 --- a/fuse_constraints/include/fuse_constraints/variable_constraints.hpp +++ b/fuse_constraints/include/fuse_constraints/variable_constraints.hpp @@ -79,23 +79,23 @@ class VariableConstraints /** * @brief Add this constraint to a single variable */ - void insert(const unsigned int constraint, const unsigned int variable); + void insert(unsigned int const constraint, unsigned int const variable); /** * @brief Add this constraint to all variables in the provided list */ - void insert(const unsigned int constraint, std::initializer_list variable_list); + void insert(unsigned int const constraint, std::initializer_list variable_list); /** * @brief Add this constraint to all variables in the provided range */ template - void insert(const unsigned int constraint, VariableIndexIterator first, VariableIndexIterator last); + void insert(unsigned int const constraint, VariableIndexIterator first, VariableIndexIterator last); /** * @brief Add a single orphan variable, i.e. a variable without constraints */ - void insert(const unsigned int variable); + void insert(unsigned int const variable); /** * @brief Insert all of the constraints connected to the requested variable into the provided @@ -104,7 +104,7 @@ class VariableConstraints * Accessing a variable id that is not part of this container results in undefined behavior */ template - OutputIterator getConstraints(const unsigned int variable_id, OutputIterator result) const; + OutputIterator getConstraints(unsigned int const variable_id, OutputIterator result) const; /** * @brief Print a human-readable description of the variable constraints to the provided stream. @@ -121,7 +121,7 @@ class VariableConstraints }; template -void VariableConstraints::insert(const unsigned int constraint, VariableIndexIterator first, VariableIndexIterator last) +void VariableConstraints::insert(unsigned int const constraint, VariableIndexIterator first, VariableIndexIterator last) { for (; first != last; ++first) { @@ -130,16 +130,16 @@ void VariableConstraints::insert(const unsigned int constraint, VariableIndexIte } template -OutputIterator VariableConstraints::getConstraints(const unsigned int variable_id, OutputIterator result) const +OutputIterator VariableConstraints::getConstraints(unsigned int const variable_id, OutputIterator result) const { - const auto& constraints = variable_constraints_[variable_id]; + auto const& constraints = variable_constraints_[variable_id]; return std::copy(std::begin(constraints), std::end(constraints), result); } /** * Stream operator for printing VariableConstraints objects. */ -std::ostream& operator<<(std::ostream& stream, const VariableConstraints& variable_constraints); +std::ostream& operator<<(std::ostream& stream, VariableConstraints const& variable_constraints); } // namespace fuse_constraints diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index 2091e8527..06d06c7d5 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -45,8 +45,8 @@ namespace fuse_constraints { AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector4d& mean, - const fuse_core::Matrix3d& covariance) + std::string const& source, fuse_variables::Orientation3DStamped const& orientation, fuse_core::Vector4d const& mean, + fuse_core::Matrix3d const& covariance) : fuse_core::Constraint(source, { orientation.uuid() }) , // NOLINT(whitespace/braces) mean_(mean) @@ -55,15 +55,15 @@ AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( } AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const Eigen::Quaterniond& mean, - const fuse_core::Matrix3d& covariance) + std::string const& source, fuse_variables::Orientation3DStamped const& orientation, Eigen::Quaterniond const& mean, + fuse_core::Matrix3d const& covariance) : AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), covariance) { } AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string& source, const fuse_variables::Orientation3DStamped& orientation, - const geometry_msgs::msg::Quaternion& mean, const std::array& covariance) + std::string const& source, fuse_variables::Orientation3DStamped const& orientation, + geometry_msgs::msg::Quaternion const& mean, std::array const& covariance) : AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), toEigen(covariance)) { } @@ -95,21 +95,21 @@ ceres::CostFunction* AbsoluteOrientation3DStampedConstraint::costFunction() cons new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_)); } -fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(const Eigen::Quaterniond& quaternion) +fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(Eigen::Quaterniond const& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z(); return eigen_quaternion_vector; } -fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(const geometry_msgs::msg::Quaternion& quaternion) +fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(geometry_msgs::msg::Quaternion const& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w, quaternion.x, quaternion.y, quaternion.z; return eigen_quaternion_vector; } -fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen(const std::array& covariance) +fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen(std::array const& covariance) { return fuse_core::Matrix3d(covariance.data()); } diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp index 29ba0d652..aa259f982 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp @@ -46,8 +46,8 @@ namespace fuse_constraints { AbsoluteOrientation3DStampedEulerConstraint::AbsoluteOrientation3DStampedEulerConstraint( - const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const fuse_core::VectorXd& mean, - const fuse_core::MatrixXd& covariance, const std::vector& axes) + std::string const& source, fuse_variables::Orientation3DStamped const& orientation, fuse_core::VectorXd const& mean, + fuse_core::MatrixXd const& covariance, std::vector const& axes) : fuse_core::Constraint(source, { orientation.uuid() }) , // NOLINT(whitespace/braces) mean_(mean) diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp index 23bd3c566..949fa221b 100644 --- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp @@ -46,10 +46,10 @@ namespace fuse_constraints { AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( - const std::string& source, const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation, const fuse_core::VectorXd& partial_mean, - const fuse_core::MatrixXd& partial_covariance, const std::vector& linear_indices, - const std::vector& angular_indices) + std::string const& source, fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation, fuse_core::VectorXd const& partial_mean, + fuse_core::MatrixXd const& partial_covariance, std::vector const& linear_indices, + std::vector const& angular_indices) : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) // NOLINT(whitespace/braces) { size_t total_variable_size = position.size() + orientation.size(); diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index 7a113262b..910e16f93 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -44,11 +44,11 @@ namespace fuse_constraints { -AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint(const std::string& source, - const fuse_variables::Position3DStamped& position, - const fuse_variables::Orientation3DStamped& orientation, - const fuse_core::Vector7d& mean, - const fuse_core::Matrix6d& covariance) +AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint(std::string const& source, + fuse_variables::Position3DStamped const& position, + fuse_variables::Orientation3DStamped const& orientation, + fuse_core::Vector7d const& mean, + fuse_core::Matrix6d const& covariance) : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) , // NOLINT mean_(mean) diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp index e3bec0ff4..b9cb761a5 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -45,9 +45,9 @@ namespace fuse_constraints { AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( - const std::string& source, const fuse_variables::Position3DStamped& position, - const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector6d& mean, - const fuse_core::Matrix6d& covariance) + std::string const& source, fuse_variables::Position3DStamped const& position, + fuse_variables::Orientation3DStamped const& orientation, fuse_core::Vector6d const& mean, + fuse_core::Matrix6d const& covariance) : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) , // NOLINT mean_(mean) @@ -56,9 +56,9 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( } AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( - const std::string& source, const fuse_variables::Position3DStamped& position, - const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector6d& partial_mean, - const fuse_core::MatrixXd& partial_covariance, const std::vector& variable_indices) + std::string const& source, fuse_variables::Position3DStamped const& position, + fuse_variables::Orientation3DStamped const& orientation, fuse_core::Vector6d const& partial_mean, + fuse_core::MatrixXd const& partial_covariance, std::vector const& variable_indices) : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) , // NOLINT mean_(partial_mean) diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp index a04225933..fe809d47f 100644 --- a/fuse_constraints/src/marginal_constraint.cpp +++ b/fuse_constraints/src/marginal_constraint.cpp @@ -51,7 +51,7 @@ void MarginalConstraint::print(std::ostream& stream) const << " source: " << source() << "\n" << " uuid: " << uuid() << "\n" << " variable:\n"; - for (const auto& variable : variables()) + for (auto const& variable : variables()) { stream << " - " << variable << "\n"; } diff --git a/fuse_constraints/src/marginal_cost_function.cpp b/fuse_constraints/src/marginal_cost_function.cpp index 34274cca3..d0bd335ef 100644 --- a/fuse_constraints/src/marginal_cost_function.cpp +++ b/fuse_constraints/src/marginal_cost_function.cpp @@ -46,25 +46,25 @@ namespace fuse_constraints { #if !CERES_SUPPORTS_MANIFOLDS MarginalCostFunction::MarginalCostFunction( - const std::vector& A, const fuse_core::VectorXd& b, - const std::vector& x_bar, - const std::vector& local_parameterizations) + std::vector const& A, fuse_core::VectorXd const& b, + std::vector const& x_bar, + std::vector const& local_parameterizations) : A_(A), b_(b), local_parameterizations_(local_parameterizations), x_bar_(x_bar) { set_num_residuals(b_.rows()); - for (const auto& x_bar : x_bar_) + for (auto const& x_bar : x_bar_) { mutable_parameter_block_sizes()->push_back(x_bar.size()); } } #else -MarginalCostFunction::MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, - const std::vector& x_bar, - const std::vector& manifolds) +MarginalCostFunction::MarginalCostFunction(std::vector const& A, fuse_core::VectorXd const& b, + std::vector const& x_bar, + std::vector const& manifolds) : A_(A), b_(b), manifolds_(manifolds), x_bar_(x_bar) { set_num_residuals(b_.rows()); - for (const auto& x_bar : x_bar_) + for (auto const& x_bar : x_bar_) { mutable_parameter_block_sizes()->push_back(x_bar.size()); } @@ -109,13 +109,13 @@ bool MarginalCostFunction::Evaluate(double const* const* parameters, double* res #if !CERES_SUPPORTS_MANIFOLDS if (local_parameterizations_[i]) { - const auto& local_parameterization = local_parameterizations_[i]; + auto const& local_parameterization = local_parameterizations_[i]; fuse_core::MatrixXd J_local(local_parameterization->LocalSize(), local_parameterization->GlobalSize()); local_parameterization->ComputeMinusJacobian(parameters[i], J_local.data()); #else if (manifolds_[i]) { - const auto& manifold = manifolds_[i]; + auto const& manifold = manifolds_[i]; fuse_core::MatrixXd J_local(manifold->TangentSize(), manifold->AmbientSize()); manifold->MinusJacobian(parameters[i], J_local.data()); #endif diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 617f03c41..de9330815 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -57,8 +57,8 @@ namespace fuse_constraints { -UuidOrdering computeEliminationOrder(const std::vector& marginalized_variables, - const fuse_core::Graph& graph) +UuidOrdering computeEliminationOrder(std::vector const& marginalized_variables, + fuse_core::Graph const& graph) { // COLAMD wants a somewhat weird structure // Variables are numbered sequentially in some arbitrary order. We call this the "variable index" @@ -80,10 +80,10 @@ UuidOrdering computeEliminationOrder(const std::vector& margina auto variable_order = UuidOrdering(); auto constraint_order = UuidOrdering(); auto variable_constraints = VariableConstraints(); - for (const auto& variable_uuid : marginalized_variables) + for (auto const& variable_uuid : marginalized_variables) { // Get all connected constraints to this variable - const auto constraints = graph.getConnectedConstraints(variable_uuid); + auto const constraints = graph.getConnectedConstraints(variable_uuid); // If the variable is orphan (it has no constraints), add it to the VariableConstraints object // without constraints New variable index is automatically generated @@ -95,10 +95,10 @@ UuidOrdering computeEliminationOrder(const std::vector& margina { // Add each constraint to the VariableConstraints object // New constraint and variable indices are automatically generated - for (const auto& constraint : constraints) + for (auto const& constraint : constraints) { unsigned int const constraint_index = constraint_order[constraint.uuid()]; - for (const auto& constraint_variable_uuid : constraint.variables()) + for (auto const& constraint_variable_uuid : constraint.variables()) { variable_constraints.insert(constraint_index, variable_order[constraint_variable_uuid]); } @@ -128,7 +128,7 @@ UuidOrdering computeEliminationOrder(const std::vector& margina // Define the variable groups used by CCOLAMD. All of the marginalized variables should be group0, // all the rest should be group1. std::vector variable_groups(variable_order.size(), 1); // Default all variables to group1 - for (const auto& variable_uuid : marginalized_variables) + for (auto const& variable_uuid : marginalized_variables) { // Reassign the marginalized variables to group0 variable_groups[variable_order.at(variable_uuid)] = 0; @@ -159,18 +159,18 @@ UuidOrdering computeEliminationOrder(const std::vector& margina return elimination_order; } -fuse_core::Transaction marginalizeVariables(const std::string& source, - const std::vector& marginalized_variables, - const fuse_core::Graph& graph) +fuse_core::Transaction marginalizeVariables(std::string const& source, + std::vector const& marginalized_variables, + fuse_core::Graph const& graph) { return marginalizeVariables(source, marginalized_variables, graph, computeEliminationOrder(marginalized_variables, graph)); } -fuse_core::Transaction marginalizeVariables(const std::string& source, - const std::vector& marginalized_variables, - const fuse_core::Graph& graph, - const fuse_constraints::UuidOrdering& elimination_order) +fuse_core::Transaction marginalizeVariables(std::string const& source, + std::vector const& marginalized_variables, + fuse_core::Graph const& graph, + fuse_constraints::UuidOrdering const& elimination_order) { // TODO(swilliams) The method used to marginalize variables assumes that all variables are fully // constrained. However, with the introduction of "variables held constant", it is @@ -180,7 +180,7 @@ fuse_core::Transaction marginalizeVariables(const std::string& source, // but that will require a major refactor. assert(std::all_of(marginalized_variables.begin(), marginalized_variables.end(), - [&elimination_order, &marginalized_variables](const fuse_core::UUID& variable_uuid) { + [&elimination_order, &marginalized_variables](fuse_core::UUID const& variable_uuid) { return elimination_order.exists(variable_uuid) && elimination_order.at(variable_uuid) < marginalized_variables.size(); })); // NOLINT @@ -188,7 +188,7 @@ fuse_core::Transaction marginalizeVariables(const std::string& source, fuse_core::Transaction transaction; // Mark all of the marginalized variables for removal - for (const auto& variable_uuid : marginalized_variables) + for (auto const& variable_uuid : marginalized_variables) { transaction.removeVariable(variable_uuid); } @@ -201,14 +201,14 @@ fuse_core::Transaction marginalizeVariables(const std::string& source, std::vector> linear_terms(variable_order.size()); for (size_t i = 0ul; i < marginalized_variables.size(); ++i) { - const auto constraints = graph.getConnectedConstraints(variable_order[i]); - for (const auto& constraint : constraints) + auto const constraints = graph.getConnectedConstraints(variable_order[i]); + for (auto const& constraint : constraints) { if (used_constraints.find(constraint.uuid()) == used_constraints.end()) { used_constraints.insert(constraint.uuid()); // Ensure all connected variables are added to the ordering - for (const auto& variable_uuid : constraint.variables()) + for (auto const& variable_uuid : constraint.variables()) { variable_order.push_back(variable_uuid); } @@ -241,7 +241,7 @@ fuse_core::Transaction marginalizeVariables(const std::string& source, // Convert all remaining linear marginals into marginal constraints for (size_t i = marginalized_variables.size(); i < linear_terms.size(); ++i) { - for (const auto& linear_term : linear_terms[i]) + for (auto const& linear_term : linear_terms[i]) { auto marginal_constraint = detail::createMarginalConstraint(source, linear_term, graph, variable_order); transaction.addConstraint(std::move(marginal_constraint)); @@ -272,8 +272,8 @@ namespace detail * - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/residual_block.cc * - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/corrector.cc */ -LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::Graph& graph, - const UuidOrdering& elimination_order) +LinearTerm linearize(fuse_core::Constraint const& constraint, fuse_core::Graph const& graph, + UuidOrdering const& elimination_order) { LinearTerm result; @@ -285,17 +285,17 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G // * Generate a vector of variable value pointers. This is needed for the Ceres API. // * Allocate a matrix for each jacobian block. We will have Ceres populate the matrix. // * Generate a vector of jacobian pointers. This is needed for the Ceres API. - const auto& variable_uuids = constraint.variables(); + auto const& variable_uuids = constraint.variables(); const size_t variable_count = variable_uuids.size(); - std::vector variable_values; + std::vector variable_values; variable_values.reserve(variable_count); std::vector jacobians; jacobians.reserve(variable_count); result.variables.reserve(variable_count); result.A.reserve(variable_count); - for (const auto& variable_uuid : variable_uuids) + for (auto const& variable_uuid : variable_uuids) { - const auto& variable = graph.getVariable(variable_uuid); + auto const& variable = graph.getVariable(variable_uuid); variable_values.push_back(variable.data()); result.variables.push_back(elimination_order.at(variable_uuid)); result.A.emplace_back(row_count, variable.size()); @@ -307,7 +307,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G bool success = cost_function->Evaluate(variable_values.data(), result.b.data(), jacobians.data()); delete cost_function; success = success && result.b.array().isFinite().all(); - for (const auto& a : result.A) + for (auto const& a : result.A) { success = success && a.array().isFinite().all(); } @@ -327,8 +327,8 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G // zero. for (size_t index = 0ul; index < variable_count; ++index) { - const auto& variable_uuid = variable_uuids[index]; - const auto& variable = graph.getVariable(variable_uuid); + auto const& variable_uuid = variable_uuids[index]; + auto const& variable = graph.getVariable(variable_uuid); #if !CERES_SUPPORTS_MANIFOLDS auto local_parameterization = variable.localParameterization(); auto& jacobian = result.A[index]; @@ -386,7 +386,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G double alpha = 0.0; if ((squared_norm > 0.0) && (rho[2] > 0.0)) { - const double d = 1.0 + 2.0 * squared_norm * rho[2] / rho[1]; + double const d = 1.0 + 2.0 * squared_norm * rho[2] / rho[1]; alpha = 1.0 - std::sqrt(d); } @@ -412,7 +412,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G return result; } -LinearTerm marginalizeNext(const std::vector& linear_terms) +LinearTerm marginalizeNext(std::vector const& linear_terms) { if (linear_terms.empty()) { @@ -426,7 +426,7 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) // because the number of variables is assumed to be small. You need 1000s of variables before the // std::set outperforms the std::vector. auto dense_to_index = std::vector(); - for (const auto& linear_term : linear_terms) + for (auto const& linear_term : linear_terms) { std::copy(linear_term.variables.begin(), linear_term.variables.end(), std::back_inserter(dense_to_index)); } @@ -444,14 +444,14 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) auto row_offsets = std::vector(); row_offsets.reserve(linear_terms.size() + 1ul); row_offsets.push_back(0u); - for (const auto& linear_term : linear_terms) + for (auto const& linear_term : linear_terms) { row_offsets.push_back(row_offsets.back() + linear_term.b.rows()); } // Compute the column offsets auto index_to_cols = std::vector(dense_to_index.back() + 1u, 0u); - for (const auto& linear_term : linear_terms) + for (auto const& linear_term : linear_terms) { for (size_t i = 0ul; i < linear_term.variables.size(); ++i) { @@ -472,11 +472,11 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) fuse_core::MatrixXd ab = fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u); for (size_t term_index = 0ul; term_index < linear_terms.size(); ++term_index) { - const auto& linear_term = linear_terms[term_index]; + auto const& linear_term = linear_terms[term_index]; auto row_offset = row_offsets[term_index]; for (size_t i = 0ul; i < linear_term.variables.size(); ++i) { - const auto& a = linear_term.A[i]; + auto const& a = linear_term.A[i]; auto dense = index_to_dense[linear_term.variables[i]]; auto column_offset = column_offsets[dense]; for (int row = 0; row < a.rows(); ++row) @@ -487,7 +487,7 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) } } } - const auto& b = linear_term.b; + auto const& b = linear_term.b; int const column_offset = static_cast(column_offsets.back()); for (int row = 0; row < b.rows(); ++row) { @@ -552,11 +552,11 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) return marginal_term; } -MarginalConstraint::SharedPtr createMarginalConstraint(const std::string& source, const LinearTerm& linear_term, - const fuse_core::Graph& graph, - const UuidOrdering& elimination_order) +MarginalConstraint::SharedPtr createMarginalConstraint(std::string const& source, LinearTerm const& linear_term, + fuse_core::Graph const& graph, + UuidOrdering const& elimination_order) { - auto index_to_variable = [&graph, &elimination_order](const unsigned int index) -> const fuse_core::Variable& { + auto index_to_variable = [&graph, &elimination_order](unsigned int const index) -> fuse_core::Variable const& { return graph.getVariable(elimination_order.at(index)); }; diff --git a/fuse_constraints/src/normal_delta.cpp b/fuse_constraints/src/normal_delta.cpp index 469064043..c75a82efa 100644 --- a/fuse_constraints/src/normal_delta.cpp +++ b/fuse_constraints/src/normal_delta.cpp @@ -39,7 +39,7 @@ namespace fuse_constraints { -NormalDelta::NormalDelta(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b) : A_(A), b_(b) +NormalDelta::NormalDelta(fuse_core::MatrixXd const& A, fuse_core::VectorXd const& b) : A_(A), b_(b) { CHECK_GT(b_.rows(), 0); CHECK_GT(A_.rows(), 0); diff --git a/fuse_constraints/src/normal_delta_orientation_2d.cpp b/fuse_constraints/src/normal_delta_orientation_2d.cpp index b78960b1e..f1807ffde 100644 --- a/fuse_constraints/src/normal_delta_orientation_2d.cpp +++ b/fuse_constraints/src/normal_delta_orientation_2d.cpp @@ -37,7 +37,7 @@ namespace fuse_constraints { -NormalDeltaOrientation2D::NormalDeltaOrientation2D(const double A, const double b) : A_(A), b_(b) +NormalDeltaOrientation2D::NormalDeltaOrientation2D(double const A, double const b) : A_(A), b_(b) { } diff --git a/fuse_constraints/src/normal_delta_pose_2d.cpp b/fuse_constraints/src/normal_delta_pose_2d.cpp index 66a0712e7..c41e12e22 100644 --- a/fuse_constraints/src/normal_delta_pose_2d.cpp +++ b/fuse_constraints/src/normal_delta_pose_2d.cpp @@ -40,7 +40,7 @@ namespace fuse_constraints { -NormalDeltaPose2D::NormalDeltaPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) : A_(A), b_(b) +NormalDeltaPose2D::NormalDeltaPose2D(fuse_core::MatrixXd const& A, fuse_core::Vector3d const& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 3); diff --git a/fuse_constraints/src/normal_prior_orientation_2d.cpp b/fuse_constraints/src/normal_prior_orientation_2d.cpp index faff1ad24..a5d3bf2c0 100644 --- a/fuse_constraints/src/normal_prior_orientation_2d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_2d.cpp @@ -37,7 +37,7 @@ namespace fuse_constraints { -NormalPriorOrientation2D::NormalPriorOrientation2D(const double A, const double b) : A_(A), b_(b) +NormalPriorOrientation2D::NormalPriorOrientation2D(double const A, double const b) : A_(A), b_(b) { } diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp index c0537dc24..d7028cba6 100644 --- a/fuse_constraints/src/normal_prior_orientation_3d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -40,7 +40,7 @@ namespace fuse_constraints { -NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) +NormalPriorOrientation3D::NormalPriorOrientation3D(fuse_core::Matrix3d const& A, fuse_core::Vector4d const& b) : A_(A), b_(b) { } diff --git a/fuse_constraints/src/normal_prior_pose_2d.cpp b/fuse_constraints/src/normal_prior_pose_2d.cpp index 76de289f1..dcb0d9fee 100644 --- a/fuse_constraints/src/normal_prior_pose_2d.cpp +++ b/fuse_constraints/src/normal_prior_pose_2d.cpp @@ -40,7 +40,7 @@ namespace fuse_constraints { -NormalPriorPose2D::NormalPriorPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) : A_(A), b_(b) +NormalPriorPose2D::NormalPriorPose2D(fuse_core::MatrixXd const& A, fuse_core::Vector3d const& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 3); diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index 4fa0864a3..8afd6169b 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -40,7 +40,7 @@ namespace fuse_constraints { -NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) : A_(A), b_(b) +NormalPriorPose3D::NormalPriorPose3D(fuse_core::Matrix6d const& A, fuse_core::Vector7d const& b) : A_(A), b_(b) { } diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp index 098168027..09df53acd 100644 --- a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -42,7 +42,7 @@ namespace fuse_constraints { -NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b) +NormalPriorPose3DEuler::NormalPriorPose3DEuler(fuse_core::MatrixXd const& A, fuse_core::Vector6d const& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); diff --git a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp index 11891f3be..9be0c8e06 100644 --- a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp @@ -45,9 +45,9 @@ namespace fuse_constraints { RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector4d& delta, - const fuse_core::Matrix3d& covariance) + std::string const& source, fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Orientation3DStamped const& orientation2, fuse_core::Vector4d const& delta, + fuse_core::Matrix3d const& covariance) : fuse_core::Constraint(source, { orientation1.uuid(), orientation2.uuid() }) , // NOLINT delta_(delta) @@ -56,17 +56,17 @@ RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( } RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Orientation3DStamped& orientation2, const Eigen::Quaterniond& delta, - const fuse_core::Matrix3d& covariance) + std::string const& source, fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Orientation3DStamped const& orientation2, Eigen::Quaterniond const& delta, + fuse_core::Matrix3d const& covariance) : RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), covariance) { } RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::Orientation3DStamped& orientation2, const geometry_msgs::msg::Quaternion& delta, - const std::array& covariance) + std::string const& source, fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::Orientation3DStamped const& orientation2, geometry_msgs::msg::Quaternion const& delta, + std::array const& covariance) : RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), toEigen(covariance)) { } @@ -99,21 +99,21 @@ ceres::CostFunction* RelativeOrientation3DStampedConstraint::costFunction() cons new NormalDeltaOrientation3DCostFunctor(sqrt_information_, delta_)); } -fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const Eigen::Quaterniond& quaternion) +fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(Eigen::Quaterniond const& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z(); return eigen_quaternion_vector; } -fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const geometry_msgs::msg::Quaternion& quaternion) +fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(geometry_msgs::msg::Quaternion const& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w, quaternion.x, quaternion.y, quaternion.z; return eigen_quaternion_vector; } -fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen(const std::array& covariance) +fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen(std::array const& covariance) { return fuse_core::Matrix3d(covariance.data()); } diff --git a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp index dd968bee6..d8d38d0e5 100644 --- a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp @@ -45,11 +45,11 @@ namespace fuse_constraints { RelativePose2DStampedConstraint::RelativePose2DStampedConstraint( - const std::string& source, const fuse_variables::Position2DStamped& position1, - const fuse_variables::Orientation2DStamped& orientation1, const fuse_variables::Position2DStamped& position2, - const fuse_variables::Orientation2DStamped& orientation2, const fuse_core::VectorXd& partial_delta, - const fuse_core::MatrixXd& partial_covariance, const std::vector& linear_indices, - const std::vector& angular_indices) + std::string const& source, fuse_variables::Position2DStamped const& position1, + fuse_variables::Orientation2DStamped const& orientation1, fuse_variables::Position2DStamped const& position2, + fuse_variables::Orientation2DStamped const& orientation2, fuse_core::VectorXd const& partial_delta, + fuse_core::MatrixXd const& partial_covariance, std::vector const& linear_indices, + std::vector const& angular_indices) : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) // NOLINT { diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 2b6913fe7..1a8c611e6 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -44,10 +44,10 @@ namespace fuse_constraints { RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( - const std::string& source, const fuse_variables::Position3DStamped& position1, - const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, - const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector7d& delta, - const fuse_core::Matrix6d& covariance) + std::string const& source, fuse_variables::Position3DStamped const& position1, + fuse_variables::Orientation3DStamped const& orientation1, fuse_variables::Position3DStamped const& position2, + fuse_variables::Orientation3DStamped const& orientation2, fuse_core::Vector7d const& delta, + fuse_core::Matrix6d const& covariance) : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) , // NOLINT delta_(delta) diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp index 91de1542d..a0cf9acdb 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -44,10 +44,10 @@ namespace fuse_constraints { RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( - const std::string& source, const fuse_variables::Position3DStamped& position1, - const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, - const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector6d& delta, - const fuse_core::Matrix6d& covariance) + std::string const& source, fuse_variables::Position3DStamped const& position1, + fuse_variables::Orientation3DStamped const& orientation1, fuse_variables::Position3DStamped const& position2, + fuse_variables::Orientation3DStamped const& orientation2, fuse_core::Vector6d const& delta, + fuse_core::Matrix6d const& covariance) : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) , // NOLINT delta_(delta) @@ -56,10 +56,10 @@ RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( } RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( - const std::string& source, const fuse_variables::Position3DStamped& position1, - const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, - const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector6d& partial_delta, - const fuse_core::MatrixXd& partial_covariance, const std::vector& variable_indices) + std::string const& source, fuse_variables::Position3DStamped const& position1, + fuse_variables::Orientation3DStamped const& orientation1, fuse_variables::Position3DStamped const& position2, + fuse_variables::Orientation3DStamped const& orientation2, fuse_core::Vector6d const& partial_delta, + fuse_core::MatrixXd const& partial_covariance, std::vector const& variable_indices) : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) , // NOLINT delta_(partial_delta) diff --git a/fuse_constraints/src/uuid_ordering.cpp b/fuse_constraints/src/uuid_ordering.cpp index baa9d89c7..2c9455e2a 100644 --- a/fuse_constraints/src/uuid_ordering.cpp +++ b/fuse_constraints/src/uuid_ordering.cpp @@ -51,39 +51,39 @@ size_t UuidOrdering::size() const return order_.size(); } -bool UuidOrdering::exists(const unsigned int index) const +bool UuidOrdering::exists(unsigned int const index) const { return index < order_.size(); } -bool UuidOrdering::exists(const fuse_core::UUID& uuid) const +bool UuidOrdering::exists(fuse_core::UUID const& uuid) const { return order_.right.find(uuid) != order_.right.end(); } -bool UuidOrdering::push_back(const fuse_core::UUID& uuid) +bool UuidOrdering::push_back(fuse_core::UUID const& uuid) { auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); return result.second; } -const fuse_core::UUID& UuidOrdering::operator[](const unsigned int index) const +fuse_core::UUID const& UuidOrdering::operator[](unsigned int const index) const { return order_.left[index].second; } -unsigned int UuidOrdering::operator[](const fuse_core::UUID& uuid) +unsigned int UuidOrdering::operator[](fuse_core::UUID const& uuid) { auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); return (*result.first).get_left(); } -const fuse_core::UUID& UuidOrdering::at(const unsigned int index) const +fuse_core::UUID const& UuidOrdering::at(unsigned int const index) const { return order_.left.at(index).second; } -unsigned int UuidOrdering::at(const fuse_core::UUID& uuid) const +unsigned int UuidOrdering::at(fuse_core::UUID const& uuid) const { return order_.right.at(uuid); } diff --git a/fuse_constraints/src/variable_constraints.cpp b/fuse_constraints/src/variable_constraints.cpp index 4ee2fec67..f5c09b279 100644 --- a/fuse_constraints/src/variable_constraints.cpp +++ b/fuse_constraints/src/variable_constraints.cpp @@ -51,7 +51,7 @@ bool VariableConstraints::empty() const size_t VariableConstraints::size() const { - auto sum_edges = [](const size_t input, const ConstraintCollection& edges) { return input + edges.size(); }; + auto sum_edges = [](const size_t input, ConstraintCollection const& edges) { return input + edges.size(); }; return std::accumulate(variable_constraints_.begin(), variable_constraints_.end(), 0u, sum_edges); } @@ -60,7 +60,7 @@ unsigned int VariableConstraints::nextVariableIndex() const return variable_constraints_.size(); } -void VariableConstraints::insert(const unsigned int constraint, const unsigned int variable) +void VariableConstraints::insert(unsigned int const constraint, unsigned int const variable) { if (variable >= variable_constraints_.size()) { @@ -69,12 +69,12 @@ void VariableConstraints::insert(const unsigned int constraint, const unsigned i variable_constraints_[variable].insert(constraint); } -void VariableConstraints::insert(const unsigned int constraint, std::initializer_list variable_list) +void VariableConstraints::insert(unsigned int const constraint, std::initializer_list variable_list) { return insert(constraint, variable_list.begin(), variable_list.end()); } -void VariableConstraints::insert(const unsigned int variable) +void VariableConstraints::insert(unsigned int const variable) { if (variable >= variable_constraints_.size()) { @@ -89,7 +89,7 @@ void VariableConstraints::print(std::ostream& stream) const { stream << variable << ": ["; - for (const auto& constraint : variable_constraints_[variable]) + for (auto const& constraint : variable_constraints_[variable]) { stream << constraint << ", "; } @@ -98,7 +98,7 @@ void VariableConstraints::print(std::ostream& stream) const } } -std::ostream& operator<<(std::ostream& stream, const VariableConstraints& variable_constraints) +std::ostream& operator<<(std::ostream& stream, VariableConstraints const& variable_constraints) { variable_constraints.print(stream); return stream; diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index b5178c359..ea7b5037d 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -53,14 +53,14 @@ * @param[in] cost_function The expected cost function * @param[in] actual_cost_function The actual cost function */ -static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function, - const ceres::CostFunction& actual_cost_function) +static void ExpectCostFunctionsAreEqual(ceres::CostFunction const& cost_function, + ceres::CostFunction const& actual_cost_function) { constexpr double tol = 1e-12; EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); - const std::vector& parameter_block_sizes = cost_function.parameter_block_sizes(); - const std::vector& actual_parameter_block_sizes = actual_cost_function.parameter_block_sizes(); + std::vector const& parameter_block_sizes = cost_function.parameter_block_sizes(); + std::vector const& actual_parameter_block_sizes = actual_cost_function.parameter_block_sizes(); EXPECT_EQ(parameter_block_sizes.size(), actual_parameter_block_sizes.size()); size_t num_parameters = 0; diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp index 18583aab0..e454a1d00 100644 --- a/fuse_constraints/test/test_absolute_constraint.cpp +++ b/fuse_constraints/test/test_absolute_constraint.cpp @@ -215,7 +215,7 @@ TEST(AbsoluteConstraint, Optimization) EXPECT_NEAR(1.0, variable->x(), 1.0e-5); EXPECT_NEAR(2.0, variable->y(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -276,7 +276,7 @@ TEST(AbsoluteConstraint, Optimization) EXPECT_NEAR(2.0, var->y(), 1.0e-5); EXPECT_NEAR(3.5, var->z(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(var->data(), var->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -377,7 +377,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) // Check EXPECT_NEAR(7.0 - 2 * M_PI, variable->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp index 520ebb5f9..c15d693a9 100644 --- a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp @@ -123,7 +123,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) EXPECT_NEAR(1.0, orientation_variable->yaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -179,7 +179,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) EXPECT_NEAR(0.0, orientation_variable->yaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -306,7 +306,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) EXPECT_NEAR(-M_PI, orientation_variable->yaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp index dade370af..3e6c3597b 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp @@ -139,7 +139,7 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); diff --git a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp index c8092ce36..9b1296ed5 100644 --- a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp @@ -132,7 +132,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); @@ -223,7 +223,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp index e7d93e9cc..b45054f12 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -196,7 +196,7 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index 43b92de91..1bd8fbbe2 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -271,7 +271,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); @@ -374,7 +374,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); diff --git a/fuse_constraints/test/test_marginal_constraint.cpp b/fuse_constraints/test/test_marginal_constraint.cpp index 58409d5f0..4191d0c5c 100644 --- a/fuse_constraints/test/test_marginal_constraint.cpp +++ b/fuse_constraints/test/test_marginal_constraint.cpp @@ -74,7 +74,7 @@ TEST(MarginalConstraint, OneVariable) x1.y() = 6.0; // Compute the actual residuals and jacobians - std::vector variable_values = { x1.data() }; + std::vector variable_values = { x1.data() }; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 2); std::vector actual_jacobians = { actual_jacobian1.data() }; @@ -131,7 +131,7 @@ TEST(MarginalConstraint, TwoVariables) x2.y() = 18.0; // Compute the actual residuals and jacobians - std::vector variable_values = { x1.data(), x2.data() }; + std::vector variable_values = { x1.data(), x2.data() }; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 2); fuse_core::MatrixXd actual_jacobian2(1, 2); @@ -186,7 +186,7 @@ TEST(MarginalConstraint, LocalParameterization) x1.z() = 0.526043; // Compute the actual residuals and jacobians - std::vector variable_values = { x1.data() }; + std::vector variable_values = { x1.data() }; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 4); std::vector actual_jacobians = { actual_jacobian1.data() }; diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index 369b906a1..7738d18d2 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -67,7 +67,7 @@ class GenericVariable : public fuse_core::Variable { } - explicit GenericVariable(const fuse_core::UUID& uuid) : fuse_core::Variable(uuid), data_{} + explicit GenericVariable(fuse_core::UUID const& uuid) : fuse_core::Variable(uuid), data_{} { } @@ -76,7 +76,7 @@ class GenericVariable : public fuse_core::Variable return 1; } - [[nodiscard]] const double* data() const override + [[nodiscard]] double const* data() const override { return &data_; } @@ -104,7 +104,7 @@ class GenericVariable : public fuse_core::Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data_; @@ -125,11 +125,11 @@ class GenericConstraint : public fuse_core::Constraint { } - explicit GenericConstraint(const fuse_core::UUID& variable1) : fuse_core::Constraint("test", { variable1 }) + explicit GenericConstraint(fuse_core::UUID const& variable1) : fuse_core::Constraint("test", { variable1 }) { } - GenericConstraint(const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + GenericConstraint(fuse_core::UUID const& variable1, fuse_core::UUID const& variable2) : fuse_core::Constraint("test", { variable1, variable2 }) { } @@ -155,7 +155,7 @@ class GenericConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } @@ -168,7 +168,7 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped FixedOrientation3DStamped() = default; - explicit FixedOrientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + explicit FixedOrientation3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL) : Orientation3DStamped(stamp, device_id) { } @@ -190,7 +190,7 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } @@ -304,7 +304,7 @@ TEST(MarginalizeVariables, ComputeEliminationOrderWithOrphanVariables) // Check all marginalized variables are in the elimination order // This check is equivalent to the assert in marginalizeVariables - for (const auto& variable_uuid : to_be_marginalized) + for (auto const& variable_uuid : to_be_marginalized) { SCOPED_TRACE(fuse_core::uuid::to_string(variable_uuid)); @@ -524,9 +524,9 @@ TEST(MarginalizeVariables, MarginalizeVariables) { l1->uuid(), l1->uuid() } }; auto expected_covariances = std::vector>(); graph.getCovariance(requests, expected_covariances); - const auto& expected_x2_cov = expected_covariances[0]; - const auto& expected_x3_cov = expected_covariances[1]; - const auto& expected_l1_cov = expected_covariances[2]; + auto const& expected_x2_cov = expected_covariances[0]; + auto const& expected_x3_cov = expected_covariances[1]; + auto const& expected_l1_cov = expected_covariances[2]; // Marginalize out X1 auto transaction = fuse_constraints::marginalizeVariables("test", { x1->uuid() }, graph); // NOLINT @@ -563,9 +563,9 @@ TEST(MarginalizeVariables, MarginalizeVariables) auto actual_covariances = std::vector>(); graph.getCovariance(requests, actual_covariances); - const auto& actual_x2_cov = actual_covariances[0]; - const auto& actual_x3_cov = actual_covariances[1]; - const auto& actual_l1_cov = actual_covariances[2]; + auto const& actual_x2_cov = actual_covariances[0]; + auto const& actual_x3_cov = actual_covariances[1]; + auto const& actual_l1_cov = actual_covariances[2]; // Compare. The post-marginal results should be identical to the pre-marginal results ASSERT_EQ(expected_x2.size(), actual_x2.size()); @@ -687,8 +687,8 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) { x3->uuid(), x3->uuid() } }; auto expected_covariances = std::vector>(); graph.getCovariance(requests, expected_covariances); - const auto& expected_x2_cov = expected_covariances[0]; - const auto& expected_x3_cov = expected_covariances[1]; + auto const& expected_x2_cov = expected_covariances[0]; + auto const& expected_x3_cov = expected_covariances[1]; // Marginalize out X1 and L1 auto transaction = fuse_constraints::marginalizeVariables("test", { x1->uuid(), l1->uuid() }, graph); // NOLINT @@ -728,8 +728,8 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) auto actual_covariances = std::vector>(); graph.getCovariance(requests, actual_covariances); - const auto& actual_x2_cov = actual_covariances[0]; - const auto& actual_x3_cov = actual_covariances[1]; + auto const& actual_x2_cov = actual_covariances[0]; + auto const& actual_x3_cov = actual_covariances[1]; // Compare. The post-marginal results should be identical to the pre-marginal results ASSERT_EQ(expected_x2.size(), actual_x2.size()); diff --git a/fuse_constraints/test/test_normal_delta_pose_2d.cpp b/fuse_constraints/test/test_normal_delta_pose_2d.cpp index 45f0f5732..d16dea40e 100644 --- a/fuse_constraints/test/test_normal_delta_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_delta_pose_2d.cpp @@ -76,7 +76,7 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor const fuse_constraints::NormalDeltaPose2D cost_function{ full_sqrt_information, full_delta }; // Create automatic differentiation cost function - const auto num_residuals = full_sqrt_information.rows(); + auto const num_residuals = full_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( new fuse_constraints::NormalDeltaPose2DCostFunctor(full_sqrt_information, full_delta), num_residuals); @@ -91,7 +91,7 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // rows using IndicesPair = std::array; std::array indices_pairs = { IndicesPair{ 0, 1 }, IndicesPair{ 0, 2 }, IndicesPair{ 1, 2 } }; - for (const auto& indices_pair : indices_pairs) + for (auto const& indices_pair : indices_pairs) { // It is a shame we need Eigen 3.4+ in order to use the slicing and indexing API documented in: // @@ -110,7 +110,7 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor const fuse_constraints::NormalDeltaPose2D cost_function{ partial_sqrt_information, full_delta }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), num_residuals); @@ -131,7 +131,7 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor const fuse_constraints::NormalDeltaPose2D cost_function{ partial_sqrt_information, full_delta }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), num_residuals); diff --git a/fuse_constraints/test/test_normal_prior_pose_2d.cpp b/fuse_constraints/test/test_normal_prior_pose_2d.cpp index 605c228b5..fa1185e90 100644 --- a/fuse_constraints/test/test_normal_prior_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_2d.cpp @@ -76,7 +76,7 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor const fuse_constraints::NormalPriorPose2D cost_function{ full_sqrt_information, full_mean }; // Create automatic differentiation cost function - const auto num_residuals = full_sqrt_information.rows(); + auto const num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( new fuse_constraints::NormalPriorPose2DCostFunctor(full_sqrt_information, full_mean), num_residuals); @@ -91,7 +91,7 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // rows using IndicesPair = std::array; std::array indices_pairs = { IndicesPair{ 0, 1 }, IndicesPair{ 0, 2 }, IndicesPair{ 1, 2 } }; - for (const auto& indices_pair : indices_pairs) + for (auto const& indices_pair : indices_pairs) { // It is a shame we need Eigen 3.4+ in order to use the slicing and indexing API documented in: // @@ -110,7 +110,7 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor const fuse_constraints::NormalPriorPose2D cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), num_residuals); @@ -131,7 +131,7 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor const fuse_constraints::NormalPriorPose2D cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), num_residuals); diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index 57a4d18d0..3b3921268 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -74,7 +74,7 @@ TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) auto q = Eigen::Quaterniond::UnitRandom(); full_mean << 1.0, 2.0, 1.0, q.w(), q.x(), q.y(), q.z(); // Create automatic differentiation cost function const fuse_constraints::NormalPriorPose3D cost_function{ full_sqrt_information, full_mean }; - const auto num_residuals = full_sqrt_information.rows(); + auto const num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose3D autodiff_cost_function( new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), num_residuals); diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp index 59150f134..71926c9de 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -77,7 +77,7 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu auto rpy = Eigen::Vector3d::Random(); full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); const fuse_constraints::NormalPriorPose3DEuler cost_function{ full_sqrt_information, full_mean }; - const auto num_residuals = full_sqrt_information.rows(); + auto const num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), num_residuals); @@ -107,7 +107,7 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); @@ -134,7 +134,7 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); @@ -159,7 +159,7 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); @@ -185,7 +185,7 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function - const auto num_residuals = partial_sqrt_information.rows(); + auto const num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp index 40b08c4ac..3dd11b1be 100644 --- a/fuse_constraints/test/test_relative_constraint.cpp +++ b/fuse_constraints/test/test_relative_constraint.cpp @@ -244,7 +244,7 @@ TEST(RelativeConstraint, Optimization) EXPECT_NEAR(1.1, x2->x(), 1.0e-5); EXPECT_NEAR(2.2, x2->y(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -341,7 +341,7 @@ TEST(RelativeConstraint, Optimization) EXPECT_NEAR(2.2, x2->y(), 1.0e-5); EXPECT_NEAR(3.15, x2->z(), 1.0e-5); // Compute the marginal covariances - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -418,7 +418,7 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) EXPECT_NEAR(1.0, x1->yaw(), 1.0e-5); EXPECT_NEAR(1.1, x2->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; diff --git a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp index 91b2a85d2..b6015cce9 100644 --- a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp @@ -172,7 +172,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) EXPECT_NEAR(0.0, orientation2->yaw(), 1.0e-5); // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); @@ -201,7 +201,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) } // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -332,7 +332,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); @@ -364,7 +364,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) } // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp index 0ddee00cc..15ec16803 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp @@ -237,7 +237,7 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); @@ -271,7 +271,7 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp index 283b7535a..9e92ab40a 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -316,7 +316,7 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); @@ -350,7 +350,7 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -502,7 +502,7 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); @@ -537,7 +537,7 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); diff --git a/fuse_core/include/fuse_core/async_motion_model.hpp b/fuse_core/include/fuse_core/async_motion_model.hpp index ac012f0ed..7a196ee52 100644 --- a/fuse_core/include/fuse_core/async_motion_model.hpp +++ b/fuse_core/include/fuse_core/async_motion_model.hpp @@ -147,12 +147,12 @@ class AsyncMotionModel : public MotionModel * @throws runtime_error if already initialized */ void initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Get the unique name of this motion model */ - const std::string& name() const override + std::string const& name() const override { return name_; } diff --git a/fuse_core/include/fuse_core/async_publisher.hpp b/fuse_core/include/fuse_core/async_publisher.hpp index ff5c0bc05..d7d55b769 100644 --- a/fuse_core/include/fuse_core/async_publisher.hpp +++ b/fuse_core/include/fuse_core/async_publisher.hpp @@ -89,12 +89,12 @@ class AsyncPublisher : public Publisher * @throws runtime_error if already initialized */ void initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Get the unique name of this publisher */ - const std::string& name() const override + std::string const& name() const override { return name_; } diff --git a/fuse_core/include/fuse_core/async_sensor_model.hpp b/fuse_core/include/fuse_core/async_sensor_model.hpp index c83b7c6d4..3b9c0e000 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.hpp +++ b/fuse_core/include/fuse_core/async_sensor_model.hpp @@ -130,7 +130,7 @@ class AsyncSensorModel : public SensorModel * @param[in] transaction_callback The function to call every time a transaction is published * @throws runtime_error if already initialized */ - void initialize(node_interfaces::NodeInterfaces interfaces, const std::string& name, + void initialize(node_interfaces::NodeInterfaces interfaces, std::string const& name, TransactionCallback transaction_callback) override; /** @@ -153,7 +153,7 @@ class AsyncSensorModel : public SensorModel /** * @brief Get the unique name of this sensor */ - const std::string& name() const override + std::string const& name() const override { return name_; } diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp b/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp index e532c8097..b93a802e1 100644 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp +++ b/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp @@ -97,7 +97,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] x_plus_delta The final variable value, of size \p GlobalSize() * @return True if successful, false otherwise */ - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override; + bool Plus(double const* x, double const* delta, double* x_plus_delta) const override; /** * @brief The Jacobian of Plus(x, delta) w.r.t delta at delta = 0, computed using automatic @@ -107,7 +107,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] jacobian The Jacobian in row-major order, of size \p GlobalSize() x \p LocalSize() * @return True is successful, false otherwise */ - bool ComputeJacobian(const double* x, double* jacobian) const override; + bool ComputeJacobian(double const* x, double* jacobian) const override; /** * @brief Generalization of the subtraction operation, implemented by the provided MinusFunctor @@ -118,7 +118,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * LocalSize() * @return True if successful, false otherwise */ - bool Minus(const double* x1, const double* x2, double* delta) const override; + bool Minus(double const* x1, double const* x2, double* delta) const override; /** * @brief The Jacobian of Minus(x1, x2) w.r.t x2 evaluated at x1 = x2 = x, computed using @@ -127,7 +127,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] jacobian The Jacobian in row-major order, of size \p LocalSize() x \p GlobalSize() * @return True is successful, false otherwise */ - bool ComputeMinusJacobian(const double* x, double* jacobian) const override; + bool ComputeMinusJacobian(double const* x, double* jacobian) const override; /** * @brief The size of the variable parameterization in the nonlinear manifold @@ -164,8 +164,8 @@ AutoDiffLocalParameterization -bool AutoDiffLocalParameterization::Plus(const double* x, - const double* delta, +bool AutoDiffLocalParameterization::Plus(double const* x, + double const* delta, double* x_plus_delta) const { return (*plus_functor_)(x, delta, x_plus_delta); @@ -173,12 +173,12 @@ bool AutoDiffLocalParameterization bool AutoDiffLocalParameterization::ComputeJacobian( - const double* x, double* jacobian) const + double const* x, double* jacobian) const { double zero_delta[kLocalSize] = {}; // zero-initialize double x_plus_delta[kGlobalSize]; - const double* parameter_ptrs[2] = { x, zero_delta }; + double const* parameter_ptrs[2] = { x, zero_delta }; double* jacobian_ptrs[2] = { NULL, jacobian }; #if !CERES_VERSION_AT_LEAST(2, 0, 0) return ceres::internal::AutoDiff::Differentiate( @@ -190,8 +190,8 @@ bool AutoDiffLocalParameterization -bool AutoDiffLocalParameterization::Minus(const double* x1, - const double* x2, +bool AutoDiffLocalParameterization::Minus(double const* x1, + double const* x2, double* delta) const { return (*minus_functor_)(x1, x2, delta); @@ -199,11 +199,11 @@ bool AutoDiffLocalParameterization bool AutoDiffLocalParameterization::ComputeMinusJacobian( - const double* x, double* jacobian) const + double const* x, double* jacobian) const { double delta[kLocalSize] = {}; // zero-initialize - const double* parameter_ptrs[2] = { x, x }; + double const* parameter_ptrs[2] = { x, x }; double* jacobian_ptrs[2] = { NULL, jacobian }; #if !CERES_VERSION_AT_LEAST(2, 0, 0) return ceres::internal::AutoDiff::Differentiate( diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index e5ffb65d6..e20c53199 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -183,7 +183,7 @@ class CallbackAdapter : public rclcpp::Waitable void execute(std::shared_ptr& data) override; - void addCallback(const std::shared_ptr& callback); + void addCallback(std::shared_ptr const& callback); void addCallback(std::shared_ptr&& callback); diff --git a/fuse_core/include/fuse_core/ceres_options.hpp b/fuse_core/include/fuse_core/ceres_options.hpp index 58022f7a3..64e29fb74 100644 --- a/fuse_core/include/fuse_core/ceres_options.hpp +++ b/fuse_core/include/fuse_core/ceres_options.hpp @@ -54,7 +54,7 @@ * For a given Ceres Solver Option , the function ToString calls ceres::ToString */ #define CERES_OPTION_TO_STRING_DEFINITION(Option) \ - static inline const char* ToString(ceres::Option value) \ + static inline char const* ToString(ceres::Option value) \ { \ return ceres::Option##ToString(value); \ } @@ -103,7 +103,7 @@ static void UpperCase(std::string* input) std::transform(input->begin(), input->end(), input->begin(), ::toupper); } -inline const char* LoggingTypeToString(LoggingType type) +inline char const* LoggingTypeToString(LoggingType type) { switch (type) { @@ -122,7 +122,7 @@ inline bool StringToLoggingType(std::string value, LoggingType* type) return false; } -inline const char* DumpFormatTypeToString(DumpFormatType type) +inline char const* DumpFormatTypeToString(DumpFormatType type) { switch (type) { @@ -201,8 +201,8 @@ template T declareCeresParam( node_interfaces::NodeInterfaces interfaces, - const std::string& parameter_name, const T& default_value, - const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor()) + std::string const& parameter_name, const T& default_value, + rcl_interfaces::msg::ParameterDescriptor const& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor()) { const std::string default_string_value{ ToString(default_value) }; @@ -233,7 +233,7 @@ T declareCeresParam( void loadCovarianceOptionsFromROS( node_interfaces::NodeInterfaces interfaces, - ceres::Covariance::Options& covariance_options, const std::string& ns = std::string()); + ceres::Covariance::Options& covariance_options, std::string const& ns = std::string()); /** * @brief Populate a ceres::Problem::Options object with information from the parameter server @@ -244,7 +244,7 @@ void loadCovarianceOptionsFromROS( * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, - ceres::Problem::Options& problem_options, const std::string& ns = std::string()); + ceres::Problem::Options& problem_options, std::string const& ns = std::string()); /** * @brief Populate a ceres::Solver::Options object with information from the parameter server @@ -257,7 +257,7 @@ void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, - ceres::Solver::Options& solver_options, const std::string& ns = std::string()); + ceres::Solver::Options& solver_options, std::string const& ns = std::string()); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/console.hpp b/fuse_core/include/fuse_core/console.hpp index 9adae01f5..e135944a2 100644 --- a/fuse_core/include/fuse_core/console.hpp +++ b/fuse_core/include/fuse_core/console.hpp @@ -62,7 +62,7 @@ class DelayedThrottleFilter * * @param[in] The throttle period in seconds */ - explicit DelayedThrottleFilter(const double period) : period_(std::chrono::duration>(period)) + explicit DelayedThrottleFilter(double const period) : period_(std::chrono::duration>(period)) { reset(); } @@ -81,7 +81,7 @@ class DelayedThrottleFilter */ bool isEnabled() { - const auto now = std::chrono::time_point_cast(std::chrono::system_clock::now()); + auto const now = std::chrono::time_point_cast(std::chrono::system_clock::now()); if (last_hit_.time_since_epoch().count() < 0.0) { diff --git a/fuse_core/include/fuse_core/constraint.hpp b/fuse_core/include/fuse_core/constraint.hpp index 2e18ee7f0..90f0287ed 100644 --- a/fuse_core/include/fuse_core/constraint.hpp +++ b/fuse_core/include/fuse_core/constraint.hpp @@ -212,7 +212,7 @@ class Constraint * * @param[in] variable_uuid_list The list of involved variable UUIDs */ - Constraint(const std::string& source, std::initializer_list variable_uuid_list); + Constraint(std::string const& source, std::initializer_list variable_uuid_list); /** * @brief Constructor @@ -220,7 +220,7 @@ class Constraint * Accepts an arbitrary number of variable UUIDs stored in a container using iterators. */ template - Constraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last); + Constraint(std::string const& source, VariableUuidIterator first, VariableUuidIterator last); /** * @brief Destructor @@ -248,7 +248,7 @@ class Constraint /** * @brief Returns the name of the sensor or motion model that generated this constraint */ - const std::string& source() const + std::string const& source() const { return source_; } @@ -321,7 +321,7 @@ class Constraint /** * @brief Read-only access to the ordered list of variable UUIDs involved in this constraint */ - const std::vector& variables() const + std::vector const& variables() const { return variables_; } @@ -396,7 +396,7 @@ class Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& source_; archive& uuid_; @@ -408,10 +408,10 @@ class Constraint /** * Stream operator implementation used for all derived Constraint classes. */ -std::ostream& operator<<(std::ostream& stream, const Constraint& constraint); +std::ostream& operator<<(std::ostream& stream, Constraint const& constraint); template -Constraint::Constraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) +Constraint::Constraint(std::string const& source, VariableUuidIterator first, VariableUuidIterator last) : source_(source), uuid_(uuid::generate()), variables_(first, last) { } diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index be9d74403..f5728bbed 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -83,7 +83,7 @@ using Matrix = Eigen::Matrix -std::string to_string(const Eigen::DenseBase& m, const int precision = 4) +std::string to_string(Eigen::DenseBase const& m, int const precision = 4) { static const Eigen::IOFormat pretty(precision, 0, ", ", "\n", "[", "]"); @@ -101,7 +101,7 @@ std::string to_string(const Eigen::DenseBase& m, const int precision = * @return True if the matrix m is symmetric; False, otherwise. */ template -bool isSymmetric(const Eigen::DenseBase& m, +bool isSymmetric(Eigen::DenseBase const& m, const typename Eigen::DenseBase::RealScalar precision = Eigen::NumTraits::Scalar>::dummy_precision()) { @@ -113,7 +113,7 @@ bool isSymmetric(const Eigen::DenseBase& m, // // See: // https://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#ae8443357b808cd393be1b51974213f9c - const auto& derived = m.derived(); + auto const& derived = m.derived(); return (derived - derived.transpose()).cwiseAbs().maxCoeff() < precision; } @@ -124,7 +124,7 @@ bool isSymmetric(const Eigen::DenseBase& m, * @return True if the matrix m is PD; False, otherwise. */ template -bool isPositiveDefinite(const Eigen::DenseBase& m) +bool isPositiveDefinite(Eigen::DenseBase const& m) { Eigen::SelfAdjointEigenSolver solver(m); return solver.eigenvalues().minCoeff() > 0.0; diff --git a/fuse_core/include/fuse_core/eigen_gtest.hpp b/fuse_core/include/fuse_core/eigen_gtest.hpp index c29a3a767..e931a6f7e 100644 --- a/fuse_core/include/fuse_core/eigen_gtest.hpp +++ b/fuse_core/include/fuse_core/eigen_gtest.hpp @@ -59,8 +59,8 @@ namespace testing * @return AssertionSuccess or AssertionFailure */ template -AssertionResult AssertMatrixEqualHelper(const char* e1, const char* e2, const Eigen::MatrixBase& v1, - const Eigen::MatrixBase& v2) +AssertionResult AssertMatrixEqualHelper(char const* e1, char const* e2, Eigen::MatrixBase const& v1, + Eigen::MatrixBase const& v2) { if (v1 == v2) { @@ -89,8 +89,8 @@ AssertionResult AssertMatrixEqualHelper(const char* e1, const char* e2, const Ei * @return AssertionSuccess or AssertionFailure */ template -AssertionResult AssertMatrixNearHelper(const char* e1, const char* e2, const Eigen::MatrixBase& v1, - const Eigen::MatrixBase& v2, double tol) +AssertionResult AssertMatrixNearHelper(char const* e1, char const* e2, Eigen::MatrixBase const& v1, + Eigen::MatrixBase const& v2, double tol) { if ((v1 - v2).cwiseAbs().maxCoeff() < tol) { diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index d4ad20815..c7444945f 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -239,7 +239,7 @@ class Graph * @param[in] constraint_uuid The UUID of the requested constraint * @return The constraint in the graph with the specified UUID */ - virtual const Constraint& getConstraint(const UUID& constraint_uuid) const = 0; + virtual Constraint const& getConstraint(const UUID& constraint_uuid) const = 0; /** * @brief Read-only access to all of the constraints in the graph @@ -293,7 +293,7 @@ class Graph * @param[in] variable_uuid The UUID of the requested variable * @return The variable in the graph with the specified UUID */ - virtual const Variable& getVariable(const UUID& variable_uuid) const = 0; + virtual Variable const& getVariable(const UUID& variable_uuid) const = 0; /** * @brief Read-only access to all of the variables in the graph @@ -352,9 +352,9 @@ class Graph * variable's tangent space/local coordinates. Otherwise it is * computed in the variable's parameter space. */ - virtual void getCovariance(const std::vector>& covariance_requests, + virtual void getCovariance(std::vector> const& covariance_requests, std::vector>& covariance_matrices, - const ceres::Covariance::Options& options = ceres::Covariance::Options(), + ceres::Covariance::Options const& options = ceres::Covariance::Options(), bool use_tangent_space = true) const = 0; /** @@ -362,7 +362,7 @@ class Graph * * @param[in] transaction A set of variable and constraints additions and deletions */ - void update(const Transaction& transaction); + void update(Transaction const& transaction); /** * @brief Optimize the values of the current set of variables, given the current set of @@ -376,7 +376,7 @@ class Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - virtual ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) = 0; + virtual ceres::Solver::Summary optimize(ceres::Solver::Options const& options = ceres::Solver::Options()) = 0; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -394,8 +394,8 @@ class Graph * optimization process */ virtual ceres::Solver::Summary - optimizeFor(const rclcpp::Duration& max_optimization_time, - const ceres::Solver::Options& options = ceres::Solver::Options(), + optimizeFor(rclcpp::Duration const& max_optimization_time, + ceres::Solver::Options const& options = ceres::Solver::Options(), rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) = 0; // NOTE(CH3): We need to copy // because clock.now() is non-const @@ -423,7 +423,7 @@ class Graph * @return True if the problem evaluation was successful; False, otherwise. */ virtual bool evaluate(double* cost, std::vector* residuals = nullptr, std::vector* gradient = nullptr, - const ceres::Problem::EvaluateOptions& options = ceres::Problem::EvaluateOptions()) const = 0; + ceres::Problem::EvaluateOptions const& options = ceres::Problem::EvaluateOptions()) const = 0; /** * @brief Structure containing the cost and residual information for a single constraint. @@ -522,7 +522,7 @@ class Graph * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& /* archive */, const unsigned int /* version */) + void serialize(Archive& /* archive */, unsigned int const /* version */) { } }; @@ -530,7 +530,7 @@ class Graph /** * Stream operator for printing Graph objects. */ -std::ostream& operator<<(std::ostream& stream, const Graph& graph); +std::ostream& operator<<(std::ostream& stream, Graph const& graph); template void Graph::getConstraintCosts(UuidForwardIterator first, UuidForwardIterator last, OutputIterator output) @@ -542,13 +542,13 @@ void Graph::getConstraintCosts(UuidForwardIterator first, UuidForwardIterator la while (first != last) { // Get the next requested constraint - const auto& constraint = getConstraint(*first); + auto const& constraint = getConstraint(*first); // Collect all of the involved variables - auto parameter_blocks = std::vector(); + auto parameter_blocks = std::vector(); parameter_blocks.reserve(constraint.variables().size()); for (auto variable_uuid : constraint.variables()) { - const auto& variable = getVariable(variable_uuid); + auto const& variable = getVariable(variable_uuid); parameter_blocks.push_back(variable.data()); } // Compute the residuals for this constraint using the cost function diff --git a/fuse_core/include/fuse_core/graph_deserializer.hpp b/fuse_core/include/fuse_core/graph_deserializer.hpp index 44f61177d..c0a7bd2d3 100644 --- a/fuse_core/include/fuse_core/graph_deserializer.hpp +++ b/fuse_core/include/fuse_core/graph_deserializer.hpp @@ -46,7 +46,7 @@ namespace fuse_core /** * @brief Serialize a graph into a message */ -void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg); +void serializeGraph(fuse_core::Graph const& graph, fuse_msgs::msg::SerializedGraph& msg); /** * @brief Deserialize a graph @@ -87,7 +87,7 @@ class GraphDeserializer * @param[in] msg The SerializedGraph message to be deserialized * @return A unique_ptr to a derived Graph object */ - fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph& msg) const; + fuse_core::Graph::UniquePtr deserialize(fuse_msgs::msg::SerializedGraph const& msg) const; private: //! Pluginlib class loader for Variable types diff --git a/fuse_core/include/fuse_core/local_parameterization.hpp b/fuse_core/include/fuse_core/local_parameterization.hpp index 9df7a40dc..b295253e2 100644 --- a/fuse_core/include/fuse_core/local_parameterization.hpp +++ b/fuse_core/include/fuse_core/local_parameterization.hpp @@ -100,7 +100,7 @@ class LocalParameterization * @param[in] delta variable of size \p LocalSize() * @param[out] x_plus_delta of size \p GlobalSize() */ - virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const = 0; + virtual bool Plus(double const* x, double const* delta, double* x_plus_delta) const = 0; /** * @brief The jacobian of Plus(x, delta) w.r.t delta at delta = 0. @@ -109,7 +109,7 @@ class LocalParameterization * @param[out] jacobian a row-major GlobalSize() x LocalSize() matrix. * @return */ - virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; + virtual bool ComputeJacobian(double const* x, double* jacobian) const = 0; /** * @brief Generalization of the subtraction operation @@ -126,7 +126,7 @@ class LocalParameterization * \p LocalSize() * @return True if successful, false otherwise */ - virtual bool Minus(const double* x, const double* y, double* y_minus_x) const = 0; + virtual bool Minus(double const* x, double const* y, double* y_minus_x) const = 0; /** * @brief The jacobian of Minus(x, y) w.r.t y at x == y @@ -136,7 +136,7 @@ class LocalParameterization * GlobalSize() * @return True if successful, false otherwise */ - virtual bool ComputeMinusJacobian(const double* x, double* jacobian) const = 0; + virtual bool ComputeMinusJacobian(double const* x, double* jacobian) const = 0; #if CERES_SUPPORTS_MANIFOLDS // If the fuse::LocalParameterization class does not inherit from the @@ -156,7 +156,7 @@ class LocalParameterization * @param[in] global_matrix is a num_rows x GlobalSize row major matrix. * @param[out] local_matrix is a num_rows x LocalSize row major matrix. */ - virtual bool MultiplyByJacobian(const double* x, const int num_rows, const double* global_matrix, + virtual bool MultiplyByJacobian(double const* x, int const num_rows, double const* global_matrix, double* local_matrix) const { using Matrix = Eigen::Matrix; @@ -191,7 +191,7 @@ class LocalParameterization * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& /* archive */, const unsigned int /* version */) + void serialize(Archive& /* archive */, unsigned int const /* version */) { } }; diff --git a/fuse_core/include/fuse_core/loss.hpp b/fuse_core/include/fuse_core/loss.hpp index b20cbda75..251054bbb 100644 --- a/fuse_core/include/fuse_core/loss.hpp +++ b/fuse_core/include/fuse_core/loss.hpp @@ -199,7 +199,7 @@ class Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) = 0; + std::string const& name) = 0; /** * @brief Returns a unique name for this loss function type. @@ -301,7 +301,7 @@ class Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& /* archive */, const unsigned int /* version */) + void serialize(Archive& /* archive */, unsigned int const /* version */) { } }; @@ -309,7 +309,7 @@ class Loss /** * Stream operator implementation used for all derived Loss classes. */ -std::ostream& operator<<(std::ostream& stream, const Loss& loss); +std::ostream& operator<<(std::ostream& stream, Loss const& loss); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss_loader.hpp b/fuse_core/include/fuse_core/loss_loader.hpp index 5fd4242b5..c001065a8 100644 --- a/fuse_core/include/fuse_core/loss_loader.hpp +++ b/fuse_core/include/fuse_core/loss_loader.hpp @@ -65,9 +65,9 @@ class LossLoader } // Delete copy and move constructors and assign operators - LossLoader(const LossLoader&) = delete; + LossLoader(LossLoader const&) = delete; LossLoader(LossLoader&&) = delete; - LossLoader& operator=(const LossLoader&) = delete; + LossLoader& operator=(LossLoader const&) = delete; LossLoader& operator=(LossLoader&&) = delete; /** @@ -76,7 +76,7 @@ class LossLoader * @param[in] lookup_name Loss function lookup name * @return Loss function instance handled by an std::unique_ptr<> */ - pluginlib::UniquePtr createUniqueInstance(const std::string& lookup_name) + pluginlib::UniquePtr createUniqueInstance(std::string const& lookup_name) { return loss_loader_.createUniqueInstance(lookup_name); } @@ -99,7 +99,7 @@ class LossLoader * @param[in] lookup_name Loss function lookup name * @return Loss function instance handled by an std::unique_ptr<> */ -inline pluginlib::UniquePtr createUniqueLoss(const std::string& lookup_name) +inline pluginlib::UniquePtr createUniqueLoss(std::string const& lookup_name) { return LossLoader::getInstance().createUniqueInstance(lookup_name); } diff --git a/fuse_core/include/fuse_core/manifold.hpp b/fuse_core/include/fuse_core/manifold.hpp index 7173a6cdc..98f94c033 100644 --- a/fuse_core/include/fuse_core/manifold.hpp +++ b/fuse_core/include/fuse_core/manifold.hpp @@ -112,7 +112,7 @@ class Manifold : public ceres::Manifold * @param[out] x_plus_delta is a \p AmbientSize() vector. * @return Return value indicates if the operation was successful or not. */ - virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const = 0; + virtual bool Plus(double const* x, double const* delta, double* x_plus_delta) const = 0; /** * @brief Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, @@ -125,7 +125,7 @@ class Manifold : public ceres::Manifold * matrix. * @return */ - virtual bool PlusJacobian(const double* x, double* jacobian) const = 0; + virtual bool PlusJacobian(double const* x, double* jacobian) const = 0; /** * @brief Generalization of the subtraction operation @@ -141,7 +141,7 @@ class Manifold : public ceres::Manifold * @param[out] y_minus_x is a \p TangentSize() vector. * @return Return value indicates if the operation was successful or not. */ - virtual bool Minus(const double* y, const double* x, double* y_minus_x) const = 0; + virtual bool Minus(double const* y, double const* x, double* y_minus_x) const = 0; /** * @brief Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e @@ -152,7 +152,7 @@ class Manifold : public ceres::Manifold * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. * @return Return value indicates whether the operation was successful or not. */ - virtual bool MinusJacobian(const double* x, double* jacobian) const = 0; + virtual bool MinusJacobian(double const* x, double* jacobian) const = 0; private: // Allow Boost Serialization access to private methods @@ -168,7 +168,7 @@ class Manifold : public ceres::Manifold * Generally unused. */ template - void serialize(Archive& /* archive */, const unsigned int /* version */) + void serialize(Archive& /* archive */, unsigned int const /* version */) { } }; diff --git a/fuse_core/include/fuse_core/manifold_adapter.hpp b/fuse_core/include/fuse_core/manifold_adapter.hpp index b85c87d6b..d22488a04 100644 --- a/fuse_core/include/fuse_core/manifold_adapter.hpp +++ b/fuse_core/include/fuse_core/manifold_adapter.hpp @@ -118,7 +118,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] x_plus_delta is a \p AmbientSize() vector. * @return Return value indicates if the operation was successful or not. */ - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + bool Plus(double const* x, double const* delta, double* x_plus_delta) const override { return local_parameterization_->Plus(x, delta, x_plus_delta); } @@ -134,7 +134,7 @@ class ManifoldAdapter : public fuse_core::Manifold * matrix. * @return */ - bool PlusJacobian(const double* x, double* jacobian) const override + bool PlusJacobian(double const* x, double* jacobian) const override { return local_parameterization_->ComputeJacobian(x, jacobian); } @@ -153,7 +153,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] y_minus_x is a \p TangentSize() vector. * @return Return value indicates if the operation was successful or not. */ - bool Minus(const double* y, const double* x, double* y_minus_x) const override + bool Minus(double const* y, double const* x, double* y_minus_x) const override { return local_parameterization_->Minus(x, y, y_minus_x); } @@ -167,7 +167,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. * @return Return value indicates whether the operation was successful or not. */ - bool MinusJacobian(const double* x, double* jacobian) const override + bool MinusJacobian(double const* x, double* jacobian) const override { return local_parameterization_->ComputeMinusJacobian(x, jacobian); } @@ -185,7 +185,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& local_parameterization_; diff --git a/fuse_core/include/fuse_core/message_buffer.hpp b/fuse_core/include/fuse_core/message_buffer.hpp index 11dde62a4..d8e4e544f 100644 --- a/fuse_core/include/fuse_core/message_buffer.hpp +++ b/fuse_core/include/fuse_core/message_buffer.hpp @@ -89,7 +89,7 @@ class MessageBuffer * timestamps that are older than the buffer length, an exception will be * thrown. */ - explicit MessageBuffer(const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); + explicit MessageBuffer(rclcpp::Duration const& buffer_length = rclcpp::Duration::max()); /** * @brief Destructor @@ -99,7 +99,7 @@ class MessageBuffer /** * @brief Read-only access to the buffer length */ - const rclcpp::Duration& bufferLength() const + rclcpp::Duration const& bufferLength() const { return buffer_length_; } @@ -107,7 +107,7 @@ class MessageBuffer /** * @brief Write access to the buffer length */ - void bufferLength(const rclcpp::Duration& buffer_length) + void bufferLength(rclcpp::Duration const& buffer_length) { buffer_length_ = buffer_length; } @@ -121,7 +121,7 @@ class MessageBuffer * @param[in] stamp The stamp to assign to the message * @param[in] msg A message */ - void insert(const rclcpp::Time& stamp, const Message& msg); + void insert(rclcpp::Time const& stamp, Message const& msg); /** * @brief Query the buffer for the set of messages between two timestamps @@ -141,7 +141,7 @@ class MessageBuffer * @return An iterator range containing all of the messages between the * specified stamps. */ - message_range query(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, bool extended_range = true); + message_range query(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, bool extended_range = true); /** * @brief Read-only access to the current set of timestamps @@ -161,7 +161,7 @@ class MessageBuffer * @brief Helper function used with boost::transform_iterators to convert the internal Buffer * value type into a const rclcpp::Time& iterator compatible with stamp_range */ - static const rclcpp::Time& extractStamp(const typename Buffer::value_type& element) + static rclcpp::Time const& extractStamp(const typename Buffer::value_type& element) { return element.first; } diff --git a/fuse_core/include/fuse_core/message_buffer_impl.hpp b/fuse_core/include/fuse_core/message_buffer_impl.hpp index 1a3cdebb3..9493dda84 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.hpp +++ b/fuse_core/include/fuse_core/message_buffer_impl.hpp @@ -47,20 +47,20 @@ namespace fuse_core { template -MessageBuffer::MessageBuffer(const rclcpp::Duration& buffer_length) : buffer_length_(buffer_length) +MessageBuffer::MessageBuffer(rclcpp::Duration const& buffer_length) : buffer_length_(buffer_length) { } template -void MessageBuffer::insert(const rclcpp::Time& stamp, const Message& msg) +void MessageBuffer::insert(rclcpp::Time const& stamp, Message const& msg) { buffer_.emplace_back(stamp, msg); purgeHistory(); } template -typename MessageBuffer::message_range MessageBuffer::query(const rclcpp::Time& beginning_stamp, - const rclcpp::Time& ending_stamp, +typename MessageBuffer::message_range MessageBuffer::query(rclcpp::Time const& beginning_stamp, + rclcpp::Time const& ending_stamp, bool extended_range) { // Verify the query is valid @@ -95,7 +95,7 @@ typename MessageBuffer::message_range MessageBuffer::query(con } // Find the entry that is strictly greater than the requested beginning stamp. If the extended // range flag is true, we will then back up one entry. - auto upper_bound_comparison = [](const auto& stamp, const auto& element) -> bool { return element.first > stamp; }; + auto upper_bound_comparison = [](auto const& stamp, auto const& element) -> bool { return element.first > stamp; }; auto beginning_iter = std::upper_bound(buffer_.begin(), buffer_.end(), beginning_stamp, upper_bound_comparison); if (extended_range) { @@ -103,7 +103,7 @@ typename MessageBuffer::message_range MessageBuffer::query(con } // Find the entry that is greater than or equal to the ending stamp. If the extended range flag is // false, we will back up one entry. - auto lower_bound_comparison = [](const auto& element, const auto& stamp) -> bool { return element.first < stamp; }; + auto lower_bound_comparison = [](auto const& element, auto const& stamp) -> bool { return element.first < stamp; }; auto ending_iter = std::lower_bound(buffer_.begin(), buffer_.end(), ending_stamp, lower_bound_comparison); if (extended_range && (ending_iter != buffer_.end())) { @@ -132,7 +132,7 @@ void MessageBuffer::purgeHistory() } // Compute the expiration time carefully, as ROS can't handle negative times - const auto& ending_stamp = buffer_.back().first; + auto const& ending_stamp = buffer_.back().first; rclcpp::Time expiration_time; if (ending_stamp.seconds() > buffer_length_.seconds()) @@ -149,7 +149,7 @@ void MessageBuffer::purgeHistory() // Be careful to ensure that: // - at least two entries remains at all times // - the buffer covers *at least* until the expiration time. Longer is acceptable. - auto is_greater = [](const auto& stamp, const auto& element) -> bool { return element.first > stamp; }; + auto is_greater = [](auto const& stamp, auto const& element) -> bool { return element.first > stamp; }; auto expiration_iter = std::upper_bound(buffer_.begin(), buffer_.end(), expiration_time, is_greater); if (expiration_iter != buffer_.begin()) { diff --git a/fuse_core/include/fuse_core/motion_model.hpp b/fuse_core/include/fuse_core/motion_model.hpp index 20c60b160..baad4176e 100644 --- a/fuse_core/include/fuse_core/motion_model.hpp +++ b/fuse_core/include/fuse_core/motion_model.hpp @@ -108,12 +108,12 @@ class MotionModel * @param[in] name A unique name to give this plugin instance */ virtual void initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name) = 0; + std::string const& name) = 0; /** * @brief Get the unique name of this motion model */ - virtual const std::string& name() const = 0; + virtual std::string const& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions diff --git a/fuse_core/include/fuse_core/parameter.hpp b/fuse_core/include/fuse_core/parameter.hpp index b4376e9f1..84995378b 100644 --- a/fuse_core/include/fuse_core/parameter.hpp +++ b/fuse_core/include/fuse_core/parameter.hpp @@ -50,7 +50,7 @@ namespace fuse_core { // Helper function to get a namespace string with a '.' suffix, but only if not empty -std::string joinParameterName(const std::string& left, const std::string& right); +std::string joinParameterName(std::string const& left, std::string const& right); // NOTE(CH3): Some of these basically mimic the behavior from rclcpp's node.hpp, but for interfaces @@ -72,9 +72,9 @@ std::string joinParameterName(const std::string& left, const std::string& right) */ template T getParam( - node_interfaces::NodeInterfaces interfaces, const std::string& parameter_name, + node_interfaces::NodeInterfaces interfaces, std::string const& parameter_name, const T& default_value, - const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), + rcl_interfaces::msg::ParameterDescriptor const& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), bool ignore_override = false) { auto params_interface = interfaces.get_node_parameters_interface(); @@ -91,7 +91,7 @@ T getParam( ignore_override) .get(); } - catch (const rclcpp::ParameterTypeException& ex) + catch (rclcpp::ParameterTypeException const& ex) { throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); } @@ -115,8 +115,8 @@ T getParam( */ template T getParam( - node_interfaces::NodeInterfaces interfaces, const std::string& parameter_name, - const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), + node_interfaces::NodeInterfaces interfaces, std::string const& parameter_name, + rcl_interfaces::msg::ParameterDescriptor const& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), bool ignore_override = false) { // get advantage of parameter value template magic to get the correct rclcpp::ParameterType from T @@ -128,7 +128,7 @@ T getParam( ->declare_parameter(parameter_name, value.get_type(), parameter_descriptor, ignore_override) .get(); } - catch (const rclcpp::ParameterTypeException& ex) + catch (rclcpp::ParameterTypeException const& ex) { throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); } @@ -140,7 +140,7 @@ namespace detail * @internal */ std::unordered_set -list_parameter_override_prefixes(const std::map& overrides, std::string prefix); +list_parameter_override_prefixes(std::map const& overrides, std::string prefix); } // namespace detail /** @@ -180,7 +180,7 @@ list_parameter_override_prefixes(node_interfaces::NodeInterfaces interfaces, - const std::string& key, std::string& value) + std::string const& key, std::string& value) { std::string default_value = ""; value = getParam(interfaces, key, default_value); @@ -208,7 +208,7 @@ inline void getParamRequired( */ template ::value || std::is_floating_point::value>> void getPositiveParam(node_interfaces::NodeInterfaces interfaces, - const std::string& parameter_name, T& default_value, const bool strict = true) + std::string const& parameter_name, T& default_value, bool const strict = true) { T value = getParam(interfaces, parameter_name, default_value); if (value < 0 || (strict && value == 0)) @@ -235,7 +235,7 @@ void getPositiveParam(node_interfaces::NodeInterfaces interfaces, - const std::string& parameter_name, rclcpp::Duration& default_value, const bool strict = true) + std::string const& parameter_name, rclcpp::Duration& default_value, bool const strict = true) { double default_value_sec = default_value.seconds(); getPositiveParam(interfaces, parameter_name, default_value_sec, strict); @@ -259,14 +259,14 @@ getPositiveParam(node_interfaces::NodeInterfaces fuse_core::Matrix getCovarianceDiagonalParam( node_interfaces::NodeInterfaces interfaces, - const std::string& parameter_name, Scalar default_value) + std::string const& parameter_name, Scalar default_value) { using Vector = typename Eigen::Matrix; std::vector diagonal(Size, default_value); diagonal = getParam(interfaces, parameter_name, diagonal); - const auto diagonal_size = diagonal.size(); + auto const diagonal_size = diagonal.size(); if (diagonal_size != Size) { throw std::invalid_argument("Invalid size of " + std::to_string(diagonal_size) + ", expected " + @@ -274,7 +274,7 @@ fuse_core::Matrix getCovarianceDiagonalParam( } if (std::any_of(diagonal.begin(), diagonal.end(), - [](const auto& value) { return value < Scalar(0); })) // NOLINT(whitespace/braces) + [](auto const& value) { return value < Scalar(0); })) // NOLINT(whitespace/braces) { throw std::invalid_argument("Invalid negative diagonal values in " + fuse_core::to_string(Vector(diagonal.data()))); } @@ -292,7 +292,7 @@ fuse_core::Matrix getCovarianceDiagonalParam( inline fuse_core::Loss::SharedPtr loadLossConfig( node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { if (!interfaces.get_node_parameters_interface()->has_parameter(name + ".type")) { diff --git a/fuse_core/include/fuse_core/publisher.hpp b/fuse_core/include/fuse_core/publisher.hpp index 6690b4fd0..ba3c94c71 100644 --- a/fuse_core/include/fuse_core/publisher.hpp +++ b/fuse_core/include/fuse_core/publisher.hpp @@ -85,12 +85,12 @@ class Publisher * @param[in] name A unique name to give this plugin instance */ virtual void initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name) = 0; + std::string const& name) = 0; /** * @brief Get the unique name of this publisher */ - virtual const std::string& name() const = 0; + virtual std::string const& name() const = 0; /** * @brief Notify the publisher that an optimization cycle is complete, and about changes to the diff --git a/fuse_core/include/fuse_core/sensor_model.hpp b/fuse_core/include/fuse_core/sensor_model.hpp index 61b7bfbd5..1b3f3e636 100644 --- a/fuse_core/include/fuse_core/sensor_model.hpp +++ b/fuse_core/include/fuse_core/sensor_model.hpp @@ -113,12 +113,12 @@ class SensorModel * @param[in] transaction_callback The function to call every time a transaction is published */ virtual void initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name, TransactionCallback transaction_callback) = 0; + std::string const& name, TransactionCallback transaction_callback) = 0; /** * @brief Get the unique name of this sensor */ - [[nodiscard]] virtual const std::string& name() const = 0; + [[nodiscard]] virtual std::string const& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions diff --git a/fuse_core/include/fuse_core/serialization.hpp b/fuse_core/include/fuse_core/serialization.hpp index 9a73f804f..c7c39acff 100644 --- a/fuse_core/include/fuse_core/serialization.hpp +++ b/fuse_core/include/fuse_core/serialization.hpp @@ -77,12 +77,12 @@ class MessageBufferStreamSource * * @param[in] data A byte vector from a ROS message */ - explicit MessageBufferStreamSource(const std::vector& data); + explicit MessageBufferStreamSource(std::vector const& data); /** * @brief The stream source is non-copyable */ - MessageBufferStreamSource operator=(const MessageBufferStreamSource&) = delete; + MessageBufferStreamSource operator=(MessageBufferStreamSource const&) = delete; /** * @brief Read up to n characters from the data vector @@ -96,7 +96,7 @@ class MessageBufferStreamSource std::streamsize read(char_type* s, std::streamsize n); private: - const std::vector& data_; //!< Reference to the source container + std::vector const& data_; //!< Reference to the source container size_t index_; //!< The next vector index to read }; @@ -122,7 +122,7 @@ class MessageBufferStreamSink /** * @brief The stream sink is non-copyable */ - MessageBufferStreamSink operator=(const MessageBufferStreamSink&) = delete; + MessageBufferStreamSink operator=(MessageBufferStreamSink const&) = delete; /** * @brief Write n characters to the data vector @@ -133,7 +133,7 @@ class MessageBufferStreamSink * @param[in] n The number of bytes to write to the stream * @return The number of bytes written */ - std::streamsize write(const char_type* s, std::streamsize n); + std::streamsize write(char_type const* s, std::streamsize n); private: std::vector& data_; //!< Reference to the destination container @@ -150,7 +150,7 @@ namespace serialization * @brief Serialize a rclcpp::Time variable using Boost Serialization */ template -void serialize(Archive& archive, rclcpp::Time& stamp, const unsigned int /* version */) +void serialize(Archive& archive, rclcpp::Time& stamp, unsigned int const /* version */) { auto nanoseconds = stamp.nanoseconds(); auto clock_type = stamp.get_clock_type(); @@ -167,7 +167,7 @@ void serialize(Archive& archive, rclcpp::Time& stamp, const unsigned int /* vers */ template inline void serialize(Archive& archive, Eigen::Matrix& matrix, - const unsigned int /* version */) + unsigned int const /* version */) { Eigen::Index rows = matrix.rows(); Eigen::Index cols = matrix.cols(); diff --git a/fuse_core/include/fuse_core/throttled_callback.hpp b/fuse_core/include/fuse_core/throttled_callback.hpp index 958229794..728a0e8f8 100644 --- a/fuse_core/include/fuse_core/throttled_callback.hpp +++ b/fuse_core/include/fuse_core/throttled_callback.hpp @@ -69,7 +69,7 @@ class ThrottledCallback */ ThrottledCallback(Callback&& keep_callback = nullptr, // NOLINT(whitespace/operators) Callback&& drop_callback = nullptr, // NOLINT(whitespace/operators) - const rclcpp::Duration& throttle_period = rclcpp::Duration(0, 0), + rclcpp::Duration const& throttle_period = rclcpp::Duration(0, 0), rclcpp::Clock::SharedPtr clock = std::make_shared()) : keep_callback_(keep_callback), drop_callback_(drop_callback), throttle_period_(throttle_period), clock_(clock) { @@ -80,7 +80,7 @@ class ThrottledCallback * * @return The current throttle period duration in seconds being used */ - const rclcpp::Duration& getThrottlePeriod() const + rclcpp::Duration const& getThrottlePeriod() const { return throttle_period_; } @@ -103,7 +103,7 @@ class ThrottledCallback * * @param[in] throttle_period The new throttle period duration in seconds to use */ - void setThrottlePeriod(const rclcpp::Duration& throttle_period) + void setThrottlePeriod(rclcpp::Duration const& throttle_period) { throttle_period_ = throttle_period; } @@ -113,7 +113,7 @@ class ThrottledCallback * * @param[in] keep_callback The new keep callback to use */ - void setKeepCallback(const Callback& keep_callback) + void setKeepCallback(Callback const& keep_callback) { keep_callback_ = keep_callback; } @@ -123,7 +123,7 @@ class ThrottledCallback * * @param[in] drop_callback The new drop callback to use */ - void setDropCallback(const Callback& drop_callback) + void setDropCallback(Callback const& drop_callback) { drop_callback_ = drop_callback; } @@ -133,7 +133,7 @@ class ThrottledCallback * * @return The last time the keep callback was called */ - const rclcpp::Time& getLastCalledTime() const + rclcpp::Time const& getLastCalledTime() const { return last_called_time_; } diff --git a/fuse_core/include/fuse_core/timestamp_manager.hpp b/fuse_core/include/fuse_core/timestamp_manager.hpp index d084b8700..e8686eefc 100644 --- a/fuse_core/include/fuse_core/timestamp_manager.hpp +++ b/fuse_core/include/fuse_core/timestamp_manager.hpp @@ -86,7 +86,7 @@ class TimestampManager * optimizer. */ using MotionModelFunction = - std::function& constraints, std::vector& variables)>; /** @@ -110,7 +110,7 @@ class TimestampManager * thrown. */ explicit TimestampManager(MotionModelFunction generator, - const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); + rclcpp::Duration const& buffer_length = rclcpp::Duration::max()); TimestampManager(TimestampManager const&) = default; TimestampManager(TimestampManager&&) = default; @@ -132,10 +132,10 @@ class TimestampManager * thrown. */ template - TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + TimestampManager(void (T::*fp)(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables), - T* obj, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); + T* obj, rclcpp::Duration const& buffer_length = rclcpp::Duration::max()); /** * @brief Constructor that accepts the motion model generator as a const member function pointer @@ -152,10 +152,10 @@ class TimestampManager * thrown. */ template - TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + TimestampManager(void (T::*fp)(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables) const, - T* obj, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); + T* obj, rclcpp::Duration const& buffer_length = rclcpp::Duration::max()); /** * @brief Destructor @@ -165,7 +165,7 @@ class TimestampManager /** * @brief Read-only access to the buffer length */ - const rclcpp::Duration& bufferLength() const + rclcpp::Duration const& bufferLength() const { return buffer_length_; } @@ -173,7 +173,7 @@ class TimestampManager /** * @brief Write access to the buffer length */ - void bufferLength(const rclcpp::Duration& buffer_length) + void bufferLength(rclcpp::Duration const& buffer_length) { buffer_length_ = buffer_length; } @@ -231,9 +231,9 @@ class TimestampManager { } - MotionModelSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, - const std::vector& constraints, - const std::vector& variables) + MotionModelSegment(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, + std::vector const& constraints, + std::vector const& variables) : beginning_stamp(beginning_stamp), ending_stamp(ending_stamp), constraints(constraints), variables(variables) { } @@ -266,7 +266,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * addSegment */ - void addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, Transaction& transaction); + void addSegment(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, Transaction& transaction); /** * @brief Remove an existing MotionModelSegment, updating the provided transaction. @@ -292,7 +292,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * splitSegment */ - void splitSegment(MotionModelHistory::iterator& iter, const rclcpp::Time& stamp, Transaction& transaction); + void splitSegment(MotionModelHistory::iterator& iter, rclcpp::Time const& stamp, Transaction& transaction); /** * @brief Remove any motion model segments that are older than \p buffer_length_ @@ -301,10 +301,10 @@ class TimestampManager }; template -TimestampManager::TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, +TimestampManager::TimestampManager(void (T::*fp)(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables), - T* obj, const rclcpp::Duration& buffer_length) + T* obj, rclcpp::Duration const& buffer_length) : TimestampManager(std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4), buffer_length) @@ -312,10 +312,10 @@ TimestampManager::TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_s } template -TimestampManager::TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, +TimestampManager::TimestampManager(void (T::*fp)(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables) const, - T* obj, const rclcpp::Duration& buffer_length) + T* obj, rclcpp::Duration const& buffer_length) : TimestampManager(std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4), buffer_length) diff --git a/fuse_core/include/fuse_core/transaction.hpp b/fuse_core/include/fuse_core/transaction.hpp index 9651d35bc..50b46e653 100644 --- a/fuse_core/include/fuse_core/transaction.hpp +++ b/fuse_core/include/fuse_core/transaction.hpp @@ -111,7 +111,7 @@ class Transaction /** * @brief Read-only access to this transaction's timestamp */ - [[nodiscard]] const rclcpp::Time& stamp() const + [[nodiscard]] rclcpp::Time const& stamp() const { return stamp_; } @@ -119,7 +119,7 @@ class Transaction /** * @brief Write access to this transaction's timestamp */ - void stamp(const rclcpp::Time& stamp) + void stamp(rclcpp::Time const& stamp) { stamp_ = stamp; } @@ -140,7 +140,7 @@ class Transaction * * @return The minimum (oldest) timestamp. */ - [[nodiscard]] const rclcpp::Time& minStamp() const; + [[nodiscard]] rclcpp::Time const& minStamp() const; /** * @brief Read-only access to the maximum (newest) timestamp among the transaction's stamp and all @@ -148,7 +148,7 @@ class Transaction * * @return The maximum (newest) timestamp. */ - [[nodiscard]] const rclcpp::Time& maxStamp() const; + [[nodiscard]] rclcpp::Time const& maxStamp() const; /** * @brief Read-only access to the added constraints @@ -200,7 +200,7 @@ class Transaction * * @param[in] stamp The timestamp to be added */ - void addInvolvedStamp(const rclcpp::Time& stamp); + void addInvolvedStamp(rclcpp::Time const& stamp); /** * @brief Add a constraint to this transaction @@ -259,7 +259,7 @@ class Transaction * @param[in] overwrite Flag indicating that variables and constraints in \p other should * overwrite existing variables and constraints with the UUIDs. */ - void merge(const Transaction& other, bool overwrite = false); + void merge(Transaction const& other, bool overwrite = false); /** * @brief Print a human-readable description of the transaction to the provided stream. @@ -324,7 +324,7 @@ class Transaction * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& stamp_; archive& added_constraints_; @@ -338,7 +338,7 @@ class Transaction /** * Stream operator for printing Transaction objects. */ -std::ostream& operator<<(std::ostream& stream, const Transaction& transaction); +std::ostream& operator<<(std::ostream& stream, Transaction const& transaction); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/transaction_deserializer.hpp b/fuse_core/include/fuse_core/transaction_deserializer.hpp index 7d5f44675..5d2579ab2 100644 --- a/fuse_core/include/fuse_core/transaction_deserializer.hpp +++ b/fuse_core/include/fuse_core/transaction_deserializer.hpp @@ -46,7 +46,7 @@ namespace fuse_core /** * @brief Serialize a transaction into a message */ -void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg); +void serializeTransaction(fuse_core::Transaction const& transaction, fuse_msgs::msg::SerializedTransaction& msg); /** * @brief Deserialize a Transaction @@ -88,7 +88,7 @@ class TransactionDeserializer * @param[IN] msg The SerializedTransaction message to be deserialized * @return A fuse Transaction object */ - fuse_core::Transaction::UniquePtr deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const; + fuse_core::Transaction::UniquePtr deserialize(fuse_msgs::msg::SerializedTransaction const& msg) const; private: //! Pluginlib class loader for Variable types diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index 0fe01c9df..7986cb61b 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -159,7 +159,7 @@ Eigen::Matrix rotationMatrix2D(const T angle) * @param[in] rpy Pointer to the roll, pitch, yaw array (3x1) * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) */ -static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian = nullptr) +static inline void quaternion2rpy(double const* q, double* rpy, double* jacobian = nullptr) { rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); @@ -168,12 +168,12 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian if (jacobian != nullptr) { Eigen::Map> jacobian_map(jacobian); - const double qw = q[0]; - const double qx = q[1]; - const double qy = q[2]; - const double qz = q[3]; - const double discr = qw * qy - qx * qz; - const double gl_limit = 0.5 - 2.0 * std::numeric_limits::epsilon(); + double const qw = q[0]; + double const qx = q[1]; + double const qy = q[2]; + double const qz = q[3]; + double const discr = qw * qy - qx * qz; + double const gl_limit = 0.5 - 2.0 * std::numeric_limits::epsilon(); if (discr > gl_limit) { @@ -245,7 +245,7 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian * @param[in] zw Pointer to the output quaternion array (4x1) that will be populated with the result of z * w * @param[in] jacobian Pointer to the jacobian of zw with respect to w (4x4, optional) */ -static inline void quaternionProduct(const double* z, const double* w, double* zw, double* jacobian = nullptr) +static inline void quaternionProduct(double const* z, double const* w, double* zw, double* jacobian = nullptr) { ceres::QuaternionProduct(z, w, zw); if (jacobian != nullptr) @@ -263,29 +263,29 @@ static inline void quaternionProduct(const double* z, const double* w, double* z * @param[in] angle_axis Pointer to the angle_axis array (3x1) * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) */ -static inline void quaternionToAngleAxis(const double* q, double* angle_axis, double* jacobian = nullptr) +static inline void quaternionToAngleAxis(double const* q, double* angle_axis, double* jacobian = nullptr) { ceres::QuaternionToAngleAxis(q, angle_axis); if (jacobian != nullptr) { Eigen::Map> jacobian_map(jacobian); - const double& q0 = q[0]; - const double& q1 = q[1]; - const double& q2 = q[2]; - const double& q3 = q[3]; - const double q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; - const double sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3; - const double sin_theta = std::hypot(q1, q2, q3); - const double cos_theta = q0; + double const& q0 = q[0]; + double const& q1 = q[1]; + double const& q2 = q[2]; + double const& q3 = q[3]; + double const q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; + double const sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3; + double const sin_theta = std::hypot(q1, q2, q3); + double const cos_theta = q0; - const double cond = std::pow(sin_theta2, 1.5); + double const cond = std::pow(sin_theta2, 1.5); if (std::fpclassify(cond) != FP_ZERO) { - const double two_theta = + double const two_theta = 2.0 * (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); // const double a = two_theta / sin_theta; - const double b = sin_theta2 * q_sum2; - const double c = two_theta / cond; + double const b = sin_theta2 * q_sum2; + double const c = two_theta / cond; jacobian_map(0, 0) = -2.0 * q1 / q_sum2; jacobian_map(0, 1) = two_theta / sin_theta + (2.0 * q0 * q1 * q1) / b - (q1 * q1 * c); jacobian_map(0, 2) = (2.0 * q0 * q1 * q2) / b - (q1 * q2 * c); diff --git a/fuse_core/include/fuse_core/uuid.hpp b/fuse_core/include/fuse_core/uuid.hpp index fd9833577..3b5788f7b 100644 --- a/fuse_core/include/fuse_core/uuid.hpp +++ b/fuse_core/include/fuse_core/uuid.hpp @@ -63,7 +63,7 @@ constexpr UUID NIL = { { 0 } }; * - "0123456789abcdef0123456789abcdef" * - "{01234567-89ab-cdef-0123-456789abcdef}" */ -inline UUID from_string(const std::string& uuid_string) +inline UUID from_string(std::string const& uuid_string) { return boost::uuids::string_generator()(uuid_string); } @@ -80,7 +80,7 @@ UUID generate(); * @param[in] byte_count The number of bytes in the data buffer * @return A repeatable UUID specific to the provided data */ -inline UUID generate(const void* data, size_t byte_count) +inline UUID generate(void const* data, size_t byte_count) { return boost::uuids::name_generator(NIL)(data, byte_count); } @@ -91,7 +91,7 @@ inline UUID generate(const void* data, size_t byte_count) * @param[in] data A data buffer held in a C-style string * @return A repeatable UUID specific to the provided data */ -inline UUID generate(const char* data) +inline UUID generate(char const* data) { return generate(data, std::strlen(data)); } @@ -102,7 +102,7 @@ inline UUID generate(const char* data) * @param[in] data A data buffer held in a C++-style string * @return A repeatable UUID specific to the provided namespace and data */ -inline UUID generate(const std::string& data) +inline UUID generate(std::string const& data) { return generate(data.c_str(), data.length()); } @@ -115,7 +115,7 @@ inline UUID generate(const std::string& data) * @param[in] byte_count The number of bytes in the data buffer * @return A repeatable UUID specific to the provided namespace and data */ -inline UUID generate(const std::string& namespace_string, const void* data, size_t byte_count) +inline UUID generate(std::string const& namespace_string, void const* data, size_t byte_count) { return boost::uuids::name_generator(generate(namespace_string))(data, byte_count); } @@ -127,7 +127,7 @@ inline UUID generate(const std::string& namespace_string, const void* data, size * @param[in] data A data buffer held in a C-style string * @return A repeatable UUID specific to the provided namespace and data */ -inline UUID generate(const std::string& namespace_string, const char* data) +inline UUID generate(std::string const& namespace_string, char const* data) { return generate(namespace_string, data, std::strlen(data)); } @@ -139,7 +139,7 @@ inline UUID generate(const std::string& namespace_string, const char* data) * @param[in] data A data buffer held in a C++-style string * @return A repeatable UUID specific to the provided namespace and data */ -inline UUID generate(const std::string& namespace_string, const std::string& data) +inline UUID generate(std::string const& namespace_string, std::string const& data) { return generate(namespace_string, data.c_str(), data.length()); } @@ -153,7 +153,7 @@ inline UUID generate(const std::string& namespace_string, const std::string& dat * @param[in] stamp An rclcpp::Time timestamp * @return A repeatable UUID specific to the provided namespace and timestamp */ -UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp); +UUID generate(std::string const& namespace_string, rclcpp::Time const& stamp); /** * @brief Generate a UUID from a namespace string, a ros timestamp, and an additional id @@ -165,7 +165,7 @@ UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp); * @param[in] id A UUID * @return A repeatable UUID specific to the provided namespace and timestamp */ -UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id); +UUID generate(std::string const& namespace_string, rclcpp::Time const& stamp, const UUID& id); /** * @brief Generate a UUID from a namespace string and a user provided id @@ -176,7 +176,7 @@ UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, co * @param[in] user_id A uint64_t user generated id * @return A repeatable UUID specific to the provided namespace and user id */ -UUID generate(const std::string& namespace_string, const uint64_t& user_id); +UUID generate(std::string const& namespace_string, uint64_t const& user_id); } // namespace uuid } // namespace fuse_core @@ -191,7 +191,7 @@ namespace std template <> struct hash { - size_t operator()(const fuse_core::UUID& id) const + size_t operator()(fuse_core::UUID const& id) const { return boost::uuids::hash_value(id); } diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 9f05f76db..6121919ef 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -282,7 +282,7 @@ class Variable * Variable::size() elements will be accessed externally. This interface is provided for * integration with Ceres, which uses raw pointers. */ - [[nodiscard]] virtual const double* data() const = 0; + [[nodiscard]] virtual double const* data() const = 0; /** * @brief Read-write access to the variable data @@ -474,7 +474,7 @@ class Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& uuid_; } @@ -483,7 +483,7 @@ class Variable /** * Stream operator implementation used for all derived Variable classes. */ -std::ostream& operator<<(std::ostream& stream, const Variable& variable); +std::ostream& operator<<(std::ostream& stream, Variable const& variable); } // namespace fuse_core diff --git a/fuse_core/src/async_motion_model.cpp b/fuse_core/src/async_motion_model.cpp index d6dd6fc47..c800773de 100644 --- a/fuse_core/src/async_motion_model.cpp +++ b/fuse_core/src/async_motion_model.cpp @@ -74,7 +74,7 @@ bool AsyncMotionModel::apply(Transaction& transaction) } void AsyncMotionModel::initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { if (initialized_) { diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index df16c96f9..ab1b41052 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -47,7 +47,7 @@ AsyncPublisher::~AsyncPublisher() } void AsyncPublisher::initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { if (initialized_) { diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index bf71909ae..dcb41ebb4 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -55,7 +55,7 @@ AsyncSensorModel::~AsyncSensorModel() } void AsyncSensorModel::initialize(node_interfaces::NodeInterfaces interfaces, - const std::string& name, TransactionCallback transaction_callback) + std::string const& name, TransactionCallback transaction_callback) { if (initialized_) { diff --git a/fuse_core/src/callback_wrapper.cpp b/fuse_core/src/callback_wrapper.cpp index a55310990..31cedbb67 100644 --- a/fuse_core/src/callback_wrapper.cpp +++ b/fuse_core/src/callback_wrapper.cpp @@ -120,7 +120,7 @@ void CallbackAdapter::execute(std::shared_ptr& data) std::static_pointer_cast(data)->call(); } -void CallbackAdapter::addCallback(const std::shared_ptr& callback) +void CallbackAdapter::addCallback(std::shared_ptr const& callback) { std::lock_guard const lock(queue_mutex_); callback_queue_.push_back(callback); diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 960fd3c00..9ac2b35f5 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -56,7 +56,7 @@ namespace fuse_core void loadCovarianceOptionsFromROS( node_interfaces::NodeInterfaces interfaces, - ceres::Covariance::Options& covariance_options, const std::string& ns) + ceres::Covariance::Options& covariance_options, std::string const& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; @@ -98,7 +98,7 @@ void loadCovarianceOptionsFromROS( } void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, - ceres::Problem::Options& problem_options, const std::string& ns) + ceres::Problem::Options& problem_options, std::string const& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; @@ -124,7 +124,7 @@ void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, - ceres::Solver::Options& solver_options, const std::string& ns) + ceres::Solver::Options& solver_options, std::string const& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; diff --git a/fuse_core/src/constraint.cpp b/fuse_core/src/constraint.cpp index 0a3690cea..38f1f2923 100644 --- a/fuse_core/src/constraint.cpp +++ b/fuse_core/src/constraint.cpp @@ -41,12 +41,12 @@ namespace fuse_core { -Constraint::Constraint(const std::string& source, std::initializer_list variable_uuid_list) +Constraint::Constraint(std::string const& source, std::initializer_list variable_uuid_list) : source_(source), uuid_(uuid::generate()), variables_(variable_uuid_list) { } -std::ostream& operator<<(std::ostream& stream, const Constraint& constraint) +std::ostream& operator<<(std::ostream& stream, Constraint const& constraint) { constraint.print(stream); return stream; diff --git a/fuse_core/src/fuse_echo.cpp b/fuse_core/src/fuse_echo.cpp index 2d2f6c0d9..c99c73b37 100644 --- a/fuse_core/src/fuse_echo.cpp +++ b/fuse_core/src/fuse_echo.cpp @@ -65,7 +65,7 @@ class FuseEcho : public rclcpp::Node rclcpp::Subscription::SharedPtr graph_sub_; rclcpp::Subscription::SharedPtr transaction_sub_; - void graphCallback(const fuse_msgs::msg::SerializedGraph& msg) + void graphCallback(fuse_msgs::msg::SerializedGraph const& msg) { std::cout << "-------------------------" << std::endl; std::cout << "GRAPH:" << std::endl; @@ -74,7 +74,7 @@ class FuseEcho : public rclcpp::Node graph->print(); } - void transactionCallback(const fuse_msgs::msg::SerializedTransaction& msg) + void transactionCallback(fuse_msgs::msg::SerializedTransaction const& msg) { std::cout << "-------------------------" << std::endl; std::cout << "TRANSACTION:" << std::endl; diff --git a/fuse_core/src/graph.cpp b/fuse_core/src/graph.cpp index ddd9cf153..6e81964ea 100644 --- a/fuse_core/src/graph.cpp +++ b/fuse_core/src/graph.cpp @@ -41,7 +41,7 @@ namespace fuse_core { -std::ostream& operator<<(std::ostream& stream, const Graph& graph) +std::ostream& operator<<(std::ostream& stream, Graph const& graph) { graph.print(stream); return stream; @@ -49,39 +49,39 @@ std::ostream& operator<<(std::ostream& stream, const Graph& graph) Graph::const_variable_range Graph::getConnectedVariables(const UUID& constraint_uuid) const { - std::function uuid_to_variable_ref = - [this](const UUID& variable_uuid) -> const Variable& { return this->getVariable(variable_uuid); }; + std::function uuid_to_variable_ref = + [this](const UUID& variable_uuid) -> Variable const& { return this->getVariable(variable_uuid); }; - const auto& constraint = getConstraint(constraint_uuid); - const auto& variable_uuids = constraint.variables(); + auto const& constraint = getConstraint(constraint_uuid); + auto const& variable_uuids = constraint.variables(); return const_variable_range(boost::make_transform_iterator(variable_uuids.cbegin(), uuid_to_variable_ref), boost::make_transform_iterator(variable_uuids.cend(), uuid_to_variable_ref)); } -void Graph::update(const Transaction& transaction) +void Graph::update(Transaction const& transaction) { // Update the graph with a new transaction. In order to keep the graph consistent, variables are // added first, followed by the constraints which might use the newly added variables. Then // constraints are removed so that the variable usage is updated. Finally, variables are removed. // Insert the new variables into the graph - for (const auto& variable : transaction.addedVariables()) + for (auto const& variable : transaction.addedVariables()) { addVariable(variable.clone()); } // Insert the new constraints into the graph - for (const auto& constraint : transaction.addedConstraints()) + for (auto const& constraint : transaction.addedConstraints()) { addConstraint(constraint.clone()); } // Delete constraints from the graph - for (const auto& constraint_uuid : transaction.removedConstraints()) + for (auto const& constraint_uuid : transaction.removedConstraints()) { removeConstraint(constraint_uuid); } // Delete variables from the graph - for (const auto& variable_uuid : transaction.removedVariables()) + for (auto const& variable_uuid : transaction.removedVariables()) { removeVariable(variable_uuid); } diff --git a/fuse_core/src/graph_deserializer.cpp b/fuse_core/src/graph_deserializer.cpp index 76fb19d53..d6af7daa4 100644 --- a/fuse_core/src/graph_deserializer.cpp +++ b/fuse_core/src/graph_deserializer.cpp @@ -38,7 +38,7 @@ namespace fuse_core { -void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg) +void serializeGraph(fuse_core::Graph const& graph, fuse_msgs::msg::SerializedGraph& msg) { // Serialize the graph into the msg.data field boost::iostreams::stream stream(msg.data); @@ -62,21 +62,21 @@ GraphDeserializer::GraphDeserializer() // Load all known plugin libraries // I believe the library containing a given Variable or Constraint type must be loaded in order to // deserialize an object of that type. But I haven't actually tested that theory. - for (const auto& class_name : variable_loader_.getDeclaredClasses()) + for (auto const& class_name : variable_loader_.getDeclaredClasses()) { variable_loader_.loadLibraryForClass(class_name); } - for (const auto& class_name : constraint_loader_.getDeclaredClasses()) + for (auto const& class_name : constraint_loader_.getDeclaredClasses()) { constraint_loader_.loadLibraryForClass(class_name); } - for (const auto& class_name : loss_loader_.getDeclaredClasses()) + for (auto const& class_name : loss_loader_.getDeclaredClasses()) { loss_loader_.loadLibraryForClass(class_name); } } -fuse_core::Graph::UniquePtr GraphDeserializer::deserialize(const fuse_msgs::msg::SerializedGraph& msg) const +fuse_core::Graph::UniquePtr GraphDeserializer::deserialize(fuse_msgs::msg::SerializedGraph const& msg) const { // Create a Graph object using pluginlib. This will throw if the plugin name is not found. // The unique ptr returned by pluginlib has a custom deleter. This makes it annoying to return diff --git a/fuse_core/src/loss.cpp b/fuse_core/src/loss.cpp index 328cf3fb6..a29d96cc8 100644 --- a/fuse_core/src/loss.cpp +++ b/fuse_core/src/loss.cpp @@ -38,7 +38,7 @@ namespace fuse_core { -std::ostream& operator<<(std::ostream& stream, const Loss& loss) +std::ostream& operator<<(std::ostream& stream, Loss const& loss) { loss.print(stream); return stream; diff --git a/fuse_core/src/parameter.cpp b/fuse_core/src/parameter.cpp index 2ffc13446..b66c02a93 100644 --- a/fuse_core/src/parameter.cpp +++ b/fuse_core/src/parameter.cpp @@ -44,17 +44,17 @@ std::unordered_set list_parameter_override_prefixes(node_interfaces::NodeInterfaces interfaces, std::string prefix) { - const std::map& overrides = + std::map const& overrides = interfaces.get_node_parameters_interface()->get_parameter_overrides(); return detail::list_parameter_override_prefixes(overrides, prefix); } std::unordered_set -detail::list_parameter_override_prefixes(const std::map& overrides, +detail::list_parameter_override_prefixes(std::map const& overrides, std::string prefix) { // TODO(sloretz) ROS 2 must have this in a header somewhere, right? - const char kParamSeparator = '.'; + char const kParamSeparator = '.'; // Find all overrides starting with "prefix.", unless the prefix is empty. // If the prefix is empty then look at all parameter overrides. @@ -64,9 +64,9 @@ detail::list_parameter_override_prefixes(const std::map output_names; - for (const auto& kv : overrides) + for (auto const& kv : overrides) { - const std::string& name = kv.first; + std::string const& name = kv.first; if (name.size() <= prefix.size()) { // Too short, no point in checking @@ -85,7 +85,7 @@ detail::list_parameter_override_prefixes(const std::map& data) : data_(data), index_(0) +MessageBufferStreamSource::MessageBufferStreamSource(std::vector const& data) : data_(data), index_(0) { } @@ -62,7 +62,7 @@ MessageBufferStreamSink::MessageBufferStreamSink(std::vector& dat { } -std::streamsize MessageBufferStreamSink::write(const char_type* s, std::streamsize n) +std::streamsize MessageBufferStreamSink::write(char_type const* s, std::streamsize n) { data_.insert(data_.end(), s, s + n); return n; diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index 93c9297bf..80269dd92 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -43,7 +43,7 @@ namespace fuse_core { -TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length) +TimestampManager::TimestampManager(MotionModelFunction generator, rclcpp::Duration const& buffer_length) : generator_(std::move(generator)), buffer_length_(buffer_length) { } @@ -51,7 +51,7 @@ TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp:: void TimestampManager::query(Transaction& transaction, bool update_variables) { // Handle the trivial cases first - const auto& stamps = transaction.involvedStamps(); + auto const& stamps = transaction.involvedStamps(); if (stamps.empty()) { return; @@ -91,8 +91,8 @@ void TimestampManager::query(Transaction& transaction, bool update_variables) for (auto previous_iter = augmented_stamps.begin(), current_iter = std::next(augmented_stamps.begin()); current_iter != augmented_stamps.end(); ++previous_iter, ++current_iter) { - const rclcpp::Time& previous_stamp = *previous_iter; - const rclcpp::Time& current_stamp = *current_iter; + rclcpp::Time const& previous_stamp = *previous_iter; + rclcpp::Time const& current_stamp = *current_iter; // Check if the timestamp pair is exactly an existing pair. If so, don't add it. auto history_iter = motion_model_history_.lower_bound(previous_stamp); if ((history_iter != motion_model_history_.end()) && (history_iter->second.beginning_stamp == previous_stamp) && @@ -104,10 +104,10 @@ void TimestampManager::query(Transaction& transaction, bool update_variables) // This ensures that the variables in the final transaction will be overwritten with the // motion model version auto transaction_variables = transaction.addedVariables(); - for (const auto& variable : history_iter->second.variables) + for (auto const& variable : history_iter->second.variables) { if (std::any_of(transaction_variables.begin(), transaction_variables.end(), - [variable_uuid = variable->uuid()](const auto& input_variable) { + [variable_uuid = variable->uuid()](auto const& input_variable) { return input_variable.uuid() == variable_uuid; })) // NOLINT { @@ -128,7 +128,7 @@ void TimestampManager::query(Transaction& transaction, bool update_variables) } } // Create the required segments - for (const auto& stamp_pair : stamp_pairs) + for (auto const& stamp_pair : stamp_pairs) { addSegment(stamp_pair.first, stamp_pair.second, motion_model_transaction); } @@ -155,14 +155,14 @@ void TimestampManager::query(Transaction& transaction, bool update_variables) TimestampManager::const_stamp_range TimestampManager::stamps() const { - std::function const extract_stamp = - [](const MotionModelHistory::value_type& element) -> const rclcpp::Time& { return element.first; }; + std::function const extract_stamp = + [](MotionModelHistory::value_type const& element) -> rclcpp::Time const& { return element.first; }; return { boost::make_transform_iterator(motion_model_history_.begin(), extract_stamp), boost::make_transform_iterator(motion_model_history_.end(), extract_stamp) }; } -void TimestampManager::addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, +void TimestampManager::addSegment(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, Transaction& transaction) { // Generate the set of constraints and variables to add @@ -172,11 +172,11 @@ void TimestampManager::addSegment(const rclcpp::Time& beginning_stamp, const rcl // Update the transaction with the generated constraints/variables transaction.addInvolvedStamp(beginning_stamp); transaction.addInvolvedStamp(ending_stamp); - for (const auto& constraint : constraints) + for (auto const& constraint : constraints) { transaction.addConstraint(constraint); } - for (const auto& variable : variables) + for (auto const& variable : variables) { transaction.addVariable(variable); } @@ -189,7 +189,7 @@ void TimestampManager::removeSegment(MotionModelHistory::iterator& iter, Transac // Mark the previously generated constraints for removal transaction.addInvolvedStamp(iter->second.beginning_stamp); transaction.addInvolvedStamp(iter->second.ending_stamp); - for (const auto& constraint : iter->second.constraints) + for (auto const& constraint : iter->second.constraints) { transaction.removeConstraint(constraint->uuid()); } @@ -200,7 +200,7 @@ void TimestampManager::removeSegment(MotionModelHistory::iterator& iter, Transac motion_model_history_.erase(iter); } -void TimestampManager::splitSegment(MotionModelHistory::iterator& iter, const rclcpp::Time& stamp, +void TimestampManager::splitSegment(MotionModelHistory::iterator& iter, rclcpp::Time const& stamp, Transaction& transaction) { rclcpp::Time const removed_beginning_stamp = iter->second.beginning_stamp; diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index 39b6a3293..bd76c2a2a 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -41,7 +41,7 @@ namespace fuse_core { -const rclcpp::Time& Transaction::minStamp() const +rclcpp::Time const& Transaction::minStamp() const { if (involved_stamps_.empty()) { @@ -51,7 +51,7 @@ const rclcpp::Time& Transaction::minStamp() const return std::min(*involved_stamps_.begin(), stamp_); } -const rclcpp::Time& Transaction::maxStamp() const +rclcpp::Time const& Transaction::maxStamp() const { if (involved_stamps_.empty()) { @@ -61,15 +61,15 @@ const rclcpp::Time& Transaction::maxStamp() const return std::max(*involved_stamps_.rbegin(), stamp_); } -void Transaction::addInvolvedStamp(const rclcpp::Time& stamp) +void Transaction::addInvolvedStamp(rclcpp::Time const& stamp) { involved_stamps_.insert(stamp); } Transaction::const_constraint_range Transaction::addedConstraints() const { - std::function const to_constraint_ref = - [](const Constraint::SharedPtr& constraint) -> const Constraint& { return *constraint; }; + std::function const to_constraint_ref = + [](Constraint::SharedPtr const& constraint) -> Constraint const& { return *constraint; }; return { boost::make_transform_iterator(added_constraints_.cbegin(), to_constraint_ref), boost::make_transform_iterator(added_constraints_.cend(), to_constraint_ref) }; @@ -88,7 +88,7 @@ void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite } // Also don't add the same constraint twice - auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr& added_constraint) { + auto is_constraint_added = [&constraint_uuid](Constraint::SharedPtr const& added_constraint) { return constraint_uuid == added_constraint->uuid(); }; auto added_constraints_iter = std::find_if(added_constraints_.begin(), added_constraints_.end(), is_constraint_added); @@ -106,7 +106,7 @@ void Transaction::removeConstraint(const UUID& constraint_uuid) { // If the constraint being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. - auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr& added_constraint) { + auto is_constraint_added = [&constraint_uuid](Constraint::SharedPtr const& added_constraint) { return constraint_uuid == added_constraint->uuid(); }; auto added_constraints_iter = std::find_if(added_constraints_.begin(), added_constraints_.end(), is_constraint_added); @@ -126,8 +126,8 @@ void Transaction::removeConstraint(const UUID& constraint_uuid) Transaction::const_variable_range Transaction::addedVariables() const { - std::function const to_variable_ref = - [](const Variable::SharedPtr& variable) -> const Variable& { return *variable; }; + std::function const to_variable_ref = + [](Variable::SharedPtr const& variable) -> Variable const& { return *variable; }; return { boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), boost::make_transform_iterator(added_variables_.cend(), to_variable_ref) }; @@ -153,7 +153,7 @@ void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite) } // Also don't add the same variable twice - auto is_variable_added = [&variable_uuid](const Variable::SharedPtr& added_variable) { + auto is_variable_added = [&variable_uuid](Variable::SharedPtr const& added_variable) { return variable_uuid == added_variable->uuid(); }; auto added_variables_iter = std::find_if(added_variables_.begin(), added_variables_.end(), is_variable_added); @@ -171,7 +171,7 @@ void Transaction::removeVariable(const UUID& variable_uuid) { // If the variable being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. - auto is_variable_added = [&variable_uuid](const Variable::SharedPtr& added_variable) { + auto is_variable_added = [&variable_uuid](Variable::SharedPtr const& added_variable) { return variable_uuid == added_variable->uuid(); }; auto added_variables_iter = std::find_if(added_variables_.begin(), added_variables_.end(), is_variable_added); @@ -190,23 +190,23 @@ void Transaction::removeVariable(const UUID& variable_uuid) } } -void Transaction::merge(const Transaction& other, bool overwrite) +void Transaction::merge(Transaction const& other, bool overwrite) { stamp_ = std::max(stamp_, other.stamp_); involved_stamps_.insert(other.involved_stamps_.begin(), other.involved_stamps_.end()); - for (const auto& added_constraint : other.added_constraints_) + for (auto const& added_constraint : other.added_constraints_) { addConstraint(added_constraint, overwrite); } - for (const auto& removed_constraint : other.removed_constraints_) + for (auto const& removed_constraint : other.removed_constraints_) { removeConstraint(removed_constraint); } - for (const auto& added_variable : other.added_variables_) + for (auto const& added_variable : other.added_variables_) { addVariable(added_variable, overwrite); } - for (const auto& removed_variable : other.removed_variables_) + for (auto const& removed_variable : other.removed_variables_) { removeVariable(removed_variable); } @@ -216,27 +216,27 @@ void Transaction::print(std::ostream& stream) const { stream << "Stamp: " << stamp_.nanoseconds() << "\n"; stream << "Involved Timestamps:\n"; - for (const auto& involved_stamp : involved_stamps_) + for (auto const& involved_stamp : involved_stamps_) { stream << " - " << involved_stamp.nanoseconds() << "\n"; } stream << "Added Variables:\n"; - for (const auto& added_variable : added_variables_) + for (auto const& added_variable : added_variables_) { stream << " - " << *added_variable << "\n"; } stream << "Added Constraints:\n"; - for (const auto& added_constraint : added_constraints_) + for (auto const& added_constraint : added_constraints_) { stream << " - " << *added_constraint << "\n"; } stream << "Removed Variables:\n"; - for (const auto& removed_variable : removed_variables_) + for (auto const& removed_variable : removed_variables_) { stream << " - " << removed_variable << "\n"; } stream << "Removed Constraints:\n"; - for (const auto& removed_constraint : removed_constraints_) + for (auto const& removed_constraint : removed_constraints_) { stream << " - " << removed_constraint << "\n"; } @@ -267,7 +267,7 @@ void Transaction::deserialize(fuse_core::TextInputArchive& archive) archive >> *this; } -std::ostream& operator<<(std::ostream& stream, const Transaction& transaction) +std::ostream& operator<<(std::ostream& stream, Transaction const& transaction) { transaction.print(stream); return stream; diff --git a/fuse_core/src/transaction_deserializer.cpp b/fuse_core/src/transaction_deserializer.cpp index 9308b5c9f..3751f5d2d 100644 --- a/fuse_core/src/transaction_deserializer.cpp +++ b/fuse_core/src/transaction_deserializer.cpp @@ -38,7 +38,7 @@ namespace fuse_core { -void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg) +void serializeTransaction(fuse_core::Transaction const& transaction, fuse_msgs::msg::SerializedTransaction& msg) { // Serialize the transaction into the msg.data field boost::iostreams::stream stream(msg.data); @@ -58,22 +58,22 @@ TransactionDeserializer::TransactionDeserializer() // Load all known plugin libraries // I believe the library containing a given Variable or Constraint must be loaded in order to // deserialize an object of that type. But I haven't actually tested that theory. - for (const auto& class_name : variable_loader_.getDeclaredClasses()) + for (auto const& class_name : variable_loader_.getDeclaredClasses()) { variable_loader_.loadLibraryForClass(class_name); } - for (const auto& class_name : constraint_loader_.getDeclaredClasses()) + for (auto const& class_name : constraint_loader_.getDeclaredClasses()) { constraint_loader_.loadLibraryForClass(class_name); } - for (const auto& class_name : loss_loader_.getDeclaredClasses()) + for (auto const& class_name : loss_loader_.getDeclaredClasses()) { loss_loader_.loadLibraryForClass(class_name); } } fuse_core::Transaction::UniquePtr -TransactionDeserializer::deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const +TransactionDeserializer::deserialize(fuse_msgs::msg::SerializedTransaction const& msg) const { // The Transaction object is not a plugin and has no derived types. That makes it much easier to // use. diff --git a/fuse_core/src/uuid.cpp b/fuse_core/src/uuid.cpp index b06fb0908..8db7ccd86 100644 --- a/fuse_core/src/uuid.cpp +++ b/fuse_core/src/uuid.cpp @@ -60,9 +60,9 @@ UUID generate() return uuid; } -UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp) +UUID generate(std::string const& namespace_string, rclcpp::Time const& stamp) { - const auto nanoseconds = stamp.nanoseconds(); + auto const nanoseconds = stamp.nanoseconds(); constexpr size_t buffer_size = sizeof(nanoseconds); std::array buffer; @@ -77,9 +77,9 @@ UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp) return generate(namespace_string, buffer.data(), buffer.size()); } -UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id) +UUID generate(std::string const& namespace_string, rclcpp::Time const& stamp, const UUID& id) { - const auto nanoseconds = stamp.nanoseconds(); + auto const nanoseconds = stamp.nanoseconds(); constexpr size_t buffer_size = sizeof(nanoseconds) + UUID::static_size(); std::array buffer; @@ -97,9 +97,9 @@ UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, co return generate(namespace_string, buffer.data(), buffer.size()); } -UUID generate(const std::string& namespace_string, const uint64_t& user_id) +UUID generate(std::string const& namespace_string, uint64_t const& user_id) { - return generate(namespace_string, reinterpret_cast(&user_id), sizeof(user_id)); + return generate(namespace_string, reinterpret_cast(&user_id), sizeof(user_id)); } } // namespace uuid diff --git a/fuse_core/src/variable.cpp b/fuse_core/src/variable.cpp index 6b8f40749..de8bf385f 100644 --- a/fuse_core/src/variable.cpp +++ b/fuse_core/src/variable.cpp @@ -42,7 +42,7 @@ Variable::Variable(const UUID& uuid) : uuid_(uuid) { } -std::ostream& operator<<(std::ostream& stream, const Variable& variable) +std::ostream& operator<<(std::ostream& stream, Variable const& variable) { variable.print(stream); return stream; diff --git a/fuse_core/test/example_constraint.hpp b/fuse_core/test/example_constraint.hpp index 4df0ef035..e931045a1 100644 --- a/fuse_core/test/example_constraint.hpp +++ b/fuse_core/test/example_constraint.hpp @@ -56,13 +56,13 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - ExampleConstraint(const std::string& source, std::initializer_list variable_uuid_list) + ExampleConstraint(std::string const& source, std::initializer_list variable_uuid_list) : fuse_core::Constraint(source, variable_uuid_list), data(0.0) { } template - ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + ExampleConstraint(std::string const& source, VariableUuidIterator first, VariableUuidIterator last) : fuse_core::Constraint(source, first, last), data(0.0) { } @@ -89,7 +89,7 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data; diff --git a/fuse_core/test/example_loss.hpp b/fuse_core/test/example_loss.hpp index 2f8c11533..61f2f921d 100644 --- a/fuse_core/test/example_loss.hpp +++ b/fuse_core/test/example_loss.hpp @@ -52,14 +52,14 @@ class ExampleLoss : public fuse_core::Loss public: FUSE_LOSS_DEFINITIONS(ExampleLoss) - explicit ExampleLoss(const double a = 1.0) : a(a) + explicit ExampleLoss(double const a = 1.0) : a(a) { } void initialize( fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, - const std::string& /*name*/) override + std::string const& /*name*/) override { } @@ -86,7 +86,7 @@ class ExampleLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a; diff --git a/fuse_core/test/example_variable.hpp b/fuse_core/test/example_variable.hpp index 4cf04aa8b..13e66bbdc 100644 --- a/fuse_core/test/example_variable.hpp +++ b/fuse_core/test/example_variable.hpp @@ -61,7 +61,7 @@ class ExampleVariable : public fuse_core::Variable { return 1; } - const double* data() const override + double const* data() const override { return &data_; } @@ -99,7 +99,7 @@ class ExampleVariable : public fuse_core::Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data_; @@ -124,7 +124,7 @@ class LegacyParameterization : public fuse_core::LocalParameterization return 3; } - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + bool Plus(double const* x, double const* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); @@ -132,7 +132,7 @@ class LegacyParameterization : public fuse_core::LocalParameterization return true; } - bool ComputeJacobian(const double* x, double* jacobian) const override + bool ComputeJacobian(double const* x, double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; @@ -155,7 +155,7 @@ class LegacyParameterization : public fuse_core::LocalParameterization return true; } - bool Minus(const double* x, const double* y, double* y_minus_x) const override + bool Minus(double const* x, double const* y, double* y_minus_x) const override { double x_inverse[4]; x_inverse[0] = x[0]; @@ -169,7 +169,7 @@ class LegacyParameterization : public fuse_core::LocalParameterization return true; } - bool ComputeMinusJacobian(const double* x, double* jacobian) const override + bool ComputeMinusJacobian(double const* x, double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; @@ -204,7 +204,7 @@ class LegacyParameterization : public fuse_core::LocalParameterization * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } @@ -223,7 +223,7 @@ class LegacyVariable : public fuse_core::Variable { return 4; } - [[nodiscard]] const double* data() const override + [[nodiscard]] double const* data() const override { return static_cast(data_); } @@ -269,7 +269,7 @@ class LegacyVariable : public fuse_core::Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data_; diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp index aff635707..6c83e8ad4 100644 --- a/fuse_core/test/launch_tests/test_parameters.cpp +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -73,7 +73,7 @@ class TestParameters : public ::testing::Test TEST_F(TestParameters, getPositiveParam) { // Load parameters enforcing they are positive: - const double default_value{ 1.0 }; + double const default_value{ 1.0 }; auto node = rclcpp::Node::make_shared("test_parameters_node"); @@ -130,7 +130,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) try { - const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance); + auto const covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance); EXPECT_EQ(size, covariance.rows()); EXPECT_EQ(size, covariance.cols()); @@ -141,7 +141,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) << expected_covariance << "\nActual\n" << covariance; } - catch (const std::exception& ex) + catch (std::exception const& ex) { FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); } @@ -155,7 +155,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) try { - const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance); + auto const covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance); EXPECT_EQ(size, covariance.rows()); EXPECT_EQ(size, covariance.cols()); @@ -165,7 +165,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) << default_covariance << "\nActual\n" << covariance; } - catch (const std::exception& ex) + catch (std::exception const& ex) { FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); } diff --git a/fuse_core/test/test_callback_wrapper.cpp b/fuse_core/test/test_callback_wrapper.cpp index 502ff2491..d3e2ddb00 100644 --- a/fuse_core/test/test_callback_wrapper.cpp +++ b/fuse_core/test/test_callback_wrapper.cpp @@ -44,12 +44,12 @@ class MyClass { public: - double processData(const std::vector& data) + double processData(std::vector const& data) { return std::accumulate(data.begin(), data.end(), 0.0); } - void processDataInPlace(const std::vector& data, double& output) + void processDataInPlace(std::vector const& data, double& output) { output = std::accumulate(data.begin(), data.end(), 0.0); } diff --git a/fuse_core/test/test_eigen.cpp b/fuse_core/test/test_eigen.cpp index f28c9679a..a8fd9e843 100644 --- a/fuse_core/test/test_eigen.cpp +++ b/fuse_core/test/test_eigen.cpp @@ -37,16 +37,16 @@ TEST(Eigen, isSymmetric) { - const auto random_matrix = fuse_core::Matrix3d::Random().eval(); + auto const random_matrix = fuse_core::Matrix3d::Random().eval(); // A symmetric matrix: - const auto symmetric_matrix = (0.5 * (random_matrix + random_matrix.transpose())).eval(); + auto const symmetric_matrix = (0.5 * (random_matrix + random_matrix.transpose())).eval(); EXPECT_TRUE(fuse_core::isSymmetric(symmetric_matrix)) << "Matrix\n" << symmetric_matrix << "\n expected to be symmetric."; // A non-symmetric matrix: - const double asymmetry_error = 1.0e-6; + double const asymmetry_error = 1.0e-6; auto non_symmetric_matrix = symmetric_matrix; non_symmetric_matrix(0, 1) += asymmetry_error; @@ -56,7 +56,7 @@ TEST(Eigen, isSymmetric) << non_symmetric_matrix << "\n expected to not be symmetric."; // Checking symmetry with precision larger than asymmetry error in non-symmetric matrix: - const double precision = 1.0e2 * asymmetry_error; + double const precision = 1.0e2 * asymmetry_error; EXPECT_TRUE(fuse_core::isSymmetric(non_symmetric_matrix, precision)) << "Matrix\n" @@ -72,11 +72,11 @@ TEST(Eigen, isSymmetric) TEST(Eigen, isPositiveDefinite) { - const auto random_matrix = fuse_core::Matrix3d::Random().eval(); + auto const random_matrix = fuse_core::Matrix3d::Random().eval(); // A Positive Definite matrix: - const auto symmetric_matrix = (0.5 * (random_matrix + random_matrix.transpose())).eval(); - const auto psd_matrix = (symmetric_matrix + 3 * fuse_core::Matrix3d::Identity()).eval(); + auto const symmetric_matrix = (0.5 * (random_matrix + random_matrix.transpose())).eval(); + auto const psd_matrix = (symmetric_matrix + 3 * fuse_core::Matrix3d::Identity()).eval(); EXPECT_TRUE(fuse_core::isPositiveDefinite(psd_matrix)) << "Matrix\n" << psd_matrix << "\n expected to be Positive Definite."; diff --git a/fuse_core/test/test_local_parameterization.cpp b/fuse_core/test/test_local_parameterization.cpp index bf5c7185e..f2a63c3c3 100644 --- a/fuse_core/test/test_local_parameterization.cpp +++ b/fuse_core/test/test_local_parameterization.cpp @@ -147,8 +147,8 @@ TEST(LocalParameterization, PlusMinus) { TestLocalParameterization parameterization; - const double x1[3] = { 1.0, 2.0, 3.0 }; - const double delta[2] = { 0.5, 1.0 }; + double const x1[3] = { 1.0, 2.0, 3.0 }; + double const delta[2] = { 0.5, 1.0 }; double x2[3] = { 0.0, 0.0, 0.0 }; bool success = parameterization.Plus(x1, delta, x2); diff --git a/fuse_core/test/test_loss.cpp b/fuse_core/test/test_loss.cpp index 4628836ee..c572c2fc2 100644 --- a/fuse_core/test/test_loss.cpp +++ b/fuse_core/test/test_loss.cpp @@ -37,7 +37,7 @@ TEST(Loss, Constructor) { - const double a{ 0.3 }; + double const a{ 0.3 }; ExampleLoss loss(a); ASSERT_EQ(a, loss.a); diff --git a/fuse_core/test/test_throttled_callback.cpp b/fuse_core/test/test_throttled_callback.cpp index 844e9472a..a257e801d 100644 --- a/fuse_core/test/test_throttled_callback.cpp +++ b/fuse_core/test/test_throttled_callback.cpp @@ -52,7 +52,7 @@ class PointPublisher : public rclcpp::Node * * @param[in] frequency The publishing frequency in Hz */ - explicit PointPublisher(const double frequency) : Node("point_publisher_node"), frequency_(frequency) + explicit PointPublisher(double const frequency) : Node("point_publisher_node"), frequency_(frequency) { publisher_ = this->create_publisher("point", 1); } @@ -115,14 +115,14 @@ class PointSensorModel : public rclcpp::Node * * @param[in] throttle_period The throttle period duration in seconds */ - explicit PointSensorModel(const rclcpp::Duration& throttle_period) + explicit PointSensorModel(rclcpp::Duration const& throttle_period) : Node("point_sensor_model_node") , throttled_callback_(std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1), std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1), throttle_period) { subscription_ = this->create_subscription( "point", 10, - std::bind(&PointThrottledCallback::callback, &throttled_callback_, + std::bind(&PointThrottledCallback::callback, &throttled_callback_, std::placeholders::_1)); } @@ -173,7 +173,7 @@ class PointSensorModel : public rclcpp::Node * * @param[in] msg A geometry_msgs::msg::Point message */ - void keepCallback(const geometry_msgs::msg::Point& msg) + void keepCallback(geometry_msgs::msg::Point const& msg) { ++kept_messages_; last_kept_message_ = std::make_shared(msg); @@ -185,7 +185,7 @@ class PointSensorModel : public rclcpp::Node * @param[in] msg A geometry_msgs::msg::Point message (not used) */ // NOTE(CH3): The msg arg here is necessary to allow binding the throttled callback - void dropCallback(const geometry_msgs::msg::Point& /*msg*/) + void dropCallback(geometry_msgs::msg::Point const& /*msg*/) { ++dropped_messages_; } @@ -239,7 +239,7 @@ TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero) // Publish some messages: const size_t num_messages = 10; - const double frequency = 10.0; + double const frequency = 10.0; auto publisher = std::make_shared(frequency); executor_->add_node(publisher); @@ -263,17 +263,17 @@ TEST_F(TestThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPe // Publish some messages at half the throttled period: const size_t num_messages = 10; - const double period_factor = 0.25; - const double period = period_factor * throttled_period.seconds(); - const double frequency = 1.0 / period; + double const period_factor = 0.25; + double const period = period_factor * throttled_period.seconds(); + double const frequency = 1.0 / period; auto publisher = std::make_shared(frequency); executor_->add_node(publisher); publisher->publish(num_messages); // Check the number of kept and dropped callbacks: - const auto expected_kept_messages = period_factor * num_messages; - const auto expected_dropped_messages = num_messages - expected_kept_messages; + auto const expected_kept_messages = period_factor * num_messages; + auto const expected_dropped_messages = num_messages - expected_kept_messages; EXPECT_NEAR(expected_kept_messages, sensor_model->getKeptMessages(), 1.0); EXPECT_NEAR(expected_dropped_messages, sensor_model->getDroppedMessages(), 1.0); @@ -293,8 +293,8 @@ TEST_F(TestThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLar // Publish some messages: const size_t num_messages = 10; - const double period = 0.1 * num_messages / throttled_period.seconds(); - const double frequency = 1.0 / period; + double const period = 0.1 * num_messages / throttled_period.seconds(); + double const frequency = 1.0 / period; auto publisher = std::make_shared(frequency); publisher->publish(num_messages); @@ -305,7 +305,7 @@ TEST_F(TestThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLar EXPECT_EQ(num_messages - 1u, sensor_model->getDroppedMessages()); // Check the message kept was the first message: - const auto last_kept_message = sensor_model->getLastKeptMessage(); + auto const last_kept_message = sensor_model->getLastKeptMessage(); ASSERT_NE(nullptr, last_kept_message); EXPECT_EQ(0.0, last_kept_message->x); } diff --git a/fuse_core/test/test_timestamp_manager.cpp b/fuse_core/test/test_timestamp_manager.cpp index 2759721d5..ef0ec6cf9 100644 --- a/fuse_core/test/test_timestamp_manager.cpp +++ b/fuse_core/test/test_timestamp_manager.cpp @@ -69,7 +69,7 @@ class TimestampManagerTestFixture : public ::testing::Test generated_time_spans.clear(); } - void generator(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + void generator(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& /*constraints*/, std::vector& /*variables*/) { diff --git a/fuse_core/test/test_transaction.cpp b/fuse_core/test/test_transaction.cpp index 7f60fa279..0da35a647 100644 --- a/fuse_core/test/test_transaction.cpp +++ b/fuse_core/test/test_transaction.cpp @@ -59,7 +59,7 @@ using fuse_core::UUID; * transaction, False otherwise. */ template -bool testInvolvedStamps(const TimeRange& expected, const Transaction& transaction) +bool testInvolvedStamps(TimeRange const& expected, Transaction const& transaction) { auto range = transaction.involvedStamps(); if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) @@ -69,10 +69,10 @@ bool testInvolvedStamps(const TimeRange& expected, const Transaction& transactio for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto& actual_stamp = *iter; + auto const& actual_stamp = *iter; bool found = false; - for (const auto& expected_stamp : expected) + for (auto const& expected_stamp : expected) { if (actual_stamp == expected_stamp) { @@ -105,7 +105,7 @@ bool testInvolvedStamps(const TimeRange& expected, const Transaction& transactio * exist in the transaction, False otherwise. */ template -bool testAddedConstraints(const ConstraintRange& expected, const Transaction& transaction) +bool testAddedConstraints(ConstraintRange const& expected, Transaction const& transaction) { auto range = transaction.addedConstraints(); if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) @@ -115,10 +115,10 @@ bool testAddedConstraints(const ConstraintRange& expected, const Transaction& tr for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto& actual_constraint = dynamic_cast(*iter); + auto const& actual_constraint = dynamic_cast(*iter); bool found = false; - for (const auto& expected_constraint : expected) + for (auto const& expected_constraint : expected) { if (actual_constraint.uuid() == expected_constraint.uuid()) { @@ -130,7 +130,7 @@ bool testAddedConstraints(const ConstraintRange& expected, const Transaction& tr { is_equal = is_equal && (expected_constraint.variables().at(i) == actual_constraint.variables().at(i)); } - const auto& expected_derived = dynamic_cast(expected_constraint); + auto const& expected_derived = dynamic_cast(expected_constraint); is_equal = is_equal && (expected_derived.data == actual_constraint.data); if (!is_equal) @@ -165,7 +165,7 @@ bool testAddedConstraints(const ConstraintRange& expected, const Transaction& tr * the transaction, False otherwise. */ template -bool testRemovedConstraints(const UuidRange& expected, const Transaction& transaction) +bool testRemovedConstraints(UuidRange const& expected, Transaction const& transaction) { auto range = transaction.removedConstraints(); if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) @@ -175,10 +175,10 @@ bool testRemovedConstraints(const UuidRange& expected, const Transaction& transa for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto& actual_constraint_uuid = *iter; + auto const& actual_constraint_uuid = *iter; bool found = false; - for (const auto& expected_constraint_uuid : expected) + for (auto const& expected_constraint_uuid : expected) { if (actual_constraint_uuid == expected_constraint_uuid) { @@ -210,7 +210,7 @@ bool testRemovedConstraints(const UuidRange& expected, const Transaction& transa * the transaction, False otherwise. */ template -bool testAddedVariables(const VariableRange& expected, const Transaction& transaction) +bool testAddedVariables(VariableRange const& expected, Transaction const& transaction) { auto range = transaction.addedVariables(); if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) @@ -220,10 +220,10 @@ bool testAddedVariables(const VariableRange& expected, const Transaction& transa for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto& actual_variable = dynamic_cast(*iter); + auto const& actual_variable = dynamic_cast(*iter); bool found = false; - for (const auto& expected_variable : expected) + for (auto const& expected_variable : expected) { if (actual_variable.uuid() == expected_variable.uuid()) { @@ -264,7 +264,7 @@ bool testAddedVariables(const VariableRange& expected, const Transaction& transa * transaction, False otherwise. */ template -bool testRemovedVariables(const UuidRange& expected, const Transaction& transaction) +bool testRemovedVariables(UuidRange const& expected, Transaction const& transaction) { auto range = transaction.removedVariables(); if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) @@ -274,10 +274,10 @@ bool testRemovedVariables(const UuidRange& expected, const Transaction& transact for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto& actual_variable_uuid = *iter; + auto const& actual_variable_uuid = *iter; bool found = false; - for (const auto& expected_variable_uuid : expected) + for (auto const& expected_variable_uuid : expected) { if (actual_variable_uuid == expected_variable_uuid) { @@ -307,8 +307,8 @@ TEST(Transaction, Empty) // A transaction with added constraints cannot be empty { - const auto variable_uuid = fuse_core::uuid::generate(); - const auto constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); + auto const variable_uuid = fuse_core::uuid::generate(); + auto const constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); Transaction transaction; transaction.addConstraint(constraint); @@ -318,7 +318,7 @@ TEST(Transaction, Empty) // A transaction with removed constraints cannot be empty { - const auto constraint_uuid = fuse_core::uuid::generate(); + auto const constraint_uuid = fuse_core::uuid::generate(); Transaction transaction; transaction.removeConstraint(constraint_uuid); @@ -328,7 +328,7 @@ TEST(Transaction, Empty) // A transaction with added variables cannot be empty { - const auto variable = ExampleVariable::make_shared(); + auto const variable = ExampleVariable::make_shared(); Transaction transaction; transaction.addVariable(variable); @@ -338,7 +338,7 @@ TEST(Transaction, Empty) // A transaction with removed variables cannot be empty { - const auto variable_uuid = fuse_core::uuid::generate(); + auto const variable_uuid = fuse_core::uuid::generate(); Transaction transaction; transaction.removeVariable(variable_uuid); diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 4aa9efd0d..6e51115fa 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -94,31 +94,31 @@ TEST(Util, wrapAngle2D) { // Wrap angle already in [-Pi, +Pi) range { - const double angle = 0.5; + double const angle = 0.5; EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle)); } // Wrap angle equal to +Pi { - const double angle = M_PI; + double const angle = M_PI; EXPECT_EQ(-angle, fuse_core::wrapAngle2D(angle)); } // Wrap angle equal to -Pi { - const double angle = -M_PI; + double const angle = -M_PI; EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle)); } // Wrap angle greater than +Pi { - const double angle = 0.5; + double const angle = 0.5; EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle + 3.0 * 2.0 * M_PI)); } // Wrap angle smaller than -Pi { - const double angle = 0.5; + double const angle = 0.5; EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle - 3.0 * 2.0 * M_PI)); } diff --git a/fuse_core/test/test_uuid.cpp b/fuse_core/test/test_uuid.cpp index 5f6225c74..694cec26f 100644 --- a/fuse_core/test/test_uuid.cpp +++ b/fuse_core/test/test_uuid.cpp @@ -183,7 +183,7 @@ TEST(UUID, CollisionSingleThread) // Check for duplicates std::unordered_set unique_uuids; - for (const auto& uuid : raw_uuids) + for (auto const& uuid : raw_uuids) { ASSERT_TRUE(unique_uuids.find(uuid) == unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); unique_uuids.insert(uuid); @@ -209,7 +209,7 @@ TEST(UUID, CollisionManyThreads) std::unordered_set unique_uuids; for (size_t i = 0; i < raw_uuids.size(); ++i) { - for (const auto& uuid : raw_uuids[i]) + for (auto const& uuid : raw_uuids[i]) { ASSERT_TRUE(unique_uuids.find(uuid) == unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); unique_uuids.insert(uuid); diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index d2eb4cc34..dd1581a8a 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -82,7 +82,7 @@ TEST(LegacyVariable, Serialization) #if CERES_SUPPORTS_MANIFOLDS struct QuaternionCostFunction { - explicit QuaternionCostFunction(const double* observation) + explicit QuaternionCostFunction(double const* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; diff --git a/fuse_graphs/benchmark/benchmark_create_problem.cpp b/fuse_graphs/benchmark/benchmark_create_problem.cpp index eb1fdc7d5..6f7c48221 100644 --- a/fuse_graphs/benchmark/benchmark_create_problem.cpp +++ b/fuse_graphs/benchmark/benchmark_create_problem.cpp @@ -63,7 +63,7 @@ class TestableHashGraph : public fuse_graphs::HashGraph class ExampleFunctor { public: - explicit ExampleFunctor(const std::vector& b) : b_(b) + explicit ExampleFunctor(std::vector const& b) : b_(b) { } @@ -92,7 +92,7 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; template - explicit ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + explicit ExampleConstraint(std::string const& source, VariableUuidIterator first, VariableUuidIterator last) : fuse_core::Constraint(source, first, last), data(std::distance(first, last), 0.0) { } @@ -128,7 +128,7 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data; @@ -157,7 +157,7 @@ TestableHashGraph makeTestableHashGraph(const size_t num_constraints, const size []() { return ExampleVariable::make_shared(); }); // NOLINT // Add variables to the graph - for (const auto& variable : variables) + for (auto const& variable : variables) { graph.addVariable(variable); } @@ -166,7 +166,7 @@ TestableHashGraph makeTestableHashGraph(const size_t num_constraints, const size std::vector variable_uuids; variable_uuids.reserve(variables.size()); std::transform(variables.begin(), variables.end(), std::back_inserter(variable_uuids), - [](const auto& variable) { return variable->uuid(); }); // NOLINT + [](auto const& variable) { return variable->uuid(); }); // NOLINT graph.addConstraint(ExampleConstraint::make_shared("test", variable_uuids.begin(), variable_uuids.end())); } @@ -176,7 +176,7 @@ TestableHashGraph makeTestableHashGraph(const size_t num_constraints, const size static void BM_createProblem(benchmark::State& state) { - const auto graph = makeTestableHashGraph(state.range(0), state.range(1)); + auto const graph = makeTestableHashGraph(state.range(0), state.range(1)); ceres::Problem problem; diff --git a/fuse_graphs/doc/graphs.md b/fuse_graphs/doc/graphs.md new file mode 100644 index 000000000..ed1bb09ed --- /dev/null +++ b/fuse_graphs/doc/graphs.md @@ -0,0 +1,3 @@ +# Graphs + +[Hash Graphs](./hash_graphs.md) diff --git a/fuse_graphs/doc/hash_graphs.md b/fuse_graphs/doc/hash_graphs.md new file mode 100644 index 000000000..dfd5ecf0b --- /dev/null +++ b/fuse_graphs/doc/hash_graphs.md @@ -0,0 +1,3 @@ +# Hash Graphs + +[Coming soon](https://github.com/PickNikRobotics/fuse/issues/23) diff --git a/fuse_graphs/include/fuse_graphs/hash_graph.hpp b/fuse_graphs/include/fuse_graphs/hash_graph.hpp index 6b33df7dd..c0d49c4d5 100644 --- a/fuse_graphs/include/fuse_graphs/hash_graph.hpp +++ b/fuse_graphs/include/fuse_graphs/hash_graph.hpp @@ -85,14 +85,14 @@ class HashGraph : public fuse_core::Graph * * @param[in] params HashGraph parameters. */ - explicit HashGraph(const HashGraphParams& params = HashGraphParams()); + explicit HashGraph(HashGraphParams const& params = HashGraphParams()); /** * @brief Copy constructor * * Performs a deep copy of the graph */ - HashGraph(const HashGraph& other); + HashGraph(HashGraph const& other); /** * @brief Destructor @@ -104,7 +104,7 @@ class HashGraph : public fuse_core::Graph * * Performs a deep copy of the graph */ - HashGraph& operator=(const HashGraph& other); + HashGraph& operator=(HashGraph const& other); /** * @brief Clear all variables and constraints from the graph object. @@ -129,7 +129,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the constraint being searched for * @return True if this constraint already exists, False otherwise */ - bool constraintExists(const fuse_core::UUID& constraint_uuid) const noexcept override; + bool constraintExists(fuse_core::UUID const& constraint_uuid) const noexcept override; /** * @brief Add a new constraint to the graph @@ -159,7 +159,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the constraint to be removed * @return True if the constraint was removed, false otherwise */ - bool removeConstraint(const fuse_core::UUID& constraint_uuid) override; + bool removeConstraint(fuse_core::UUID const& constraint_uuid) override; /** * @brief Read-only access to a constraint from the graph by UUID @@ -171,7 +171,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the requested constraint * @return The constraint in the graph with the specified UUID */ - const fuse_core::Constraint& getConstraint(const fuse_core::UUID& constraint_uuid) const override; + fuse_core::Constraint const& getConstraint(fuse_core::UUID const& constraint_uuid) const override; /** * @brief Read-only access to all of the constraints in the graph @@ -193,7 +193,7 @@ class HashGraph : public fuse_core::Graph * @return A read-only iterator range containing all constraints that involve the specified * variable */ - fuse_core::Graph::const_constraint_range getConnectedConstraints(const fuse_core::UUID& variable_uuid) const override; + fuse_core::Graph::const_constraint_range getConnectedConstraints(fuse_core::UUID const& variable_uuid) const override; /** * @brief Check if the variable already exists in the graph @@ -204,7 +204,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the variable being searched for * @return True if this variable already exists, False otherwise */ - bool variableExists(const fuse_core::UUID& variable_uuid) const noexcept override; + bool variableExists(fuse_core::UUID const& variable_uuid) const noexcept override; /** * @brief Add a new variable to the graph @@ -232,7 +232,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the variable to be removed * @return True if the variable was removed, false otherwise */ - bool removeVariable(const fuse_core::UUID& variable_uuid) override; + bool removeVariable(fuse_core::UUID const& variable_uuid) override; /** * @brief Read-only access to a variable in the graph by UUID @@ -243,7 +243,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the requested variable * @return The variable in the graph with the specified UUID */ - const fuse_core::Variable& getVariable(const fuse_core::UUID& variable_uuid) const override; + fuse_core::Variable const& getVariable(fuse_core::UUID const& variable_uuid) const override; /** * @brief Read-only access to all of the variables in the graph @@ -272,7 +272,7 @@ class HashGraph : public fuse_core::Graph * optimization, or if the variable's value is allowed to change during * optimization. */ - void holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant = true) override; + void holdVariable(fuse_core::UUID const& variable_uuid, bool hold_constant = true) override; /** * @brief Check whether a variable is on hold or not @@ -280,7 +280,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The variable to test * @return True if the variable is on hold, false otherwise */ - bool isVariableOnHold(const fuse_core::UUID& variable_uuid) const override; + bool isVariableOnHold(fuse_core::UUID const& variable_uuid) const override; /** * @brief Compute the marginal covariance blocks for the requested set of variable pairs. @@ -307,10 +307,10 @@ class HashGraph : public fuse_core::Graph * variable's tangent space/local coordinates. Otherwise it is * computed in the variable's parameter space. */ - void getCovariance(const std::vector>& covariance_requests, + void getCovariance(std::vector> const& covariance_requests, std::vector>& covariance_matrices, - const ceres::Covariance::Options& options = ceres::Covariance::Options(), - const bool use_tangent_space = true) const override; + ceres::Covariance::Options const& options = ceres::Covariance::Options(), + bool const use_tangent_space = true) const override; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -328,7 +328,7 @@ class HashGraph : public fuse_core::Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) override; + ceres::Solver::Summary optimize(ceres::Solver::Options const& options = ceres::Solver::Options()) override; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -345,8 +345,8 @@ class HashGraph : public fuse_core::Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - ceres::Solver::Summary optimizeFor(const rclcpp::Duration& max_optimization_time, - const ceres::Solver::Options& options = ceres::Solver::Options(), + ceres::Solver::Summary optimizeFor(rclcpp::Duration const& max_optimization_time, + ceres::Solver::Options const& options = ceres::Solver::Options(), rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) override; /** @@ -373,7 +373,7 @@ class HashGraph : public fuse_core::Graph * @return True if the problem evaluation was successful; False, otherwise. */ bool evaluate(double* cost, std::vector* residuals = nullptr, std::vector* gradient = nullptr, - const ceres::Problem::EvaluateOptions& options = ceres::Problem::EvaluateOptions()) const override; + ceres::Problem::EvaluateOptions const& options = ceres::Problem::EvaluateOptions()) const override; /** * @brief Print a human-readable description of the graph to the provided stream. @@ -418,7 +418,7 @@ class HashGraph : public fuse_core::Graph * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& constraints_; @@ -440,7 +440,7 @@ namespace serialization * @brief Serialize a ceres::Problem::Options object using Boost Serialization */ template -void serialize(Archive& archive, ceres::Problem::Options& options, const unsigned int /* version */) +void serialize(Archive& archive, ceres::Problem::Options& options, unsigned int const /* version */) { archive& options.cost_function_ownership; archive& options.disable_all_safety_checks; diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index b99ae0aeb..c99a48cba 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -50,30 +50,30 @@ namespace fuse_graphs { -HashGraph::HashGraph(const HashGraphParams& params) : problem_options_(params.problem_options) +HashGraph::HashGraph(HashGraphParams const& params) : problem_options_(params.problem_options) { // Set Ceres loss function ownership according to the fuse_core::Loss specification problem_options_.loss_function_ownership = fuse_core::Loss::Ownership; } -HashGraph::HashGraph(const HashGraph& other) +HashGraph::HashGraph(HashGraph const& other) : constraints_by_variable_uuid_(other.constraints_by_variable_uuid_) , problem_options_(other.problem_options_) , variables_on_hold_(other.variables_on_hold_) { // Make a deep copy of the constraints std::transform(other.constraints_.begin(), other.constraints_.end(), std::inserter(constraints_, constraints_.end()), - [](const Constraints::value_type& uuid_constraint) -> Constraints::value_type { + [](Constraints::value_type const& uuid_constraint) -> Constraints::value_type { return { uuid_constraint.first, uuid_constraint.second->clone() }; }); // NOLINT(whitespace/braces) // Make a deep copy of the variables std::transform(other.variables_.begin(), other.variables_.end(), std::inserter(variables_, variables_.end()), - [](const Variables::value_type& uuid_variable) -> Variables::value_type { + [](Variables::value_type const& uuid_variable) -> Variables::value_type { return { uuid_variable.first, uuid_variable.second->clone() }; }); // NOLINT(whitespace/braces) } -HashGraph& HashGraph::operator=(const HashGraph& other) +HashGraph& HashGraph::operator=(HashGraph const& other) { // Make a copy (might throw an exception) HashGraph tmp(other); @@ -99,7 +99,7 @@ fuse_core::Graph::UniquePtr HashGraph::clone() const return HashGraph::make_unique(*this); } -bool HashGraph::constraintExists(const fuse_core::UUID& constraint_uuid) const noexcept +bool HashGraph::constraintExists(fuse_core::UUID const& constraint_uuid) const noexcept { // map.find() does not itself throw exceptions, but may as a result of the key comparison // operator. Because the UUID comparisons are marked as noexcept by Boost, I feel safe marking @@ -116,7 +116,7 @@ bool HashGraph::addConstraint(fuse_core::Constraint::SharedPtr constraint) return false; } // Check that all of the referenced variables exist. Throw a logic_error if they do not. - for (const auto& variable_uuid : constraint->variables()) + for (auto const& variable_uuid : constraint->variables()) { if (!variableExists(variable_uuid)) { @@ -127,14 +127,14 @@ bool HashGraph::addConstraint(fuse_core::Constraint::SharedPtr constraint) // Add the constraint to the list of known constraints constraints_.emplace(constraint->uuid(), constraint); // Also add it to the variable-constraint cross reference - for (const auto& variable_uuid : constraint->variables()) + for (auto const& variable_uuid : constraint->variables()) { constraints_by_variable_uuid_[variable_uuid].push_back(constraint->uuid()); } return true; } -bool HashGraph::removeConstraint(const fuse_core::UUID& constraint_uuid) +bool HashGraph::removeConstraint(fuse_core::UUID const& constraint_uuid) { // Check if the constraint exists auto constraints_iter = constraints_.find(constraint_uuid); @@ -143,7 +143,7 @@ bool HashGraph::removeConstraint(const fuse_core::UUID& constraint_uuid) return false; } // Remove the constraint from the cross-reference data structure - for (const auto& variable_uuid : constraints_iter->second->variables()) + for (auto const& variable_uuid : constraints_iter->second->variables()) { auto& constraints = constraints_by_variable_uuid_.at(variable_uuid); constraints.erase(std::remove(constraints.begin(), constraints.end(), constraint_uuid), constraints.end()); @@ -153,7 +153,7 @@ bool HashGraph::removeConstraint(const fuse_core::UUID& constraint_uuid) return true; } -const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& constraint_uuid) const +fuse_core::Constraint const& HashGraph::getConstraint(fuse_core::UUID const& constraint_uuid) const { auto constraints_iter = constraints_.find(constraint_uuid); if (constraints_iter == constraints_.end()) @@ -165,8 +165,8 @@ const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& con fuse_core::Graph::const_constraint_range HashGraph::getConstraints() const noexcept { - std::function const to_constraint_ref = - [](const Constraints::value_type& uuid_constraint) -> const fuse_core::Constraint& { + std::function const to_constraint_ref = + [](Constraints::value_type const& uuid_constraint) -> fuse_core::Constraint const& { return *uuid_constraint.second; }; @@ -174,17 +174,17 @@ fuse_core::Graph::const_constraint_range HashGraph::getConstraints() const noexc boost::make_transform_iterator(constraints_.cend(), to_constraint_ref) }; } -fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(const fuse_core::UUID& variable_uuid) const +fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(fuse_core::UUID const& variable_uuid) const { auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); if (cross_reference_iter != constraints_by_variable_uuid_.end()) { - std::function const uuid_to_constraint_ref = - [this](const fuse_core::UUID& constraint_uuid) -> const fuse_core::Constraint& { + std::function const uuid_to_constraint_ref = + [this](fuse_core::UUID const& constraint_uuid) -> fuse_core::Constraint const& { return this->getConstraint(constraint_uuid); }; - const auto& constraints = cross_reference_iter->second; + auto const& constraints = cross_reference_iter->second; return { boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref) }; } @@ -201,7 +201,7 @@ fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(cons "), but that variable does not exist in this graph."); } -bool HashGraph::variableExists(const fuse_core::UUID& variable_uuid) const noexcept +bool HashGraph::variableExists(fuse_core::UUID const& variable_uuid) const noexcept { auto variables_iter = variables_.find(variable_uuid); return variables_iter != variables_.end(); @@ -222,7 +222,7 @@ bool HashGraph::addVariable(fuse_core::Variable::SharedPtr variable) return true; } -bool HashGraph::removeVariable(const fuse_core::UUID& variable_uuid) +bool HashGraph::removeVariable(fuse_core::UUID const& variable_uuid) { // Check if the variable exists auto variables_iter = variables_.find(variable_uuid); @@ -250,7 +250,7 @@ bool HashGraph::removeVariable(const fuse_core::UUID& variable_uuid) return true; } -const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variable_uuid) const +fuse_core::Variable const& HashGraph::getVariable(fuse_core::UUID const& variable_uuid) const { auto variables_iter = variables_.find(variable_uuid); if (variables_iter == variables_.end()) @@ -262,14 +262,14 @@ const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variabl fuse_core::Graph::const_variable_range HashGraph::getVariables() const noexcept { - std::function const to_variable_ref = - [](const Variables::value_type& uuid_variable) -> const fuse_core::Variable& { return *uuid_variable.second; }; + std::function const to_variable_ref = + [](Variables::value_type const& uuid_variable) -> fuse_core::Variable const& { return *uuid_variable.second; }; return { boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), boost::make_transform_iterator(variables_.cend(), to_variable_ref) }; } -void HashGraph::holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant) +void HashGraph::holdVariable(fuse_core::UUID const& variable_uuid, bool hold_constant) { // Adjust the variable setting in the Ceres Problem object if (hold_constant) @@ -282,14 +282,14 @@ void HashGraph::holdVariable(const fuse_core::UUID& variable_uuid, bool hold_con } } -bool HashGraph::isVariableOnHold(const fuse_core::UUID& variable_uuid) const +bool HashGraph::isVariableOnHold(fuse_core::UUID const& variable_uuid) const { return variables_on_hold_.find(variable_uuid) != variables_on_hold_.end(); } -void HashGraph::getCovariance(const std::vector>& covariance_requests, +void HashGraph::getCovariance(std::vector> const& covariance_requests, std::vector>& covariance_matrices, - const ceres::Covariance::Options& options, const bool use_tangent_space) const + ceres::Covariance::Options const& options, bool const use_tangent_space) const { // Avoid doing a bunch of work if the request is empty if (covariance_requests.empty()) @@ -302,8 +302,8 @@ void HashGraph::getCovariance(const std::vector& x, - const std::pair& y) { + auto symmetric_equal = [](std::pair const& x, + std::pair const& y) { return ((x.first == y.first) && (x.second == y.second)) || ((x.first == y.second) && (x.second == y.first)); }; // Convert the covariance requests into the input structure needed by Ceres. Namely, we must @@ -311,13 +311,13 @@ void HashGraph::getCovariance(const std::vector> unique_covariance_blocks; - std::vector> all_covariance_blocks; + std::vector> unique_covariance_blocks; + std::vector> all_covariance_blocks; all_covariance_blocks.resize(covariance_requests.size()); covariance_matrices.resize(covariance_requests.size()); for (size_t i = 0; i < covariance_requests.size(); ++i) { - const auto& request = covariance_requests.at(i); + auto const& request = covariance_requests.at(i); auto variable1_iter = variables_.find(request.first); if (variable1_iter == variables_.end()) { @@ -363,11 +363,11 @@ void HashGraph::getCovariance(const std::vector* residuals, std::vector* gradient, - const ceres::Problem::EvaluateOptions& options) const + ceres::Problem::EvaluateOptions const& options) const { ceres::Problem problem(problem_options_); createProblem(problem); @@ -439,14 +439,14 @@ void HashGraph::print(std::ostream& stream) const { stream << "HashGraph\n" << " constraints:\n"; - for (const auto& constraint : constraints_) + for (auto const& constraint : constraints_) { stream << " - " << *constraint.second << "\n"; } stream << " variables:\n"; - for (const auto& variable : variables_) + for (auto const& variable : variables_) { - const auto is_on_hold = variables_on_hold_.find(variable.first) != variables_on_hold_.end(); + auto const is_on_hold = variables_on_hold_.find(variable.first) != variables_on_hold_.end(); stream << " - " << *variable.second << "\n" << " on_hold: " << std::boolalpha << is_on_hold << "\n"; @@ -493,7 +493,7 @@ void HashGraph::createProblem(ceres::Problem& problem) const // We need the memory address of each variable value referenced by this constraint parameter_blocks.clear(); parameter_blocks.reserve(constraint.variables().size()); - for (const auto& uuid : constraint.variables()) + for (auto const& uuid : constraint.variables()) { parameter_blocks.push_back(variables_.at(uuid)->data()); } diff --git a/fuse_graphs/test/covariance_constraint.hpp b/fuse_graphs/test/covariance_constraint.hpp index a63a9cf09..658470a26 100644 --- a/fuse_graphs/test/covariance_constraint.hpp +++ b/fuse_graphs/test/covariance_constraint.hpp @@ -85,19 +85,19 @@ class CovarianceCostFunction : public ceres::CostFunction { if (jacobians[0] != NULL) { - static const double jacobian0[] = { 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + static double const jacobian0[] = { 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.0, -6.0, 3.0, -2.0 }; std::copy(jacobian0, jacobian0 + 16, jacobians[0]); } if (jacobians[1] != NULL) { - static const double jacobian1[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, + static double const jacobian1[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0 }; std::copy(jacobian1, jacobian1 + 24, jacobians[1]); } if (jacobians[2] != NULL) { - static const double jacobian2[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 2.0 }; + static double const jacobian2[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 2.0 }; std::copy(jacobian2, jacobian2 + 8, jacobians[2]); } } @@ -116,8 +116,8 @@ class CovarianceConstraint : public fuse_core::Constraint CovarianceConstraint() = default; - CovarianceConstraint(const std::string& source, const fuse_core::UUID& variable1_uuid, - const fuse_core::UUID& variable2_uuid, const fuse_core::UUID& variable3_uuid) + CovarianceConstraint(std::string const& source, fuse_core::UUID const& variable1_uuid, + fuse_core::UUID const& variable2_uuid, fuse_core::UUID const& variable3_uuid) : fuse_core::Constraint(source, { variable1_uuid, variable2_uuid, variable3_uuid }) { } @@ -142,7 +142,7 @@ class CovarianceConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } diff --git a/fuse_graphs/test/example_constraint.hpp b/fuse_graphs/test/example_constraint.hpp index bbaa07937..b23d74088 100644 --- a/fuse_graphs/test/example_constraint.hpp +++ b/fuse_graphs/test/example_constraint.hpp @@ -53,7 +53,7 @@ class ExampleFunctor { public: - explicit ExampleFunctor(const double& b) : b_(b) + explicit ExampleFunctor(double const& b) : b_(b) { } @@ -78,7 +78,7 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - explicit ExampleConstraint(const std::string& source, const fuse_core::UUID& variable_uuid) + explicit ExampleConstraint(std::string const& source, fuse_core::UUID const& variable_uuid) : fuse_core::Constraint(source, { variable_uuid }) , // NOLINT data(0.0) @@ -107,7 +107,7 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data; diff --git a/fuse_graphs/test/example_loss.hpp b/fuse_graphs/test/example_loss.hpp index fdc8ea883..846b15868 100644 --- a/fuse_graphs/test/example_loss.hpp +++ b/fuse_graphs/test/example_loss.hpp @@ -53,14 +53,14 @@ class ExampleLoss : public fuse_core::Loss public: FUSE_LOSS_DEFINITIONS(ExampleLoss) - explicit ExampleLoss(const double a = 1.0) : a(a) + explicit ExampleLoss(double const a = 1.0) : a(a) { } void initialize( fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, - const std::string& /*name*/) override + std::string const& /*name*/) override { } @@ -87,7 +87,7 @@ class ExampleLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a; diff --git a/fuse_graphs/test/example_variable.hpp b/fuse_graphs/test/example_variable.hpp index ba0bdca6e..f84812fb5 100644 --- a/fuse_graphs/test/example_variable.hpp +++ b/fuse_graphs/test/example_variable.hpp @@ -61,7 +61,7 @@ class ExampleVariable : public fuse_core::Variable { return data_.size(); } - const double* data() const override + double const* data() const override { return data_.data(); } @@ -87,7 +87,7 @@ class ExampleVariable : public fuse_core::Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data_; diff --git a/fuse_graphs/test/test_hash_graph.cpp b/fuse_graphs/test/test_hash_graph.cpp index 484f01dfb..0674ca738 100644 --- a/fuse_graphs/test/test_hash_graph.cpp +++ b/fuse_graphs/test/test_hash_graph.cpp @@ -65,7 +65,7 @@ class HashGraphTestFixture : public ::testing::Test * @param[in] actual - The actual variable * @return True if all the properties match, false otherwise */ - bool compareVariables(const fuse_core::Variable& expected, const fuse_core::Variable& actual) + bool compareVariables(fuse_core::Variable const& expected, fuse_core::Variable const& actual) { failure_description = ""; bool variables_equal = true; @@ -122,7 +122,7 @@ class HashGraphTestFixture : public ::testing::Test * @param[in] actual - The actual constraint * @return True if all the properties match, false otherwise */ - bool compareConstraints(const fuse_core::Constraint& expected, const fuse_core::Constraint& actual) + bool compareConstraints(fuse_core::Constraint const& expected, fuse_core::Constraint const& actual) { failure_description = ""; bool constraints_equal = true; @@ -300,10 +300,10 @@ TEST_F(HashGraphTestFixture, GetVariable) graph.addVariable(variable2); // Verify all of the variables are available - const fuse_core::Variable& actual1 = graph.getVariable(variable1->uuid()); + fuse_core::Variable const& actual1 = graph.getVariable(variable1->uuid()); EXPECT_TRUE(compareVariables(*variable1, actual1)) << failure_description; - const fuse_core::Variable& actual2 = graph.getVariable(variable2->uuid()); + fuse_core::Variable const& actual2 = graph.getVariable(variable2->uuid()); EXPECT_TRUE(compareVariables(*variable2, actual2)) << failure_description; } @@ -332,7 +332,7 @@ TEST_F(HashGraphTestFixture, GetVariables) ASSERT_EQ(3, std::distance(variables.begin(), variables.end())); // Verify we received the correct variables - for (const auto& actual : variables) + for (auto const& actual : variables) { if (actual.uuid() == variable1->uuid()) { @@ -384,7 +384,7 @@ TEST_F(HashGraphTestFixture, GetConnectedVariables) { auto actual_variables = graph.getConnectedVariables(constraint1->uuid()); ASSERT_EQ(1, std::distance(actual_variables.begin(), actual_variables.end())); - for (const auto& actual : actual_variables) + for (auto const& actual : actual_variables) { if (actual.uuid() == variable1->uuid()) { @@ -399,7 +399,7 @@ TEST_F(HashGraphTestFixture, GetConnectedVariables) { auto actual_variables = graph.getConnectedVariables(constraint2->uuid()); ASSERT_EQ(1, std::distance(actual_variables.begin(), actual_variables.end())); - for (const auto& actual : actual_variables) + for (auto const& actual : actual_variables) { if (actual.uuid() == variable2->uuid()) { @@ -547,10 +547,10 @@ TEST_F(HashGraphTestFixture, GetConstraint) graph.addConstraint(constraint2); // Verify all of the constraints are available - const fuse_core::Constraint& actual1 = graph.getConstraint(constraint1->uuid()); + fuse_core::Constraint const& actual1 = graph.getConstraint(constraint1->uuid()); EXPECT_TRUE(compareConstraints(*constraint1, actual1)) << failure_description; - const fuse_core::Constraint& actual2 = graph.getConstraint(constraint2->uuid()); + fuse_core::Constraint const& actual2 = graph.getConstraint(constraint2->uuid()); EXPECT_TRUE(compareConstraints(*constraint2, actual2)) << failure_description; } @@ -583,7 +583,7 @@ TEST_F(HashGraphTestFixture, GetConstraints) ASSERT_EQ(3, std::distance(constraints.begin(), constraints.end())); // Verify we received the correct constraints - for (const auto& actual : constraints) + for (auto const& actual : constraints) { if (actual.uuid() == constraint1->uuid()) { @@ -638,7 +638,7 @@ TEST_F(HashGraphTestFixture, GetConnectedConstraints) { auto actual_constraints = graph.getConnectedConstraints(variable1->uuid()); ASSERT_EQ(2, std::distance(actual_constraints.begin(), actual_constraints.end())); - for (const auto& actual : actual_constraints) + for (auto const& actual : actual_constraints) { if (actual.uuid() == constraint1->uuid()) { @@ -658,7 +658,7 @@ TEST_F(HashGraphTestFixture, GetConnectedConstraints) { auto actual_constraints = graph.getConnectedConstraints(variable2->uuid()); ASSERT_EQ(1, std::distance(actual_constraints.begin(), actual_constraints.end())); - for (const auto& actual : actual_constraints) + for (auto const& actual : actual_constraints) { if (actual.uuid() == constraint2->uuid()) { @@ -816,11 +816,11 @@ TEST_F(HashGraphTestFixture, GetCovariance) covariance_requests.emplace_back(z->uuid(), y->uuid()); std::vector> covariance_matrices; graph.getCovariance(covariance_requests, covariance_matrices); - const std::vector& actual0 = covariance_matrices.at(0); - const std::vector& actual1 = covariance_matrices.at(1); - const std::vector& actual2 = covariance_matrices.at(2); - const std::vector& actual3 = covariance_matrices.at(3); - const std::vector& actual4 = covariance_matrices.at(4); + std::vector const& actual0 = covariance_matrices.at(0); + std::vector const& actual1 = covariance_matrices.at(1); + std::vector const& actual2 = covariance_matrices.at(2); + std::vector const& actual3 = covariance_matrices.at(3); + std::vector const& actual4 = covariance_matrices.at(4); // Compare with the expected blocks // full covariance = { @@ -900,11 +900,11 @@ TEST_F(HashGraphTestFixture, Copy) { fuse_graphs::HashGraph other(graph); // Verify the copy - for (const auto& constraint : graph.getConstraints()) + for (auto const& constraint : graph.getConstraints()) { EXPECT_TRUE(other.constraintExists(constraint.uuid())); } - for (const auto& variable : graph.getVariables()) + for (auto const& variable : graph.getVariables()) { EXPECT_TRUE(other.variableExists(variable.uuid())); } @@ -920,11 +920,11 @@ TEST_F(HashGraphTestFixture, Copy) fuse_graphs::HashGraph other; other = graph; // Verify the copy - for (const auto& constraint : graph.getConstraints()) + for (auto const& constraint : graph.getConstraints()) { EXPECT_TRUE(other.constraintExists(constraint.uuid())); } - for (const auto& variable : graph.getVariables()) + for (auto const& variable : graph.getVariables()) { EXPECT_TRUE(other.variableExists(variable.uuid())); } @@ -939,11 +939,11 @@ TEST_F(HashGraphTestFixture, Copy) { auto other = graph.clone(); // Verify the copy - for (const auto& constraint : graph.getConstraints()) + for (auto const& constraint : graph.getConstraints()) { EXPECT_TRUE(other->constraintExists(constraint.uuid())); } - for (const auto& variable : graph.getVariables()) + for (auto const& variable : graph.getVariables()) { EXPECT_TRUE(other->variableExists(variable.uuid())); } @@ -993,11 +993,11 @@ TEST_F(HashGraphTestFixture, Serialization) } // Verify the copy - for (const auto& constraint : expected.getConstraints()) + for (auto const& constraint : expected.getConstraints()) { EXPECT_TRUE(actual.constraintExists(constraint.uuid())); } - for (const auto& variable : expected.getVariables()) + for (auto const& variable : expected.getVariables()) { EXPECT_TRUE(actual.variableExists(variable.uuid())); } diff --git a/fuse_loss/include/fuse_loss/arctan_loss.hpp b/fuse_loss/include/fuse_loss/arctan_loss.hpp index f83cdff0e..90b28370c 100644 --- a/fuse_loss/include/fuse_loss/arctan_loss.hpp +++ b/fuse_loss/include/fuse_loss/arctan_loss.hpp @@ -65,7 +65,7 @@ class ArctanLoss : public fuse_core::Loss * * @param[in] a ArctanLoss parameter 'a'. See Ceres documentation for more details */ - explicit ArctanLoss(const double a = 1.0); + explicit ArctanLoss(double const a = 1.0); /** * @brief Destructor @@ -86,7 +86,7 @@ class ArctanLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -123,7 +123,7 @@ class ArctanLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -142,7 +142,7 @@ class ArctanLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/cauchy_loss.hpp b/fuse_loss/include/fuse_loss/cauchy_loss.hpp index a4b8bb77e..a99678afa 100644 --- a/fuse_loss/include/fuse_loss/cauchy_loss.hpp +++ b/fuse_loss/include/fuse_loss/cauchy_loss.hpp @@ -65,7 +65,7 @@ class CauchyLoss : public fuse_core::Loss * * @param[in] a CauchyLoss parameter 'a'. See Ceres documentation for more details */ - explicit CauchyLoss(const double a = 1.0); + explicit CauchyLoss(double const a = 1.0); /** * @brief Destructor @@ -86,7 +86,7 @@ class CauchyLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -123,7 +123,7 @@ class CauchyLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -142,7 +142,7 @@ class CauchyLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/composed_loss.hpp b/fuse_loss/include/fuse_loss/composed_loss.hpp index 62ac6dd80..90cf61d80 100644 --- a/fuse_loss/include/fuse_loss/composed_loss.hpp +++ b/fuse_loss/include/fuse_loss/composed_loss.hpp @@ -71,8 +71,8 @@ class ComposedLoss : public fuse_core::Loss * 'f(g(s))'. If it is nullptr the fuse_loss::TrivialLoss is used. Defaults to * nullptr. */ - explicit ComposedLoss(const std::shared_ptr& f_loss = nullptr, - const std::shared_ptr& g_loss = nullptr); + explicit ComposedLoss(std::shared_ptr const& f_loss = nullptr, + std::shared_ptr const& g_loss = nullptr); /** * @brief Destructor @@ -93,7 +93,7 @@ class ComposedLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -140,7 +140,7 @@ class ComposedLoss : public fuse_core::Loss * * @param[in] loss Parameter 'f_loss'. */ - void fLoss(const std::shared_ptr& f_loss) + void fLoss(std::shared_ptr const& f_loss) { f_loss_ = f_loss; } @@ -150,7 +150,7 @@ class ComposedLoss : public fuse_core::Loss * * @param[in] loss Parameter 'g_loss'. */ - void gLoss(const std::shared_ptr& g_loss) + void gLoss(std::shared_ptr const& g_loss) { g_loss_ = g_loss; } @@ -174,7 +174,7 @@ class ComposedLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& f_loss_; diff --git a/fuse_loss/include/fuse_loss/dcs_loss.hpp b/fuse_loss/include/fuse_loss/dcs_loss.hpp index 870933340..bbbd9466b 100644 --- a/fuse_loss/include/fuse_loss/dcs_loss.hpp +++ b/fuse_loss/include/fuse_loss/dcs_loss.hpp @@ -68,7 +68,7 @@ class DCSLoss : public fuse_core::Loss * * @param[in] a DCSLoss parameter 'a' */ - explicit DCSLoss(const double a = 1.0); + explicit DCSLoss(double const a = 1.0); /** * @brief Destructor @@ -89,7 +89,7 @@ class DCSLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -126,7 +126,7 @@ class DCSLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -145,7 +145,7 @@ class DCSLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/fair_loss.hpp b/fuse_loss/include/fuse_loss/fair_loss.hpp index d4a022c13..282e00700 100644 --- a/fuse_loss/include/fuse_loss/fair_loss.hpp +++ b/fuse_loss/include/fuse_loss/fair_loss.hpp @@ -68,7 +68,7 @@ class FairLoss : public fuse_core::Loss * * @param[in] a FairLoss parameter 'a' */ - explicit FairLoss(const double a = 1.0); + explicit FairLoss(double const a = 1.0); /** * @brief Destructor @@ -89,7 +89,7 @@ class FairLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -126,7 +126,7 @@ class FairLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -145,7 +145,7 @@ class FairLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp b/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp index 265aaf12c..ddcf9fb21 100644 --- a/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp +++ b/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp @@ -68,7 +68,7 @@ class GemanMcClureLoss : public fuse_core::Loss * * @param[in] a GemanMcClureLoss parameter 'a' */ - explicit GemanMcClureLoss(const double a = 1.0); + explicit GemanMcClureLoss(double const a = 1.0); /** * @brief Destructor @@ -89,7 +89,7 @@ class GemanMcClureLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -126,7 +126,7 @@ class GemanMcClureLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -145,7 +145,7 @@ class GemanMcClureLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/huber_loss.hpp b/fuse_loss/include/fuse_loss/huber_loss.hpp index e38b479da..9bb96ee37 100644 --- a/fuse_loss/include/fuse_loss/huber_loss.hpp +++ b/fuse_loss/include/fuse_loss/huber_loss.hpp @@ -65,7 +65,7 @@ class HuberLoss : public fuse_core::Loss * * @param[in] a HuberLoss parameter 'a'. See Ceres documentation for more details */ - explicit HuberLoss(const double a = 1.0); + explicit HuberLoss(double const a = 1.0); /** * @brief Destructor @@ -86,7 +86,7 @@ class HuberLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -123,7 +123,7 @@ class HuberLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -142,7 +142,7 @@ class HuberLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/loss_function.hpp b/fuse_loss/include/fuse_loss/loss_function.hpp index 68ba629eb..d067416dd 100644 --- a/fuse_loss/include/fuse_loss/loss_function.hpp +++ b/fuse_loss/include/fuse_loss/loss_function.hpp @@ -145,14 +145,14 @@ namespace ceres class DCSLoss : public ceres::LossFunction { public: - explicit DCSLoss(const double a) : a_(a) + explicit DCSLoss(double const a) : a_(a) { } void Evaluate(double, double* rho) const override; private: - const double a_; + double const a_; }; // Fair, similar to the L1 - L2 estimators, that try to take the advantage of the L1 estimators to @@ -169,15 +169,15 @@ class DCSLoss : public ceres::LossFunction class FairLoss : public ceres::LossFunction { public: - explicit FairLoss(const double a) : a_(a), b_(a * a) + explicit FairLoss(double const a) : a_(a), b_(a * a) { } void Evaluate(double, double*) const override; private: - const double a_; - const double b_; + double const a_; + double const b_; }; // Geman-McClure, similarly to Tukey loss, it tries to reduce the effect of large errors, but it @@ -220,14 +220,14 @@ class FairLoss : public ceres::LossFunction class GemanMcClureLoss : public ceres::LossFunction { public: - explicit GemanMcClureLoss(const double a) : b_(a * a) + explicit GemanMcClureLoss(double const a) : b_(a * a) { } void Evaluate(double, double*) const override; private: - const double b_; + double const b_; }; // Welsch, similar to Tukey loss, it tries to reduce the effect of large errors, but it does not @@ -243,15 +243,15 @@ class GemanMcClureLoss : public ceres::LossFunction class WelschLoss : public ceres::LossFunction { public: - explicit WelschLoss(const double a) : b_(a * a), c_(-1.0 / b_) + explicit WelschLoss(double const a) : b_(a * a), c_(-1.0 / b_) { } void Evaluate(double, double*) const override; private: - const double b_; - const double c_; + double const b_; + double const c_; }; } // namespace ceres diff --git a/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp b/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp index 267f28c52..eae40ca00 100644 --- a/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp +++ b/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp @@ -70,7 +70,7 @@ class HSVColormap colormap_.reserve(size); double hue = 0.0; - const double hue_increment = 1.0 / size; + double const hue_increment = 1.0 / size; for (size_t i = 0; i < size; ++i, hue += hue_increment) { QColor color; @@ -80,7 +80,7 @@ class HSVColormap } } - const QColor& operator[](const size_t i) const + QColor const& operator[](const size_t i) const { return colormap_[i]; } @@ -97,11 +97,11 @@ class HSVColormap class LossEvaluator { public: - explicit LossEvaluator(const std::vector& residuals) : residuals_(residuals) + explicit LossEvaluator(std::vector const& residuals) : residuals_(residuals) { } - std::vector rho(const ceres::LossFunction* loss_function) const + std::vector rho(ceres::LossFunction const* loss_function) const { std::vector rhos; rhos.reserve(residuals_.size()); @@ -115,7 +115,7 @@ class LossEvaluator // See: https://github.com/ceres-solver/ceres- // solver/blob/master/internal/ceres/residual_block.cc#L165 std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(rhos), - [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + [&loss_function](auto const& r) { // NOLINT(whitespace/braces) double rho[3]; loss_function->Evaluate(r * r, rho); return 0.5 * rho[0]; @@ -124,7 +124,7 @@ class LossEvaluator return rhos; } - std::vector influence(const ceres::LossFunction* loss_function) const + std::vector influence(ceres::LossFunction const* loss_function) const { std::vector influence; influence.reserve(residuals_.size()); @@ -155,7 +155,7 @@ class LossEvaluator // where \rho(r) = 0.5 * rho(r^2) is the inverse of rho(s) = 2 * \rho(sqrt(s)), // because s = r^2 and r = sqrt(s). std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(influence), - [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + [&loss_function](auto const& r) { // NOLINT(whitespace/braces) double rho[3]; loss_function->Evaluate(r * r, rho); return r * rho[1]; @@ -164,7 +164,7 @@ class LossEvaluator return influence; } - std::vector weight(const ceres::LossFunction* loss_function) const + std::vector weight(ceres::LossFunction const* loss_function) const { std::vector weight; weight.reserve(residuals_.size()); @@ -181,7 +181,7 @@ class LossEvaluator // // That is, rho[1]. std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(weight), - [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + [&loss_function](auto const& r) { // NOLINT(whitespace/braces) double rho[3]; loss_function->Evaluate(r * r, rho); return rho[1]; @@ -190,7 +190,7 @@ class LossEvaluator return weight; } - std::vector secondDerivative(const ceres::LossFunction* loss_function) const + std::vector secondDerivative(ceres::LossFunction const* loss_function) const { std::vector second_derivative; second_derivative.reserve(residuals_.size()); @@ -203,7 +203,7 @@ class LossEvaluator // // That is, rho[2]. std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(second_derivative), - [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + [&loss_function](auto const& r) { // NOLINT(whitespace/braces) double rho[3]; loss_function->Evaluate(r * r, rho); return rho[2]; @@ -212,7 +212,7 @@ class LossEvaluator return second_derivative; } - const std::vector& getResiduals() const + std::vector const& getResiduals() const { return residuals_; } @@ -224,7 +224,7 @@ class LossEvaluator class QwtLossPlot { public: - QwtLossPlot(const std::vector& residuals, const HSVColormap& colormap) + QwtLossPlot(std::vector const& residuals, HSVColormap const& colormap) : residuals_(QVector(residuals.begin(), residuals.end())) , loss_evaluator_(residuals) , colormap_(colormap) @@ -249,12 +249,12 @@ class QwtLossPlot plot_.insertLegend(&legend_); } - static std::string getName(const std::string& type) + static std::string getName(std::string const& type) { return type.substr(11, type.size() - 15); } - QwtPlotCurve* createCurve(const std::string& name, const std::vector& values) + QwtPlotCurve* createCurve(std::string const& name, std::vector const& values) { QwtPlotCurve* curve = new QwtPlotCurve(name.c_str()); @@ -266,27 +266,27 @@ class QwtLossPlot return curve; } - void plotRho(const std::shared_ptr& loss) + void plotRho(std::shared_ptr const& loss) { curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.rho(loss->lossFunction()))); } - void plotInfluence(const std::shared_ptr& loss) + void plotInfluence(std::shared_ptr const& loss) { curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.influence(loss->lossFunction()))); } - void plotWeight(const std::shared_ptr& loss) + void plotWeight(std::shared_ptr const& loss) { curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.weight(loss->lossFunction()))); } - void plotSecondDerivative(const std::shared_ptr& loss) + void plotSecondDerivative(std::shared_ptr const& loss) { curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.secondDerivative(loss->lossFunction()))); } - void save(const std::string& filename) + void save(std::string const& filename) { QwtPlotRenderer renderer; renderer.renderDocument(&plot_, filename.c_str(), QSizeF(300, 200)); diff --git a/fuse_loss/include/fuse_loss/scaled_loss.hpp b/fuse_loss/include/fuse_loss/scaled_loss.hpp index a8ed6ccab..f7b56adc0 100644 --- a/fuse_loss/include/fuse_loss/scaled_loss.hpp +++ b/fuse_loss/include/fuse_loss/scaled_loss.hpp @@ -67,7 +67,7 @@ class ScaledLoss : public fuse_core::Loss * @param[in] a ScaledLoss parameter 'a'. See Ceres documentation for more details. * @param[in] loss The loss function to scale. Its output is scaled/multiplied by 'a'. */ - explicit ScaledLoss(const double a = 1.0, const std::shared_ptr& loss = nullptr); + explicit ScaledLoss(double const a = 1.0, std::shared_ptr const& loss = nullptr); /** * @brief Destructor @@ -88,7 +88,7 @@ class ScaledLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -135,7 +135,7 @@ class ScaledLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -145,7 +145,7 @@ class ScaledLoss : public fuse_core::Loss * * @param[in] loss Parameter 'loss'. */ - void loss(const std::shared_ptr& loss) + void loss(std::shared_ptr const& loss) { loss_ = loss; } @@ -165,7 +165,7 @@ class ScaledLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/softlone_loss.hpp b/fuse_loss/include/fuse_loss/softlone_loss.hpp index 740c7034b..89912bc05 100644 --- a/fuse_loss/include/fuse_loss/softlone_loss.hpp +++ b/fuse_loss/include/fuse_loss/softlone_loss.hpp @@ -65,7 +65,7 @@ class SoftLOneLoss : public fuse_core::Loss * * @param[in] a SoftLOneLoss parameter 'a'. See Ceres documentation for more details */ - explicit SoftLOneLoss(const double a = 1.0); + explicit SoftLOneLoss(double const a = 1.0); /** * @brief Destructor @@ -86,7 +86,7 @@ class SoftLOneLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -123,7 +123,7 @@ class SoftLOneLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -142,7 +142,7 @@ class SoftLOneLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/tolerant_loss.hpp b/fuse_loss/include/fuse_loss/tolerant_loss.hpp index 26cd8c9cb..0eac05db2 100644 --- a/fuse_loss/include/fuse_loss/tolerant_loss.hpp +++ b/fuse_loss/include/fuse_loss/tolerant_loss.hpp @@ -66,7 +66,7 @@ class TolerantLoss : public fuse_core::Loss * @param[in] a TolerantLoss parameter 'a'. See Ceres documentation for more details * @param[in] b TolerantLoss parameter 'b'. See Ceres documentation for more details */ - explicit TolerantLoss(const double a = 1.0, const double b = 0.1); + explicit TolerantLoss(double const a = 1.0, double const b = 0.1); /** * @brief Destructor @@ -87,7 +87,7 @@ class TolerantLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -134,7 +134,7 @@ class TolerantLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -144,7 +144,7 @@ class TolerantLoss : public fuse_core::Loss * * @param[in] b Parameter 'b'. */ - void b(const double b) + void b(double const b) { b_ = b; } @@ -164,7 +164,7 @@ class TolerantLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/trivial_loss.hpp b/fuse_loss/include/fuse_loss/trivial_loss.hpp index 130199cce..45ec83b84 100644 --- a/fuse_loss/include/fuse_loss/trivial_loss.hpp +++ b/fuse_loss/include/fuse_loss/trivial_loss.hpp @@ -83,7 +83,7 @@ class TrivialLoss : public fuse_core::Loss void initialize( fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, - const std::string& /*name*/) override + std::string const& /*name*/) override { } @@ -119,7 +119,7 @@ class TrivialLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } diff --git a/fuse_loss/include/fuse_loss/tukey_loss.hpp b/fuse_loss/include/fuse_loss/tukey_loss.hpp index 8d43d516d..e50d862a3 100644 --- a/fuse_loss/include/fuse_loss/tukey_loss.hpp +++ b/fuse_loss/include/fuse_loss/tukey_loss.hpp @@ -65,7 +65,7 @@ class TukeyLoss : public fuse_core::Loss * * @param[in] a TukeyLoss parameter 'a'. See Ceres documentation for more details */ - explicit TukeyLoss(const double a = 1.0); + explicit TukeyLoss(double const a = 1.0); /** * @brief Destructor @@ -86,7 +86,7 @@ class TukeyLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -123,7 +123,7 @@ class TukeyLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -142,7 +142,7 @@ class TukeyLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/include/fuse_loss/welsch_loss.hpp b/fuse_loss/include/fuse_loss/welsch_loss.hpp index 092b8b681..979bc906b 100644 --- a/fuse_loss/include/fuse_loss/welsch_loss.hpp +++ b/fuse_loss/include/fuse_loss/welsch_loss.hpp @@ -68,7 +68,7 @@ class WelschLoss : public fuse_core::Loss * * @param[in] a WelschLoss parameter 'a' */ - explicit WelschLoss(const double a = 1.0); + explicit WelschLoss(double const a = 1.0); /** * @brief Destructor @@ -89,7 +89,7 @@ class WelschLoss : public fuse_core::Loss fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. @@ -126,7 +126,7 @@ class WelschLoss : public fuse_core::Loss * * @param[in] a Parameter 'a'. */ - void a(const double a) + void a(double const a) { a_ = a; } @@ -145,7 +145,7 @@ class WelschLoss : public fuse_core::Loss * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& a_; diff --git a/fuse_loss/src/arctan_loss.cpp b/fuse_loss/src/arctan_loss.cpp index 8736ea0c7..15518f492 100644 --- a/fuse_loss/src/arctan_loss.cpp +++ b/fuse_loss/src/arctan_loss.cpp @@ -42,7 +42,7 @@ namespace fuse_loss { -ArctanLoss::ArctanLoss(const double a) : a_(a) +ArctanLoss::ArctanLoss(double const a) : a_(a) { } @@ -50,7 +50,7 @@ void ArctanLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/cauchy_loss.cpp b/fuse_loss/src/cauchy_loss.cpp index 372dd978b..af7255537 100644 --- a/fuse_loss/src/cauchy_loss.cpp +++ b/fuse_loss/src/cauchy_loss.cpp @@ -42,7 +42,7 @@ namespace fuse_loss { -CauchyLoss::CauchyLoss(const double a) : a_(a) +CauchyLoss::CauchyLoss(double const a) : a_(a) { } @@ -50,7 +50,7 @@ void CauchyLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/composed_loss.cpp b/fuse_loss/src/composed_loss.cpp index bafb40940..a6ed59ac4 100644 --- a/fuse_loss/src/composed_loss.cpp +++ b/fuse_loss/src/composed_loss.cpp @@ -44,8 +44,8 @@ namespace fuse_loss { -ComposedLoss::ComposedLoss(const std::shared_ptr& f_loss, - const std::shared_ptr& g_loss) +ComposedLoss::ComposedLoss(std::shared_ptr const& f_loss, + std::shared_ptr const& g_loss) : f_loss_(f_loss), g_loss_(g_loss) { } @@ -54,7 +54,7 @@ void ComposedLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { f_loss_ = fuse_core::loadLossConfig(interfaces, name + ".f_loss"); g_loss_ = fuse_core::loadLossConfig(interfaces, name + ".g_loss"); diff --git a/fuse_loss/src/dcs_loss.cpp b/fuse_loss/src/dcs_loss.cpp index 571cbfd5f..73583140a 100644 --- a/fuse_loss/src/dcs_loss.cpp +++ b/fuse_loss/src/dcs_loss.cpp @@ -43,7 +43,7 @@ namespace fuse_loss { -DCSLoss::DCSLoss(const double a) : a_(a) +DCSLoss::DCSLoss(double const a) : a_(a) { } @@ -51,7 +51,7 @@ void DCSLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/fair_loss.cpp b/fuse_loss/src/fair_loss.cpp index cc40cab7f..93550225e 100644 --- a/fuse_loss/src/fair_loss.cpp +++ b/fuse_loss/src/fair_loss.cpp @@ -43,7 +43,7 @@ namespace fuse_loss { -FairLoss::FairLoss(const double a) : a_(a) +FairLoss::FairLoss(double const a) : a_(a) { } @@ -51,7 +51,7 @@ void FairLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/geman_mcclure_loss.cpp b/fuse_loss/src/geman_mcclure_loss.cpp index ce4ebd814..9c63a404b 100644 --- a/fuse_loss/src/geman_mcclure_loss.cpp +++ b/fuse_loss/src/geman_mcclure_loss.cpp @@ -43,7 +43,7 @@ namespace fuse_loss { -GemanMcClureLoss::GemanMcClureLoss(const double a) : a_(a) +GemanMcClureLoss::GemanMcClureLoss(double const a) : a_(a) { } @@ -51,7 +51,7 @@ void GemanMcClureLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/huber_loss.cpp b/fuse_loss/src/huber_loss.cpp index a9b0b2d73..9829a0a75 100644 --- a/fuse_loss/src/huber_loss.cpp +++ b/fuse_loss/src/huber_loss.cpp @@ -42,7 +42,7 @@ namespace fuse_loss { -HuberLoss::HuberLoss(const double a) : a_(a) +HuberLoss::HuberLoss(double const a) : a_(a) { } @@ -50,7 +50,7 @@ void HuberLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/loss_function.cpp b/fuse_loss/src/loss_function.cpp index 3e5c9c892..7e35ab9ea 100644 --- a/fuse_loss/src/loss_function.cpp +++ b/fuse_loss/src/loss_function.cpp @@ -44,8 +44,8 @@ void DCSLoss::Evaluate(double s, double rho[3]) const if (s > a_) { // Outlier region - const double inv = 1.0 / (a_ + s); - const double scale = 2.0 * a_ * inv; + double const inv = 1.0 / (a_ + s); + double const scale = 2.0 * a_ * inv; rho[0] = a_ * (3.0 * s - a_) * inv; rho[1] = scale * scale; @@ -62,9 +62,9 @@ void DCSLoss::Evaluate(double s, double rho[3]) const void FairLoss::Evaluate(double s, double rho[3]) const { - const double r = std::sqrt(s); - const double ra = r / a_; - const double sum = 1.0 + ra; + double const r = std::sqrt(s); + double const ra = r / a_; + double const sum = 1.0 + ra; rho[0] = 2.0 * b_ * (ra - std::log(sum)); rho[1] = 1.0 / sum; @@ -73,9 +73,9 @@ void FairLoss::Evaluate(double s, double rho[3]) const void GemanMcClureLoss::Evaluate(double s, double rho[3]) const { - const double sum = b_ + s; - const double inv = 1.0 / sum; - const double scale = b_ * inv; + double const sum = b_ + s; + double const inv = 1.0 / sum; + double const scale = b_ * inv; rho[0] = s * scale; rho[1] = scale * scale; @@ -84,7 +84,7 @@ void GemanMcClureLoss::Evaluate(double s, double rho[3]) const void WelschLoss::Evaluate(double s, double rho[3]) const { - const double exp = std::exp(s * c_); + double const exp = std::exp(s * c_); rho[0] = b_ * (1 - exp); rho[1] = exp; diff --git a/fuse_loss/src/scaled_loss.cpp b/fuse_loss/src/scaled_loss.cpp index 840a9c7ee..a2f129eea 100644 --- a/fuse_loss/src/scaled_loss.cpp +++ b/fuse_loss/src/scaled_loss.cpp @@ -43,7 +43,7 @@ namespace fuse_loss { -ScaledLoss::ScaledLoss(const double a, const std::shared_ptr& loss) : a_(a), loss_(loss) +ScaledLoss::ScaledLoss(double const a, std::shared_ptr const& loss) : a_(a), loss_(loss) { } @@ -51,7 +51,7 @@ void ScaledLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); loss_ = fuse_core::loadLossConfig(interfaces, name + ".loss"); diff --git a/fuse_loss/src/softlone_loss.cpp b/fuse_loss/src/softlone_loss.cpp index 3b9c87d1f..3e20d96d1 100644 --- a/fuse_loss/src/softlone_loss.cpp +++ b/fuse_loss/src/softlone_loss.cpp @@ -42,7 +42,7 @@ namespace fuse_loss { -SoftLOneLoss::SoftLOneLoss(const double a) : a_(a) +SoftLOneLoss::SoftLOneLoss(double const a) : a_(a) { } @@ -50,7 +50,7 @@ void SoftLOneLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/tolerant_loss.cpp b/fuse_loss/src/tolerant_loss.cpp index c46eb4cbb..c6c839b47 100644 --- a/fuse_loss/src/tolerant_loss.cpp +++ b/fuse_loss/src/tolerant_loss.cpp @@ -42,7 +42,7 @@ namespace fuse_loss { -TolerantLoss::TolerantLoss(const double a, const double b) : a_(a), b_(b) +TolerantLoss::TolerantLoss(double const a, double const b) : a_(a), b_(b) { } @@ -50,7 +50,7 @@ void TolerantLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); b_ = fuse_core::getParam(interfaces, name + ".b", b_); diff --git a/fuse_loss/src/tukey_loss.cpp b/fuse_loss/src/tukey_loss.cpp index 1570e89f8..98e1cbc6b 100644 --- a/fuse_loss/src/tukey_loss.cpp +++ b/fuse_loss/src/tukey_loss.cpp @@ -43,7 +43,7 @@ namespace fuse_loss { -TukeyLoss::TukeyLoss(const double a) : a_(a) +TukeyLoss::TukeyLoss(double const a) : a_(a) { } @@ -51,7 +51,7 @@ void TukeyLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/src/welsch_loss.cpp b/fuse_loss/src/welsch_loss.cpp index bc913fd86..20fc99422 100644 --- a/fuse_loss/src/welsch_loss.cpp +++ b/fuse_loss/src/welsch_loss.cpp @@ -43,7 +43,7 @@ namespace fuse_loss { -WelschLoss::WelschLoss(const double a) : a_(a) +WelschLoss::WelschLoss(double const a) : a_(a) { } @@ -51,7 +51,7 @@ void WelschLoss::initialize( fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } diff --git a/fuse_loss/test/test_composed_loss.cpp b/fuse_loss/test/test_composed_loss.cpp index de607895d..b24862845 100644 --- a/fuse_loss/test/test_composed_loss.cpp +++ b/fuse_loss/test/test_composed_loss.cpp @@ -58,7 +58,7 @@ TEST(ComposedLoss, Constructor) ASSERT_NO_THROW(composed_loss_function.reset(composed_loss.lossFunction())); ASSERT_NE(nullptr, composed_loss_function); - const double s = 1.5; + double const s = 1.5; double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); @@ -81,10 +81,10 @@ TEST(ComposedLoss, Constructor) ASSERT_NO_THROW(composed_loss_function.reset(composed_loss.lossFunction())); ASSERT_NE(nullptr, composed_loss_function); - const auto f_loss_function = std::unique_ptr(f_loss->lossFunction()); + auto const f_loss_function = std::unique_ptr(f_loss->lossFunction()); ASSERT_NE(nullptr, f_loss_function); - const double s = 1.5; + double const s = 1.5; double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); @@ -116,10 +116,10 @@ TEST(ComposedLoss, Constructor) ASSERT_NO_THROW(composed_loss_function.reset(composed_loss.lossFunction())); ASSERT_NE(nullptr, composed_loss_function); - const auto g_loss_function = std::unique_ptr(g_loss->lossFunction()); + auto const g_loss_function = std::unique_ptr(g_loss->lossFunction()); ASSERT_NE(nullptr, g_loss_function); - const double s = 1.5; + double const s = 1.5; double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); @@ -153,13 +153,13 @@ TEST(ComposedLoss, Constructor) ASSERT_NO_THROW(composed_loss_function.reset(composed_loss.lossFunction())); ASSERT_NE(nullptr, composed_loss_function); - const auto f_loss_function = std::unique_ptr(f_loss->lossFunction()); + auto const f_loss_function = std::unique_ptr(f_loss->lossFunction()); ASSERT_NE(nullptr, f_loss_function); - const auto g_loss_function = std::unique_ptr(g_loss->lossFunction()); + auto const g_loss_function = std::unique_ptr(g_loss->lossFunction()); ASSERT_NE(nullptr, g_loss_function); - const double s = 1.5; + double const s = 1.5; double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); @@ -185,7 +185,7 @@ TEST(ComposedLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) : data(data) + explicit CostFunctor(double const data) : data(data) { } @@ -205,19 +205,19 @@ TEST(ComposedLoss, Optimization) double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{ 1.0 }; + double const inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{ 10.0 }; + double const outlier{ 10.0 }; ceres::CostFunction* cost_function_outlier = new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create an 'f' loss - const double a{ 0.05 }; + double const a{ 0.05 }; std::shared_ptr f_loss{ new fuse_loss::HuberLoss(a) }; // Create an 'g' loss - const double scaled_a{ 0.5 }; + double const scaled_a{ 0.5 }; std::shared_ptr g_loss{ new fuse_loss::ScaledLoss(scaled_a) }; // Create a composed loss, which illustrates the case of scaling the residuals by a factor with a @@ -273,12 +273,12 @@ TEST(ComposedLoss, Optimization) TEST(ComposedLoss, Serialization) { // Construct an 'f' loss - const double f_loss_a{ 0.3 }; + double const f_loss_a{ 0.3 }; std::shared_ptr f_loss{ new fuse_loss::HuberLoss(f_loss_a) }; // Construct a 'g' loss - const double g_loss_a{ 0.3 }; - const double g_loss_b{ 0.6 }; + double const g_loss_a{ 0.3 }; + double const g_loss_b{ 0.6 }; std::shared_ptr g_loss{ new fuse_loss::TolerantLoss(g_loss_a, g_loss_b) }; // Construct a composed loss @@ -299,15 +299,15 @@ TEST(ComposedLoss, Serialization) } // Compare - const auto expected_loss_function = std::unique_ptr(actual.lossFunction()); - const auto actual_loss_function = std::unique_ptr(actual.lossFunction()); + auto const expected_loss_function = std::unique_ptr(actual.lossFunction()); + auto const actual_loss_function = std::unique_ptr(actual.lossFunction()); ASSERT_NE(nullptr, actual_loss_function); EXPECT_NE(nullptr, actual.fLoss()); EXPECT_NE(nullptr, actual.gLoss()); // Test inlier (s <= g_loss_a*g_loss_a) - const double s = 0.95 * g_loss_a * g_loss_a; + double const s = 0.95 * g_loss_a * g_loss_a; double expected_rho[3] = { 0.0 }; expected_loss_function->Evaluate(s, expected_rho); @@ -320,7 +320,7 @@ TEST(ComposedLoss, Serialization) } // Test outlier (s > g_loss_b) - const double s_outlier = 1.05 * g_loss_a * g_loss_a; + double const s_outlier = 1.05 * g_loss_a * g_loss_a; expected_loss_function->Evaluate(s_outlier, expected_rho); actual_loss_function->Evaluate(s_outlier, actual_rho); diff --git a/fuse_loss/test/test_huber_loss.cpp b/fuse_loss/test/test_huber_loss.cpp index f21ac62d7..a47db95ee 100644 --- a/fuse_loss/test/test_huber_loss.cpp +++ b/fuse_loss/test/test_huber_loss.cpp @@ -49,7 +49,7 @@ TEST(HuberLoss, Constructor) // Create a loss with a parameter { - const double a{ 0.3 }; + double const a{ 0.3 }; fuse_loss::HuberLoss loss(a); ASSERT_EQ(a, loss.a()); } @@ -57,7 +57,7 @@ TEST(HuberLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) : data(data) + explicit CostFunctor(double const data) : data(data) { } @@ -77,10 +77,10 @@ TEST(HuberLoss, Optimization) double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{ 1.0 }; + double const inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{ 10.0 }; + double const outlier{ 10.0 }; ceres::CostFunction* cost_function_outlier = new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); @@ -134,7 +134,7 @@ TEST(HuberLoss, Optimization) TEST(HuberLoss, Serialization) { // Construct a loss - const double a{ 0.3 }; + double const a{ 0.3 }; fuse_loss::HuberLoss expected(a); // Serialize the loss into an archive @@ -156,7 +156,7 @@ TEST(HuberLoss, Serialization) EXPECT_NE(nullptr, actual.lossFunction()); // Test inlier (s <= a*a) - const double s = 0.95 * a * a; + double const s = 0.95 * a * a; double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); @@ -165,7 +165,7 @@ TEST(HuberLoss, Serialization) EXPECT_EQ(0.0, rho[2]); // Test outlier - const double s_outlier = 1.05 * a * a; + double const s_outlier = 1.05 * a * a; actual.lossFunction()->Evaluate(s_outlier, rho); // In the outlier region rho() satisfies: diff --git a/fuse_loss/test/test_loss_function.cpp b/fuse_loss/test/test_loss_function.cpp index 005177faa..4865d2f02 100644 --- a/fuse_loss/test/test_loss_function.cpp +++ b/fuse_loss/test/test_loss_function.cpp @@ -64,7 +64,7 @@ namespace // Compares the values of rho'(s) and rho''(s) computed by the // callback with estimates obtained by symmetric finite differencing // of rho(s). -void AssertLossFunctionIsValid(const ceres::LossFunction& loss, double s) +void AssertLossFunctionIsValid(ceres::LossFunction const& loss, double s) { ASSERT_GT(s, 0); @@ -87,7 +87,7 @@ void AssertLossFunctionIsValid(const ceres::LossFunction& loss, double s) // Use symmetric finite differencing to estimate rho'(s) and // rho''(s). - const double kH = 1e-4; + double const kH = 1e-4; // Values at s + kH. double fwd[3]; // Values at s - kH. @@ -96,11 +96,11 @@ void AssertLossFunctionIsValid(const ceres::LossFunction& loss, double s) loss.Evaluate(s - kH, bwd); // First derivative. - const double fd_1 = (fwd[0] - bwd[0]) / (2 * kH); + double const fd_1 = (fwd[0] - bwd[0]) / (2 * kH); ASSERT_NEAR(fd_1, rho[1], 1e-6); // Second derivative. - const double fd_2 = (fwd[0] - 2 * rho[0] + bwd[0]) / (kH * kH); + double const fd_2 = (fwd[0] - 2 * rho[0] + bwd[0]) / (kH * kH); ASSERT_NEAR(fd_2, rho[2], 1e-6); } @@ -144,7 +144,7 @@ TEST(LossFunction, GemanMcClureLoss) AssertLossFunctionIsValid(ceres::GemanMcClureLoss(1.3), 0.357); AssertLossFunctionIsValid(ceres::GemanMcClureLoss(1.3), 1.792); // Check that at s = 0: rho = [0, 1, -2/b]. - const double a = 0.7; + double const a = 0.7; double rho[3]; ceres::GemanMcClureLoss(a).Evaluate(0.0, rho); @@ -160,7 +160,7 @@ TEST(LossFunction, WelschLoss) AssertLossFunctionIsValid(ceres::WelschLoss(1.3), 0.357); AssertLossFunctionIsValid(ceres::WelschLoss(1.3), 1.792); // Check that at s = 0: rho = [0, 1, -1/b]. - const double a = 0.7; + double const a = 0.7; double rho[3]; ceres::WelschLoss(a).Evaluate(0.0, rho); diff --git a/fuse_loss/test/test_qwt_loss_plot.cpp b/fuse_loss/test/test_qwt_loss_plot.cpp index 21d67d7b5..5f7343e05 100644 --- a/fuse_loss/test/test_qwt_loss_plot.cpp +++ b/fuse_loss/test/test_qwt_loss_plot.cpp @@ -57,9 +57,9 @@ class QwtLossPlotTest : public testing::Test QwtLossPlotTest() { // Generate samples: - const double step{ 0.01 }; + double const step{ 0.01 }; const size_t half_samples{ 1000 }; - const double x_min = -(half_samples * step); + double const x_min = -(half_samples * step); const size_t samples{ 2 * half_samples + 1 }; @@ -106,7 +106,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) plot.setAxisScale(QwtPlot::yLeft, 0.0, 15.0); // Create a curve for each loss rho function: - for (const auto& loss : losses) + for (auto const& loss : losses) { rho_loss_plot.plotRho(loss); } @@ -124,7 +124,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) influence_plot.setAxisScale(QwtPlot::yLeft, -3.0, 3.0); // Create a curve for each loss rho function: - for (const auto& loss : losses) + for (auto const& loss : losses) { influence_loss_plot.plotInfluence(loss); } @@ -142,7 +142,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) weight_plot.setAxisScale(QwtPlot::yLeft, 0.0, 1.5); // Create a curve for each loss rho function: - for (const auto& loss : losses) + for (auto const& loss : losses) { weight_loss_plot.plotWeight(loss); } @@ -160,7 +160,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) second_derivative_plot.setAxisScale(QwtPlot::yLeft, -0.15, 0.15); // Create a curve for each loss rho function: - for (const auto& loss : losses) + for (auto const& loss : losses) { second_derivative_loss_plot.plotSecondDerivative(loss); } diff --git a/fuse_loss/test/test_scaled_loss.cpp b/fuse_loss/test/test_scaled_loss.cpp index c732a7536..512b862d6 100644 --- a/fuse_loss/test/test_scaled_loss.cpp +++ b/fuse_loss/test/test_scaled_loss.cpp @@ -53,7 +53,7 @@ TEST(ScaledLoss, Constructor) // Create a loss with a parameter { - const double a{ 0.3 }; + double const a{ 0.3 }; fuse_loss::ScaledLoss scaled_loss(a); EXPECT_EQ(a, scaled_loss.a()); EXPECT_EQ(nullptr, scaled_loss.loss()); @@ -63,7 +63,7 @@ TEST(ScaledLoss, Constructor) { std::shared_ptr loss{ new fuse_loss::HuberLoss }; - const double a{ 0.3 }; + double const a{ 0.3 }; fuse_loss::ScaledLoss scaled_loss(a, loss); EXPECT_EQ(a, scaled_loss.a()); EXPECT_NE(nullptr, scaled_loss.loss()); @@ -73,7 +73,7 @@ TEST(ScaledLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) : data(data) + explicit CostFunctor(double const data) : data(data) { } @@ -93,19 +93,19 @@ TEST(ScaledLoss, Optimization) double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{ 1.0 }; + double const inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{ 10.0 }; + double const outlier{ 10.0 }; ceres::CostFunction* cost_function_outlier = new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create loss - const double a{ 0.1 }; + double const a{ 0.1 }; std::shared_ptr loss{ new fuse_loss::HuberLoss(a) }; // Create a scaled loss, which should not have a significant impact in this test - const double scaled_a{ 0.7 }; + double const scaled_a{ 0.7 }; fuse_loss::ScaledLoss scaled_loss(scaled_a, loss); // Build the problem. @@ -155,11 +155,11 @@ TEST(ScaledLoss, Optimization) TEST(ScaledLoss, Serialization) { // Construct a loss - const double loss_a{ 0.3 }; + double const loss_a{ 0.3 }; std::shared_ptr loss{ new fuse_loss::HuberLoss(loss_a) }; // Construct a scaled loss - const double a{ 0.7 }; + double const a{ 0.7 }; fuse_loss::ScaledLoss expected(a, loss); // Serialize the loss into an archive @@ -182,7 +182,7 @@ TEST(ScaledLoss, Serialization) EXPECT_NE(nullptr, actual.loss()); // Test inlier (s <= loss_a*loss_a) - const double s = 0.95 * loss_a * loss_a; + double const s = 0.95 * loss_a * loss_a; double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); @@ -191,7 +191,7 @@ TEST(ScaledLoss, Serialization) EXPECT_EQ(0.0, rho[2]); // Test outlier - const double s_outlier = 1.05 * loss_a * loss_a; + double const s_outlier = 1.05 * loss_a * loss_a; actual.lossFunction()->Evaluate(s_outlier, rho); // In the outlier region rho() satisfies: diff --git a/fuse_loss/test/test_tukey_loss.cpp b/fuse_loss/test/test_tukey_loss.cpp index 702d293a6..a3c13a606 100644 --- a/fuse_loss/test/test_tukey_loss.cpp +++ b/fuse_loss/test/test_tukey_loss.cpp @@ -51,7 +51,7 @@ TEST(TukeyLoss, Constructor) // Create a loss with a parameter { - const double a{ 0.3 }; + double const a{ 0.3 }; fuse_loss::TukeyLoss loss(a); ASSERT_EQ(a, loss.a()); } @@ -72,7 +72,7 @@ TEST(TukeyLoss, Evaluate) struct CostFunctor { - explicit CostFunctor(const double data) : data(data) + explicit CostFunctor(double const data) : data(data) { } @@ -92,10 +92,10 @@ TEST(TukeyLoss, Optimization) double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{ 1.0 }; + double const inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{ 10.0 }; + double const outlier{ 10.0 }; ceres::CostFunction* cost_function_outlier = new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); @@ -150,7 +150,7 @@ TEST(TukeyLoss, Optimization) TEST(TukeyLoss, Serialization) { // Construct a loss - const double a{ 0.3 }; + double const a{ 0.3 }; fuse_loss::TukeyLoss expected(a); // Serialize the loss into an archive @@ -172,7 +172,7 @@ TEST(TukeyLoss, Serialization) EXPECT_NE(nullptr, actual.lossFunction()); // Test inlier (s <= a*a) - const double s = 0.95 * a * a; + double const s = 0.95 * a * a; double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); @@ -181,7 +181,7 @@ TEST(TukeyLoss, Serialization) EXPECT_GT(0.0, rho[2]); // Test outlier - const double s_outlier = 1.05 * a * a; + double const s_outlier = 1.05 * a * a; actual.lossFunction()->Evaluate(s_outlier, rho); // In the outlier region rho() satisfies: diff --git a/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp b/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp index ad3cc33de..2caf9f5cb 100644 --- a/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp @@ -61,12 +61,12 @@ class Unicycle2DStateCostFunction : public benchmark::Fixture static const fuse_models::Unicycle2DStateCostFunction cost_function; // Parameters - static const double* parameters[]; + static double const* parameters[]; // Residuals fuse_core::Vector8d residuals; - static const std::vector& block_sizes; + static std::vector const& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; @@ -76,45 +76,45 @@ class Unicycle2DStateCostFunction : public benchmark::Fixture private: // Cost function process noise and covariance - static const double process_noise_diagonal[]; + static double const process_noise_diagonal[]; static const fuse_core::Matrix8d covariance; // Parameter blocks - static const double position1[]; - static const double yaw1[]; - static const double vel_linear1[]; - static const double vel_yaw1[]; - static const double acc_linear1[]; - - static const double position2[]; - static const double yaw2[]; - static const double vel_linear2[]; - static const double vel_yaw2[]; - static const double acc_linear2[]; + static double const position1[]; + static double const yaw1[]; + static double const vel_linear1[]; + static double const vel_yaw1[]; + static double const acc_linear1[]; + + static double const position2[]; + static double const yaw2[]; + static double const vel_linear2[]; + static double const vel_yaw2[]; + static double const acc_linear2[]; // Jacobian matrices std::vector J; }; // Cost function process noise and covariance -const double Unicycle2DStateCostFunction::process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; +double const Unicycle2DStateCostFunction::process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d Unicycle2DStateCostFunction::covariance = fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); // Parameter blocks -const double Unicycle2DStateCostFunction::position1[] = { 0.0, 0.0 }; -const double Unicycle2DStateCostFunction::yaw1[] = { 0.0 }; -const double Unicycle2DStateCostFunction::vel_linear1[] = { 1.0, 0.0 }; -const double Unicycle2DStateCostFunction::vel_yaw1[] = { 1.570796327 }; -const double Unicycle2DStateCostFunction::acc_linear1[] = { 1.0, 0.0 }; - -const double Unicycle2DStateCostFunction::position2[] = { 0.105, 0.0 }; -const double Unicycle2DStateCostFunction::yaw2[] = { 0.1570796327 }; -const double Unicycle2DStateCostFunction::vel_linear2[] = { 1.1, 0.0 }; -const double Unicycle2DStateCostFunction::vel_yaw2[] = { 1.570796327 }; -const double Unicycle2DStateCostFunction::acc_linear2[] = { 1.0, 0.0 }; +double const Unicycle2DStateCostFunction::position1[] = { 0.0, 0.0 }; +double const Unicycle2DStateCostFunction::yaw1[] = { 0.0 }; +double const Unicycle2DStateCostFunction::vel_linear1[] = { 1.0, 0.0 }; +double const Unicycle2DStateCostFunction::vel_yaw1[] = { 1.570796327 }; +double const Unicycle2DStateCostFunction::acc_linear1[] = { 1.0, 0.0 }; + +double const Unicycle2DStateCostFunction::position2[] = { 0.105, 0.0 }; +double const Unicycle2DStateCostFunction::yaw2[] = { 0.1570796327 }; +double const Unicycle2DStateCostFunction::vel_linear2[] = { 1.1, 0.0 }; +double const Unicycle2DStateCostFunction::vel_yaw2[] = { 1.570796327 }; +double const Unicycle2DStateCostFunction::acc_linear2[] = { 1.0, 0.0 }; // Analytic cost function const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt().matrixU()); @@ -122,11 +122,11 @@ const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covarian const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{ dt, sqrt_information }; // Parameters -const double* Unicycle2DStateCostFunction::parameters[] = { // NOLINT(whitespace/braces) +double const* Unicycle2DStateCostFunction::parameters[] = { // NOLINT(whitespace/braces) position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; -const std::vector& Unicycle2DStateCostFunction::block_sizes = cost_function.parameter_block_sizes(); +std::vector const& Unicycle2DStateCostFunction::block_sizes = cost_function.parameter_block_sizes(); const size_t Unicycle2DStateCostFunction::num_parameter_blocks = block_sizes.size(); const size_t Unicycle2DStateCostFunction::num_residuals = cost_function.num_residuals(); diff --git a/fuse_models/doc/motion_models/motion_models.md b/fuse_models/doc/motion_models/motion_models.md new file mode 100644 index 000000000..b103c542a --- /dev/null +++ b/fuse_models/doc/motion_models/motion_models.md @@ -0,0 +1,3 @@ +# Motion Models + +[Coming soon](https://github.com/PickNikRobotics/fuse/issues/23) diff --git a/fuse_models/doc/sensor_models/sensor_models.md b/fuse_models/doc/sensor_models/sensor_models.md new file mode 100644 index 000000000..e106f946d --- /dev/null +++ b/fuse_models/doc/sensor_models/sensor_models.md @@ -0,0 +1,5 @@ +# Sensor Models + +[More coming soon](https://github.com/PickNikRobotics/fuse/issues/23) + +[Transform sensor](./transform_sensor.md) diff --git a/fuse_models/doc/sensor_models/transform_sensor.md b/fuse_models/doc/sensor_models/transform_sensor.md new file mode 100644 index 000000000..11c3efb9f --- /dev/null +++ b/fuse_models/doc/sensor_models/transform_sensor.md @@ -0,0 +1,31 @@ +# Transform Sensor + +## [Source Code](../../src/transform_sensor.cpp) + +## Explanation + +This sensor is intended to be used to estimate the position of a body relative to some sensor when only measurements of points on that body can be taken. +For example, this sensor is useful in the case of measuring april tags on some external body and using those measurements to estimate the full pose and twist of that body. +An example of this application can be seen [here](../../../fuse_tutorials/config/fuse_apriltag_tutorial.yaml) (or try `ros2 launch fuse_tutorials fuse_apriltag_tutorial.launch.py` to see the tutorial in action). + +More generally, this sensor takes a transform between some set `estimation_frame` and one of a predefined set of `transforms`. When it receives such a transform, it will then apply a transform to this measurement to change it to be a measurement of the given `target_frame`. Then, it generates a 3D pose transaction for use by its parent optimizer. + +A slightly confusing aspect of the sensor is the need for multiple definitions of the target frame. Every frame in `transforms` needs a corresponding `target_frame` to be published for it to be used. This is simply because `tf` uses a tree data structure, and the target frames are leaves. These should all end up being in the same global location (discounting measurement noise) but will have different names. + +## Example + +For example, say we have `estimation_frame` `camera`, `transforms` {`apriltag_1`, `apriltag_2`}, and `target_frame` `robot_center_of_mass`. + +Our transform sensor receives a transform (published on the standard `/tf` topic) from `camera` to `apriltag_1`. +This measurement transform has translation (-1, 0, 0) (ignore rotation in this example). +This is the transformation to apply to go from `camera` frame to `apriltag_1` frame, so the translation is the negative of the position of the april tag in camera frame. + +There is a static (though it doesn't *need* to be static) transform published from `apriltag_1` to `target_frame_apriltag_1`. +This transform has translation (-0.25, -0.5, 0) (again, ignoring rotation). +It should be noted that this is the negative position of the target frame in the april tag's frame. + +Our sensor will take the measurement from `camera` to `apriltag_1` and transform it to a pose from `camera` to `target_frame` by 'adding' the transform from `apriltag_1` to `target_frame_apriltag_1`. +Thus, our measurement of the actual position of `target_frame` that will be used to generate the constraint will be (1.25, 0.5, 0). +It is not (-1.25, -0.5, 0) because the translation of `T_a_to_b` is the negation of the pose of `b` in frame `a`. + +Finally, this transaction is sent to the optimizer, as normal. diff --git a/fuse_models/include/fuse_models/acceleration_2d.hpp b/fuse_models/include/fuse_models/acceleration_2d.hpp index 3236ccf4b..a4d190354 100644 --- a/fuse_models/include/fuse_models/acceleration_2d.hpp +++ b/fuse_models/include/fuse_models/acceleration_2d.hpp @@ -91,13 +91,13 @@ class Acceleration2D : public fuse_core::AsyncSensorModel * @brief Shadowing extension to the AsyncSensorModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for acceleration messages * @param[in] msg - The acceleration message to process */ - void process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg); + void process(geometry_msgs::msg::AccelWithCovarianceStamped const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index 71e81a24a..af69ccd69 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -65,7 +65,7 @@ namespace common * @param[in] dimension - The erroneous dimension name * @throws runtime_error */ -inline void throwDimensionError(const std::string& dimension) +inline void throwDimensionError(std::string const& dimension) { std::string error = "Dimension " + dimension + " is not valid for this type."; RCLCPP_ERROR_STREAM(rclcpp::get_logger("fuse"), error); @@ -82,7 +82,7 @@ inline void throwDimensionError(const std::string& dimension) * @throws runtime_error if the dimension name is invalid */ template -std::enable_if_t::value, size_t> toIndex(const std::string& dimension) +std::enable_if_t::value, size_t> toIndex(std::string const& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "x") @@ -109,7 +109,7 @@ std::enable_if_t::value, size_t> toIndex(const std::string& dime * @throws runtime_error if the dimension name is invalid */ template -std::enable_if_t::value, size_t> toIndex(const std::string& dimension) +std::enable_if_t::value, size_t> toIndex(std::string const& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "x") @@ -140,7 +140,7 @@ std::enable_if_t::value, size_t> toIndex(const std::string& dime * @throws runtime_error if the dimension name is invalid */ template -std::enable_if_t::value, size_t> toIndex(const std::string& dimension) +std::enable_if_t::value, size_t> toIndex(std::string const& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "yaw" || lower_dim == "z") @@ -163,7 +163,7 @@ std::enable_if_t::value, size_t> toIndex(const std::string& dim * @throws runtime_error if the dimension name is invalid */ template -std::enable_if_t::value && !is_orientation::value, size_t> toIndex(const std::string& dimension) +std::enable_if_t::value && !is_orientation::value, size_t> toIndex(std::string const& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); if (lower_dim == "roll" || lower_dim == "x") @@ -185,7 +185,7 @@ std::enable_if_t::value && !is_orientation::value, size_t> t } template -std::enable_if_t::value && is_orientation::value, size_t> toIndex(const std::string& dimension) +std::enable_if_t::value && is_orientation::value, size_t> toIndex(std::string const& dimension) { // Trick to get roll, pitch, yaw indexes as 0, 1, 2 auto lower_dim = boost::algorithm::to_lower_copy(dimension); @@ -218,7 +218,7 @@ std::enable_if_t::value && is_orientation::value, size_t> to * @throws runtime_error if any dimension name is invalid */ template -std::vector getDimensionIndices(const std::vector& dimension_names) +std::vector getDimensionIndices(std::vector const& dimension_names) { std::vector indices; indices.reserve(dimension_names.size()); diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 16f8a98c1..1c412faba 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -99,9 +99,9 @@ namespace tf2 * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& data_in, +inline void doTransform(geometry_msgs::msg::TwistWithCovarianceStamped const& data_in, geometry_msgs::msg::TwistWithCovarianceStamped& data_out, - const geometry_msgs::msg::TransformStamped& transform) // NOLINT + geometry_msgs::msg::TransformStamped const& transform) // NOLINT { tf2::Vector3 vl; fromMsg(data_in.twist.twist.linear, vl); @@ -125,9 +125,9 @@ inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& da * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline void doTransform(const geometry_msgs::msg::AccelWithCovarianceStamped& data_in, +inline void doTransform(geometry_msgs::msg::AccelWithCovarianceStamped const& data_in, geometry_msgs::msg::AccelWithCovarianceStamped& data_out, - const geometry_msgs::msg::TransformStamped& transform) + geometry_msgs::msg::TransformStamped const& transform) { tf2::Vector3 al; fromMsg(data_in.accel.accel.linear, al); @@ -159,12 +159,12 @@ namespace common * @param[in] rhs_indices - RHS vector of indices * @param[in] rhs_offset - RHS offset to be added to the RHS vector indices (defaults to 0) */ -inline std::vector mergeIndices(const std::vector& lhs_indices, const std::vector& rhs_indices, +inline std::vector mergeIndices(std::vector const& lhs_indices, std::vector const& rhs_indices, const size_t rhs_offset = 0u) { auto merged_indices = boost::copy_range>(boost::range::join(lhs_indices, rhs_indices)); - const auto rhs_it = merged_indices.begin() + static_cast(lhs_indices.size()); + auto const rhs_it = merged_indices.begin() + static_cast(lhs_indices.size()); std::transform(rhs_it, merged_indices.end(), rhs_it, std::bind(std::plus(), std::placeholders::_1, rhs_offset)); @@ -182,8 +182,8 @@ inline std::vector mergeIndices(const std::vector& lhs_indices, * @param[in,out] mean_partial - The partial measurement mean to which we want to append * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append */ -inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, const fuse_core::MatrixXd& covariance_full, - const std::vector& indices, fuse_core::VectorXd& mean_partial, +inline void populatePartialMeasurement(fuse_core::VectorXd const& mean_full, fuse_core::MatrixXd const& covariance_full, + std::vector const& indices, fuse_core::VectorXd& mean_partial, fuse_core::MatrixXd& covariance_partial) { for (int64_t r = 0; r < static_cast(indices.size()); ++r) @@ -206,7 +206,7 @@ inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, con * @param[in] indices - The indices we want to include in the sub-measurement * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append */ -inline void populatePartialMeasurement(const fuse_core::MatrixXd& covariance_full, const std::vector& indices, +inline void populatePartialMeasurement(fuse_core::MatrixXd const& covariance_full, std::vector const& indices, fuse_core::MatrixXd& covariance_partial) { for (int64_t r = 0; r < static_cast(indices.size()); ++r) @@ -226,9 +226,9 @@ inline void populatePartialMeasurement(const fuse_core::MatrixXd& covariance_ful * @param[in] covariance_partial - The partial measurement covariance we want to validate * @param[in] precision - The precision to validate the partial measurements covariance is symmetric */ -inline void validatePartialMeasurement(const fuse_core::VectorXd& mean_partial, - const fuse_core::MatrixXd& covariance_partial, - const double precision = Eigen::NumTraits::dummy_precision()) +inline void validatePartialMeasurement(fuse_core::VectorXd const& mean_partial, + fuse_core::MatrixXd const& covariance_partial, + double const precision = Eigen::NumTraits::dummy_precision()) { if (!mean_partial.allFinite()) { @@ -248,8 +248,8 @@ inline void validatePartialMeasurement(const fuse_core::VectorXd& mean_partial, } } -inline void validateMeasurement(const fuse_core::VectorXd& mean, const fuse_core::MatrixXd& covariance, - const double precision = Eigen::NumTraits::dummy_precision()) +inline void validateMeasurement(fuse_core::VectorXd const& mean, fuse_core::MatrixXd const& covariance, + double const precision = Eigen::NumTraits::dummy_precision()) { if (!mean.allFinite()) { @@ -279,8 +279,8 @@ inline void validateMeasurement(const fuse_core::VectorXd& mean, const fuse_core * @return true if the transform succeeded, false otherwise */ template -bool transformMessage(const tf2_ros::Buffer& tf_buffer, const T& input, T& output, - const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) +bool transformMessage(tf2_ros::Buffer const& tf_buffer, const T& input, T& output, + rclcpp::Duration const& tf_timeout = rclcpp::Duration(0, 0)) { try { @@ -296,7 +296,7 @@ bool transformMessage(const tf2_ros::Buffer& tf_buffer, const T& input, T& outpu tf2::doTransform(input, output, trans); return true; } - catch (const tf2::TransformException& ex) + catch (tf2::TransformException const& ex) { RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 5.0 * 1000, "Could not transform message from " << input.header.frame_id << " to " @@ -328,14 +328,14 @@ bool transformMessage(const tf2_ros::Buffer& tf_buffer, const T& input, T& outpu * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAbsolutePoseWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, - const std::vector& position_indices, - const std::vector& orientation_indices, - const tf2_ros::Buffer& tf_buffer, const bool validate, +inline bool processAbsolutePoseWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + fuse_core::Loss::SharedPtr const& loss, std::string const& target_frame, + std::vector const& position_indices, + std::vector const& orientation_indices, + tf2_ros::Buffer const& tf_buffer, bool const validate, fuse_core::Transaction& transaction, - const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) + rclcpp::Duration const& tf_timeout = rclcpp::Duration(0, 0)) { if (position_indices.empty() && orientation_indices.empty()) { @@ -388,7 +388,7 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - const auto indices = mergeIndices(position_indices, orientation_indices, position->size()); + auto const indices = mergeIndices(position_indices, orientation_indices, position->size()); populatePartialMeasurement(pose_mean, pose_covariance, indices, pose_mean_partial, pose_covariance_partial); @@ -398,7 +398,7 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f { validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source @@ -445,14 +445,14 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAbsolutePose3DWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, - const std::vector& position_indices, - const std::vector& orientation_indices, - const tf2_ros::Buffer& tf_buffer, const bool validate, +inline bool processAbsolutePose3DWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + fuse_core::Loss::SharedPtr const& loss, std::string const& target_frame, + std::vector const& position_indices, + std::vector const& orientation_indices, + tf2_ros::Buffer const& tf_buffer, bool const validate, fuse_core::Transaction& transaction, - const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) + rclcpp::Duration const& tf_timeout = rclcpp::Duration(0, 0)) { if (position_indices.empty() && orientation_indices.empty()) { @@ -505,7 +505,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const { validatePartialMeasurement(pose_mean, pose_covariance, 1e-5); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source @@ -538,10 +538,10 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); // Set the components which are not measured to zero - const auto indices = mergeIndices(position_indices, orientation_indices, 3); + auto const indices = mergeIndices(position_indices, orientation_indices, 3); std::replace_if( pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), - [&indices, &pose_mean_partial](const double& value) { + [&indices, &pose_mean_partial](double const& value) { return std::find(indices.begin(), indices.end(), &value - pose_mean_partial.data()) == indices.end(); }, 0.0); @@ -554,7 +554,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const { validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source @@ -607,14 +607,14 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPoseWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, - const bool independent, - const fuse_core::Matrix3d& minimum_pose_relative_covariance, - const fuse_core::Loss::SharedPtr& loss, - const std::vector& position_indices, - const std::vector& orientation_indices, const bool validate, +inline bool processDifferentialPoseWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose1, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose2, + bool const independent, + fuse_core::Matrix3d const& minimum_pose_relative_covariance, + fuse_core::Loss::SharedPtr const& loss, + std::vector const& position_indices, + std::vector const& orientation_indices, bool const validate, fuse_core::Transaction& transaction) { if (position_indices.empty() && orientation_indices.empty()) @@ -643,8 +643,8 @@ inline bool processDifferentialPoseWithCovariance(const std::string& source, con orientation2->yaw() = pose2_2d.yaw(); // Create the delta for the constraint - const double sy = ::sin(-pose1_2d.yaw()); - const double cy = ::cos(-pose1_2d.yaw()); + double const sy = ::sin(-pose1_2d.yaw()); + double const cy = ::cos(-pose1_2d.yaw()); double x_diff = pose2_2d.x() - pose1_2d.x(); double y_diff = pose2_2d.y() - pose1_2d.y(); fuse_core::VectorXd pose_relative_mean(3); @@ -861,7 +861,7 @@ inline bool processDifferentialPoseWithCovariance(const std::string& source, con fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), pose_relative_mean_partial.rows()); - const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); + auto const indices = mergeIndices(position_indices, orientation_indices, position1->size()); populatePartialMeasurement(pose_relative_mean, pose_relative_covariance, indices, pose_relative_mean_partial, pose_relative_covariance_partial); @@ -872,7 +872,7 @@ inline bool processDifferentialPoseWithCovariance(const std::string& source, con { validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source @@ -931,14 +931,14 @@ inline bool processDifferentialPoseWithCovariance(const std::string& source, con * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPose3DWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, - const bool independent, - const fuse_core::Matrix6d& minimum_pose_relative_covariance, - const fuse_core::Loss::SharedPtr& loss, - const std::vector& position_indices, - const std::vector& orientation_indices, const bool validate, +inline bool processDifferentialPose3DWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose1, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose2, + bool const independent, + fuse_core::Matrix6d const& minimum_pose_relative_covariance, + fuse_core::Loss::SharedPtr const& loss, + std::vector const& position_indices, + std::vector const& orientation_indices, bool const validate, fuse_core::Transaction& transaction) { // Create the pose variables @@ -1061,7 +1061,7 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c { validateMeasurement(pose_relative_mean, pose_relative_covariance, 1e-5); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" @@ -1096,14 +1096,14 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; - const auto indices = mergeIndices(position_indices, orientation_indices, 3); + auto const indices = mergeIndices(position_indices, orientation_indices, 3); fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); // Set the components which are not measured to zero std::replace_if( pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), - [&indices, &pose_relative_mean_partial](const double& value) { + [&indices, &pose_relative_mean_partial](double const& value) { return std::find(indices.begin(), indices.end(), &value - pose_relative_mean_partial.data()) == indices.end(); }, 0.0); @@ -1116,7 +1116,7 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c { validateMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-5); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source @@ -1175,16 +1175,16 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPoseWithTwistCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, - const fuse_core::Matrix3d& minimum_pose_relative_covariance, - const fuse_core::Matrix3d& twist_covariance_offset, - const fuse_core::Loss::SharedPtr& loss, - const std::vector& position_indices, - const std::vector& orientation_indices, - const bool validate, fuse_core::Transaction& transaction) +inline bool processDifferentialPoseWithTwistCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose1, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose2, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, + fuse_core::Matrix3d const& minimum_pose_relative_covariance, + fuse_core::Matrix3d const& twist_covariance_offset, + fuse_core::Loss::SharedPtr const& loss, + std::vector const& position_indices, + std::vector const& orientation_indices, + bool const validate, fuse_core::Transaction& transaction) { if (position_indices.empty() && orientation_indices.empty()) { @@ -1212,7 +1212,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source orientation2->yaw() = pose2_2d.yaw(); // Create the delta for the constraint - const auto delta = pose1_2d.inverseTimes(pose2_2d); + auto const delta = pose1_2d.inverseTimes(pose2_2d); fuse_core::VectorXd pose_relative_mean(3); pose_relative_mean << delta.x(), delta.y(), delta.yaw(); @@ -1257,7 +1257,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source // // It is also common that for the same reason, the twist covariance T12 already has a minimum // covariance offset added to it by the publisher, so we have to remove it before using it. - const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); + auto const dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); if (dt < 1e-6) { @@ -1278,7 +1278,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), pose_relative_mean_partial.rows()); - const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); + auto const indices = mergeIndices(position_indices, orientation_indices, position1->size()); populatePartialMeasurement(pose_relative_mean, pose_relative_covariance, indices, pose_relative_mean_partial, pose_relative_covariance_partial); @@ -1289,7 +1289,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source { validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" @@ -1348,16 +1348,16 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPose3DWithTwistCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, - const fuse_core::Matrix6d& minimum_pose_relative_covariance, - const fuse_core::Matrix6d& twist_covariance_offset, - const fuse_core::Loss::SharedPtr& loss, - const std::vector& position_indices, - const std::vector& orientation_indices, - const bool validate, fuse_core::Transaction& transaction) +inline bool processDifferentialPose3DWithTwistCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose1, + geometry_msgs::msg::PoseWithCovarianceStamped const& pose2, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, + fuse_core::Matrix6d const& minimum_pose_relative_covariance, + fuse_core::Matrix6d const& twist_covariance_offset, + fuse_core::Loss::SharedPtr const& loss, + std::vector const& position_indices, + std::vector const& orientation_indices, + bool const validate, fuse_core::Transaction& transaction) { if (position_indices.empty() && orientation_indices.empty()) { @@ -1392,12 +1392,12 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour orientation2->w() = pose2_tf2.getRotation().w(); // Create the delta for the constraint - const auto delta = pose1_tf2.inverseTimes(pose2_tf2); + auto const delta = pose1_tf2.inverseTimes(pose2_tf2); // Create the covariance components for the constraint Eigen::Map cov(twist.twist.covariance.data()); - const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); + auto const dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); if (dt < 1e-6) { @@ -1426,7 +1426,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour { validatePartialMeasurement(pose_relative_mean, pose_relative_covariance, 1e-4); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" @@ -1459,10 +1459,10 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour .getRPY(pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); // Set the components which are not measured to zero - const auto indices = mergeIndices(position_indices, orientation_indices, 3); + auto const indices = mergeIndices(position_indices, orientation_indices, 3); std::replace_if( pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), - [&indices, &pose_relative_mean_partial](const double& value) { + [&indices, &pose_relative_mean_partial](double const& value) { return std::find(indices.begin(), indices.end(), &value - pose_relative_mean_partial.data()) == indices.end(); }, 0.0); @@ -1475,7 +1475,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour { validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-4); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" @@ -1528,14 +1528,14 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processTwistWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, - const fuse_core::Loss::SharedPtr& linear_velocity_loss, - const fuse_core::Loss::SharedPtr& angular_velocity_loss, - const std::string& target_frame, const std::vector& linear_indices, - const std::vector& angular_indices, const tf2_ros::Buffer& tf_buffer, - const bool validate, fuse_core::Transaction& transaction, - const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) +inline bool processTwistWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, + fuse_core::Loss::SharedPtr const& linear_velocity_loss, + fuse_core::Loss::SharedPtr const& angular_velocity_loss, + std::string const& target_frame, std::vector const& linear_indices, + std::vector const& angular_indices, tf2_ros::Buffer const& tf_buffer, + bool const validate, fuse_core::Transaction& transaction, + rclcpp::Duration const& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do if (linear_indices.empty() && angular_indices.empty()) @@ -1595,7 +1595,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor { validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear velocity measurement from '" << source @@ -1637,7 +1637,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor { validatePartialMeasurement(angular_vel_vector, angular_vel_covariance); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Invalid partial angular velocity measurement from '" @@ -1695,14 +1695,14 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processTwist3DWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, - const fuse_core::Loss::SharedPtr& linear_velocity_loss, - const fuse_core::Loss::SharedPtr& angular_velocity_loss, - const std::string& target_frame, const std::vector& linear_indices, - const std::vector& angular_indices, const tf2_ros::Buffer& tf_buffer, - const bool validate, fuse_core::Transaction& transaction, - const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) +inline bool processTwist3DWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, + fuse_core::Loss::SharedPtr const& linear_velocity_loss, + fuse_core::Loss::SharedPtr const& angular_velocity_loss, + std::string const& target_frame, std::vector const& linear_indices, + std::vector const& angular_indices, tf2_ros::Buffer const& tf_buffer, + bool const validate, fuse_core::Transaction& transaction, + rclcpp::Duration const& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do if (linear_indices.empty() && angular_indices.empty()) @@ -1763,7 +1763,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c { validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial, 1e-5); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear velocity measurement from '" << source @@ -1815,7 +1815,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c { validatePartialMeasurement(angular_vel_mean_partial, angular_vel_covariance_partial, 1e-5); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Invalid partial angular velocity measurement from '" @@ -1867,12 +1867,12 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAccelWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::AccelWithCovarianceStamped& acceleration, - const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, - const std::vector& indices, const tf2_ros::Buffer& tf_buffer, - const bool validate, fuse_core::Transaction& transaction, - const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) +inline bool processAccelWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::AccelWithCovarianceStamped const& acceleration, + fuse_core::Loss::SharedPtr const& loss, std::string const& target_frame, + std::vector const& indices, tf2_ros::Buffer const& tf_buffer, + bool const validate, fuse_core::Transaction& transaction, + rclcpp::Duration const& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do if (indices.empty()) @@ -1925,7 +1925,7 @@ inline bool processAccelWithCovariance(const std::string& source, const fuse_cor { validatePartialMeasurement(accel_mean_partial, accel_covariance_partial); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear acceleration measurement from '" @@ -1970,12 +1970,12 @@ inline bool processAccelWithCovariance(const std::string& source, const fuse_cor * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAccel3DWithCovariance(const std::string& source, const fuse_core::UUID& device_id, - const geometry_msgs::msg::AccelWithCovarianceStamped& acceleration, - const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, - const std::vector& indices, const tf2_ros::Buffer& tf_buffer, - const bool validate, fuse_core::Transaction& transaction, - const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) +inline bool processAccel3DWithCovariance(std::string const& source, fuse_core::UUID const& device_id, + geometry_msgs::msg::AccelWithCovarianceStamped const& acceleration, + fuse_core::Loss::SharedPtr const& loss, std::string const& target_frame, + std::vector const& indices, tf2_ros::Buffer const& tf_buffer, + bool const validate, fuse_core::Transaction& transaction, + rclcpp::Duration const& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do if (indices.empty()) @@ -2022,7 +2022,7 @@ inline bool processAccel3DWithCovariance(const std::string& source, const fuse_c { validatePartialMeasurement(accel_mean_partial, accel_covariance_partial, 1e-4); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear acceleration measurement from '" @@ -2062,8 +2062,8 @@ inline bool processAccel3DWithCovariance(const std::string& source, const fuse_c * @param[in] velocity_norm_min - The minimum velocity norm */ inline void scaleProcessNoiseCovariance(fuse_core::Matrix8d& process_noise_covariance, - const tf2_2d::Vector2& velocity_linear, const double velocity_yaw, - const double velocity_norm_min) + tf2_2d::Vector2 const& velocity_linear, double const velocity_yaw, + double const velocity_norm_min) { // A more principled approach would be to get the current velocity from the state, make a diagonal // matrix from it, and then rotate it to be in the world frame (i.e., the same frame as the pose @@ -2101,9 +2101,9 @@ inline void scaleProcessNoiseCovariance(fuse_core::Matrix8d& process_noise_covar * @param[in] velocity_angular_norm_min - The minimum angular velocity norm */ inline void scaleProcessNoiseCovariance(fuse_core::Matrix15d& process_noise_covariance, - const fuse_core::Vector3d& velocity_linear, - const fuse_core::Vector3d& velocity_angular, - const double velocity_linear_norm_min, const double velocity_angular_norm_min) + fuse_core::Vector3d const& velocity_linear, + fuse_core::Vector3d const& velocity_angular, + double const velocity_linear_norm_min, double const velocity_angular_norm_min) { fuse_core::Matrix6d velocity; velocity.setIdentity(); diff --git a/fuse_models/include/fuse_models/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index 803d5f62d..2c4a30b4f 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.hpp +++ b/fuse_models/include/fuse_models/common/variable_traits.hpp @@ -54,99 +54,99 @@ namespace common template struct is_linear_2d { - static const bool value = false; + static bool const value = false; }; template <> struct is_linear_2d { - static const bool value = true; + static bool const value = true; }; template <> struct is_linear_2d { - static const bool value = true; + static bool const value = true; }; template <> struct is_linear_2d { - static const bool value = true; + static bool const value = true; }; template struct is_linear_3d { - static const bool value = false; + static bool const value = false; }; template <> struct is_linear_3d { - static const bool value = true; + static bool const value = true; }; template <> struct is_linear_3d { - static const bool value = true; + static bool const value = true; }; template <> struct is_linear_3d { - static const bool value = true; + static bool const value = true; }; template struct is_angular_2d { - static const bool value = false; + static bool const value = false; }; template <> struct is_angular_2d { - static const bool value = true; + static bool const value = true; }; template <> struct is_angular_2d { - static const bool value = true; + static bool const value = true; }; template struct is_angular_3d { - static const bool value = false; + static bool const value = false; }; template <> struct is_angular_3d { - static const bool value = true; + static bool const value = true; }; template <> struct is_angular_3d { - static const bool value = true; + static bool const value = true; }; template struct is_orientation { - static const bool value = false; + static bool const value = false; }; template <> struct is_orientation { - static const bool value = true; + static bool const value = true; }; template <> struct is_orientation { - static const bool value = true; + static bool const value = true; }; } // namespace common diff --git a/fuse_models/include/fuse_models/graph_ignition.hpp b/fuse_models/include/fuse_models/graph_ignition.hpp index 6d105ba99..4cac96428 100644 --- a/fuse_models/include/fuse_models/graph_ignition.hpp +++ b/fuse_models/include/fuse_models/graph_ignition.hpp @@ -96,7 +96,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * @brief Shadowing extension to the AsyncSensorModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -124,7 +124,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph */ - void subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg); + void subscriberCallback(fuse_msgs::msg::SerializedGraph const& msg); /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph @@ -146,7 +146,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * * @param[in] msg - The graph message */ - void process(const fuse_msgs::msg::SerializedGraph& msg, std::function post_process = nullptr); + void process(fuse_msgs::msg::SerializedGraph const& msg, std::function post_process = nullptr); /** * @brief Create and send a transaction equivalent to the supplied graph @@ -154,7 +154,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * @param[in] graph - The graph * @param[in] stamp - The graph stamp */ - void sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp); + void sendGraph(fuse_core::Graph const& graph, rclcpp::Time const& stamp); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The IMU message to process */ - void process(const sensor_msgs::msg::Imu& msg); + void process(sensor_msgs::msg::Imu const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -156,8 +156,8 @@ class Imu2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + void processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool const validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The IMU message to process */ - void process(const sensor_msgs::msg::Imu& msg); + void process(sensor_msgs::msg::Imu const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -161,8 +161,8 @@ class Imu3D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, bool validate, + void processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces& constraints, std::vector& variables); diff --git a/fuse_models/include/fuse_models/odometry_2d.hpp b/fuse_models/include/fuse_models/odometry_2d.hpp index d43083ce5..93aaa996e 100644 --- a/fuse_models/include/fuse_models/odometry_2d.hpp +++ b/fuse_models/include/fuse_models/odometry_2d.hpp @@ -107,13 +107,13 @@ class Odometry2D : public fuse_core::AsyncSensorModel * @brief Shadowing extension to the AsyncSensorModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const nav_msgs::msg::Odometry& msg); + void process(nav_msgs::msg::Odometry const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -147,8 +147,8 @@ class Odometry2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + void processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool const validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; protected: /** @@ -173,7 +173,7 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher * structure * @return true if the checks pass, false otherwise */ - bool getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id, + bool getState(fuse_core::Graph const& graph, rclcpp::Time const& stamp, fuse_core::UUID const& device_id, fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp index bec0dfce7..34ad0446d 100644 --- a/fuse_models/include/fuse_models/odometry_3d.hpp +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -111,13 +111,13 @@ class Odometry3D : public fuse_core::AsyncSensorModel * @brief Shadowing extension to the AsyncSensorModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const nav_msgs::msg::Odometry& msg); + void process(nav_msgs::msg::Odometry const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -151,8 +151,8 @@ class Odometry3D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, bool validate, + void processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; protected: // helper function @@ -183,7 +183,7 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * structure * @return true if the checks pass, false otherwise */ - bool getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id, + bool getState(fuse_core::Graph const& graph, rclcpp::Time const& stamp, fuse_core::UUID const& device_id, fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index 1532422b3..249b8392e 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -94,7 +94,7 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @brief Shadowing extension to the AsyncMotionModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; void print(std::ostream& stream = std::cout) const; @@ -159,7 +159,7 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * ending_stamp. The variables should include initial values for the * optimizer. */ - void generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + void generateMotionModel(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables); @@ -188,8 +188,8 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @param[in] state_history The state history object to be updated * @param[in] buffer_length States older than this in the history will be pruned */ - static void updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, - const rclcpp::Duration& buffer_length); + static void updateStateHistoryEstimates(fuse_core::Graph const& graph, StateHistory& state_history, + rclcpp::Duration const& buffer_length); /** * @brief Validate the motion model state #1, state #2 and process noise covariance @@ -202,8 +202,8 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @param[in] process_noise_covariance The process noise covariance, after it is scaled and * multiplied by dt */ - static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, - const fuse_core::Matrix15d& process_noise_covariance); + static void validateMotionModel(StateHistoryElement const& state1, StateHistoryElement const& state2, + fuse_core::Matrix15d const& process_noise_covariance); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -138,7 +138,7 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); + void subscriberCallback(geometry_msgs::msg::PoseWithCovarianceStamped const& msg); /** * @brief Triggers the publication of a new prior transaction at the supplied pose @@ -167,7 +167,7 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) */ - void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, std::function post_process = nullptr); + void process(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, std::function post_process = nullptr); /** * @brief Create and send a prior transaction based on the supplied pose @@ -178,7 +178,7 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) */ - void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); + void sendPrior(geometry_msgs::msg::PoseWithCovarianceStamped const& pose); fuse_core::node_interfaces::NodeInterfaces - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& dt_; diff --git a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp index fdf6b4f64..99f8ce766 100644 --- a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp @@ -64,7 +64,7 @@ struct Acceleration2DParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp index b22826135..010a6d785 100644 --- a/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp @@ -61,7 +61,7 @@ struct GraphIgnitionParams : public fuse_models::parameters::ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); reset_service = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "reset_service"), reset_service); diff --git a/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp index f8cf24fe8..91454dbb4 100644 --- a/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp @@ -67,7 +67,7 @@ struct Imu2DParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { angular_velocity_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index b64372911..d3c80d965 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -67,7 +67,7 @@ struct Imu3DParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { angular_velocity_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp index f178d8bda..a39fc8e29 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp @@ -68,7 +68,7 @@ struct Odometry2DParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { position_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp index c65f9c361..927adfc16 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp @@ -73,7 +73,7 @@ struct Odometry2DPublisherParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf); invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf); @@ -108,7 +108,7 @@ struct Odometry2DPublisherParams : public ParameterBase world_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "world_frame_id"), world_frame_id); - const bool frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && + bool const frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && map_frame_id != base_link_output_frame_id && odom_frame_id != base_link_frame_id && odom_frame_id != base_link_output_frame_id && (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 9fce065cd..64c75c601 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -65,7 +65,7 @@ struct Odometry3DParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { position_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index e74fdcb5e..6d1f922b9 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -71,7 +71,7 @@ struct Odometry3DPublisherParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf); invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf); @@ -111,7 +111,7 @@ struct Odometry3DPublisherParams : public ParameterBase world_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "world_frame_id"), world_frame_id); - const bool frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && + bool const frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && map_frame_id != base_link_output_frame_id && odom_frame_id != base_link_frame_id && odom_frame_id != base_link_output_frame_id && (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp index a187989f3..ea6393a81 100644 --- a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -66,7 +66,7 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { publish_on_startup = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_on_startup"), publish_on_startup); @@ -88,7 +88,7 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase "is actually length " + std::to_string(sigma_vector.size())); } - auto is_sigma_valid = [](const double sigma) { return std::isfinite(sigma) && (sigma > 0); }; + auto is_sigma_valid = [](double const sigma) { return std::isfinite(sigma) && (sigma > 0); }; if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " @@ -106,7 +106,7 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase throw std::invalid_argument("The supplied initial_state parameter must be length 15, but is actually length " + std::to_string(state_vector.size())); } - auto is_state_valid = [](const double state) { return std::isfinite(state); }; + auto is_state_valid = [](double const state) { return std::isfinite(state); }; if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " diff --git a/fuse_models/include/fuse_models/parameters/parameter_base.hpp b/fuse_models/include/fuse_models/parameters/parameter_base.hpp index 0508d9041..7e20ec979 100644 --- a/fuse_models/include/fuse_models/parameters/parameter_base.hpp +++ b/fuse_models/include/fuse_models/parameters/parameter_base.hpp @@ -62,7 +62,7 @@ struct ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) = 0; + std::string const& ns) = 0; }; /** @@ -77,7 +77,7 @@ struct ParameterBase template inline std::vector loadSensorConfig(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { std::vector dimensions; dimensions = fuse_core::getParam(interfaces, name, dimensions); diff --git a/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp b/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp index 82bbe0cc0..b36df77df 100644 --- a/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp @@ -66,7 +66,7 @@ struct Pose2DParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { position_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/transaction_params.hpp b/fuse_models/include/fuse_models/parameters/transaction_params.hpp index 95669e555..277bad388 100644 --- a/fuse_models/include/fuse_models/parameters/transaction_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transaction_params.hpp @@ -63,7 +63,7 @@ struct TransactionParams : public fuse_models::parameters::ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index 1e714c241..33917ef1c 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -63,7 +63,7 @@ struct TransformSensorParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { position_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp b/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp index 62061d1b7..ce5817745 100644 --- a/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp @@ -66,7 +66,7 @@ struct Twist2DParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { linear_indices = loadSensorConfig( interfaces, fuse_core::joinParameterName(ns, "linear_dimensions")); diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp index d55023858..e22207b02 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp @@ -66,7 +66,7 @@ struct Unicycle2DIgnitionParams : public ParameterBase fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& ns) + std::string const& ns) { publish_on_startup = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_on_startup"), publish_on_startup); @@ -88,7 +88,7 @@ struct Unicycle2DIgnitionParams : public ParameterBase "is actually length " + std::to_string(sigma_vector.size())); } - auto is_sigma_valid = [](const double sigma) { return std::isfinite(sigma) && (sigma > 0); }; + auto is_sigma_valid = [](double const sigma) { return std::isfinite(sigma) && (sigma > 0); }; if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " @@ -106,7 +106,7 @@ struct Unicycle2DIgnitionParams : public ParameterBase throw std::invalid_argument("The supplied initial_state parameter must be length 8, but is actually length " + std::to_string(state_vector.size())); } - auto is_state_valid = [](const double state) { return std::isfinite(state); }; + auto is_state_valid = [](double const state) { return std::isfinite(state); }; if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " diff --git a/fuse_models/include/fuse_models/pose_2d.hpp b/fuse_models/include/fuse_models/pose_2d.hpp index c5bc02ae7..c28c6b3d5 100644 --- a/fuse_models/include/fuse_models/pose_2d.hpp +++ b/fuse_models/include/fuse_models/pose_2d.hpp @@ -100,13 +100,13 @@ class Pose2D : public fuse_core::AsyncSensorModel * @brief Shadowing extension to the AsyncSensorModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); + void process(geometry_msgs::msg::PoseWithCovarianceStamped const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -138,7 +138,7 @@ class Pose2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, bool validate, + void processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; protected: /** @@ -105,7 +105,7 @@ class Transaction : public fuse_core::AsyncSensorModel * @brief Callback for transaction messages * @param[in] msg - The transaction message to process */ - void process(const fuse_msgs::msg::SerializedTransaction& msg); + void process(fuse_msgs::msg::SerializedTransaction const& msg); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for tf messages * @param[in] msg - The IMU message to process */ - void process(const MessageType& msg); + void process(MessageType const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device diff --git a/fuse_models/include/fuse_models/twist_2d.hpp b/fuse_models/include/fuse_models/twist_2d.hpp index de3494c59..58110c1dc 100644 --- a/fuse_models/include/fuse_models/twist_2d.hpp +++ b/fuse_models/include/fuse_models/twist_2d.hpp @@ -91,13 +91,13 @@ class Twist2D : public fuse_core::AsyncSensorModel * @brief Shadowing extension to the AsyncSensorModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for twist messages * @param[in] msg - The twist message to process */ - void process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg); + void process(geometry_msgs::msg::TwistWithCovarianceStamped const& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device diff --git a/fuse_models/include/fuse_models/unicycle_2d.hpp b/fuse_models/include/fuse_models/unicycle_2d.hpp index 808d9865e..2c2a988e8 100644 --- a/fuse_models/include/fuse_models/unicycle_2d.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d.hpp @@ -92,7 +92,7 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * @brief Shadowing extension to the AsyncMotionModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; void print(std::ostream& stream = std::cout) const; @@ -156,7 +156,7 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * ending_stamp. The variables should include initial values for the * optimizer. */ - void generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + void generateMotionModel(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables); @@ -185,8 +185,8 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * @param[in] state_history The state history object to be updated * @param[in] buffer_length States older than this in the history will be pruned */ - static void updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, - const rclcpp::Duration& buffer_length); + static void updateStateHistoryEstimates(fuse_core::Graph const& graph, StateHistory& state_history, + rclcpp::Duration const& buffer_length); /** * @brief Validate the motion model state #1, state #2 and process noise covariance @@ -199,8 +199,8 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * @param[in] process_noise_covariance The process noise covariance, after it is scaled and * multiplied by dt */ - static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, - const fuse_core::Matrix8d& process_noise_covariance); + static void validateMotionModel(StateHistoryElement const& state1, StateHistoryElement const& state2, + fuse_core::Matrix8d const& process_noise_covariance); fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -135,20 +139,21 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); + void subscriberCallback(geometry_msgs::msg::PoseWithCovarianceStamped const& msg); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseServiceCallback(rclcpp::Service::SharedPtr service, - std::shared_ptr, const fuse_msgs::srv::SetPose::Request::SharedPtr req); + bool setPoseServiceCallback(rclcpp::Service::SharedPtr const& service, + std::shared_ptr const&, + fuse_msgs::srv::SetPose::Request::SharedPtr const& req); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr const& service, + std::shared_ptr const& request_id, + fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr const& req); protected: /** @@ -164,7 +169,8 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, std::function post_process = nullptr); + void process(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + std::function const& post_process = nullptr); /** * @brief Create and send a prior transaction based on the supplied pose @@ -175,7 +181,7 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); + void sendPrior(geometry_msgs::msg::PoseWithCovarianceStamped const& pose); fuse_core::node_interfaces::NodeInterfaces> jacobian(jacobians[2]); jacobian << cy_dt, -sy_dt, sy_dt, cy_dt, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0; @@ -175,8 +175,8 @@ inline void predict(const double position1_x, const double position1_y, const do // Jacobian wrt acc_linear1 if (jacobians[4]) { - const double cy_half_dt2 = cy * half_dt2; - const double sy_half_dt2 = sy * half_dt2; + double const cy_half_dt2 = cy * half_dt2; + double const sy_half_dt2 = sy * half_dt2; Eigen::Map> jacobian(jacobians[4]); jacobian << cy_half_dt2, -sy_half_dt2, sy_half_dt2, cy_half_dt2, 0, 0, dt, 0, 0, dt, 0, 0, 1, 0, 0, 1; @@ -221,8 +221,8 @@ inline void predict(const T* const position1, const T* const yaw1, const T* cons * @param[in] acc_linear2 - The second linear acceleration * @param[in] jacobian - The jacobian wrt the state */ -inline void predict(const tf2_2d::Transform& pose1, const tf2_2d::Vector2& vel_linear1, const double vel_yaw1, - const tf2_2d::Vector2& acc_linear1, const double dt, tf2_2d::Transform& pose2, +inline void predict(tf2_2d::Transform const& pose1, tf2_2d::Vector2 const& vel_linear1, double const vel_yaw1, + tf2_2d::Vector2 const& acc_linear1, double const dt, tf2_2d::Transform& pose2, tf2_2d::Vector2& vel_linear2, double& vel_yaw2, tf2_2d::Vector2& acc_linear2, fuse_core::Matrix8d& jacobian) { @@ -279,8 +279,8 @@ inline void predict(const tf2_2d::Transform& pose1, const tf2_2d::Vector2& vel_l * @param[in] vel_yaw2 - The second yaw velocity * @param[in] acc_linear2 - The second linear acceleration */ -inline void predict(const tf2_2d::Transform& pose1, const tf2_2d::Vector2& vel_linear1, const double vel_yaw1, - const tf2_2d::Vector2& acc_linear1, const double dt, tf2_2d::Transform& pose2, +inline void predict(tf2_2d::Transform const& pose1, tf2_2d::Vector2 const& vel_linear1, double const vel_yaw1, + tf2_2d::Vector2 const& acc_linear1, double const dt, tf2_2d::Transform& pose2, tf2_2d::Vector2& vel_linear2, double& vel_yaw2, tf2_2d::Vector2& acc_linear2) { double x_pred{}; diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp index cdae9ce35..2bafd1853 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp @@ -93,7 +93,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A); + Unicycle2DStateCostFunction(double const dt, fuse_core::Matrix8d const& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -269,7 +269,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, //!< information matrix }; -Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) +Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(double const dt, fuse_core::Matrix8d const& A) : dt_(dt), A_(A) { } diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp index a37f49d69..6ba7c9ee4 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp @@ -91,7 +91,7 @@ class Unicycle2DStateCostFunctor * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A); + Unicycle2DStateCostFunctor(double const dt, fuse_core::Matrix8d const& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -118,7 +118,7 @@ class Unicycle2DStateCostFunctor //!< information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(double const dt, fuse_core::Matrix8d const& A) : dt_(dt), A_(A) { } diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp index ae86596ca..85e0c7b55 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp @@ -92,17 +92,17 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * @param[in] covariance - The covariance matrix used to weight the constraint. Order is (x, y, * yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateKinematicConstraint(const std::string& source, const fuse_variables::Position2DStamped& position1, - const fuse_variables::Orientation2DStamped& yaw1, - const fuse_variables::VelocityLinear2DStamped& linear_velocity1, - const fuse_variables::VelocityAngular2DStamped& yaw_velocity1, - const fuse_variables::AccelerationLinear2DStamped& linear_acceleration1, - const fuse_variables::Position2DStamped& position2, - const fuse_variables::Orientation2DStamped& yaw2, - const fuse_variables::VelocityLinear2DStamped& linear_velocity2, - const fuse_variables::VelocityAngular2DStamped& yaw_velocity2, - const fuse_variables::AccelerationLinear2DStamped& linear_acceleration2, - const fuse_core::Matrix8d& covariance); + Unicycle2DStateKinematicConstraint(std::string const& source, fuse_variables::Position2DStamped const& position1, + fuse_variables::Orientation2DStamped const& yaw1, + fuse_variables::VelocityLinear2DStamped const& linear_velocity1, + fuse_variables::VelocityAngular2DStamped const& yaw_velocity1, + fuse_variables::AccelerationLinear2DStamped const& linear_acceleration1, + fuse_variables::Position2DStamped const& position2, + fuse_variables::Orientation2DStamped const& yaw2, + fuse_variables::VelocityLinear2DStamped const& linear_velocity2, + fuse_variables::VelocityAngular2DStamped const& yaw_velocity2, + fuse_variables::AccelerationLinear2DStamped const& linear_acceleration2, + fuse_core::Matrix8d const& covariance); /** * @brief Destructor @@ -124,7 +124,7 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * * Order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - const fuse_core::Matrix8d& sqrtInformation() const + fuse_core::Matrix8d const& sqrtInformation() const { return sqrt_information_; } @@ -174,7 +174,7 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& dt_; diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 85a914eaf..10afe8ae8 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -54,7 +54,7 @@ Acceleration2D::Acceleration2D() } void Acceleration2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -96,7 +96,7 @@ void Acceleration2D::onStart() sub_ = rclcpp::create_subscription( interfaces_, params_.topic, params_.queue_size, - std::bind(&AccelerationThrottledCallback::callback, + std::bind(&AccelerationThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -107,7 +107,7 @@ void Acceleration2D::onStop() sub_.reset(); } -void Acceleration2D::process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg) +void Acceleration2D::process(geometry_msgs::msg::AccelWithCovarianceStamped const& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 3c757a91b..c0f706d2a 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -51,7 +51,7 @@ GraphIgnition::GraphIgnition() } void GraphIgnition::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -99,13 +99,13 @@ void GraphIgnition::stop() started_ = false; } -void GraphIgnition::subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg) +void GraphIgnition::subscriberCallback(fuse_msgs::msg::SerializedGraph const& msg) { try { process(msg); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } @@ -123,7 +123,7 @@ bool GraphIgnition::setGraphServiceCallback(rclcpp::Servicesend_response(*request_id, response); }); } - catch (const std::exception& e) + catch (std::exception const& e) { fuse_msgs::srv::SetGraph::Response response; response.success = false; @@ -134,7 +134,7 @@ bool GraphIgnition::setGraphServiceCallback(rclcpp::Service post_process) +void GraphIgnition::process(fuse_msgs::msg::SerializedGraph const& msg, std::function post_process) { // Verify we are in the correct state to process set graph requests if (!started_) @@ -146,7 +146,7 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg, std::fun // NOTE(methylDragon): We convert the Graph::UniquePtr to a shared pointer so it can be passed as // a copyable object to the deferred service call's std::function<> arg to // satisfy the requirement that std::function<> arguments are copyable. - const auto graph = std::shared_ptr(std::move(graph_deserializer_.deserialize(msg))); + auto const graph = std::shared_ptr(std::move(graph_deserializer_.deserialize(msg))); // Validate the requested graph before we do anything if (boost::empty(graph->getConstraints())) @@ -197,19 +197,19 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg, std::fun } } -void GraphIgnition::sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp) +void GraphIgnition::sendGraph(fuse_core::Graph const& graph, rclcpp::Time const& stamp) { // Create a transaction equivalent to the graph auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(stamp); // Add variables - for (const auto& variable : graph.getVariables()) + for (auto const& variable : graph.getVariables()) { transaction->addVariable(variable.clone()); // If the variable is a fuse_variables::Stamped variable, set the involved stamp - const auto stamped_variable = dynamic_cast(&variable); + auto const stamped_variable = dynamic_cast(&variable); if (stamped_variable) { transaction->addInvolvedStamp(stamped_variable->stamp()); @@ -224,7 +224,7 @@ void GraphIgnition::sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& } // Add constraints - for (const auto& constraint : graph.getConstraints()) + for (auto const& constraint : graph.getConstraints()) { transaction->addConstraint(constraint.clone()); } diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index f4aeb51f8..9c93efdcb 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -60,7 +60,7 @@ Imu2D::Imu2D() } void Imu2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -106,7 +106,7 @@ void Imu2D::onStart() sub_ = rclcpp::create_subscription( interfaces_, params_.topic, params_.queue_size, - std::bind(&ImuThrottledCallback::callback, &throttled_callback_, + std::bind(&ImuThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -117,7 +117,7 @@ void Imu2D::onStop() sub_.reset(); } -void Imu2D::process(const sensor_msgs::msg::Imu& msg) +void Imu2D::process(sensor_msgs::msg::Imu const& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -150,7 +150,7 @@ void Imu2D::process(const sensor_msgs::msg::Imu& msg) twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; - const bool validate = !params_.disable_checks; + bool const validate = !params_.disable_checks; if (params_.differential) { @@ -205,8 +205,8 @@ void Imu2D::process(const sensor_msgs::msg::Imu& msg) sendTransaction(transaction); } -void Imu2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, +void Imu2D::processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool const validate, fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 492bae9b8..6cc348cbc 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -60,7 +60,7 @@ Imu3D::Imu3D() } void Imu3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -106,7 +106,7 @@ void Imu3D::onStart() sub_ = rclcpp::create_subscription( interfaces_, params_.topic, params_.queue_size, - std::bind(&ImuThrottledCallback::callback, &throttled_callback_, + std::bind(&ImuThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -117,7 +117,7 @@ void Imu3D::onStop() sub_.reset(); } -void Imu3D::process(const sensor_msgs::msg::Imu& msg) +void Imu3D::process(sensor_msgs::msg::Imu const& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -150,7 +150,7 @@ void Imu3D::process(const sensor_msgs::msg::Imu& msg) twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; - const bool validate = !params_.disable_checks; + bool const validate = !params_.disable_checks; if (params_.differential) { @@ -205,8 +205,8 @@ void Imu3D::process(const sensor_msgs::msg::Imu& msg) sendTransaction(transaction); } -void Imu3D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, +void Imu3D::processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool const validate, fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); diff --git a/fuse_models/src/mujoco_model.cpp b/fuse_models/src/mujoco_model.cpp index 1a251e4fb..5c87646e8 100644 --- a/fuse_models/src/mujoco_model.cpp +++ b/fuse_models/src/mujoco_model.cpp @@ -10,7 +10,7 @@ MujocoMotionModel::MujocoMotionModel() { } -void MujocoMotionModel::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, +void MujocoMotionModel::generateMotionModel(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables) { @@ -39,7 +39,7 @@ bool MujocoMotionModel::applyCallback(fuse_core::Transaction& transaction) // Now actually generate the motion model segments timestamp_manager_.query(transaction, true); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "An error occurred while completing the motion model query. Error: " << e.what()); diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index 73582ef49..e218274d4 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -59,7 +59,7 @@ Odometry2D::Odometry2D() } void Odometry2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -105,7 +105,7 @@ void Odometry2D::onStart() sub_ = rclcpp::create_subscription( interfaces_, params_.topic, params_.queue_size, - std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, + std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -116,7 +116,7 @@ void Odometry2D::onStop() sub_.reset(); } -void Odometry2D::process(const nav_msgs::msg::Odometry& msg) +void Odometry2D::process(nav_msgs::msg::Odometry const& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -132,7 +132,7 @@ void Odometry2D::process(const nav_msgs::msg::Odometry& msg) twist.header.frame_id = msg.child_frame_id; twist.twist = msg.twist; - const bool validate = !params_.disable_checks; + bool const validate = !params_.disable_checks; if (params_.differential) { @@ -155,8 +155,8 @@ void Odometry2D::process(const nav_msgs::msg::Odometry& msg) sendTransaction(transaction); } -void Odometry2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, +void Odometry2D::processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool const validate, fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index c99d6708e..b54eddbda 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -70,7 +70,7 @@ Odometry2DPublisher::Odometry2DPublisher() } void Odometry2DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) + fuse_core::node_interfaces::NodeInterfaces interfaces, std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -113,7 +113,7 @@ void Odometry2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr fuse_core::Graph::ConstSharedPtr graph) { // Find the most recent common timestamp - const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); + auto const latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); if (0u == latest_stamp.nanoseconds()) { { @@ -203,7 +203,7 @@ void Odometry2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr latest_covariance_valid = true; } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM(logger_, "An error occurred computing the covariance information for " << latest_stamp.nanoseconds() << ". The covariance will be set to zero.\n" @@ -260,8 +260,8 @@ void Odometry2DPublisher::onStop() publish_timer_->cancel(); } -bool Odometry2DPublisher::getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, - const fuse_core::UUID& device_id, fuse_core::UUID& position_uuid, +bool Odometry2DPublisher::getState(fuse_core::Graph const& graph, rclcpp::Time const& stamp, + fuse_core::UUID const& device_id, fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, @@ -270,23 +270,23 @@ bool Odometry2DPublisher::getState(const fuse_core::Graph& graph, const rclcpp:: try { position_uuid = fuse_variables::Position2DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); orientation_uuid = fuse_variables::Orientation2DStamped(stamp, device_id).uuid(); auto orientation_variable = - dynamic_cast(graph.getVariable(orientation_uuid)); + dynamic_cast(graph.getVariable(orientation_uuid)); velocity_linear_uuid = fuse_variables::VelocityLinear2DStamped(stamp, device_id).uuid(); auto velocity_linear_variable = - dynamic_cast(graph.getVariable(velocity_linear_uuid)); + dynamic_cast(graph.getVariable(velocity_linear_uuid)); velocity_angular_uuid = fuse_variables::VelocityAngular2DStamped(stamp, device_id).uuid(); auto velocity_angular_variable = - dynamic_cast(graph.getVariable(velocity_angular_uuid)); + dynamic_cast(graph.getVariable(velocity_angular_uuid)); acceleration_linear_uuid = fuse_variables::AccelerationLinear2DStamped(stamp, device_id).uuid(); auto acceleration_linear_variable = - dynamic_cast(graph.getVariable(acceleration_linear_uuid)); + dynamic_cast(graph.getVariable(acceleration_linear_uuid)); odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); @@ -306,7 +306,7 @@ bool Odometry2DPublisher::getState(const fuse_core::Graph& graph, const rclcpp:: acceleration.accel.accel.angular.y = 0.0; acceleration.accel.accel.angular.z = 0.0; } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); @@ -356,7 +356,7 @@ void Odometry2DPublisher::publishTimerCallback() tf2_2d::Vector2 velocity_linear; tf2::fromMsg(odom_output.twist.twist.linear, velocity_linear); - const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); + double const dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); fuse_core::Matrix8d jacobian; @@ -499,7 +499,7 @@ void Odometry2DPublisher::publishTimerCallback() map_to_odom.child_frame_id = params_.odom_frame_id; trans = map_to_odom; } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, "Could not lookup the " << params_.base_link_frame_id << "->" diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index 53c115c53..b88b6b777 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -59,7 +59,7 @@ Odometry3D::Odometry3D() } void Odometry3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -105,7 +105,7 @@ void Odometry3D::onStart() sub_ = rclcpp::create_subscription( interfaces_, params_.topic, params_.queue_size, - std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, + std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -116,7 +116,7 @@ void Odometry3D::onStop() sub_.reset(); } -void Odometry3D::process(const nav_msgs::msg::Odometry& msg) +void Odometry3D::process(nav_msgs::msg::Odometry const& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -132,7 +132,7 @@ void Odometry3D::process(const nav_msgs::msg::Odometry& msg) twist.header.frame_id = msg.child_frame_id; twist.twist = msg.twist; - const bool validate = !params_.disable_checks; + bool const validate = !params_.disable_checks; if (params_.differential) { @@ -155,8 +155,8 @@ void Odometry3D::process(const nav_msgs::msg::Odometry& msg) sendTransaction(transaction); } -void Odometry3D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, +void Odometry3D::processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + geometry_msgs::msg::TwistWithCovarianceStamped const& twist, bool const validate, fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index e49359421..3b322b406 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -76,7 +76,7 @@ Odometry3DPublisher::Odometry3DPublisher() } void Odometry3DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) + fuse_core::node_interfaces::NodeInterfaces interfaces, std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -136,7 +136,7 @@ void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr fuse_core::Graph::ConstSharedPtr graph) { // Find the most recent common timestamp - const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); + auto const latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); if (0u == latest_stamp.nanoseconds()) { { @@ -314,7 +314,7 @@ void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr latest_covariance_valid = true; } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM(logger_, "An error occurred computing the covariance information for " << latest_stamp.nanoseconds() << ". The covariance will be set to zero.\n" @@ -371,8 +371,8 @@ void Odometry3DPublisher::onStop() publish_timer_->cancel(); } -bool Odometry3DPublisher::getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, - const fuse_core::UUID& device_id, fuse_core::UUID& position_uuid, +bool Odometry3DPublisher::getState(fuse_core::Graph const& graph, rclcpp::Time const& stamp, + fuse_core::UUID const& device_id, fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, @@ -381,23 +381,23 @@ bool Odometry3DPublisher::getState(const fuse_core::Graph& graph, const rclcpp:: try { position_uuid = fuse_variables::Position3DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); orientation_uuid = fuse_variables::Orientation3DStamped(stamp, device_id).uuid(); auto orientation_variable = - dynamic_cast(graph.getVariable(orientation_uuid)); + dynamic_cast(graph.getVariable(orientation_uuid)); velocity_linear_uuid = fuse_variables::VelocityLinear3DStamped(stamp, device_id).uuid(); auto velocity_linear_variable = - dynamic_cast(graph.getVariable(velocity_linear_uuid)); + dynamic_cast(graph.getVariable(velocity_linear_uuid)); velocity_angular_uuid = fuse_variables::VelocityAngular3DStamped(stamp, device_id).uuid(); auto velocity_angular_variable = - dynamic_cast(graph.getVariable(velocity_angular_uuid)); + dynamic_cast(graph.getVariable(velocity_angular_uuid)); acceleration_linear_uuid = fuse_variables::AccelerationLinear3DStamped(stamp, device_id).uuid(); auto acceleration_linear_variable = - dynamic_cast(graph.getVariable(acceleration_linear_uuid)); + dynamic_cast(graph.getVariable(acceleration_linear_uuid)); odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); @@ -420,7 +420,7 @@ bool Odometry3DPublisher::getState(const fuse_core::Graph& graph, const rclcpp:: acceleration.accel.accel.angular.y = 0.0; acceleration.accel.accel.angular.z = 0.0; } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); @@ -441,7 +441,7 @@ void Odometry3DPublisher::predict(tf2::Transform& pose, nav_msgs::msg::Odometry& geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output, bool latest_covariance_valid) const { - const double dt = to_predict_to.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); + double const dt = to_predict_to.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); // Convert pose in Eigen representation fuse_core::Vector3d position; fuse_core::Vector3d velocity_linear; @@ -561,7 +561,7 @@ void Odometry3DPublisher::publishTF(nav_msgs::msg::Odometry const& odom_output, map_to_odom.child_frame_id = params_.odom_frame_id; trans = map_to_odom; } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, "Could not lookup the " << params_.base_link_frame_id << "->" << params_.odom_frame_id diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp index bb3778cd2..a1d06f350 100644 --- a/fuse_models/src/omnidirectional_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -65,25 +65,25 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3D, fuse_core::MotionModel) namespace std { -inline bool isfinite(const fuse_core::Vector3d& vector) +inline bool isfinite(fuse_core::Vector3d const& vector) { return std::isfinite(vector.x()) && std::isfinite(vector.y()) && std::isfinite(vector.z()); } -inline bool isNormalized(const Eigen::Quaterniond& q) +inline bool isNormalized(Eigen::Quaterniond const& q) { return std::sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()) - 1.0 < Eigen::NumTraits::dummy_precision(); } -std::string to_string(const fuse_core::Vector3d& vector) +std::string to_string(fuse_core::Vector3d const& vector) { std::ostringstream oss; oss << vector; return oss.str(); } -std::string to_string(const Eigen::Quaterniond& quaternion) +std::string to_string(Eigen::Quaterniond const& quaternion) { std::ostringstream oss; oss << quaternion; @@ -95,8 +95,8 @@ namespace fuse_core { template -inline void validateCovariance(const Eigen::DenseBase& covariance, - const double precision = Eigen::NumTraits::dummy_precision()) +inline void validateCovariance(Eigen::DenseBase const& covariance, + double const precision = Eigen::NumTraits::dummy_precision()) { if (!fuse_core::isSymmetric(covariance, precision)) { @@ -129,7 +129,7 @@ Omnidirectional3D::Omnidirectional3D() void Omnidirectional3D::print(std::ostream& stream) const { stream << "state history:\n"; - for (const auto& state : state_history_) + for (auto const& state : state_history_) { stream << "- stamp: " << state.first.nanoseconds() << "\n"; state.second.print(stream); @@ -183,7 +183,7 @@ bool Omnidirectional3D::applyCallback(fuse_core::Transaction& transaction) // Now actually generate the motion model segments timestamp_manager_.query(transaction, true); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "An error occurred while completing the motion model query. Error: " << e.what()); @@ -198,7 +198,7 @@ void Omnidirectional3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) } void Omnidirectional3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncMotionModel::initialize(interfaces, name); @@ -251,7 +251,7 @@ void Omnidirectional3D::onStart() state_history_.clear(); } -void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, +void Omnidirectional3D::generateMotionModel(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables) { @@ -292,7 +292,7 @@ void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, } // If dt is zero, we only need to update the state history: - const double dt = (ending_stamp - beginning_stamp).seconds(); + double const dt = (ending_stamp - beginning_stamp).seconds(); if (dt == 0.0) { @@ -393,7 +393,7 @@ void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, { validateMotionModel(state1, state2, process_noise_covariance); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "Invalid '" << name_ << "' motion model: " << ex.what()); @@ -423,8 +423,8 @@ void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, variables.push_back(acceleration_linear2); } -void Omnidirectional3D::updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, - const rclcpp::Duration& buffer_length) +void Omnidirectional3D::updateStateHistoryEstimates(fuse_core::Graph const& graph, StateHistory& state_history, + rclcpp::Duration const& buffer_length) { if (state_history.empty()) { @@ -432,7 +432,7 @@ void Omnidirectional3D::updateStateHistoryEstimates(const fuse_core::Graph& grap } // Compute the expiration time carefully, as ROS can't handle negative times - const auto& ending_stamp = state_history.rbegin()->first; + auto const& ending_stamp = state_history.rbegin()->first; rclcpp::Time expiration_time; if (ending_stamp.seconds() > buffer_length.seconds()) @@ -462,18 +462,18 @@ void Omnidirectional3D::updateStateHistoryEstimates(const fuse_core::Graph& grap // state for (auto current_iter = state_history.begin(); current_iter != state_history.end(); ++current_iter) { - const auto& current_stamp = current_iter->first; + auto const& current_stamp = current_iter->first; auto& current_state = current_iter->second; if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.orientation_uuid) && graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_angular_uuid) && graph.variableExists(current_state.acc_linear_uuid)) { // This pose does exist in the graph. Update it directly. - const auto& position = graph.getVariable(current_state.position_uuid); - const auto& orientation = graph.getVariable(current_state.orientation_uuid); - const auto& vel_linear = graph.getVariable(current_state.vel_linear_uuid); - const auto& vel_angular = graph.getVariable(current_state.vel_angular_uuid); - const auto& acc_linear = graph.getVariable(current_state.acc_linear_uuid); + auto const& position = graph.getVariable(current_state.position_uuid); + auto const& orientation = graph.getVariable(current_state.orientation_uuid); + auto const& vel_linear = graph.getVariable(current_state.vel_linear_uuid); + auto const& vel_angular = graph.getVariable(current_state.vel_angular_uuid); + auto const& acc_linear = graph.getVariable(current_state.acc_linear_uuid); current_state.position.x() = position.data()[fuse_variables::Position3DStamped::X]; current_state.position.y() = position.data()[fuse_variables::Position3DStamped::Y]; @@ -499,8 +499,8 @@ void Omnidirectional3D::updateStateHistoryEstimates(const fuse_core::Graph& grap else if (current_iter != state_history.begin()) { auto previous_iter = std::prev(current_iter); - const auto& previous_stamp = previous_iter->first; - const auto& previous_state = previous_iter->second; + auto const& previous_stamp = previous_iter->first; + auto const& previous_state = previous_iter->second; // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its @@ -514,14 +514,14 @@ void Omnidirectional3D::updateStateHistoryEstimates(const fuse_core::Graph& grap } } -void Omnidirectional3D::validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, - const fuse_core::Matrix15d& process_noise_covariance) +void Omnidirectional3D::validateMotionModel(StateHistoryElement const& state1, StateHistoryElement const& state2, + fuse_core::Matrix15d const& process_noise_covariance) { try { state1.validate(); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); } @@ -530,7 +530,7 @@ void Omnidirectional3D::validateMotionModel(const StateHistoryElement& state1, c { state2.validate(); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); } @@ -538,13 +538,13 @@ void Omnidirectional3D::validateMotionModel(const StateHistoryElement& state1, c { fuse_core::validateCovariance(process_noise_covariance); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); } } -std::ostream& operator<<(std::ostream& stream, const Omnidirectional3D& omnidirectional_3d) +std::ostream& operator<<(std::ostream& stream, Omnidirectional3D const& omnidirectional_3d) { omnidirectional_3d.print(stream); return stream; diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp index 5e37654dd..cf9b98fa4 100644 --- a/fuse_models/src/omnidirectional_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -76,7 +76,7 @@ Omnidirectional3DIgnition::Omnidirectional3DIgnition() } void Omnidirectional3DIgnition::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name, + fuse_core::node_interfaces::NodeInterfaces interfaces, std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; @@ -158,13 +158,13 @@ void Omnidirectional3DIgnition::stop() started_ = false; } -void Omnidirectional3DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) +void Omnidirectional3DIgnition::subscriberCallback(geometry_msgs::msg::PoseWithCovarianceStamped const& msg) { try { process(msg); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } @@ -182,7 +182,7 @@ bool Omnidirectional3DIgnition::setPoseServiceCallback(rclcpp::Servicesend_response(*request_id, response); }); } - catch (const std::exception& e) + catch (std::exception const& e) { fuse_msgs::srv::SetPose::Response response; response.success = false; @@ -204,7 +204,7 @@ bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( service->send_response(*request_id, response); }); } - catch (const std::exception& e) + catch (std::exception const& e) { fuse_msgs::srv::SetPoseDeprecated::Response response; RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); @@ -213,7 +213,7 @@ bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Omnidirectional3DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, +void Omnidirectional3DIgnition::process(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, std::function post_process) { // Verify we are in the correct state to process set pose requests @@ -316,9 +316,9 @@ void Omnidirectional3DIgnition::process(const geometry_msgs::msg::PoseWithCovari } } -void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) +void Omnidirectional3DIgnition::sendPrior(geometry_msgs::msg::PoseWithCovarianceStamped const& pose) { - const auto& stamp = pose.header.stamp; + auto const& stamp = pose.header.stamp; // Create variables for the full state. // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped diff --git a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp index 7b222f762..6d053827e 100644 --- a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -49,15 +49,15 @@ namespace fuse_models { Omnidirectional3DStateKinematicConstraint::Omnidirectional3DStateKinematicConstraint( - const std::string& source, const fuse_variables::Position3DStamped& position1, - const fuse_variables::Orientation3DStamped& orientation1, - const fuse_variables::VelocityLinear3DStamped& velocity_linear1, - const fuse_variables::VelocityAngular3DStamped& velocity_angular1, - const fuse_variables::AccelerationLinear3DStamped& acceleration_linear1, - const fuse_variables::Position3DStamped& position2, const fuse_variables::Orientation3DStamped& orientation2, - const fuse_variables::VelocityLinear3DStamped& velocity_linear2, - const fuse_variables::VelocityAngular3DStamped& velocity_angular2, - const fuse_variables::AccelerationLinear3DStamped& acceleration_linear2, const fuse_core::Matrix15d& covariance) + std::string const& source, fuse_variables::Position3DStamped const& position1, + fuse_variables::Orientation3DStamped const& orientation1, + fuse_variables::VelocityLinear3DStamped const& velocity_linear1, + fuse_variables::VelocityAngular3DStamped const& velocity_angular1, + fuse_variables::AccelerationLinear3DStamped const& acceleration_linear1, + fuse_variables::Position3DStamped const& position2, fuse_variables::Orientation3DStamped const& orientation2, + fuse_variables::VelocityLinear3DStamped const& velocity_linear2, + fuse_variables::VelocityAngular3DStamped const& velocity_angular2, + fuse_variables::AccelerationLinear3DStamped const& acceleration_linear2, fuse_core::Matrix15d const& covariance) : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), velocity_linear1.uuid(), velocity_angular1.uuid(), acceleration_linear1.uuid(), position2.uuid(), orientation2.uuid(), velocity_linear2.uuid(), diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 82b2010ab..3f5df49e9 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -57,7 +57,7 @@ Pose2D::Pose2D() } void Pose2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -99,7 +99,7 @@ void Pose2D::onStart() sub_ = rclcpp::create_subscription( interfaces_, params_.topic, params_.queue_size, - std::bind(&PoseThrottledCallback::callback, + std::bind(&PoseThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -110,13 +110,13 @@ void Pose2D::onStop() sub_.reset(); } -void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) +void Pose2D::process(geometry_msgs::msg::PoseWithCovarianceStamped const& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(msg.header.stamp); - const bool validate = !params_.disable_checks; + bool const validate = !params_.disable_checks; if (params_.differential) { @@ -133,7 +133,7 @@ void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) sendTransaction(transaction); } -void Pose2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, +void Pose2D::processDifferential(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, bool const validate, fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp index dc3392315..910610d16 100644 --- a/fuse_models/src/transaction.cpp +++ b/fuse_models/src/transaction.cpp @@ -47,7 +47,7 @@ Transaction::Transaction() : fuse_core::AsyncSensorModel(1) } void Transaction::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -74,7 +74,7 @@ void Transaction::onStop() sub_.reset(); } -void Transaction::process(const fuse_msgs::msg::SerializedTransaction& msg) +void Transaction::process(fuse_msgs::msg::SerializedTransaction const& msg) { // Deserialize and send the transaction to the plugin's parent sendTransaction(transaction_deserializer_.deserialize(msg)->clone()); diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 5af842452..7e36ddad0 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -65,7 +65,7 @@ TransformSensor::TransformSensor() } void TransformSensor::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -120,7 +120,7 @@ void TransformSensor::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription(interfaces_, "/tf", params_.queue_size, - std::bind(&AprilTagThrottledCallback::callback, + std::bind(&AprilTagThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -212,7 +212,7 @@ void TransformSensor::process(MessageType const& msg) pose.pose.covariance[i * 7] = params_.pose_covariance[i]; } - const bool validate = !params_.disable_checks; + bool const validate = !params_.disable_checks; common::processAbsolutePose3DWithCovariance(name(), device_id_, pose, params_.pose_loss, "", params_.position_indices, params_.orientation_indices, *tf_buffer_, validate, *transaction, params_.tf_timeout); diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index 4574c5517..811fcc3f3 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -54,7 +54,7 @@ Twist2D::Twist2D() } void Twist2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -96,7 +96,7 @@ void Twist2D::onStart() sub_ = rclcpp::create_subscription( interfaces_, params_.topic, params_.queue_size, - std::bind(&TwistThrottledCallback::callback, + std::bind(&TwistThrottledCallback::callback, &throttled_callback_, std::placeholders::_1), sub_options); } @@ -107,7 +107,7 @@ void Twist2D::onStop() sub_.reset(); } -void Twist2D::process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg) +void Twist2D::process(geometry_msgs::msg::TwistWithCovarianceStamped const& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 9930b921a..63b7ab7c9 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -65,24 +65,24 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle2D, fuse_core::MotionModel) namespace std { -inline bool isfinite(const tf2_2d::Vector2& vector) +inline bool isfinite(tf2_2d::Vector2 const& vector) { return std::isfinite(vector.x()) && std::isfinite(vector.y()); } -inline bool isfinite(const tf2_2d::Transform& transform) +inline bool isfinite(tf2_2d::Transform const& transform) { return std::isfinite(transform.x()) && std::isfinite(transform.y()) && std::isfinite(transform.yaw()); } -std::string to_string(const tf2_2d::Vector2& vector) +std::string to_string(tf2_2d::Vector2 const& vector) { std::ostringstream oss; oss << vector; return oss.str(); } -std::string to_string(const tf2_2d::Transform& transform) +std::string to_string(tf2_2d::Transform const& transform) { std::ostringstream oss; oss << transform; @@ -95,8 +95,8 @@ namespace fuse_core { template -inline void validateCovariance(const Eigen::DenseBase& covariance, - const double precision = Eigen::NumTraits::dummy_precision()) +inline void validateCovariance(Eigen::DenseBase const& covariance, + double const precision = Eigen::NumTraits::dummy_precision()) { if (!fuse_core::isSymmetric(covariance, precision)) { @@ -128,7 +128,7 @@ Unicycle2D::Unicycle2D() void Unicycle2D::print(std::ostream& stream) const { stream << "state history:\n"; - for (const auto& state : state_history_) + for (auto const& state : state_history_) { stream << "- stamp: " << state.first.nanoseconds() << "\n"; state.second.print(stream); @@ -180,7 +180,7 @@ bool Unicycle2D::applyCallback(fuse_core::Transaction& transaction) // Now actually generate the motion model segments timestamp_manager_.query(transaction, true); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "An error occurred while completing the motion model query. Error: " << e.what()); @@ -195,7 +195,7 @@ void Unicycle2D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) } void Unicycle2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncMotionModel::initialize(interfaces, name); @@ -245,7 +245,7 @@ void Unicycle2D::onStart() state_history_.clear(); } -void Unicycle2D::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, +void Unicycle2D::generateMotionModel(rclcpp::Time const& beginning_stamp, rclcpp::Time const& ending_stamp, std::vector& constraints, std::vector& variables) { @@ -287,7 +287,7 @@ void Unicycle2D::generateMotionModel(const rclcpp::Time& beginning_stamp, const } // If dt is zero, we only need to update the state history: - const double dt = (ending_stamp - beginning_stamp).seconds(); + double const dt = (ending_stamp - beginning_stamp).seconds(); if (dt == 0.0) { @@ -367,7 +367,7 @@ void Unicycle2D::generateMotionModel(const rclcpp::Time& beginning_stamp, const { validateMotionModel(state1, state2, process_noise_covariance); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, "Invalid '" << name_ << "' motion model: " << ex.what()); @@ -394,8 +394,8 @@ void Unicycle2D::generateMotionModel(const rclcpp::Time& beginning_stamp, const variables.push_back(acceleration_linear2); } -void Unicycle2D::updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, - const rclcpp::Duration& buffer_length) +void Unicycle2D::updateStateHistoryEstimates(fuse_core::Graph const& graph, StateHistory& state_history, + rclcpp::Duration const& buffer_length) { if (state_history.empty()) { @@ -403,7 +403,7 @@ void Unicycle2D::updateStateHistoryEstimates(const fuse_core::Graph& graph, Stat } // Compute the expiration time carefully, as ROS can't handle negative times - const auto& ending_stamp = state_history.rbegin()->first; + auto const& ending_stamp = state_history.rbegin()->first; rclcpp::Time expiration_time; if (ending_stamp.seconds() > buffer_length.seconds()) @@ -433,18 +433,18 @@ void Unicycle2D::updateStateHistoryEstimates(const fuse_core::Graph& graph, Stat // state for (auto current_iter = state_history.begin(); current_iter != state_history.end(); ++current_iter) { - const auto& current_stamp = current_iter->first; + auto const& current_stamp = current_iter->first; auto& current_state = current_iter->second; if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.yaw_uuid) && graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_yaw_uuid) && graph.variableExists(current_state.acc_linear_uuid)) { // This pose does exist in the graph. Update it directly. - const auto& position = graph.getVariable(current_state.position_uuid); - const auto& yaw = graph.getVariable(current_state.yaw_uuid); - const auto& vel_linear = graph.getVariable(current_state.vel_linear_uuid); - const auto& vel_yaw = graph.getVariable(current_state.vel_yaw_uuid); - const auto& acc_linear = graph.getVariable(current_state.acc_linear_uuid); + auto const& position = graph.getVariable(current_state.position_uuid); + auto const& yaw = graph.getVariable(current_state.yaw_uuid); + auto const& vel_linear = graph.getVariable(current_state.vel_linear_uuid); + auto const& vel_yaw = graph.getVariable(current_state.vel_yaw_uuid); + auto const& acc_linear = graph.getVariable(current_state.acc_linear_uuid); current_state.pose.setX(position.data()[fuse_variables::Position2DStamped::X]); current_state.pose.setY(position.data()[fuse_variables::Position2DStamped::Y]); @@ -458,8 +458,8 @@ void Unicycle2D::updateStateHistoryEstimates(const fuse_core::Graph& graph, Stat else if (current_iter != state_history.begin()) { auto previous_iter = std::prev(current_iter); - const auto& previous_stamp = previous_iter->first; - const auto& previous_state = previous_iter->second; + auto const& previous_stamp = previous_iter->first; + auto const& previous_state = previous_iter->second; // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its @@ -472,14 +472,14 @@ void Unicycle2D::updateStateHistoryEstimates(const fuse_core::Graph& graph, Stat } } -void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, - const fuse_core::Matrix8d& process_noise_covariance) +void Unicycle2D::validateMotionModel(StateHistoryElement const& state1, StateHistoryElement const& state2, + fuse_core::Matrix8d const& process_noise_covariance) { try { state1.validate(); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); } @@ -488,7 +488,7 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St { state2.validate(); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); } @@ -497,13 +497,13 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St { fuse_core::validateCovariance(process_noise_covariance); } - catch (const std::runtime_error& ex) + catch (std::runtime_error const& ex) { throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); } } -std::ostream& operator<<(std::ostream& stream, const Unicycle2D& unicycle_2d) +std::ostream& operator<<(std::ostream& stream, Unicycle2D const& unicycle_2d) { unicycle_2d.print(stream); return stream; diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 4b40dade5..02464ca8f 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -75,7 +75,7 @@ Unicycle2DIgnition::Unicycle2DIgnition() } void Unicycle2DIgnition::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -149,21 +149,21 @@ void Unicycle2DIgnition::stop() started_ = false; } -void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) +void Unicycle2DIgnition::subscriberCallback(geometry_msgs::msg::PoseWithCovarianceStamped const& msg) { try { process(msg); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPose::Request::SharedPtr req) +bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr const& service, + std::shared_ptr const& request_id, + fuse_msgs::srv::SetPose::Request::SharedPtr const& req) { try { @@ -173,7 +173,7 @@ bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Servicesend_response(*request_id, response); }); } - catch (const std::exception& e) + catch (std::exception const& e) { fuse_msgs::srv::SetPose::Response response; response.success = false; @@ -185,8 +185,9 @@ bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) + rclcpp::Service::SharedPtr const& service, + std::shared_ptr const& request_id, + fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr const& req) { try { @@ -195,7 +196,7 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( service->send_response(*request_id, response); }); } - catch (const std::exception& e) + catch (std::exception const& e) { fuse_msgs::srv::SetPoseDeprecated::Response response; RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); @@ -204,8 +205,8 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - std::function post_process) +void Unicycle2DIgnition::process(geometry_msgs::msg::PoseWithCovarianceStamped const& pose, + std::function const& post_process) { // Verify we are in the correct state to process set pose requests if (!started_) @@ -219,10 +220,10 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta std::to_string(pose.pose.pose.position.x) + ", " + std::to_string(pose.pose.pose.position.y) + ")."); } - auto orientation_norm = std::sqrt(pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + - pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + - pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + - pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + auto orientation_norm = std::sqrt((pose.pose.pose.orientation.x * pose.pose.pose.orientation.x) + + (pose.pose.pose.orientation.y * pose.pose.pose.orientation.y) + + (pose.pose.pose.orientation.z * pose.pose.pose.orientation.z) + + (pose.pose.pose.orientation.w * pose.pose.pose.orientation.w)); if (std::abs(orientation_norm - 1.0) > 1.0e-3) { throw std::invalid_argument( @@ -267,8 +268,8 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta // It needs to be free to handle the response to this service call. // Have a callback do the rest of the work when a response comes. auto result_future = reset_client_->async_send_request( - srv, [this, post_process, pose](rclcpp::Client::SharedFuture result) { - (void)result; + // NOLINTNEXTLINE(performance-unnecessary-value-param) + srv, [this, post_process, pose](rclcpp::Client::SharedFuture /*result*/) { // Now that the pose has been validated and the optimizer has been reset, actually send the // initial state constraints to the optimizer sendPrior(pose); @@ -288,9 +289,9 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta } } -void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) +void Unicycle2DIgnition::sendPrior(geometry_msgs::msg::PoseWithCovarianceStamped const& pose) { - const auto& stamp = pose.header.stamp; + auto const& stamp = pose.header.stamp; // Create variables for the full state. // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 51281873b..ec786db57 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -50,14 +50,14 @@ namespace fuse_models { Unicycle2DStateKinematicConstraint::Unicycle2DStateKinematicConstraint( - const std::string& source, const fuse_variables::Position2DStamped& position1, - const fuse_variables::Orientation2DStamped& yaw1, const fuse_variables::VelocityLinear2DStamped& linear_velocity1, - const fuse_variables::VelocityAngular2DStamped& yaw_velocity1, - const fuse_variables::AccelerationLinear2DStamped& linear_acceleration1, - const fuse_variables::Position2DStamped& position2, const fuse_variables::Orientation2DStamped& yaw2, - const fuse_variables::VelocityLinear2DStamped& linear_velocity2, - const fuse_variables::VelocityAngular2DStamped& yaw_velocity2, - const fuse_variables::AccelerationLinear2DStamped& linear_acceleration2, const fuse_core::Matrix8d& covariance) + std::string const& source, fuse_variables::Position2DStamped const& position1, + fuse_variables::Orientation2DStamped const& yaw1, fuse_variables::VelocityLinear2DStamped const& linear_velocity1, + fuse_variables::VelocityAngular2DStamped const& yaw_velocity1, + fuse_variables::AccelerationLinear2DStamped const& linear_acceleration1, + fuse_variables::Position2DStamped const& position2, fuse_variables::Orientation2DStamped const& yaw2, + fuse_variables::VelocityLinear2DStamped const& linear_velocity2, + fuse_variables::VelocityAngular2DStamped const& yaw_velocity2, + fuse_variables::AccelerationLinear2DStamped const& linear_acceleration2, fuse_core::Matrix8d const& covariance) : fuse_core::Constraint(source, { position1.uuid(), yaw1.uuid(), linear_velocity1.uuid(), yaw_velocity1.uuid(), linear_acceleration1.uuid(), position2.uuid(), yaw2.uuid(), linear_velocity2.uuid(), yaw_velocity2.uuid(), linear_acceleration2.uuid() }) diff --git a/fuse_models/test/example_constraint.hpp b/fuse_models/test/example_constraint.hpp index a33cdb666..e16630584 100644 --- a/fuse_models/test/example_constraint.hpp +++ b/fuse_models/test/example_constraint.hpp @@ -59,13 +59,13 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - ExampleConstraint(const std::string& source, std::initializer_list variable_uuid_list) + ExampleConstraint(std::string const& source, std::initializer_list variable_uuid_list) : fuse_core::Constraint(source, variable_uuid_list), data(0.0) { } template - ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + ExampleConstraint(std::string const& source, VariableUuidIterator first, VariableUuidIterator last) : fuse_core::Constraint(source, first, last), data(0.0) { } @@ -77,7 +77,7 @@ class ExampleConstraint : public fuse_core::Constraint << " uuid: " << uuid() << '\n' << " variables: ["; - const auto& variable_uuids = variables(); + auto const& variable_uuids = variables(); if (!variable_uuids.empty()) { std::copy(variable_uuids.begin(), variable_uuids.end() - 1, std::ostream_iterator(stream, ", ")); @@ -107,7 +107,7 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data; diff --git a/fuse_models/test/example_variable.hpp b/fuse_models/test/example_variable.hpp index 182974429..1be768af5 100644 --- a/fuse_models/test/example_variable.hpp +++ b/fuse_models/test/example_variable.hpp @@ -61,7 +61,7 @@ class ExampleVariable : public fuse_core::Variable return 1; } - const double* data() const override + double const* data() const override { return &data_; } @@ -92,7 +92,7 @@ class ExampleVariable : public fuse_core::Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data_; diff --git a/fuse_models/test/example_variable_stamped.hpp b/fuse_models/test/example_variable_stamped.hpp index 7768ba7e9..568fc04ed 100644 --- a/fuse_models/test/example_variable_stamped.hpp +++ b/fuse_models/test/example_variable_stamped.hpp @@ -55,7 +55,7 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables ExampleVariableStamped() = default; - explicit ExampleVariableStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + explicit ExampleVariableStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL) : fuse_core::Variable(fuse_core::uuid::generate(detail::type(), stamp, device_id)) , Stamped(stamp, device_id) , data_(0.0) @@ -67,7 +67,7 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables return 1; } - const double* data() const override + double const* data() const override { return &data_; } @@ -100,7 +100,7 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_models/test/test_graph_ignition.cpp b/fuse_models/test/test_graph_ignition.cpp index d4776b249..f0390469d 100644 --- a/fuse_models/test/test_graph_ignition.cpp +++ b/fuse_models/test/test_graph_ignition.cpp @@ -77,7 +77,7 @@ static std::string failure_description; // NOLINT(runtime/string) * @brief Compare all the properties of two Variable objects * @return True if all the properties match, false otherwise */ -bool compareVariables(const fuse_core::Variable& expected, const fuse_core::Variable& actual) +bool compareVariables(fuse_core::Variable const& expected, fuse_core::Variable const& actual) { failure_description = ""; bool variables_equal = true; @@ -118,7 +118,7 @@ bool compareVariables(const fuse_core::Variable& expected, const fuse_core::Vari * @brief Compare all the properties of two Constraint objects * @return True if all the properties match, false otherwise */ -bool compareConstraints(const fuse_core::Constraint& expected, const fuse_core::Constraint& actual) +bool compareConstraints(fuse_core::Constraint const& expected, fuse_core::Constraint const& actual) { failure_description = ""; bool constraints_equal = true; @@ -160,22 +160,22 @@ bool compareConstraints(const fuse_core::Constraint& expected, const fuse_core:: namespace fuse_core { -bool operator==(const fuse_core::Variable& rhs, const fuse_core::Variable& lhs) +bool operator==(fuse_core::Variable const& rhs, fuse_core::Variable const& lhs) { return compareVariables(rhs, lhs); } -bool operator!=(const fuse_core::Variable& rhs, const fuse_core::Variable& lhs) +bool operator!=(fuse_core::Variable const& rhs, fuse_core::Variable const& lhs) { return !(rhs == lhs); } -bool operator==(const fuse_core::Constraint& rhs, const fuse_core::Constraint& lhs) +bool operator==(fuse_core::Constraint const& rhs, fuse_core::Constraint const& lhs) { return compareConstraints(rhs, lhs); } -bool operator!=(const fuse_core::Constraint& rhs, const fuse_core::Constraint& lhs) +bool operator!=(fuse_core::Constraint const& rhs, fuse_core::Constraint const& lhs) { return !(rhs == lhs); } @@ -270,7 +270,7 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) // Check the transaction is equivalent to the graph, i.e. it has the same constraints and // transactions - const auto transaction = callback_future.get(); + auto const transaction = callback_future.get(); ASSERT_EQ(boost::size(graph.getConstraints()), boost::size(transaction->addedConstraints())); ASSERT_EQ(boost::size(graph.getVariables()), boost::size(transaction->addedVariables())); @@ -291,29 +291,29 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) // transaction, we cannot compare them with the straightforward approach mentioned above. Instead, // we need to check that all added constraints and variables are in the graph, and check they are // the same. - for (const auto& added_constraint : transaction->addedConstraints()) + for (auto const& added_constraint : transaction->addedConstraints()) { try { - const auto& constraint = graph.getConstraint(added_constraint.uuid()); + auto const& constraint = graph.getConstraint(added_constraint.uuid()); EXPECT_EQ(constraint, added_constraint) << failure_description; } - catch (const std::out_of_range& ex) + catch (std::out_of_range const& ex) { ADD_FAILURE() << ex.what(); } } - for (const auto& added_variable : transaction->addedVariables()) + for (auto const& added_variable : transaction->addedVariables()) { try { - const auto& variable = graph.getVariable(added_variable.uuid()); + auto const& variable = graph.getVariable(added_variable.uuid()); EXPECT_EQ(variable, added_variable) << failure_description; } - catch (const std::out_of_range& ex) + catch (std::out_of_range const& ex) { ADD_FAILURE() << ex.what(); } @@ -386,7 +386,7 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) // Check the transaction is equivalent to the graph, i.e. it has the same constraints and // transactions - const auto transaction = callback_future.get(); + auto const transaction = callback_future.get(); ASSERT_EQ(boost::size(graph.getConstraints()), boost::size(transaction->addedConstraints())); ASSERT_EQ(boost::size(graph.getVariables()), boost::size(transaction->addedVariables())); @@ -407,29 +407,29 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) // transaction, we cannot compare them with the straightforward approach mentioned above. Instead, // we need to check that all added constraints and variables are in the graph, and check they are // the same. - for (const auto& added_constraint : transaction->addedConstraints()) + for (auto const& added_constraint : transaction->addedConstraints()) { try { - const auto& constraint = graph.getConstraint(added_constraint.uuid()); + auto const& constraint = graph.getConstraint(added_constraint.uuid()); EXPECT_EQ(constraint, added_constraint) << failure_description; } - catch (const std::out_of_range& ex) + catch (std::out_of_range const& ex) { ADD_FAILURE() << ex.what(); } } - for (const auto& added_variable : transaction->addedVariables()) + for (auto const& added_variable : transaction->addedVariables()) { try { - const auto& variable = graph.getVariable(added_variable.uuid()); + auto const& variable = graph.getVariable(added_variable.uuid()); EXPECT_EQ(variable, added_variable) << failure_description; } - catch (const std::out_of_range& ex) + catch (std::out_of_range const& ex) { ADD_FAILURE() << ex.what(); } diff --git a/fuse_models/test/test_omnidirectional_3d_ignition.cpp b/fuse_models/test/test_omnidirectional_3d_ignition.cpp index 2dc1e8b3b..bf48f90ca 100644 --- a/fuse_models/test/test_omnidirectional_3d_ignition.cpp +++ b/fuse_models/test/test_omnidirectional_3d_ignition.cpp @@ -75,11 +75,11 @@ void transactionCallback(fuse_core::Transaction::SharedPtr transaction) * @brief Helper function for fetching the desired constraint from a transaction */ template -const Derived* getConstraint(const fuse_core::Transaction& transaction) +Derived const* getConstraint(fuse_core::Transaction const& transaction) { - for (const auto& constraint : transaction.addedConstraints()) + for (auto const& constraint : transaction.addedConstraints()) { - auto derived = dynamic_cast(&constraint); + auto derived = dynamic_cast(&constraint); if (derived) { return derived; diff --git a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp index db0b93b63..619d206b8 100644 --- a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -47,40 +47,40 @@ TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + double const process_noise_diagonal[] = { 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3 }; const fuse_core::Matrix15d covariance = fuse_core::Vector15d(process_noise_diagonal).asDiagonal(); - const double dt{ 0.1 }; + double const dt{ 0.1 }; const fuse_core::Matrix15d sqrt_information{ covariance.inverse().llt().matrixU() }; const fuse_models::Omnidirectional3DStateCostFunction cost_function{ dt, sqrt_information }; // Evaluate cost function - const double position1[3] = { 0.0, 0.0, 0.0 }; - const double orientation1[4] = { 1.0, 0.0, 0.0, 0.0 }; - const double vel_linear1[3] = { 1.0, 1.0, 1.0 }; - const double vel_angular1[3] = { 1.570796327, 1.570796327, 1.570796327 }; - const double acc_linear1[3] = { 1.0, 1.0, 1.0 }; + double const position1[3] = { 0.0, 0.0, 0.0 }; + double const orientation1[4] = { 1.0, 0.0, 0.0, 0.0 }; + double const vel_linear1[3] = { 1.0, 1.0, 1.0 }; + double const vel_angular1[3] = { 1.570796327, 1.570796327, 1.570796327 }; + double const acc_linear1[3] = { 1.0, 1.0, 1.0 }; - const double position2[3] = { 0.105, 0.105, 0.105 }; + double const position2[3] = { 0.105, 0.105, 0.105 }; Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitX()); - const double orientation2[4] = { q2.w(), q2.x(), q2.y(), q2.z() }; - const double vel_linear2[3] = { 1.1, 1.1, 1.1 }; - const double vel_angular2[3] = { 1.570796327, 1.570796327, 1.570796327 }; - const double acc_linear2[3] = { 1.0, 1.0, 1.0 }; + double const orientation2[4] = { q2.w(), q2.x(), q2.y(), q2.z() }; + double const vel_linear2[3] = { 1.1, 1.1, 1.1 }; + double const vel_angular2[3] = { 1.570796327, 1.570796327, 1.570796327 }; + double const acc_linear2[3] = { 1.0, 1.0, 1.0 }; - const double* parameters[10] = { position1, orientation1, vel_linear1, vel_angular1, acc_linear1, + double const* parameters[10] = { position1, orientation1, vel_linear1, vel_angular1, acc_linear1, position2, orientation2, vel_linear2, vel_angular2, acc_linear2 }; fuse_core::Vector15d residuals; - const auto& block_sizes = cost_function.parameter_block_sizes(); - const auto num_parameter_blocks = block_sizes.size(); + auto const& block_sizes = cost_function.parameter_block_sizes(); + auto const num_parameter_blocks = block_sizes.size(); - const auto num_residuals = cost_function.num_residuals(); + auto const num_residuals = cost_function.num_residuals(); std::vector J(num_parameter_blocks); std::vector jacobians(num_parameter_blocks); @@ -101,7 +101,7 @@ TEST(CostFunction, evaluateCostFunction) ceres::NumericDiffOptions numeric_diff_options; #if CERES_VERSION_AT_LEAST(2, 1, 0) - std::vector parameterizations; + std::vector parameterizations; ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); #else ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); diff --git a/fuse_models/test/test_sensor_proc.cpp b/fuse_models/test/test_sensor_proc.cpp index c61455768..eb0fe6fd1 100644 --- a/fuse_models/test/test_sensor_proc.cpp +++ b/fuse_models/test/test_sensor_proc.cpp @@ -19,7 +19,7 @@ TEST(TestSuite, mergeXYPositionAndYawOrientationIndices) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, @@ -34,7 +34,7 @@ TEST(TestSuite, mergeXPositionAndYawOrientationIndices) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, @@ -49,7 +49,7 @@ TEST(TestSuite, mergeXYPositionAndEmptyOrientationIndices) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); @@ -62,7 +62,7 @@ TEST(TestSuite, mergeEmptyPositionAndYawOrientationIndices) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(orientation_indices.size(), merged_indices.size()); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); @@ -75,7 +75,7 @@ TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_TRUE(merged_indices.empty()); } @@ -93,7 +93,7 @@ TEST(TestSuite, populatePartialMeasurementXYPositionYawOrientation) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); @@ -118,7 +118,7 @@ TEST(TestSuite, populatePartialMeasurementXPositionYawOrientation) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); @@ -157,7 +157,7 @@ TEST(TestSuite, populatePartialMeasurementEmptyPositionYawOrientation) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); @@ -182,7 +182,7 @@ TEST(TestSuite, populatePartialMeasurementXYPositionEmptyOrientation) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); @@ -207,7 +207,7 @@ TEST(TestSuite, populatePartialMeasurementEmptyPositionEmptyOrientation) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); diff --git a/fuse_models/test/test_sensor_proc_3d.cpp b/fuse_models/test/test_sensor_proc_3d.cpp index 5c1caaa88..7e0f825fc 100644 --- a/fuse_models/test/test_sensor_proc_3d.cpp +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -18,7 +18,7 @@ TEST(TestSuite, mergeFullPositionAndFullOrientationIndices) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, @@ -33,7 +33,7 @@ TEST(TestSuite, mergeXYPositionAndRollYawOrientationIndices) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, @@ -48,7 +48,7 @@ TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); @@ -61,7 +61,7 @@ TEST(TestSuite, mergeEmptyPositionAndFullOrientationIndices) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(orientation_indices.size(), merged_indices.size()); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); @@ -74,7 +74,7 @@ TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_TRUE(merged_indices.empty()); } @@ -102,11 +102,11 @@ TEST(TestSuite, populatePartialMeasurements) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + auto const merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); std::replace_if( pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), - [&merged_indices, &pose_mean_partial](const double& value) { + [&merged_indices, &pose_mean_partial](double const& value) { return std::find(merged_indices.begin(), merged_indices.end(), &value - pose_mean_partial.data()) == merged_indices.end(); }, diff --git a/fuse_models/test/test_unicycle_2d_ignition.cpp b/fuse_models/test/test_unicycle_2d_ignition.cpp index 8122e8513..1abeb02df 100644 --- a/fuse_models/test/test_unicycle_2d_ignition.cpp +++ b/fuse_models/test/test_unicycle_2d_ignition.cpp @@ -72,11 +72,11 @@ void transactionCallback(fuse_core::Transaction::SharedPtr transaction) * @brief Helper function for fetching the desired constraint from a transaction */ template -const Derived* getConstraint(const fuse_core::Transaction& transaction) +Derived const* getConstraint(fuse_core::Transaction const& transaction) { - for (const auto& constraint : transaction.addedConstraints()) + for (auto const& constraint : transaction.addedConstraints()) { - auto derived = dynamic_cast(&constraint); + auto derived = dynamic_cast(&constraint); if (derived) { return derived; diff --git a/fuse_models/test/test_unicycle_2d_predict.cpp b/fuse_models/test/test_unicycle_2d_predict.cpp index d57547005..161f47e54 100644 --- a/fuse_models/test/test_unicycle_2d_predict.cpp +++ b/fuse_models/test/test_unicycle_2d_predict.cpp @@ -223,15 +223,15 @@ TEST(Predict, predictObjects) TEST(Predict, predictJacobians) { - const double position1_x = 0.0; - const double position1_y = 0.0; - const double yaw1 = 0.0; - const double vel_linear1_x = 1.0; - const double vel_linear1_y = 0.0; - const double vel_yaw1 = 1.570796327; - const double acc_linear1_x = 1.0; - const double acc_linear1_y = 0.0; - const double dt = 0.1; + double const position1_x = 0.0; + double const position1_y = 0.0; + double const yaw1 = 0.0; + double const vel_linear1_x = 1.0; + double const vel_linear1_y = 0.0; + double const vel_yaw1 = 1.570796327; + double const acc_linear1_x = 1.0; + double const acc_linear1_y = 0.0; + double const dt = 0.1; double position2_x = 0.0; double position2_y = 0.0; double yaw2 = 0.0; @@ -242,7 +242,7 @@ TEST(Predict, predictJacobians) double acc_linear2_y = 0.0; const std::array block_sizes = { 2, 1, 2, 1, 2 }; - const auto num_parameter_blocks = block_sizes.size(); + auto const num_parameter_blocks = block_sizes.size(); const size_t num_residuals{ 8 }; diff --git a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp index c548d44a8..e4c6e0bd7 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -47,36 +47,36 @@ TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; + double const process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d covariance = fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); - const double dt{ 0.1 }; + double const dt{ 0.1 }; const fuse_core::Matrix8d sqrt_information{ covariance.inverse().llt().matrixU() }; const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information }; // Evaluate cost function - const double position1[] = { 0.0, 0.0 }; - const double yaw1[] = { 0.0 }; - const double vel_linear1[] = { 1.0, 0.0 }; - const double vel_yaw1[] = { 1.570796327 }; - const double acc_linear1[] = { 1.0, 0.0 }; - - const double position2[] = { 0.105, 0.0 }; - const double yaw2[] = { 0.1570796327 }; - const double vel_linear2[] = { 1.1, 0.0 }; - const double vel_yaw2[] = { 1.570796327 }; - const double acc_linear2[] = { 1.0, 0.0 }; - - const double* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + double const position1[] = { 0.0, 0.0 }; + double const yaw1[] = { 0.0 }; + double const vel_linear1[] = { 1.0, 0.0 }; + double const vel_yaw1[] = { 1.570796327 }; + double const acc_linear1[] = { 1.0, 0.0 }; + + double const position2[] = { 0.105, 0.0 }; + double const yaw2[] = { 0.1570796327 }; + double const vel_linear2[] = { 1.1, 0.0 }; + double const vel_yaw2[] = { 1.570796327 }; + double const acc_linear2[] = { 1.0, 0.0 }; + + double const* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; fuse_core::Vector8d residuals; - const auto& block_sizes = cost_function.parameter_block_sizes(); - const auto num_parameter_blocks = block_sizes.size(); + auto const& block_sizes = cost_function.parameter_block_sizes(); + auto const num_parameter_blocks = block_sizes.size(); - const auto num_residuals = cost_function.num_residuals(); + auto const num_residuals = cost_function.num_residuals(); std::vector J(num_parameter_blocks); std::vector jacobians(num_parameter_blocks); diff --git a/fuse_optimizers/doc/optimizers.md b/fuse_optimizers/doc/optimizers.md new file mode 100644 index 000000000..2fa8ac22f --- /dev/null +++ b/fuse_optimizers/doc/optimizers.md @@ -0,0 +1,3 @@ +# Optimizers + +[Coming soon](https://github.com/PickNikRobotics/fuse/issues/23) diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp index 969df9624..47170c476 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp @@ -130,7 +130,7 @@ class BatchOptimizer : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - TransactionQueueElement(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) + TransactionQueueElement(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction) : sensor_name(sensor_name), transaction(std::move(transaction)) { } @@ -213,7 +213,7 @@ class BatchOptimizer : public Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** * @brief Update and publish diagnostics diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index 2d229b678..34ac1b26c 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -144,15 +144,15 @@ class FixedLagSmoother : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - const rclcpp::Time& stamp() const + rclcpp::Time const& stamp() const { return transaction->stamp(); } - const rclcpp::Time& minStamp() const + rclcpp::Time const& minStamp() const { return transaction->minStamp(); } - const rclcpp::Time& maxStamp() const + rclcpp::Time const& maxStamp() const { return transaction->maxStamp(); } @@ -238,7 +238,7 @@ class FixedLagSmoother : public Optimizer * @param[in] new_transaction All new, non-marginal-related transactions that *will be* applied to * the graph */ - void preprocessMarginalization(const fuse_core::Transaction& new_transaction); + void preprocessMarginalization(fuse_core::Transaction const& new_transaction); /** * @brief Compute the oldest timestamp that is part of the configured lag window @@ -255,7 +255,7 @@ class FixedLagSmoother : public Optimizer * @return A container with the set of variables to marginalize out. Order of the variables is not * specified. */ - std::vector computeVariablesToMarginalize(const rclcpp::Time& lag_expiration); + std::vector computeVariablesToMarginalize(rclcpp::Time const& lag_expiration); /** * @brief Perform any required post-marginalization bookkeeping @@ -266,7 +266,7 @@ class FixedLagSmoother : public Optimizer * @param[in] marginal_transaction The actual changes to the graph caused my marginalizing out the * requested variables. */ - void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction); + void postprocessMarginalization(fuse_core::Transaction const& marginal_transaction); /** * @brief Function that optimizes all constraints, designed to be run in a separate thread. @@ -299,7 +299,7 @@ class FixedLagSmoother : public Optimizer * sensor transactions * @param[in] lag_expiration The oldest timestamp that should remain in the graph */ - void processQueue(fuse_core::Transaction& transaction, const rclcpp::Time& lag_expiration); + void processQueue(fuse_core::Transaction& transaction, rclcpp::Time const& lag_expiration); /** * @brief Service callback that resets the optimizer to its original state @@ -319,7 +319,7 @@ class FixedLagSmoother : public Optimizer /** * @brief Thread-safe write access to the optimizer start time */ - void setStartTime(const rclcpp::Time& start_time) + void setStartTime(rclcpp::Time const& start_time) { std::lock_guard lock(start_time_mutex_); start_time_ = start_time; @@ -338,7 +338,7 @@ class FixedLagSmoother : public Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** * @brief Update and publish diagnostics diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp index 1616c12a0..3c5c076b7 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp @@ -135,7 +135,7 @@ class Optimizer * @param[in] model The sensor model * @param[in] ignition Whether this sensor model is an ignition one or not */ - SensorModelInfo(SensorModelUniquePtr model, const bool ignition) : model(std::move(model)), ignition(ignition) + SensorModelInfo(SensorModelUniquePtr model, bool const ignition) : model(std::move(model)), ignition(ignition) { } @@ -183,7 +183,7 @@ class Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - virtual void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0; + virtual void transactionCallback(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0; /** * @brief Configure the motion model plugins specified on the parameter server @@ -222,7 +222,7 @@ class Optimizer * @return Flag indicating if all motion model constraints were successfully * generated */ - bool applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const; + bool applyMotionModels(std::string const& sensor_name, fuse_core::Transaction& transaction) const; /** * @brief Send the sensors, motion models, and publishers updated graph information @@ -240,7 +240,7 @@ class Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction); + void injectCallback(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction); /** * @brief Clear all of the callbacks inserted into the callback queue by the injectCallback() diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp index 425ab73a2..46c6ac08c 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp @@ -112,7 +112,7 @@ class VariableStampIndex * * @param[in] transaction The set of variables and constraints to add and remove */ - void addNewTransaction(const fuse_core::Transaction& transaction); + void addNewTransaction(fuse_core::Transaction const& transaction); /** * @brief Update the index with the information from a marginal transaction @@ -122,7 +122,7 @@ class VariableStampIndex * * @param[in] transaction The set of variables and constraints to remove */ - void addMarginalTransaction(const fuse_core::Transaction& transaction); + void addMarginalTransaction(fuse_core::Transaction const& transaction); /** * @brief Add all variables that are not directly connected to a stamped variable with a timestamp @@ -133,11 +133,11 @@ class VariableStampIndex * @param[out] result An output iterator capable of receiving fuse_core::UUID objects */ template - void query(const rclcpp::Time& stamp, OutputUuidIterator result) const + void query(rclcpp::Time const& stamp, OutputUuidIterator result) const { // First get all of the stamped variables greater than or equal to the input stamp std::unordered_set recent_variable_uuids; - for (const auto& variable_stamp_pair : stamped_index_) + for (auto const& variable_stamp_pair : stamped_index_) { if (variable_stamp_pair.second >= stamp) { @@ -147,21 +147,21 @@ class VariableStampIndex // Now find all of the variables connected to the recent variables std::unordered_set connected_variable_uuids; - for (const auto& recent_variable_uuid : recent_variable_uuids) + for (auto const& recent_variable_uuid : recent_variable_uuids) { // Add the recent variable to ensure connected_variable_uuids is a superset of // recent_variable_uuids connected_variable_uuids.insert(recent_variable_uuid); - const auto variables_iter = variables_.find(recent_variable_uuid); + auto const variables_iter = variables_.find(recent_variable_uuid); if (variables_iter != variables_.end()) { - for (const auto& connected_constraint_uuid : variables_iter->second) + for (auto const& connected_constraint_uuid : variables_iter->second) { - const auto constraints_iter = constraints_.find(connected_constraint_uuid); + auto const constraints_iter = constraints_.find(connected_constraint_uuid); if (constraints_iter != constraints_.end()) { - for (const auto& connected_variable_uuid : constraints_iter->second) + for (auto const& connected_variable_uuid : constraints_iter->second) { connected_variable_uuids.insert(connected_variable_uuid); } @@ -171,7 +171,7 @@ class VariableStampIndex } // Return the set of variables that are not connected - for (const auto& variable : variables_) + for (auto const& variable : variables_) { if (connected_variable_uuids.find(variable.first) == connected_variable_uuids.end()) { @@ -195,23 +195,23 @@ class VariableStampIndex /** * @brief Update this VariableStampIndex with the added constraints from the provided transaction */ - void applyAddedConstraints(const fuse_core::Transaction& transaction); + void applyAddedConstraints(fuse_core::Transaction const& transaction); /** * @brief Update this VariableStampIndex with the added variables from the provided transaction */ - void applyAddedVariables(const fuse_core::Transaction& transaction); + void applyAddedVariables(fuse_core::Transaction const& transaction); /** * @brief Update this VariableStampIndex with the removed constraints from the provided * transaction */ - void applyRemovedConstraints(const fuse_core::Transaction& transaction); + void applyRemovedConstraints(fuse_core::Transaction const& transaction); /** * @brief Update this VariableStampIndex with the removed variables from the provided transaction */ - void applyRemovedVariables(const fuse_core::Transaction& transaction); + void applyRemovedVariables(fuse_core::Transaction const& transaction); }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index 78b530e99..9fd963728 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -184,7 +184,7 @@ void BatchOptimizer::optimizerTimerCallback() } } -void BatchOptimizer::transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) +void BatchOptimizer::transactionCallback(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // Add the new transaction to the pending set // Either we haven't "started" yet and we want to keep a short history of transactions around diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index e853fdc47..b730bd882 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -121,7 +121,7 @@ FixedLagSmoother::~FixedLagSmoother() void FixedLagSmoother::autostart() { if (std::none_of(sensor_models_.begin(), sensor_models_.end(), - [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) + [](auto const& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) { // No ignition sensors were provided. Auto-start. started_ = true; @@ -130,7 +130,7 @@ void FixedLagSmoother::autostart() } } -void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction& new_transaction) +void FixedLagSmoother::preprocessMarginalization(fuse_core::Transaction const& new_transaction) { timestamp_tracking_.addNewTransaction(new_transaction); } @@ -150,14 +150,14 @@ rclcpp::Time FixedLagSmoother::computeLagExpirationTime() const return (start_time + params_.lag_duration < now) ? now - params_.lag_duration : start_time; } -std::vector FixedLagSmoother::computeVariablesToMarginalize(const rclcpp::Time& lag_expiration) +std::vector FixedLagSmoother::computeVariablesToMarginalize(rclcpp::Time const& lag_expiration) { auto marginalize_variable_uuids = std::vector(); timestamp_tracking_.query(lag_expiration, std::back_inserter(marginalize_variable_uuids)); return marginalize_variable_uuids; } -void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) +void FixedLagSmoother::postprocessMarginalization(fuse_core::Transaction const& marginal_transaction) { timestamp_tracking_.addMarginalTransaction(marginal_transaction); } @@ -212,7 +212,7 @@ void FixedLagSmoother::optimizationLoop() { graph_->update(*new_transaction); } - catch (const std::exception& ex) + catch (std::exception const& ex) { std::ostringstream oss; oss << "Graph:\n"; @@ -230,7 +230,7 @@ void FixedLagSmoother::optimizationLoop() summary_ = graph_->optimize(params_.solver_options); // Optimization is complete. Notify all the things about the graph changes. - const auto new_transaction_stamp = new_transaction->stamp(); + auto const new_transaction_stamp = new_transaction->stamp(); notify(std::move(new_transaction), graph_->clone()); // Abort if optimization failed. Not converging is not a failure because the solution found is @@ -290,7 +290,7 @@ void FixedLagSmoother::optimizerTimerCallback() } } -void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const rclcpp::Time& lag_expiration) +void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, rclcpp::Time const& lag_expiration) { // We need to get the pending transactions from the queue std::lock_guard const pending_transactions_lock(pending_transactions_mutex_); @@ -317,7 +317,7 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r // always the oldest one that started things up. ignited_ = false; - const auto transaction_rbegin = pending_transactions_.rbegin(); + auto const transaction_rbegin = pending_transactions_.rbegin(); auto& element = *transaction_rbegin; if (!sensor_models_.at(element.sensor_name).ignition) { @@ -350,9 +350,9 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r // optimization cycle. erase(pending_transactions_, transaction_rbegin); - const auto pending_ignition_transaction_iter = + auto const pending_ignition_transaction_iter = std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), - [this](const auto& element) { // NOLINT(whitespace/braces) + [this](auto const& element) { // NOLINT(whitespace/braces) return sensor_models_.at(element.sensor_name).ignition; }); // NOLINT(whitespace/braces) if (pending_ignition_transaction_iter == pending_transactions_.rend()) @@ -377,7 +377,7 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r } } // Use the most recent transaction time as the current time - const auto current_time = pending_transactions_.front().stamp(); + auto const current_time = pending_transactions_.front().stamp(); // Attempt to process each pending transaction auto sensor_blacklist = std::vector(); @@ -385,7 +385,7 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r while (transaction_riter != pending_transactions_.rend()) { auto& element = *transaction_riter; - const auto& min_stamp = element.minStamp(); + auto const& min_stamp = element.minStamp(); if (min_stamp < lag_expiration) { RCLCPP_DEBUG_STREAM(logger_, "The current lag expiration time is " @@ -413,7 +413,7 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r { // The motion model processing failed. // Check the transaction timeout to determine if it should be removed or skipped. - const auto& max_stamp = element.maxStamp(); + auto const& max_stamp = element.maxStamp(); if (max_stamp + params_.transaction_timeout < current_time) { // Warn that this transaction has expired, then skip it. @@ -475,11 +475,11 @@ bool FixedLagSmoother::resetServiceCallback(std::shared_ptrmaxStamp(); + auto const max_time = transaction->maxStamp(); if (started_ && max_time < start_time) { RCLCPP_DEBUG_STREAM(logger_, "Received a transaction before the start time from sensor '" @@ -494,7 +494,7 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ // Add the new transaction to the pending set // The pending set is arranged "smallest stamp last" to making popping off the back more // efficient - auto comparator = [](const rclcpp::Time& value, const TransactionQueueElement& element) { + auto comparator = [](rclcpp::Time const& value, TransactionQueueElement const& element) { return value >= element.stamp(); }; auto position = @@ -522,7 +522,7 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ pending_transactions_.erase( std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), [&sensor_name, max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) + &min_time = start_time](auto const& transaction) { // NOLINT(whitespace/braces) return transaction.sensor_name != sensor_name && (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); }), // NOLINT(whitespace/braces) @@ -556,7 +556,7 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ * @param[in] level The diagnostic status level * @param[in] message The diagnostic status message */ -diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string& message) +diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus(const int8_t level, std::string const& message) { diagnostic_msgs::msg::DiagnosticStatus status; @@ -598,7 +598,7 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe Optimizer::setDiagnostics(status); // Load std::atomic flag that indicates whether the optimizer has started or not - const bool started = started_; + bool const started = started_; status.add("Started", started); { @@ -648,8 +648,8 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe if (0u != optimization_deadline.nanoseconds()) { - const auto optimization_request_time = optimization_deadline - params_.optimization_period; - const auto time_since_last_optimization_request = clock_->now() - optimization_request_time; + auto const optimization_request_time = optimization_deadline - params_.optimization_period; + auto const time_since_last_optimization_request = clock_->now() - optimization_request_time; status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.seconds()); } } diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 404411ada..72dd322e7 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -117,7 +117,7 @@ void Optimizer::loadMotionModels() fuse_core::list_parameter_override_prefixes(interfaces_, "motion_models."); // declare config parameters for each model - for (const auto& param_name : motion_model_names) + for (auto const& param_name : motion_model_names) { ModelConfig& config = motion_model_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); @@ -150,7 +150,7 @@ void Optimizer::loadMotionModels() // now load the models defined above - for (const ModelConfig& config : motion_model_config) + for (ModelConfig const& config : motion_model_config) { // Create a motion_model object using pluginlib. This will throw if the plugin name is not // found. @@ -185,7 +185,7 @@ void Optimizer::loadSensorModels() fuse_core::list_parameter_override_prefixes(interfaces_, "sensor_models."); // declare config parameters for each model - for (const auto& param_name : sensor_model_names) + for (auto const& param_name : sensor_model_names) { ModelConfig& config = sensor_model_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); @@ -258,7 +258,7 @@ void Optimizer::loadSensorModels() } // now load the models defined above - for (const ModelConfig& config : sensor_model_config) + for (ModelConfig const& config : sensor_model_config) { // Create a sensor object using pluginlib. This will throw if the plugin name is not found. auto sensor_model = sensor_model_loader_.createUniqueInstance(config.type); @@ -272,7 +272,7 @@ void Optimizer::loadSensorModels() // Parse out the list of associated motion models, if any associated_motion_models_[config.name] = config.associated_motion_models; - for (const auto& motion_model_name : config.associated_motion_models) + for (auto const& motion_model_name : config.associated_motion_models) { if (motion_models_.find(motion_model_name) == motion_models_.end()) { @@ -303,7 +303,7 @@ void Optimizer::loadPublishers() fuse_core::list_parameter_override_prefixes(interfaces_, "publishers."); // declare config parameters for each model - for (const auto& param_name : publisher_names) + for (auto const& param_name : publisher_names) { PublisherConfig& config = publisher_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); @@ -337,7 +337,7 @@ void Optimizer::loadPublishers() // now load the models defined above - for (const PublisherConfig& config : publisher_config) + for (PublisherConfig const& config : publisher_config) { // Create a publisher object using pluginlib. This will throw if the plugin name is not found. auto publisher = publisher_loader_.createUniqueInstance(config.type); @@ -350,7 +350,7 @@ void Optimizer::loadPublishers() diagnostic_updater_.force_update(); } -bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const +bool Optimizer::applyMotionModels(std::string const& sensor_name, fuse_core::Transaction& transaction) const { // Check for trivial cases where we don't have to do anything auto iter = associated_motion_models_.find(sensor_name); @@ -359,15 +359,15 @@ bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Tra return true; } // Generate constraints for each configured motion model - const auto& motion_model_names = iter->second; + auto const& motion_model_names = iter->second; bool success = true; - for (const auto& motion_model_name : motion_model_names) + for (auto const& motion_model_name : motion_model_names) { try { success &= motion_models_.at(motion_model_name)->apply(transaction); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM(logger_, "Error generating constraints for sensor '" << sensor_name << "' from motion model '" << motion_model_name @@ -381,39 +381,39 @@ bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Tra void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr const& transaction, fuse_core::Graph::ConstSharedPtr const& graph) { - for (const auto& name_sensor_model : sensor_models_) + for (auto const& name_sensor_model : sensor_models_) { try { name_sensor_model.second.model->graphCallback(graph); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on sensor '" << name_sensor_model.first << "'. Error: " << e.what()); continue; } } - for (const auto& name_motion_model : motion_models_) + for (auto const& name_motion_model : motion_models_) { try { name_motion_model.second->graphCallback(graph); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on motion model '" << name_motion_model.first << ". Error: " << e.what()); continue; } } - for (const auto& name_publisher : publishers_) + for (auto const& name_publisher : publishers_) { try { name_publisher.second->notify(transaction, graph); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_ERROR_STREAM(logger_, "Failed calling notify() on publisher '" << name_publisher.first << ". Error: " << e.what()); @@ -422,7 +422,7 @@ void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr const& transaction } } -void Optimizer::injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) +void Optimizer::injectCallback(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // We are going to insert a call to the derived class's transactionCallback() method into the // global callback queue. This returns execution to the sensor's thread quickly by moving the @@ -440,15 +440,15 @@ void Optimizer::clearCallbacks() void Optimizer::startPlugins() { - for (const auto& name_plugin : motion_models_) + for (auto const& name_plugin : motion_models_) { name_plugin.second->start(); } - for (const auto& name_plugin : sensor_models_) + for (auto const& name_plugin : sensor_models_) { name_plugin.second.model->start(); } - for (const auto& name_plugin : publishers_) + for (auto const& name_plugin : publishers_) { name_plugin.second->start(); } @@ -458,15 +458,15 @@ void Optimizer::startPlugins() void Optimizer::stopPlugins() { - for (const auto& name_plugin : publishers_) + for (auto const& name_plugin : publishers_) { name_plugin.second->stop(); } - for (const auto& name_plugin : sensor_models_) + for (auto const& name_plugin : sensor_models_) { name_plugin.second.model->stop(); } - for (const auto& name_plugin : motion_models_) + for (auto const& name_plugin : motion_models_) { name_plugin.second->stop(); } @@ -486,7 +486,7 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimizer exists"); - auto print_key = [](const std::string& result, const auto& entry) { return result + entry.first + ' '; }; + auto print_key = [](std::string const& result, auto const& entry) { return result + entry.first + ' '; }; status.add("Sensor Models", std::accumulate(sensor_models_.begin(), sensor_models_.end(), std::string(), print_key)); status.add("Motion Models", std::accumulate(motion_models_.begin(), motion_models_.end(), std::string(), print_key)); diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index 032ae1de4..6c1ec481e 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -45,7 +45,7 @@ namespace fuse_optimizers { rclcpp::Time VariableStampIndex::currentStamp() const { - auto compare_stamps = [](const StampedMap::value_type& lhs, const StampedMap::value_type& rhs) { + auto compare_stamps = [](StampedMap::value_type const& lhs, StampedMap::value_type const& rhs) { return lhs.second < rhs.second; }; auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps); @@ -56,7 +56,7 @@ rclcpp::Time VariableStampIndex::currentStamp() const return { 0, 0, RCL_ROS_TIME }; } -void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transaction) +void VariableStampIndex::addNewTransaction(fuse_core::Transaction const& transaction) { applyAddedVariables(transaction); applyAddedConstraints(transaction); @@ -64,7 +64,7 @@ void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transac applyRemovedVariables(transaction); } -void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction& transaction) +void VariableStampIndex::addMarginalTransaction(fuse_core::Transaction const& transaction) { // Only the removed variables and removed constraints should be applied to the VariableStampIndex // No variables will be added by a marginal transaction, and the added constraints add variable @@ -74,23 +74,23 @@ void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction& tr applyRemovedVariables(transaction); } -void VariableStampIndex::applyAddedConstraints(const fuse_core::Transaction& transaction) +void VariableStampIndex::applyAddedConstraints(fuse_core::Transaction const& transaction) { - for (const auto& constraint : transaction.addedConstraints()) + for (auto const& constraint : transaction.addedConstraints()) { constraints_[constraint.uuid()].insert(constraint.variables().begin(), constraint.variables().end()); - for (const auto& variable_uuid : constraint.variables()) + for (auto const& variable_uuid : constraint.variables()) { variables_[variable_uuid].insert(constraint.uuid()); } } } -void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& transaction) +void VariableStampIndex::applyAddedVariables(fuse_core::Transaction const& transaction) { - for (const auto& variable : transaction.addedVariables()) + for (auto const& variable : transaction.addedVariables()) { - const auto* stamped_variable = dynamic_cast(&variable); + auto const* stamped_variable = dynamic_cast(&variable); if (stamped_variable != nullptr) { stamped_index_[variable.uuid()] = stamped_variable->stamp(); @@ -99,11 +99,11 @@ void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& trans } } -void VariableStampIndex::applyRemovedConstraints(const fuse_core::Transaction& transaction) +void VariableStampIndex::applyRemovedConstraints(fuse_core::Transaction const& transaction) { - for (const auto& constraint_uuid : transaction.removedConstraints()) + for (auto const& constraint_uuid : transaction.removedConstraints()) { - for (const auto& variable_uuid : constraints_[constraint_uuid]) + for (auto const& variable_uuid : constraints_[constraint_uuid]) { variables_[variable_uuid].erase(constraint_uuid); } @@ -111,9 +111,9 @@ void VariableStampIndex::applyRemovedConstraints(const fuse_core::Transaction& t } } -void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction& transaction) +void VariableStampIndex::applyRemovedVariables(fuse_core::Transaction const& transaction) { - for (const auto& variable_uuid : transaction.removedVariables()) + for (auto const& variable_uuid : transaction.removedVariables()) { stamped_index_.erase(variable_uuid); variables_.erase(variable_uuid); diff --git a/fuse_optimizers/test/launch_tests/common.hpp b/fuse_optimizers/test/launch_tests/common.hpp index 3beac67fc..db78ab7b0 100644 --- a/fuse_optimizers/test/launch_tests/common.hpp +++ b/fuse_optimizers/test/launch_tests/common.hpp @@ -50,7 +50,7 @@ * @return The output stream with the vector printed into it */ template -std::ostream& operator<<(std::ostream& os, const std::vector& v) +std::ostream& operator<<(std::ostream& os, std::vector const& v) { os << '['; @@ -73,11 +73,11 @@ std::ostream& operator<<(std::ostream& os, const std::vector& v) * @return The output stream with the vector printed into it */ template -std::ostream& operator<<(std::ostream& os, const std::unordered_map& m) +std::ostream& operator<<(std::ostream& os, std::unordered_map const& m) { os << '['; - for (const auto& entry : m) + for (auto const& entry : m) { os << entry.first << ", "; } @@ -96,13 +96,13 @@ std::ostream& operator<<(std::ostream& os, const std::unordered_map& m) * @return A vector with the symmetric difference strings */ template -std::vector set_symmetric_difference(const std::vector& lhs, - const std::unordered_map& rhs) +std::vector set_symmetric_difference(std::vector const& lhs, + std::unordered_map const& rhs) { // Retrieve the keys: std::vector rhs_keys; std::transform(rhs.begin(), rhs.end(), std::back_inserter(rhs_keys), - [](const auto& pair) { return pair.first; }); // NOLINT(whitespace/braces) + [](auto const& pair) { return pair.first; }); // NOLINT(whitespace/braces) // Sort the keys so we can use std::set_symmetric_difference: std::sort(rhs_keys.begin(), rhs_keys.end()); diff --git a/fuse_optimizers/test/launch_tests/example_optimizer.hpp b/fuse_optimizers/test/launch_tests/example_optimizer.hpp index 1aed0d680..12c957a05 100644 --- a/fuse_optimizers/test/launch_tests/example_optimizer.hpp +++ b/fuse_optimizers/test/launch_tests/example_optimizer.hpp @@ -55,22 +55,22 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer { } - const MotionModels& getMotionModels() const + MotionModels const& getMotionModels() const { return motion_models_; } - const SensorModels& getSensorModels() const + SensorModels const& getSensorModels() const { return sensor_models_; } - const Publishers& getPublishers() const + Publishers const& getPublishers() const { return publishers_; } - void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override + void transactionCallback(std::string const& sensor_name, fuse_core::Transaction::SharedPtr transaction) override { (void)sensor_name; (void)transaction; diff --git a/fuse_optimizers/test/launch_tests/test_optimizer.cpp b/fuse_optimizers/test/launch_tests/test_optimizer.cpp index 3356c9b0c..d5a71beb8 100644 --- a/fuse_optimizers/test/launch_tests/test_optimizer.cpp +++ b/fuse_optimizers/test/launch_tests/test_optimizer.cpp @@ -49,9 +49,9 @@ TEST(Optimizer, Constructor) ExampleOptimizer optimizer(*node); // Check the motion and sensor models, and publishers were loaded: - const auto& motion_models = optimizer.getMotionModels(); - const auto& sensor_models = optimizer.getSensorModels(); - const auto& publishers = optimizer.getPublishers(); + auto const& motion_models = optimizer.getMotionModels(); + auto const& sensor_models = optimizer.getSensorModels(); + auto const& publishers = optimizer.getPublishers(); EXPECT_FALSE(motion_models.empty()); EXPECT_FALSE(sensor_models.empty()); @@ -72,9 +72,9 @@ TEST(Optimizer, Constructor) // Compute the symmetric difference between the expected and actual motion and sensor models, and // publishers: - const auto difference_motion_models = set_symmetric_difference(expected_motion_models, motion_models); - const auto difference_sensor_models = set_symmetric_difference(expected_sensor_models, sensor_models); - const auto difference_publishers = set_symmetric_difference(expected_publishers, publishers); + auto const difference_motion_models = set_symmetric_difference(expected_motion_models, motion_models); + auto const difference_sensor_models = set_symmetric_difference(expected_sensor_models, sensor_models); + auto const difference_publishers = set_symmetric_difference(expected_publishers, publishers); // Check the symmetric difference is empty, i.e. the actual motion and sensor models, and // publishers are the same as the expected ones: diff --git a/fuse_optimizers/test/test_variable_stamp_index.cpp b/fuse_optimizers/test/test_variable_stamp_index.cpp index 02c2a4506..0f0e522c4 100644 --- a/fuse_optimizers/test/test_variable_stamp_index.cpp +++ b/fuse_optimizers/test/test_variable_stamp_index.cpp @@ -59,7 +59,7 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp public: FUSE_VARIABLE_DEFINITIONS(StampedVariable) - explicit StampedVariable(const rclcpp::Time& stamp = rclcpp::Time(0, 0, RCL_ROS_TIME)) + explicit StampedVariable(rclcpp::Time const& stamp = rclcpp::Time(0, 0, RCL_ROS_TIME)) : fuse_core::Variable(fuse_core::uuid::generate()), fuse_variables::Stamped(stamp), data_{} { } @@ -69,7 +69,7 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp return 1; } - const double* data() const override + double const* data() const override { return &data_; } @@ -97,7 +97,7 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& boost::serialization::base_object(*this); @@ -124,7 +124,7 @@ class UnstampedVariable : public fuse_core::Variable return 1; } - const double* data() const override + double const* data() const override { return &data_; } @@ -152,7 +152,7 @@ class UnstampedVariable : public fuse_core::Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data_; @@ -171,23 +171,23 @@ class GenericConstraint : public fuse_core::Constraint GenericConstraint() = default; - GenericConstraint(const std::string& source, std::initializer_list variable_uuids) + GenericConstraint(std::string const& source, std::initializer_list variable_uuids) : Constraint(source, variable_uuids) { } - explicit GenericConstraint(const std::string& source, const fuse_core::UUID& variable1) + explicit GenericConstraint(std::string const& source, fuse_core::UUID const& variable1) : fuse_core::Constraint(source, { variable1 }) { } - GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + GenericConstraint(std::string const& source, fuse_core::UUID const& variable1, fuse_core::UUID const& variable2) : fuse_core::Constraint(source, { variable1, variable2 }) { } - GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2, - const fuse_core::UUID& variable3) + GenericConstraint(std::string const& source, fuse_core::UUID const& variable1, fuse_core::UUID const& variable2, + fuse_core::UUID const& variable3) : fuse_core::Constraint(source, { variable1, variable2, variable3 }) { } @@ -213,7 +213,7 @@ class GenericConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } diff --git a/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp b/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp index 516391022..730e96173 100644 --- a/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp @@ -79,7 +79,7 @@ class Path2DPublisher : public fuse_core::AsyncPublisher * @brief Shadowing extension to the AsyncPublisher::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or diff --git a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp index a00b66688..14b82a0cd 100644 --- a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp @@ -134,7 +134,7 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher * @brief Shadowing extension to the AsyncPublisher::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or diff --git a/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp b/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp index fc62ba01a..809d062bb 100644 --- a/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp @@ -71,7 +71,7 @@ class SerializedPublisher : public fuse_core::AsyncPublisher * @brief Shadowing extension to the AsyncPublisher::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or @@ -104,14 +104,14 @@ class SerializedPublisher : public fuse_core::AsyncPublisher * whenever needed * @param[in] stamp A rclcpp::Time stamp used for the serialized graph message published */ - void graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const; + void graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, rclcpp::Time const& stamp) const; std::string frame_id_; //!< The name of the frame for the serialized graph and transaction //!< messages published rclcpp::Publisher::SharedPtr graph_publisher_; rclcpp::Publisher::SharedPtr transaction_publisher_; - using GraphPublisherCallback = std::function; + using GraphPublisherCallback = std::function; using GraphPublisherThrottledCallback = fuse_core::ThrottledCallback; GraphPublisherThrottledCallback graph_publisher_throttled_callback_; //!< The graph publisher //!< throttled callback diff --git a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp index 0de70671a..7c8ebadf5 100644 --- a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp +++ b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp @@ -75,7 +75,7 @@ class StampedVariableSynchronizer * * @param[in] device_id The device id to use for all variable types */ - explicit StampedVariableSynchronizer(const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit StampedVariableSynchronizer(fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Find the latest timestamp for which variables of all the specified template types exist @@ -86,7 +86,7 @@ class StampedVariableSynchronizer * @param[in] graph The complete graph * @return The latest timestamp shared by all requested variable types */ - rclcpp::Time findLatestCommonStamp(const fuse_core::Transaction& transaction, const fuse_core::Graph& graph); + rclcpp::Time findLatestCommonStamp(fuse_core::Transaction const& transaction, fuse_core::Graph const& graph); private: fuse_core::UUID device_id_; //!< The device_id to use with the Stamped classes @@ -101,7 +101,7 @@ class StampedVariableSynchronizer * for a given time */ template - void updateTime(const VariableRange& variable_range, const fuse_core::Graph& graph); + void updateTime(VariableRange const& variable_range, fuse_core::Graph const& graph); }; namespace detail @@ -201,8 +201,8 @@ constexpr bool allStampedVariables = all_stamped_variables::value; template struct all_variables_exist { - static bool value(const fuse_core::Graph& /*graph*/, const rclcpp::Time& /*stamp*/, - const fuse_core::UUID& /*device_id*/) + static bool value(fuse_core::Graph const& /*graph*/, rclcpp::Time const& /*stamp*/, + fuse_core::UUID const& /*device_id*/) { return true; } @@ -222,7 +222,7 @@ struct all_variables_exist template struct all_variables_exist { - static bool value(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + static bool value(fuse_core::Graph const& graph, rclcpp::Time const& stamp, fuse_core::UUID const& device_id) { return graph.variableExists(T(stamp, device_id).uuid()) && all_variables_exist::value(graph, stamp, device_id); @@ -243,7 +243,7 @@ struct all_variables_exist template struct is_variable_in_pack { - static bool value(const fuse_core::Variable& /*variable*/) + static bool value(fuse_core::Variable const& /*variable*/) { return false; } @@ -261,7 +261,7 @@ struct is_variable_in_pack template struct is_variable_in_pack { - static bool value(const fuse_core::Variable& variable) + static bool value(fuse_core::Variable const& variable) { auto derived = dynamic_cast(&variable); return static_cast(derived) || is_variable_in_pack::value(variable); @@ -271,7 +271,7 @@ struct is_variable_in_pack } // namespace detail template -StampedVariableSynchronizer::StampedVariableSynchronizer(const fuse_core::UUID& device_id) +StampedVariableSynchronizer::StampedVariableSynchronizer(fuse_core::UUID const& device_id) : device_id_(device_id) , // NOTE(CH3): Uninitialized, for getting latest We use RCL_ROS_TIME so time comparisons are @@ -284,8 +284,8 @@ StampedVariableSynchronizer::StampedVariableSynchronizer(const fuse_core: } template -rclcpp::Time StampedVariableSynchronizer::findLatestCommonStamp(const fuse_core::Transaction& transaction, - const fuse_core::Graph& graph) +rclcpp::Time StampedVariableSynchronizer::findLatestCommonStamp(fuse_core::Transaction const& transaction, + fuse_core::Graph const& graph) { // Clear the previous stamp if the variable was deleted if (0u != latest_common_stamp_.nanoseconds() && @@ -305,13 +305,13 @@ rclcpp::Time StampedVariableSynchronizer::findLatestCommonStamp(const fus template template -void StampedVariableSynchronizer::updateTime(const VariableRange& variable_range, const fuse_core::Graph& graph) +void StampedVariableSynchronizer::updateTime(VariableRange const& variable_range, fuse_core::Graph const& graph) { - for (const auto& candidate_variable : variable_range) + for (auto const& candidate_variable : variable_range) { if (detail::is_variable_in_pack::value(candidate_variable)) { - const auto& stamped_variable = dynamic_cast(candidate_variable); + auto const& stamped_variable = dynamic_cast(candidate_variable); if ((stamped_variable.stamp() > latest_common_stamp_) && (stamped_variable.deviceId() == device_id_) && (detail::all_variables_exist::value(graph, stamped_variable.stamp(), device_id_))) { diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index 1b9675ae1..ff9547bdc 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -63,7 +63,7 @@ Path2DPublisher::Path2DPublisher() : fuse_core::AsyncPublisher(1), device_id_(fu } void Path2DPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -108,18 +108,18 @@ void Path2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /*tr } // Extract all of the 2D pose variables to the path std::vector poses; - for (const auto& variable : graph->getVariables()) + for (auto const& variable : graph->getVariables()) { - auto orientation = dynamic_cast(&variable); + auto orientation = dynamic_cast(&variable); if (orientation && (orientation->deviceId() == device_id_)) { - const auto& stamp = orientation->stamp(); + auto const& stamp = orientation->stamp(); auto position_uuid = fuse_variables::Position2DStamped(stamp, device_id_).uuid(); if (!graph->variableExists(position_uuid)) { continue; } - auto position = dynamic_cast(&graph->getVariable(position_uuid)); + auto position = dynamic_cast(&graph->getVariable(position_uuid)); geometry_msgs::msg::PoseStamped pose; pose.header.stamp = stamp; pose.header.frame_id = frame_id_; @@ -136,7 +136,7 @@ void Path2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /*tr return; } // Sort the poses by timestamp - auto compare_stamps = [](const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2) { + auto compare_stamps = [](geometry_msgs::msg::PoseStamped const& pose1, geometry_msgs::msg::PoseStamped const& pose2) { if (pose1.header.stamp.sec == pose2.header.stamp.sec) { return pose1.header.stamp.nanosec < pose2.header.stamp.nanosec; @@ -165,7 +165,7 @@ void Path2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /*tr geometry_msgs::msg::PoseArray pose_array_msg; pose_array_msg.header = header; std::transform(poses.begin(), poses.end(), std::back_inserter(pose_array_msg.poses), - [](const geometry_msgs::msg::PoseStamped& pose) { return pose.pose; }); // NOLINT(whitespace/braces) + [](geometry_msgs::msg::PoseStamped const& pose) { return pose.pose; }); // NOLINT(whitespace/braces) pose_array_publisher_->publish(pose_array_msg); } } diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index 67c014773..7e7b0bded 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -62,23 +62,23 @@ PLUGINLIB_EXPORT_CLASS(fuse_publishers::Pose2DPublisher, fuse_core::Publisher); namespace { -bool findPose(rclcpp::Logger logger, rclcpp::Clock clock, const fuse_core::Graph& graph, const rclcpp::Time& stamp, - const fuse_core::UUID& device_id, fuse_core::UUID& orientation_uuid, fuse_core::UUID& position_uuid, +bool findPose(rclcpp::Logger logger, rclcpp::Clock clock, fuse_core::Graph const& graph, rclcpp::Time const& stamp, + fuse_core::UUID const& device_id, fuse_core::UUID& orientation_uuid, fuse_core::UUID& position_uuid, geometry_msgs::msg::Pose& pose) { try { orientation_uuid = fuse_variables::Orientation2DStamped(stamp, device_id).uuid(); auto orientation_variable = - dynamic_cast(graph.getVariable(orientation_uuid)); + dynamic_cast(graph.getVariable(orientation_uuid)); position_uuid = fuse_variables::Position2DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); pose.position.x = position_variable.x(); pose.position.y = position_variable.y(); pose.position.z = 0.0; pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.yaw())); } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 10.0 * 1000, "Failed to find a pose at time " << stamp.nanoseconds() << ". Error" << e.what()); @@ -109,7 +109,7 @@ Pose2DPublisher::Pose2DPublisher() } void Pose2DPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -276,7 +276,7 @@ void Pose2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr tran // reason tf_transform_ = map_to_odom; } - catch (const std::exception& e) + catch (std::exception const& e) { RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 2.0 * 1000, "Could not lookup the transform " << base_frame_ << "->" << odom_frame_ diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index 5dad52ea4..0d9b24a98 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -58,7 +58,7 @@ SerializedPublisher::SerializedPublisher() } void SerializedPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) + fuse_core::node_interfaces::NodeInterfaces interfaces, std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -105,7 +105,7 @@ void SerializedPublisher::onInit() void SerializedPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph) { - const auto& stamp = transaction->stamp(); + auto const& stamp = transaction->stamp(); if (graph_publisher_->get_subscription_count() > 0) { graph_publisher_throttled_callback_(graph, stamp); @@ -121,7 +121,7 @@ void SerializedPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr } } -void SerializedPublisher::graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const +void SerializedPublisher::graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, rclcpp::Time const& stamp) const { fuse_msgs::msg::SerializedGraph msg; msg.header.stamp = stamp; diff --git a/fuse_publishers/test/test_path_2d_publisher.cpp b/fuse_publishers/test/test_path_2d_publisher.cpp index 05ebcd059..8a1abe94a 100644 --- a/fuse_publishers/test/test_path_2d_publisher.cpp +++ b/fuse_publishers/test/test_path_2d_publisher.cpp @@ -169,13 +169,13 @@ class Path2DPublisherTestFixture : public ::testing::Test rclcpp::shutdown(); } - void pathCallback(const nav_msgs::msg::Path& msg) + void pathCallback(nav_msgs::msg::Path const& msg) { path_msg_ = msg; received_path_msg_ = true; } - void poseArrayCallback(const geometry_msgs::msg::PoseArray& msg) + void poseArrayCallback(geometry_msgs::msg::PoseArray const& msg) { pose_array_msg_ = msg; received_pose_array_msg_ = true; diff --git a/fuse_publishers/test/test_pose_2d_publisher.cpp b/fuse_publishers/test/test_pose_2d_publisher.cpp index ee04675c8..26310a195 100644 --- a/fuse_publishers/test/test_pose_2d_publisher.cpp +++ b/fuse_publishers/test/test_pose_2d_publisher.cpp @@ -177,19 +177,19 @@ class Pose2DPublisherTestFixture : public ::testing::Test rclcpp::shutdown(); } - void poseCallback(const geometry_msgs::msg::PoseStamped& msg) + void poseCallback(geometry_msgs::msg::PoseStamped const& msg) { received_pose_msg_ = true; pose_msg_ = msg; } - void poseWithCovarianceCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) + void poseWithCovarianceCallback(geometry_msgs::msg::PoseWithCovarianceStamped const& msg) { received_pose_with_covariance_msg_ = true; pose_with_covariance_msg_ = msg; } - void tfCallback(const tf2_msgs::msg::TFMessage& msg) + void tfCallback(tf2_msgs::msg::TFMessage const& msg) { received_tf_msg_ = true; tf_msg_ = msg; diff --git a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp index 226782169..1b51494c0 100644 --- a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp +++ b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp @@ -131,7 +131,7 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * base AsyncPublisher class. */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) override; + std::string const& name) override; /** * @brief Perform any required initialization for the publisher diff --git a/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp b/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp index a9a6e6b64..d7bd6c8df 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp @@ -114,8 +114,8 @@ class RangeConstraint : public fuse_core::Constraint * @param[in] z - The distance measured between the robot and beacon by our new sensor * @param[in] sigma - The uncertainty of measured distance */ - RangeConstraint(const std::string& source, const fuse_variables::Position2DStamped& robot_position, - const fuse_variables::Point2DLandmark& beacon_position, const double z, const double sigma); + RangeConstraint(std::string const& source, fuse_variables::Position2DStamped const& robot_position, + fuse_variables::Point2DLandmark const& beacon_position, double const z, double const sigma); /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -163,7 +163,7 @@ class RangeConstraint : public fuse_core::Constraint * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& sigma_; diff --git a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp b/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp index 3a2920796..cee48792e 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp @@ -89,7 +89,7 @@ class RangeCostFunctor * @param[in] z The measured range to the beacon * @param[in] sigma The standard deviation of the range measurement */ - RangeCostFunctor(const double z, const double sigma) : sigma_(sigma), z_(z) + RangeCostFunctor(double const z, double const sigma) : sigma_(sigma), z_(z) { } diff --git a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp index 29dc12400..d94ccb7f6 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp @@ -135,7 +135,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * @brief Shadowing extension to the AsyncSensorModel::initialize call */ void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + std::string const& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Receives the set of known beacon positions @@ -148,7 +148,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * * @param[in] msg - Message containing the database of known but noisy beacon positions. */ - void priorBeaconsCallback(const sensor_msgs::msg::PointCloud2& msg); + void priorBeaconsCallback(sensor_msgs::msg::PointCloud2 const& msg); /** * @brief Callback for range measurement messages @@ -159,7 +159,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * * @param[in] msg - The range message to process */ - void rangesCallback(const sensor_msgs::msg::PointCloud2& msg); + void rangesCallback(sensor_msgs::msg::PointCloud2 const& msg); protected: /** diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 06bf0c745..8ae1f4e47 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -202,7 +202,7 @@ tf2_msgs::msg::TFMessage aprilTagPoses(Robot const& robot) return msg; } -tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& robot) +tf2_msgs::msg::TFMessage simulateAprilTag(Robot const& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp index 4317e096b..6dfaf50a2 100644 --- a/fuse_tutorials/src/beacon_publisher.cpp +++ b/fuse_tutorials/src/beacon_publisher.cpp @@ -52,7 +52,7 @@ PLUGINLIB_EXPORT_CLASS(fuse_tutorials::BeaconPublisher, fuse_core::Publisher); namespace fuse_tutorials { void BeaconPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name) + std::string const& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -79,10 +79,10 @@ void BeaconPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /* t // This is where all of the processing happens in this publisher implementation. All of the // beacons are represented as fuse_variables::Point2DLandmark objects. We loop through the // variables in the graph and keep a pointer to the variables that are the correct type. - auto beacons = std::vector(); - for (const auto& variable : graph->getVariables()) + auto beacons = std::vector(); + for (auto const& variable : graph->getVariables()) { - const auto beacon = dynamic_cast(&variable); + auto const beacon = dynamic_cast(&variable); if (beacon) { beacons.push_back(beacon); @@ -111,7 +111,7 @@ void BeaconPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /* t sensor_msgs::PointCloud2Iterator id_it(msg, "id"); for (auto id = 0u; id < beacons.size(); ++id) { - const auto& beacon = beacons.at(id); + auto const& beacon = beacons.at(id); *x_it = static_cast(beacon->x()); *y_it = static_cast(beacon->y()); *z_it = 0.0f; diff --git a/fuse_tutorials/src/range_constraint.cpp b/fuse_tutorials/src/range_constraint.cpp index de4f8887f..ad8050083 100644 --- a/fuse_tutorials/src/range_constraint.cpp +++ b/fuse_tutorials/src/range_constraint.cpp @@ -51,9 +51,9 @@ namespace fuse_tutorials // variable order defined in the RangeCostFunctor must match the variable order provided to the base // class Constraint constructor. In this case, robot position, then the beacon position // fuse_core::Constraint(source, { robot_position.uuid(), beacon_position.uuid() }) -RangeConstraint::RangeConstraint(const std::string& source, const fuse_variables::Position2DStamped& robot_position, - const fuse_variables::Point2DLandmark& beacon_position, const double z, - const double sigma) +RangeConstraint::RangeConstraint(std::string const& source, fuse_variables::Position2DStamped const& robot_position, + fuse_variables::Point2DLandmark const& beacon_position, double const z, + double const sigma) : fuse_core::Constraint(source, { robot_position.uuid(), beacon_position.uuid() }) , // NOLINT sigma_(sigma) diff --git a/fuse_tutorials/src/range_sensor_model.cpp b/fuse_tutorials/src/range_sensor_model.cpp index 89305d04d..04656c2e6 100644 --- a/fuse_tutorials/src/range_sensor_model.cpp +++ b/fuse_tutorials/src/range_sensor_model.cpp @@ -49,13 +49,13 @@ PLUGINLIB_EXPORT_CLASS(fuse_tutorials::RangeSensorModel, fuse_core::SensorModel) namespace fuse_tutorials { void RangeSensorModel::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string& name, fuse_core::TransactionCallback transaction_callback) + std::string const& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); } -void RangeSensorModel::priorBeaconsCallback(const sensor_msgs::msg::PointCloud2& msg) +void RangeSensorModel::priorBeaconsCallback(sensor_msgs::msg::PointCloud2 const& msg) { // Store a copy of the beacon database. We use a map to allow efficient lookups by ID number. sensor_msgs::PointCloud2ConstIterator x_it(msg, "x"); @@ -117,7 +117,7 @@ void RangeSensorModel::onStop() sub_.reset(); } -void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2& msg) +void RangeSensorModel::rangesCallback(sensor_msgs::msg::PointCloud2 const& msg) { // We received a new message for our sensor. This is where most of the processing happens for our // sensor model. We take the published ROS message and transform it into one or more Constraints, diff --git a/fuse_tutorials/src/range_sensor_simulator.cpp b/fuse_tutorials/src/range_sensor_simulator.cpp index 34f0f429a..172db0d40 100644 --- a/fuse_tutorials/src/range_sensor_simulator.cpp +++ b/fuse_tutorials/src/range_sensor_simulator.cpp @@ -107,14 +107,14 @@ std::vector createBeacons() /** * @brief Create a noisy set of beacon priors from the true set of beacons */ -std::vector createNoisyBeacons(const std::vector& beacons) +std::vector createNoisyBeacons(std::vector const& beacons) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; static std::normal_distribution<> noise{ 0.0, BEACON_SIGMA }; auto noisy_beacons = std::vector(); - for (const auto& beacon : beacons) + for (auto const& beacon : beacons) { noisy_beacons.push_back({ beacon.x + noise(generator), beacon.y + noise(generator) }); // NOLINT } @@ -124,7 +124,7 @@ std::vector createNoisyBeacons(const std::vector& beacons) /** * @brief Convert the set of beacons into a pointcloud for visualization purposes */ -sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud(const std::vector& beacons, rclcpp::Clock& clock) +sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud(std::vector const& beacons, rclcpp::Clock& clock) { auto msg = std::make_shared(); msg->header.stamp = clock.now(); @@ -144,7 +144,7 @@ sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud(const std::vector(beacon.x); *y_it = static_cast(beacon.y); *z_it = 10.0f; @@ -161,7 +161,7 @@ sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud(const std::vector(); msg->header.stamp = state.stamp; @@ -201,7 +201,7 @@ nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) * The state estimator will not run until it has been sent a starting pose. */ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, - const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) + Robot const& state, rclcpp::Clock::SharedPtr const& clock, rclcpp::Logger const& logger) { // Send the initial localization signal to the state estimator auto srv = std::make_shared(); @@ -252,7 +252,7 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces& beacons) +sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor(Robot const& robot, std::vector const& beacons) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; @@ -333,7 +333,7 @@ sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor(const Robot& robot, for (auto id = 0u; id < beacons.size(); ++id) { // Compute the distance to each beacon - const auto& beacon = beacons.at(id); + auto const& beacon = beacons.at(id); auto dx = robot.x - beacon.x; auto dy = robot.y - beacon.y; auto range = std::sqrt(dx * dx + dy * dy) + noise(generator); diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index a4b3bd1a5..465421604 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -197,7 +197,7 @@ sensor_msgs::msg::Imu simulateImu(Robot const& robot) return msg; } -nav_msgs::msg::Odometry simulateOdometry(const Robot& robot) +nav_msgs::msg::Odometry simulateOdometry(Robot const& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; @@ -230,7 +230,7 @@ nav_msgs::msg::Odometry simulateOdometry(const Robot& robot) } void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, - const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) + Robot const& state, rclcpp::Clock::SharedPtr const& clock, rclcpp::Logger const& logger) { // Send the initial localization signal to the state estimator auto srv = std::make_shared(); diff --git a/fuse_variables/doc/variables.md b/fuse_variables/doc/variables.md new file mode 100644 index 000000000..846b912a0 --- /dev/null +++ b/fuse_variables/doc/variables.md @@ -0,0 +1,3 @@ +# Variables + +[Coming soon](https://github.com/PickNikRobotics/fuse/issues/23) diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp index c1601dac4..e4e41c909 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp @@ -86,8 +86,8 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationAngular2DStamped(const rclcpp::Time& stamp, - const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit AccelerationAngular2DStamped(rclcpp::Time const& stamp, + fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the angular acceleration. @@ -100,7 +100,7 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped /** * @brief Read-only access to the angular acceleration. */ - const double& yaw() const + double const& yaw() const { return data_[YAW]; } @@ -136,7 +136,7 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp index 39abe9994..74c75ace7 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp @@ -88,8 +88,8 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationAngular3DStamped(const rclcpp::Time& stamp, - const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit AccelerationAngular3DStamped(rclcpp::Time const& stamp, + fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the roll (X-axis) angular acceleration. @@ -102,7 +102,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the roll (X-axis) angular acceleration. */ - const double& roll() const + double const& roll() const { return data_[ROLL]; } @@ -118,7 +118,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the pitch (Y-axis) angular acceleration. */ - const double& pitch() const + double const& pitch() const { return data_[PITCH]; } @@ -134,7 +134,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the yaw (Z-axis) angular acceleration. */ - const double& yaw() const + double const& yaw() const { return data_[YAW]; } @@ -170,7 +170,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp index 86b6a7659..acd57c5a9 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp @@ -87,8 +87,8 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationLinear2DStamped(const rclcpp::Time& stamp, - const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit AccelerationLinear2DStamped(rclcpp::Time const& stamp, + fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear acceleration. @@ -101,7 +101,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped /** * @brief Read-only access to the X-axis linear acceleration. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -117,7 +117,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped /** * @brief Read-only access to the Y-axis linear acceleration. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -153,7 +153,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp index e5147accd..5a8b52340 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp @@ -88,8 +88,8 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationLinear3DStamped(const rclcpp::Time& stamp, - const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit AccelerationLinear3DStamped(rclcpp::Time const& stamp, + fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear acceleration. @@ -102,7 +102,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the X-axis linear acceleration. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -118,7 +118,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the Y-axis linear acceleration. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -134,7 +134,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the Z-axis linear acceleration. */ - const double& z() const + double const& z() const { return data_[Z]; } @@ -170,7 +170,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp index 7c452e961..5c180e0d6 100644 --- a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp +++ b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp @@ -82,7 +82,7 @@ class FixedSizeVariable : public fuse_core::Variable /** * @brief Constructor */ - explicit FixedSizeVariable(const fuse_core::UUID& uuid) + explicit FixedSizeVariable(fuse_core::UUID const& uuid) : fuse_core::Variable(uuid), data_{} // zero-initialize the data array { } @@ -106,7 +106,7 @@ class FixedSizeVariable : public fuse_core::Variable /** * @brief Read-only access to the variable data */ - const double* data() const override + double const* data() const override { return data_.data(); } @@ -122,7 +122,7 @@ class FixedSizeVariable : public fuse_core::Variable /** * @brief Read-only access to the variable data as a std::array */ - const std::array& array() const + std::array const& array() const { return data_; } @@ -150,7 +150,7 @@ class FixedSizeVariable : public fuse_core::Variable * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); archive& data_; diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index 871f33ce7..b89012446 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -78,27 +78,27 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati return 1; } - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + bool Plus(double const* x, double const* delta, double* x_plus_delta) const override { // Compute the angle increment as a linear update, and handle the 2*Pi rollover x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); return true; } - bool ComputeJacobian(const double* /*x*/, double* jacobian) const override + bool ComputeJacobian(double const* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; } - bool Minus(const double* x, const double* y, double* y_minus_x) const override + bool Minus(double const* x, double const* y, double* y_minus_x) const override { // Compute the difference from x to y, and handle the 2*Pi rollover y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; } - bool ComputeMinusJacobian(const double* /*x*/, double* jacobian) const override + bool ComputeMinusJacobian(double const* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; @@ -116,7 +116,7 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } @@ -145,27 +145,27 @@ class Orientation2DManifold : public fuse_core::Manifold return 1; } - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + bool Plus(double const* x, double const* delta, double* x_plus_delta) const override { // Compute the angle increment as a linear update, and handle the 2*Pi rollover x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); return true; } - bool PlusJacobian(const double* /*x*/, double* jacobian) const override + bool PlusJacobian(double const* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; } - bool Minus(const double* y, const double* x, double* y_minus_x) const override + bool Minus(double const* y, double const* x, double* y_minus_x) const override { // Compute the difference from y to x, and handle the 2*Pi rollover y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; } - bool MinusJacobian(const double* /*x*/, double* jacobian) const override + bool MinusJacobian(double const* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; @@ -182,7 +182,7 @@ class Orientation2DManifold : public fuse_core::Manifold * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } @@ -222,7 +222,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Orientation2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit Orientation2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the heading angle. @@ -235,7 +235,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped /** * @brief Read-only access to the heading angle. */ - const double& yaw() const + double const& yaw() const { return data_[YAW]; } @@ -292,7 +292,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index ceae3a676..df8f2d230 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -96,7 +96,7 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return 3; } - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + bool Plus(double const* x, double const* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); @@ -104,7 +104,7 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool ComputeJacobian(const double* x, double* jacobian) const override + bool ComputeJacobian(double const* x, double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; @@ -127,7 +127,7 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool Minus(const double* x, const double* y, double* y_minus_x) const override + bool Minus(double const* x, double const* y, double* y_minus_x) const override { double x_inverse[4]; QuaternionInverse(x, static_cast(x_inverse)); @@ -137,7 +137,7 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool ComputeMinusJacobian(const double* x, double* jacobian) const override + bool ComputeMinusJacobian(double const* x, double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; @@ -172,7 +172,7 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } @@ -201,7 +201,7 @@ class Orientation3DManifold : public fuse_core::Manifold return 3; } - bool Plus(const double* x, const double* delta, double* x_plus_delta) const override + bool Plus(double const* x, double const* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); @@ -209,7 +209,7 @@ class Orientation3DManifold : public fuse_core::Manifold return true; } - bool PlusJacobian(const double* x, double* jacobian) const override + bool PlusJacobian(double const* x, double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; @@ -232,7 +232,7 @@ class Orientation3DManifold : public fuse_core::Manifold return true; } - bool Minus(const double* y, const double* x, double* y_minus_x) const override + bool Minus(double const* y, double const* x, double* y_minus_x) const override { double x_inverse[4]; QuaternionInverse(x, static_cast(x_inverse)); @@ -242,7 +242,7 @@ class Orientation3DManifold : public fuse_core::Manifold return true; } - bool MinusJacobian(const double* x, double* jacobian) const override + bool MinusJacobian(double const* x, double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; @@ -276,7 +276,7 @@ class Orientation3DManifold : public fuse_core::Manifold * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } @@ -332,7 +332,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Orientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit Orientation3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the quaternion w component @@ -345,7 +345,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped /** * @brief Read-only access to the quaternion w component */ - const double& w() const + double const& w() const { return data_[W]; } @@ -361,7 +361,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped /** * @brief Read-only access to the quaternion x component */ - const double& x() const + double const& x() const { return data_[X]; } @@ -377,7 +377,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped /** * @brief Read-only access to the quaternion y component */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -393,7 +393,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped /** * @brief Read-only access to the quaternion z component */ - const double& z() const + double const& z() const { return data_[Z]; } @@ -469,7 +469,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.hpp b/fuse_variables/include/fuse_variables/pinhole_camera.hpp index 854460661..1fdd1c168 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera.hpp @@ -85,15 +85,15 @@ class PinholeCamera : public FixedSizeVariable<4> * * @param[in] camera_id The id associated to a camera */ - explicit PinholeCamera(const uint64_t& camera_id); + explicit PinholeCamera(uint64_t const& camera_id); /** * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters * * @param[in] camera_id The id associated to a camera */ - explicit PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id, const double& fx, const double& fy, - const double& cx, const double& cy); + explicit PinholeCamera(fuse_core::UUID const& uuid, uint64_t const& camera_id, double const& fx, double const& fy, + double const& cx, double const& cy); /** * @brief Read-write access to the cx parameter. @@ -106,7 +106,7 @@ class PinholeCamera : public FixedSizeVariable<4> /** * @brief Read-only access to the cx parameter. */ - const double& cx() const + double const& cx() const { return data_[CX]; } @@ -122,7 +122,7 @@ class PinholeCamera : public FixedSizeVariable<4> /** * @brief Read-only access to the cy parameter. */ - const double& cy() const + double const& cy() const { return data_[CY]; } @@ -138,7 +138,7 @@ class PinholeCamera : public FixedSizeVariable<4> /** * @brief Read-only access to the fx parameter. */ - const double& fx() const + double const& fx() const { return data_[FX]; } @@ -154,7 +154,7 @@ class PinholeCamera : public FixedSizeVariable<4> /** * @brief Read-only access to the fy parameter. */ - const double& fy() const + double const& fy() const { return data_[FY]; } @@ -162,7 +162,7 @@ class PinholeCamera : public FixedSizeVariable<4> /** * @brief Read-only access to the id */ - const uint64_t& id() const + uint64_t const& id() const { return id_; } @@ -193,7 +193,7 @@ class PinholeCamera : public FixedSizeVariable<4> * * @param[in] camera_id The id associated to a camera */ - PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id); + PinholeCamera(fuse_core::UUID const& uuid, uint64_t const& camera_id); private: // Allow Boost Serialization access to private methods @@ -210,7 +210,7 @@ class PinholeCamera : public FixedSizeVariable<4> * Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& id_; diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp index ee5a53d7a..54b5716ed 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp @@ -68,15 +68,15 @@ class PinholeCameraFixed : public PinholeCamera * * @param[in] camera_id The id associated to a camera */ - explicit PinholeCameraFixed(const uint64_t& camera_id); + explicit PinholeCameraFixed(uint64_t const& camera_id); /** * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters * * @param[in] camera_id The id associated to a camera */ - explicit PinholeCameraFixed(const uint64_t& camera_id, const double& fx, const double& fy, const double& cx, - const double& cy); + explicit PinholeCameraFixed(uint64_t const& camera_id, double const& fx, double const& fy, double const& cx, + double const& cy); /** * @brief Specifies if the value of the variable should not be changed during optimization */ @@ -111,7 +111,7 @@ class PinholeCameraFixed : public PinholeCamera * Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object(*this); } diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp index c097e9e74..8ff92354e 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp @@ -79,7 +79,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point2DFixedLandmark(const uint64_t& landmark_id); + explicit Point2DFixedLandmark(uint64_t const& landmark_id); /** * @brief Read-write access to the X-axis position. @@ -92,7 +92,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> /** * @brief Read-only access to the X-axis position. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -108,7 +108,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> /** * @brief Read-only access to the Y-axis position. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -116,7 +116,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> /** * @brief Read-only access to the id */ - const uint64_t& id() const + uint64_t const& id() const { return id_; } @@ -158,7 +158,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& id_; diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp index 558fc8e74..ccc537d34 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp @@ -79,7 +79,7 @@ class Point2DLandmark : public FixedSizeVariable<2> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point2DLandmark(const uint64_t& landmark_id); + explicit Point2DLandmark(uint64_t const& landmark_id); /** * @brief Read-write access to the X-axis position. @@ -92,7 +92,7 @@ class Point2DLandmark : public FixedSizeVariable<2> /** * @brief Read-only access to the X-axis position. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -108,7 +108,7 @@ class Point2DLandmark : public FixedSizeVariable<2> /** * @brief Read-only access to the Y-axis position. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -116,7 +116,7 @@ class Point2DLandmark : public FixedSizeVariable<2> /** * @brief Read-only access to the id */ - const uint64_t& id() const + uint64_t const& id() const { return id_; } @@ -153,7 +153,7 @@ class Point2DLandmark : public FixedSizeVariable<2> * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& id_; diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp index ca2eaa86f..74e6d8742 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp @@ -80,7 +80,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point3DFixedLandmark(const uint64_t& landmark_id); + explicit Point3DFixedLandmark(uint64_t const& landmark_id); /** * @brief Read-write access to the X-axis position. @@ -93,7 +93,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the X-axis position. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -109,7 +109,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the Y-axis position. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -125,7 +125,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the Z-axis position. */ - const double& z() const + double const& z() const { return data_[Z]; } @@ -133,7 +133,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the id */ - const uint64_t& id() const + uint64_t const& id() const { return id_; } @@ -175,7 +175,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& id_; diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp index 630e51e4a..6839e7413 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp @@ -83,7 +83,7 @@ class Point3DLandmark : public FixedSizeVariable<3> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point3DLandmark(const uint64_t& landmark_id); + explicit Point3DLandmark(uint64_t const& landmark_id); /** * @brief Read-write access to the X-axis position. @@ -96,7 +96,7 @@ class Point3DLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the X-axis position. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -112,7 +112,7 @@ class Point3DLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the Y-axis position. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -128,7 +128,7 @@ class Point3DLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the Z-axis position. */ - const double& z() const + double const& z() const { return data_[Z]; } @@ -136,7 +136,7 @@ class Point3DLandmark : public FixedSizeVariable<3> /** * @brief Read-only access to the id */ - const uint64_t& id() const + uint64_t const& id() const { return id_; } @@ -173,7 +173,7 @@ class Point3DLandmark : public FixedSizeVariable<3> * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& id_; diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp index 55459d899..ac4c00afe 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp @@ -88,7 +88,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * robots or devices * */ - explicit Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit Position2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis position. @@ -101,7 +101,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped /** * @brief Read-only access to the X-axis position. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -117,7 +117,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped /** * @brief Read-only access to the Y-axis position. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -153,7 +153,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp index faa614f9d..19d61be2b 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp @@ -88,7 +88,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Position3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit Position3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis position. @@ -101,7 +101,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the X-axis position. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -117,7 +117,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the Y-axis position. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -133,7 +133,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the Z-axis position. */ - const double& z() const + double const& z() const { return data_[Z]; } @@ -169,7 +169,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/stamped.hpp b/fuse_variables/include/fuse_variables/stamped.hpp index 98f186b72..13e57c4ca 100644 --- a/fuse_variables/include/fuse_variables/stamped.hpp +++ b/fuse_variables/include/fuse_variables/stamped.hpp @@ -67,7 +67,7 @@ class Stamped /** * @brief Constructor */ - explicit Stamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + explicit Stamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL) : device_id_(device_id), stamp_(stamp) { } @@ -84,7 +84,7 @@ class Stamped /** * @brief Read-only access to the associated timestamp. */ - const rclcpp::Time& stamp() const + rclcpp::Time const& stamp() const { return stamp_; } @@ -92,7 +92,7 @@ class Stamped /** * @brief Read-only access to the associated device ID. */ - const fuse_core::UUID& deviceId() const + fuse_core::UUID const& deviceId() const { return device_id_; } @@ -113,7 +113,7 @@ class Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& device_id_; archive& stamp_; diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp index 4b32ca169..758103c93 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp @@ -86,7 +86,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit VelocityAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit VelocityAngular2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the angular velocity. @@ -99,7 +99,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped /** * @brief Read-only access to the angular velocity. */ - const double& yaw() const + double const& yaw() const { return data_[YAW]; } @@ -135,7 +135,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp index 48a79e6bd..62f7c8167 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp @@ -88,7 +88,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit VelocityAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit VelocityAngular3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the roll (X-axis) angular velocity. @@ -101,7 +101,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the roll (X-axis) angular velocity. */ - const double& roll() const + double const& roll() const { return data_[ROLL]; } @@ -117,7 +117,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the pitch (Y-axis) angular velocity. */ - const double& pitch() const + double const& pitch() const { return data_[PITCH]; } @@ -133,7 +133,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the yaw (Z-axis) angular velocity. */ - const double& yaw() const + double const& yaw() const { return data_[YAW]; } @@ -169,7 +169,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp index d70b9d05d..ce8211518 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp @@ -88,7 +88,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * robots or devices * */ - explicit VelocityLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); + explicit VelocityLinear2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear velocity. @@ -101,7 +101,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped /** * @brief Read-only access to the X-axis linear velocity. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -117,7 +117,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped /** * @brief Read-only access to the Y-axis linear velocity. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -153,7 +153,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp index 23bf71587..0520ece6d 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp @@ -89,7 +89,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * robots or devices * */ - explicit VelocityLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_ids = fuse_core::uuid::NIL); + explicit VelocityLinear3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_ids = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear velocity. @@ -102,7 +102,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the X-axis linear velocity. */ - const double& x() const + double const& x() const { return data_[X]; } @@ -118,7 +118,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the Y-axis linear velocity. */ - const double& y() const + double const& y() const { return data_[Y]; } @@ -134,7 +134,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped /** * @brief Read-only access to the Z-axis linear velocity. */ - const double& z() const + double const& z() const { return data_[Z]; } @@ -170,7 +170,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); diff --git a/fuse_variables/src/acceleration_angular_2d_stamped.cpp b/fuse_variables/src/acceleration_angular_2d_stamped.cpp index 2cdb40ed6..e9892bc87 100644 --- a/fuse_variables/src/acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_2d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -AccelerationAngular2DStamped::AccelerationAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +AccelerationAngular2DStamped::AccelerationAngular2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable<1>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/acceleration_angular_3d_stamped.cpp b/fuse_variables/src/acceleration_angular_3d_stamped.cpp index 9e4f35401..d2a334982 100644 --- a/fuse_variables/src/acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_3d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -AccelerationAngular3DStamped::AccelerationAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +AccelerationAngular3DStamped::AccelerationAngular3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable<3>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/acceleration_linear_2d_stamped.cpp b/fuse_variables/src/acceleration_linear_2d_stamped.cpp index 47d2c2ca2..a116165a4 100644 --- a/fuse_variables/src/acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_2d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -AccelerationLinear2DStamped::AccelerationLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +AccelerationLinear2DStamped::AccelerationLinear2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/acceleration_linear_3d_stamped.cpp b/fuse_variables/src/acceleration_linear_3d_stamped.cpp index 5018a0742..5c2f3e545 100644 --- a/fuse_variables/src/acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_3d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -AccelerationLinear3DStamped::AccelerationLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +AccelerationLinear3DStamped::AccelerationLinear3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 4f01dffb7..cde3b33e8 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -47,7 +47,7 @@ namespace fuse_variables { -Orientation2DStamped::Orientation2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +Orientation2DStamped::Orientation2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index 895ee1a4b..00e6a03e4 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -46,7 +46,7 @@ namespace fuse_variables { -Orientation3DStamped::Orientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +Orientation3DStamped::Orientation3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable<4>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/pinhole_camera.cpp b/fuse_variables/src/pinhole_camera.cpp index 68583c0bb..01dca7baf 100644 --- a/fuse_variables/src/pinhole_camera.cpp +++ b/fuse_variables/src/pinhole_camera.cpp @@ -47,18 +47,18 @@ namespace fuse_variables { -PinholeCamera::PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id) +PinholeCamera::PinholeCamera(fuse_core::UUID const& uuid, uint64_t const& camera_id) : FixedSizeVariable(uuid), id_(camera_id) { } -PinholeCamera::PinholeCamera(const uint64_t& camera_id) +PinholeCamera::PinholeCamera(uint64_t const& camera_id) : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) { } -PinholeCamera::PinholeCamera(const fuse_core::UUID& /*uuid*/, const uint64_t& camera_id, const double& fx, - const double& fy, const double& cx, const double& cy) +PinholeCamera::PinholeCamera(fuse_core::UUID const& /*uuid*/, uint64_t const& camera_id, double const& fx, + double const& fy, double const& cx, double const& cy) : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) { data_[FX] = fx; diff --git a/fuse_variables/src/pinhole_camera_fixed.cpp b/fuse_variables/src/pinhole_camera_fixed.cpp index 29c0f79af..35fb03572 100644 --- a/fuse_variables/src/pinhole_camera_fixed.cpp +++ b/fuse_variables/src/pinhole_camera_fixed.cpp @@ -45,13 +45,13 @@ namespace fuse_variables { -PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id) +PinholeCameraFixed::PinholeCameraFixed(uint64_t const& camera_id) : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) { } -PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id, const double& fx, const double& fy, const double& cx, - const double& cy) +PinholeCameraFixed::PinholeCameraFixed(uint64_t const& camera_id, double const& fx, double const& fy, double const& cx, + double const& cy) : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id, fx, fy, cx, cy) { } diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp index 88886eb6f..0620c7aa2 100644 --- a/fuse_variables/src/point_2d_fixed_landmark.cpp +++ b/fuse_variables/src/point_2d_fixed_landmark.cpp @@ -42,7 +42,7 @@ namespace fuse_variables { -Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) +Point2DFixedLandmark::Point2DFixedLandmark(uint64_t const& landmark_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp index aede91f22..ac698d5f9 100644 --- a/fuse_variables/src/point_2d_landmark.cpp +++ b/fuse_variables/src/point_2d_landmark.cpp @@ -42,7 +42,7 @@ namespace fuse_variables { -Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) +Point2DLandmark::Point2DLandmark(uint64_t const& landmark_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp index 47f553761..16a647f8a 100644 --- a/fuse_variables/src/point_3d_fixed_landmark.cpp +++ b/fuse_variables/src/point_3d_fixed_landmark.cpp @@ -42,7 +42,7 @@ namespace fuse_variables { -Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) +Point3DFixedLandmark::Point3DFixedLandmark(uint64_t const& landmark_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp index 0367b7920..e77615d2e 100644 --- a/fuse_variables/src/point_3d_landmark.cpp +++ b/fuse_variables/src/point_3d_landmark.cpp @@ -42,7 +42,7 @@ namespace fuse_variables { -Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) +Point3DLandmark::Point3DLandmark(uint64_t const& landmark_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } diff --git a/fuse_variables/src/position_2d_stamped.cpp b/fuse_variables/src/position_2d_stamped.cpp index 3e8675a4c..6fb1b1407 100644 --- a/fuse_variables/src/position_2d_stamped.cpp +++ b/fuse_variables/src/position_2d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -Position2DStamped::Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +Position2DStamped::Position2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/position_3d_stamped.cpp b/fuse_variables/src/position_3d_stamped.cpp index 26e23aa42..4b5a9cf3d 100644 --- a/fuse_variables/src/position_3d_stamped.cpp +++ b/fuse_variables/src/position_3d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -Position3DStamped::Position3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +Position3DStamped::Position3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/velocity_angular_2d_stamped.cpp b/fuse_variables/src/velocity_angular_2d_stamped.cpp index d3a0a1f4f..bac8d88d1 100644 --- a/fuse_variables/src/velocity_angular_2d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_2d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -VelocityAngular2DStamped::VelocityAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +VelocityAngular2DStamped::VelocityAngular2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/velocity_angular_3d_stamped.cpp b/fuse_variables/src/velocity_angular_3d_stamped.cpp index 25e8d3538..c88e316ad 100644 --- a/fuse_variables/src/velocity_angular_3d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_3d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -VelocityAngular3DStamped::VelocityAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +VelocityAngular3DStamped::VelocityAngular3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/velocity_linear_2d_stamped.cpp b/fuse_variables/src/velocity_linear_2d_stamped.cpp index d6db6c014..bd6903d6d 100644 --- a/fuse_variables/src/velocity_linear_2d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_2d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -VelocityLinear2DStamped::VelocityLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +VelocityLinear2DStamped::VelocityLinear2DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/src/velocity_linear_3d_stamped.cpp b/fuse_variables/src/velocity_linear_3d_stamped.cpp index 1bb1d42d0..2633c3408 100644 --- a/fuse_variables/src/velocity_linear_3d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_3d_stamped.cpp @@ -44,7 +44,7 @@ namespace fuse_variables { -VelocityLinear3DStamped::VelocityLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) +VelocityLinear3DStamped::VelocityLinear3DStamped(rclcpp::Time const& stamp, fuse_core::UUID const& device_id) : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } diff --git a/fuse_variables/test/test_fixed_size_variable.cpp b/fuse_variables/test/test_fixed_size_variable.cpp index 9c39e2a83..31d7d3f0c 100644 --- a/fuse_variables/test/test_fixed_size_variable.cpp +++ b/fuse_variables/test/test_fixed_size_variable.cpp @@ -69,7 +69,7 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> * @param[in] version - The version of the archive being read/written. Generally unused. */ template - void serialize(Archive& archive, const unsigned int /* version */) + void serialize(Archive& archive, unsigned int const /* version */) { archive& boost::serialization::base_object>(*this); } @@ -89,7 +89,7 @@ TEST(FixedSizeVariable, Data) TestVariable variable; EXPECT_NO_THROW(variable.data()[0] = 1.0); EXPECT_NO_THROW(variable.data()[1] = 2.0); - const TestVariable& const_variable = variable; + TestVariable const& const_variable = variable; bool success = true; EXPECT_NO_THROW(success = success && const_variable.data()[0] == 1.0); EXPECT_NO_THROW(success = success && const_variable.data()[1] == 2.0); @@ -104,7 +104,7 @@ TEST(FixedSizeVariable, Array) EXPECT_NO_THROW(variable.array().at(1) = 2.0); EXPECT_NO_THROW(variable.array().front() = 3.0); EXPECT_NO_THROW(variable.array().back() = 4.0); - const TestVariable& const_variable = variable; + TestVariable const& const_variable = variable; bool success = true; EXPECT_NO_THROW(success = success && const_variable.array()[0] == 3.0); EXPECT_NO_THROW(success = success && const_variable.array().at(1) == 4.0); diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index 44acad27c..fdc522802 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -350,7 +350,7 @@ TEST(Orientation3DStamped, Optimization) TEST(Orientation3DStamped, Euler) { - const double rad_to_deg = 180.0 / M_PI; + double const rad_to_deg = 180.0 / M_PI; // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees Orientation3DStamped orientation_r(rclcpp::Time(12345678, 910111213)); diff --git a/fuse_viz/include/fuse_viz/conversions.hpp b/fuse_viz/include/fuse_viz/conversions.hpp index 63897a506..7881e4ba4 100644 --- a/fuse_viz/include/fuse_viz/conversions.hpp +++ b/fuse_viz/include/fuse_viz/conversions.hpp @@ -62,7 +62,7 @@ namespace tf2 * @param[out] msg 6x6 covariance message, which is stored as a plain array with 36 elements. */ template -inline void toMsg(const Eigen::MatrixBase& covariance, std::array& msg) +inline void toMsg(Eigen::MatrixBase const& covariance, std::array& msg) { using Scalar = typename Derived::Scalar; using Matrix6 = Eigen::Matrix; @@ -82,39 +82,39 @@ inline void toMsg(const Eigen::MatrixBase& covariance, std::array(position.x()), static_cast(position.y()), static_cast(position.z()) }; } -inline Ogre::Quaternion toOgre(const tf2::Quaternion& orientation) +inline Ogre::Quaternion toOgre(tf2::Quaternion const& orientation) { return { static_cast(orientation.w()), static_cast(orientation.x()), // NOLINT static_cast(orientation.y()), static_cast(orientation.z()) }; } -inline Ogre::Vector3 toOgre(const fuse_variables::Position2DStamped& position) +inline Ogre::Vector3 toOgre(fuse_variables::Position2DStamped const& position) { return { static_cast(position.x()), static_cast(position.y()), 0 }; } -inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped& orientation) +inline Ogre::Quaternion toOgre(fuse_variables::Orientation2DStamped const& orientation) { return toOgre(toTF(orientation)); } @@ -124,24 +124,24 @@ inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped& orien namespace { -inline tf2::Transform getPose(const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation) +inline tf2::Transform getPose(fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation) { return tf2::Transform(toTF(orientation), toTF(position)); } -inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UUID& position_uuid, - const fuse_core::UUID& orientation_uuid) +inline tf2::Transform getPose(fuse_core::Graph const& graph, fuse_core::UUID const& position_uuid, + fuse_core::UUID const& orientation_uuid) { - const auto position = dynamic_cast(&graph.getVariable(position_uuid)); + auto const position = dynamic_cast(&graph.getVariable(position_uuid)); if (!position) { throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + " from graph as fuse_variables::Position2DStamped."); } - const auto orientation = - dynamic_cast(&graph.getVariable(orientation_uuid)); + auto const orientation = + dynamic_cast(&graph.getVariable(orientation_uuid)); if (!orientation) { throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp b/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp index 120a511df..910254f43 100644 --- a/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp +++ b/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp @@ -86,9 +86,9 @@ class MappedCovarianceProperty : public rviz_common::properties::BoolProperty RGB, }; - MappedCovarianceProperty(const QString& name = "Covariance", bool default_value = false, - const QString& description = QString(), rviz_common::properties::Property* parent = 0, - const char* changed_slot = 0, QObject* receiver = 0); + MappedCovarianceProperty(QString const& name = "Covariance", bool default_value = false, + QString const& description = QString(), rviz_common::properties::Property* parent = 0, + char const* changed_slot = 0, QObject* receiver = 0); virtual ~MappedCovarianceProperty(); @@ -96,9 +96,9 @@ class MappedCovarianceProperty : public rviz_common::properties::BoolProperty bool getOrientationBool(); // Methods to manage the unordered map of Covariance Visuals - MappedCovarianceVisualPtr createAndInsertVisual(const std::string& key, Ogre::SceneManager* scene_manager, + MappedCovarianceVisualPtr createAndInsertVisual(std::string const& key, Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node); - void eraseVisual(const std::string& key); + void eraseVisual(std::string const& key); void clearVisual(); size_t sizeVisual(); @@ -111,9 +111,9 @@ private Q_SLOTS: void updateColorStyleChoice(); private: - void updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr& visual); - void updateOrientationFrame(const MappedCovarianceVisualPtr& visual); - void updateVisibility(const MappedCovarianceVisualPtr& visual); + void updateColorAndAlphaAndScaleAndOffset(MappedCovarianceVisualPtr const& visual); + void updateOrientationFrame(MappedCovarianceVisualPtr const& visual); + void updateVisibility(MappedCovarianceVisualPtr const& visual); std::unordered_map covariances_; diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp index b5f4d3dc1..6acb5a1a4 100644 --- a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp +++ b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp @@ -121,7 +121,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param b Blue component */ virtual void setPositionColor(float r, float g, float b, float a); - void setPositionColor(const Ogre::ColourValue& color); + void setPositionColor(Ogre::ColourValue const& color); /** * \brief Set the color of the orientation covariance. Values are in the range [0, 1] @@ -131,7 +131,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param b Blue component */ virtual void setOrientationColor(float r, float g, float b, float a); - void setOrientationColor(const Ogre::ColourValue& color); + void setOrientationColor(Ogre::ColourValue const& color); void setOrientationColorToRGB(float a); /** @brief Set the covariance. @@ -139,10 +139,10 @@ class MappedCovarianceVisual : public rviz_rendering::Object * This effectively changes the orientation and scale of position and orientation * covariance shapes */ - virtual void setCovariance(const geometry_msgs::msg::PoseWithCovariance& pose); + virtual void setCovariance(geometry_msgs::msg::PoseWithCovariance const& pose); - virtual const Ogre::Vector3& getPositionCovarianceScale(); - virtual const Ogre::Quaternion& getPositionCovarianceOrientation(); + virtual Ogre::Vector3 const& getPositionCovarianceScale(); + virtual Ogre::Quaternion const& getPositionCovarianceOrientation(); /** * \brief Get the root scene node of the position part of this covariance @@ -180,7 +180,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object /** * \brief Sets user data on all ogre objects we own */ - virtual void setUserData(const Ogre::Any& data); + virtual void setUserData(Ogre::Any const& data); /** * \brief Sets visibility of this covariance @@ -202,12 +202,12 @@ class MappedCovarianceVisual : public rviz_rendering::Object /** * \brief Sets position of the frame this covariance is attached */ - virtual void setPosition(const Ogre::Vector3& position); + virtual void setPosition(Ogre::Vector3 const& position); /** * \brief Sets orientation of the frame this covariance is attached */ - virtual void setOrientation(const Ogre::Quaternion& orientation); + virtual void setOrientation(Ogre::Quaternion const& orientation); /** * \brief Sets which frame to attach the covariance of the orientation @@ -215,8 +215,8 @@ class MappedCovarianceVisual : public rviz_rendering::Object virtual void setRotatingFrame(bool use_rotating_frame); private: - void updatePosition(const Eigen::Matrix6d& covariance); - void updateOrientation(const Eigen::Matrix6d& covariance, ShapeIndex index); + void updatePosition(Eigen::Matrix6d const& covariance); + void updateOrientation(Eigen::Matrix6d const& covariance, ShapeIndex index); void updateOrientationVisibility(); Ogre::SceneNode* root_node_ = nullptr; @@ -241,19 +241,19 @@ class MappedCovarianceVisual : public rviz_rendering::Object Ogre::Vector3 current_ori_scale_[kNumOriShapes]; float current_ori_scale_factor_; - static const float max_degrees; + static float const max_degrees; private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - virtual void setScale(const Ogre::Vector3&) + virtual void setScale(Ogre::Vector3 const&) { } virtual void setColor(float, float, float, float) { } - virtual const Ogre::Vector3& getPosition(); - virtual const Ogre::Quaternion& getOrientation(); + virtual Ogre::Vector3 const& getPosition(); + virtual Ogre::Quaternion const& getOrientation(); // Make MappedCovarianceProperty friend class so it create MappedCovarianceVisual objects friend class MappedCovarianceProperty; diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp b/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp index ed01eac0b..bc0cdefcc 100644 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp +++ b/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp @@ -73,16 +73,16 @@ class Pose2DStampedProperty : public rviz_common::properties::BoolProperty using Visual = Pose2DStampedVisual; using VisualPtr = std::shared_ptr; - Pose2DStampedProperty(const QString& name = "Pose2DStamped", bool default_value = true, - const QString& description = QString(), Property* parent = NULL, - const char* changed_slot = NULL, QObject* receiver = NULL); + Pose2DStampedProperty(QString const& name = "Pose2DStamped", bool default_value = true, + QString const& description = QString(), Property* parent = NULL, + char const* changed_slot = NULL, QObject* receiver = NULL); ~Pose2DStampedProperty() override = default; VisualPtr createAndInsertOrUpdateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation); - void eraseVisual(const fuse_core::UUID& uuid); + fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation); + void eraseVisual(fuse_core::UUID const& uuid); void clearVisual(); public Q_SLOTS: @@ -96,12 +96,12 @@ private Q_SLOTS: void updateTextScale(); private: - void updateAxesAlpha(const VisualPtr& constraint); - void updateScale(const VisualPtr& constraint); - void updateShowText(const VisualPtr& constraint); - void updateSphereColorAlpha(const VisualPtr& constraint); - void updateTextScale(const VisualPtr& constraint); - void updateVisibility(const VisualPtr& constraint); + void updateAxesAlpha(VisualPtr const& constraint); + void updateScale(VisualPtr const& constraint); + void updateShowText(VisualPtr const& constraint); + void updateSphereColorAlpha(VisualPtr const& constraint); + void updateTextScale(VisualPtr const& constraint); + void updateVisibility(VisualPtr const& constraint); std::unordered_map variables_; diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp index 67ed1bdbc..a148e13ad 100644 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp +++ b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp @@ -89,8 +89,8 @@ class Pose2DStampedVisual : public rviz_rendering::Object * @param[in] visible Initial visibility. */ Pose2DStampedVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation, const bool visible = true); + fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation, bool const visible = true); public: ~Pose2DStampedVisual() override; @@ -100,8 +100,8 @@ class Pose2DStampedVisual : public rviz_rendering::Object * @param[in] position 2D position stamped variable. * @param[in] orientation 2D orientation stamped variable. */ - void setPose2DStamped(const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation); + void setPose2DStamped(fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation); /** * @brief Get the root scene node of this variable visual. @@ -115,17 +115,17 @@ class Pose2DStampedVisual : public rviz_rendering::Object /** * @brief Sets user data on all ogre objects we own */ - void setUserData(const Ogre::Any& data) override; + void setUserData(Ogre::Any const& data) override; - void setSphereColor(const float r, const float g, const float b, const float a); + void setSphereColor(float const r, float const g, float const b, float const a); - void setAxesAlpha(const float alpha); + void setAxesAlpha(float const alpha); - void setScale(const Ogre::Vector3& scale) override; + void setScale(Ogre::Vector3 const& scale) override; - void setTextScale(const Ogre::Vector3& scale); + void setTextScale(Ogre::Vector3 const& scale); - void setTextVisible(const bool visible); + void setTextVisible(bool const visible); /** * @brief Sets visibility of this constraint @@ -137,15 +137,15 @@ class Pose2DStampedVisual : public rviz_rendering::Object /** * @brief Sets position of the frame this constraint is attached */ - void setPosition(const Ogre::Vector3& position) override; + void setPosition(Ogre::Vector3 const& position) override; /** * @brief Sets orientation of the frame this constraint is attached */ - void setOrientation(const Ogre::Quaternion& orientation) override; + void setOrientation(Ogre::Quaternion const& orientation) override; private: - void setPose2DStamped(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); + void setPose2DStamped(Ogre::Vector3 const& position, Ogre::Quaternion const& orientation); Ogre::SceneNode* root_node_ = nullptr; Ogre::SceneNode* sphere_node_ = nullptr; @@ -164,8 +164,8 @@ class Pose2DStampedVisual : public rviz_rendering::Object void setColor(float, float, float, float) override { } - const Ogre::Vector3& getPosition() override; - const Ogre::Quaternion& getOrientation() override; + Ogre::Vector3 const& getPosition() override; + Ogre::Quaternion const& getOrientation() override; // Make Pose2DStampedProperty friend class so it create Pose2DStampedVisual objects friend class Pose2DStampedProperty; diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp index 7bfb32ebd..095cc8ffc 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp @@ -90,20 +90,20 @@ class RelativePose2DStampedConstraintProperty : public BoolProperty using Visual = RelativePose2DStampedConstraintVisual; using VisualPtr = std::shared_ptr; - RelativePose2DStampedConstraintProperty(const QString& name = "RelativePose2DStampedConstraint", - bool default_value = true, const QString& description = QString(), - Property* parent = NULL, const char* changed_slot = NULL, + RelativePose2DStampedConstraintProperty(QString const& name = "RelativePose2DStampedConstraint", + bool default_value = true, QString const& description = QString(), + Property* parent = NULL, char const* changed_slot = NULL, QObject* receiver = NULL); ~RelativePose2DStampedConstraintProperty() override = default; VisualPtr createAndInsertVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_constraints::RelativePose2DStampedConstraint& constraint, - const fuse_core::Graph& graph); - void eraseVisual(const fuse_core::UUID& uuid); + fuse_constraints::RelativePose2DStampedConstraint const& constraint, + fuse_core::Graph const& graph); + void eraseVisual(fuse_core::UUID const& uuid); void clearVisual(); - void setColor(const QColor& color); + void setColor(QColor const& color); public Q_SLOTS: void updateVisibility(); @@ -121,17 +121,17 @@ private Q_SLOTS: void updateTextScale(); private: - void updateColor(const VisualPtr& constraint); - void updateErrorLineAlpha(const VisualPtr& constraint); - void updateErrorLineWidth(const VisualPtr& constraint); - void updateLossMinBrightness(const VisualPtr& constraint); - void updateRelativePoseAxesAlpha(const VisualPtr& constraint); - void updateRelativePoseAxesScale(const VisualPtr& constraint); - void updateRelativePoseLineAlpha(const VisualPtr& constraint); - void updateRelativePoseLineWidth(const VisualPtr& constraint); - void updateShowText(const VisualPtr& constraint); - void updateTextScale(const VisualPtr& constraint); - void updateVisibility(const VisualPtr& constraint); + void updateColor(VisualPtr const& constraint); + void updateErrorLineAlpha(VisualPtr const& constraint); + void updateErrorLineWidth(VisualPtr const& constraint); + void updateLossMinBrightness(VisualPtr const& constraint); + void updateRelativePoseAxesAlpha(VisualPtr const& constraint); + void updateRelativePoseAxesScale(VisualPtr const& constraint); + void updateRelativePoseLineAlpha(VisualPtr const& constraint); + void updateRelativePoseLineWidth(VisualPtr const& constraint); + void updateShowText(VisualPtr const& constraint); + void updateTextScale(VisualPtr const& constraint); + void updateVisibility(VisualPtr const& constraint); std::unordered_map constraints_; diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp index 59a38650c..503623145 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp @@ -109,8 +109,8 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object * @param[in] visible Initial visibility. */ RelativePose2DStampedConstraintVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_constraints::RelativePose2DStampedConstraint& constraint, - const bool visible = true); + fuse_constraints::RelativePose2DStampedConstraint const& constraint, + bool const visible = true); public: using CovarianceVisualPtr = MappedCovarianceProperty::MappedCovarianceVisualPtr; @@ -123,7 +123,7 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object * @param[in] graph fuse_core::Graph, used to retrieve the first/source and second/target * constraint variables pose. */ - void setConstraint(const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph); + void setConstraint(fuse_constraints::RelativePose2DStampedConstraint const& constraint, fuse_core::Graph const& graph); /** * @brief Get the root scene node of this constraint visual @@ -137,25 +137,25 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object /** * @brief Sets user data on all ogre objects we own */ - void setUserData(const Ogre::Any& data) override; + void setUserData(Ogre::Any const& data) override; - void setRelativePoseLineWidth(const float line_width); + void setRelativePoseLineWidth(float const line_width); - void setErrorLineWidth(const float line_width); + void setErrorLineWidth(float const line_width); - void setLossMinBrightness(const float min_brightness); + void setLossMinBrightness(float const min_brightness); - void setRelativePoseLineColor(const float r, const float g, const float b, const float a); + void setRelativePoseLineColor(float const r, float const g, float const b, float const a); - void setErrorLineColor(const float r, const float g, const float b, const float a); + void setErrorLineColor(float const r, float const g, float const b, float const a); - void setRelativePoseAxesAlpha(const float alpha); + void setRelativePoseAxesAlpha(float const alpha); - void setRelativePoseAxesScale(const Ogre::Vector3& scale); + void setRelativePoseAxesScale(Ogre::Vector3 const& scale); - void setTextScale(const Ogre::Vector3& scale); + void setTextScale(Ogre::Vector3 const& scale); - void setTextVisible(const bool visible); + void setTextVisible(bool const visible); /** * @brief Sets visibility of this constraint @@ -167,24 +167,24 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object /** * @brief Sets position of the frame this constraint is attached */ - void setPosition(const Ogre::Vector3& position) override; + void setPosition(Ogre::Vector3 const& position) override; /** * @brief Sets orientation of the frame this constraint is attached */ - void setOrientation(const Ogre::Quaternion& orientation) override; + void setOrientation(Ogre::Quaternion const& orientation) override; - const CovarianceVisualPtr& getCovariance() const + CovarianceVisualPtr const& getCovariance() const { return covariance_; } - void setCovariance(const CovarianceVisualPtr& covariance) + void setCovariance(CovarianceVisualPtr const& covariance) { covariance_ = covariance; } - const std::string& getSource() const + std::string const& getSource() const { return source_; } @@ -212,16 +212,16 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - void setScale(const Ogre::Vector3&) override + void setScale(Ogre::Vector3 const&) override { } void setColor(float, float, float, float) override { } - const Ogre::Vector3& getPosition() override; - const Ogre::Quaternion& getOrientation() override; + Ogre::Vector3 const& getPosition() override; + Ogre::Quaternion const& getOrientation() override; - Ogre::ColourValue computeLossErrorLineColor(const Ogre::ColourValue& color, const float loss_scale); + Ogre::ColourValue computeLossErrorLineColor(Ogre::ColourValue const& color, float const loss_scale); // Make RelativePose2DStampedConstraintProperty friend class so it create // RelativePose2DStampedConstraintVisual objects diff --git a/fuse_viz/include/fuse_viz/serialized_graph_display.hpp b/fuse_viz/include/fuse_viz/serialized_graph_display.hpp index bd8505a9b..282c91128 100644 --- a/fuse_viz/include/fuse_viz/serialized_graph_display.hpp +++ b/fuse_viz/include/fuse_viz/serialized_graph_display.hpp @@ -83,7 +83,7 @@ class SerializedGraphDisplay : public rviz_common::MessageFilterDisplaygetFloat(); float pos_scale = position_scale_property_->getFloat(); @@ -183,13 +183,13 @@ void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset(const Mapped void MappedCovarianceProperty::updateVisibility() { - for (const auto& entry : covariances_) + for (auto const& entry : covariances_) { updateVisibility(entry.second); } } -void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr& visual) +void MappedCovarianceProperty::updateVisibility(MappedCovarianceVisualPtr const& visual) { bool show_covariance = getBool(); if (!show_covariance) @@ -207,19 +207,19 @@ void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr& void MappedCovarianceProperty::updateOrientationFrame() { - for (const auto& entry : covariances_) + for (auto const& entry : covariances_) { updateOrientationFrame(entry.second); } } -void MappedCovarianceProperty::updateOrientationFrame(const MappedCovarianceVisualPtr& visual) +void MappedCovarianceProperty::updateOrientationFrame(MappedCovarianceVisualPtr const& visual) { bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local); visual->setRotatingFrame(use_rotating_frame); } -void MappedCovarianceProperty::eraseVisual(const std::string& key) +void MappedCovarianceProperty::eraseVisual(std::string const& key) { covariances_.erase(key); } @@ -235,7 +235,7 @@ size_t MappedCovarianceProperty::sizeVisual() } MappedCovarianceProperty::MappedCovarianceVisualPtr -MappedCovarianceProperty::createAndInsertVisual(const std::string& key, Ogre::SceneManager* scene_manager, +MappedCovarianceProperty::createAndInsertVisual(std::string const& key, Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) { bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local); diff --git a/fuse_viz/src/mapped_covariance_visual.cpp b/fuse_viz/src/mapped_covariance_visual.cpp index 27e19f9d9..d2b733b45 100644 --- a/fuse_viz/src/mapped_covariance_visual.cpp +++ b/fuse_viz/src/mapped_covariance_visual.cpp @@ -107,7 +107,7 @@ void makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues } } -void computeShapeScaleAndOrientation3D(const Eigen::Matrix3d& covariance, Ogre::Vector3& scale, +void computeShapeScaleAndOrientation3D(Eigen::Matrix3d const& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation) { Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity()); @@ -153,7 +153,7 @@ enum Plane XY_PLANE // normal is z-axis }; -void computeShapeScaleAndOrientation2D(const Eigen::Matrix2d& covariance, Ogre::Vector3& scale, +void computeShapeScaleAndOrientation2D(Eigen::Matrix2d const& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation, Plane plane = XY_PLANE) { Eigen::Vector2d eigenvalues(Eigen::Vector2d::Identity()); @@ -224,7 +224,7 @@ void radianScaleToMetricScaleBounded(Ogre::Real& radian_scale, float max_degrees } // namespace -const float MappedCovarianceVisual::max_degrees = 89.0; +float const MappedCovarianceVisual::max_degrees = 89.0; MappedCovarianceVisual::MappedCovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation, bool is_visible, float pos_scale, @@ -296,7 +296,7 @@ MappedCovarianceVisual::MappedCovarianceVisual(Ogre::SceneManager* scene_manager // from the pose origin, although it's only noticeable with big scales. // FIXME: Find the right value from the cone.mesh file, or implement a class that draws // something like a 2D "pie slice" and use it instead of the cone. - static const double cone_origin_to_top = 0.49115; + static double const cone_origin_to_top = 0.49115; orientation_offset_node_[kYaw2D]->setPosition(cone_origin_to_top * Ogre::Vector3::UNIT_X); orientation_offset_node_[kYaw2D]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Z)); @@ -324,7 +324,7 @@ MappedCovarianceVisual::~MappedCovarianceVisual() scene_manager_->destroySceneNode(root_node_); } -void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCovariance& pose) +void MappedCovarianceVisual::setCovariance(geometry_msgs::msg::PoseWithCovariance const& pose) { // check for NaN in covariance for (unsigned i = 0; i < 3; ++i) @@ -371,7 +371,7 @@ void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCov } } -void MappedCovarianceVisual::updatePosition(const Eigen::Matrix6d& covariance) +void MappedCovarianceVisual::updatePosition(Eigen::Matrix6d const& covariance) { // Compute shape and orientation for the position part of covariance Ogre::Vector3 shape_scale; @@ -398,7 +398,7 @@ void MappedCovarianceVisual::updatePosition(const Eigen::Matrix6d& covariance) } } -void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d& covariance, ShapeIndex index) +void MappedCovarianceVisual::updateOrientation(Eigen::Matrix6d const& covariance, ShapeIndex index) { Ogre::Vector3 shape_scale; Ogre::Quaternion shape_orientation; @@ -545,12 +545,12 @@ void MappedCovarianceVisual::setOrientationScale(float ori_scale) } } -void MappedCovarianceVisual::setPositionColor(const Ogre::ColourValue& c) +void MappedCovarianceVisual::setPositionColor(Ogre::ColourValue const& c) { position_shape_->setColor(c); } -void MappedCovarianceVisual::setOrientationColor(const Ogre::ColourValue& c) +void MappedCovarianceVisual::setOrientationColor(Ogre::ColourValue const& c) { for (int i = 0; i < kNumOriShapes; i++) { @@ -576,17 +576,17 @@ void MappedCovarianceVisual::setOrientationColor(float r, float g, float b, floa setOrientationColor(Ogre::ColourValue(r, g, b, a)); } -const Ogre::Vector3& MappedCovarianceVisual::getPositionCovarianceScale() +Ogre::Vector3 const& MappedCovarianceVisual::getPositionCovarianceScale() { return position_node_->getScale(); } -const Ogre::Quaternion& MappedCovarianceVisual::getPositionCovarianceOrientation() +Ogre::Quaternion const& MappedCovarianceVisual::getPositionCovarianceOrientation() { return position_node_->getOrientation(); } -void MappedCovarianceVisual::setUserData(const Ogre::Any& data) +void MappedCovarianceVisual::setUserData(Ogre::Any const& data) { position_shape_->setUserData(data); for (int i = 0; i < kNumOriShapes; i++) @@ -620,22 +620,22 @@ void MappedCovarianceVisual::updateOrientationVisibility() orientation_offset_node_[kYaw2D]->setVisible(orientation_visible_ && pose_2d_); } -const Ogre::Vector3& MappedCovarianceVisual::getPosition() +Ogre::Vector3 const& MappedCovarianceVisual::getPosition() { return position_node_->getPosition(); } -const Ogre::Quaternion& MappedCovarianceVisual::getOrientation() +Ogre::Quaternion const& MappedCovarianceVisual::getOrientation() { return position_node_->getOrientation(); } -void MappedCovarianceVisual::setPosition(const Ogre::Vector3& position) +void MappedCovarianceVisual::setPosition(Ogre::Vector3 const& position) { root_node_->setPosition(position); } -void MappedCovarianceVisual::setOrientation(const Ogre::Quaternion& orientation) +void MappedCovarianceVisual::setOrientation(Ogre::Quaternion const& orientation) { root_node_->setOrientation(orientation); } diff --git a/fuse_viz/src/pose_2d_stamped_property.cpp b/fuse_viz/src/pose_2d_stamped_property.cpp index 378c764ad..513170809 100644 --- a/fuse_viz/src/pose_2d_stamped_property.cpp +++ b/fuse_viz/src/pose_2d_stamped_property.cpp @@ -51,8 +51,8 @@ using rviz_common::properties::ColorProperty; using rviz_common::properties::FloatProperty; using rviz_common::properties::Property; -Pose2DStampedProperty::Pose2DStampedProperty(const QString& name, bool default_value, const QString& description, - Property* parent, const char* changed_slot, QObject* receiver) +Pose2DStampedProperty::Pose2DStampedProperty(QString const& name, bool default_value, QString const& description, + Property* parent, char const* changed_slot, QObject* receiver) // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of // this constructor : BoolProperty(name, default_value, description, parent) @@ -99,8 +99,8 @@ Pose2DStampedProperty::Pose2DStampedProperty(const QString& name, bool default_v Pose2DStampedProperty::VisualPtr Pose2DStampedProperty::createAndInsertOrUpdateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation) + fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation) { auto& visual = variables_[position.uuid()]; @@ -125,7 +125,7 @@ Pose2DStampedProperty::createAndInsertOrUpdateVisual(Ogre::SceneManager* scene_m return visual; } -void Pose2DStampedProperty::eraseVisual(const fuse_core::UUID& uuid) +void Pose2DStampedProperty::eraseVisual(fuse_core::UUID const& uuid) { variables_.erase(uuid); } @@ -183,36 +183,36 @@ void Pose2DStampedProperty::updateTextScale() } } -void Pose2DStampedProperty::updateAxesAlpha(const VisualPtr& variable) +void Pose2DStampedProperty::updateAxesAlpha(VisualPtr const& variable) { variable->setAxesAlpha(axes_alpha_property_->getFloat()); } -void Pose2DStampedProperty::updateScale(const VisualPtr& variable) +void Pose2DStampedProperty::updateScale(VisualPtr const& variable) { variable->setScale(Ogre::Vector3{ scale_property_->getFloat() }); // NOLINT(whitespace/braces) } -void Pose2DStampedProperty::updateShowText(const VisualPtr& variable) +void Pose2DStampedProperty::updateShowText(VisualPtr const& variable) { variable->setTextVisible(show_text_property_->getBool()); } -void Pose2DStampedProperty::updateSphereColorAlpha(const VisualPtr& variable) +void Pose2DStampedProperty::updateSphereColorAlpha(VisualPtr const& variable) { - const auto color = color_property_->getColor(); + auto const color = color_property_->getColor(); variable->setSphereColor(color.redF(), color.greenF(), color.blueF(), sphere_alpha_property_->getFloat()); } -void Pose2DStampedProperty::updateTextScale(const VisualPtr& variable) +void Pose2DStampedProperty::updateTextScale(VisualPtr const& variable) { variable->setTextScale(Ogre::Vector3{ text_scale_property_->getFloat() }); // NOLINT } -void Pose2DStampedProperty::updateVisibility(const VisualPtr& variable) +void Pose2DStampedProperty::updateVisibility(VisualPtr const& variable) { - const auto visible = getBool(); + auto const visible = getBool(); variable->setVisible(visible); variable->setTextVisible(visible && show_text_property_->getBool()); diff --git a/fuse_viz/src/pose_2d_stamped_visual.cpp b/fuse_viz/src/pose_2d_stamped_visual.cpp index f9e2c4f3b..4836e22c6 100644 --- a/fuse_viz/src/pose_2d_stamped_visual.cpp +++ b/fuse_viz/src/pose_2d_stamped_visual.cpp @@ -51,8 +51,8 @@ namespace fuse_viz using rviz_rendering::MovableText; Pose2DStampedVisual::Pose2DStampedVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation, const bool visible) + fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation, bool const visible) : Object(scene_manager), root_node_(parent_node->createChildSceneNode()), visible_(visible) { // Create sphere: @@ -65,7 +65,7 @@ Pose2DStampedVisual::Pose2DStampedVisual(Ogre::SceneManager* scene_manager, Ogre axes_ = std::make_shared(scene_manager_, axes_node_, 10.0, 1.0); // Create text: - const auto caption = position.type() + "::" + fuse_core::uuid::to_string(position.uuid()) + '\n' + + auto const caption = position.type() + "::" + fuse_core::uuid::to_string(position.uuid()) + '\n' + orientation.type() + "::" + fuse_core::uuid::to_string(orientation.uuid()); text_ = new MovableText(caption); text_->setCaption(caption); @@ -93,28 +93,28 @@ Pose2DStampedVisual::~Pose2DStampedVisual() scene_manager_->destroySceneNode(root_node_); } -void Pose2DStampedVisual::setPose2DStamped(const fuse_variables::Position2DStamped& position, - const fuse_variables::Orientation2DStamped& orientation) +void Pose2DStampedVisual::setPose2DStamped(fuse_variables::Position2DStamped const& position, + fuse_variables::Orientation2DStamped const& orientation) { setPose2DStamped(toOgre(position), toOgre(orientation)); } -void Pose2DStampedVisual::setUserData(const Ogre::Any& data) +void Pose2DStampedVisual::setUserData(Ogre::Any const& data) { axes_->setUserData(data); sphere_->setUserData(data); } -void Pose2DStampedVisual::setSphereColor(const float r, const float g, const float b, const float a) +void Pose2DStampedVisual::setSphereColor(float const r, float const g, float const b, float const a) { sphere_->setColor(r, g, b, a); } -void Pose2DStampedVisual::setAxesAlpha(const float alpha) +void Pose2DStampedVisual::setAxesAlpha(float const alpha) { - static const auto& default_x_color_ = axes_->getDefaultXColor(); - static const auto& default_y_color_ = axes_->getDefaultYColor(); - static const auto& default_z_color_ = axes_->getDefaultZColor(); + static auto const& default_x_color_ = axes_->getDefaultXColor(); + static auto const& default_y_color_ = axes_->getDefaultYColor(); + static auto const& default_z_color_ = axes_->getDefaultZColor(); axes_->setXColor(Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, alpha)); // NOLINT @@ -124,29 +124,29 @@ void Pose2DStampedVisual::setAxesAlpha(const float alpha) alpha)); // NOLINT } -void Pose2DStampedVisual::setScale(const Ogre::Vector3& scale) +void Pose2DStampedVisual::setScale(Ogre::Vector3 const& scale) { sphere_->setScale(scale); axes_->setScale(scale); } -void Pose2DStampedVisual::setTextScale(const Ogre::Vector3& scale) +void Pose2DStampedVisual::setTextScale(Ogre::Vector3 const& scale) { text_node_->setScale(scale); } -void Pose2DStampedVisual::setTextVisible(const bool visible) +void Pose2DStampedVisual::setTextVisible(bool const visible) { text_->setVisible(visible); } -void Pose2DStampedVisual::setVisible(const bool visible) +void Pose2DStampedVisual::setVisible(bool const visible) { sphere_node_->setVisible(visible); axes_node_->setVisible(visible); } -void Pose2DStampedVisual::setPose2DStamped(const Ogre::Vector3& position, const Ogre::Quaternion& orientation) +void Pose2DStampedVisual::setPose2DStamped(Ogre::Vector3 const& position, Ogre::Quaternion const& orientation) { axes_->setPosition(position); axes_->setOrientation(orientation); @@ -154,22 +154,22 @@ void Pose2DStampedVisual::setPose2DStamped(const Ogre::Vector3& position, const text_node_->setPosition(position); } -const Ogre::Vector3& Pose2DStampedVisual::getPosition() +Ogre::Vector3 const& Pose2DStampedVisual::getPosition() { return root_node_->getPosition(); } -const Ogre::Quaternion& Pose2DStampedVisual::getOrientation() +Ogre::Quaternion const& Pose2DStampedVisual::getOrientation() { return root_node_->getOrientation(); } -void Pose2DStampedVisual::setPosition(const Ogre::Vector3& position) +void Pose2DStampedVisual::setPosition(Ogre::Vector3 const& position) { root_node_->setPosition(position); } -void Pose2DStampedVisual::setOrientation(const Ogre::Quaternion& orientation) +void Pose2DStampedVisual::setOrientation(Ogre::Quaternion const& orientation) { root_node_->setOrientation(orientation); } diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp index 1816b5af2..cb194de4d 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp @@ -54,7 +54,7 @@ using rviz_common::properties::FloatProperty; using rviz_common::properties::Property; RelativePose2DStampedConstraintProperty::RelativePose2DStampedConstraintProperty( - const QString& name, bool default_value, const QString& description, Property* parent, const char* changed_slot, + QString const& name, bool default_value, QString const& description, Property* parent, char const* changed_slot, QObject* receiver) // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of // this constructor @@ -128,7 +128,7 @@ RelativePose2DStampedConstraintProperty::RelativePose2DStampedConstraintProperty RelativePose2DStampedConstraintProperty::VisualPtr RelativePose2DStampedConstraintProperty::createAndInsertVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph) + fuse_constraints::RelativePose2DStampedConstraint const& constraint, fuse_core::Graph const& graph) { VisualPtr visual{ new Visual(scene_manager, parent_node, constraint) }; constraints_[constraint.uuid()] = visual; @@ -152,7 +152,7 @@ RelativePose2DStampedConstraintProperty::VisualPtr RelativePose2DStampedConstrai return visual; } -void RelativePose2DStampedConstraintProperty::eraseVisual(const fuse_core::UUID& uuid) +void RelativePose2DStampedConstraintProperty::eraseVisual(fuse_core::UUID const& uuid) { covariance_property_->eraseVisual(fuse_core::uuid::to_string(uuid)); constraints_.erase(uuid); @@ -164,7 +164,7 @@ void RelativePose2DStampedConstraintProperty::clearVisual() constraints_.clear(); } -void RelativePose2DStampedConstraintProperty::setColor(const QColor& color) +void RelativePose2DStampedConstraintProperty::setColor(QColor const& color) { color_property_->setColor(color); } @@ -257,70 +257,70 @@ void RelativePose2DStampedConstraintProperty::updateTextScale() } } -void RelativePose2DStampedConstraintProperty::updateColor(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateColor(VisualPtr const& constraint) { - const auto color = color_property_->getColor(); + auto const color = color_property_->getColor(); constraint->setRelativePoseLineColor(color.redF(), color.greenF(), color.blueF(), relative_pose_line_alpha_property_->getFloat()); constraint->setErrorLineColor(color.redF(), color.greenF(), color.blueF(), error_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha(VisualPtr const& constraint) { - const auto color = color_property_->getColor(); + auto const color = color_property_->getColor(); constraint->setErrorLineColor(color.redF(), color.greenF(), color.blueF(), error_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateErrorLineWidth(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateErrorLineWidth(VisualPtr const& constraint) { constraint->setErrorLineWidth(error_line_width_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateLossMinBrightness(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateLossMinBrightness(VisualPtr const& constraint) { constraint->setLossMinBrightness(loss_min_brightness_property_->getFloat()); updateErrorLineAlpha(constraint); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha(VisualPtr const& constraint) { constraint->setRelativePoseAxesAlpha(relative_pose_axes_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale(VisualPtr const& constraint) { constraint->setRelativePoseAxesScale(Ogre::Vector3{ relative_pose_axes_scale_property_->getFloat() }); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha(VisualPtr const& constraint) { - const auto color = color_property_->getColor(); + auto const color = color_property_->getColor(); constraint->setRelativePoseLineColor(color.redF(), color.greenF(), color.blueF(), relative_pose_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth(VisualPtr const& constraint) { constraint->setRelativePoseLineWidth(relative_pose_line_width_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateShowText(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateShowText(VisualPtr const& constraint) { constraint->setTextVisible(show_text_property_->getBool()); } -void RelativePose2DStampedConstraintProperty::updateTextScale(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateTextScale(VisualPtr const& constraint) { constraint->setTextScale(Ogre::Vector3{ text_scale_property_->getFloat() }); // NOLINT } -void RelativePose2DStampedConstraintProperty::updateVisibility(const VisualPtr& constraint) +void RelativePose2DStampedConstraintProperty::updateVisibility(VisualPtr const& constraint) { - const auto visible = getBool(); + auto const visible = getBool(); constraint->setVisible(visible); constraint->setTextVisible(visible && show_text_property_->getBool()); diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp index 9beb1b6cf..401015b1f 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp @@ -66,14 +66,14 @@ namespace fuse_viz * @param[in] constraint A constraint * @return The constraint name of the form: source@type::uuid */ -std::string constraint_name(const fuse_constraints::RelativePose2DStampedConstraint& constraint) +std::string constraint_name(fuse_constraints::RelativePose2DStampedConstraint const& constraint) { return constraint.source() + '@' + constraint.type() + "::" + fuse_core::uuid::to_string(constraint.uuid()); } RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, - const fuse_constraints::RelativePose2DStampedConstraint& constraint, const bool visible) + fuse_constraints::RelativePose2DStampedConstraint const& constraint, bool const visible) : Object(scene_manager) , root_node_(parent_node->createChildSceneNode()) , source_(constraint.source()) @@ -96,7 +96,7 @@ RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual( relative_pose_axes_ = std::make_shared(scene_manager_, relative_pose_axes_node_, 10.0, 1.0); // Draw text: - const auto caption = constraint_name(constraint); + auto const caption = constraint_name(constraint); text_ = new MovableText(caption); text_->setCaption(caption); text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_ABOVE); @@ -122,19 +122,19 @@ RelativePose2DStampedConstraintVisual::~RelativePose2DStampedConstraintVisual() } void RelativePose2DStampedConstraintVisual::setConstraint( - const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph) + fuse_constraints::RelativePose2DStampedConstraint const& constraint, fuse_core::Graph const& graph) { // Update constraint relative pose line: - const auto& variables = constraint.variables(); + auto const& variables = constraint.variables(); - const auto pose1 = getPose(graph, variables.at(0), variables.at(1)); + auto const pose1 = getPose(graph, variables.at(0), variables.at(1)); - const auto& delta = constraint.delta(); + auto const& delta = constraint.delta(); const tf2::Transform pose_delta{ tf2::Quaternion{ tf2::Vector3{ 0, 0, 1 }, delta[2] }, tf2::Vector3{ delta[0], delta[1], 0 } }; - const auto absolute_pose = pose1 * pose_delta; + auto const absolute_pose = pose1 * pose_delta; - const auto absolute_position_ogre = toOgre(absolute_pose.getOrigin()); + auto const absolute_position_ogre = toOgre(absolute_pose.getOrigin()); relative_pose_line_->clear(); relative_pose_line_->addPoint(toOgre(pose1.getOrigin())); @@ -150,7 +150,7 @@ void RelativePose2DStampedConstraintVisual::setConstraint( covariance_->setCovariance(relative_pose_msg); // Update constraint error line: - const auto pose2 = getPose(graph, variables.at(2), variables.at(3)); + auto const pose2 = getPose(graph, variables.at(2), variables.at(3)); error_line_->clear(); error_line_->addPoint(absolute_position_ogre); @@ -161,12 +161,12 @@ void RelativePose2DStampedConstraintVisual::setConstraint( if (loss_function) { // Evaluate cost function without loss: - const double position1[] = { pose1.getOrigin().getX(), pose1.getOrigin().getY() }; - const double yaw1[] = { tf2::getYaw(pose1.getRotation()) }; - const double position2[] = { pose2.getOrigin().getX(), pose2.getOrigin().getY() }; - const double yaw2[] = { tf2::getYaw(pose2.getRotation()) }; + double const position1[] = { pose1.getOrigin().getX(), pose1.getOrigin().getY() }; + double const yaw1[] = { tf2::getYaw(pose1.getRotation()) }; + double const position2[] = { pose2.getOrigin().getX(), pose2.getOrigin().getY() }; + double const yaw2[] = { tf2::getYaw(pose2.getRotation()) }; - const double* parameters[] = { position1, yaw1, position2, yaw2 }; + double const* parameters[] = { position1, yaw1, position2, yaw2 }; auto cost_function = constraint.costFunction(); @@ -181,7 +181,7 @@ void RelativePose2DStampedConstraintVisual::setConstraint( // // See https://github.com/ceres-solver/ceres- // solver/blob/master/internal/ceres/residual_block.cc#L159 - const auto squared_norm = residuals.squaredNorm(); + auto const squared_norm = residuals.squaredNorm(); // Evaluate the loss as in: // https://github.com/ceres-solver/ceres- @@ -221,7 +221,7 @@ void RelativePose2DStampedConstraintVisual::setConstraint( loss_scale_ = squared_norm == 0.0 ? 1.0 : rho[0] / squared_norm; // Compute error line color with the loss function impact: - const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); + auto const loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); error_line_->setColor(loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, error_line_color_.a); } @@ -234,7 +234,7 @@ void RelativePose2DStampedConstraintVisual::setConstraint( text_node_->setPosition(absolute_position_ogre); } -void RelativePose2DStampedConstraintVisual::setUserData(const Ogre::Any& data) +void RelativePose2DStampedConstraintVisual::setUserData(Ogre::Any const& data) { relative_pose_line_->setUserData(data); error_line_->setUserData(data); @@ -242,28 +242,28 @@ void RelativePose2DStampedConstraintVisual::setUserData(const Ogre::Any& data) covariance_->setUserData(data); } -void RelativePose2DStampedConstraintVisual::setRelativePoseLineWidth(const float line_width) +void RelativePose2DStampedConstraintVisual::setRelativePoseLineWidth(float const line_width) { relative_pose_line_->setLineWidth(line_width); } -void RelativePose2DStampedConstraintVisual::setErrorLineWidth(const float line_width) +void RelativePose2DStampedConstraintVisual::setErrorLineWidth(float const line_width) { error_line_->setLineWidth(line_width); } -void RelativePose2DStampedConstraintVisual::setLossMinBrightness(const float min_brightness) +void RelativePose2DStampedConstraintVisual::setLossMinBrightness(float const min_brightness) { min_brightness_ = min_brightness; } -void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor(const float r, const float g, const float b, - const float a) +void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor(float const r, float const g, float const b, + float const a) { relative_pose_line_->setColor(r, g, b, a); } -void RelativePose2DStampedConstraintVisual::setErrorLineColor(const float r, const float g, const float b, const float a) +void RelativePose2DStampedConstraintVisual::setErrorLineColor(float const r, float const g, float const b, float const a) { // Cache error line color w/o the loss function impact, so we can change its darkness based on the // loss function impact on the constraint cost: Note that we cannot recover/retrieve the color @@ -274,16 +274,16 @@ void RelativePose2DStampedConstraintVisual::setErrorLineColor(const float r, con error_line_color_.a = a; // Compute error line color with the impact of the loss function, in case the constraint has one: - const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); + auto const loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); error_line_->setColor(loss_error_line_color.r, loss_error_line_color.r, loss_error_line_color.b, loss_error_line_color.a); } -void RelativePose2DStampedConstraintVisual::setRelativePoseAxesAlpha(const float alpha) +void RelativePose2DStampedConstraintVisual::setRelativePoseAxesAlpha(float const alpha) { - static const auto& default_x_color_ = relative_pose_axes_->getDefaultXColor(); - static const auto& default_y_color_ = relative_pose_axes_->getDefaultYColor(); - static const auto& default_z_color_ = relative_pose_axes_->getDefaultZColor(); + static auto const& default_x_color_ = relative_pose_axes_->getDefaultXColor(); + static auto const& default_y_color_ = relative_pose_axes_->getDefaultYColor(); + static auto const& default_z_color_ = relative_pose_axes_->getDefaultZColor(); relative_pose_axes_->setXColor( Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, alpha)); // NOLINT @@ -293,50 +293,50 @@ void RelativePose2DStampedConstraintVisual::setRelativePoseAxesAlpha(const float Ogre::ColourValue(default_z_color_.r, default_z_color_.g, default_z_color_.b, alpha)); // NOLINT } -void RelativePose2DStampedConstraintVisual::setRelativePoseAxesScale(const Ogre::Vector3& scale) +void RelativePose2DStampedConstraintVisual::setRelativePoseAxesScale(Ogre::Vector3 const& scale) { relative_pose_axes_->setScale(scale); } -void RelativePose2DStampedConstraintVisual::setTextScale(const Ogre::Vector3& scale) +void RelativePose2DStampedConstraintVisual::setTextScale(Ogre::Vector3 const& scale) { text_node_->setScale(scale); } -void RelativePose2DStampedConstraintVisual::setTextVisible(const bool visible) +void RelativePose2DStampedConstraintVisual::setTextVisible(bool const visible) { text_->setVisible(visible); } -void RelativePose2DStampedConstraintVisual::setVisible(const bool visible) +void RelativePose2DStampedConstraintVisual::setVisible(bool const visible) { relative_pose_line_node_->setVisible(visible); error_line_node_->setVisible(visible); relative_pose_axes_node_->setVisible(visible); } -const Ogre::Vector3& RelativePose2DStampedConstraintVisual::getPosition() +Ogre::Vector3 const& RelativePose2DStampedConstraintVisual::getPosition() { return root_node_->getPosition(); } -const Ogre::Quaternion& RelativePose2DStampedConstraintVisual::getOrientation() +Ogre::Quaternion const& RelativePose2DStampedConstraintVisual::getOrientation() { return root_node_->getOrientation(); } -void RelativePose2DStampedConstraintVisual::setPosition(const Ogre::Vector3& position) +void RelativePose2DStampedConstraintVisual::setPosition(Ogre::Vector3 const& position) { root_node_->setPosition(position); } -void RelativePose2DStampedConstraintVisual::setOrientation(const Ogre::Quaternion& orientation) +void RelativePose2DStampedConstraintVisual::setOrientation(Ogre::Quaternion const& orientation) { root_node_->setOrientation(orientation); } -Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineColor(const Ogre::ColourValue& color, - const float loss_scale) +Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineColor(Ogre::ColourValue const& color, + float const loss_scale) { // Skip if the loss scale is negative, which means the constraint has no loss: if (loss_scale < 0.0) @@ -354,10 +354,10 @@ Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineCol // // However, we cannot do this because it changes the color of the error line, which should be // consistent for all constraints visuals. Instead, we clamp the minimum brightness: - const auto min_brightness = std::min(min_brightness_, brightness); + auto const min_brightness = std::min(min_brightness_, brightness); // Scale brightness by the loss scale within the [min_brightness, 1] range: - const auto loss_brightness = min_brightness + (brightness - min_brightness) * loss_scale; + auto const loss_brightness = min_brightness + (brightness - min_brightness) * loss_scale; // Set error line color with the loss brightness: Ogre::ColourValue loss_error_line_color; diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp index 4f6cac411..01fdaa429 100644 --- a/fuse_viz/src/serialized_graph_display.cpp +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -118,14 +118,14 @@ void SerializedGraphDisplay::onDisable() root_node_->setVisible(false); } -void SerializedGraphDisplay::load(const rviz_common::Config& config) +void SerializedGraphDisplay::load(rviz_common::Config const& config) { MFDClass::load(config); // Cache constraint config for each source in order to apply it when the // RelativePose2DStampedConstraintProperty is created the first time a constraint of each source // is present in the graph: - const auto constraints_config = config.mapGetChild("Constraints"); + auto const constraints_config = config.mapGetChild("Constraints"); for (rviz_common::Config::MapIterator iter = constraints_config.mapIterator(); iter.isValid(); iter.advance()) { @@ -140,7 +140,7 @@ void SerializedGraphDisplay::updateShowVariables() void SerializedGraphDisplay::updateShowConstraints() { - const auto visible = show_constraints_property_->getBool(); + auto const visible = show_constraints_property_->getBool(); for (auto& entry : constraint_source_properties_) { @@ -188,39 +188,39 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con entry.second = false; } - const auto graph = graph_deserializer_.deserialize(msg); + auto const graph = graph_deserializer_.deserialize(msg); - for (const auto& variable : graph->getVariables()) + for (auto const& variable : graph->getVariables()) { - const auto orientation = dynamic_cast(&variable); + auto const orientation = dynamic_cast(&variable); if (!orientation) { continue; } - const auto position_uuid = fuse_variables::Position2DStamped(orientation->stamp(), orientation->deviceId()).uuid(); + auto const position_uuid = fuse_variables::Position2DStamped(orientation->stamp(), orientation->deviceId()).uuid(); if (!graph->variableExists(position_uuid)) { continue; } - const auto position = dynamic_cast(&graph->getVariable(position_uuid)); + auto const position = dynamic_cast(&graph->getVariable(position_uuid)); variable_property_->createAndInsertOrUpdateVisual(scene_manager_, root_node_, *position, *orientation); variables_changed_map_[position_uuid] = true; } - for (const auto& constraint : graph->getConstraints()) + for (auto const& constraint : graph->getConstraints()) { - const auto relative_pose = dynamic_cast(&constraint); + auto const relative_pose = dynamic_cast(&constraint); if (!relative_pose) { continue; } - const auto constraint_uuid = constraint.uuid(); - const auto& constraint_source = constraint.source(); + auto const constraint_uuid = constraint.uuid(); + auto const& constraint_source = constraint.source(); if (source_color_map_.find(constraint_source) == source_color_map_.end()) { @@ -229,23 +229,23 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con // the spectrum. This is achieved by traversing a virtual complete binary tree in breadth- // first order. Each node represents a sampling position in the hue interval (0, 1) based on // the current level and the number of nodes in that level (m) - const auto n = source_color_map_.size() + 1; + auto const n = source_color_map_.size() + 1; const size_t level = std::floor(std::log2(n)); - const auto m = n + 1 - std::pow(2, level); - const auto hue = (2 * (m - 1) + 1) / std::pow(2, level + 1); + auto const m = n + 1 - std::pow(2, level); + auto const hue = (2 * (m - 1) + 1) / std::pow(2, level + 1); auto& source_color = source_color_map_[constraint_source]; source_color.setHSB(hue, 1.0, 1.0); // Insert constraint sorted alphabetically: - const auto description = constraint_source + ' ' + constraint.type() + " constraint."; + auto const description = constraint_source + ' ' + constraint.type() + " constraint."; - const auto constraint_source_property = + auto const constraint_source_property = new RelativePose2DStampedConstraintProperty(QString::fromStdString(constraint_source), true, QString::fromStdString(description), nullptr, SLOT(queueRender()), this); - const auto result = constraint_source_properties_.insert( + auto const result = constraint_source_properties_.insert( { constraint_source, constraint_source_property }); // NOLINT(whitespace/braces) if (!result.second) @@ -281,7 +281,7 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con constraints_changed_map_[constraint_uuid] = true; } - for (const auto& entry : variables_changed_map_) + for (auto const& entry : variables_changed_map_) { if (!entry.second) { @@ -301,7 +301,7 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con } } - for (const auto& entry : constraints_changed_map_) + for (auto const& entry : constraints_changed_map_) { if (!entry.second) { diff --git a/run_clang_tidy.bash b/run_clang_tidy.bash index 9fb7598e8..cc8522031 100755 --- a/run_clang_tidy.bash +++ b/run_clang_tidy.bash @@ -1,4 +1,4 @@ #!/bin/bash # -j $(nproc --all) runs with all cores, but the prepended nice runs with a low priority so it won't make your computer unusable while clang tidy is going. The "$@" at the end passes all the filenames from pre-commit so it should only look for clang tidy fixes in the files you directly changed in the commit that is being checked. -nice run-clang-tidy -p ../../build_dbg -j $(nproc --all) -quiet -fix "$@" +nice run-clang-tidy -p ../../build_dbg -j $(nproc --all) -quiet -fix -extra-arg=-std=c++17 "$@"