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-2018, 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>
16 
17 #if MRPT_HAS_OPENGL_GLUT
18 #ifdef _WIN32
19 // Windows:
20 #include <windows.h>
21 #endif
22 
23 #ifdef __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(_WIN32)
32 // WINDOWS:
33 #if defined(_MSC_VER)
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::poses;
45 using namespace mrpt::math;
46 
49 
50 void CAngularObservationMesh::addTriangle(
51  const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3) const
52 {
53  const TPoint3D* arr[3] = {&p1, &p2, &p3};
55  for (size_t i = 0; i < 3; i++)
56  {
57  t.x[i] = arr[i]->x;
58  t.y[i] = arr[i]->y;
59  t.z[i] = arr[i]->z;
60  t.r[i] = m_color.R * (1.f / 255);
61  t.g[i] = m_color.G * (1.f / 255);
62  t.b[i] = m_color.B * (1.f / 255);
63  t.a[i] = m_color.A * (1.f / 255);
64  }
65  triangles.push_back(t);
66  CRenderizableDisplayList::notifyChange();
67 }
68 
69 void CAngularObservationMesh::updateMesh() const
70 {
71  CRenderizableDisplayList::notifyChange();
72 
73  size_t numRows = scanSet.size();
74  triangles.clear();
75  if (numRows <= 1)
76  {
77  actualMesh.setSize(0, 0);
78  validityMatrix.setSize(0, 0);
79  meshUpToDate = true;
80  return;
81  }
82  if (pitchBounds.size() != numRows && pitchBounds.size() != 2) return;
83  size_t numCols = scanSet[0].scan.size();
84  actualMesh.setSize(numRows, numCols);
85  validityMatrix.setSize(numRows, numCols);
86  double* pitchs = new double[numRows];
87  if (pitchBounds.size() == 2)
88  {
89  double p1 = pitchBounds[0];
90  double p2 = pitchBounds[1];
91  for (size_t i = 0; i < numRows; i++)
92  pitchs[i] = p1 + (p2 - p1) * static_cast<double>(i) /
93  static_cast<double>(numRows - 1);
94  }
95  else
96  for (size_t i = 0; i < numRows; i++) pitchs[i] = pitchBounds[i];
97  const bool rToL = scanSet[0].rightToLeft;
98  for (size_t i = 0; i < numRows; i++)
99  {
100  const auto& scan = scanSet[i].scan;
101  const auto& valid = scanSet[i].validRange;
102  const double pitchIncr = scanSet[i].deltaPitch;
103  const double aperture = scanSet[i].aperture;
104  const CPose3D origin = scanSet[i].sensorPose;
105  // This is not an error...
106  for (size_t j = 0; j < numCols; j++)
107  if ((validityMatrix(i, j) = (valid[j] != 0)))
108  {
109  double pYaw = aperture * ((static_cast<double>(j) /
110  static_cast<double>(numCols - 1)) -
111  0.5);
112  // Without the pitch since it's already within each sensorPose:
113  actualMesh(i, j) =
114  ((origin +
115  CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
116  CPoint3D(scan[j], 0, 0))
117  .asTPoint();
118  }
119  }
120  delete[] pitchs;
121  triangles.reserve(2 * (numRows - 1) * (numCols - 1));
122  for (size_t k = 0; k < numRows - 1; k++)
123  {
124  for (size_t j = 0; j < numCols - 1; j++)
125  {
126  int b1 = validityMatrix(k, j) ? 1 : 0;
127  int b2 = validityMatrix(k, j + 1) ? 1 : 0;
128  int b3 = validityMatrix(k + 1, j) ? 1 : 0;
129  int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
130  switch (b1 + b2 + b3 + b4)
131  {
132  case 0:
133  case 1:
134  case 2:
135  break;
136  case 3:
137  if (!b1)
138  addTriangle(
139  actualMesh(k, j + 1), actualMesh(k + 1, j),
140  actualMesh(k + 1, j + 1));
141  else if (!b2)
142  addTriangle(
143  actualMesh(k, j), actualMesh(k + 1, j),
144  actualMesh(k + 1, j + 1));
145  else if (!b3)
146  addTriangle(
147  actualMesh(k, j), actualMesh(k, j + 1),
148  actualMesh(k + 1, j + 1));
149  else if (!b4)
150  addTriangle(
151  actualMesh(k, j), actualMesh(k, j + 1),
152  actualMesh(k + 1, j));
153  break;
154  case 4:
155  addTriangle(
156  actualMesh(k, j), actualMesh(k, j + 1),
157  actualMesh(k + 1, j));
158  addTriangle(
159  actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
160  actualMesh(k + 1, j));
161  }
162  }
163  }
164  meshUpToDate = true;
165 }
166 
167 void CAngularObservationMesh::render_dl() const
168 {
169 #if MRPT_HAS_OPENGL_GLUT
170  if (mEnableTransparency)
171  {
174  }
175  else
176  {
179  }
182  if (!meshUpToDate) updateMesh();
183  if (!mWireframe) glBegin(GL_TRIANGLES);
184  for (size_t i = 0; i < triangles.size(); i++)
185  {
186  const CSetOfTriangles::TTriangle& t = triangles[i];
187  float ax = t.x[1] - t.x[0];
188  float bx = t.x[2] - t.x[0];
189  float ay = t.y[1] - t.y[0];
190  float by = t.y[2] - t.y[0];
191  float az = t.z[1] - t.z[0];
192  float bz = t.z[2] - t.z[0];
193  glNormal3f(ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);
194  if (mWireframe) glBegin(GL_LINE_LOOP);
195  for (int k = 0; k < 3; k++)
196  {
197  glColor4f(t.r[k], t.g[k], t.b[k], t.a[k]);
198  glVertex3f(t.x[k], t.y[k], t.z[k]);
199  }
200  if (mWireframe) glEnd();
201  }
202  if (!mWireframe) glEnd();
205 #endif
206 }
207 
209  const mrpt::poses::CPose3D& o, double& dist) const
210 {
212  MRPT_UNUSED_PARAM(dist);
213  // TODO: redo
214  return false;
215 }
216 
217 bool CAngularObservationMesh::setScanSet(
218  const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
219 {
220  CRenderizableDisplayList::notifyChange();
221 
222  // Returns false if the scan is inconsistent
223  if (scans.size() > 0)
224  {
225  size_t setSize = scans[0].scan.size();
226  bool rToL = scans[0].rightToLeft;
228  scans.begin() + 1;
229  it != scans.end(); ++it)
230  {
231  if (it->scan.size() != setSize) return false;
232  if (it->rightToLeft != rToL) return false;
233  }
234  }
235  scanSet = scans;
236  meshUpToDate = false;
237  return true;
238 }
239 
240 void CAngularObservationMesh::setPitchBounds(
241  const double initial, const double final)
242 {
243  CRenderizableDisplayList::notifyChange();
244 
245  pitchBounds.clear();
246  pitchBounds.push_back(initial);
247  pitchBounds.push_back(final);
248  meshUpToDate = false;
249 }
250 void CAngularObservationMesh::setPitchBounds(const std::vector<double>& bounds)
251 {
252  CRenderizableDisplayList::notifyChange();
253 
254  pitchBounds = bounds;
255  meshUpToDate = false;
256 }
257 void CAngularObservationMesh::getPitchBounds(
258  double& initial, double& final) const
259 {
260  initial = pitchBounds.front();
261  final = pitchBounds.back();
262 }
263 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds) const
264 {
265  bounds = pitchBounds;
266 }
267 void CAngularObservationMesh::getScanSet(
268  std::vector<CObservation2DRangeScan>& scans) const
269 {
270  scans = scanSet;
271 }
272 
273 void CAngularObservationMesh::generateSetOfTriangles(
274  CSetOfTriangles::Ptr& res) const
275 {
276  if (!meshUpToDate) updateMesh();
277  res->insertTriangles(triangles.begin(), triangles.end());
278  // for (vector<CSetOfTriangles::TTriangle>::iterator
279  // it=triangles.begin();it!=triangles.end();++it) res->insertTriangle(*it);
280 }
281 
283 {
287  {
288  m->insertObservation(&obj);
289  }
290 };
291 
292 void CAngularObservationMesh::generatePointCloud(CPointsMap* out_map) const
293 {
294  ASSERT_(out_map);
295  out_map->clear();
296  /* size_t numRows=scanSet.size();
297  if ((pitchBounds.size()!=numRows)&&(pitchBounds.size()!=2)) return;
298  std::vector<double> pitchs(numRows);
299  if (pitchBounds.size()==2) {
300  double p1=pitchBounds[0];
301  double p2=pitchBounds[1];
302  for (size_t i=0;i<numRows;i++)
303  pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
304  } else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
305  for (size_t i=0;i<numRows;i++) out_map->insertObservation(&scanSet[i]);
306  */
307 
308  std::for_each(
309  scanSet.begin(), scanSet.end(), CAngularObservationMesh_fnctr(out_map));
310 }
311 
312 uint8_t CAngularObservationMesh::serializeGetVersion() const { return 0; }
313 void CAngularObservationMesh::serializeTo(
315 {
316  writeToStreamRender(out);
317  // Version 0:
318  out << pitchBounds << scanSet << mWireframe << mEnableTransparency;
319 }
320 
321 void CAngularObservationMesh::serializeFrom(
323 {
324  switch (version)
325  {
326  case 0:
327  readFromStreamRender(in);
328  in >> pitchBounds >> scanSet >> mWireframe >> mEnableTransparency;
329  break;
330  default:
332  };
333  meshUpToDate = false;
334 }
335 
337  std::vector<double>& vals) const
338 {
339  double value = initialValue();
340  double incr = increment();
341  size_t am = amount();
342  vals.resize(am);
343  for (size_t i = 0; i < am - 1; i++, value += incr) vals[i] = value;
344  vals[am - 1] = finalValue();
345 }
346 
347 void CAngularObservationMesh::getTracedRays(CSetOfLines::Ptr& res) const
348 {
349  if (!meshUpToDate) updateMesh();
350  size_t count = 0;
351  for (size_t i = 0; i < validityMatrix.rows(); i++)
352  for (size_t j = 0; j < validityMatrix.cols(); j++)
353  if (validityMatrix(i, j)) count++;
354  res->reserve(count);
355  for (size_t i = 0; i < actualMesh.rows(); i++)
356  for (size_t j = 0; j < actualMesh.cols(); j++)
357  if (validityMatrix(i, j))
358  res->appendLine(
359  (scanSet[i].sensorPose).asTPose(), actualMesh(i, j));
360 }
361 
363 {
364  public:
366  const CPoint3D& pDist;
367  std::vector<double> pitchs;
369  CSetOfLines::Ptr& l, const CPoint3D& p, const std::vector<double>& pi)
370  : lins(l), pDist(p), pitchs()
371  {
372  pitchs.reserve(pi.size());
373  for (std::vector<double>::const_reverse_iterator it = pi.rbegin();
374  it != pi.rend(); ++it)
375  pitchs.push_back(*it);
376  }
378  {
379  size_t hm = obs.scan.size();
380  for (auto it = obs.validRange.begin(); it != obs.validRange.end(); ++it)
381  if (*it) hm--;
382  lins->reserve(hm);
383  for (size_t i = 0; i < obs.scan.size(); i++)
384  if (!obs.validRange[i])
385  {
386  double yaw =
387  obs.aperture * ((static_cast<double>(i) /
388  static_cast<double>(obs.scan.size() - 1)) -
389  0.5);
390  lins->appendLine(
391  obs.sensorPose.asTPose(),
392  (obs.sensorPose +
393  CPose3D(
394  0, 0, 0, obs.rightToLeft ? yaw : -yaw,
395  obs.deltaPitch * i + pitchs.back(), 0) +
396  pDist)
397  .asTPoint());
398  }
399  pitchs.pop_back();
400  }
401 };
402 void CAngularObservationMesh::getUntracedRays(
403  CSetOfLines::Ptr& res, double dist) const
404 {
405  for_each(
406  scanSet.begin(), scanSet.end(),
407  FAddUntracedLines(res, dist, pitchBounds));
408 }
409 
411 {
412  TPolygon3D res(3);
413  for (size_t i = 0; i < 3; i++)
414  {
415  res[i].x = t.x[i];
416  res[i].y = t.y[i];
417  res[i].z = t.z[i];
418  }
419  return res;
420 }
421 
422 void CAngularObservationMesh::generateSetOfTriangles(
423  std::vector<TPolygon3D>& res) const
424 {
425  if (!meshUpToDate) updateMesh();
426  res.resize(triangles.size());
427  transform(
428  triangles.begin(), triangles.end(), res.begin(), createFromTriangle);
429 }
430 
431 void CAngularObservationMesh::getBoundingBox(
432  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
433 {
434  if (!meshUpToDate) updateMesh();
435 
436  bb_min = mrpt::math::TPoint3D(
437  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
438  std::numeric_limits<double>::max());
439  bb_max = mrpt::math::TPoint3D(
440  -std::numeric_limits<double>::max(),
441  -std::numeric_limits<double>::max(),
442  -std::numeric_limits<double>::max());
443 
444  for (size_t i = 0; i < triangles.size(); i++)
445  {
446  const CSetOfTriangles::TTriangle& t = triangles[i];
447 
448  keep_min(bb_min.x, t.x[0]);
449  keep_max(bb_max.x, t.x[0]);
450  keep_min(bb_min.y, t.y[0]);
451  keep_max(bb_max.y, t.y[0]);
452  keep_min(bb_min.z, t.z[0]);
453  keep_max(bb_max.z, t.z[0]);
454 
455  keep_min(bb_min.x, t.x[1]);
456  keep_max(bb_max.x, t.x[1]);
457  keep_min(bb_min.y, t.y[1]);
458  keep_max(bb_max.y, t.y[1]);
459  keep_min(bb_min.z, t.z[1]);
460  keep_max(bb_max.z, t.z[1]);
461 
462  keep_min(bb_min.x, t.x[2]);
463  keep_max(bb_max.x, t.x[2]);
464  keep_min(bb_min.y, t.y[2]);
465  keep_max(bb_max.y, t.y[2]);
466  keep_min(bb_min.z, t.z[2]);
467  keep_max(bb_max.z, t.z[2]);
468  }
469 
470  // Convert to coordinates of my parent:
471  m_pose.composePoint(bb_min, bb_min);
472  m_pose.composePoint(bb_max, bb_max);
473 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1046
GLuint GLuint GLsizei count
Definition: glext.h:3528
void operator()(const CObservation2DRangeScan &obs)
A mesh built from a set of 2D laser scan observations.
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::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:2590
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
#define GL_TRIANGLES
Definition: glew.h:276
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
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...
#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
GLAPI void GLAPIENTRY glShadeModel(GLenum mode)
#define GL_LIGHTING
Definition: glew.h:385
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
unsigned char uint8_t
Definition: rptypes.h:41
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:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
std::vector< double > pitchs
This base provides a set of functions for maths stuff.
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...
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
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...
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:33
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
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...
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)...
void operator()(const CObservation2DRangeScan &obj)
GLAPI void GLAPIENTRY glEnd(void)
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
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
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019