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
64  // Load into cache:
65  if (!m_cache_valid)
66  {
67  m_cache_valid = true;
71 
73  }
74 
75  size_t i, n;
76  const float *x, *y, *z;
77 
79  if (!n || !x) return;
80 
83 
84  // LINES
85  // ----------------------------
86  if (n > 1 && m_enable_line)
87  {
90 
93 
94  for (i = 0; i < n - 1; i++)
95  {
96  glVertex3f(x[i], y[i], z[i]);
97  glVertex3f(x[i + 1], y[i + 1], z[i + 1]);
98  }
99  glEnd();
101  }
102 
103  // POINTS
104  // ----------------------------
105  if (n > 0 && m_enable_points)
106  {
109 
112 
113  for (i = 0; i < n; i++)
114  {
115  glVertex3f(x[i], y[i], z[i]);
116  }
117  glEnd();
119  }
120 
121  // SURFACE:
122  // ------------------------------
123  if (n > 1 && m_enable_surface)
124  {
126 
128 
129  for (i = 0; i < n - 1; i++)
130  {
131  glVertex3f(
133  m_scan.sensorPose.z());
134  glVertex3f(x[i], y[i], z[i]);
135  glVertex3f(x[i + 1], y[i + 1], z[i + 1]);
136  }
137  glEnd();
139  }
140 
142 
143 #endif
144 }
145 
146 uint8_t CPlanarLaserScan::serializeGetVersion() const { return 1; }
148 {
150  out << m_scan;
154  << m_enable_points << m_enable_line << m_enable_surface; // new in v1
155 }
156 
158  mrpt::serialization::CArchive& in, uint8_t version)
159 {
160  switch (version)
161  {
162  case 0:
163  case 1:
164  {
166  in >> m_scan;
167  in >> m_line_width >> m_line_R >> m_line_G >> m_line_B >>
170  m_plane_B >> m_plane_A;
171 
172  if (version >= 1)
173  {
175  m_enable_surface; // new in v1
176  }
177  else
178  {
180  }
181  }
182  break;
183  default:
185  };
186 }
187 
190 {
191  // Load into cache:
192  if (!m_cache_valid)
193  {
194  m_cache_valid = true;
198 
200  }
201 
202  size_t n;
203  const float *x, *y, *z;
204 
206  if (!n || !x) return;
207 
209  std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
210  std::numeric_limits<double>::max());
212  -std::numeric_limits<double>::max(),
213  -std::numeric_limits<double>::max(),
214  -std::numeric_limits<double>::max());
215 
216  for (size_t i = 0; i < n; i++)
217  {
218  keep_min(bb_min.x, x[i]);
219  keep_max(bb_max.x, x[i]);
220  keep_min(bb_min.y, y[i]);
221  keep_max(bb_max.y, y[i]);
222  keep_min(bb_min.z, z[i]);
223  keep_max(bb_max.z, z[i]);
224  }
225 
226  // Convert to coordinates of my parent:
229 }
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) ...
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...
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:253
GLdouble GLdouble z
Definition: glext.h:3879
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
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:54
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
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
This base provides a set of functions for maths stuff.
void writeToStreamRender(mrpt::serialization::CArchive &out) const
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)
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...
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:218
mrpt::vision::TStereoCalibResults out
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:273
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, 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...
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:183
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:234
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: cfb2df9d3 Fri Nov 15 06:57:14 2019 +0100 at vie nov 15 07:00:11 CET 2019