Main MRPT website > C++ reference for MRPT 1.9.9
CEllipsoid.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "opengl-precomp.h" // Precompiled header
11 
12 #include <mrpt/opengl/CEllipsoid.h>
13 #include <mrpt/math/CMatrix.h>
14 #include <mrpt/math/geometry.h>
17 
18 #include "opengl_internals.h"
19 
20 using namespace mrpt;
21 using namespace mrpt::opengl;
22 
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  {
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  CMatrixDouble M;
71  M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint();
72 
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 =
83  ccos * M.get_unsafe(0, 0) + ssin * M.get_unsafe(1, 0);
84  const float y =
85  ccos * M.get_unsafe(0, 1) + ssin * M.get_unsafe(1, 1);
86 
87  glVertex2f(x, y);
88  } // end for points on ellipse
89 
90  glEnd();
91 
92  // 2D: Save bounding box:
93  const double max_radius =
94  m_quantiles * std::max(m_eigVal(0, 0), m_eigVal(1, 1));
95  m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
96  m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
97  // Convert to coordinates of my parent:
98  m_pose.composePoint(m_bb_min, m_bb_min);
99  m_pose.composePoint(m_bb_max, m_bb_max);
100 
102  }
103  else
104  {
105  // ---------------------
106  // 3D ellipsoid
107  // ---------------------
108  GLfloat mat[16];
109 
110  // A homogeneous transformation matrix, in this order:
111  //
112  // 0 4 8 12
113  // 1 5 9 13
114  // 2 6 10 14
115  // 3 7 11 15
116  //
117  mat[3] = mat[7] = mat[11] = 0;
118  mat[15] = 1;
119  mat[12] = mat[13] = mat[14] = 0;
120 
121  mat[0] = m_eigVec(0, 0);
122  mat[1] = m_eigVec(1, 0);
123  mat[2] = m_eigVec(2, 0); // New X-axis
124  mat[4] = m_eigVec(0, 1);
125  mat[5] = m_eigVec(1, 1);
126  mat[6] = m_eigVec(2, 1); // New X-axis
127  mat[8] = m_eigVec(0, 2);
128  mat[9] = m_eigVec(1, 2);
129  mat[10] = m_eigVec(2, 2); // New X-axis
130 
131  GLUquadricObj* obj = gluNewQuadric();
133 
134  if (!m_drawSolid3D)
135  glDisable(GL_LIGHTING); // Disable lights when drawing lines
136 
137  gluQuadricDrawStyle(obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
138 
139  glPushMatrix();
140  glMultMatrixf(mat);
141  glScalef(
142  m_eigVal(0, 0) * m_quantiles, m_eigVal(1, 1) * m_quantiles,
143  m_eigVal(2, 2) * m_quantiles);
144 
145  gluSphere(obj, 1, m_3D_segments, m_3D_segments);
147 
148  glPopMatrix();
149 
150  gluDeleteQuadric(obj);
152 
153  // 3D: Save bounding box:
154  const double max_radius =
155  m_quantiles *
156  std::max(
157  m_eigVal(0, 0), std::max(m_eigVal(1, 1), m_eigVal(2, 2)));
158  m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
159  m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
160  // Convert to coordinates of my parent:
161  m_pose.composePoint(m_bb_min, m_bb_min);
162  m_pose.composePoint(m_bb_max, m_bb_max);
163  }
164 
166 
168  }
170  cout << "Covariance matrix leading to error is:" << endl
171  << m_cov << endl;);
172 #endif
173 }
174 
177 {
178  writeToStreamRender(out);
179  out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments
180  << (uint32_t)m_3D_segments << m_lineWidth;
181 }
182 
185 {
186  switch (version)
187  {
188  case 0:
189  case 1:
190  {
191  uint32_t i;
192  readFromStreamRender(in);
193  if (version == 0)
194  {
195  CMatrix c;
196  in >> c;
197  m_cov = c.cast<double>();
198  }
199  else
200  {
201  in >> m_cov;
202  }
203 
204  in >> m_drawSolid3D >> m_quantiles;
205  in >> i;
206  m_2D_segments = i;
207  in >> i;
208  m_3D_segments = i;
209  in >> m_lineWidth;
210 
211  // Update cov. matrix cache:
212  m_prevComputedCov = m_cov;
213  setCovMatrix(m_cov);
214  }
215  break;
216  default:
218  };
220 }
221 
222 bool quickSolveEqn(double a, double b_2, double c, double& t)
223 {
224  double delta = square(b_2) - a * c;
225  if (delta == 0)
226  return (t = -b_2 / a) >= 0;
227  else if (delta > 0)
228  {
229  delta = sqrt(delta);
230  if ((t = (-b_2 - delta) / a) >= 0)
231  return true;
232  else
233  return (t = (-b_2 + delta) / a) >= 0;
234  }
235  else
236  return false;
237 }
238 
239 bool CEllipsoid::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
240 {
241  if (m_cov.rows() != 3) return false;
242  TLine3D lin, lin2;
243  createFromPoseX((o - this->m_pose).asTPose(), lin);
244  lin.unitarize(); // By adding this line, distance from any point of the
245  // line to its base is exactly equal to the "t".
246  for (size_t i = 0; i < 3; i++)
247  {
248  lin2.pBase[i] = 0;
249  lin2.director[i] = 0;
250  for (size_t j = 0; j < 3; j++)
251  {
252  double vji = m_eigVec(j, i);
253  lin2.pBase[i] += vji * lin.pBase[j];
254  lin2.director[i] += vji * lin.director[j];
255  }
256  }
257  double a = 0, b_2 = 0, c = -square(m_quantiles);
258  for (size_t i = 0; i < 3; i++)
259  {
260  double ev = m_eigVal(i, i);
261  a += square(lin2.director[i] / ev);
262  b_2 += lin2.director[i] * lin2.pBase[i] / square(ev);
263  c += square(lin2.pBase[i] / ev);
264  }
265  return quickSolveEqn(a, b_2, c, dist);
266 }
267 
269  const mrpt::math::CMatrixDouble& m, int resizeToSize)
270 {
271  MRPT_START
272 
273  ASSERT_(m.cols() == m.rows());
274  ASSERT_(
275  m.rows() == 2 || m.rows() == 3 ||
276  (resizeToSize > 0 && (resizeToSize == 2 || resizeToSize == 3)));
277 
278  m_cov = m;
279  if (resizeToSize > 0 && resizeToSize < (int)m.rows())
280  m_cov.setSize(resizeToSize, resizeToSize);
281 
282  if (m_cov == m_prevComputedCov) return; // Done.
283 
285 
286  // Handle the special case of an ellipsoid of volume = 0
287  const double d = m_cov.det();
288  if (d == 0 || d != d) // Note: "d!=d" is a great test for invalid numbers,
289  // don't remove!
290  {
291  // All zeros:
292  m_prevComputedCov = m_cov;
293  m_eigVec.zeros(3, 3);
294  m_eigVal.zeros(3, 3);
295  }
296  else
297  {
298  // Not null matrix: compute the eigen-vectors & values:
299  m_prevComputedCov = m_cov;
300  if (m_cov.eigenVectors(m_eigVec, m_eigVal))
301  {
302  m_eigVal = m_eigVal.array().sqrt().matrix();
303  // Do the scale at render to avoid recomputing the m_eigVal for
304  // different m_quantiles
305  }
306  else
307  {
308  m_eigVec.zeros(3, 3);
309  m_eigVal.zeros(3, 3);
310  }
311  }
312 
313  MRPT_END
314 }
315 
317  const mrpt::math::CMatrixFloat& m, int resizeToSize)
318 {
320  setCovMatrix(CMatrixDouble(m), resizeToSize);
321 }
322 
323 /** Evaluates the bounding box of this object (including possible children) in
324  * the coordinate frame of the object parent. */
326  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
327 {
328  bb_min = m_bb_min;
329  bb_max = m_bb_max;
330 }
mrpt::math::CMatrixDouble
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixTemplateNumeric.h:144
mrpt::opengl::CEllipsoid::getBoundingBox
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:325
glBegin
GLAPI void GLAPIENTRY glBegin(GLenum mode)
mrpt::math::createFromPoseX
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:940
matrix_serialization.h
geometry.h
t
GLdouble GLdouble t
Definition: glext.h:3689
glPopMatrix
GLAPI void GLAPIENTRY glPopMatrix(void)
GL_SRC_ALPHA
#define GL_SRC_ALPHA
Definition: glew.h:286
mrpt::math::TLine3D::pBase
TPoint3D pBase
Base point.
Definition: lightweight_geom_data.h:1250
c
const GLubyte * c
Definition: glext.h:6313
GL_BLEND
#define GL_BLEND
Definition: glew.h:432
MRPT_END_WITH_CLEAN_UP
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:268
mrpt::opengl::CRenderizableDisplayList
A renderizable object suitable for rendering with OpenGL's display lists.
Definition: CRenderizableDisplayList.h:39
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
CMatrix.h
glDisable
GLAPI void GLAPIENTRY glDisable(GLenum cap)
mrpt::opengl::CEllipsoid::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CEllipsoid.cpp:183
mrpt::opengl::CRenderizableDisplayList::notifyChange
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated)
Definition: CRenderizableDisplayList.h:57
glEnable
GLAPI void GLAPIENTRY glEnable(GLenum cap)
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
CEllipsoid.h
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
GL_LINE_LOOP
#define GL_LINE_LOOP
Definition: glew.h:274
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::opengl::CEllipsoid::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CEllipsoid.cpp:175
glPushMatrix
GLAPI void GLAPIENTRY glPushMatrix(void)
glVertex2f
GLAPI void GLAPIENTRY glVertex2f(GLfloat x, GLfloat y)
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
mrpt::math::CMatrixTemplateNumeric< double >
glEnd
GLAPI void GLAPIENTRY glEnd(void)
mrpt::opengl::gl_utils::checkOpenGLError
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:143
GL_LIGHTING
#define GL_LIGHTING
Definition: glew.h:385
glLineWidth
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
mrpt::math::CMatrix
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
GL_ONE_MINUS_SRC_ALPHA
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:287
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::opengl::CEllipsoid::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CEllipsoid.cpp:176
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
glScalef
GLAPI void GLAPIENTRY glScalef(GLfloat x, GLfloat y, GLfloat z)
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:114
mrpt::opengl::CEllipsoid::traceRay
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
Definition: CEllipsoid.cpp:239
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
mrpt::opengl::CEllipsoid::setCovMatrix
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:268
opengl-precomp.h
mrpt::math::TLine3D::unitarize
void unitarize()
Unitarize director vector.
Definition: lightweight_geom_data.cpp:776
opengl_internals.h
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
void
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
glMultMatrixf
GLAPI void GLAPIENTRY glMultMatrixf(const GLfloat *m)
GLfloat
float GLfloat
Definition: glew.h:217
quickSolveEqn
bool quickSolveEqn(double a, double b_2, double c, double &t)
Definition: CEllipsoid.cpp:222
in
GLuint in
Definition: glext.h:7274
mrpt::math::TLine3D
3D line, represented by a base point and a director vector.
Definition: lightweight_geom_data.h:1244
M_2PI
#define M_2PI
Definition: common.h:58
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
mrpt::math::TLine3D::director
double director[3]
Director vector.
Definition: lightweight_geom_data.h:1254
y
GLenum GLint GLint y
Definition: glext.h:3538
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
x
GLenum GLint x
Definition: glext.h:3538
mrpt::opengl::CEllipsoid
A 2D ellipse or 3D ellipsoid, depending on the size of the m_cov matrix (2x2 or 3x3).
Definition: CEllipsoid.h:47
glBlendFunc
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST