MRPT  1.9.9
CPointCloud.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-2019, 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 "opengl-precomp.h" // Precompiled header
11 
12 #include <mrpt/core/round.h> // round()
17 
18 #include "opengl_internals.h"
19 
20 using namespace mrpt;
21 using namespace mrpt::opengl;
22 using namespace mrpt::img;
23 using namespace mrpt::math;
24 using namespace std;
25 
28 
30 {
32 }
34  float value)
35 {
37 }
38 
40 {
42 }
44 {
46 }
47 
49 
50 /*---------------------------------------------------------------
51  render
52  ---------------------------------------------------------------*/
54  : m_xs(),
55  m_ys(),
56  m_zs(),
57 
58  m_colorFromDepth_min(0, 0, 0),
59  m_colorFromDepth_max(0, 0, 1)
60 {
61  markAllPointsAsNew();
62 }
63 
64 /*---------------------------------------------------------------
65  render
66  ---------------------------------------------------------------*/
67 void CPointCloud::render() const
68 {
69 #if MRPT_HAS_OPENGL_GLUT
70 
71  ASSERT_(m_xs.size() == m_ys.size());
72  ASSERT_(m_xs.size() == m_zs.size());
73 
74  octree_assure_uptodate(); // Rebuild octree if needed
75  m_last_rendered_count_ongoing = 0;
76 
77  // Info needed by octree renderer:
80 
81  if (m_colorFromDepth)
82  {
83  if (!m_minmax_valid)
84  {
85  m_minmax_valid = true;
86  if (!m_zs.empty())
88  m_colorFromDepth == CPointCloud::colZ
89  ? m_zs
90  : (m_colorFromDepth == CPointCloud::colY ? m_ys : m_xs),
91  m_min, m_max);
92  else
93  m_max = m_min = 0;
94  }
95 
96  m_max_m_min = m_max - m_min;
97  if (std::abs(m_max_m_min) < 1e-4)
98  m_max_m_min = -1;
99  else
100  m_min = m_max - m_max_m_min * 1.01f;
101  m_max_m_min_inv = 1.0 / m_max_m_min;
102  }
103 
104  if (m_color.A != 255)
105  {
108  }
109  else
110  {
112  }
113 
114  // Slopes of color interpolation:
115  m_col_slop.R = m_colorFromDepth_max.R - m_colorFromDepth_min.R;
116  m_col_slop.G = m_colorFromDepth_max.G - m_colorFromDepth_min.G;
117  m_col_slop.B = m_colorFromDepth_max.B - m_colorFromDepth_min.B;
118 
119  m_col_slop_inv.R = m_col_slop.R != 0 ? 1.0f / m_col_slop.R : 0;
120  m_col_slop_inv.G = m_col_slop.G != 0 ? 1.0f / m_col_slop.G : 0;
121  m_col_slop_inv.B = m_col_slop.B != 0 ? 1.0f / m_col_slop.B : 0;
122 
123  glPointSize(m_pointSize);
124  if (m_pointSmooth)
126  else
128 
129  // Disable lighting for point clouds:
131 
133  glColor4ub(
134  m_color.R, m_color.G, m_color.B,
135  m_color.A); // The default if m_colorFromDepth=false
136  octree_render(ri); // Render all points recursively:
137  glEnd();
138 
140 
141  if (m_color.A != 255) glDisable(GL_BLEND);
142 
143  if (m_pointSmooth) glDisable(GL_POINT_SMOOTH);
144 
145  m_last_rendered_count = m_last_rendered_count_ongoing;
146 
148 #endif
149 }
150 
151 inline void CPointCloud::internal_render_one_point(size_t i) const
152 {
153 #if MRPT_HAS_OPENGL_GLUT
154  if (m_colorFromDepth != colNone && m_max_m_min > 0)
155  {
156  const float depthCol =
157  (m_colorFromDepth == colX
158  ? m_xs[i]
159  : (m_colorFromDepth == colY ? m_ys[i] : m_zs[i]));
160 
161  float f = (depthCol - m_min) * m_max_m_min_inv;
162  f = std::max(0.0f, min(1.0f, f));
163 
164  glColor4f(
165  m_colorFromDepth_min.R + f * m_col_slop_inv.R,
166  m_colorFromDepth_min.G + f * m_col_slop_inv.G,
167  m_colorFromDepth_min.B + f * m_col_slop_inv.B,
168  m_color.A * (1.0f / 255.f));
169  }
170  glVertex3f(m_xs[i], m_ys[i], m_zs[i]);
171 #else
173 #endif
174 }
175 
176 /** Render a subset of points (required by octree renderer) */
178  const bool all, const std::vector<size_t>& idxs,
179  const float render_area_sqpixels) const
180 {
181 #if MRPT_HAS_OPENGL_GLUT
182 
183  const size_t N = (all ? m_xs.size() : idxs.size());
184  const size_t decimation = mrpt::round(std::max(
185  1.0f, static_cast<float>(
187  render_area_sqpixels))));
188 
189  m_last_rendered_count_ongoing += N / decimation;
190 
191  if (all)
192  {
193  for (size_t i = 0; i < N; i++) internal_render_one_point(i);
194  }
195  else
196  {
197  const size_t Np = idxs.size();
198  for (size_t i = 0; i < Np; i += decimation)
199  internal_render_one_point(idxs[i]);
200  }
201 #else
202  MRPT_UNUSED_PARAM(all);
203  MRPT_UNUSED_PARAM(idxs);
204  MRPT_UNUSED_PARAM(render_area_sqpixels);
205 #endif
206 }
209 {
211  out["colorFromDepth"] = static_cast<int32_t>(m_colorFromDepth);
212  out["pointSize"] = m_pointSize;
213  for (size_t i = 0; i < m_xs.size(); i++)
214  {
215  out["xs"][i] = m_xs[i];
216  }
217  for (size_t i = 0; i < m_ys.size(); i++)
218  {
219  out["ys"][i] = m_ys[i];
220  }
221  for (size_t i = 0; i < m_zs.size(); i++)
222  {
223  out["zs"][i] = m_zs[i];
224  }
225  out["colorFromDepth_min"]["R"] = m_colorFromDepth_min.R;
226  out["colorFromDepth_min"]["G"] = m_colorFromDepth_min.G;
227  out["colorFromDepth_min"]["B"] = m_colorFromDepth_min.B;
228  out["colorFromDepth_max"]["R"] = m_colorFromDepth_max.R;
229  out["colorFromDepth_max"]["G"] = m_colorFromDepth_max.G;
230  out["colorFromDepth_max"]["B"] = m_colorFromDepth_max.B;
231  out["pointSmooth"] = m_pointSmooth;
232 }
234 {
235  uint8_t version;
237  switch (version)
238  {
239  case 1:
240  {
241  /**
242  * currently below is being left to what is being set by
243  * the default constructor i.e.,CPointCloud::colNone
244  */
245  // m_colorFromDepth = static_cast<float>(in["colorDepth"]);
246  m_pointSize = static_cast<float>(in["pointSize"]);
247  for (size_t i = 0; i < m_xs.size(); i++)
248  {
249  m_xs[i] = static_cast<float>(in["xs"][i]);
250  }
251  for (size_t i = 0; i < m_ys.size(); i++)
252  {
253  m_ys[i] = static_cast<float>(in["ys"][i]);
254  }
255  for (size_t i = 0; i < m_zs.size(); i++)
256  {
257  m_zs[i] = static_cast<float>(in["zs"][i]);
258  }
259  m_colorFromDepth_min.R =
260  static_cast<float>(in["colorFromDepth_min"]["R"]);
261  m_colorFromDepth_min.G =
262  static_cast<float>(in["colorFromDepth_min"]["G"]);
263  m_colorFromDepth_min.B =
264  static_cast<float>(in["colorFromDepth_min"]["B"]);
265  m_colorFromDepth_max.R =
266  static_cast<float>(in["colorFromDepth_max"]["R"]);
267  m_colorFromDepth_max.G =
268  static_cast<float>(in["colorFromDepth_max"]["G"]);
269  m_colorFromDepth_max.B =
270  static_cast<float>(in["colorFromDepth_max"]["B"]);
271  m_pointSmooth = static_cast<bool>(in["pointSmooth"]);
272  }
273  break;
274  default:
276  }
277 }
280 {
281  writeToStreamRender(out);
282  // Changed from bool to enum/int32_t in version 3.
283  out << static_cast<int32_t>(m_colorFromDepth);
284  out << m_xs << m_ys << m_zs;
285 
286  // Added in version 1.
287  out << m_pointSize;
288 
289  // New in version 2:
290  out << m_colorFromDepth_min.R << m_colorFromDepth_min.G
291  << m_colorFromDepth_min.B;
292  out << m_colorFromDepth_max.R << m_colorFromDepth_max.G
293  << m_colorFromDepth_max.B;
294 
295  // New in version 4:
296  out << m_pointSmooth;
297 }
298 
301 {
302  switch (version)
303  {
304  case 0:
305  case 1:
306  case 2:
307  case 3:
308  case 4:
309  {
310  readFromStreamRender(in);
311  if (version >= 3)
312  {
313  int32_t axis;
314  in >> axis;
315  m_colorFromDepth = Axis(axis);
316  }
317  else
318  {
319  bool colorFromZ;
320  in >> colorFromZ;
321  m_colorFromDepth =
323  }
324  in >> m_xs >> m_ys >> m_zs;
325 
326  if (version >= 1)
327  in >> m_pointSize;
328  else
329  m_pointSize = 1;
330 
331  if (version >= 2)
332  {
333  in >> m_colorFromDepth_min.R >> m_colorFromDepth_min.G >>
334  m_colorFromDepth_min.B;
335  in >> m_colorFromDepth_max.R >> m_colorFromDepth_max.G >>
336  m_colorFromDepth_max.B;
337  }
338  else
339  {
340  m_colorFromDepth_min = TColorf(0, 0, 0);
341  m_colorFromDepth_max.R = m_color.R * 255.f;
342  m_colorFromDepth_max.G = m_color.G * 255.f;
343  m_colorFromDepth_max.B = m_color.B * 255.f;
344  }
345 
346  if (version >= 4)
347  in >> m_pointSmooth;
348  else
349  m_pointSmooth = false;
350  }
351  break;
352  default:
354  };
355 
356  markAllPointsAsNew();
357 }
358 
359 /*---------------------------------------------------------------
360  clear
361 ---------------------------------------------------------------*/
363 {
364  m_xs.clear();
365  m_ys.clear();
366  m_zs.clear();
367  markAllPointsAsNew();
368 }
369 
370 /*---------------------------------------------------------------
371  insertPoint
372 ---------------------------------------------------------------*/
373 void CPointCloud::insertPoint(float x, float y, float z)
374 {
375  m_xs.push_back(x);
376  m_ys.push_back(y);
377  m_zs.push_back(z);
378 
379  m_minmax_valid = false;
380 
381  // JL: TODO note: Well, this can be clearly done much more efficiently
382  // but...I don't have time! :-(
383  markAllPointsAsNew();
384 }
385 
386 /** Write an individual point (checks for "i" in the valid range only in Debug).
387  */
389  size_t i, const float x, const float y, const float z)
390 {
391 #ifdef _DEBUG
392  ASSERT_BELOW_(i, size());
393 #endif
394  m_xs[i] = x;
395  m_ys[i] = y;
396  m_zs[i] = z;
397 
398  m_minmax_valid = false;
399 
400  // JL: TODO note: Well, this can be clearly done much more efficiently
401  // but...I don't have time! :-(
402  markAllPointsAsNew();
403 }
404 
405 /*---------------------------------------------------------------
406  setGradientColors
407 ---------------------------------------------------------------*/
409  const mrpt::img::TColorf& colorMin, const mrpt::img::TColorf& colorMax)
410 {
411  m_colorFromDepth_min = colorMin;
412  m_colorFromDepth_max = colorMax;
413 }
414 
415 // Do needed internal work if all points are new (octree rebuilt,...)
417 {
418  m_minmax_valid = false;
419  octree_mark_as_outdated();
420 }
421 
422 /** In a base class, reserve memory to prepare subsequent calls to
423  * PLY_import_set_vertex */
425 {
426  this->resize(N);
427 }
428 
429 /** In a base class, will be called after PLY_import_set_vertex_count() once for
430  * each loaded point.
431  * \param pt_color Will be nullptr if the loaded file does not provide color
432  * info.
433  */
435  const size_t idx, const mrpt::math::TPoint3Df& pt,
436  const mrpt::img::TColorf* pt_color)
437 {
438  MRPT_UNUSED_PARAM(pt_color);
439  this->setPoint(idx, pt.x, pt.y, pt.z);
440 }
441 
442 /** In a base class, return the number of vertices */
443 size_t CPointCloud::PLY_export_get_vertex_count() const { return this->size(); }
444 /** In a base class, will be called after PLY_export_get_vertex_count() once for
445  * each exported point.
446  * \param pt_color Will be nullptr if the loaded file does not provide color
447  * info.
448  */
450  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
451  mrpt::img::TColorf& pt_color) const
452 {
453  MRPT_UNUSED_PARAM(pt_color);
454  pt_has_color = false;
455 
456  pt.x = m_xs[idx];
457  pt.y = m_ys[idx];
458  pt.z = m_zs[idx];
459 }
void render() const override
Render.
Definition: CPointCloud.cpp:67
void insertPoint(float x, float y, float z)
Adds a new point to the cloud.
GLdouble GLdouble z
Definition: glext.h:3879
#define min(a, b)
float OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL_value
Definition: CPointCloud.cpp:26
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLAPI void GLAPIENTRY glPointSize(GLfloat size)
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
size_t OCTREE_RENDER_MAX_POINTS_PER_NODE()
Default value = 1e5.
Definition: CPointCloud.cpp:39
This file implements several operations that operate element-wise on individual or pairs of container...
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:40
STL namespace.
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
enum mrpt::opengl::CPointCloud::Axis colNone
void clear()
Empty the list of points.
#define GL_LIGHTING
Definition: glew.h:386
void setGradientColors(const mrpt::img::TColorf &colorMin, const mrpt::img::TColorf &colorMax)
Sets the colors used as extremes when colorFromDepth is enabled.
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
unsigned char uint8_t
Definition: rptypes.h:44
Virtual base class for "schematic archives" (JSON, XML,...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
void OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL(float value)
Default value = 0.01 points/px^2.
Definition: CPointCloud.cpp:33
Information about the rendering process being issued.
Definition: gl_utils.h:31
#define GL_POINT_SMOOTH
Definition: glew.h:364
GLAPI void GLAPIENTRY glBegin(GLenum mode)
size_t OCTREE_RENDER_MAX_POINTS_PER_NODE_value
Definition: CPointCloud.cpp:27
#define GL_BLEND
Definition: glew.h:433
#define GL_POINTS
Definition: glew.h:273
void internal_render_one_point(size_t i) const
void minimum_maximum(const std::vector< T > &V, T &curMin, T &curMax)
Return the maximum and minimum values of a std::vector.
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
GLAPI void GLAPIENTRY glColor4ub(GLubyte red, GLubyte green, GLubyte blue, GLubyte alpha)
void PLY_import_set_vertex_count(const size_t N) override
In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex.
void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::img::TColorf *pt_color=nullptr) override
In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
__int32 int32_t
Definition: rptypes.h:49
#define GL_SRC_ALPHA
Definition: glew.h:287
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void render_subset(const bool all, const std::vector< size_t > &idxs, const float render_area_sqpixels) const
Render a subset of points (required by octree renderer)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void markAllPointsAsNew()
Do needed internal work if all points are new (octree rebuilt,...)
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
A RGB color - floats in the range [0,1].
Definition: TColor.h:77
GLuint in
Definition: glext.h:7391
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
void getCurrentRenderingInfo(TRenderInfo &ri)
Gather useful information on the render parameters.
Definition: gl_utils.cpp:228
size_t PLY_export_get_vertex_count() const override
In a base class, return the number of vertices.
void PLY_export_get_vertex(const size_t idx, mrpt::math::TPoint3Df &pt, bool &pt_has_color, mrpt::img::TColorf &pt_color) const override
In a base class, will be called after PLY_export_get_vertex_count() once for each exported point...
GLAPI void GLAPIENTRY glEnd(void)
void setPoint(size_t i, const float x, const float y, const float z)
Write an individual point (checks for "i" in the valid range only in Debug).
GLenum GLint GLint y
Definition: glext.h:3542
GLsizei const GLfloat * value
Definition: glext.h:4134
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
GLsizeiptr size
Definition: glext.h:3934
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
GLenum GLint x
Definition: glext.h:3542
GLAPI void GLAPIENTRY glDisable(GLenum cap)
A cloud of points, all with the same color or each depending on its value along a particular coordina...
Definition: CPointCloud.h:43
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019