CARLA
SemanticLidarData.h
Go to the documentation of this file.
1 // Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #pragma once
8 
9 #include "carla/rpc/Location.h"
10 
11 #include <cstdint>
12 #include <vector>
13 #include <numeric>
14 
15 namespace carla {
16 namespace sensor {
17 
18 namespace s11n {
19  class SemanticLidarSerializer;
20  class SemanticLidarHeaderView;
21 }
22 
23 namespace data {
24 
25  /// Helper class to store and serialize the data generated by a RawLidar.
26  ///
27  /// The header of a Lidar measurement consists of an array of uint32_t's in
28  /// the following layout
29  ///
30  /// {
31  /// Horizontal angle (float),
32  /// Channel count,
33  /// Point count of channel 0,
34  /// ...
35  /// Point count of channel n,
36  /// }
37  ///
38  /// The points are stored in an array of detections, each detection consist in
39  /// four floats, the point detected and the angle between the casted ray and
40  /// the normal of the hitted object, and two unsigned integers, the index
41  /// and the semantic tag of the hitted object
42  ///
43  /// {
44  /// X0, Y0, Z0, Cos(TH0), idx_0, tag_0
45  /// ...
46  /// Xn, Yn, Zn, Cos(THn), idx_n, tag_n
47  /// }
48  ///
49 
50  #pragma pack(push, 1)
52  public:
53  geom::Location point{};
54  float cos_inc_angle{};
55  uint32_t object_idx{};
56  uint32_t object_tag{};
57 
58  SemanticLidarDetection() = default;
59 
60  SemanticLidarDetection(float x, float y, float z, float cosTh, uint32_t idx, uint32_t tag) :
61  point(x, y, z), cos_inc_angle{cosTh}, object_idx{idx}, object_tag{tag} { }
62  SemanticLidarDetection(geom::Location p, float cosTh, uint32_t idx, uint32_t tag) :
63  point(p), cos_inc_angle{cosTh}, object_idx{idx}, object_tag{tag} { }
64 
65  void WritePlyHeaderInfo(std::ostream& out) const{
66  out << "property float32 x\n" \
67  "property float32 y\n" \
68  "property float32 z\n" \
69  "property float32 CosAngle\n" \
70  "property uint32 ObjIdx\n" \
71  "property uint32 ObjTag";
72  }
73 
74  void WriteDetection(std::ostream& out) const{
75  out << point.x << ' ' << point.y << ' ' << point.z << ' ' \
76  << cos_inc_angle << ' ' << object_idx << ' ' << object_tag;
77  }
78  };
79  #pragma pack(pop)
80 
82  static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size");
83 
84  protected:
85  enum Index : size_t {
88  SIZE
89  };
90 
91  public:
92  explicit SemanticLidarData(uint32_t ChannelCount = 0u)
93  : _header(Index::SIZE + ChannelCount, 0u) {
94  _header[Index::ChannelCount] = ChannelCount;
95  }
96 
97  SemanticLidarData &operator=(SemanticLidarData &&) = default;
98 
99  virtual ~SemanticLidarData() {}
100 
101  float GetHorizontalAngle() const {
102  return reinterpret_cast<const float &>(_header[Index::HorizontalAngle]);
103  }
104 
105  void SetHorizontalAngle(float angle) {
106  std::memcpy(&_header[Index::HorizontalAngle], &angle, sizeof(uint32_t));
107  }
108 
109  uint32_t GetChannelCount() const {
110  return _header[Index::ChannelCount];
111  }
112 
113  virtual void ResetSerPoints(std::vector<uint32_t> points_per_channel) {
114  DEBUG_ASSERT(GetChannelCount() > points_per_channel.size());
115  std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
116 
117  for (auto idxChannel = 0u; idxChannel < GetChannelCount(); ++idxChannel)
118  _header[Index::SIZE + idxChannel] = points_per_channel[idxChannel];
119 
120  uint32_t total_points = static_cast<uint32_t>(
121  std::accumulate(points_per_channel.begin(), points_per_channel.end(), 0));
122 
123  _ser_points.clear();
124  _ser_points.reserve(total_points);
125  }
126 
127  virtual void WritePointSync(SemanticLidarDetection &detection) {
128  _ser_points.emplace_back(detection);
129  }
130 
131  protected:
132  std::vector<uint32_t> _header;
134 
135  private:
136  std::vector<SemanticLidarDetection> _ser_points;
137 
140  };
141 
142 } // namespace s11n
143 } // namespace sensor
144 } // namespace carla
SemanticLidarData(uint32_t ChannelCount=0u)
void WriteDetection(std::ostream &out) const
virtual void ResetSerPoints(std::vector< uint32_t > points_per_channel)
static const FString SIZE
Helper class to store and serialize the data generated by a RawLidar.
A view over the header of a Lidar measurement.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
std::vector< SemanticLidarDetection > _ser_points
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
Serializes the data generated by Lidar sensors.
SemanticLidarDetection(geom::Location p, float cosTh, uint32_t idx, uint32_t tag)
virtual void WritePointSync(SemanticLidarDetection &detection)
void WritePlyHeaderInfo(std::ostream &out) const
SemanticLidarDetection(float x, float y, float z, float cosTh, uint32_t idx, uint32_t tag)