15 #ifndef BELUGA_ROS_PARTICLE_CLOUD_HPP
16 #define BELUGA_ROS_PARTICLE_CLOUD_HPP
21 #include <type_traits>
23 #include <range/v3/range/primitives.hpp>
24 #include <range/v3/view/take_exactly.hpp>
26 #include <sophus/se2.hpp>
27 #include <sophus/se3.hpp>
28 #include <sophus/types.hpp>
35 #include <beluga_ros/messages.hpp>
53 beluga_ros::msg::ColorRGBA message;
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}));
72 template <
typename Scalar>
80 : linear_resolution(_linear_resolution), angular_resolution(_angular_resolution) {}
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);
105 class Particle = ranges::range_value_t<Particles>,
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&
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();
132 template <
class Particles,
class Message>
134 static_assert(ranges::sized_range<decltype(particles)>);
148 class Particle = ranges::range_value_t<Particles>,
151 class Scalar =
typename State::Scalar,
152 typename = std::enable_if_t<std::is_same_v<State, typename Sophus::SE2<Scalar>>>>
154 Particles&& particles,
155 Scalar linear_resolution,
156 Scalar angular_resolution,
157 beluga_ros::msg::MarkerArray& message) {
168 auto max_bin_weight =
Weight{1e-3};
169 auto states = beluga::views::states(particles);
170 auto weights = beluga::views::weights(particles);
172 auto histogram = StateHistogram{
176 auto& bin_weight = histogram[
state];
178 if (bin_weight > max_bin_weight) {
179 max_bin_weight = bin_weight;
183 message.markers.clear();
185 if (histogram.empty()) {
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;
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();
206 message.markers.reserve(2);
207 auto& arrow_bodies = message.markers.emplace_back();
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);
216 arrow_bodies.colors.reserve(histogram.size() * 2);
218 auto& arrow_heads = message.markers.emplace_back();
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);
230 arrow_heads.colors.reserve(histogram.size() * 3);
232 auto min_scale_factor =
Weight{1.0};
236 const auto scale_factor = max(
weight / max_bin_weight, 1e-1);
237 if (scale_factor < min_scale_factor) {
238 min_scale_factor = scale_factor;
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));
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);
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)));
255 arrow_heads.colors.insert(arrow_heads.colors.end(), 3, vertex_color);
258 arrow_bodies.scale.x =
static_cast<double>(min_scale_factor * kArrowHeadWidth) * 0.8;
271 class Particle = ranges::range_value_t<Particles>,
273 class Scalar =
typename State::Scalar>
275 constexpr
auto kDefaultLinearResolution = Scalar{1e-3};
276 constexpr
auto kDefaultAngularResolution = Scalar{1e-3};
278 std::forward<Particles>(particles), kDefaultLinearResolution, kDefaultAngularResolution, message);
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
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.