Skip to content

Commit

Permalink
run pre-commit
Browse files Browse the repository at this point in the history
Signed-off-by: fbattocchia <florencia.battocchia@creativa77.com.ar>
  • Loading branch information
fbattocchia committed Feb 26, 2025
1 parent 5cac558 commit 97474dc
Show file tree
Hide file tree
Showing 7 changed files with 10 additions and 12 deletions.
4 changes: 2 additions & 2 deletions beluga/include/beluga/sensor/likelihood_field_model.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,9 @@
#include <beluga/actions/overlay.hpp>
#include <beluga/algorithm/distance_map.hpp>
#include <beluga/sensor/data/value_grid.hpp>
#include <beluga/sensor/likelihood_field_model_common.hpp>
#include <range/v3/action/transform.hpp>
#include <sophus/se2.hpp>
#include <beluga/sensor/likelihood_field_model_common.hpp>

/**
* \file
Expand Down Expand Up @@ -77,7 +77,7 @@ class LikelihoodFieldModel : public LikelihoodFieldModelCommon<OccupancyGrid> {
* for particle states.
*/
explicit LikelihoodFieldModel(const param_type& params, const map_type& grid)
: LikelihoodFieldModelCommon<OccupancyGrid>(params,grid) {}
: LikelihoodFieldModelCommon<OccupancyGrid>(params, grid) {}

/// Returns a state weighting function conditioned on 2D lidar hits.
/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ class LikelihoodFieldModelCommon {
param_type params_;
ValueGrid2<float> likelihood_field_;
Sophus::SE2d world_to_likelihood_field_transform_;

static ValueGrid2<float> make_likelihood_field(const param_type& params, const OccupancyGrid& grid) {
const auto squared_distance = [&grid](std::size_t first, std::size_t second) {
return static_cast<float>((grid.coordinates_at(first) - grid.coordinates_at(second)).squaredNorm());
Expand Down Expand Up @@ -146,7 +146,7 @@ class LikelihoodFieldModelCommon {
auto likelihood_values = std::move(distance_map) | //
ranges::actions::transform(truncate_to_max_distance) | //
ranges::actions::transform(to_likelihood);

return ValueGrid2<float>{std::move(likelihood_values), grid.width(), grid.resolution()};
}
};
Expand Down
6 changes: 3 additions & 3 deletions beluga/include/beluga/sensor/likelihood_field_prob_model.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,9 @@
#include <beluga/actions/overlay.hpp>
#include <beluga/algorithm/distance_map.hpp>
#include <beluga/sensor/data/value_grid.hpp>
#include <beluga/sensor/likelihood_field_model_common.hpp>
#include <range/v3/action/transform.hpp>
#include <sophus/se2.hpp>
#include <beluga/sensor/likelihood_field_model_common.hpp>

/**
* \file
Expand All @@ -38,7 +38,7 @@ namespace beluga {
/**
* See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3.
*/
using LikelihoodFieldProbModelParam = LikelihoodFieldModelCommonParam;
using LikelihoodFieldProbModelParam = LikelihoodFieldModelCommonParam;

/// Likelihood field prob sensor model for range finders.
/**
Expand Down Expand Up @@ -77,7 +77,7 @@ class LikelihoodFieldProbModel : public LikelihoodFieldModelCommon<OccupancyGrid
* for particle states.
*/
explicit LikelihoodFieldProbModel(const param_type& params, const map_type& grid)
: LikelihoodFieldModelCommon<OccupancyGrid>(params,grid) {}
: LikelihoodFieldModelCommon<OccupancyGrid>(params, grid) {}

/// Returns a state weighting function conditioned on 2D lidar hits.
/**
Expand Down
2 changes: 1 addition & 1 deletion beluga/test/beluga/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,8 @@ add_executable(
sensor/test_bearing_sensor_model.cpp
sensor/test_landmark_sensor_model.cpp
sensor/test_lfm_with_unknown_space.cpp
sensor/test_likelihood_field_model_common.cpp
sensor/test_likelihood_field_model.cpp
sensor/test_likelihood_field_model_common.cpp
sensor/test_likelihood_field_prob_model.cpp
sensor/test_ndt_model.cpp
test_3d_embedding.cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@ using beluga::testing::StaticOccupancyGrid;
using UUT = beluga::LikelihoodFieldModelCommon<StaticOccupancyGrid<5, 5>>;

TEST(LikelihoodFieldModelCommon, LikelihoodField) {

constexpr double kResolution = 0.5;
// clang-format off
const auto grid = StaticOccupancyGrid<5, 5>{{
Expand Down
4 changes: 2 additions & 2 deletions beluga_ros/include/beluga_ros/amcl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -109,8 +109,8 @@ class Amcl {

/// Sensor model variant type for runtime selection support.
using sensor_model_variant = std::variant<
beluga::LikelihoodFieldModel<beluga_ros::OccupancyGrid>, //
beluga::LikelihoodFieldProbModel<beluga_ros::OccupancyGrid>, //
beluga::LikelihoodFieldModel<beluga_ros::OccupancyGrid>, //
beluga::LikelihoodFieldProbModel<beluga_ros::OccupancyGrid>, //
beluga::BeamSensorModel<beluga_ros::OccupancyGrid>>;

/// Execution policy variant type for runtime selection support.
Expand Down
1 change: 0 additions & 1 deletion beluga_ros/src/amcl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,6 @@ auto Amcl::update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_sc

force_update_ = false;
return beluga::cluster_based_estimate(beluga::views::states(particles_), beluga::views::weights(particles_));

}

} // namespace beluga_ros

0 comments on commit 97474dc

Please sign in to comment.