MRPT  1.9.9
CMesh.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/img/color_maps.h>
14 #include <mrpt/opengl/CMesh.h>
16 #include <mrpt/poses/CPose3D.h>
18 #include <Eigen/Dense>
19 #include "opengl_internals.h"
20 
21 using namespace mrpt;
22 using namespace mrpt::opengl;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace std;
26 using mrpt::img::CImage;
27 
29 
31  bool enableTransparency, float xMin_p, float xMax_p, float yMin_p,
32  float yMax_p)
33  : m_textureImage(0, 0),
34  m_enableTransparency(enableTransparency),
35 
36  Z(0, 0),
37  mask(0, 0),
38  U(0, 0),
39  V(0, 0),
40  C(0, 0),
41  C_r(0, 0),
42  C_g(0, 0),
43  C_b(0, 0),
44 
45  xMin(xMin_p),
46  xMax(xMax_p),
47  yMin(yMin_p),
48  yMax(yMax_p)
49 
50 {
51  m_color.A = 255;
52  m_color.R = 0;
53  m_color.G = 0;
54  m_color.B = 150;
55 }
56 
57 CMesh::~CMesh() = default;
59 {
61 
62  // Remember:
63  /** List of triangles in the mesh */
64  // mutable
65  // std::vector<std::pair<CSetOfTriangles::TTriangle,TTriangleVertexIndices>
66  // > actualMesh;
67  /** The accumulated normals & counts for each vertex, so normals can be
68  * averaged. */
69  // mutable std::vector<std::pair<mrpt::math::TPoint3D,size_t> >
70  // vertex_normals;
71 
72  const auto cols = Z.cols();
73  const auto rows = Z.rows();
74 
75  actualMesh.clear();
76  if (cols == 0 && rows == 0) return; // empty mesh
77 
78  ASSERT_(cols > 0 && rows > 0);
79  ASSERT_(xMax > xMin && yMax > yMin);
80 
81  // we have 1 more row & col of vertices than of triangles:
82  vertex_normals.assign(
83  (1 + cols) * (1 + rows),
84  std::pair<TPoint3D, size_t>(TPoint3D(0, 0, 0), 0));
85 
86  float cR[3], cG[3], cB[3], cA[3];
87  cA[0] = cA[1] = cA[2] = m_color.A / 255.f;
88 
89  if ((m_colorFromZ) || (m_isImage))
90  {
91  updateColorsMatrix();
92  }
93  else
94  {
95  cR[0] = cR[1] = cR[2] = m_color.R / 255.f;
96  cG[0] = cG[1] = cG[2] = m_color.G / 255.f;
97  cB[0] = cB[1] = cB[2] = m_color.B / 255.f;
98  }
99 
100  bool useMask = false;
101  if (mask.cols() != 0 && mask.rows() != 0)
102  {
103  ASSERT_(mask.cols() == cols && mask.rows() == rows);
104  useMask = true;
105  }
106  const float sCellX = (xMax - xMin) / (rows - 1);
107  const float sCellY = (yMax - yMin) / (cols - 1);
108 
110  for (int iX = 0; iX < rows - 1; iX++)
111  for (int iY = 0; iY < cols - 1; iY++)
112  {
113  if (useMask && (!mask(iX, iY) || !mask(iX + 1, iY + 1))) continue;
114  tri.x[0] = xMin + iX * sCellX;
115  tri.y[0] = yMin + iY * sCellY;
116  tri.z[0] = Z(iX, iY);
117  tri.x[2] = tri.x[0] + sCellX;
118  tri.y[2] = tri.y[0] + sCellY;
119  tri.z[2] = Z(iX + 1, iY + 1);
120 
121  // Vertex indices:
123  tvi.vind[0] = iX + rows * iY;
124  tvi.vind[2] = (iX + 1) + rows * (iY + 1);
125 
126  // Each quadrangle has up to 2 triangles:
127  // [0]
128  // |
129  // |
130  // [1]--[2]
131  // Order: 0,1,2
132  if (!useMask || mask(iX + 1, iY))
133  {
134  tri.x[1] = tri.x[2];
135  tri.y[1] = tri.y[0];
136  tri.z[1] = Z(iX + 1, iY);
137  for (int i = 0; i < 3; i++)
138  tri.a[i] = cA[i]; // Assign alpha channel
139 
140  if (m_colorFromZ)
141  {
142  colormap(
143  m_colorMap, C(iX, iY), tri.r[0], tri.g[0], tri.b[0]);
144  colormap(
145  m_colorMap, C(iX + 1, iY), tri.r[1], tri.g[1],
146  tri.b[1]);
147  colormap(
148  m_colorMap, C(iX + 1, iY + 1), tri.r[2], tri.g[2],
149  tri.b[2]);
150  }
151  else if (m_isImage)
152  {
153  if (m_textureImage.isColor())
154  {
155  tri.r[0] = tri.r[1] = tri.r[2] = C_r(iX, iY);
156  tri.g[0] = tri.g[1] = tri.g[2] = C_g(iX, iY);
157  tri.b[0] = tri.b[1] = tri.b[2] = C_b(iX, iY);
158  }
159  else
160  {
161  tri.r[0] = tri.r[1] = tri.r[2] = C(iX, iY);
162  tri.g[0] = tri.g[1] = tri.g[2] = C(iX, iY);
163  tri.b[0] = tri.b[1] = tri.b[2] = C(iX, iY);
164  }
165  }
166  else
167  {
168  tri.r[0] = tri.r[1] = tri.r[2] = m_color.R / 255.f;
169  tri.g[0] = tri.g[1] = tri.g[2] = m_color.G / 255.f;
170  tri.b[0] = tri.b[1] = tri.b[2] = m_color.B / 255.f;
171  }
172 
173  // Compute normal of this triangle, and add it up to the 3
174  // neighboring vertices:
175  // A = P1 - P0, B = P2 - P0
176  float ax = tri.x[1] - tri.x[0];
177  float bx = tri.x[2] - tri.x[0];
178  float ay = tri.y[1] - tri.y[0];
179  float by = tri.y[2] - tri.y[0];
180  float az = tri.z[1] - tri.z[0];
181  float bz = tri.z[2] - tri.z[0];
182  const TPoint3D this_normal(
183  ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);
184 
185  // Vertex indices:
186  tvi.vind[1] = iX + 1 + rows * iY;
187 
188  // Add triangle:
189  actualMesh.emplace_back(tri, tvi);
190 
191  // For averaging normals:
192  for (unsigned long k : tvi.vind)
193  {
194  vertex_normals[k].first += this_normal;
195  vertex_normals[k].second++;
196  }
197  }
198  // 2:
199  // [0]--[1->2]
200  // \ |
201  // \|
202  // [2->1]
203  // Order: 0,2,1
204  if (!useMask || mask(iX, iY + 1))
205  {
206  tri.x[1] = tri.x[2];
207  tri.y[1] = tri.y[2];
208  tri.z[1] = tri.z[2];
209 
210  tri.x[2] = tri.x[0];
211  // tri.y[2]=tri.y[1];
212  tri.z[2] = Z(iX, iY + 1);
213  if (m_colorFromZ)
214  {
215  colormap(
216  m_colorMap, C(iX, iY), tri.r[0], tri.g[0], tri.b[0]);
217  colormap(
218  m_colorMap, C(iX + 1, iY + 1), tri.r[1], tri.g[1],
219  tri.b[1]);
220  colormap(
221  m_colorMap, C(iX, iY + 1), tri.r[2], tri.g[2],
222  tri.b[2]);
223  }
224  else if (m_isImage)
225  {
226  if (m_textureImage.isColor())
227  {
228  tri.r[0] = tri.r[1] = tri.r[2] = C_r(iX, iY);
229  tri.g[0] = tri.g[1] = tri.g[2] = C_g(iX, iY);
230  tri.b[0] = tri.b[1] = tri.b[2] = C_b(iX, iY);
231  }
232  else
233  {
234  tri.r[0] = tri.r[1] = tri.r[2] = C(iX, iY);
235  tri.g[0] = tri.g[1] = tri.g[2] = C(iX, iY);
236  tri.b[0] = tri.b[1] = tri.b[2] = C(iX, iY);
237  }
238  }
239  else
240  {
241  tri.r[0] = tri.r[1] = tri.r[2] = m_color.R / 255.f;
242  tri.g[0] = tri.g[1] = tri.g[2] = m_color.G / 255.f;
243  tri.b[0] = tri.b[1] = tri.b[2] = m_color.B / 255.f;
244  }
245 
246  // Compute normal of this triangle, and add it up to the 3
247  // neighboring vertices:
248  // A = P1 - P0, B = P2 - P0
249  float ax = tri.x[1] - tri.x[0];
250  float bx = tri.x[2] - tri.x[0];
251  float ay = tri.y[1] - tri.y[0];
252  float by = tri.y[2] - tri.y[0];
253  float az = tri.z[1] - tri.z[0];
254  float bz = tri.z[2] - tri.z[0];
255  const TPoint3D this_normal(
256  ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);
257 
258  // Vertex indices:
259  tvi.vind[1] = tvi.vind[2];
260  tvi.vind[2] = iX + rows * (iY + 1);
261 
262  // Add triangle:
263  actualMesh.emplace_back(tri, tvi);
264 
265  // For averaging normals:
266  for (unsigned long k : tvi.vind)
267  {
268  vertex_normals[k].first += this_normal;
269  vertex_normals[k].second++;
270  }
271  }
272  }
273 
274  // Average normals:
275  for (auto& vertex_normal : vertex_normals)
276  {
277  const size_t N = vertex_normal.second;
278  if (N > 0) vertex_normal.first *= 1.0 / N;
279  }
280 
281  trianglesUpToDate = true;
282  polygonsUpToDate = false;
283 }
284 
285 /*---------------------------------------------------------------
286  render
287  ---------------------------------------------------------------*/
288 void CMesh::render_dl() const
289 {
290 #if MRPT_HAS_OPENGL_GLUT
291  if (m_enableTransparency)
292  {
293  glDisable(GL_DEPTH_TEST);
294  glEnable(GL_BLEND);
295  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
296  }
297  else
298  {
299  glEnable(GL_DEPTH_TEST);
300  glDisable(GL_BLEND);
301  }
302  glEnable(GL_NORMALIZE); // So the GPU normalizes the normals instead of
303  // doing it in the CPU
304  glEnable(GL_COLOR_MATERIAL);
305  glShadeModel(GL_SMOOTH);
306  if (!trianglesUpToDate) updateTriangles();
307  if (!m_isWireFrame) glBegin(GL_TRIANGLES);
308  for (auto& i : actualMesh)
309  {
310  const CSetOfTriangles::TTriangle& t = i.first;
311  const TTriangleVertexIndices& tvi = i.second;
312 
313  if (m_isWireFrame)
314  {
315  glDisable(GL_LIGHTING); // Disable lights when drawing lines
316  glBegin(GL_LINE_LOOP);
317  }
318  for (int k = 0; k < 3; k++)
319  {
320  const mrpt::math::TPoint3D& n = vertex_normals[tvi.vind[k]].first;
321  glNormal3f(n.x, n.y, n.z);
322  glColor4f(t.r[k], t.g[k], t.b[k], t.a[k]);
323  glVertex3f(t.x[k], t.y[k], t.z[k]);
324  }
325  if (m_isWireFrame)
326  {
327  glEnd();
328  glEnable(GL_LIGHTING);
329  }
330  }
331  if (!m_isWireFrame) glEnd();
332  glDisable(GL_BLEND);
333  glDisable(GL_NORMALIZE);
334  glEnable(GL_DEPTH_TEST);
335 #endif
336 }
337 
338 /*---------------------------------------------------------------
339  assignImage
340  ---------------------------------------------------------------*/
341 void CMesh::assignImage(const CImage& img)
342 {
343  MRPT_START
344 
345  // Make a copy:
346  m_textureImage = img;
347 
348  // Delete content in Z
349  Z.setZero(img.getHeight(), img.getWidth());
350 
351  m_modified_Image = true;
352  m_enableTransparency = false;
353  m_colorFromZ = false;
354  m_isImage = true;
355  trianglesUpToDate = false;
356 
358 
359  MRPT_END
360 }
361 
363  const CImage& img, const mrpt::math::CMatrixDynamic<float>& in_Z)
364 {
365  MRPT_START
366 
367  ASSERT_(
368  (img.getWidth() == static_cast<size_t>(in_Z.cols())) &&
369  (img.getHeight() == static_cast<size_t>(in_Z.rows())));
370 
371  Z = in_Z;
372 
373  // Make a copy:
374  m_textureImage = img;
375 
376  m_modified_Image = true;
377  m_enableTransparency = false;
378  m_colorFromZ = false;
379  m_isImage = true;
380  trianglesUpToDate = false;
381 
383 
384  MRPT_END
385 }
386 
387 uint8_t CMesh::serializeGetVersion() const { return 1; }
389 {
390  writeToStreamRender(out);
391 
392  // Version 0:
393  out << m_textureImage;
394  out << xMin << xMax << yMin << yMax;
395  out << Z << U << V << mask; // We don't need to serialize C, it's computed
396  out << m_enableTransparency;
397  out << m_colorFromZ;
398  // new in v1
399  out << m_isWireFrame;
400  out << int16_t(m_colorMap);
401 }
402 
404 {
405  switch (version)
406  {
407  case 0:
408  case 1:
409  {
410  readFromStreamRender(in);
411 
412  in >> m_textureImage;
413 
414  in >> xMin;
415  in >> xMax;
416  in >> yMin;
417  in >> yMax;
418 
419  in >> Z >> U >> V >> mask;
420  in >> m_enableTransparency;
421  in >> m_colorFromZ;
422 
423  if (version >= 1)
424  {
425  in >> m_isWireFrame;
426  int16_t i;
427  in >> i;
428  m_colorMap = mrpt::img::TColormap(i);
429  }
430  else
431  m_isWireFrame = false;
432 
433  m_modified_Z = true;
434  }
435  trianglesUpToDate = false;
436  break;
437  default:
439  };
440  trianglesUpToDate = false;
442 }
443 
445 {
446  if ((!m_modified_Z) && (!m_modified_Image)) return;
447 
449 
450  if (m_isImage)
451  {
452  const int cols = m_textureImage.getWidth();
453  const int rows = m_textureImage.getHeight();
454 
455  if ((cols != Z.cols()) || (rows != Z.rows()))
456  printf("\nTexture Image and Z sizes have to be equal");
457 
458  else if (m_textureImage.isColor())
459  {
460  C_r.setSize(rows, cols);
461  C_g.setSize(rows, cols);
462  C_b.setSize(rows, cols);
463  m_textureImage.getAsRGBMatrices(C_r, C_g, C_b);
464  }
465  else
466  {
467  C.setSize(rows, cols);
468  m_textureImage.getAsMatrix(C);
469  }
470  }
471  else
472  {
473  const size_t cols = Z.cols();
474  const size_t rows = Z.rows();
475  C.setSize(rows, cols);
476 
477  // Color is proportional to height:
478  C = Z;
479 
480  // If mask is empty -> Normalize the whole mesh
481  if (mask.empty()) mrpt::math::normalize(C, 0.01f, 0.99f);
482 
483  // Else -> Normalize color ignoring masked-out cells:
484  else
485  {
486  float val_max = -std::numeric_limits<float>::max(),
487  val_min = std::numeric_limits<float>::max();
488  bool any_valid = false;
489 
490  for (size_t c = 0; c < cols; c++)
491  for (size_t r = 0; r < rows; r++)
492  {
493  if (!mask(r, c)) continue;
494  any_valid = true;
495  const float val = C(r, c);
496  mrpt::keep_max(val_max, val);
497  mrpt::keep_min(val_min, val);
498  }
499 
500  if (any_valid)
501  {
502  float minMaxDelta = val_max - val_min;
503  if (minMaxDelta == 0) minMaxDelta = 1;
504  const float minMaxDelta_ = 1.0f / minMaxDelta;
505  C.array() = (C.array() - val_min) * minMaxDelta_;
506  }
507  }
508  }
509 
510  m_modified_Image = false;
511  m_modified_Z = false;
512  trianglesUpToDate = false;
513 }
514 
516 {
517  Z = in_Z;
518  m_modified_Z = true;
519  trianglesUpToDate = false;
520 
521  // Delete previously loaded images
522  m_isImage = false;
523 
525 }
526 
528 {
529  mask = in_mask;
530  trianglesUpToDate = false;
532 }
533 
537 {
538  U = in_U;
539  V = in_V;
541 }
542 
543 bool CMesh::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
544 {
545  if (!trianglesUpToDate || !polygonsUpToDate) updatePolygons();
546  return mrpt::math::traceRay(tmpPolys, (o - this->m_pose).asTPose(), dist);
547 }
548 
549 static math::TPolygon3D tmpPoly(3);
551  const std::pair<CSetOfTriangles::TTriangle, CMesh::TTriangleVertexIndices>&
552  p)
553 {
554  const CSetOfTriangles::TTriangle& t = p.first;
555  for (size_t i = 0; i < 3; i++)
556  {
557  tmpPoly[i].x = t.x[i];
558  tmpPoly[i].y = t.y[i];
559  tmpPoly[i].z = t.z[i];
560  }
562 }
563 
565 {
566  if (!trianglesUpToDate) updateTriangles();
567  size_t N = actualMesh.size();
568  tmpPolys.resize(N);
569  transform(
570  actualMesh.begin(), actualMesh.end(), tmpPolys.begin(),
572  polygonsUpToDate = true;
574 }
575 
578 {
579  bb_min.x = xMin;
580  bb_min.y = yMin;
581  bb_min.z = Z.minCoeff();
582 
583  bb_max.x = xMax;
584  bb_max.y = yMax;
585  bb_max.z = Z.maxCoeff();
586 
587  // Convert to coordinates of my parent:
588  m_pose.composePoint(bb_min, bb_min);
589  m_pose.composePoint(bb_max, bb_max);
590 }
~CMesh() override
Private, virtual destructor: only can be deleted from smart pointers.
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
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 colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
Definition: color_maps.cpp:114
TColormap
Different colormaps for use in mrpt::img::colormap()
Definition: color_maps.h:29
#define MRPT_START
Definition: exceptions.h:241
void assignImageAndZ(const mrpt::img::CImage &img, const mrpt::math::CMatrixDynamic< float > &in_Z)
Assigns a texture image and Z simultaneously, and disable transparency.
Definition: CMesh.cpp:362
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CMesh.cpp:387
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:2565
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Slightly heavyweight type to speed-up calculations with polygons in 3D.
Definition: geometry.h:33
This file implements several operations that operate element-wise on individual or pairs of container...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CMesh.cpp:403
void setUV(const mrpt::math::CMatrixDynamic< float > &in_U, const mrpt::math::CMatrixDynamic< float > &in_V)
Sets the (u,v) texture coordinates (in range [0,1]) for each cell.
Definition: CMesh.cpp:534
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:845
void render_dl() const override
Render.
Definition: CMesh.cpp:288
STL namespace.
void setMask(const mrpt::math::CMatrixDynamic< float > &in_mask)
This method sets the boolean mask of valid heights for each position (cell) in the mesh grid...
Definition: CMesh.cpp:527
void assignImage(const mrpt::img::CImage &img)
Assigns a texture image, and disable transparency.
Definition: CMesh.cpp:341
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CMesh.cpp:388
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::img::CImage CImage
Definition: utils/CImage.h:5
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:814
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Trace ray.
Definition: CMesh.cpp:543
int val
Definition: mrpt_jpeglib.h:957
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
void updatePolygons() const
Definition: CMesh.cpp:564
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void setZ(const mrpt::math::CMatrixDynamic< float > &in_Z)
This method sets the matrix of heights for each position (cell) in the mesh grid. ...
Definition: CMesh.cpp:515
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...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
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
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: CMesh.cpp:576
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
void updateColorsMatrix() const
Called internally to assure C is updated.
Definition: CMesh.cpp:444
const auto bb_max
mrpt::math::TPolygonWithPlane createPolygonFromTriangle(const std::pair< CSetOfTriangles::TTriangle, CMesh::TTriangleVertexIndices > &p)
Definition: CMesh.cpp:550
const auto bb_min
A planar (XY) grid where each cell has an associated height and, optionally, a texture map...
Definition: CMesh.h:36
This template class provides the basic functionality for a general 2D any-size, resizable container o...
static math::TPolygon3D tmpPoly(3)
void updateTriangles() const
Called internally to assure the triangle list is updated.
Definition: CMesh.cpp:58
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:19
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7fcfece08 Sat Feb 22 22:47:46 2020 +0100 at sáb feb 22 23:00:10 CET 2020