diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp index e7f98b0ca..21f462111 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -107,44 +107,29 @@ struct kalman, //! specialized cases? Same question applies to other parameters. //! @todo Pass the arguments by universal reference? observation_state_function observation_state_h{ - [&h = h](const state &state_x, - const UpdateTypes &...update_pack) -> output_model { - static_cast(state_x); - (static_cast(update_pack), ...); - return h; - }}; + [&h = h]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const UpdateTypes &...update_pack) + -> output_model { return h; }}; noise_observation_function noise_observation_r{ - [&r = r](const state &state_x, const output &output_z, - const UpdateTypes &...update_pack) -> output_uncertainty { - static_cast(state_x); - static_cast(output_z); - (static_cast(update_pack), ...); - return r; - }}; + [&r = r]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const output &output_z, + [[maybe_unused]] const UpdateTypes &...update_pack) + -> output_uncertainty { return r; }}; transition_state_function transition_state_f{ - [&f = f](const state &state_x, - const PredictionTypes &...prediction_pack) -> state_transition { - static_cast(state_x); - (static_cast(prediction_pack), ...); - return f; - }}; + [&f = f]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const PredictionTypes &...prediction_pack) + -> state_transition { return f; }}; noise_process_function noise_process_q{ - [&q = q](const state &state_x, const PredictionTypes &...prediction_pack) - -> process_uncertainty { - static_cast(state_x); - (static_cast(prediction_pack), ...); - return q; - }}; + [&q = q]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const PredictionTypes &...prediction_pack) + -> process_uncertainty { return q; }}; transition_function transition{ [&f = f](const state &state_x, - const PredictionTypes &...prediction_pack) -> state { - (static_cast(prediction_pack), ...); - return f * state_x; - }}; + [[maybe_unused]] const PredictionTypes &...prediction_pack) + -> state { return f * state_x; }}; observation_function observation{ [&h = h](const state &state_x, - const UpdateTypes &...update_pack) -> output { - (static_cast(update_pack), ...); + [[maybe_unused]] const UpdateTypes &...update_pack) -> output { return h * state_x; }}; @@ -238,50 +223,35 @@ struct kalman, //! specialized cases? Same question applies to other parameters. //! @todo Pass the arguments by universal reference? observation_state_function observation_state_h{ - [&h = h](const state &state_x, - const UpdateTypes &...update_pack) -> output_model { - static_cast(state_x); - (static_cast(update_pack), ...); - return h; - }}; + [&h = h]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const UpdateTypes &...update_pack) + -> output_model { return h; }}; noise_observation_function noise_observation_r{ - [&r = r](const state &state_x, const output &output_z, - const UpdateTypes &...update_pack) -> output_uncertainty { - static_cast(state_x); - static_cast(output_z); - (static_cast(update_pack), ...); - return r; - }}; + [&r = r]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const output &output_z, + [[maybe_unused]] const UpdateTypes &...update_pack) + -> output_uncertainty { return r; }}; transition_state_function transition_state_f{ - [&f = f](const state &state_x, const input &input_u, - const PredictionTypes &...prediction_pack) -> state_transition { - static_cast(state_x); - static_cast(input_u); - (static_cast(prediction_pack), ...); - return f; - }}; + [&f = f]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const input &input_u, + [[maybe_unused]] const PredictionTypes &...prediction_pack) + -> state_transition { return f; }}; noise_process_function noise_process_q{ - [&q = q](const state &state_x, const PredictionTypes &...prediction_pack) - -> process_uncertainty { - static_cast(state_x); - (static_cast(prediction_pack), ...); - return q; - }}; + [&q = q]([[maybe_unused]] const state &state_x, + [[maybe_unused]] const PredictionTypes &...prediction_pack) + -> process_uncertainty { return q; }}; transition_control_function transition_control_g{ - [&g = g](const PredictionTypes &...prediction_pack) -> input_control { - (static_cast(prediction_pack), ...); - return g; - }}; + [&g = g]([[maybe_unused]] const PredictionTypes &...prediction_pack) + -> input_control { return g; }}; transition_function transition{ - [&f = f, &g = g](const state &state_x, const input &input_u, - const PredictionTypes &...prediction_pack) -> state { - (static_cast(prediction_pack), ...); + [&f = f, &g = g]( + const state &state_x, const input &input_u, + [[maybe_unused]] const PredictionTypes &...prediction_pack) -> state { return f * state_x + g * input_u; }}; observation_function observation{ [&h = h](const state &state_x, - const UpdateTypes &...update_pack) -> output { - (static_cast(update_pack), ...); + [[maybe_unused]] const UpdateTypes &...update_pack) -> output { return h * state_x; }}; diff --git a/include/fcarouge/internal/utility.hpp b/include/fcarouge/internal/utility.hpp index 79f0e3948..ab8d009a0 100644 --- a/include/fcarouge/internal/utility.hpp +++ b/include/fcarouge/internal/utility.hpp @@ -48,9 +48,8 @@ template concept arithmetic = std::integral || std::floating_point; struct empty { - inline constexpr explicit empty(auto &&...any) noexcept { + inline constexpr explicit empty([[maybe_unused]] auto &&...any) noexcept { // Constructs from anything for all initializations compatibility. - (static_cast(any), ...); } }; diff --git a/sample/kf_2x1x1_rocket_altitude.cpp b/sample/kf_2x1x1_rocket_altitude.cpp index 1c36f469b..5c066f94b 100644 --- a/sample/kf_2x1x1_rocket_altitude.cpp +++ b/sample/kf_2x1x1_rocket_altitude.cpp @@ -69,20 +69,18 @@ template using vector = Eigen::Vector; // the system random acceleration. The accelerometer error v is much lower // than system's random acceleration, therefore we use ϵ^2 as a multiplier of // the process noise matrix. This makes our estimation uncertainty much lower! - filter.q( - [](const kalman::state &x, const std::chrono::milliseconds &delta_time) { - static_cast(x); - const auto dt{std::chrono::duration(delta_time).count()}; - return kalman::process_uncertainty{ - {0.1 * 0.1 * dt * dt * dt * dt / 4, 0.1 * 0.1 * dt * dt * dt / 2}, - {0.1 * 0.1 * dt * dt * dt / 2, 0.1 * 0.1 * dt * dt}}; - }); + filter.q([]([[maybe_unused]] const kalman::state &x, + const std::chrono::milliseconds &delta_time) { + const auto dt{std::chrono::duration(delta_time).count()}; + return kalman::process_uncertainty{ + {0.1 * 0.1 * dt * dt * dt * dt / 4, 0.1 * 0.1 * dt * dt * dt / 2}, + {0.1 * 0.1 * dt * dt * dt / 2, 0.1 * 0.1 * dt * dt}}; + }); // The state transition matrix F would be: - filter.f([](const kalman::state &x, const kalman::input &u, + filter.f([]([[maybe_unused]] const kalman::state &x, + [[maybe_unused]] const kalman::input &u, const std::chrono::milliseconds &delta_time) { - static_cast(x); - static_cast(u); const auto dt{std::chrono::duration(delta_time).count()}; return kalman::state_transition{{1, dt}, {0, 1}}; }); diff --git a/sample/kf_8x4x0_deep_sort_bounding_box.cpp b/sample/kf_8x4x0_deep_sort_bounding_box.cpp index 5b1f0d127..3a643f85d 100644 --- a/sample/kf_8x4x0_deep_sort_bounding_box.cpp +++ b/sample/kf_8x4x0_deep_sort_bounding_box.cpp @@ -215,17 +215,16 @@ struct divide final { {0, 0, 0, 1, 0, 0, 0, 0}}); // Observation, measurement noise covariance. - filter.r( - [position_weight](const kalman::state &x, - const kalman::output &z) -> kalman::output_uncertainty { - static_cast(z); - return vector{position_weight * x(3), position_weight * x(3), - 1e-1f, position_weight * x(3)} - .array() - .square() - .matrix() - .asDiagonal(); - }); + filter.r([position_weight](const kalman::state &x, + [[maybe_unused]] const kalman::output &z) + -> kalman::output_uncertainty { + return vector{position_weight * x(3), position_weight * x(3), + 1e-1f, position_weight * x(3)} + .array() + .square() + .matrix() + .asDiagonal(); + }); // And so on, run a step of the filter, updating and predicting, every frame. for (const auto &output : measured) { diff --git a/test/eigen_f.cpp b/test/eigen_f.cpp index d032b3e16..18c928a3f 100644 --- a/test/eigen_f.cpp +++ b/test/eigen_f.cpp @@ -107,16 +107,13 @@ struct divide final { } { - const auto f{[](const kalman::state &x, const kalman::input &u, - const int &i, const float &fp, - const double &d) -> kalman::state_transition { - static_cast(x); - static_cast(d); - static_cast(fp); - static_cast(i); - static_cast(u); - return matrix::Identity(); - }}; + const auto f{ + []([[maybe_unused]] const kalman::state &x, + [[maybe_unused]] const kalman::input &u, + [[maybe_unused]] const int &i, [[maybe_unused]] const float &fp, + [[maybe_unused]] const double &d) -> kalman::state_transition { + return matrix::Identity(); + }}; filter.f(f); assert(filter.f() == z5x5); filter.predict(0, 0.f, 0., z3); @@ -124,16 +121,13 @@ struct divide final { } { - const auto f{[](const kalman::state &x, const kalman::input &u, - const int &i, const float &fp, - const double &d) -> kalman::state_transition { - static_cast(x); - static_cast(d); - static_cast(fp); - static_cast(i); - static_cast(u); - return matrix::Zero(); - }}; + const auto f{ + []([[maybe_unused]] const kalman::state &x, + [[maybe_unused]] const kalman::input &u, + [[maybe_unused]] const int &i, [[maybe_unused]] const float &fp, + [[maybe_unused]] const double &d) -> kalman::state_transition { + return matrix::Zero(); + }}; filter.f(std::move(f)); assert(filter.f() == i5x5); filter.predict(0, 0.f, 0., z3); diff --git a/test/eigen_h.cpp b/test/eigen_h.cpp index 3d4c9fbb5..a0e7b73ea 100644 --- a/test/eigen_h.cpp +++ b/test/eigen_h.cpp @@ -106,12 +106,10 @@ struct divide final { } { - const auto h{[](const kalman::state &x, const double &d, const float &f, - const int &i) -> kalman::output_model { - static_cast(x); - static_cast(d); - static_cast(f); - static_cast(i); + const auto h{[]([[maybe_unused]] const kalman::state &x, + [[maybe_unused]] const double &d, + [[maybe_unused]] const float &f, + [[maybe_unused]] const int &i) -> kalman::output_model { return matrix::Identity(); }}; filter.h(h); @@ -121,12 +119,10 @@ struct divide final { } { - const auto h{[](const kalman::state &x, const double &d, const float &f, - const int &i) -> kalman::output_model { - static_cast(x); - static_cast(d); - static_cast(f); - static_cast(i); + const auto h{[]([[maybe_unused]] const kalman::state &x, + [[maybe_unused]] const double &d, + [[maybe_unused]] const float &f, + [[maybe_unused]] const int &i) -> kalman::output_model { return matrix::Zero(); }}; filter.h(std::move(h)); diff --git a/test/f.cpp b/test/f.cpp index 89cc13f4d..7be99c08c 100644 --- a/test/f.cpp +++ b/test/f.cpp @@ -75,10 +75,8 @@ namespace { } { - const auto f{[](const kalman::state &x) -> kalman::state_transition { - static_cast(x); - return 6.; - }}; + const auto f{[]([[maybe_unused]] const kalman::state &x) + -> kalman::state_transition { return 6.; }}; filter.f(f); assert(filter.f() == 5); filter.predict(); @@ -86,10 +84,8 @@ namespace { } { - const auto f{[](const kalman::state &x) -> kalman::state_transition { - static_cast(x); - return 7.; - }}; + const auto f{[]([[maybe_unused]] const kalman::state &x) + -> kalman::state_transition { return 7.; }}; filter.f(std::move(f)); assert(filter.f() == 6); filter.predict(); diff --git a/test/h.cpp b/test/h.cpp index 7538f57a4..90d936a17 100644 --- a/test/h.cpp +++ b/test/h.cpp @@ -75,10 +75,10 @@ namespace { } { - const auto h{[](const kalman::state &x) -> kalman::output_model { - static_cast(x); - return 6.; - }}; + const auto h{ + []([[maybe_unused]] const kalman::state &x) -> kalman::output_model { + return 6.; + }}; filter.h(h); assert(filter.h() == 5); filter.update(0.); @@ -86,10 +86,10 @@ namespace { } { - const auto h{[](const kalman::state &x) -> kalman::output_model { - static_cast(x); - return 7.; - }}; + const auto h{ + []([[maybe_unused]] const kalman::state &x) -> kalman::output_model { + return 7.; + }}; filter.h(std::move(h)); assert(filter.h() == 6); filter.update(0.);