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].scan.size();
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& 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 (const auto& t : triangles)
185  {
186  float ax = t.x[1] - t.x[0];
187  float bx = t.x[2] - t.x[0];
188  float ay = t.y[1] - t.y[0];
189  float by = t.y[2] - t.y[0];
190  float az = t.z[1] - t.z[0];
191  float bz = t.z[2] - t.z[0];
192  glNormal3f(ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);
193  if (mWireframe) glBegin(GL_LINE_LOOP);
194  for (int k = 0; k < 3; k++)
195  {
196  glColor4f(t.r[k], t.g[k], t.b[k], t.a[k]);
197  glVertex3f(t.x[k], t.y[k], t.z[k]);
198  }
199  if (mWireframe) glEnd();
200  }
201  if (!mWireframe) glEnd();
204 #endif
205 }
206 
208  const mrpt::poses::CPose3D& o, double& dist) const
209 {
211  MRPT_UNUSED_PARAM(dist);
212  // TODO: redo
213  return false;
214 }
215 
216 bool CAngularObservationMesh::setScanSet(
217  const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
218 {
219  CRenderizableDisplayList::notifyChange();
220 
221  // Returns false if the scan is inconsistent
222  if (scans.size() > 0)
223  {
224  size_t setSize = scans[0].scan.size();
225  bool rToL = scans[0].rightToLeft;
226  for (auto it = scans.begin() + 1; it != scans.end(); ++it)
227  {
228  if (it->scan.size() != setSize) return false;
229  if (it->rightToLeft != rToL) return false;
230  }
231  }
232  scanSet = scans;
233  meshUpToDate = false;
234  return true;
235 }
236 
237 void CAngularObservationMesh::setPitchBounds(
238  const double initial, const double final)
239 {
240  CRenderizableDisplayList::notifyChange();
241 
242  pitchBounds.clear();
243  pitchBounds.push_back(initial);
244  pitchBounds.push_back(final);
245  meshUpToDate = false;
246 }
247 void CAngularObservationMesh::setPitchBounds(const std::vector<double>& bounds)
248 {
249  CRenderizableDisplayList::notifyChange();
250 
251  pitchBounds = bounds;
252  meshUpToDate = false;
253 }
254 void CAngularObservationMesh::getPitchBounds(
255  double& initial, double& final) const
256 {
257  initial = pitchBounds.front();
258  final = pitchBounds.back();
259 }
260 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds) const
261 {
262  bounds = pitchBounds;
263 }
264 void CAngularObservationMesh::getScanSet(
265  std::vector<CObservation2DRangeScan>& scans) const
266 {
267  scans = scanSet;
268 }
269 
270 void CAngularObservationMesh::generateSetOfTriangles(
271  CSetOfTriangles::Ptr& res) const
272 {
273  if (!meshUpToDate) updateMesh();
274  res->insertTriangles(triangles.begin(), triangles.end());
275  // for (vector<CSetOfTriangles::TTriangle>::iterator
276  // it=triangles.begin();it!=triangles.end();++it) res->insertTriangle(*it);
277 }
278 
280 {
284  {
286  }
287 };
288 
289 void CAngularObservationMesh::generatePointCloud(CPointsMap* out_map) const
290 {
291  ASSERT_(out_map);
292  out_map->clear();
293  /* size_t numRows=scanSet.size();
294  if ((pitchBounds.size()!=numRows)&&(pitchBounds.size()!=2)) return;
295  std::vector<double> pitchs(numRows);
296  if (pitchBounds.size()==2) {
297  double p1=pitchBounds[0];
298  double p2=pitchBounds[1];
299  for (size_t i=0;i<numRows;i++)
300  pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
301  } else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
302  for (size_t i=0;i<numRows;i++) out_map->insertObservation(&scanSet[i]);
303  */
304 
305  std::for_each(
306  scanSet.begin(), scanSet.end(), CAngularObservationMesh_fnctr(out_map));
307 }
308 
309 uint8_t CAngularObservationMesh::serializeGetVersion() const { return 0; }
310 void CAngularObservationMesh::serializeTo(
312 {
313  writeToStreamRender(out);
314  // Version 0:
315  out << pitchBounds << scanSet << mWireframe << mEnableTransparency;
316 }
317 
318 void CAngularObservationMesh::serializeFrom(
320 {
321  switch (version)
322  {
323  case 0:
324  readFromStreamRender(in);
325  in >> pitchBounds >> scanSet >> mWireframe >> mEnableTransparency;
326  break;
327  default:
329  };
330  meshUpToDate = false;
331 }
332 
334  std::vector<double>& vals) const
335 {
336  double value = initialValue();
337  double incr = increment();
338  size_t am = amount();
339  vals.resize(am);
340  for (size_t i = 0; i < am - 1; i++, value += incr) vals[i] = value;
341  vals[am - 1] = finalValue();
342 }
343 
344 void CAngularObservationMesh::getTracedRays(CSetOfLines::Ptr& res) const
345 {
346  if (!meshUpToDate) updateMesh();
347  size_t count = 0;
348  for (int i = 0; i < validityMatrix.rows(); i++)
349  for (int j = 0; j < validityMatrix.cols(); j++)
350  if (validityMatrix(i, j)) count++;
351  res->reserve(count);
352  for (int i = 0; i < actualMesh.rows(); i++)
353  for (int j = 0; j < actualMesh.cols(); j++)
354  if (validityMatrix(i, j))
355  res->appendLine(
356  (scanSet[i].sensorPose).asTPose(), actualMesh(i, j));
357 }
358 
360 {
361  public:
363  const CPoint3D& pDist;
364  std::vector<double> pitchs;
366  CSetOfLines::Ptr& l, const CPoint3D& p, const std::vector<double>& pi)
367  : lins(l), pDist(p), pitchs()
368  {
369  pitchs.reserve(pi.size());
370  for (auto it = pi.rbegin(); it != pi.rend(); ++it)
371  pitchs.push_back(*it);
372  }
374  {
375  size_t hm = obs.scan.size();
376  for (char it : obs.validRange)
377  if (it) hm--;
378  lins->reserve(hm);
379  for (size_t i = 0; i < obs.scan.size(); i++)
380  if (!obs.validRange[i])
381  {
382  double yaw =
383  obs.aperture * ((static_cast<double>(i) /
384  static_cast<double>(obs.scan.size() - 1)) -
385  0.5);
386  lins->appendLine(
387  obs.sensorPose.asTPose(),
388  (obs.sensorPose +
389  CPose3D(
390  0, 0, 0, obs.rightToLeft ? yaw : -yaw,
391  obs.deltaPitch * i + pitchs.back(), 0) +
392  pDist)
393  .asTPoint());
394  }
395  pitchs.pop_back();
396  }
397 };
398 void CAngularObservationMesh::getUntracedRays(
399  CSetOfLines::Ptr& res, double dist) const
400 {
401  for_each(
402  scanSet.begin(), scanSet.end(),
403  FAddUntracedLines(res, dist, pitchBounds));
404 }
405 
407 {
408  TPolygon3D res(3);
409  for (size_t i = 0; i < 3; i++)
410  {
411  res[i].x = t.x[i];
412  res[i].y = t.y[i];
413  res[i].z = t.z[i];
414  }
415  return res;
416 }
417 
418 void CAngularObservationMesh::generateSetOfTriangles(
419  std::vector<TPolygon3D>& res) const
420 {
421  if (!meshUpToDate) updateMesh();
422  res.resize(triangles.size());
423  transform(
424  triangles.begin(), triangles.end(), res.begin(), createFromTriangle);
425 }
426 
427 void CAngularObservationMesh::getBoundingBox(
429 {
430  if (!meshUpToDate) updateMesh();
431 
433  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
434  std::numeric_limits<double>::max());
436  -std::numeric_limits<double>::max(),
437  -std::numeric_limits<double>::max(),
438  -std::numeric_limits<double>::max());
439 
440  for (const auto& t : triangles)
441  {
442  keep_min(bb_min.x, t.x[0]);
443  keep_max(bb_max.x, t.x[0]);
444  keep_min(bb_min.y, t.y[0]);
445  keep_max(bb_max.y, t.y[0]);
446  keep_min(bb_min.z, t.z[0]);
447  keep_max(bb_max.z, t.z[0]);
448 
449  keep_min(bb_min.x, t.x[1]);
450  keep_max(bb_max.x, t.x[1]);
451  keep_min(bb_min.y, t.y[1]);
452  keep_max(bb_max.y, t.y[1]);
453  keep_min(bb_min.z, t.z[1]);
454  keep_max(bb_max.z, t.z[1]);
455 
456  keep_min(bb_min.x, t.x[2]);
457  keep_max(bb_max.x, t.x[2]);
458  keep_min(bb_min.y, t.y[2]);
459  keep_max(bb_max.y, t.y[2]);
460  keep_min(bb_min.z, t.z[2]);
461  keep_max(bb_max.z, t.z[2]);
462  }
463 
464  // Convert to coordinates of my parent:
465  m_pose.composePoint(bb_min, bb_min);
466  m_pose.composePoint(bb_max, bb_max);
467 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:765
GLuint GLuint GLsizei count
Definition: glext.h:3532
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:2567
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
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: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
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:44
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.
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.
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)
#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:53
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: d046c631e Tue Jul 23 10:47:41 2019 -0700 at mar jul 23 19:50:16 CEST 2019