17 #if MRPT_HAS_OPENGL_GLUT 18 #ifdef MRPT_OS_WINDOWS 24 #include <OpenGL/gl.h> 31 #if MRPT_HAS_OPENGL_GLUT && defined(MRPT_OS_WINDOWS) 33 #if defined(_MSC_VER) || defined(__BORLANDC__) 34 #pragma comment(lib, "opengl32.lib") 35 #pragma comment(lib, "GlU32.lib") 37 #endif // MRPT_HAS_OPENGL_GLUT 51 void CAngularObservationMesh::addTriangle(
54 const TPoint3D* arr[3] = {&p1, &p2, &p3};
56 for (
size_t i = 0; i < 3; i++)
61 t.r[i] = m_color.R * (1.f / 255);
62 t.g[i] = m_color.G * (1.f / 255);
63 t.b[i] = m_color.B * (1.f / 255);
64 t.a[i] = m_color.A * (1.f / 255);
66 triangles.push_back(
t);
67 CRenderizableDisplayList::notifyChange();
70 void CAngularObservationMesh::updateMesh()
const 72 CRenderizableDisplayList::notifyChange();
74 size_t numRows = scanSet.size();
78 actualMesh.setSize(0, 0);
79 validityMatrix.setSize(0, 0);
83 if (pitchBounds.size() != numRows && pitchBounds.size() != 2)
return;
84 size_t numCols = scanSet[0].scan.size();
85 actualMesh.setSize(numRows, numCols);
86 validityMatrix.setSize(numRows, numCols);
87 double* pitchs =
new double[numRows];
88 if (pitchBounds.size() == 2)
90 double p1 = pitchBounds[0];
91 double p2 = pitchBounds[1];
92 for (
size_t i = 0; i < numRows; i++)
94 (p2 - p1) *
static_cast<double>(i) /
95 static_cast<double>(numRows - 1);
98 for (
size_t i = 0; i < numRows; i++) pitchs[i] = pitchBounds[i];
99 const bool rToL = scanSet[0].rightToLeft;
100 for (
size_t i = 0; i < numRows; i++)
102 const std::vector<float>& scan = scanSet[i].scan;
103 const std::vector<char> valid = scanSet[i].validRange;
104 const double pitchIncr = scanSet[i].deltaPitch;
105 const double aperture = scanSet[i].aperture;
106 const CPose3D origin = scanSet[i].sensorPose;
108 for (
size_t j = 0; j < numCols; j++)
109 if ((validityMatrix(i, j) = (valid[j] != 0)))
111 double pYaw = aperture * ((
static_cast<double>(j) /
112 static_cast<double>(numCols - 1)) -
117 CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
122 triangles.reserve(2 * (numRows - 1) * (numCols - 1));
123 for (
size_t k = 0; k < numRows - 1; k++)
125 for (
size_t j = 0; j < numCols - 1; j++)
127 int b1 = validityMatrix(k, j) ? 1 : 0;
128 int b2 = validityMatrix(k, j + 1) ? 1 : 0;
129 int b3 = validityMatrix(k + 1, j) ? 1 : 0;
130 int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
131 switch (
b1 +
b2 +
b3 + b4)
140 actualMesh(k, j + 1), actualMesh(k + 1, j),
141 actualMesh(k + 1, j + 1));
144 actualMesh(k, j), actualMesh(k + 1, j),
145 actualMesh(k + 1, j + 1));
148 actualMesh(k, j), actualMesh(k, j + 1),
149 actualMesh(k + 1, j + 1));
152 actualMesh(k, j), actualMesh(k, j + 1),
153 actualMesh(k + 1, j));
157 actualMesh(k, j), actualMesh(k, j + 1),
158 actualMesh(k + 1, j));
160 actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
161 actualMesh(k + 1, j));
168 void CAngularObservationMesh::render_dl()
const 170 #if MRPT_HAS_OPENGL_GLUT 171 if (mEnableTransparency)
183 if (!meshUpToDate) updateMesh();
185 for (
size_t i = 0; i < triangles.size(); i++)
188 float ax =
t.
x[1] -
t.x[0];
189 float bx =
t.x[2] -
t.x[0];
190 float ay =
t.y[1] -
t.y[0];
191 float by =
t.y[2] -
t.y[0];
192 float az =
t.z[1] -
t.z[0];
193 float bz =
t.z[2] -
t.z[0];
196 for (
int i = 0; i < 3; i++)
201 if (mWireframe)
glEnd();
203 if (!mWireframe)
glEnd();
218 bool CAngularObservationMesh::setScanSet(
219 const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
221 CRenderizableDisplayList::notifyChange();
224 if (scans.size() > 0)
226 size_t setSize = scans[0].scan.size();
227 bool rToL = scans[0].rightToLeft;
230 it != scans.end(); ++it)
232 if (it->scan.size() !=
setSize)
return false;
233 if (it->rightToLeft != rToL)
return false;
237 meshUpToDate =
false;
241 void CAngularObservationMesh::setPitchBounds(
242 const double initial,
const double final)
244 CRenderizableDisplayList::notifyChange();
247 pitchBounds.push_back(initial);
248 pitchBounds.push_back(
final);
249 meshUpToDate =
false;
251 void CAngularObservationMesh::setPitchBounds(
const std::vector<double>& bounds)
253 CRenderizableDisplayList::notifyChange();
255 pitchBounds = bounds;
256 meshUpToDate =
false;
258 void CAngularObservationMesh::getPitchBounds(
259 double& initial,
double&
final)
const 261 initial = pitchBounds.front();
262 final = pitchBounds.back();
264 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds)
const 266 bounds = pitchBounds;
268 void CAngularObservationMesh::getScanSet(
269 std::vector<CObservation2DRangeScan>& scans)
const 274 void CAngularObservationMesh::generateSetOfTriangles(
277 if (!meshUpToDate) updateMesh();
278 res->insertTriangles(triangles.begin(), triangles.end());
293 void CAngularObservationMesh::generatePointCloud(
CPointsMap* out_map)
const 317 void CAngularObservationMesh::writeToStream(
324 writeToStreamRender(out);
326 out << pitchBounds << scanSet << mWireframe << mEnableTransparency;
334 void CAngularObservationMesh::readFromStream(
340 readFromStreamRender(
in);
341 in >> pitchBounds >> scanSet >> mWireframe >> mEnableTransparency;
346 meshUpToDate =
false;
350 std::vector<double>& vals)
const 352 double value = initialValue();
353 double incr = increment();
354 size_t am = amount();
356 for (
size_t i = 0; i < am - 1; i++,
value += incr) vals[i] =
value;
357 vals[am - 1] = finalValue();
362 if (!meshUpToDate) updateMesh();
364 for (
size_t i = 0; i < validityMatrix.getRowCount(); i++)
365 for (
size_t j = 0; j < validityMatrix.getColCount(); j++)
366 if (validityMatrix(i, j))
count++;
368 for (
size_t i = 0; i < actualMesh.getRowCount(); i++)
369 for (
size_t j = 0; j < actualMesh.getColCount(); j++)
370 if (validityMatrix(i, j))
372 TPose3D(scanSet[i].sensorPose), actualMesh(i, j));
383 : lins(l), pDist(
p), pitchs()
385 pitchs.reserve(pi.size());
386 for (std::vector<double>::const_reverse_iterator it = pi.rbegin();
387 it != pi.rend(); ++it)
388 pitchs.push_back(*it);
397 for (
size_t i = 0; i < obs.
scan.
size(); i++)
401 obs.
aperture * ((
static_cast<double>(i) /
402 static_cast<double>(obs.
scan.
size() - 1)) -
416 void CAngularObservationMesh::getUntracedRays(
420 scanSet.begin(), scanSet.end(),
427 for (
size_t i = 0; i < 3; i++)
436 void CAngularObservationMesh::generateSetOfTriangles(
437 std::vector<TPolygon3D>&
res)
const 439 if (!meshUpToDate) updateMesh();
440 res.resize(triangles.size());
445 void CAngularObservationMesh::getBoundingBox(
448 if (!meshUpToDate) updateMesh();
451 std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
452 std::numeric_limits<double>::max());
454 -std::numeric_limits<double>::max(),
455 -std::numeric_limits<double>::max(),
456 -std::numeric_limits<double>::max());
458 for (
size_t i = 0; i < triangles.size(); i++)
485 m_pose.composePoint(bb_min, bb_min);
486 m_pose.composePoint(bb_max, bb_max);
void clear()
Erase all the contents of the map.
GLuint GLuint GLsizei count
void operator()(const CObservation2DRangeScan &obs)
A mesh built from a set of 2D laser scan observations.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLboolean GLenum GLenum GLvoid * values
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
STLCONTAINER::const_iterator begin() const
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
const Scalar * const_iterator
#define GL_ONE_MINUS_SRC_ALPHA
#define GL_COLOR_MATERIAL
IMPLEMENTS_SERIALIZABLE(CAngularObservationMesh, CRenderizableDisplayList, mrpt::opengl) void CAngularObservationMesh
std::shared_ptr< CSetOfTriangles > Ptr
GLAPI void GLAPIENTRY glShadeModel(GLenum mode)
GLsizei GLsizei GLuint * obj
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
A renderizable object suitable for rendering with OpenGL's display lists.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
std::vector< double > pitchs
This base provides a set of functions for maths stuff.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
FAddUntracedLines(CSetOfLines::Ptr &l, const CPoint3D &p, const std::vector< double > &pi)
This namespace contains representation of robot actions and observations.
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
double x
X,Y,Z coordinates.
CAngularObservationMesh_fnctr(CPointsMap *p)
GLAPI void GLAPIENTRY glBegin(GLenum mode)
A class used to store a 3D point.
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...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
GLAPI void GLAPIENTRY glVertex3f(GLfloat x, GLfloat y, GLfloat z)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STLCONTAINER::const_iterator end() const
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
TPolygon3D createFromTriangle(const CSetOfTriangles::TTriangle &t)
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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
The namespace for 3D scene representation and rendering.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
void operator()(const CObservation2DRangeScan &obj)
GLAPI void GLAPIENTRY glEnd(void)
std::shared_ptr< CSetOfLines > Ptr
GLsizei const GLfloat * value
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters.
GLuint GLenum GLenum transform
GLAPI void GLAPIENTRY glDisable(GLenum cap)
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
3D polygon, inheriting from std::vector<TPoint3D>
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.