Skip to content

Commit

Permalink
[filter] modernize unused variable support (#254)
Browse files Browse the repository at this point in the history
  • Loading branch information
FrancoisCarouge committed Jan 7, 2023
1 parent 2e48e9c commit fc7ef4c
Show file tree
Hide file tree
Showing 8 changed files with 90 additions and 138 deletions.
102 changes: 36 additions & 66 deletions include/fcarouge/internal/kalman.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -107,44 +107,29 @@ struct kalman<State, Output, void, Divide, pack<UpdateTypes...>,
//! 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<void>(state_x);
(static_cast<void>(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<void>(state_x);
static_cast<void>(output_z);
(static_cast<void>(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<void>(state_x);
(static_cast<void>(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<void>(state_x);
(static_cast<void>(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<void>(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<void>(update_pack), ...);
[[maybe_unused]] const UpdateTypes &...update_pack) -> output {
return h * state_x;
}};

Expand Down Expand Up @@ -238,50 +223,35 @@ struct kalman<State, Output, Input, Divide, pack<UpdateTypes...>,
//! 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<void>(state_x);
(static_cast<void>(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<void>(state_x);
static_cast<void>(output_z);
(static_cast<void>(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<void>(state_x);
static_cast<void>(input_u);
(static_cast<void>(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<void>(state_x);
(static_cast<void>(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<void>(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<void>(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<void>(update_pack), ...);
[[maybe_unused]] const UpdateTypes &...update_pack) -> output {
return h * state_x;
}};

Expand Down
3 changes: 1 addition & 2 deletions include/fcarouge/internal/utility.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,8 @@ template <typename Type>
concept arithmetic = std::integral<Type> || std::floating_point<Type>;

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<void>(any), ...);
}
};

Expand Down
20 changes: 9 additions & 11 deletions sample/kf_2x1x1_rocket_altitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,20 +69,18 @@ template <typename Type, auto Size> using vector = Eigen::Vector<Type, Size>;
// 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<void>(x);
const auto dt{std::chrono::duration<double>(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<double>(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<void>(x);
static_cast<void>(u);
const auto dt{std::chrono::duration<double>(delta_time).count()};
return kalman::state_transition{{1, dt}, {0, 1}};
});
Expand Down
21 changes: 10 additions & 11 deletions sample/kf_8x4x0_deep_sort_bounding_box.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<void>(z);
return vector<float, 4>{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<float, 4>{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) {
Expand Down
34 changes: 14 additions & 20 deletions test/eigen_f.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -107,33 +107,27 @@ 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<void>(x);
static_cast<void>(d);
static_cast<void>(fp);
static_cast<void>(i);
static_cast<void>(u);
return matrix<double, 5, 5>::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<double, 5, 5>::Identity();
}};
filter.f(f);
assert(filter.f() == z5x5);
filter.predict(0, 0.f, 0., z3);
assert(filter.f() == i5x5);
}

{
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<void>(x);
static_cast<void>(d);
static_cast<void>(fp);
static_cast<void>(i);
static_cast<void>(u);
return matrix<double, 5, 5>::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<double, 5, 5>::Zero();
}};
filter.f(std::move(f));
assert(filter.f() == i5x5);
filter.predict(0, 0.f, 0., z3);
Expand Down
20 changes: 8 additions & 12 deletions test/eigen_h.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<void>(x);
static_cast<void>(d);
static_cast<void>(f);
static_cast<void>(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<double, 4, 5>::Identity();
}};
filter.h(h);
Expand All @@ -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<void>(x);
static_cast<void>(d);
static_cast<void>(f);
static_cast<void>(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<double, 4, 5>::Zero();
}};
filter.h(std::move(h));
Expand Down
12 changes: 4 additions & 8 deletions test/f.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -75,21 +75,17 @@ namespace {
}

{
const auto f{[](const kalman::state &x) -> kalman::state_transition {
static_cast<void>(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();
assert(filter.f() == 6);
}

{
const auto f{[](const kalman::state &x) -> kalman::state_transition {
static_cast<void>(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();
Expand Down
16 changes: 8 additions & 8 deletions test/h.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -75,21 +75,21 @@ namespace {
}

{
const auto h{[](const kalman::state &x) -> kalman::output_model {
static_cast<void>(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.);
assert(filter.h() == 6);
}

{
const auto h{[](const kalman::state &x) -> kalman::output_model {
static_cast<void>(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.);
Expand Down

0 comments on commit fc7ef4c

Please sign in to comment.