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 }
n
GLenum GLsizei n
Definition: glext.h:5074
mrpt::keep_min
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: core/include/mrpt/core/bits_math.h:124
mrpt::opengl::CPlanarLaserScan::m_line_B
float m_line_B
Definition: CPlanarLaserScan.h:66
glBegin
GLAPI void GLAPIENTRY glBegin(GLenum mode)
mrpt::maps::CMetricMap::insertObservation
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:95
IMPLEMENTS_SERIALIZABLE
IMPLEMENTS_SERIALIZABLE(CPlanarLaserScan, CRenderizableDisplayList, mrpt::opengl) CPlanarLaserScan
Definition: CPlanarLaserScan.cpp:42
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
GL_SRC_ALPHA
#define GL_SRC_ALPHA
Definition: glew.h:286
glColor4f
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints
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
GL_BLEND
#define GL_BLEND
Definition: glew.h:432
mrpt::opengl::CRenderizableDisplayList
A renderizable object suitable for rendering with OpenGL's display lists.
Definition: CRenderizableDisplayList.h:39
mrpt::math::TPoint3D::z
double z
Definition: lightweight_geom_data.h:385
glDisable
GLAPI void GLAPIENTRY glDisable(GLenum cap)
mrpt::opengl::CRenderizableDisplayList::notifyChange
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated)
Definition: CRenderizableDisplayList.h:57
mrpt::opengl::CPlanarLaserScan::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPlanarLaserScan.cpp:172
glEnable
GLAPI void GLAPIENTRY glEnable(GLenum cap)
mrpt::opengl::CPlanarLaserScan::getBoundingBox
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...
Definition: CPlanarLaserScan.cpp:213
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::opengl::CPlanarLaserScan::m_scan
mrpt::obs::CObservation2DRangeScan m_scan
Definition: CPlanarLaserScan.h:61
glVertex3f
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
mrpt::obs::CObservation2DRangeScan::resizeScan
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
Definition: CObservation2DRangeScan.cpp:538
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::opengl::CPlanarLaserScan::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPlanarLaserScan.cpp:182
mrpt::opengl::CPlanarLaserScan::m_enable_points
bool m_enable_points
Definition: CPlanarLaserScan.h:73
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
mrpt::opengl::CPlanarLaserScan::m_points_B
float m_points_B
Definition: CPlanarLaserScan.h:69
glEnd
GLAPI void GLAPIENTRY glEnd(void)
mrpt::opengl::CPlanarLaserScan::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPlanarLaserScan.cpp:171
mrpt::opengl::CPlanarLaserScan::CPlanarLaserScan
CPlanarLaserScan()
Constructor.
mrpt::maps::CPointsMap::TInsertionOptions::isPlanarMap
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
mrpt::containers::ContainerReadOnlyProxyAccessor::size
size_t size() const
Definition: ContainerReadOnlyProxyAccessor.h:42
mrpt::opengl::CPlanarLaserScan::m_points_width
float m_points_width
Definition: CPlanarLaserScan.h:68
mrpt::opengl::CRenderizable::m_pose
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:57
mrpt::opengl::CRenderizable::checkOpenGLError
static void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: CRenderizable.cpp:301
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:144
GL_TRIANGLES
#define GL_TRIANGLES
Definition: glew.h:276
glLineWidth
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
GL_ONE_MINUS_SRC_ALPHA
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
mrpt::keep_max
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: core/include/mrpt/core/bits_math.h:131
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
mrpt::math::TPoint3D::x
double x
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:385
GL_POINTS
#define GL_POINTS
Definition: glew.h:272
glPointSize
GLAPI void GLAPIENTRY glPointSize(GLfloat size)
mrpt::opengl::CPlanarLaserScan::m_plane_B
float m_plane_B
Definition: CPlanarLaserScan.h:71
mrpt::opengl::CPlanarLaserScan::m_enable_line
bool m_enable_line
Definition: CPlanarLaserScan.h:74
mrpt::opengl::CPlanarLaserScan::m_line_width
float m_line_width
Definition: CPlanarLaserScan.h:65
mrpt::opengl::CPlanarLaserScan::m_points_G
float m_points_G
Definition: CPlanarLaserScan.h:69
GL_LINES
#define GL_LINES
Definition: glew.h:273
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
mrpt::math::TPoint3D::y
double y
Definition: lightweight_geom_data.h:385
mrpt::opengl::CPlanarLaserScan::m_cache_valid
bool m_cache_valid
Definition: CPlanarLaserScan.h:63
mrpt::poses::CPose3D::composePoint
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
mrpt::obs::CObservation2DRangeScan::scan
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.h:103
mrpt::maps::CPointsMap::getPointsBuffer
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
mrpt::opengl::CPlanarLaserScan::m_points_R
float m_points_R
Definition: CPlanarLaserScan.h:69
mrpt::maps::CPointsMap::insertionOptions
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:257
mrpt::opengl::CPlanarLaserScan::m_line_A
float m_line_A
Definition: CPlanarLaserScan.h:66
maps-precomp.h
mrpt::opengl::CPlanarLaserScan::m_cache_points
mrpt::maps::CSimplePointsMap m_cache_points
Definition: CPlanarLaserScan.h:62
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
z
GLdouble GLdouble z
Definition: glext.h:3872
in
GLuint in
Definition: glext.h:7274
mrpt::opengl::CPlanarLaserScan::m_points_A
float m_points_A
Definition: CPlanarLaserScan.h:69
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
CPlanarLaserScan.h
mrpt::opengl::CPlanarLaserScan::m_plane_R
float m_plane_R
Definition: CPlanarLaserScan.h:71
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::opengl::CRenderizable::writeToStreamRender
void writeToStreamRender(mrpt::serialization::CArchive &out) const
Definition: CRenderizable.cpp:110
mrpt::opengl::CPlanarLaserScan::m_line_R
float m_line_R
Definition: CPlanarLaserScan.h:66
mrpt::opengl::CPlanarLaserScan::m_plane_A
float m_plane_A
Definition: CPlanarLaserScan.h:71
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:133
mrpt::obs::CObservation2DRangeScan::validRange
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
Definition: CObservation2DRangeScan.h:119
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
mrpt::opengl::CPlanarLaserScan::m_line_G
float m_line_G
Definition: CPlanarLaserScan.h:66
mrpt::opengl::CRenderizableDisplayList::readFromStreamRender
void readFromStreamRender(mrpt::serialization::CArchive &in)
Definition: CRenderizableDisplayList.h:65
y
GLenum GLint GLint y
Definition: glext.h:3538
x
GLenum GLint x
Definition: glext.h:3538
mrpt::opengl::CPlanarLaserScan::m_plane_G
float m_plane_G
Definition: CPlanarLaserScan.h:71
mrpt::opengl::CPlanarLaserScan::render_dl
void render_dl() const override
Render.
Definition: CPlanarLaserScan.cpp:84
glBlendFunc
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
mrpt::opengl::CPlanarLaserScan
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
Definition: CPlanarLaserScan.h:57
mrpt::opengl::CPlanarLaserScan::m_enable_surface
bool m_enable_surface
Definition: CPlanarLaserScan.h:75



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST