Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
13 #include <mrpt/opengl/CEllipsoid.h>
14 #include <mrpt/math/CMatrix.h>
15 #include <mrpt/math/geometry.h>
17 #include <mrpt/utils/CStream.h>
18 
19 #include "opengl_internals.h"
20 
21 using namespace mrpt;
22 using namespace mrpt::opengl;
23 using namespace mrpt::utils;
24 using namespace mrpt::math;
25 using namespace std;
26 
28 
29 /*---------------------------------------------------------------
30  render
31  ---------------------------------------------------------------*/
32 void CEllipsoid::render_dl() const
33 {
34 #if MRPT_HAS_OPENGL_GLUT
36 
37  const size_t dim = m_cov.getColCount();
38 
39  if(m_eigVal(0,0) != 0.0 && m_eigVal(1,1) != 0.0 && (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;i++,ang+= (M_2PI/m_2D_segments))
77  {
78  double ccos = cos(ang);
79  double ssin = sin(ang);
80 
81  const float x = ccos * M.get_unsafe(0,0) + ssin * M.get_unsafe(1,0);
82  const float y = ccos * M.get_unsafe(0,1) + ssin * M.get_unsafe(1,1);
83 
84  glVertex2f( x,y );
85  } // end for points on ellipse
86 
87  glEnd();
88 
89  // 2D: Save bounding box:
90  const double max_radius = m_quantiles * std::max( m_eigVal(0,0), m_eigVal(1,1) );
91  m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0);
92  m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0);
93  // Convert to coordinates of my parent:
94  m_pose.composePoint(m_bb_min, m_bb_min);
95  m_pose.composePoint(m_bb_max, m_bb_max);
96 
98  }
99  else
100  {
101  // ---------------------
102  // 3D ellipsoid
103  // ---------------------
104  GLfloat mat[16];
105 
106  // A homogeneous transformation matrix, in this order:
107  //
108  // 0 4 8 12
109  // 1 5 9 13
110  // 2 6 10 14
111  // 3 7 11 15
112  //
113  mat[3] = mat[7] = mat[11] = 0;
114  mat[15] = 1;
115  mat[12] = mat[13] = mat[14] = 0;
116 
117  mat[0] = m_eigVec(0,0); mat[1] = m_eigVec(1,0); mat[2] = m_eigVec(2,0); // New X-axis
118  mat[4] = m_eigVec(0,1); mat[5] = m_eigVec(1,1); mat[6] = m_eigVec(2,1); // New X-axis
119  mat[8] = m_eigVec(0,2); mat[9] = m_eigVec(1,2); mat[10] = m_eigVec(2,2); // New X-axis
120 
121  GLUquadricObj *obj = gluNewQuadric();
123 
124  if (!m_drawSolid3D) glDisable(GL_LIGHTING); // Disable lights when drawing lines
125 
126  gluQuadricDrawStyle( obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
127 
128  glPushMatrix();
129  glMultMatrixf( mat );
130  glScalef(m_eigVal(0,0)*m_quantiles,m_eigVal(1,1)*m_quantiles,m_eigVal(2,2)*m_quantiles);
131 
132  gluSphere( obj, 1,m_3D_segments,m_3D_segments);
134 
135  glPopMatrix();
136 
137  gluDeleteQuadric(obj);
139 
140  // 3D: Save bounding box:
141  const double max_radius = m_quantiles * std::max( m_eigVal(0,0), std::max(m_eigVal(1,1), m_eigVal(2,2) ) );
142  m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0);
143  m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0);
144  // Convert to coordinates of my parent:
145  m_pose.composePoint(m_bb_min, m_bb_min);
146  m_pose.composePoint(m_bb_max, m_bb_max);
147  }
148 
149 
151 
153  }
155  cout << "Covariance matrix leading to error is:" << endl << m_cov << endl; \
156  );
157 #endif
158 }
159 
160 /*---------------------------------------------------------------
161  Implements the writing to a CStream capability of
162  CSerializable objects
163  ---------------------------------------------------------------*/
165 {
166  if (version)
167  *version = 1;
168  else
169  {
170  writeToStreamRender(out);
171  out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments << (uint32_t)m_3D_segments << m_lineWidth;
172  }
173 }
174 
175 /*---------------------------------------------------------------
176  Implements the reading from a CStream capability of
177  CSerializable objects
178  ---------------------------------------------------------------*/
180 {
181  switch(version)
182  {
183  case 0:
184  case 1:
185  {
186  uint32_t i;
187  readFromStreamRender(in);
188  if (version==0)
189  {
190  CMatrix c;
191  in >> c; m_cov = c.cast<double>();
192  }
193  else
194  {
195  in >> m_cov;
196  }
197 
198  in >> m_drawSolid3D >> m_quantiles;
199  in >> i; m_2D_segments = i;
200  in >> i; m_3D_segments = i;
201  in >> m_lineWidth;
202 
203  // Update cov. matrix cache:
204  m_prevComputedCov = m_cov;
205  setCovMatrix(m_cov);
206 
207  } break;
208  default:
210 
211  };
213 }
214 
215 bool quickSolveEqn(double a,double b_2,double c,double &t) {
216  double delta=square(b_2)-a*c;
217  if (delta==0) return (t=-b_2/a)>=0;
218  else if (delta>0) {
219  delta=sqrt(delta);
220  if ((t=(-b_2-delta)/a)>=0) return true;
221  else return (t=(-b_2+delta)/a)>=0;
222  } else return false;
223 }
224 
225 bool CEllipsoid::traceRay(const mrpt::poses::CPose3D &o,double &dist) const {
226  if (m_cov.getRowCount()!=3) return false;
227  TLine3D lin,lin2;
228  createFromPoseX(o-this->m_pose,lin);
229  lin.unitarize(); //By adding this line, distance from any point of the line to its base is exactly equal to the "t".
230  for (size_t i=0;i<3;i++) {
231  lin2.pBase[i]=0;
232  lin2.director[i]=0;
233  for (size_t j=0;j<3;j++) {
234  double vji=m_eigVec(j,i);
235  lin2.pBase[i]+=vji*lin.pBase[j];
236  lin2.director[i]+=vji*lin.director[j];
237  }
238  }
239  double a=0,b_2=0,c=-square(m_quantiles);
240  for (size_t i=0;i<3;i++) {
241  double ev=m_eigVal(i,i);
242  a+=square(lin2.director[i]/ev);
243  b_2+=lin2.director[i]*lin2.pBase[i]/square(ev);
244  c+=square(lin2.pBase[i]/ev);
245  }
246  return quickSolveEqn(a,b_2,c,dist);
247 }
248 
249 void CEllipsoid::setCovMatrix( const mrpt::math::CMatrixDouble &m, int resizeToSize)
250 {
251  MRPT_START
252 
253  ASSERT_( m.getColCount() == m.getRowCount() );
254  ASSERT_( size(m,1)==2 || size(m,1)==3 || (resizeToSize>0 && (resizeToSize==2 || resizeToSize==3)));
255 
256  m_cov = m;
257  if (resizeToSize>0 && resizeToSize<(int)size(m,1))
258  m_cov.setSize(resizeToSize,resizeToSize);
259 
260  if (m_cov==m_prevComputedCov)
261  return; // Done.
262 
264 
265  // Handle the special case of an ellipsoid of volume = 0
266  const double d=m_cov.det();
267  if (d==0 || d!=d) // Note: "d!=d" is a great test for invalid numbers, don't remove!
268  {
269  // All zeros:
270  m_prevComputedCov = m_cov;
271  m_eigVec.zeros(3,3);
272  m_eigVal.zeros(3,3);
273  }
274  else
275  {
276  // Not null matrix: compute the eigen-vectors & values:
277  m_prevComputedCov = m_cov;
278  if (m_cov.eigenVectors(m_eigVec,m_eigVal)) {
279  m_eigVal = m_eigVal.array().sqrt().matrix();
280  // Do the scale at render to avoid recomputing the m_eigVal for different m_quantiles
281  } else {
282  m_eigVec.zeros(3,3);
283  m_eigVal.zeros(3,3);
284  }
285  }
286 
287 
288  MRPT_END
289 }
290 
291 void CEllipsoid::setCovMatrix( const mrpt::math::CMatrixFloat &m, int resizeToSize)
292 {
294  setCovMatrix( CMatrixDouble(m), resizeToSize);
295 }
296 
297 /** Evaluates the bounding box of this object (including possible children) in the coordinate frame of the object parent. */
299 {
300  bb_min = m_bb_min;
301  bb_max = m_bb_max;
302 }
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
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:249
GLAPI void GLAPIENTRY glMultMatrixf(const GLfloat *m)
#define MRPT_END_WITH_CLEAN_UP(stuff)
GLAPI void GLAPIENTRY glEnable(GLenum cap)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool quickSolveEqn(double a, double b_2, double c, double &t)
Definition: CEllipsoid.cpp:215
GLAPI void GLAPIENTRY glPopMatrix(void)
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const MRPT_OVERRIDE
Ray tracing.
Definition: CEllipsoid.cpp:225
TPoint3D pBase
Base point.
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
STL namespace.
#define GL_ONE_MINUS_SRC_ALPHA
Definition: glew.h:283
#define GL_LIGHTING
Definition: glew.h:381
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
float GLfloat
Definition: glew.h:213
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
#define M_2PI
Definition: mrpt_macros.h:380
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
void BASE_IMPEXP createFromPoseX(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
Definition: geometry.cpp:781
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CEllipsoid.cpp:164
int version
Definition: mrpt_jpeglib.h:898
GLAPI void GLAPIENTRY glBegin(GLenum mode)
#define GL_BLEND
Definition: glew.h:428
#define GL_LINE_LOOP
Definition: glew.h:270
void unitarize()
Unitarize director vector.
double director[3]
Director vector.
#define MRPT_START
#define GL_SRC_ALPHA
Definition: glew.h:282
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLAPI void GLAPIENTRY glScalef(GLfloat x, GLfloat y, GLfloat z)
This file implements matrix/vector text and binary serialization.
void OPENGL_IMPEXP checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:134
GLuint in
Definition: glext.h:6301
The namespace for 3D scene representation and rendering.
#define ASSERT_(f)
GLAPI void GLAPIENTRY glEnd(void)
GLenum GLint GLint y
Definition: glext.h:3516
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CEllipsoid.cpp:179
GLsizeiptr size
Definition: glext.h:3779
A 2D ellipse or 3D ellipsoid, depending on the size of the m_cov matrix (2x2 or 3x3).
Definition: CEllipsoid.h:43
GLenum GLint x
Definition: glext.h:3516
GLAPI void GLAPIENTRY glPushMatrix(void)
Lightweight 3D point.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
unsigned __int32 uint32_t
Definition: rptypes.h:49
GLAPI void GLAPIENTRY glVertex2f(GLfloat x, GLfloat y)
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const MRPT_OVERRIDE
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
Definition: CEllipsoid.cpp:298
3D line, represented by a base point and a director vector.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019