MRPT  2.0.4
CPlanarLaserScan.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
14 
15 using namespace mrpt;
16 using namespace mrpt::opengl;
17 using namespace mrpt::math;
18 using namespace std;
19 
21 
23 {
25  m_scan.resizeScan(0);
26 }
27 
29 {
30  switch (rc.shader_id)
31  {
33  if (m_enable_surface) CRenderizableShaderTriangles::render(rc);
34  break;
36  if (m_enable_line) CRenderizableShaderWireFrame::render(rc);
37  break;
39  if (m_enable_points) CRenderizableShaderPoints::render(rc);
40  break;
41  };
42 }
44 {
45  // Load into cache:
46  if (!m_cache_valid)
47  {
48  m_cache_valid = true;
49  m_cache_points.clear();
50  m_cache_points.insertionOptions.minDistBetweenLaserPoints = 0;
51  m_cache_points.insertionOptions.isPlanarMap = false;
52 
53  m_cache_points.insertObservation(m_scan);
54  }
55 
59 }
60 
62 {
65  vbd.clear();
66  cbd.clear();
67 
68  size_t n;
69  const float *x, *y, *z;
70  m_cache_points.getPointsBuffer(n, x, y, z);
71  if (!n) return;
72 
73  for (size_t i = 0; i < n - 1; i++)
74  {
75  vbd.emplace_back(x[i], y[i], z[i]);
76  vbd.emplace_back(x[i + 1], y[i + 1], z[i + 1]);
77  }
78 
79  cbd.assign(
80  vbd.size(),
81  mrpt::img::TColorf(m_line_R, m_line_G, m_line_B, m_line_A).asTColor());
82 }
83 
85 {
87  tris.clear();
88 
89  size_t n;
90  const float *x, *y, *z;
91  m_cache_points.getPointsBuffer(n, x, y, z);
92  if (!n) return;
93 
94  using P3f = mrpt::math::TPoint3Df;
95 
96  for (size_t i = 0; i < n - 1; i++)
97  {
98  tris.emplace_back(
99  P3f(m_scan.sensorPose.x(), m_scan.sensorPose.y(),
100  m_scan.sensorPose.z()),
101  P3f(x[i], y[i], z[i]), P3f(x[i + 1], y[i + 1], z[i + 1]));
102  }
103 
104  for (auto& t : tris)
105  {
106  t.computeNormals();
107  t.setColor(
108  mrpt::img::TColorf(m_plane_R, m_plane_G, m_plane_B, m_plane_A));
109  }
110 }
111 
113 {
116  vbd.clear();
117 
118  size_t n;
119  const float *x, *y, *z;
120  m_cache_points.getPointsBuffer(n, x, y, z);
121 
122  for (size_t i = 0; i < n; i++) vbd.emplace_back(x[i], y[i], z[i]);
123 
124  cbd.assign(
125  vbd.size(),
126  mrpt::img::TColorf(m_points_R, m_points_G, m_points_B, m_points_A)
127  .asTColor());
128 }
129 
130 uint8_t CPlanarLaserScan::serializeGetVersion() const { return 2; }
132 {
133  writeToStreamRender(out);
134  out << m_scan;
135  out << m_line_R << m_line_G << m_line_B << m_line_A << m_points_R
136  << m_points_G << m_points_B << m_points_A << m_plane_R << m_plane_G
137  << m_plane_B << m_plane_A << m_enable_points << m_enable_line
138  << m_enable_surface; // new in v1
139 }
140 
142  mrpt::serialization::CArchive& in, uint8_t version)
143 {
144  switch (version)
145  {
146  case 0:
147  case 1:
148  {
149  readFromStreamRender(in);
150  in >> m_scan;
151 
152  if (version >= 2)
153  { // m_line_width
154  float dummy;
155  in >> dummy;
156  }
157 
158  in >> m_line_R >> m_line_G >> m_line_B >> m_line_A;
159 
160  if (version >= 2)
161  { // m_points_width
162  float dummy;
163  in >> dummy;
164  }
165  in >> m_points_R >> m_points_G >> m_points_B >> m_points_A >>
166  m_plane_R >> m_plane_G >> m_plane_B >> m_plane_A;
167 
168  if (version >= 1)
169  {
170  in >> m_enable_points >> m_enable_line >>
171  m_enable_surface; // new in v1
172  }
173  else
174  {
175  m_enable_points = m_enable_line = m_enable_surface = true;
176  }
177  }
178  break;
179  default:
181  };
182 }
183 
186 {
187  // Load into cache:
188  if (!m_cache_valid)
189  {
190  m_cache_valid = true;
191  m_cache_points.clear();
192  m_cache_points.insertionOptions.minDistBetweenLaserPoints = 0;
193  m_cache_points.insertionOptions.isPlanarMap = false;
194 
195  m_cache_points.insertObservation(m_scan);
196  }
197 
198  size_t n;
199  const float *x, *y, *z;
200 
201  m_cache_points.getPointsBuffer(n, x, y, z);
202  if (!n || !x) return;
203 
205  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
206  std::numeric_limits<double>::max());
208  -std::numeric_limits<double>::max(),
209  -std::numeric_limits<double>::max(),
210  -std::numeric_limits<double>::max());
211 
212  for (size_t i = 0; i < n; i++)
213  {
214  keep_min(bb_min.x, x[i]);
215  keep_max(bb_max.x, x[i]);
216  keep_min(bb_min.y, y[i]);
217  keep_max(bb_max.y, y[i]);
218  keep_min(bb_min.z, z[i]);
219  keep_max(bb_max.z, z[i]);
220  }
221 
222  // Convert to coordinates of my parent:
223  m_pose.composePoint(bb_min, bb_min);
224  m_pose.composePoint(bb_max, bb_max);
225 }
226 
228 {
229  return {d2f(m_scan.sensorPose.x()), d2f(m_scan.sensorPose.y()),
230  d2f(m_scan.sensorPose.z())};
231 }
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void onUpdateBuffers_Triangles() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
void onUpdateBuffers_Wireframe() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
STL namespace.
mrpt::math::TPoint3Df getLocalRepresentativePoint() const override
Provide a representative point (in object local coordinates), used to sort objects by eye-distance wh...
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
Context for calls to render()
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
std::vector< mrpt::img::TColor > m_color_buffer_data
float d2f(const double d)
shortcut for static_cast<float>(double)
This base provides a set of functions for maths stuff.
static constexpr shader_id_t WIREFRAME
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
static constexpr shader_id_t TRIANGLES
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
std::vector< mrpt::img::TColor > m_color_buffer_data
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
std::vector< mrpt::opengl::TTriangle > m_triangles
List of triangles.
mrpt::vision::TStereoCalibResults out
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
const auto bb_max
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
const auto bb_min
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
TColor asTColor() const
Returns the 0-255 integer version of this color: RGBA_u8.
Definition: TColor.h:101
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
void onUpdateBuffers_Points() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
static constexpr shader_id_t POINTS
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 3e8ecd670 Mon May 18 14:23:19 2020 +0200 at lun may 18 14:30:11 CEST 2020