33 #if MRPT_HAS_OPENGL_GLUT 36 const size_t dim = m_cov.getColCount();
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)
71 M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint();
76 for (i = 0, ang = 0; i < m_2D_segments;
77 i++, ang += (
M_2PI / m_2D_segments))
79 double ccos = cos(ang);
80 double ssin = sin(ang);
83 ccos * M.get_unsafe(0, 0) + ssin * M.get_unsafe(1, 0);
85 ccos * M.get_unsafe(0, 1) + ssin * M.get_unsafe(1, 1);
93 const double max_radius =
94 m_quantiles * std::max(m_eigVal(0, 0), m_eigVal(1, 1));
98 m_pose.composePoint(m_bb_min, m_bb_min);
99 m_pose.composePoint(m_bb_max, m_bb_max);
117 mat[3] = mat[7] = mat[11] = 0;
119 mat[12] = mat[13] = mat[14] = 0;
121 mat[0] = m_eigVec(0, 0);
122 mat[1] = m_eigVec(1, 0);
123 mat[2] = m_eigVec(2, 0);
124 mat[4] = m_eigVec(0, 1);
125 mat[5] = m_eigVec(1, 1);
126 mat[6] = m_eigVec(2, 1);
127 mat[8] = m_eigVec(0, 2);
128 mat[9] = m_eigVec(1, 2);
129 mat[10] = m_eigVec(2, 2);
131 GLUquadricObj*
obj = gluNewQuadric();
137 gluQuadricDrawStyle(
obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
142 m_eigVal(0, 0) * m_quantiles, m_eigVal(1, 1) * m_quantiles,
143 m_eigVal(2, 2) * m_quantiles);
145 gluSphere(
obj, 1, m_3D_segments, m_3D_segments);
150 gluDeleteQuadric(
obj);
154 const double max_radius =
157 m_eigVal(0, 0), std::max(m_eigVal(1, 1), m_eigVal(2, 2)));
161 m_pose.composePoint(m_bb_min, m_bb_min);
162 m_pose.composePoint(m_bb_max, m_bb_max);
170 cout <<
"Covariance matrix leading to error is:" << endl
185 writeToStreamRender(out);
186 out << m_cov << m_drawSolid3D << m_quantiles << (
uint32_t)m_2D_segments
187 << (
uint32_t)m_3D_segments << m_lineWidth;
203 readFromStreamRender(
in);
208 m_cov =
c.cast<
double>();
215 in >> m_drawSolid3D >> m_quantiles;
223 m_prevComputedCov = m_cov;
237 return (
t = -b_2 /
a) >= 0;
241 if ((
t = (-b_2 - delta) /
a) >= 0)
244 return (
t = (-b_2 + delta) /
a) >= 0;
252 if (m_cov.getRowCount() != 3)
return false;
257 for (
size_t i = 0; i < 3; i++)
261 for (
size_t j = 0; j < 3; j++)
263 double vji = m_eigVec(j, i);
268 double a = 0, b_2 = 0,
c = -
square(m_quantiles);
269 for (
size_t i = 0; i < 3; i++)
271 double ev = m_eigVal(i, i);
284 ASSERT_(m.getColCount() == m.getRowCount());
286 size(m, 1) == 2 ||
size(m, 1) == 3 ||
287 (resizeToSize > 0 && (resizeToSize == 2 || resizeToSize == 3)));
290 if (resizeToSize > 0 && resizeToSize < (
int)
size(m, 1))
291 m_cov.setSize(resizeToSize, resizeToSize);
293 if (m_cov == m_prevComputedCov)
return;
298 const double d = m_cov.det();
299 if (d == 0 || d != d)
303 m_prevComputedCov = m_cov;
304 m_eigVec.zeros(3, 3);
305 m_eigVal.zeros(3, 3);
310 m_prevComputedCov = m_cov;
311 if (m_cov.eigenVectors(m_eigVec, m_eigVal))
313 m_eigVal = m_eigVal.array().sqrt().matrix();
319 m_eigVec.zeros(3, 3);
320 m_eigVal.zeros(3, 3);
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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...
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)
GLAPI void GLAPIENTRY glPopMatrix(void)
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) ...
#define GL_ONE_MINUS_SRC_ALPHA
GLsizei GLsizei GLuint * obj
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
T square(const T x)
Inline function for the square of a number.
A renderizable object suitable for rendering with OpenGL's display lists.
void createFromPoseX(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
This base provides a set of functions for maths stuff.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
GLAPI void GLAPIENTRY glBegin(GLenum mode)
void unitarize()
Unitarize director vector.
double director[3]
Director vector.
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
GLAPI void GLAPIENTRY glScalef(GLfloat x, GLfloat y, GLfloat z)
This file implements matrix/vector text and binary serialization.
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
The namespace for 3D scene representation and rendering.
GLAPI void GLAPIENTRY glEnd(void)
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
A 2D ellipse or 3D ellipsoid, depending on the size of the m_cov matrix (2x2 or 3x3).
GLAPI void GLAPIENTRY glPushMatrix(void)
This class is a "CSerializable" wrapper for "CMatrixFloat".
unsigned __int32 uint32_t
GLAPI void GLAPIENTRY glVertex2f(GLfloat x, GLfloat y)
GLAPI void GLAPIENTRY glDisable(GLenum cap)
GLubyte GLubyte GLubyte a
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...
3D line, represented by a base point and a director vector.