MRPT  2.0.4
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-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 "opengl-precomp.h" // Precompiled header
11 
12 #include <mrpt/core/round.h> // round()
17 
18 #include <mrpt/opengl/opengl_api.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 CPointCloud::CPointCloud() { markAllPointsAsNew(); }
51 
53 {
54  {
55  mrpt::math::TPoint3Df tst[2];
56  // was static_assert(), error in gcc9.1, cannot use ptr+3 in constexpr.
57  ASSERTMSG_(
58  &tst[1].x == (&tst[0].x + 3), "memory layout not as expected");
59  ASSERTMSG_(
60  &tst[1].y == (&tst[0].y + 3), "memory layout not as expected");
61  ASSERTMSG_(
62  &tst[1].z == (&tst[0].z + 3), "memory layout not as expected");
63  }
64 
65  const auto N = m_points.size();
66 
67  octree_assure_uptodate(); // Rebuild octree if needed
68  m_last_rendered_count_ongoing = 0;
69 
70  if (m_colorFromDepth != colNone)
71  {
72  if (!m_minmax_valid)
73  {
74  m_minmax_valid = true;
75  if (!m_points.empty())
76  {
77  const float* vs = m_colorFromDepth == CPointCloud::colZ
78  ? &m_points[0].z
79  : (m_colorFromDepth == CPointCloud::colY
80  ? &m_points[0].y
81  : &m_points[0].x);
82  m_min = m_max = vs[0];
83  for (size_t i = 1; i < N; i++)
84  {
85  float v = vs[3 * i];
86  if (v < m_min) m_min = v;
87  if (v > m_max) m_max = v;
88  }
89  }
90  else
91  m_max = m_min = 0;
92  }
93 
94  m_max_m_min = m_max - m_min;
95  if (std::abs(m_max_m_min) < 1e-4)
96  m_max_m_min = -1;
97  else
98  m_min = m_max - m_max_m_min * 1.01f;
99  m_max_m_min_inv = 1.0f / m_max_m_min;
100  }
101 
102  // Slopes of color interpolation:
103  m_col_slop.R = m_colorFromDepth_max.R - m_colorFromDepth_min.R;
104  m_col_slop.G = m_colorFromDepth_max.G - m_colorFromDepth_min.G;
105  m_col_slop.B = m_colorFromDepth_max.B - m_colorFromDepth_min.B;
106 
107  m_col_slop_inv.R = m_col_slop.R != 0 ? 1.0f / m_col_slop.R : 0;
108  m_col_slop_inv.G = m_col_slop.G != 0 ? 1.0f / m_col_slop.G : 0;
109  m_col_slop_inv.B = m_col_slop.B != 0 ? 1.0f / m_col_slop.B : 0;
110 
111  // TODO: Restore rendering using octrees?
112  // octree_render(*rc.state); // Render all points recursively:
113 
114  // ------------------------------
115  // Fill the shader buffers
116  // ------------------------------
117  // "CRenderizableShaderPoints::m_vertex_buffer_data" is already done, since
118  // "m_points" is an alias for it.
119 
120  // color buffer:
122  cbd.clear();
123  cbd.reserve(N);
124 
125  // color for each point:
126  if (m_colorFromDepth != colNone && m_max_m_min > 0)
127  {
128  for (size_t i = 0; i < N; i++)
129  {
130  const float depthCol =
131  (m_colorFromDepth == colX
132  ? m_points[i].x
133  : (m_colorFromDepth == colY ? m_points[i].y
134  : m_points[i].z));
135 
136  float f = (depthCol - m_min) * m_max_m_min_inv;
137  f = std::max(0.0f, min(1.0f, f));
138 
139  cbd.push_back({f2u8(m_colorFromDepth_min.R + f * m_col_slop_inv.R),
140  f2u8(m_colorFromDepth_min.G + f * m_col_slop_inv.G),
141  f2u8(m_colorFromDepth_min.B + f * m_col_slop_inv.B),
142  m_color.A});
143  }
144  }
145  else
146  {
147  // all points: same color
148  cbd.assign(N, m_color);
149  }
150 
151  m_last_rendered_count = m_last_rendered_count_ongoing;
152 }
153 
155  [maybe_unused]] size_t i) const
156 {
157 #if MRPT_HAS_OPENGL_GLUT
158 #endif
159 }
160 
161 /** Render a subset of points (required by octree renderer) */
163  [[maybe_unused]] const bool all,
164  [[maybe_unused]] const std::vector<size_t>& idxs,
165  [[maybe_unused]] const float render_area_sqpixels) const
166 {
167 #if 0 && MRPT_HAS_OPENGL_GLUT
168  // Disabled for now... (Feb 2020)
169 
170  const size_t N = (all ? m_xs.size() : idxs.size());
171  const size_t decimation = mrpt::round(std::max(
172  1.0f, d2f(
174  render_area_sqpixels))));
175 
176  m_last_rendered_count_ongoing += N / decimation;
177 
178  if (all)
179  {
180  for (size_t i = 0; i < N; i++) internal_render_one_point(i);
181  }
182  else
183  {
184  const size_t Np = idxs.size();
185  for (size_t i = 0; i < Np; i += decimation)
186  internal_render_one_point(idxs[i]);
187  }
188 #endif
189 }
192 {
194  out["colorFromDepth"] = static_cast<int32_t>(m_colorFromDepth);
195  out["pointSize"] = m_pointSize;
196  const auto N = m_points.size();
197  out["N"] = static_cast<uint64_t>(N);
198  for (size_t i = 0; i < N; i++)
199  {
200  out["xs"][i] = m_points[i].x;
201  out["ys"][i] = m_points[i].y;
202  out["zs"][i] = m_points[i].z;
203  }
204  out["colorFromDepth_min"]["R"] = m_colorFromDepth_min.R;
205  out["colorFromDepth_min"]["G"] = m_colorFromDepth_min.G;
206  out["colorFromDepth_min"]["B"] = m_colorFromDepth_min.B;
207  out["colorFromDepth_max"]["R"] = m_colorFromDepth_max.R;
208  out["colorFromDepth_max"]["G"] = m_colorFromDepth_max.G;
209  out["colorFromDepth_max"]["B"] = m_colorFromDepth_max.B;
210  out["pointSmooth"] = m_pointSmooth;
211 }
213 {
214  uint8_t version;
216  switch (version)
217  {
218  case 1:
219  {
220  /**
221  * currently below is being left to what is being set by
222  * the default constructor i.e.,CPointCloud::colNone
223  */
224  // m_colorFromDepth = static_cast<float>(in["colorDepth"]);
225  m_pointSize = static_cast<float>(in["pointSize"]);
226  const size_t N = static_cast<uint64_t>(in["N"]);
227  m_points.resize(N);
228  for (size_t i = 0; i < N; i++)
229  {
230  m_points[i].x = static_cast<float>(in["xs"][i]);
231  m_points[i].y = static_cast<float>(in["ys"][i]);
232  m_points[i].z = static_cast<float>(in["zs"][i]);
233  }
234  m_colorFromDepth_min.R =
235  static_cast<float>(in["colorFromDepth_min"]["R"]);
236  m_colorFromDepth_min.G =
237  static_cast<float>(in["colorFromDepth_min"]["G"]);
238  m_colorFromDepth_min.B =
239  static_cast<float>(in["colorFromDepth_min"]["B"]);
240  m_colorFromDepth_max.R =
241  static_cast<float>(in["colorFromDepth_max"]["R"]);
242  m_colorFromDepth_max.G =
243  static_cast<float>(in["colorFromDepth_max"]["G"]);
244  m_colorFromDepth_max.B =
245  static_cast<float>(in["colorFromDepth_max"]["B"]);
246  m_pointSmooth = static_cast<bool>(in["pointSmooth"]);
247  }
248  break;
249  default:
251  }
252 }
253 uint8_t CPointCloud::serializeGetVersion() const { return 6; }
255 {
256  writeToStreamRender(out);
257  // Changed from bool to enum/int32_t in version 3.
258  out.WriteAs<int32_t>(m_colorFromDepth);
259 
260  // out << m_xs << m_ys << m_zs;// was: v4
261  out.WriteAs<uint32_t>(m_points.size());
262  for (const auto& pt : m_points) out << pt;
263 
264  // New in version 2:
265  out << m_colorFromDepth_min.R << m_colorFromDepth_min.G
266  << m_colorFromDepth_min.B;
267  out << m_colorFromDepth_max.R << m_colorFromDepth_max.G
268  << m_colorFromDepth_max.B;
269 
270  // New in version 4:
271  out << m_pointSmooth;
272 
273  // new v6:
275 }
276 
278  mrpt::serialization::CArchive& in, uint8_t version)
279 {
280  switch (version)
281  {
282  case 0:
283  case 1:
284  case 2:
285  case 3:
286  case 4:
287  case 5:
288  case 6:
289  {
290  readFromStreamRender(in);
291  if (version >= 3)
292  {
293  int32_t axis;
294  in >> axis;
295  m_colorFromDepth = Axis(axis);
296  }
297  else
298  {
299  bool colorFromZ;
300  in >> colorFromZ;
301  m_colorFromDepth =
303  }
304  if (version < 5)
305  {
306  std::vector<float> xs, ys, zs;
307  in >> xs >> ys >> zs;
308  this->setAllPoints(xs, ys, zs);
309  }
310  else
311  {
312  // New in v5:
313  auto N = in.ReadAs<uint32_t>();
314  m_points.resize(N);
315  for (auto& pt : m_points) in >> pt;
316  }
317 
318  if (version >= 1 && version < 6)
319  in >> m_pointSize;
320  else
321  m_pointSize = 1;
322 
323  if (version >= 2)
324  {
325  in >> m_colorFromDepth_min.R >> m_colorFromDepth_min.G >>
326  m_colorFromDepth_min.B;
327  in >> m_colorFromDepth_max.R >> m_colorFromDepth_max.G >>
328  m_colorFromDepth_max.B;
329  }
330  else
331  {
332  m_colorFromDepth_min = TColorf(0, 0, 0);
333  m_colorFromDepth_max.R = m_color.R * 255.f;
334  m_colorFromDepth_max.G = m_color.G * 255.f;
335  m_colorFromDepth_max.B = m_color.B * 255.f;
336  }
337 
338  if (version >= 4)
339  in >> m_pointSmooth;
340  else
341  m_pointSmooth = false;
342 
343  if (version >= 6) CRenderizableShaderPoints::params_deserialize(in);
344  }
345  break;
346  default:
348  };
349 
350  markAllPointsAsNew();
352 }
353 
355 {
356  m_points.clear();
357  markAllPointsAsNew();
359 }
360 
361 void CPointCloud::insertPoint(float x, float y, float z)
362 {
363  m_points.emplace_back(x, y, z);
364 
365  m_minmax_valid = false;
366 
367  // JL: TODO note: Well, this can be clearly done much more efficiently
368  // but...I don't have time! :-(
369  markAllPointsAsNew();
371 }
372 
373 /** Write an individual point (checks for "i" in the valid range only in
374  * Debug).
375  */
377  size_t i, const float x, const float y, const float z)
378 {
379  m_points.at(i) = {x, y, z};
380 
381  m_minmax_valid = false;
382 
383  // JL: TODO note: Well, this can be clearly done much more efficiently
384  // but...I don't have time! :-(
385  markAllPointsAsNew();
387 }
388 
389 /*---------------------------------------------------------------
390  setGradientColors
391 ---------------------------------------------------------------*/
393  const mrpt::img::TColorf& colorMin, const mrpt::img::TColorf& colorMax)
394 {
395  m_colorFromDepth_min = colorMin;
396  m_colorFromDepth_max = colorMax;
397 }
398 
399 // Do needed internal work if all points are new (octree rebuilt,...)
401 {
402  m_minmax_valid = false;
403  octree_mark_as_outdated();
404 }
405 
406 /** In a base class, reserve memory to prepare subsequent calls to
407  * PLY_import_set_vertex */
409 {
410  this->resize(N);
411 }
412 
413 /** In a base class, will be called after PLY_import_set_vertex_count() once
414  * for each loaded point. \param pt_color Will be nullptr if the loaded file
415  * does not provide color info.
416  */
418  const size_t idx, const mrpt::math::TPoint3Df& pt,
419  [[maybe_unused]] const mrpt::img::TColorf* pt_color)
420 {
421  this->setPoint(idx, pt.x, pt.y, pt.z);
422 }
423 
424 /** In a base class, return the number of vertices */
425 size_t CPointCloud::PLY_export_get_vertex_count() const { return this->size(); }
426 /** In a base class, will be called after PLY_export_get_vertex_count() once
427  * for each exported point. \param pt_color Will be nullptr if the loaded
428  * file does not provide color info.
429  */
431  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
432  [[maybe_unused]] mrpt::img::TColorf& pt_color) const
433 {
434  pt_has_color = false;
435 
436  pt = m_points[idx];
437 }
438 
439 void CPointCloud::setAllPoints(const std::vector<mrpt::math::TPoint3D>& pts)
440 {
441  const auto N = pts.size();
442  m_points.resize(N);
443  for (size_t i = 0; i < N; i++) m_points[i] = pts[i];
444  m_minmax_valid = false;
445  markAllPointsAsNew();
447 }
void insertPoint(float x, float y, float z)
Adds a new point to the cloud.
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.
size_t size(const MATRIXLIKE &m, const int dim)
#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.
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:48
STL namespace.
void setAllPoints(const std::vector< T > &x, const std::vector< T > &y, const std::vector< T > &z)
Set the list of (X,Y,Z) point coordinates, all at once, from three vectors with their coordinates...
Definition: CPointCloud.h:133
void clear()
Empty the list of points.
void setGradientColors(const mrpt::img::TColorf &colorMin, const mrpt::img::TColorf &colorMax)
Sets the colors used as extremes when colorFromDepth is enabled.
Virtual base class for "schematic archives" (JSON, XML,...)
#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)
void params_deserialize(mrpt::serialization::CArchive &in)
This base provides a set of functions for maths stuff.
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
void OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL(float value)
Default value = 0.01 points/px^2.
Definition: CPointCloud.cpp:33
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
size_t OCTREE_RENDER_MAX_POINTS_PER_NODE_value
Definition: CPointCloud.cpp:27
void internal_render_one_point(size_t i) const
void params_serialize(mrpt::serialization::CArchive &out) const
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
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.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
uint8_t f2u8(const float f)
converts a float [0,1] into an uint8_t [0,255] (without checking for out of bounds) ...
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,...)
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
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...
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).
void onUpdateBuffers_Points() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
Definition: CPointCloud.cpp:52
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
images resize(NUM_IMGS)
A cloud of points, all with the same color or each depending on its value along a particular coordina...
Definition: CPointCloud.h:44
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:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020