MRPT  1.9.9
CGeneralizedCylinder.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 "opengl-precomp.h" // Precompiled header
11 
12 #include <mrpt/math/geometry.h>
13 #include <mrpt/math/ops_matrices.h> // for extract*()
15 #include <mrpt/poses/CPose3D.h>
18 #include <Eigen/Dense>
19 #include "opengl_internals.h"
20 
21 using namespace mrpt;
22 using namespace mrpt::math;
23 using namespace mrpt::opengl;
24 using namespace mrpt::poses;
25 
26 using namespace std;
27 
30 
31 void CGeneralizedCylinder::TQuadrilateral::calculateNormal()
32 {
33  double ax = points[1].x - points[0].x;
34  double ay = points[1].y - points[0].y;
35  double az = points[1].z - points[0].z;
36  double bx = points[2].x - points[0].x;
37  double by = points[2].y - points[0].y;
38  double bz = points[2].z - points[0].z;
39  normal[0] = az * by - ay * bz;
40  normal[1] = ax * bz - az * bx;
41  normal[2] = ay * bx - ax * by;
42  double s = 0;
43  for (double i : normal) s += i * i;
44  s = sqrt(s);
45  for (double& i : normal) i /= s;
46 }
47 
48 #if MRPT_HAS_OPENGL_GLUT
49 class FQuadrilateralRenderer
50 {
51  private:
52  const mrpt::img::TColor& color;
53 
54  public:
55  void operator()(const CGeneralizedCylinder::TQuadrilateral& t) const
56  {
57  glNormal3d(t.normal[0], t.normal[1], t.normal[2]);
58  for (const auto& point : t.points)
59  glVertex3d(point.x, point.y, point.z);
60  }
61  FQuadrilateralRenderer(const mrpt::img::TColor& c) : color(c) {}
62  ~FQuadrilateralRenderer() = default;
63 };
64 #endif
65 
66 void CGeneralizedCylinder::getMeshIterators(
67  const vector<TQuadrilateral>& m,
68  vector<TQuadrilateral>::const_iterator& begin,
69  vector<TQuadrilateral>::const_iterator& end) const
70 {
71  if (fullyVisible)
72  {
73  begin = m.begin();
74  end = m.end();
75  }
76  else
77  {
78  ASSERT_(getNumberOfSections() > 0);
79  size_t qps =
80  m.size() / getNumberOfSections(); // quadrilaterals per section
81  begin = m.begin() + qps * firstSection;
82  end = m.begin() + qps * lastSection;
83  }
84 }
85 
86 void CGeneralizedCylinder::render_dl() const
87 {
88 #if MRPT_HAS_OPENGL_GLUT
89  if (!meshUpToDate) updateMesh();
91  glEnable(GL_BLEND);
92  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
93 
94  glBegin(GL_QUADS);
95  glColor4ub(m_color.R, m_color.G, m_color.B, m_color.A);
96  vector<TQuadrilateral>::const_iterator begin, end;
97  getMeshIterators(mesh, begin, end);
98  for_each(begin, end, FQuadrilateralRenderer(m_color));
99  glEnd();
100  if (m_color.A != 1.0) glDisable(GL_BLEND);
101 
102 #endif
103 }
104 
105 inline void createMesh(
106  const CMatrixDynamic<TPoint3D_data<double>>& pointsMesh, size_t R, size_t C,
107  vector<CGeneralizedCylinder::TQuadrilateral>& mesh)
108 {
109  mesh.reserve(R * C);
110  for (size_t i = 0; i < R; i++)
111  for (size_t j = 0; j < C; j++)
112  mesh.emplace_back(
113  TPoint3D(pointsMesh(i, j)), TPoint3D(pointsMesh(i, j + 1)),
114  TPoint3D(pointsMesh(i + 1, j + 1)),
115  TPoint3D(pointsMesh(i + 1, j)));
116 }
117 
118 bool CGeneralizedCylinder::traceRay(const CPose3D& o, double& dist) const
119 {
120  if (!meshUpToDate || !polysUpToDate) updatePolys();
121  return math::traceRay(polys, (o - this->m_pose).asTPose(), dist);
122 }
123 
124 void CGeneralizedCylinder::updateMesh() const
125 {
126  CRenderizableDisplayList::notifyChange();
127 
128  size_t A = axis.size();
129  vector<TPoint3D> genX = generatrix;
130  if (closed && genX.size() > 2) genX.push_back(genX[0]);
131  size_t G = genX.size();
132  mesh.clear();
133  if (A > 1 && G > 1)
134  {
135  pointsMesh = CMatrixDynamic<TPoint3D_data<double>>(A, G);
136  for (size_t i = 0; i < A; i++)
137  for (size_t j = 0; j < G; j++)
138  pointsMesh(i, j) = axis[i].composePoint(genX[j]);
139  createMesh(pointsMesh, A - 1, G - 1, mesh);
140  }
141  meshUpToDate = true;
142  polysUpToDate = false;
143 }
144 
145 uint8_t CGeneralizedCylinder::serializeGetVersion() const { return 1; }
146 void CGeneralizedCylinder::serializeTo(mrpt::serialization::CArchive& out) const
147 {
148  writeToStreamRender(out);
149  out << axis << generatrix; // In version 0, axis was a vector<TPoint3D>. In
150  // version 1, it is a vector<CPose3D>.
151 }
152 
153 void CGeneralizedCylinder::serializeFrom(
154  mrpt::serialization::CArchive& in, uint8_t version)
155 {
156  switch (version)
157  {
158  case 0:
159  {
160  readFromStreamRender(in);
161  vector<TPoint3D> a;
162  in >> a >> generatrix;
163  generatePoses(a, axis);
164  meshUpToDate = false;
165  polysUpToDate = false;
166  break;
167  }
168  case 1:
169  readFromStreamRender(in);
170  // version 0
171  in >> axis >> generatrix;
172  meshUpToDate = false;
173  polysUpToDate = false;
174  break;
175  default:
177  };
178  CRenderizableDisplayList::notifyChange();
179 }
180 
182  CPolyhedron::Ptr& poly, const vector<TPoint3D>& profile,
183  const CPose3D& pose)
184 {
185  math::TPolygon3D p(profile.size());
186  for (size_t i = 0; i < profile.size(); i++)
187  pose.composePoint(
188  profile[i].x, profile[i].y, profile[i].z, p[i].x, p[i].y, p[i].z);
189  vector<math::TPolygon3D> convexPolys;
190  if (!math::splitInConvexComponents(p, convexPolys))
191  convexPolys.push_back(p);
192  poly = std::make_shared<CPolyhedron>(convexPolys);
193 }
194 
195 void CGeneralizedCylinder::getOrigin(CPolyhedron::Ptr& poly) const
196 {
197  if (!meshUpToDate) updateMesh();
198  if (axis.size() < 2 || generatrix.size() < 3)
199  throw std::logic_error("Not enough points.");
200  size_t i = fullyVisible ? 0 : firstSection;
201  generatePolygon(poly, generatrix, axis[i]);
202  poly->setPose(this->m_pose);
203  poly->setColor(getColor());
204 }
205 
206 void CGeneralizedCylinder::getEnd(CPolyhedron::Ptr& poly) const
207 {
208  if (!meshUpToDate) updateMesh();
209  if (axis.size() < 2 || generatrix.size() < 3)
210  throw std::logic_error("Not enough points.");
211  size_t i = (fullyVisible ? axis.size() : lastSection) - 1;
212  generatePolygon(poly, generatrix, axis[i]);
213  poly->setPose(this->m_pose);
214  poly->setColor(getColor());
215 }
216 
217 void CGeneralizedCylinder::generateSetOfPolygons(
218  std::vector<TPolygon3D>& res) const
219 {
220  if (!meshUpToDate || !polysUpToDate) updatePolys();
221  size_t N = polys.size();
222  res.resize(N);
223  for (size_t i = 0; i < N; i++) res[i] = polys[i].poly;
224 }
225 
226 void CGeneralizedCylinder::getClosedSection(
227  size_t index1, size_t index2, mrpt::opengl::CPolyhedron::Ptr& poly) const
228 {
229  if (index1 > index2) swap(index1, index2);
230  if (index2 >= axis.size() - 1) throw std::logic_error("Out of range");
231  if (!meshUpToDate) updateMesh();
232  auto ROIpoints = CMatrixDynamic<TPoint3D_data<double>>(
233  pointsMesh.asEigen().block(index1, 0, index2 + 1, pointsMesh.cols()));
234 
235  // At this point, ROIpoints contains a matrix of TPoints in which the number
236  // of rows equals (index2-index1)+2 and there is a column
237  // for each vertex in the generatrix.
238  if (!closed)
239  {
240  CVectorDynamic<TPoint3D_data<double>> vec(ROIpoints.rows());
241  vec.asEigen() = ROIpoints.col(0);
242  ROIpoints.appendCol(vec);
243  }
244  vector<TPoint3D> vertices;
245  vertices.reserve(ROIpoints.rows() * ROIpoints.cols());
246  for (const auto& d : ROIpoints) vertices.push_back(TPoint3D(d));
247 
248  size_t nr = ROIpoints.rows() - 1;
249  size_t nc = ROIpoints.cols() - 1;
250  vector<vector<uint32_t>> faces;
251  faces.reserve(nr * nc + 2);
252  vector<uint32_t> tmp(4);
253  for (size_t i = 0; i < nr; i++)
254  for (size_t j = 0; j < nc; j++)
255  {
256  size_t base = (nc + 1) * i + j;
257  tmp[0] = base;
258  tmp[1] = base + 1;
259  tmp[2] = base + nc + 2;
260  tmp[3] = base + nc + 1;
261  faces.push_back(tmp);
262  }
263  tmp.resize(nr + 1);
264  for (size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 1);
265  faces.push_back(tmp);
266  for (size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 2) - 1;
267  poly = CPolyhedron::Create(vertices, faces);
268 }
269 
270 void CGeneralizedCylinder::removeVisibleSectionAtStart()
271 {
272  CRenderizableDisplayList::notifyChange();
273  if (fullyVisible)
274  {
275  if (!getNumberOfSections()) throw std::logic_error("No more sections");
276  fullyVisible = false;
277  firstSection = 1;
278  lastSection = getNumberOfSections();
279  }
280  else if (firstSection >= lastSection)
281  throw std::logic_error("No more sections");
282  else
283  firstSection++;
284 }
285 void CGeneralizedCylinder::removeVisibleSectionAtEnd()
286 {
287  CRenderizableDisplayList::notifyChange();
288  if (fullyVisible)
289  {
290  if (!getNumberOfSections()) throw std::logic_error("No more sections");
291  fullyVisible = false;
292  firstSection = 0;
293  lastSection = getNumberOfSections() - 1;
294  }
295  else if (firstSection >= lastSection)
296  throw std::logic_error("No more sections");
297  else
298  lastSection--;
299 }
300 
301 void CGeneralizedCylinder::updatePolys() const
302 {
303  CRenderizableDisplayList::notifyChange();
304 
305  if (!meshUpToDate) updateMesh();
306  size_t N = mesh.size();
307  polys.resize(N);
308  TPolygon3D tmp(4);
309  for (size_t i = 0; i < N; i++)
310  {
311  for (size_t j = 0; j < 4; j++) tmp[j] = mesh[i].points[j];
312  polys[i] = tmp;
313  }
314  polysUpToDate = true;
315 }
316 
317 void CGeneralizedCylinder::generatePoses(
318  const vector<TPoint3D>& pIn, std::vector<mrpt::poses::CPose3D>& pOut)
319 {
320  size_t N = pIn.size();
321  if (N == 0)
322  {
323  pOut.resize(0);
324  return;
325  }
326  vector<double> yaws;
327  yaws.reserve(N);
328  vector<TPoint3D>::const_iterator it1 = pIn.begin(), it2;
329  for (;;)
330  if ((it2 = it1 + 1) == pIn.end())
331  break;
332  else
333  {
334  yaws.push_back(atan2(it2->y - it1->y, it2->x - it1->x));
335  it1 = it2;
336  }
337  yaws.push_back(*yaws.rbegin());
338  pOut.resize(N);
339  for (size_t i = 0; i < N; i++)
340  {
341  const TPoint3D& p = pIn[i];
342  pOut[i] = CPose3D(p.x, p.y, p.z, yaws[i], 0, 0);
343  }
344 }
345 
346 bool CGeneralizedCylinder::getFirstSectionPose(CPose3D& p)
347 {
348  if (axis.size() <= 0) return false;
349  p = axis[0];
350  return true;
351 }
352 
353 bool CGeneralizedCylinder::getLastSectionPose(CPose3D& p)
354 {
355  if (axis.size() <= 0) return false;
356  p = *axis.rbegin();
357  return true;
358 }
359 
360 bool CGeneralizedCylinder::getFirstVisibleSectionPose(CPose3D& p)
361 {
362  if (fullyVisible) return getFirstSectionPose(p);
363  if (getVisibleSections() <= 0) return false;
364  p = axis[firstSection];
365  return true;
366 }
367 
368 bool CGeneralizedCylinder::getLastVisibleSectionPose(CPose3D& p)
369 {
370  if (fullyVisible) return getLastSectionPose(p);
371  if (getVisibleSections() <= 0) return false;
372  p = axis[lastSection];
373  return true;
374 }
375 
376 void CGeneralizedCylinder::getBoundingBox(
378 {
379  bb_min = TPoint3D(0, 0, 0);
380  bb_max = TPoint3D(0, 0, 0);
381 
382  // Convert to coordinates of my parent:
383  m_pose.composePoint(bb_min, bb_min);
384  m_pose.composePoint(bb_max, bb_max);
385 }
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:2372
Trivially copiable underlying data for TPoint3D.
Definition: TPoint3D.h:20
Template for column vectors of dynamic size, compatible with Eigen.
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:2565
const double G
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
void createMesh(const CMatrixDynamic< TPoint3D_data< double >> &pointsMesh, size_t R, size_t C, vector< CGeneralizedCylinder::TQuadrilateral > &mesh)
STL namespace.
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
mrpt::math::TPoint3D points[4]
Quadrilateral`&#39;s points.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
void generatePolygon(CPolyhedron::Ptr &poly, const vector< TPoint3D > &profile, const CPose3D &pose)
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
IMPLEMENTS_SERIALIZABLE(CGeneralizedCylinder, CRenderizableDisplayList, mrpt::opengl) void CGeneralizedCylinder
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
const_iterator end() const
Definition: ts_hash_map.h:246
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Definition: ts_hash_map.h:240
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
This object represents any figure obtained by extruding any profile along a given axis...
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
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
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
A RGB color - 8bit.
Definition: TColor.h:20
Auxiliary struct holding any quadrilateral, represented by foour points.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:19



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