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 
78  m_cache_points.getPointsBuffer(n, x, y, z);
79  if (!n || !x) return;
80 
81  glEnable(GL_BLEND);
82  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
83 
84  // LINES
85  // ----------------------------
86  if (n > 1 && m_enable_line)
87  {
88  glLineWidth(m_line_width);
90 
91  glBegin(GL_LINES);
92  glColor4f(m_line_R, m_line_G, m_line_B, m_line_A);
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  {
107  glPointSize(m_points_width);
109 
110  glBegin(GL_POINTS);
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  {
125  glBegin(GL_TRIANGLES);
126 
127  glColor4f(m_plane_R, m_plane_G, m_plane_B, m_plane_A);
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 
141  glDisable(GL_BLEND);
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  {
174  in >> m_enable_points >> m_enable_line >>
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 
205  m_cache_points.getPointsBuffer(n, x, y, z);
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
mrpt::maps::CSimplePointsMap m_cache_points
STL namespace.
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:54
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.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
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.
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
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
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
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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020