Main MRPT website > C++ reference for MRPT 1.5.7
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 
40 
41 using namespace mrpt;
42 using namespace mrpt::obs;
43 using namespace mrpt::maps;
44 using namespace mrpt::math;
45 using namespace mrpt::opengl;
46 using namespace mrpt::utils;
47 using namespace mrpt::poses;
48 using namespace mrpt::math;
49 
51 
52 void CAngularObservationMesh::addTriangle(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) const {
53  const TPoint3D *arr[3]={&p1,&p2,&p3};
55  for (size_t i=0;i<3;i++) {
56  t.x[i]=arr[i]->x;
57  t.y[i]=arr[i]->y;
58  t.z[i]=arr[i]->z;
59  t.r[i]=m_color.R*(1.f/255);
60  t.g[i]=m_color.G*(1.f/255);
61  t.b[i]=m_color.B*(1.f/255);
62  t.a[i]=m_color.A*(1.f/255);
63  }
64  triangles.push_back(t);
65  CRenderizableDisplayList::notifyChange();
66 }
67 
68 void CAngularObservationMesh::updateMesh() const {
69  CRenderizableDisplayList::notifyChange();
70 
71  size_t numRows=scanSet.size();
72  triangles.clear();
73  if (numRows<=1) {
74  actualMesh.setSize(0,0);
75  validityMatrix.setSize(0,0);
76  meshUpToDate=true;
77  return;
78  }
79  if (pitchBounds.size()!=numRows&&pitchBounds.size()!=2) return;
80  size_t numCols=scanSet[0].scan.size();
81  actualMesh.setSize(numRows,numCols);
82  validityMatrix.setSize(numRows,numCols);
83  double *pitchs=new double[numRows];
84  if (pitchBounds.size()==2) {
85  double p1=pitchBounds[0];
86  double p2=pitchBounds[1];
87  for (size_t i=0;i<numRows;i++) pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
88  } else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
89  const bool rToL=scanSet[0].rightToLeft;
90  for (size_t i=0;i<numRows;i++) {
91  const std::vector<float> &scan=scanSet[i].scan;
92  const std::vector<char> valid=scanSet[i].validRange;
93  const double pitchIncr=scanSet[i].deltaPitch;
94  const double aperture=scanSet[i].aperture;
95  const CPose3D origin=scanSet[i].sensorPose;
96  //This is not an error...
97  for (size_t j=0;j<numCols;j++) if ((validityMatrix(i,j)=(valid[j]!=0))) {
98  double pYaw=aperture*((static_cast<double>(j)/static_cast<double>(numCols-1))-0.5);
99  // Without the pitch since it's already within each sensorPose:
100  actualMesh(i,j)=(origin+CPose3D(0,0,0,rToL?pYaw:-pYaw,pitchIncr))+CPoint3D(scan[j],0,0);
101  }
102  }
103  delete[] pitchs;
104  triangles.reserve(2*(numRows-1)*(numCols-1));
105  for (size_t k=0;k<numRows-1;k++) {
106  for (size_t j=0;j<numCols-1;j++) {
107  int b1=validityMatrix(k,j)?1:0;
108  int b2=validityMatrix(k,j+1)?1:0;
109  int b3=validityMatrix(k+1,j)?1:0;
110  int b4=validityMatrix(k+1,j+1)?1:0;
111  switch (b1+b2+b3+b4) {
112  case 0:
113  case 1:
114  case 2:
115  break;
116  case 3:
117  if (!b1) addTriangle(actualMesh(k,j+1),actualMesh(k+1,j),actualMesh(k+1,j+1));
118  else if (!b2) addTriangle(actualMesh(k,j),actualMesh(k+1,j),actualMesh(k+1,j+1));
119  else if (!b3) addTriangle(actualMesh(k,j),actualMesh(k,j+1),actualMesh(k+1,j+1));
120  else if (!b4) addTriangle(actualMesh(k,j),actualMesh(k,j+1),actualMesh(k+1,j));
121  break;
122  case 4:
123  addTriangle(actualMesh(k,j),actualMesh(k,j+1),actualMesh(k+1,j));
124  addTriangle(actualMesh(k+1,j+1),actualMesh(k,j+1),actualMesh(k+1,j));
125  }
126  }
127  }
128  meshUpToDate=true;
129 }
130 
131 void CAngularObservationMesh::render_dl() const {
132 #if MRPT_HAS_OPENGL_GLUT
133  if (mEnableTransparency) {
136  } else {
139  }
142  if (!meshUpToDate) updateMesh();
143  if (!mWireframe) glBegin(GL_TRIANGLES);
144  for (size_t i=0;i<triangles.size();i++) {
145  const CSetOfTriangles::TTriangle &t=triangles[i];
146  float ax=t.x[1]-t.x[0];
147  float bx=t.x[2]-t.x[0];
148  float ay=t.y[1]-t.y[0];
149  float by=t.y[2]-t.y[0];
150  float az=t.z[1]-t.z[0];
151  float bz=t.z[2]-t.z[0];
152  glNormal3f(ay*bz-az*by,az*bx-ax*bz,ax*by-ay*bx);
153  if (mWireframe) glBegin(GL_LINE_LOOP);
154  for (int i=0;i<3;i++) {
155  glColor4f(t.r[i],t.g[i],t.b[i],t.a[i]);
156  glVertex3f(t.x[i],t.y[i],t.z[i]);
157  }
158  if (mWireframe) glEnd();
159  }
160  if (!mWireframe) glEnd();
163 #endif
164 }
165 
166 bool CAngularObservationMesh::traceRay(const mrpt::poses::CPose3D &o,double &dist) const {
168  MRPT_UNUSED_PARAM(dist);
169  //TODO: redo
170  return false;
171 }
172 
173 bool CAngularObservationMesh::setScanSet(const std::vector<mrpt::obs::CObservation2DRangeScan> &scans) {
174  CRenderizableDisplayList::notifyChange();
175 
176  //Returns false if the scan is inconsistent
177  if (scans.size()>0) {
178  size_t setSize=scans[0].scan.size();
179  bool rToL=scans[0].rightToLeft;
180  for (std::vector<CObservation2DRangeScan>::const_iterator it=scans.begin()+1;it!=scans.end();++it) {
181  if (it->scan.size()!=setSize) return false;
182  if (it->rightToLeft!=rToL) return false;
183  }
184  }
185  scanSet=scans;
186  meshUpToDate=false;
187  return true;
188 }
189 
190 void CAngularObservationMesh::setPitchBounds(const double initial,const double final) {
191  CRenderizableDisplayList::notifyChange();
192 
193  pitchBounds.clear();
194  pitchBounds.push_back(initial);
195  pitchBounds.push_back(final);
196  meshUpToDate=false;
197 }
198 void CAngularObservationMesh::setPitchBounds(const std::vector<double> &bounds) {
199  CRenderizableDisplayList::notifyChange();
200 
201  pitchBounds=bounds;
202  meshUpToDate=false;
203 }
204 void CAngularObservationMesh::getPitchBounds(double &initial,double &final) const {
205  initial=pitchBounds.front();
206  final=pitchBounds.back();
207 }
208 void CAngularObservationMesh::getPitchBounds(std::vector<double> &bounds) const {
209  bounds=pitchBounds;
210 }
211 void CAngularObservationMesh::getScanSet(std::vector<CObservation2DRangeScan> &scans) const {
212  scans=scanSet;
213 }
214 
215 void CAngularObservationMesh::generateSetOfTriangles(CSetOfTrianglesPtr &res) const {
216  if (!meshUpToDate) updateMesh();
217  res->insertTriangles(triangles.begin(),triangles.end());
218  //for (vector<CSetOfTriangles::TTriangle>::iterator it=triangles.begin();it!=triangles.end();++it) res->insertTriangle(*it);
219 }
220 
224  inline void operator()(const CObservation2DRangeScan &obj) {
225  m->insertObservation(&obj);
226  }
227  };
228 
229 void CAngularObservationMesh::generatePointCloud(CPointsMap *out_map) const {
230  ASSERT_(out_map);
231  out_map->clear();
232 /* size_t numRows=scanSet.size();
233  if ((pitchBounds.size()!=numRows)&&(pitchBounds.size()!=2)) return;
234  std::vector<double> pitchs(numRows);
235  if (pitchBounds.size()==2) {
236  double p1=pitchBounds[0];
237  double p2=pitchBounds[1];
238  for (size_t i=0;i<numRows;i++) pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
239  } else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
240  for (size_t i=0;i<numRows;i++) out_map->insertObservation(&scanSet[i]);
241 */
242 
243  std::for_each(scanSet.begin(),scanSet.end(),CAngularObservationMesh_fnctr(out_map));
244 }
245 
246 /*---------------------------------------------------------------
247  Implements the writing to a CStream capability of
248  CSerializable objects
249  ---------------------------------------------------------------*/
250 void CAngularObservationMesh::writeToStream(mrpt::utils::CStream &out,int *version) const {
251  if (version) *version=0;
252  else {
253  writeToStreamRender(out);
254  //Version 0:
255  out<<pitchBounds<<scanSet<<mWireframe<<mEnableTransparency;
256  }
257 }
258 
259 /*---------------------------------------------------------------
260  Implements the reading from a CStream capability of
261  CSerializable objects
262  ---------------------------------------------------------------*/
263 void CAngularObservationMesh::readFromStream(mrpt::utils::CStream &in,int version) {
264  switch (version) {
265  case 0:
266  readFromStreamRender(in);
267  in>>pitchBounds>>scanSet>>mWireframe>>mEnableTransparency;
268  break;
269  default:
271  };
272  meshUpToDate=false;
273 }
274 
275 void CAngularObservationMesh::TDoubleRange::values(std::vector<double> &vals) const {
276  double value=initialValue();
277  double incr=increment();
278  size_t am=amount();
279  vals.resize(am);
280  for (size_t i=0;i<am-1;i++,value+=incr) vals[i]=value;
281  vals[am-1]=finalValue();
282 }
283 
284 void CAngularObservationMesh::getTracedRays(CSetOfLinesPtr &res) const {
285  if (!meshUpToDate) updateMesh();
286  size_t count=0;
287  for (size_t i=0;i<validityMatrix.getRowCount();i++) for (size_t j=0;j<validityMatrix.getColCount();j++) if (validityMatrix(i,j)) count++;
288  res->reserve(count);
289  for (size_t i=0;i<actualMesh.getRowCount();i++) for (size_t j=0;j<actualMesh.getColCount();j++) if (validityMatrix(i,j)) res->appendLine(TPose3D(scanSet[i].sensorPose),actualMesh(i,j));
290 }
291 
293 public:
294  CSetOfLinesPtr &lins;
295  const CPoint3D &pDist;
296  std::vector<double> pitchs;
297  FAddUntracedLines(CSetOfLinesPtr &l,const CPoint3D &p,const std::vector<double> &pi):lins(l),pDist(p),pitchs() {
298  pitchs.reserve(pi.size());
299  for (std::vector<double>::const_reverse_iterator it=pi.rbegin();it!=pi.rend();++it) pitchs.push_back(*it);
300  }
302  size_t hm=obs.scan.size();
303  for (std::vector<char>::const_iterator it=obs.validRange.begin();it!=obs.validRange.end();++it) if (*it) hm--;
304  lins->reserve(hm);
305  for (size_t i=0;i<obs.scan.size();i++) if (!obs.validRange[i]) {
306  double yaw=obs.aperture*((static_cast<double>(i)/static_cast<double>(obs.scan.size()-1))-0.5);
307  lins->appendLine(TPoint3D(obs.sensorPose),TPoint3D(obs.sensorPose+CPose3D(0,0,0,obs.rightToLeft?yaw:-yaw,obs.deltaPitch*i+pitchs.back(),0)+pDist));
308  }
309  pitchs.pop_back();
310  }
311 };
312 void CAngularObservationMesh::getUntracedRays(CSetOfLinesPtr &res,double dist) const {
313  for_each(scanSet.begin(),scanSet.end(),FAddUntracedLines(res,dist,pitchBounds));
314 }
315 
317  TPolygon3D res(3);
318  for (size_t i=0;i<3;i++) {
319  res[i].x=t.x[i];
320  res[i].y=t.y[i];
321  res[i].z=t.z[i];
322  }
323  return res;
324 }
325 
326 void CAngularObservationMesh::generateSetOfTriangles(std::vector<TPolygon3D> &res) const {
327  if (!meshUpToDate) updateMesh();
328  res.resize(triangles.size());
329  transform(triangles.begin(),triangles.end(),res.begin(),createFromTriangle);
330 }
331 
332 void CAngularObservationMesh::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
333 {
334  if (!meshUpToDate) updateMesh();
335 
336  bb_min = mrpt::math::TPoint3D(std::numeric_limits<double>::max(),std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
337  bb_max = mrpt::math::TPoint3D(-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max());
338 
339  for (size_t i=0;i<triangles.size();i++)
340  {
341  const CSetOfTriangles::TTriangle &t=triangles[i];
342 
343  keep_min(bb_min.x, t.x[0]); keep_max(bb_max.x, t.x[0]);
344  keep_min(bb_min.y, t.y[0]); keep_max(bb_max.y, t.y[0]);
345  keep_min(bb_min.z, t.z[0]); keep_max(bb_max.z, t.z[0]);
346 
347  keep_min(bb_min.x, t.x[1]); keep_max(bb_max.x, t.x[1]);
348  keep_min(bb_min.y, t.y[1]); keep_max(bb_max.y, t.y[1]);
349  keep_min(bb_min.z, t.z[1]); keep_max(bb_max.z, t.z[1]);
350 
351  keep_min(bb_min.x, t.x[2]); keep_max(bb_max.x, t.x[2]);
352  keep_min(bb_min.y, t.y[2]); keep_max(bb_max.y, t.y[2]);
353  keep_min(bb_min.z, t.z[2]); keep_max(bb_max.z, t.z[2]);
354  }
355 
356  // Convert to coordinates of my parent:
357  m_pose.composePoint(bb_min, bb_min);
358  m_pose.composePoint(bb_max, bb_max);
359 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
GLuint GLuint GLsizei count
Definition: glext.h:3512
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.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
GLbyte GLbyte bz
Definition: glext.h:5451
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLboolean GLenum GLenum GLvoid * values
Definition: glext.h:3535
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool BASE_IMPEXP traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:1989
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
#define GL_TRIANGLES
Definition: glew.h:272
const Scalar * const_iterator
Definition: eigen_plugins.h:24
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:283
double z
X,Y,Z coordinates.
#define GL_COLOR_MATERIAL
Definition: glew.h:388
#define GL_DEPTH_TEST
Definition: glew.h:397
#define GL_SMOOTH
Definition: glew.h:631
GLAPI void GLAPIENTRY glShadeModel(GLenum mode)
#define GL_LIGHTING
Definition: glew.h:381
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
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.
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.
int version
Definition: mrpt_jpeglib.h:898
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:428
#define GL_LINE_LOOP
Definition: glew.h:270
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:176
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:282
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:171
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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:6301
The namespace for 3D scene representation and rendering.
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)
GLbyte by
Definition: glext.h:5451
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLsizei const GLfloat * value
Definition: glext.h:3929
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
GLuint res
Definition: glext.h:6298
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
Lightweight 3D point.
GLuint GLenum GLenum transform
Definition: glext.h:6092
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLfloat GLfloat p
Definition: glext.h:5587
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:102
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
FAddUntracedLines(CSetOfLinesPtr &l, const CPoint3D &p, const std::vector< double > &pi)
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019