MRPT  1.9.9
CEllipsoid.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/CMatrixF.h>
13 #include <mrpt/math/TLine3D.h>
14 #include <mrpt/math/geometry.h>
16 #include <mrpt/opengl/CEllipsoid.h>
18 #include <Eigen/Dense>
19 #include "opengl_internals.h"
20 
21 using namespace mrpt;
22 using namespace mrpt::opengl;
23 using namespace mrpt::math;
24 using namespace std;
25 
27 
28 /*---------------------------------------------------------------
29  render
30  ---------------------------------------------------------------*/
31 void CEllipsoid::render_dl() const
32 {
33 #if MRPT_HAS_OPENGL_GLUT
35 
36  const size_t dim = m_cov.cols();
37 
38  if (m_eigVal(0, 0) != 0.0 && m_eigVal(1, 1) != 0.0 &&
39  (dim == 2 || m_eigVal(2, 2) != 0.0) && m_quantiles != 0.0)
40  {
41  glEnable(GL_BLEND);
43  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
45  glLineWidth(m_lineWidth);
47 
48  if (dim == 2)
49  {
50  glDisable(GL_LIGHTING); // Disable lights when drawing lines
51 
52  // ---------------------
53  // 2D ellipse
54  // ---------------------
55 
56  /* Equivalent MATLAB code:
57  *
58  * q=1;
59  * [vec val]=eig(C);
60  * M=(q*val*vec)';
61  * R=M*[x;y];
62  * xx=R(1,:);yy=R(2,:);
63  * plot(xx,yy), axis equal;
64  */
65 
66  double ang;
67  unsigned int i;
68 
69  // Compute the new vectors for the ellipsoid:
70  auto M = CMatrixDouble(m_eigVal.asEigen() * m_eigVec.transpose());
71  M *= double(m_quantiles);
72 
73  glBegin(GL_LINE_LOOP);
74 
75  // Compute the points of the 2D ellipse:
76  for (i = 0, ang = 0; i < m_2D_segments;
77  i++, ang += (M_2PI / m_2D_segments))
78  {
79  double ccos = cos(ang);
80  double ssin = sin(ang);
81 
82  const float x = ccos * M(0, 0) + ssin * M(1, 0);
83  const float y = ccos * M(0, 1) + ssin * M(1, 1);
84 
85  glVertex2f(x, y);
86  } // end for points on ellipse
87 
88  glEnd();
89 
90  // 2D: Save bounding box:
91  const double max_radius =
92  m_quantiles * std::max(m_eigVal(0, 0), m_eigVal(1, 1));
93  m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
94  m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
95  // Convert to coordinates of my parent:
96  m_pose.composePoint(m_bb_min, m_bb_min);
97  m_pose.composePoint(m_bb_max, m_bb_max);
98 
99  glEnable(GL_LIGHTING);
100  }
101  else
102  {
103  // ---------------------
104  // 3D ellipsoid
105  // ---------------------
106  GLfloat mat[16];
107 
108  // A homogeneous transformation matrix, in this order:
109  //
110  // 0 4 8 12
111  // 1 5 9 13
112  // 2 6 10 14
113  // 3 7 11 15
114  //
115  mat[3] = mat[7] = mat[11] = 0;
116  mat[15] = 1;
117  mat[12] = mat[13] = mat[14] = 0;
118 
119  mat[0] = m_eigVec(0, 0);
120  mat[1] = m_eigVec(1, 0);
121  mat[2] = m_eigVec(2, 0); // New X-axis
122  mat[4] = m_eigVec(0, 1);
123  mat[5] = m_eigVec(1, 1);
124  mat[6] = m_eigVec(2, 1); // New X-axis
125  mat[8] = m_eigVec(0, 2);
126  mat[9] = m_eigVec(1, 2);
127  mat[10] = m_eigVec(2, 2); // New X-axis
128 
129  GLUquadricObj* obj = gluNewQuadric();
131 
132  if (!m_drawSolid3D)
133  glDisable(GL_LIGHTING); // Disable lights when drawing lines
134 
135  gluQuadricDrawStyle(obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
136 
137  glPushMatrix();
138  glMultMatrixf(mat);
139  glScalef(
140  m_eigVal(0, 0) * m_quantiles, m_eigVal(1, 1) * m_quantiles,
141  m_eigVal(2, 2) * m_quantiles);
142 
143  gluSphere(obj, 1, m_3D_segments, m_3D_segments);
145 
146  glPopMatrix();
147 
148  gluDeleteQuadric(obj);
150 
151  // 3D: Save bounding box:
152  const double max_radius =
153  m_quantiles *
154  std::max(
155  m_eigVal(0, 0), std::max(m_eigVal(1, 1), m_eigVal(2, 2)));
156  m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
157  m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
158  // Convert to coordinates of my parent:
159  m_pose.composePoint(m_bb_min, m_bb_min);
160  m_pose.composePoint(m_bb_max, m_bb_max);
161  }
162 
163  glDisable(GL_BLEND);
164 
165  glEnable(GL_LIGHTING);
166  }
167  MRPT_END_WITH_CLEAN_UP(cout << "Covariance matrix leading to error is:"
168  << endl
169  << m_cov << endl;);
170 #endif
171 }
172 
173 uint8_t CEllipsoid::serializeGetVersion() const { return 1; }
175 {
176  writeToStreamRender(out);
177  out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments
178  << (uint32_t)m_3D_segments << m_lineWidth;
179 }
180 
182  mrpt::serialization::CArchive& in, uint8_t version)
183 {
184  switch (version)
185  {
186  case 0:
187  case 1:
188  {
189  uint32_t i;
190  readFromStreamRender(in);
191  if (version == 0)
192  {
193  CMatrixF c;
194  in >> c;
195  m_cov = c.cast_double();
196  }
197  else
198  {
199  in >> m_cov;
200  }
201 
202  in >> m_drawSolid3D >> m_quantiles;
203  in >> i;
204  m_2D_segments = i;
205  in >> i;
206  m_3D_segments = i;
207  in >> m_lineWidth;
208 
209  // Update cov. matrix cache:
210  setCovMatrix(m_cov);
211  }
212  break;
213  default:
215  };
217 }
218 
219 bool quickSolveEqn(double a, double b_2, double c, double& t)
220 {
221  double delta = square(b_2) - a * c;
222  if (delta == 0)
223  return (t = -b_2 / a) >= 0;
224  else if (delta > 0)
225  {
226  delta = sqrt(delta);
227  if ((t = (-b_2 - delta) / a) >= 0)
228  return true;
229  else
230  return (t = (-b_2 + delta) / a) >= 0;
231  }
232  else
233  return false;
234 }
235 
236 bool CEllipsoid::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
237 {
238  if (m_cov.rows() != 3) return false;
239  TLine3D lin, lin2;
240  createFromPoseX((o - this->m_pose).asTPose(), lin);
241  lin.unitarize(); // By adding this line, distance from any point of the
242  // line to its base is exactly equal to the "t".
243  for (size_t i = 0; i < 3; i++)
244  {
245  lin2.pBase[i] = 0;
246  lin2.director[i] = 0;
247  for (size_t j = 0; j < 3; j++)
248  {
249  double vji = m_eigVec(j, i);
250  lin2.pBase[i] += vji * lin.pBase[j];
251  lin2.director[i] += vji * lin.director[j];
252  }
253  }
254  double a = 0, b_2 = 0, c = -square(m_quantiles);
255  for (size_t i = 0; i < 3; i++)
256  {
257  double ev = m_eigVal(i, i);
258  a += square(lin2.director[i] / ev);
259  b_2 += lin2.director[i] * lin2.pBase[i] / square(ev);
260  c += square(lin2.pBase[i] / ev);
261  }
262  return quickSolveEqn(a, b_2, c, dist);
263 }
264 
266  const mrpt::math::CMatrixDouble& m, int resizeToSize)
267 {
268  MRPT_START
269 
270  ASSERT_(m.cols() == m.rows());
271  ASSERT_(
272  m.rows() == 2 || m.rows() == 3 ||
273  (resizeToSize > 0 && (resizeToSize == 2 || resizeToSize == 3)));
274 
275  m_cov = m;
276  if (resizeToSize > 0 && resizeToSize < (int)m.rows())
277  m_cov.setSize(resizeToSize, resizeToSize);
278 
279  if (m_cov == m_prevComputedCov) return; // Done.
280 
281  m_prevComputedCov = m_cov;
282 
284 
285  // Handle the special case of an ellipsoid of volume = 0
286  const double d = m_cov.det();
287  if (d == 0 || d != d) // Note: "d!=d" is a great test for invalid numbers,
288  // don't remove!
289  {
290  // All zeros:
291  m_eigVec.setZero(3, 3);
292  m_eigVal.setZero(3, 3);
293  }
294  else
295  {
296  // Not null matrix: compute the eigen-vectors & values:
297  std::vector<double> eigvals;
298  if (m_cov.eig_symmetric(m_eigVec, eigvals))
299  {
300  // Do the scale at render to avoid recomputing the m_eigVal for
301  // different m_quantiles
302  m_eigVal.setDiagonal(eigvals);
303  m_eigVal.array() = m_eigVal.array().sqrt().matrix();
304  }
305  else
306  {
307  m_eigVec.setZero(3, 3);
308  m_eigVal.setZero(3, 3);
309  }
310  }
311 
312  MRPT_END
313 }
314 
316  const mrpt::math::CMatrixFloat& m, int resizeToSize)
317 {
319  setCovMatrix(CMatrixDouble(m), resizeToSize);
320 }
321 
322 /** Evaluates the bounding box of this object (including possible children) in
323  * the coordinate frame of the object parent. */
326 {
327  bb_min = m_bb_min;
328  bb_max = m_bb_max;
329 }
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
#define MRPT_START
Definition: exceptions.h:241
void setCovMatrix(const mrpt::math::CMatrixDouble &m, int resizeToSize=-1)
Set the 2x2 or 3x3 covariance matrix that will determine the aspect of the ellipsoid (if resizeToSize...
Definition: CEllipsoid.cpp:265
#define M_2PI
Definition: common.h:58
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
bool quickSolveEqn(double a, double b_2, double c, double &t)
Definition: CEllipsoid.cpp:219
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
STL namespace.
CMatrixDynamic< double > cast_double() const
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CEllipsoid.cpp:174
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
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CEllipsoid.cpp:173
T square(const T x)
Inline function for the square of a number.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CEllipsoid.cpp:181
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
void unitarize()
Unitarize director vector.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
Definition: CEllipsoid.cpp:236
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
Definition: geometry.cpp:933
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
This file implements matrix/vector text and binary serialization.
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
const auto bb_min
A 2D ellipse or 3D ellipsoid, depending on the size of the m_cov matrix (2x2 or 3x3).
Definition: CEllipsoid.h:44
Lightweight 3D point.
Definition: TPoint3D.h:90
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...
Definition: CEllipsoid.cpp:324
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019