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);