17 #if MRPT_HAS_OPENGL_GLUT 24 #include <OpenGL/gl.h> 31 #if MRPT_HAS_OPENGL_GLUT && defined(_WIN32) 34 #pragma comment(lib, "opengl32.lib") 35 #pragma comment(lib, "GlU32.lib") 37 #endif // MRPT_HAS_OPENGL_GLUT 50 void CAngularObservationMesh::addTriangle(
53 const TPoint3D* arr[3] = {&p1, &p2, &p3};
55 for (
size_t i = 0; i < 3; i++)
60 t.r[i] = m_color.R * (1.f / 255);
61 t.g[i] = m_color.G * (1.f / 255);
62 t.b[i] = m_color.B * (1.f / 255);
63 t.a[i] = m_color.A * (1.f / 255);
65 triangles.push_back(
t);
66 CRenderizableDisplayList::notifyChange();
69 void CAngularObservationMesh::updateMesh()
const 71 CRenderizableDisplayList::notifyChange();
73 size_t numRows = scanSet.size();
77 actualMesh.setSize(0, 0);
78 validityMatrix.setSize(0, 0);
82 if (pitchBounds.size() != numRows && pitchBounds.size() != 2)
return;
83 size_t numCols = scanSet[0].scan.size();
84 actualMesh.setSize(numRows, numCols);
85 validityMatrix.setSize(numRows, numCols);
86 double* pitchs =
new double[numRows];
87 if (pitchBounds.size() == 2)
89 double p1 = pitchBounds[0];
90 double p2 = pitchBounds[1];
91 for (
size_t i = 0; i < numRows; i++)
92 pitchs[i] = p1 + (p2 - p1) *
static_cast<double>(i) /
93 static_cast<double>(numRows - 1);
96 for (
size_t i = 0; i < numRows; i++) pitchs[i] = pitchBounds[i];
97 const bool rToL = scanSet[0].rightToLeft;
98 for (
size_t i = 0; i < numRows; i++)
100 const auto& scan = scanSet[i].scan;
101 const auto& valid = scanSet[i].validRange;
102 const double pitchIncr = scanSet[i].deltaPitch;
103 const double aperture = scanSet[i].aperture;
104 const CPose3D origin = scanSet[i].sensorPose;
106 for (
size_t j = 0; j < numCols; j++)
107 if ((validityMatrix(i, j) = (valid[j] != 0)))
109 double pYaw = aperture * ((
static_cast<double>(j) /
110 static_cast<double>(numCols - 1)) -
115 CPose3D(0, 0, 0, rToL ? pYaw : -pYaw, pitchIncr)) +
121 triangles.reserve(2 * (numRows - 1) * (numCols - 1));
122 for (
size_t k = 0; k < numRows - 1; k++)
124 for (
size_t j = 0; j < numCols - 1; j++)
126 int b1 = validityMatrix(k, j) ? 1 : 0;
127 int b2 = validityMatrix(k, j + 1) ? 1 : 0;
128 int b3 = validityMatrix(k + 1, j) ? 1 : 0;
129 int b4 = validityMatrix(k + 1, j + 1) ? 1 : 0;
130 switch (
b1 +
b2 +
b3 + b4)
139 actualMesh(k, j + 1), actualMesh(k + 1, j),
140 actualMesh(k + 1, j + 1));
143 actualMesh(k, j), actualMesh(k + 1, j),
144 actualMesh(k + 1, j + 1));
147 actualMesh(k, j), actualMesh(k, j + 1),
148 actualMesh(k + 1, j + 1));
151 actualMesh(k, j), actualMesh(k, j + 1),
152 actualMesh(k + 1, j));
156 actualMesh(k, j), actualMesh(k, j + 1),
157 actualMesh(k + 1, j));
159 actualMesh(k + 1, j + 1), actualMesh(k, j + 1),
160 actualMesh(k + 1, j));
167 void CAngularObservationMesh::render_dl()
const 169 #if MRPT_HAS_OPENGL_GLUT 170 if (mEnableTransparency)
182 if (!meshUpToDate) updateMesh();
184 for (
size_t i = 0; i < triangles.size(); i++)
187 float ax =
t.
x[1] -
t.x[0];
188 float bx =
t.x[2] -
t.x[0];
189 float ay =
t.y[1] -
t.y[0];
190 float by =
t.y[2] -
t.y[0];
191 float az =
t.z[1] -
t.z[0];
192 float bz =
t.z[2] -
t.z[0];
195 for (
int k = 0; k < 3; k++)
200 if (mWireframe)
glEnd();
202 if (!mWireframe)
glEnd();
217 bool CAngularObservationMesh::setScanSet(
218 const std::vector<mrpt::obs::CObservation2DRangeScan>& scans)
220 CRenderizableDisplayList::notifyChange();
223 if (scans.size() > 0)
225 size_t setSize = scans[0].scan.size();
226 bool rToL = scans[0].rightToLeft;
229 it != scans.end(); ++it)
231 if (it->scan.size() !=
setSize)
return false;
232 if (it->rightToLeft != rToL)
return false;
236 meshUpToDate =
false;
240 void CAngularObservationMesh::setPitchBounds(
241 const double initial,
const double final)
243 CRenderizableDisplayList::notifyChange();
246 pitchBounds.push_back(initial);
247 pitchBounds.push_back(
final);
248 meshUpToDate =
false;
250 void CAngularObservationMesh::setPitchBounds(
const std::vector<double>& bounds)
252 CRenderizableDisplayList::notifyChange();
254 pitchBounds = bounds;
255 meshUpToDate =
false;
257 void CAngularObservationMesh::getPitchBounds(
258 double& initial,
double&
final)
const 260 initial = pitchBounds.front();
261 final = pitchBounds.back();
263 void CAngularObservationMesh::getPitchBounds(std::vector<double>& bounds)
const 265 bounds = pitchBounds;
267 void CAngularObservationMesh::getScanSet(
268 std::vector<CObservation2DRangeScan>& scans)
const 273 void CAngularObservationMesh::generateSetOfTriangles(
276 if (!meshUpToDate) updateMesh();
277 res->insertTriangles(triangles.begin(), triangles.end());
292 void CAngularObservationMesh::generatePointCloud(
CPointsMap* out_map)
const 312 uint8_t CAngularObservationMesh::serializeGetVersion()
const {
return 0; }
313 void CAngularObservationMesh::serializeTo(
316 writeToStreamRender(out);
318 out << pitchBounds << scanSet << mWireframe << mEnableTransparency;
321 void CAngularObservationMesh::serializeFrom(
327 readFromStreamRender(
in);
328 in >> pitchBounds >> scanSet >> mWireframe >> mEnableTransparency;
333 meshUpToDate =
false;
337 std::vector<double>& vals)
const 339 double value = initialValue();
340 double incr = increment();
341 size_t am = amount();
343 for (
size_t i = 0; i < am - 1; i++,
value += incr) vals[i] =
value;
344 vals[am - 1] = finalValue();
349 if (!meshUpToDate) updateMesh();
351 for (
size_t i = 0; i < validityMatrix.rows(); i++)
352 for (
size_t j = 0; j < validityMatrix.cols(); j++)
353 if (validityMatrix(i, j))
count++;
355 for (
size_t i = 0; i < actualMesh.rows(); i++)
356 for (
size_t j = 0; j < actualMesh.cols(); j++)
357 if (validityMatrix(i, j))
359 (scanSet[i].sensorPose).asTPose(), actualMesh(i, j));
370 : lins(l), pDist(
p), pitchs()
372 pitchs.reserve(pi.size());
373 for (std::vector<double>::const_reverse_iterator it = pi.rbegin();
374 it != pi.rend(); ++it)
375 pitchs.push_back(*it);
383 for (
size_t i = 0; i < obs.
scan.
size(); i++)
387 obs.
aperture * ((
static_cast<double>(i) /
388 static_cast<double>(obs.
scan.
size() - 1)) -
402 void CAngularObservationMesh::getUntracedRays(
406 scanSet.begin(), scanSet.end(),
413 for (
size_t i = 0; i < 3; i++)
422 void CAngularObservationMesh::generateSetOfTriangles(
423 std::vector<TPolygon3D>&
res)
const 425 if (!meshUpToDate) updateMesh();
426 res.resize(triangles.size());
431 void CAngularObservationMesh::getBoundingBox(
434 if (!meshUpToDate) updateMesh();
437 std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
438 std::numeric_limits<double>::max());
440 -std::numeric_limits<double>::max(),
441 -std::numeric_limits<double>::max(),
442 -std::numeric_limits<double>::max());
444 for (
size_t i = 0; i < triangles.size(); i++)
471 m_pose.composePoint(bb_min, bb_min);
472 m_pose.composePoint(bb_max, bb_max);
void clear()
Erase all the contents of the map.
mrpt::math::TPose3D asTPose() const
GLuint GLuint GLsizei count
void operator()(const CObservation2DRangeScan &obs)
A mesh built from a set of 2D laser scan observations.
GLAPI void GLAPIENTRY glEnable(GLenum cap)
GLboolean GLenum GLenum GLvoid * values
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
GLAPI void GLAPIENTRY glNormal3f(GLfloat nx, GLfloat ny, GLfloat nz)
#define GL_ONE_MINUS_SRC_ALPHA
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...
#define GL_COLOR_MATERIAL
IMPLEMENTS_SERIALIZABLE(CAngularObservationMesh, CRenderizableDisplayList, mrpt::opengl) void CAngularObservationMesh
GLAPI void GLAPIENTRY glShadeModel(GLenum mode)
GLsizei GLsizei GLuint * obj
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
A renderizable object suitable for rendering with OpenGL's display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define ASSERT_(f)
Defines an assertion mechanism.
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.
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::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
STLCONTAINER::const_iterator end() const
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...
double x
X,Y,Z coordinates.
CAngularObservationMesh_fnctr(CPointsMap *p)
GLAPI void GLAPIENTRY glBegin(GLenum mode)
A class used to store a 3D point.
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.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
TPolygon3D createFromTriangle(const CSetOfTriangles::TTriangle &t)
Virtual base class for "archives": classes abstracting I/O streams.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
STLCONTAINER::const_iterator begin() const
double deltaPitch
If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitc...
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)
GLsizei const GLfloat * value
GLAPI void GLAPIENTRY glColor4f(GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
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.
const Scalar * const_iterator
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.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.