diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h index 7470434e..dd6e7b3e 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h @@ -91,7 +91,7 @@ class NormalDeltaOrientation3DEulerCostFunctor NormalDeltaOrientation3DEulerCostFunctor( const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b, - const std::vector &axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}); + const std::vector &axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}); // NOLINT(whitespace/braces) /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. diff --git a/fuse_models/include/fuse_models/common/sensor_config.h b/fuse_models/include/fuse_models/common/sensor_config.h index 0ead8ad6..1702cf2d 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.h +++ b/fuse_models/include/fuse_models/common/sensor_config.h @@ -237,7 +237,7 @@ std::vector toEulerEnumVector(const euler_indices.reserve(indices.size()); std::transform(indices.begin(), indices.end(), std::back_inserter(euler_indices), - [](int value) { return static_cast(value + 4U); }); + [](int value) { return static_cast(value + 4U); }); // NOLINT(whitespace/braces) return euler_indices; } diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.h b/fuse_models/include/fuse_models/unicycle_3d_predict.h index aabf2e5a..a59edbd3 100644 --- a/fuse_models/include/fuse_models/unicycle_3d_predict.h +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.h @@ -666,13 +666,16 @@ inline void predict( acc_linear_z_pred, jacobians.data()); - pose2.setOrigin(tf2::Vector3{x_pred, y_pred, z_pred}); + pose2.setOrigin(tf2::Vector3{x_pred, y_pred, z_pred}); // NOLINT(whitespace/braces) Eigen::Quaterniond orientation2 = Eigen::AngleAxisd(yaw_pred, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch_pred, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll_pred, Eigen::Vector3d::UnitX()); - pose2.setRotation({orientation2.x(), orientation2.y(), orientation2.z(), orientation2.w()}); + pose2.setRotation({orientation2.x(), // NOLINT(whitespace/braces) + orientation2.y(), + orientation2.z(), + orientation2.w()}); // NOLINT(whitespace/braces) vel_linear2.setX(vel_linear_x_pred); vel_linear2.setY(vel_linear_y_pred); @@ -776,7 +779,7 @@ inline void predict( acc_linear_y_pred, acc_linear_z_pred); - pose2.setOrigin(tf2::Vector3{x_pred, y_pred, z_pred}); + pose2.setOrigin(tf2::Vector3{x_pred, y_pred, z_pred}); // NOLINT(whitespace/braces) Eigen::Quaterniond orientation_pred_q = Eigen::AngleAxisd(yaw_pred, Eigen::Vector3d::UnitZ()) * diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp index bddabe6f..b7e5806b 100644 --- a/fuse_models/test/test_unicycle_3d_predict.cpp +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -162,8 +162,8 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // Out of plane motion - pose1.setOrigin({0.0, 0.0, 0.0}); - pose1.setRotation({ 0.0, 0.0, 0.0, 1.0}); + pose1.setOrigin({0.0, 0.0, 0.0}); // NOLINT(whitespace/braces) + pose1.setRotation({ 0.0, 0.0, 0.0, 1.0}); // NOLINT(whitespace/braces) vel_linear1 = {0.0, 0.0, 0.1}; vel_angular1 = {1.570796327, 0.0, 0.0}; acc_linear1 = {0.0, 0.0, 1.0}; @@ -199,8 +199,8 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); // General 3D motion (these value are checked against rl predict() equations) - pose1.setOrigin({0.0, 0.0, 0.0}); - pose1.setRotation({-0.003, -0.943, 0.314, 0.110}); // RPY {-2.490, -0.206, 3.066} + pose1.setOrigin({0.0, 0.0, 0.0}); // NOLINT(whitespace/braces) + pose1.setRotation({-0.003, -0.943, 0.314, 0.110}); // RPY {-2.490, -0.206, 3.066} NOLINT(whitespace/braces) vel_linear1 = {0.1, 0.2, 0.1}; vel_angular1 = {1.570796327, 1.570796327, -1.570796327}; acc_linear1 = {-0.5, 1.0, 1.0};