MRPT  2.0.1
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-2020, 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 
48 
49 void CAngularObservationMesh::addTriangle(
50  const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3) const
51 {
53  t.vertices[0].xyzrgba.pt = p1;
54  t.vertices[1].xyzrgba.pt = p2;
55  t.vertices[2].xyzrgba.pt = p3;
56  t.computeNormals();
57  t.setColor(m_color);
58 
59  triangles.emplace_back(std::move(t));
60  CRenderizable::notifyChange();
61 }
62 
63 void CAngularObservationMesh::updateMesh() const
64 {
65  CRenderizable::notifyChange();
66 
67  size_t numRows = scanSet.size();
68  triangles.clear();
69  if (numRows <= 1)
70  {
71  actualMesh.setSize(0, 0);
72  validityMatrix.setSize(0, 0);
73  meshUpToDate = true;
74  return;
75  }
76  if (pitchBounds.size() != numRows && pitchBounds.size() != 2) return;
77  size_t numCols = scanSet[0].getScanSize();
78  actualMesh.setSize(numRows, numCols);
79  validityMatrix.setSize(numRows, numCols);
80  std::vector<double> pitchs(numRows);
81  if (pitchBounds.size() == 2)
82  {
83  double p1 = pitchBounds[0];
84  double p2 = pitchBounds[1];
85  for (size_t i = 0; i < numRows; i++)
86  pitchs[i] = p1 + (p2 - p1) * static_cast<double>(i) /
87  static_cast<double>(numRows - 1);
88  }
89  else
90  for (size_t i = 0; i < numRows; i++) pitchs[i] = pitchBounds[i];
91  const bool rToL = scanSet[0].rightToLeft;
92  for (size_t i = 0; i < numRows; i++)
93  {
94  const auto& ss_i = scanSet[i];
95  const double pitchIncr = scanSet[i].deltaPitch;
96  const double aperture = scanSet[i].aperture;
97  const CPose3D origin = scanSet[i].sensorPose;
98  // This is not an error...
99  for (size_t j = 0; j < numCols; j++)
100  if ((validityMatrix(i, j) = ss_i.getScanRangeValidity(j)))
101  {
102  double pYaw = aperture * ((static_cast<double>(j) /
103  static_cast<double>(numCols - 1)) -
104  0.5);
105  // Without the pitch since it's already within each sensorPose:
106  actualMesh(i, j) =
107  ((origin +
108  CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
109  CPoint3D(ss_i.getScanRange(j), 0, 0))
110  .asTPoint();
111  }
112  }
113 
114  triangles.reserve(2 * (numRows - 1) * (numCols - 1));
115  for (size_t k = 0; k < numRows - 1; k++)
116  {
117  for (size_t j = 0; j < numCols - 1; j++)
118  {
119  int b1 = validityMatrix(k, j) ? 1 : 0;
120  int b2 = validityMatrix(k, j + 1) ? 1 : 0;
121  int b3 = validityMatrix(k + 1, j) ? 1 : 0;
122  int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
123  switch (b1 + b2 + b3 + b4)
124  {
125  case 0:
126  case 1:
127  case 2:
128  break;
129  case 3:
130  if (!b1)
131  addTriangle(
132  actualMesh(k, j + 1), actualMesh(k + 1, j),
133  actualMesh(k + 1, j + 1));
134  else if (!b2)
135  addTriangle(
136  actualMesh(k, j), actualMesh(k + 1, j),
137  actualMesh(k + 1, j + 1));
138  else if (!b3)
139  addTriangle(
140  actualMesh(k, j), actualMesh(k, j + 1),
141  actualMesh(k + 1, j + 1));
142  else if (!b4)
143  addTriangle(
144  actualMesh(k, j), actualMesh(k, j + 1),
145  actualMesh(k + 1, j));
146  break;
147  case 4:
148  addTriangle(
149  actualMesh(k, j), actualMesh(k, j + 1),
150  actualMesh(k + 1, j));
151  addTriangle(
152  actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
153  actualMesh(k + 1, j));
154  }
155  }
156  }
157  meshUpToDate = true;
158 }
159 
160 void CAngularObservationMesh::render(const RenderContext& rc) const
161 {
162  switch (rc.shader_id)
163  {
164  case DefaultShaderID::TRIANGLES:
165  if (!m_Wireframe) CRenderizableShaderTriangles::render(rc);
166  break;
167  case DefaultShaderID::WIREFRAME:
168  if (m_Wireframe) CRenderizableShaderWireFrame::render(rc);
169  break;
170  };
171 }
172 void CAngularObservationMesh::renderUpdateBuffers() const
173 {
174  CRenderizableShaderTriangles::renderUpdateBuffers();
175  CRenderizableShaderWireFrame::renderUpdateBuffers();
176 }
177 
178 void CAngularObservationMesh::onUpdateBuffers_Wireframe()
179 {
180  auto& vbd = CRenderizableShaderWireFrame::m_vertex_buffer_data;
181  auto& cbd = CRenderizableShaderWireFrame::m_color_buffer_data;
182  vbd.clear();
183  cbd.clear();
184 
185  for (const auto& t : triangles)
186  {
187  // Was: glBegin(GL_LINE_LOOP);
188  for (int k = 0; k <= 3; k++)
189  {
190  int kk = k % 3;
191  vbd.emplace_back(t.x(kk), t.y(kk), t.z(kk));
192  cbd.emplace_back(t.r(kk), t.g(kk), t.b(kk), t.a(kk));
193  }
194  }
195 }
196 
197 void CAngularObservationMesh::onUpdateBuffers_Triangles()
198 {
199  auto& tris = CRenderizableShaderTriangles::m_triangles;
200 
201  tris = this->triangles;
202 
203  // All faces, all vertices, same color:
204  for (auto& t : tris)
205  {
206  t.setColor(m_color);
207  t.computeNormals();
208  }
209 }
210 
212  [[maybe_unused]] const mrpt::poses::CPose3D& o,
213  [[maybe_unused]] double& dist) const
214 {
215  // TODO: redo
216  return false;
217 }
218 
219 bool CAngularObservationMesh::setScanSet(
220  const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
221 {
222  CRenderizable::notifyChange();
223 
224  // Returns false if the scan is inconsistent
225  if (scans.size() > 0)
226  {
227  size_t setSize = scans[0].getScanSize();
228  bool rToL = scans[0].rightToLeft;
229  for (auto it = scans.begin() + 1; it != scans.end(); ++it)
230  {
231  if (it->getScanSize() != setSize) return false;
232  if (it->rightToLeft != rToL) return false;
233  }
234  }
235  scanSet = scans;
236  meshUpToDate = false;
237  CRenderizable::notifyChange();
238  return true;
239 }
240 
241 void CAngularObservationMesh::setPitchBounds(
242  const double initial, const double final)
243 {
244  CRenderizable::notifyChange();
245 
246  pitchBounds.clear();
247  pitchBounds.push_back(initial);
248  pitchBounds.push_back(final);
249  meshUpToDate = false;
250  CRenderizable::notifyChange();
251 }
252 void CAngularObservationMesh::setPitchBounds(const std::vector<double>& bounds)
253 {
254  CRenderizable::notifyChange();
255 
256  pitchBounds = bounds;
257  meshUpToDate = false;
258  CRenderizable::notifyChange();
259 }
260 void CAngularObservationMesh::getPitchBounds(
261  double& initial, double& final) const
262 {
263  initial = pitchBounds.front();
264  final = pitchBounds.back();
265 }
266 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds) const
267 {
268  bounds = pitchBounds;
269 }
270 void CAngularObservationMesh::getScanSet(
271  std::vector<CObservation2DRangeScan>& scans) const
272 {
273  scans = scanSet;
274 }
275 
276 void CAngularObservationMesh::generateSetOfTriangles(
277  CSetOfTriangles::Ptr& res) const
278 {
279  if (!meshUpToDate) updateMesh();
280  res->insertTriangles(triangles.begin(), triangles.end());
281 }
282 
284 {
287  inline void operator()(const CObservation2DRangeScan& obj)
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 uint8_t CAngularObservationMesh::serializeGetVersion() const { return 0; }
314 void CAngularObservationMesh::serializeTo(
316 {
317  writeToStreamRender(out);
318  // Version 0:
319  out << pitchBounds << scanSet << m_Wireframe << mEnableTransparency;
320 }
321 
322 void CAngularObservationMesh::serializeFrom(
323  mrpt::serialization::CArchive& in, uint8_t version)
324 {
325  switch (version)
326  {
327  case 0:
328  readFromStreamRender(in);
329  in >> pitchBounds >> scanSet >> m_Wireframe >> mEnableTransparency;
330  break;
331  default:
333  };
334  meshUpToDate = false;
335  CRenderizable::notifyChange();
336 }
337 
338 void CAngularObservationMesh::TDoubleRange::values(
339  std::vector<double>& vals) const
340 {
341  double value = initialValue();
342  double incr = increment();
343  size_t am = amount();
344  vals.resize(am);
345  for (size_t i = 0; i < am - 1; i++, value += incr) vals[i] = value;
346  vals[am - 1] = finalValue();
347 }
348 
349 void CAngularObservationMesh::getTracedRays(CSetOfLines::Ptr& res) const
350 {
351  if (!meshUpToDate) updateMesh();
352  size_t count = 0;
353  for (int i = 0; i < validityMatrix.rows(); i++)
354  for (int j = 0; j < validityMatrix.cols(); j++)
355  if (validityMatrix(i, j)) count++;
356  res->reserve(count);
357  for (int i = 0; i < actualMesh.rows(); i++)
358  for (int j = 0; j < actualMesh.cols(); j++)
359  if (validityMatrix(i, j))
360  res->appendLine(
361  (scanSet[i].sensorPose).asTPose(), actualMesh(i, j));
362 }
363 
365 {
366  public:
368  const CPoint3D& pDist;
369  std::vector<double> pitchs;
371  CSetOfLines::Ptr& l, const CPoint3D& p, const std::vector<double>& pi)
372  : lins(l), pDist(p), pitchs()
373  {
374  pitchs.reserve(pi.size());
375  for (auto it = pi.rbegin(); it != pi.rend(); ++it)
376  pitchs.push_back(*it);
377  }
379  {
380  size_t hm = obs.getScanSize();
381  for (size_t i = 0; i < obs.getScanSize(); i++)
382  if (obs.getScanRangeValidity(i)) hm--;
383  lins->reserve(hm);
384  for (size_t i = 0; i < obs.getScanSize(); i++)
385  if (!obs.getScanRangeValidity(i))
386  {
387  double yaw = obs.aperture *
388  ((static_cast<double>(i) /
389  static_cast<double>(obs.getScanSize() - 1)) -
390  0.5);
391  lins->appendLine(
392  obs.sensorPose.asTPose(),
393  (obs.sensorPose +
394  CPose3D(
395  0, 0, 0, obs.rightToLeft ? yaw : -yaw,
396  obs.deltaPitch * i + pitchs.back(), 0) +
397  pDist)
398  .asTPoint());
399  }
400  pitchs.pop_back();
401  }
402 };
403 void CAngularObservationMesh::getUntracedRays(
404  CSetOfLines::Ptr& res, double dist) const
405 {
406  for_each(
407  scanSet.begin(), scanSet.end(),
408  FAddUntracedLines(res, dist, pitchBounds));
409 }
410 
412 {
413  TPolygon3D res(3);
414  for (size_t i = 0; i < 3; i++) res[i] = t.vertices[i].xyzrgba.pt;
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:759
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.
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:2484
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
A triangle (float coordinates) with RGBA colors (u8) and UV (texture coordinates) for each vertex...
Definition: TTriangle.h:35
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
size_t getScanSize() const
Get number of scan rays.
TPolygon3D createFromTriangle(const mrpt::opengl::TTriangle &t)
Context for calls to render()
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
std::array< Vertex, 3 > vertices
Definition: TTriangle.h:88
#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.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
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...
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...
void setColor(const mrpt::img::TColor &c)
Sets the color of all vertices.
Definition: TTriangle.h:116
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...
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...
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
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)
void computeNormals()
Compute the three normals from the cross-product of "v01 x v02".
Definition: TTriangle.cpp:21
const auto bb_min
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:20
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.
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020