MRPT  1.9.9
CPlanarLaserScan.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 
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  ---------------------------------------------------------------*/
48 CPlanarLaserScan::CPlanarLaserScan() : m_scan(), m_cache_points() {}
49 /*---------------------------------------------------------------
50  clear
51  ---------------------------------------------------------------*/
53 {
55  m_scan.resizeScan(0);
56 }
57 
58 /*---------------------------------------------------------------
59  render
60  ---------------------------------------------------------------*/
62 {
63 #if MRPT_HAS_OPENGL_GLUT
65 
66  // Load into cache:
67  if (!m_cache_valid)
68  {
69  m_cache_valid = true;
73 
75  }
76 
77  size_t i, n;
78  const float *x, *y, *z;
79 
81  if (!n || !x) return;
82 
85 
86  // LINES
87  // ----------------------------
88  if (n > 1 && m_enable_line)
89  {
92 
95 
96  for (i = 0; i < n - 1; i++)
97  {
98  glVertex3f(x[i], y[i], z[i]);
99  glVertex3f(x[i + 1], y[i + 1], z[i + 1]);
100  }
101  glEnd();
103  }
104 
105  // POINTS
106  // ----------------------------
107  if (n > 0 && m_enable_points)
108  {
111 
114 
115  for (i = 0; i < n; i++)
116  {
117  glVertex3f(x[i], y[i], z[i]);
118  }
119  glEnd();
121  }
122 
123  // SURFACE:
124  // ------------------------------
125  if (n > 1 && m_enable_surface)
126  {
128 
130 
131  for (i = 0; i < n - 1; i++)
132  {
133  glVertex3f(
135  m_scan.sensorPose.z());
136  glVertex3f(x[i], y[i], z[i]);
137  glVertex3f(x[i + 1], y[i + 1], z[i + 1]);
138  }
139  glEnd();
141  }
142 
144 
145 #endif
146 }
147 
150 {
151  writeToStreamRender(out);
152  out << m_scan;
153  out << m_line_width << m_line_R << m_line_G << m_line_B << m_line_A
156  << m_enable_points << m_enable_line << m_enable_surface; // new in v1
157 }
158 
161 {
162  switch (version)
163  {
164  case 0:
165  case 1:
166  {
168  in >> m_scan;
169  in >> m_line_width >> m_line_R >> m_line_G >> m_line_B >>
172  m_plane_B >> m_plane_A;
173 
174  if (version >= 1)
175  {
177  m_enable_surface; // new in v1
178  }
179  else
180  {
182  }
183  }
184  break;
185  default:
187  };
188 }
189 
192 {
193  // Load into cache:
194  if (!m_cache_valid)
195  {
196  m_cache_valid = true;
200 
202  }
203 
204  size_t n;
205  const float *x, *y, *z;
206 
208  if (!n || !x) return;
209 
211  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
212  std::numeric_limits<double>::max());
214  -std::numeric_limits<double>::max(),
215  -std::numeric_limits<double>::max(),
216  -std::numeric_limits<double>::max());
217 
218  for (size_t i = 0; i < n; i++)
219  {
220  keep_min(bb_min.x, x[i]);
221  keep_max(bb_max.x, x[i]);
222  keep_min(bb_min.y, y[i]);
223  keep_max(bb_max.y, y[i]);
224  keep_min(bb_min.z, z[i]);
225  keep_max(bb_max.z, z[i]);
226  }
227 
228  // Convert to coordinates of my parent:
231 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
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...
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:251
GLdouble GLdouble z
Definition: glext.h:3879
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< 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:367
GLAPI void GLAPIENTRY glEnable(GLenum cap)
mrpt::maps::CSimplePointsMap m_cache_points
GLAPI void GLAPIENTRY glPointSize(GLfloat size)
GLenum GLsizei n
Definition: glext.h:5136
#define GL_TRIANGLES
Definition: glew.h:277
STL namespace.
#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...
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:54
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
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() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:433
#define GL_POINTS
Definition: glew.h:273
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:287
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:54
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:224
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
IMPLEMENTS_SERIALIZABLE(CPlanarLaserScan, CRenderizableDisplayList, mrpt::opengl) CPlanarLaserScan
GLuint in
Definition: glext.h:7391
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
GLAPI void GLAPIENTRY glEnd(void)
GLenum GLint GLint y
Definition: glext.h:3542
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:271
const auto bb_min
#define GL_LINES
Definition: glew.h:274
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
GLAPI void GLAPIENTRY glDisable(GLenum cap)
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
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)
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:232
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: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019