MRPT  1.9.9
CAngularObservationMesh.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 "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].getScanSize();
84  actualMesh.setSize(numRows, numCols);
85  validityMatrix.setSize(numRows, numCols);
86  auto* 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& ss_i = scanSet[i];
101  const double pitchIncr = scanSet[i].deltaPitch;
102  const double aperture = scanSet[i].aperture;
103  const CPose3D origin = scanSet[i].sensorPose;
104  // This is not an error...
105  for (size_t j = 0; j < numCols; j++)
106  if ((validityMatrix(i, j) = ss_i.getScanRangeValidity(j)))
107  {
108  double pYaw = aperture * ((static_cast<double>(j) /
109  static_cast<double>(numCols - 1)) -
110  0.5);
111  // Without the pitch since it's already within each sensorPose:
112  actualMesh(i, j) =
113  ((origin +
114  CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
115  CPoint3D(ss_i.getScanRange(j), 0, 0))
116  .asTPoint();
117  }
118  }
119  delete[] pitchs;
120  triangles.reserve(2 * (numRows - 1) * (numCols - 1));
121  for (size_t k = 0; k < numRows - 1; k++)
122  {
123  for (size_t j = 0; j < numCols - 1; j++)
124  {
125  int b1 = validityMatrix(k, j) ? 1 : 0;
126  int b2 = validityMatrix(k, j + 1) ? 1 : 0;
127  int b3 = validityMatrix(k + 1, j) ? 1 : 0;
128  int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
129  switch (b1 + b2 + b3 + b4)
130  {
131  case 0:
132  case 1:
133  case 2:
134  break;
135  case 3:
136  if (!b1)
137  addTriangle(
138  actualMesh(k, j + 1), actualMesh(k + 1, j),
139  actualMesh(k + 1, j + 1));
140  else if (!b2)
141  addTriangle(
142  actualMesh(k, j), actualMesh(k + 1, j),
143  actualMesh(k + 1, j + 1));
144  else if (!b3)
145  addTriangle(
146  actualMesh(k, j), actualMesh(k, j + 1),
147  actualMesh(k + 1, j + 1));
148  else if (!b4)
149  addTriangle(
150  actualMesh(k, j), actualMesh(k, j + 1),
151  actualMesh(k + 1, j));
152  break;
153  case 4:
154  addTriangle(
155  actualMesh(k, j), actualMesh(k, j + 1),
156  actualMesh(k + 1, j));
157  addTriangle(
158  actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
159  actualMesh(k + 1, j));
160  }
161  }
162  }
163  meshUpToDate = true;
164 }
165 
166 void CAngularObservationMesh::render_dl() const
167 {
168 #if MRPT_HAS_OPENGL_GLUT
169  if (mEnableTransparency)
170  {
173  }
174  else
175  {
178  }
181  if (!meshUpToDate) updateMesh();
182  if (!mWireframe) glBegin(GL_TRIANGLES);
183  for (const auto& t : triangles)
184  {
185  float ax = t.x[1] - t.x[0];
186  float bx = t.x[2] - t.x[0];
187  float ay = t.y[1] - t.y[0];
188  float by = t.y[2] - t.y[0];
189  float az = t.z[1] - t.z[0];
190  float bz = t.z[2] - t.z[0];
191  glNormal3f(ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);
192  if (mWireframe) glBegin(GL_LINE_LOOP);
193  for (int k = 0; k < 3; k++)
194  {
195  glColor4f(t.r[k], t.g[k], t.b[k], t.a[k]);
196  glVertex3f(t.x[k], t.y[k], t.z[k]);
197  }
198  if (mWireframe) glEnd();
199  }
200  if (!mWireframe) glEnd();
203 #endif
204 }
205 
207  const mrpt::poses::CPose3D& o, double& dist) const
208 {
210  MRPT_UNUSED_PARAM(dist);
211  // TODO: redo
212  return false;
213 }
214 
215 bool CAngularObservationMesh::setScanSet(
216  const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
217 {
218  CRenderizableDisplayList::notifyChange();
219 
220  // Returns false if the scan is inconsistent
221  if (scans.size() > 0)
222  {
223  size_t setSize = scans[0].getScanSize();
224  bool rToL = scans[0].rightToLeft;
225  for (auto it = scans.begin() + 1; it != scans.end(); ++it)
226  {
227  if (it->getScanSize() != setSize) return false;
228  if (it->rightToLeft != rToL) return false;
229  }
230  }
231  scanSet = scans;
232  meshUpToDate = false;
233  return true;
234 }
235 
236 void CAngularObservationMesh::setPitchBounds(
237  const double initial, const double final)
238 {
239  CRenderizableDisplayList::notifyChange();
240 
241  pitchBounds.clear();
242  pitchBounds.push_back(initial);
243  pitchBounds.push_back(final);
244  meshUpToDate = false;
245 }
246 void CAngularObservationMesh::setPitchBounds(const std::vector<double>& bounds)
247 {
248  CRenderizableDisplayList::notifyChange();
249 
250  pitchBounds = bounds;
251  meshUpToDate = false;
252 }
253 void CAngularObservationMesh::getPitchBounds(
254  double& initial, double& final) const
255 {
256  initial = pitchBounds.front();
257  final = pitchBounds.back();
258 }
259 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds) const
260 {
261  bounds = pitchBounds;
262 }
263 void CAngularObservationMesh::getScanSet(
264  std::vector<CObservation2DRangeScan>& scans) const
265 {
266  scans = scanSet;
267 }
268 
269 void CAngularObservationMesh::generateSetOfTriangles(
270  CSetOfTriangles::Ptr& res) const
271 {
272  if (!meshUpToDate) updateMesh();
273  res->insertTriangles(triangles.begin(), triangles.end());
274  // for (vector<CSetOfTriangles::TTriangle>::iterator
275  // it=triangles.begin();it!=triangles.end();++it) res->insertTriangle(*it);
276 }
277 
279 {
283  {
285  }
286 };
287 
288 void CAngularObservationMesh::generatePointCloud(CPointsMap* out_map) const
289 {
290  ASSERT_(out_map);
291  out_map->clear();
292  /* size_t numRows=scanSet.size();
293  if ((pitchBounds.size()!=numRows)&&(pitchBounds.size()!=2)) return;
294  std::vector<double> pitchs(numRows);
295  if (pitchBounds.size()==2) {
296  double p1=pitchBounds[0];
297  double p2=pitchBounds[1];
298  for (size_t i=0;i<numRows;i++)
299  pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
300  } else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
301  for (size_t i=0;i<numRows;i++) out_map->insertObservation(&scanSet[i]);
302  */
303 
304  std::for_each(
305  scanSet.begin(), scanSet.end(), CAngularObservationMesh_fnctr(out_map));
306 }
307 
308 uint8_t CAngularObservationMesh::serializeGetVersion() const { return 0; }
309 void CAngularObservationMesh::serializeTo(
311 {
312  writeToStreamRender(out);
313  // Version 0:
314  out << pitchBounds << scanSet << mWireframe << mEnableTransparency;
315 }
316 
317 void CAngularObservationMesh::serializeFrom(
318  mrpt::serialization::CArchive& in, uint8_t version)
319 {
320  switch (version)
321  {
322  case 0:
323  readFromStreamRender(in);
324  in >> pitchBounds >> scanSet >> mWireframe >> mEnableTransparency;
325  break;
326  default:
328  };
329  meshUpToDate = false;
330 }
331 
333  std::vector<double>& vals) const
334 {
335  double value = initialValue();
336  double incr = increment();
337  size_t am = amount();
338  vals.resize(am);
339  for (size_t i = 0; i < am - 1; i++, value += incr) vals[i] = value;
340  vals[am - 1] = finalValue();
341 }
342 
343 void CAngularObservationMesh::getTracedRays(CSetOfLines::Ptr& res) const
344 {
345  if (!meshUpToDate) updateMesh();
346  size_t count = 0;
347  for (int i = 0; i < validityMatrix.rows(); i++)
348  for (int j = 0; j < validityMatrix.cols(); j++)
349  if (validityMatrix(i, j)) count++;
350  res->reserve(count);
351  for (int i = 0; i < actualMesh.rows(); i++)
352  for (int j = 0; j < actualMesh.cols(); j++)
353  if (validityMatrix(i, j))
354  res->appendLine(
355  (scanSet[i].sensorPose).asTPose(), actualMesh(i, j));
356 }
357 
359 {
360  public:
362  const CPoint3D& pDist;
363  std::vector<double> pitchs;
365  CSetOfLines::Ptr& l, const CPoint3D& p, const std::vector<double>& pi)
366  : lins(l), pDist(p), pitchs()
367  {
368  pitchs.reserve(pi.size());
369  for (auto it = pi.rbegin(); it != pi.rend(); ++it)
370  pitchs.push_back(*it);
371  }
373  {
374  size_t hm = obs.getScanSize();
375  for (size_t i = 0; i < obs.getScanSize(); i++)
376  if (obs.getScanRangeValidity(i)) hm--;
377  lins->reserve(hm);
378  for (size_t i = 0; i < obs.getScanSize(); i++)
379  if (!obs.getScanRangeValidity(i))
380  {
381  double yaw = obs.aperture *
382  ((static_cast<double>(i) /
383  static_cast<double>(obs.getScanSize() - 1)) -
384  0.5);
385  lins->appendLine(
386  obs.sensorPose.asTPose(),
387  (obs.sensorPose +
388  CPose3D(
389  0, 0, 0, obs.rightToLeft ? yaw : -yaw,
390  obs.deltaPitch * i + pitchs.back(), 0) +
391  pDist)
392  .asTPoint());
393  }
394  pitchs.pop_back();
395  }
396 };
397 void CAngularObservationMesh::getUntracedRays(
398  CSetOfLines::Ptr& res, double dist) const
399 {
400  for_each(
401  scanSet.begin(), scanSet.end(),
402  FAddUntracedLines(res, dist, pitchBounds));
403 }
404 
406 {
407  TPolygon3D res(3);
408  for (size_t i = 0; i < 3; i++)
409  {
410  res[i].x = t.x[i];
411  res[i].y = t.y[i];
412  res[i].z = t.z[i];
413  }
414  return res;
415 }
416 
417 void CAngularObservationMesh::generateSetOfTriangles(
418  std::vector<TPolygon3D>& res) const
419 {
420  if (!meshUpToDate) updateMesh();
421  res.resize(triangles.size());
422  transform(
423  triangles.begin(), triangles.end(), res.begin(), createFromTriangle);
424 }
425 
426 void CAngularObservationMesh::getBoundingBox(
428 {
429  if (!meshUpToDate) updateMesh();
430 
432  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
433  std::numeric_limits<double>::max());
435  -std::numeric_limits<double>::max(),
436  -std::numeric_limits<double>::max(),
437  -std::numeric_limits<double>::max());
438 
439  for (const auto& t : triangles)
440  {
441  keep_min(bb_min.x, t.x[0]);
442  keep_max(bb_max.x, t.x[0]);
443  keep_min(bb_min.y, t.y[0]);
444  keep_max(bb_max.y, t.y[0]);
445  keep_min(bb_min.z, t.z[0]);
446  keep_max(bb_max.z, t.z[0]);
447 
448  keep_min(bb_min.x, t.x[1]);
449  keep_max(bb_max.x, t.x[1]);
450  keep_min(bb_min.y, t.y[1]);
451  keep_max(bb_max.y, t.y[1]);
452  keep_min(bb_min.z, t.z[1]);
453  keep_max(bb_max.z, t.z[1]);
454 
455  keep_min(bb_min.x, t.x[2]);
456  keep_max(bb_max.x, t.x[2]);
457  keep_min(bb_min.y, t.y[2]);
458  keep_max(bb_max.y, t.y[2]);
459  keep_min(bb_min.z, t.z[2]);
460  keep_max(bb_max.z, t.z[2]);
461  }
462 
463  // Convert to coordinates of my parent:
464  m_pose.composePoint(bb_min, bb_min);
465  m_pose.composePoint(bb_max, bb_max);
466 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:775
GLuint GLuint GLsizei count
Definition: glext.h:3532
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 operator()(const CObservation2DRangeScan &obs)
A mesh built from a set of 2D laser scan observations.
GLdouble GLdouble t
Definition: glext.h:3695
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
GLbyte GLbyte bz
Definition: glext.h:6193
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3586
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:2569
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
#define GL_TRIANGLES
Definition: glew.h:277
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:288
#define GL_COLOR_MATERIAL
Definition: glew.h:393
#define GL_DEPTH_TEST
Definition: glew.h:402
IMPLEMENTS_SERIALIZABLE(CAngularObservationMesh, CRenderizableDisplayList, mrpt::opengl) void CAngularObservationMesh
#define GL_SMOOTH
Definition: glew.h:636
GLAPI void GLAPIENTRY glShadeModel(GLenum mode)
#define GL_LIGHTING
Definition: glew.h:386
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
size_t getScanSize() const
Get number of scan rays.
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
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
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
std::vector< double > pitchs
This base provides a set of functions for maths stuff.
FAddUntracedLines(CSetOfLines::Ptr &l, const CPoint3D &p, const std::vector< double > &pi)
This namespace contains representation of robot actions and observations.
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:433
#define GL_LINE_LOOP
Definition: glew.h:275
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
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...
#define GL_SRC_ALPHA
Definition: glew.h:287
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:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
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:7391
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)...
const auto bb_max
void operator()(const CObservation2DRangeScan &obj)
GLAPI void GLAPIENTRY glEnd(void)
GLbyte by
Definition: glext.h:6193
const auto bb_min
GLsizei const GLfloat * value
Definition: glext.h:4134
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
GLuint res
Definition: glext.h:7385
Lightweight 3D point.
Definition: TPoint3D.h:90
GLuint GLenum GLenum transform
Definition: glext.h:7089
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLfloat GLfloat p
Definition: glext.h:6398
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>
Definition: TPolygon3D.h:18
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93
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
bool getScanRangeValidity(const size_t i) const
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019