Main MRPT website > C++ reference for MRPT 1.9.9
CPlanarLaserScan.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 
14 
15 #if MRPT_HAS_OPENGL_GLUT
16 #ifdef _WIN32
17 // Windows:
18 #include <windows.h>
19 #endif
20 
21 #ifdef __APPLE__
22 #include <OpenGL/gl.h>
23 #else
24 #include <GL/gl.h>
25 #endif
26 #endif
27 
28 // Include libraries in linking:
29 #if MRPT_HAS_OPENGL_GLUT && defined(_WIN32)
30 // WINDOWS:
31 #if defined(_MSC_VER)
32 #pragma comment(lib, "opengl32.lib")
33 #pragma comment(lib, "GlU32.lib")
34 #endif
35 #endif // MRPT_HAS_OPENGL_GLUT
36 
37 using namespace mrpt;
38 using namespace mrpt::opengl;
39 using namespace mrpt::math;
40 using namespace std;
41 
44 
45 /*---------------------------------------------------------------
46  Constructor
47  ---------------------------------------------------------------*/
49  : m_scan(),
50  m_cache_points(),
51  m_cache_valid(false),
52  m_line_width(1),
53  m_line_R(1.f),
54  m_line_G(0.f),
55  m_line_B(0.f),
56  m_line_A(0.5f),
57  m_points_width(3),
58  m_points_R(1.0f),
59  m_points_G(0.0f),
60  m_points_B(0.0f),
61  m_points_A(1.0f),
62  m_plane_R(0.01f),
63  m_plane_G(0.01f),
64  m_plane_B(0.6f),
65  m_plane_A(0.6f),
66  m_enable_points(true),
67  m_enable_line(true),
68  m_enable_surface(true)
69 {
70 }
71 
72 /*---------------------------------------------------------------
73  clear
74  ---------------------------------------------------------------*/
76 {
78  m_scan.resizeScan(0);
79 }
80 
81 /*---------------------------------------------------------------
82  render
83  ---------------------------------------------------------------*/
85 {
86 #if MRPT_HAS_OPENGL_GLUT
88 
89  // Load into cache:
90  if (!m_cache_valid)
91  {
92  m_cache_valid = true;
96 
98  }
99 
100  size_t i, n;
101  const float *x, *y, *z;
102 
104  if (!n || !x) return;
105 
108 
109  // LINES
110  // ----------------------------
111  if (n > 1 && m_enable_line)
112  {
115 
116  glBegin(GL_LINES);
118 
119  for (i = 0; i < n - 1; i++)
120  {
121  glVertex3f(x[i], y[i], z[i]);
122  glVertex3f(x[i + 1], y[i + 1], z[i + 1]);
123  }
124  glEnd();
126  }
127 
128  // POINTS
129  // ----------------------------
130  if (n > 0 && m_enable_points)
131  {
134 
137 
138  for (i = 0; i < n; i++)
139  {
140  glVertex3f(x[i], y[i], z[i]);
141  }
142  glEnd();
144  }
145 
146  // SURFACE:
147  // ------------------------------
148  if (n > 1 && m_enable_surface)
149  {
151 
153 
154  for (i = 0; i < n - 1; i++)
155  {
156  glVertex3f(
158  m_scan.sensorPose.z());
159  glVertex3f(x[i], y[i], z[i]);
160  glVertex3f(x[i + 1], y[i + 1], z[i + 1]);
161  }
162  glEnd();
164  }
165 
167 
168 #endif
169 }
170 
173 {
174  writeToStreamRender(out);
175  out << m_scan;
176  out << m_line_width << m_line_R << m_line_G << m_line_B << m_line_A
179  << m_enable_points << m_enable_line << m_enable_surface; // new in v1
180 }
181 
184 {
185  switch (version)
186  {
187  case 0:
188  case 1:
189  {
191  in >> m_scan;
192  in >> m_line_width >> m_line_R >> m_line_G >> m_line_B >>
195  m_plane_B >> m_plane_A;
196 
197  if (version >= 1)
198  {
200  m_enable_surface; // new in v1
201  }
202  else
203  {
205  }
206  }
207  break;
208  default:
210  };
211 }
212 
214  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
215 {
216  // Load into cache:
217  if (!m_cache_valid)
218  {
219  m_cache_valid = true;
223 
225  }
226 
227  size_t n;
228  const float *x, *y, *z;
229 
231  if (!n || !x) return;
232 
233  bb_min = mrpt::math::TPoint3D(
234  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
235  std::numeric_limits<double>::max());
236  bb_max = mrpt::math::TPoint3D(
237  -std::numeric_limits<double>::max(),
238  -std::numeric_limits<double>::max(),
239  -std::numeric_limits<double>::max());
240 
241  for (size_t i = 0; i < n; i++)
242  {
243  keep_min(bb_min.x, x[i]);
244  keep_max(bb_max.x, x[i]);
245  keep_min(bb_min.y, y[i]);
246  keep_max(bb_max.y, y[i]);
247  keep_min(bb_min.z, z[i]);
248  keep_max(bb_max.z, z[i]);
249  }
250 
251  // Convert to coordinates of my parent:
252  m_pose.composePoint(bb_min, bb_min);
253  m_pose.composePoint(bb_max, bb_max);
254 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:237
GLdouble GLdouble z
Definition: glext.h:3872
GLAPI void GLAPIENTRY glEnable(GLenum cap)
mrpt::maps::CSimplePointsMap m_cache_points
GLAPI void GLAPIENTRY glPointSize(GLfloat size)
GLenum GLsizei n
Definition: glext.h:5074
#define GL_TRIANGLES
Definition: glew.h:276
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
STL namespace.
#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...
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:57
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
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...
void writeToStreamRender(mrpt::serialization::CArchive &out) const
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
void render_dl() const override
Render.
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
double x
X,Y,Z coordinates.
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:432
#define GL_POINTS
Definition: glew.h:272
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
mrpt::obs::CObservation2DRangeScan m_scan
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define GL_SRC_ALPHA
Definition: glew.h:286
static void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map...
Definition: CPointsMap.cpp:245
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:379
IMPLEMENTS_SERIALIZABLE(CPlanarLaserScan, CRenderizableDisplayList, mrpt::opengl) CPlanarLaserScan
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLAPI void GLAPIENTRY glEnd(void)
GLenum GLint GLint y
Definition: glext.h:3538
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:257
#define GL_LINES
Definition: glew.h:273
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
GLAPI void GLAPIENTRY glDisable(GLenum cap)
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:95
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void readFromStreamRender(mrpt::serialization::CArchive &in)
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:218
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.



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