18 #include <Eigen/Dense> 33 #if MRPT_HAS_OPENGL_GLUT 36 const size_t dim = m_cov.cols();
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)
70 auto M =
CMatrixDouble(m_eigVal.asEigen() * m_eigVec.transpose());
71 M *= double(m_quantiles);
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);
82 const float x = ccos * M(0, 0) + ssin * M(1, 0);
83 const float y = ccos * M(0, 1) + ssin * M(1, 1);
91 const double max_radius =
92 m_quantiles * std::max(m_eigVal(0, 0), m_eigVal(1, 1));
96 m_pose.composePoint(m_bb_min, m_bb_min);
97 m_pose.composePoint(m_bb_max, m_bb_max);
115 mat[3] = mat[7] = mat[11] = 0;
117 mat[12] = mat[13] = mat[14] = 0;
119 mat[0] = m_eigVec(0, 0);
120 mat[1] = m_eigVec(1, 0);
121 mat[2] = m_eigVec(2, 0);
122 mat[4] = m_eigVec(0, 1);
123 mat[5] = m_eigVec(1, 1);
124 mat[6] = m_eigVec(2, 1);
125 mat[8] = m_eigVec(0, 2);
126 mat[9] = m_eigVec(1, 2);
127 mat[10] = m_eigVec(2, 2);
129 GLUquadricObj*
obj = gluNewQuadric();
135 gluQuadricDrawStyle(
obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
140 m_eigVal(0, 0) * m_quantiles, m_eigVal(1, 1) * m_quantiles,
141 m_eigVal(2, 2) * m_quantiles);
143 gluSphere(
obj, 1, m_3D_segments, m_3D_segments);
148 gluDeleteQuadric(
obj);
152 const double max_radius =
155 m_eigVal(0, 0), std::max(m_eigVal(1, 1), m_eigVal(2, 2)));
159 m_pose.composePoint(m_bb_min, m_bb_min);
160 m_pose.composePoint(m_bb_max, m_bb_max);
176 writeToStreamRender(out);
177 out << m_cov << m_drawSolid3D << m_quantiles << (
uint32_t)m_2D_segments
178 << (
uint32_t)m_3D_segments << m_lineWidth;
190 readFromStreamRender(
in);
195 m_cov =
c.cast_double();
202 in >> m_drawSolid3D >> m_quantiles;
223 return (
t = -b_2 /
a) >= 0;
227 if ((
t = (-b_2 - delta) /
a) >= 0)
230 return (
t = (-b_2 + delta) /
a) >= 0;
238 if (m_cov.rows() != 3)
return false;
243 for (
size_t i = 0; i < 3; i++)
247 for (
size_t j = 0; j < 3; j++)
249 double vji = m_eigVec(j, i);
254 double a = 0, b_2 = 0,
c = -
square(m_quantiles);
255 for (
size_t i = 0; i < 3; i++)
257 double ev = m_eigVal(i, i);
273 (resizeToSize > 0 && (resizeToSize == 2 || resizeToSize == 3)));
276 if (resizeToSize > 0 && resizeToSize < (
int)m.
rows())
277 m_cov.
setSize(resizeToSize, resizeToSize);
279 if (m_cov == m_prevComputedCov)
return;
281 m_prevComputedCov = m_cov;
286 const double d = m_cov.det();
287 if (d == 0 || d != d)
291 m_eigVec.setZero(3, 3);
292 m_eigVal.setZero(3, 3);
297 std::vector<double> eigvals;
298 if (m_cov.eig_symmetric(m_eigVec, eigvals))
302 m_eigVal.setDiagonal(eigvals);
303 m_eigVal.array() = m_eigVal.array().sqrt().matrix();
307 m_eigVec.setZero(3, 3);
308 m_eigVal.setZero(3, 3);
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
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)
GLAPI void GLAPIENTRY glEnable(GLenum cap)
#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)
GLAPI void GLAPIENTRY glPopMatrix(void)
TPoint3D pBase
Base point.
#define GL_ONE_MINUS_SRC_ALPHA
#define MRPT_END_WITH_CLEAN_UP(stuff)
GLsizei GLsizei GLuint * obj
GLAPI void GLAPIENTRY glLineWidth(GLfloat width)
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
GLAPI void GLAPIENTRY glBlendFunc(GLenum sfactor, GLenum dfactor)
A renderizable object suitable for rendering with OpenGL's display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
This base provides a set of functions for maths stuff.
std::array< double, 3 > director
Director vector.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
GLAPI void GLAPIENTRY glBegin(GLenum mode)
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".
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.
Virtual base class for "archives": classes abstracting I/O streams.
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
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.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
The namespace for 3D scene representation and rendering.
GLAPI void GLAPIENTRY glEnd(void)
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
A 2D ellipse or 3D ellipsoid, depending on the size of the m_cov matrix (2x2 or 3x3).
GLAPI void GLAPIENTRY glPushMatrix(void)
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...
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
3D line, represented by a base point and a director vector.