/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/particle_cloud.hpp Source File

/home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/particle_cloud.hpp Source File#

Beluga ROS: /home/runner/work/beluga/beluga/beluga_ros/include/beluga_ros/particle_cloud.hpp Source File
Beluga ROS
particle_cloud.hpp
Go to the documentation of this file.
1 // Copyright 2024 Ekumen, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef BELUGA_ROS_PARTICLE_CLOUD_HPP
16 #define BELUGA_ROS_PARTICLE_CLOUD_HPP
17 
18 #include <cmath>
19 #include <map>
20 #include <memory>
21 #include <type_traits>
22 
23 #include <range/v3/range/primitives.hpp>
24 #include <range/v3/view/take_exactly.hpp>
25 
26 #include <sophus/se2.hpp>
27 #include <sophus/se3.hpp>
28 #include <sophus/types.hpp>
29 
31 #include <beluga/primitives.hpp>
32 #include <beluga/views/sample.hpp>
33 #include <beluga/views/zip.hpp>
34 
35 #include <beluga_ros/messages.hpp>
36 #include <beluga_ros/tf2_eigen.hpp>
38 
44 namespace beluga_ros {
45 
46 namespace detail {
47 
49 
52 inline beluga_ros::msg::ColorRGBA alphaHueToRGBA(float hue, float alpha) {
53  beluga_ros::msg::ColorRGBA message;
54  // https://en.wikipedia.org/wiki/HSL_and_HSV#HSV_to_RGB_alternative
55  // specialized for V = S = 1 and using single precision floats because
56  // that is what the color message expects.
57  const auto kr = std::fmod(5.0F + hue / 60.0F, 6.0F);
58  const auto kg = std::fmod(3.0F + hue / 60.0F, 6.0F);
59  const auto kb = std::fmod(1.0F + hue / 60.0F, 6.0F);
60  message.r = 1.0F - 1.0F * std::max(0.0F, std::min({kr, 4.0F - kr, 1.0F}));
61  message.g = 1.0F - 1.0F * std::max(0.0F, std::min({kg, 4.0F - kg, 1.0F}));
62  message.b = 1.0F - 1.0F * std::max(0.0F, std::min({kb, 4.0F - kb, 1.0F}));
63  message.a = alpha;
64  return message;
65 }
66 
68 template <class T>
70 
72 template <typename Scalar>
73 struct almost_equal_to<Sophus::SE2<Scalar>> {
75 
79  explicit almost_equal_to(Scalar _linear_resolution, Scalar _angular_resolution)
80  : linear_resolution(_linear_resolution), angular_resolution(_angular_resolution) {}
81 
83  bool operator()(const Sophus::SE2<Scalar>& a, const Sophus::SE2<Scalar>& b) const {
84  using std::abs, std::atan, std::tan;
85  return (abs(a.translation().x() - b.translation().x()) < linear_resolution) &&
86  (abs(a.translation().y() - b.translation().y()) < linear_resolution) &&
87  (abs(atan(tan(a.so2().log() - b.so2().log()))) < angular_resolution);
88  }
89 
90  const Scalar linear_resolution;
91  const Scalar angular_resolution;
92 };
93 
94 } // namespace detail
95 
97 
103 template <
104  class Particles,
105  class Particle = ranges::range_value_t<Particles>,
106  class State = typename beluga::state_t<Particle>,
107  class Scalar = typename State::Scalar,
108  typename = std::enable_if_t<
109  std::is_same_v<State, typename Sophus::SE2<Scalar>> || std::is_same_v<State, typename Sophus::SE3<Scalar>>>>
110 beluga_ros::msg::PoseArray&
111 assign_particle_cloud(Particles&& particles, std::size_t size, beluga_ros::msg::PoseArray& message) {
112  static_assert(ranges::sized_range<decltype(particles)>);
113  message.poses.clear();
114  if (ranges::size(particles) > 0) {
115  message.poses.reserve(size);
116  for (const auto& particle : particles | beluga::views::sample | ranges::views::take_exactly(size)) {
117  auto& pose = message.poses.emplace_back();
118  tf2::toMsg(beluga::state(particle), pose);
119  }
120  }
121  return message;
122 }
123 
125 
132 template <class Particles, class Message>
133 Message& assign_particle_cloud(Particles&& particles, Message& message) {
134  static_assert(ranges::sized_range<decltype(particles)>);
135  return assign_particle_cloud(std::forward<Particles>(particles), ranges::size(particles), message);
136 }
137 
139 
146 template <
147  class Particles,
148  class Particle = ranges::range_value_t<Particles>,
149  class State = typename beluga::state_t<Particle>,
150  class Weight = typename beluga::weight_t<Particle>,
151  class Scalar = typename State::Scalar,
152  typename = std::enable_if_t<std::is_same_v<State, typename Sophus::SE2<Scalar>>>>
153 beluga_ros::msg::MarkerArray& assign_particle_cloud(
154  Particles&& particles,
155  Scalar linear_resolution,
156  Scalar angular_resolution,
157  beluga_ros::msg::MarkerArray& message) {
158  // Particle weights from the filter may or may not be representative of the
159  // true distribution. If we resampled, they are not, and there will be multiple copies
160  // of the most likely candidates, all with unit weight. In this case the number of copies
161  // is a proxy for the prob density at each candidate. If we did not resample before updating
162  // the estimation and publishing this message (which can happen if the resample interval
163  // is set to something other than 1), then all particles are expected to be different
164  // and their weights are proportional to the prob density at each candidate.
165  //
166  // Only the combination of both the state distribution and the candidate weights together
167  // provide information about the probability density at each candidate.
168  auto max_bin_weight = Weight{1e-3};
169  auto states = beluga::views::states(particles);
170  auto weights = beluga::views::weights(particles);
171  using StateHistogram = std::unordered_map<State, Weight, beluga::spatial_hash<State>, detail::almost_equal_to<State>>;
172  auto histogram = StateHistogram{
173  10U, beluga::spatial_hash<State>{linear_resolution, angular_resolution},
174  detail::almost_equal_to<State>{linear_resolution, angular_resolution}};
175  for (const auto& [state, weight] : beluga::views::zip(states, weights)) {
176  auto& bin_weight = histogram[state];
177  bin_weight += weight;
178  if (bin_weight > max_bin_weight) {
179  max_bin_weight = bin_weight;
180  }
181  }
182 
183  message.markers.clear();
184 
185  if (histogram.empty()) {
186  return message;
187  }
188 
189  // There are no arrow list markers yet (https://github.com/ros2/rviz/pull/972)
190  // so we replicate them using a line list for arrow bodies and a triangle list
191  // for arrow heads.
192  constexpr auto kArrowBodyLength = Scalar{0.5};
193  constexpr auto kArrowHeadLength = Scalar{0.1};
194  constexpr auto kArrowHeadWidth = kArrowHeadLength / Scalar{5.0};
195  constexpr auto kArrowLength = kArrowBodyLength + kArrowHeadLength;
196 
197  using Translation = typename State::TranslationType;
198  const auto arrow_body_base = Translation::Zero();
199  const auto arrow_head_tip = kArrowLength * Translation::UnitX();
200  const auto arrow_head_base = kArrowBodyLength * Translation::UnitX();
201  const auto arrow_head_right_corner =
202  kArrowBodyLength * Translation::UnitX() - (kArrowHeadWidth / Scalar{2.0}) * Translation::UnitY();
203  const auto arrow_head_left_corner =
204  kArrowBodyLength * Translation::UnitX() + (kArrowHeadWidth / Scalar{2.0}) * Translation::UnitY();
205 
206  message.markers.reserve(2);
207  auto& arrow_bodies = message.markers.emplace_back();
208  arrow_bodies.id = 0;
209  arrow_bodies.ns = "bodies";
210  arrow_bodies.color.a = 1.0;
211  arrow_bodies.pose.orientation.w = 1.0;
212  arrow_bodies.lifetime.sec = 1;
213  arrow_bodies.type = beluga_ros::msg::Marker::LINE_LIST;
214  arrow_bodies.action = beluga_ros::msg::Marker::ADD;
215  arrow_bodies.points.reserve(histogram.size() * 2); // 2 vertices per arrow body
216  arrow_bodies.colors.reserve(histogram.size() * 2);
217 
218  auto& arrow_heads = message.markers.emplace_back();
219  arrow_heads.id = 1;
220  arrow_heads.ns = "heads";
221  arrow_heads.scale.x = 1.0;
222  arrow_heads.scale.y = 1.0;
223  arrow_heads.scale.z = 1.0;
224  arrow_heads.color.a = 1.0;
225  arrow_heads.pose.orientation.w = 1.0;
226  arrow_heads.lifetime.sec = 1;
227  arrow_heads.type = beluga_ros::msg::Marker::TRIANGLE_LIST;
228  arrow_heads.action = beluga_ros::msg::Marker::ADD;
229  arrow_heads.points.reserve(histogram.size() * 3); // 3 vertices per arrow head
230  arrow_heads.colors.reserve(histogram.size() * 3);
231 
232  auto min_scale_factor = Weight{1.0};
233  for (const auto& [state, weight] : histogram) {
234  // fix markers scale ratio to ensure they can still be seen
235  using std::max;
236  const auto scale_factor = max(weight / max_bin_weight, 1e-1); // or 1:10
237  if (scale_factor < min_scale_factor) {
238  min_scale_factor = scale_factor;
239  }
240 
241  // use an inverted rainbow scale of decreasing opacity: bright red
242  // for the most likely state and dim purple for the least likely one
243  const auto vertex_color = detail::alphaHueToRGBA(
244  static_cast<float>((1.0 - scale_factor) * 270.0), static_cast<float>(0.25 + 0.75 * scale_factor));
245 
246  // linearly scale arrow sizes using particle weights too
247  arrow_bodies.points.push_back(tf2::toMsg(state * (scale_factor * arrow_body_base)));
248  arrow_bodies.points.push_back(tf2::toMsg(state * (scale_factor * arrow_head_base)));
249  arrow_bodies.colors.insert(arrow_bodies.colors.end(), 2, vertex_color);
250 
251  arrow_heads.points.push_back(tf2::toMsg(state * (scale_factor * arrow_head_left_corner)));
252  arrow_heads.points.push_back(tf2::toMsg(state * (scale_factor * arrow_head_right_corner)));
253  arrow_heads.points.push_back(tf2::toMsg(state * (scale_factor * arrow_head_tip)));
254 
255  arrow_heads.colors.insert(arrow_heads.colors.end(), 3, vertex_color);
256  }
257 
258  arrow_bodies.scale.x = static_cast<double>(min_scale_factor * kArrowHeadWidth) * 0.8;
259 
260  return message;
261 }
262 
264 
269 template <
270  class Particles,
271  class Particle = ranges::range_value_t<Particles>,
272  class State = typename beluga::state_t<Particle>,
273  class Scalar = typename State::Scalar>
274 beluga_ros::msg::MarkerArray& assign_particle_cloud(Particles&& particles, beluga_ros::msg::MarkerArray& message) {
275  constexpr auto kDefaultLinearResolution = Scalar{1e-3}; // ie. 1 mm
276  constexpr auto kDefaultAngularResolution = Scalar{1e-3}; // ie. 0.05 degrees
277  return assign_particle_cloud(
278  std::forward<Particles>(particles), kDefaultLinearResolution, kDefaultAngularResolution, message);
279 }
280 
281 } // namespace beluga_ros
282 
283 #endif // BELUGA_ROS_PARTICLE_CLOUD_HPP
Sophus namespace extension for message conversion function overload resolution (ADL enabled).
Definition: tf2_sophus.hpp:337
The main Beluga ROS namespace.
Definition: amcl.hpp:44
beluga_ros::msg::PoseArray & assign_particle_cloud(Particles &&particles, std::size_t size, beluga_ros::msg::PoseArray &message)
Assign a pose distribution to a particle cloud message.
Definition: particle_cloud.hpp:111
typename particle_traits< T >::weight_type weight_t
constexpr state_detail::state_fn state
Numeric< double, struct WeightTag > Weight
constexpr weight_detail::weight_fn weight
typename particle_traits< T >::state_type state_t
beluga_ros::msg::Point toMsg(const Eigen::Matrix< Scalar, 2, 1 > &in)
Converts an Eigen Vector2 type to a Point message.
Definition: tf2_eigen.hpp:46
beluga_ros::msg::ColorRGBA alphaHueToRGBA(float hue, float alpha)
Make an RGBA color based on hue alone.
Definition: particle_cloud.hpp:52
constexpr auto states
constexpr auto weights
bool operator()(const Sophus::SE2< Scalar > &a, const Sophus::SE2< Scalar > &b) const
Compares a and b for near equality.
Definition: particle_cloud.hpp:83
const Scalar angular_resolution
Resolution for rotational coordinates, in radians.
Definition: particle_cloud.hpp:91
almost_equal_to(Scalar _linear_resolution, Scalar _angular_resolution)
Constructs near equality functor.
Definition: particle_cloud.hpp:79
const Scalar linear_resolution
Resolution for translational coordinates, in meters.
Definition: particle_cloud.hpp:90
std::equal_to equivalent for near equality operations.
Definition: particle_cloud.hpp:69
Message conversion API overloads for Eigen types.
Message conversion API overloads for Sophus types.