MRPT  2.0.3
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 }
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::opengl::CRenderizable::notifyChange
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
Definition: CRenderizable.h:315
mrpt::math::TPoint3D_< double >
mrpt::opengl::CRenderizableShaderTriangles::render
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
Definition: CRenderizableShaderTriangles.cpp:45
mrpt::opengl::CRenderizableShaderPoints::render
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
Definition: CRenderizableShaderPoints.cpp:52
mrpt::opengl::CPlanarLaserScan::renderUpdateBuffers
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers,...
Definition: CPlanarLaserScan.cpp:43
mrpt::opengl::CRenderizable
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
mrpt::opengl::CPlanarLaserScan::render
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
Definition: CPlanarLaserScan.cpp:28
mrpt::opengl::CRenderizableShaderWireFrame::m_color_buffer_data
std::vector< mrpt::img::TColor > m_color_buffer_data
Definition: CRenderizableShaderWireFrame.h:68
mrpt::opengl::CPlanarLaserScan::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPlanarLaserScan.cpp:131
mrpt::opengl::CRenderizableShaderTriangles::m_triangles
std::vector< mrpt::opengl::TTriangle > m_triangles
List of triangles.
Definition: CRenderizableShaderTriangles.h:53
mrpt::img::TColorf::asTColor
TColor asTColor() const
Returns the 0-255 integer version of this color: RGBA_u8
Definition: TColor.h:101
mrpt::opengl::CPlanarLaserScan::getBoundingBox
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...
Definition: CPlanarLaserScan.cpp:184
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::math::TPoint3Df
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::opengl::CPlanarLaserScan::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPlanarLaserScan.cpp:141
mrpt::opengl::CPlanarLaserScan::onUpdateBuffers_Points
void onUpdateBuffers_Points() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
Definition: CPlanarLaserScan.cpp:112
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::opengl::CPlanarLaserScan::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPlanarLaserScan.cpp:130
bb_max
const auto bb_max
Definition: CPose3DPDFGrid_unittest.cpp:25
mrpt::opengl::CPlanarLaserScan::onUpdateBuffers_Triangles
void onUpdateBuffers_Triangles() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
Definition: CPlanarLaserScan.cpp:84
mrpt::opengl::DefaultShaderID::WIREFRAME
static constexpr shader_id_t WIREFRAME
Definition: DefaultShaders.h:25
bb_min
const auto bb_min
Definition: CPose3DPDFGrid_unittest.cpp:23
mrpt::opengl::DefaultShaderID::TRIANGLES
static constexpr shader_id_t TRIANGLES
Definition: DefaultShaders.h:27
mrpt::opengl::CRenderizableShaderWireFrame::m_vertex_buffer_data
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
Definition: CRenderizableShaderWireFrame.h:67
mrpt::opengl::CRenderizableShaderPoints::renderUpdateBuffers
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers,...
mrpt::img::TColorf
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
mrpt::d2f
float d2f(const double d)
shortcut for static_cast<float>(double)
Definition: core/include/mrpt/core/bits_math.h:189
mrpt::opengl::CRenderizableShaderWireFrame::render
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
Definition: CRenderizableShaderWireFrame.cpp:52
mrpt::keep_max
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.
Definition: core/include/mrpt/core/bits_math.h:152
mrpt::keep_min
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.
Definition: core/include/mrpt/core/bits_math.h:145
mrpt::opengl::CRenderizable::RenderContext
Context for calls to render()
Definition: CRenderizable.h:266
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::opengl::CPlanarLaserScan::getLocalRepresentativePoint
mrpt::math::TPoint3Df getLocalRepresentativePoint() const override
Provide a representative point (in object local coordinates), used to sort objects by eye-distance wh...
Definition: CPlanarLaserScan.cpp:227
mrpt::opengl::CRenderizable::RenderContext::shader_id
mrpt::opengl::shader_id_t shader_id
Definition: CRenderizable.h:272
mrpt::opengl::CRenderizableShaderTriangles::renderUpdateBuffers
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers,...
maps-precomp.h
mrpt::opengl::CPlanarLaserScan::onUpdateBuffers_Wireframe
void onUpdateBuffers_Wireframe() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
Definition: CPlanarLaserScan.cpp:61
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::opengl::CRenderizableShaderPoints::m_vertex_buffer_data
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
Definition: CRenderizableShaderPoints.h:94
CPlanarLaserScan.h
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::opengl::CRenderizableShaderWireFrame::renderUpdateBuffers
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers,...
mrpt::opengl::CRenderizableShaderPoints::m_color_buffer_data
std::vector< mrpt::img::TColor > m_color_buffer_data
Definition: CRenderizableShaderPoints.h:95
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::opengl::DefaultShaderID::POINTS
static constexpr shader_id_t POINTS
Definition: DefaultShaders.h:24
mrpt::opengl::CPlanarLaserScan
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
Definition: CPlanarLaserScan.h:57



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020