Main MRPT website > C++ reference for MRPT 1.9.9
CAngularObservationMesh.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
13 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/utils/CStream.h>
16 
17 #if MRPT_HAS_OPENGL_GLUT
18 #ifdef MRPT_OS_WINDOWS
19 // Windows:
20 #include <windows.h>
21 #endif
22 
23 #ifdef MRPT_OS_APPLE
24 #include <OpenGL/gl.h>
25 #else
26 #include <GL/gl.h>
27 #endif
28 #endif
29 
30 // Include libraries in linking:
31 #if MRPT_HAS_OPENGL_GLUT && defined(MRPT_OS_WINDOWS)
32 // WINDOWS:
33 #if defined(_MSC_VER) || defined(__BORLANDC__)
34 #pragma comment(lib, "opengl32.lib")
35 #pragma comment(lib, "GlU32.lib")
36 #endif
37 #endif // MRPT_HAS_OPENGL_GLUT
38 
39 using namespace mrpt;
40 using namespace mrpt::obs;
41 using namespace mrpt::maps;
42 using namespace mrpt::math;
43 using namespace mrpt::opengl;
44 using namespace mrpt::utils;
45 using namespace mrpt::poses;
46 using namespace mrpt::math;
47 
50 
51 void CAngularObservationMesh::addTriangle(
52  const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3) const
53 {
54  const TPoint3D* arr[3] = {&p1, &p2, &p3};
56  for (size_t i = 0; i < 3; i++)
57  {
58  t.x[i] = arr[i]->x;
59  t.y[i] = arr[i]->y;
60  t.z[i] = arr[i]->z;
61  t.r[i] = m_color.R * (1.f / 255);
62  t.g[i] = m_color.G * (1.f / 255);
63  t.b[i] = m_color.B * (1.f / 255);
64  t.a[i] = m_color.A * (1.f / 255);
65  }
66  triangles.push_back(t);
67  CRenderizableDisplayList::notifyChange();
68 }
69 
70 void CAngularObservationMesh::updateMesh() const
71 {
72  CRenderizableDisplayList::notifyChange();
73 
74  size_t numRows = scanSet.size();
75  triangles.clear();
76  if (numRows <= 1)
77  {
78  actualMesh.setSize(0, 0);
79  validityMatrix.setSize(0, 0);
80  meshUpToDate = true;
81  return;
82  }
83  if (pitchBounds.size() != numRows && pitchBounds.size() != 2) return;
84  size_t numCols = scanSet[0].scan.size();
85  actualMesh.setSize(numRows, numCols);
86  validityMatrix.setSize(numRows, numCols);
87  double* pitchs = new double[numRows];
88  if (pitchBounds.size() == 2)
89  {
90  double p1 = pitchBounds[0];
91  double p2 = pitchBounds[1];
92  for (size_t i = 0; i < numRows; i++)
93  pitchs[i] = p1 +
94  (p2 - p1) * static_cast<double>(i) /
95  static_cast<double>(numRows - 1);
96  }
97  else
98  for (size_t i = 0; i < numRows; i++) pitchs[i] = pitchBounds[i];
99  const bool rToL = scanSet[0].rightToLeft;
100  for (size_t i = 0; i < numRows; i++)
101  {
102  const std::vector<float>& scan = scanSet[i].scan;
103  const std::vector<char> valid = scanSet[i].validRange;
104  const double pitchIncr = scanSet[i].deltaPitch;
105  const double aperture = scanSet[i].aperture;
106  const CPose3D origin = scanSet[i].sensorPose;
107  // This is not an error...
108  for (size_t j = 0; j < numCols; j++)
109  if ((validityMatrix(i, j) = (valid[j] != 0)))
110  {
111  double pYaw = aperture * ((static_cast<double>(j) /
112  static_cast<double>(numCols - 1)) -
113  0.5);
114  // Without the pitch since it's already within each sensorPose:
115  actualMesh(i, j) =
116  (origin +
117  CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
118  CPoint3D(scan[j], 0, 0);
119  }
120  }
121  delete[] pitchs;
122  triangles.reserve(2 * (numRows - 1) * (numCols - 1));
123  for (size_t k = 0; k < numRows - 1; k++)
124  {
125  for (size_t j = 0; j < numCols - 1; j++)
126  {
127  int b1 = validityMatrix(k, j) ? 1 : 0;
128  int b2 = validityMatrix(k, j + 1) ? 1 : 0;
129  int b3 = validityMatrix(k + 1, j) ? 1 : 0;
130  int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
131  switch (b1 + b2 + b3 + b4)
132  {
133  case 0:
134  case 1:
135  case 2:
136  break;
137  case 3:
138  if (!b1)
139  addTriangle(
140  actualMesh(k, j + 1), actualMesh(k + 1, j),
141  actualMesh(k + 1, j + 1));
142  else if (!b2)
143  addTriangle(
144  actualMesh(k, j), actualMesh(k + 1, j),
145  actualMesh(k + 1, j + 1));
146  else if (!b3)
147  addTriangle(
148  actualMesh(k, j), actualMesh(k, j + 1),
149  actualMesh(k + 1, j + 1));
150  else if (!b4)
151  addTriangle(
152  actualMesh(k, j), actualMesh(k, j + 1),
153  actualMesh(k + 1, j));
154  break;
155  case 4:
156  addTriangle(
157  actualMesh(k, j), actualMesh(k, j + 1),
158  actualMesh(k + 1, j));
159  addTriangle(
160  actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
161  actualMesh(k + 1, j));
162  }
163  }
164  }
165  meshUpToDate = true;
166 }
167 
168 void CAngularObservationMesh::render_dl() const
169 {
170 #if MRPT_HAS_OPENGL_GLUT
171  if (mEnableTransparency)
172  {
175  }
176  else
177  {
180  }
183  if (!meshUpToDate) updateMesh();
184  if (!mWireframe) glBegin(GL_TRIANGLES);
185  for (size_t i = 0; i < triangles.size(); i++)
186  {
187  const CSetOfTriangles::TTriangle& t = triangles[i];
188  float ax = t.x[1] - t.x[0];
189  float bx = t.x[2] - t.x[0];
190  float ay = t.y[1] - t.y[0];
191  float by = t.y[2] - t.y[0];
192  float az = t.z[1] - t.z[0];
193  float bz = t.z[2] - t.z[0];
194  glNormal3f(ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);
195  if (mWireframe) glBegin(GL_LINE_LOOP);
196  for (int i = 0; i < 3; i++)
197  {
198  glColor4f(t.r[i], t.g[i], t.b[i], t.a[i]);
199  glVertex3f(t.x[i], t.y[i], t.z[i]);
200  }
201  if (mWireframe) glEnd();
202  }
203  if (!mWireframe) glEnd();
206 #endif
207 }
208 
210  const mrpt::poses::CPose3D& o, double& dist) const
211 {
213  MRPT_UNUSED_PARAM(dist);
214  // TODO: redo
215  return false;
216 }
217 
218 bool CAngularObservationMesh::setScanSet(
219  const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
220 {
221  CRenderizableDisplayList::notifyChange();
222 
223  // Returns false if the scan is inconsistent
224  if (scans.size() > 0)
225  {
226  size_t setSize = scans[0].scan.size();
227  bool rToL = scans[0].rightToLeft;
229  scans.begin() + 1;
230  it != scans.end(); ++it)
231  {
232  if (it->scan.size() != setSize) return false;
233  if (it->rightToLeft != rToL) return false;
234  }
235  }
236  scanSet = scans;
237  meshUpToDate = false;
238  return true;
239 }
240 
241 void CAngularObservationMesh::setPitchBounds(
242  const double initial, const double final)
243 {
244  CRenderizableDisplayList::notifyChange();
245 
246  pitchBounds.clear();
247  pitchBounds.push_back(initial);
248  pitchBounds.push_back(final);
249  meshUpToDate = false;
250 }
251 void CAngularObservationMesh::setPitchBounds(const std::vector<double>& bounds)
252 {
253  CRenderizableDisplayList::notifyChange();
254 
255  pitchBounds = bounds;
256  meshUpToDate = false;
257 }
258 void CAngularObservationMesh::getPitchBounds(
259  double& initial, double& final) const
260 {
261  initial = pitchBounds.front();
262  final = pitchBounds.back();
263 }
264 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds) const
265 {
266  bounds = pitchBounds;
267 }
268 void CAngularObservationMesh::getScanSet(
269  std::vector<CObservation2DRangeScan>& scans) const
270 {
271  scans = scanSet;
272 }
273 
274 void CAngularObservationMesh::generateSetOfTriangles(
275  CSetOfTriangles::Ptr& res) const
276 {
277  if (!meshUpToDate) updateMesh();
278  res->insertTriangles(triangles.begin(), triangles.end());
279  // for (vector<CSetOfTriangles::TTriangle>::iterator
280  // it=triangles.begin();it!=triangles.end();++it) res->insertTriangle(*it);
281 }
282 
284 {
288  {
289  m->insertObservation(&obj);
290  }
291 };
292 
293 void CAngularObservationMesh::generatePointCloud(CPointsMap* out_map) const
294 {
295  ASSERT_(out_map);
296  out_map->clear();
297  /* size_t numRows=scanSet.size();
298  if ((pitchBounds.size()!=numRows)&&(pitchBounds.size()!=2)) return;
299  std::vector<double> pitchs(numRows);
300  if (pitchBounds.size()==2) {
301  double p1=pitchBounds[0];
302  double p2=pitchBounds[1];
303  for (size_t i=0;i<numRows;i++)
304  pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
305  } else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
306  for (size_t i=0;i<numRows;i++) out_map->insertObservation(&scanSet[i]);
307  */
308 
309  std::for_each(
310  scanSet.begin(), scanSet.end(), CAngularObservationMesh_fnctr(out_map));
311 }
312 
313 /*---------------------------------------------------------------
314  Implements the writing to a CStream capability of
315  CSerializable objects
316  ---------------------------------------------------------------*/
317 void CAngularObservationMesh::writeToStream(
318  mrpt::utils::CStream& out, int* version) const
319 {
320  if (version)
321  *version = 0;
322  else
323  {
324  writeToStreamRender(out);
325  // Version 0:
326  out << pitchBounds << scanSet << mWireframe << mEnableTransparency;
327  }
328 }
329 
330 /*---------------------------------------------------------------
331  Implements the reading from a CStream capability of
332  CSerializable objects
333  ---------------------------------------------------------------*/
334 void CAngularObservationMesh::readFromStream(
335  mrpt::utils::CStream& in, int version)
336 {
337  switch (version)
338  {
339  case 0:
340  readFromStreamRender(in);
341  in >> pitchBounds >> scanSet >> mWireframe >> mEnableTransparency;
342  break;
343  default:
345  };
346  meshUpToDate = false;
347 }
348 
350  std::vector<double>& vals) const
351 {
352  double value = initialValue();
353  double incr = increment();
354  size_t am = amount();
355  vals.resize(am);
356  for (size_t i = 0; i < am - 1; i++, value += incr) vals[i] = value;
357  vals[am - 1] = finalValue();
358 }
359 
360 void CAngularObservationMesh::getTracedRays(CSetOfLines::Ptr& res) const
361 {
362  if (!meshUpToDate) updateMesh();
363  size_t count = 0;
364  for (size_t i = 0; i < validityMatrix.getRowCount(); i++)
365  for (size_t j = 0; j < validityMatrix.getColCount(); j++)
366  if (validityMatrix(i, j)) count++;
367  res->reserve(count);
368  for (size_t i = 0; i < actualMesh.getRowCount(); i++)
369  for (size_t j = 0; j < actualMesh.getColCount(); j++)
370  if (validityMatrix(i, j))
371  res->appendLine(
372  TPose3D(scanSet[i].sensorPose), actualMesh(i, j));
373 }
374 
376 {
377  public:
379  const CPoint3D& pDist;
380  std::vector<double> pitchs;
382  CSetOfLines::Ptr& l, const CPoint3D& p, const std::vector<double>& pi)
383  : lins(l), pDist(p), pitchs()
384  {
385  pitchs.reserve(pi.size());
386  for (std::vector<double>::const_reverse_iterator it = pi.rbegin();
387  it != pi.rend(); ++it)
388  pitchs.push_back(*it);
389  }
391  {
392  size_t hm = obs.scan.size();
394  it != obs.validRange.end(); ++it)
395  if (*it) hm--;
396  lins->reserve(hm);
397  for (size_t i = 0; i < obs.scan.size(); i++)
398  if (!obs.validRange[i])
399  {
400  double yaw =
401  obs.aperture * ((static_cast<double>(i) /
402  static_cast<double>(obs.scan.size() - 1)) -
403  0.5);
404  lins->appendLine(
405  TPoint3D(obs.sensorPose),
406  TPoint3D(
407  obs.sensorPose +
408  CPose3D(
409  0, 0, 0, obs.rightToLeft ? yaw : -yaw,
410  obs.deltaPitch * i + pitchs.back(), 0) +
411  pDist));
412  }
413  pitchs.pop_back();
414  }
415 };
416 void CAngularObservationMesh::getUntracedRays(
417  CSetOfLines::Ptr& res, double dist) const
418 {
419  for_each(
420  scanSet.begin(), scanSet.end(),
421  FAddUntracedLines(res, dist, pitchBounds));
422 }
423 
425 {
426  TPolygon3D res(3);
427  for (size_t i = 0; i < 3; i++)
428  {
429  res[i].x = t.x[i];
430  res[i].y = t.y[i];
431  res[i].z = t.z[i];
432  }
433  return res;
434 }
435 
436 void CAngularObservationMesh::generateSetOfTriangles(
437  std::vector<TPolygon3D>& res) const
438 {
439  if (!meshUpToDate) updateMesh();
440  res.resize(triangles.size());
441  transform(
442  triangles.begin(), triangles.end(), res.begin(), createFromTriangle);
443 }
444 
445 void CAngularObservationMesh::getBoundingBox(
446  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
447 {
448  if (!meshUpToDate) updateMesh();
449 
450  bb_min = mrpt::math::TPoint3D(
451  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
452  std::numeric_limits<double>::max());
453  bb_max = mrpt::math::TPoint3D(
454  -std::numeric_limits<double>::max(),
455  -std::numeric_limits<double>::max(),
456  -std::numeric_limits<double>::max());
457 
458  for (size_t i = 0; i < triangles.size(); i++)
459  {
460  const CSetOfTriangles::TTriangle& t = triangles[i];
461 
462  keep_min(bb_min.x, t.x[0]);
463  keep_max(bb_max.x, t.x[0]);
464  keep_min(bb_min.y, t.y[0]);
465  keep_max(bb_max.y, t.y[0]);
466  keep_min(bb_min.z, t.z[0]);
467  keep_max(bb_max.z, t.z[0]);
468 
469  keep_min(bb_min.x, t.x[1]);
470  keep_max(bb_max.x, t.x[1]);
471  keep_min(bb_min.y, t.y[1]);
472  keep_max(bb_max.y, t.y[1]);
473  keep_min(bb_min.z, t.z[1]);
474  keep_max(bb_max.z, t.z[1]);
475 
476  keep_min(bb_min.x, t.x[2]);
477  keep_max(bb_max.x, t.x[2]);
478  keep_min(bb_min.y, t.y[2]);
479  keep_max(bb_max.y, t.y[2]);
480  keep_min(bb_min.z, t.z[2]);
481  keep_max(bb_max.z, t.z[2]);
482  }
483 
484  // Convert to coordinates of my parent:
485  m_pose.composePoint(bb_min, bb_min);
486  m_pose.composePoint(bb_max, bb_max);
487 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
GLuint GLuint GLsizei count
Definition: glext.h:3528
void operator()(const CObservation2DRangeScan &obs)
A mesh built from a set of 2D laser scan observations.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
GLdouble GLdouble t
Definition: glext.h:3689
GLbyte GLbyte bz
Definition: glext.h:6105
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3582
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:2582
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
#define GL_TRIANGLES
Definition: glew.h:276
const Scalar * const_iterator
Definition: eigen_plugins.h:27
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
#define GL_COLOR_MATERIAL
Definition: glew.h:392
#define GL_DEPTH_TEST
Definition: glew.h:401
IMPLEMENTS_SERIALIZABLE(CAngularObservationMesh, CRenderizableDisplayList, mrpt::opengl) void CAngularObservationMesh
#define GL_SMOOTH
Definition: glew.h:635
std::shared_ptr< CSetOfTriangles > Ptr
GLAPI void GLAPIENTRY glShadeModel(GLenum mode)
#define GL_LIGHTING
Definition: glew.h:385
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:41
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:63
std::vector< double > pitchs
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
FAddUntracedLines(CSetOfLines::Ptr &l, const CPoint3D &p, const std::vector< double > &pi)
This namespace contains representation of robot actions and observations.
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
double x
X,Y,Z coordinates.
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:432
#define GL_LINE_LOOP
Definition: glew.h:274
A class used to store a 3D point.
Definition: CPoint3D.h:32
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: bits.h:227
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
#define GL_SRC_ALPHA
Definition: glew.h:286
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
TPolygon3D createFromTriangle(const CSetOfTriangles::TTriangle &t)
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: bits.h:220
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
#define ASSERT_(f)
void operator()(const CObservation2DRangeScan &obj)
GLAPI void GLAPIENTRY glEnd(void)
std::shared_ptr< CSetOfLines > Ptr
Definition: CSetOfLines.h:37
GLbyte by
Definition: glext.h:6105
GLsizei const GLfloat * value
Definition: glext.h:4117
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
GLuint res
Definition: glext.h:7268
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters.
Lightweight 3D point.
GLuint GLenum GLenum transform
Definition: glext.h:6975
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLfloat GLfloat p
Definition: glext.h:6305
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:95
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
3D polygon, inheriting from std::vector<TPoint3D>
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019