9 #ifndef opengl_CGeneralizedEllipsoidTemplate_H 10 #define opengl_CGeneralizedEllipsoidTemplate_H 28 const float lineWidth,
29 const uint32_t slices,
30 const uint32_t stacks);
39 const uint32_t slices,
40 const uint32_t stacks);
41 template <>
void OPENGL_IMPEXP generalizedEllipsoidPoints<2>(
const mrpt::math::CMatrixFixedNumeric<double,2,2> & U,
const mrpt::math::CMatrixFixedNumeric<double,2,1> &
mean,std::vector<mrpt::math::CMatrixFixedNumeric<float,2,1> > &out_params_pts,
const uint32_t slices,
const uint32_t stacks);
42 template <>
void OPENGL_IMPEXP generalizedEllipsoidPoints<3>(
const mrpt::math::CMatrixFixedNumeric<double,3,3> & U,
const mrpt::math::CMatrixFixedNumeric<double,3,1> &
mean,std::vector<mrpt::math::CMatrixFixedNumeric<float,3,1> > &out_params_pts,
const uint32_t slices,
const uint32_t stacks);
70 template <
typename MATRIX,
typename VECTOR>
74 ASSERT_( new_cov.getColCount() == new_cov.getRowCount() && new_cov.getColCount() == DIM )
77 m_needToRecomputeEigenVals =
true;
129 if (m_needToRecomputeEigenVals)
131 m_needToRecomputeEigenVals =
false;
133 const double d=m_cov.det();
134 if (fabs(d)<1e-20 || d!=d)
137 m_U.setZero(DIM,DIM);
147 for (
int i=0;i<DIM;i++)
148 if (m_U.coeff(i,i)==0)
154 std::vector<array_parameter_t> params_pts;
155 const cov_matrix_t Uscaled =
static_cast<double>(m_quantiles) * m_U;
156 detail::generalizedEllipsoidPoints<DIM>(Uscaled,m_mean, params_pts,m_numSegments,m_numSegments);
159 std::vector<array_point_t> render_pts;
160 this->transformFromParameterSpace(params_pts,render_pts);
163 m_bb_min =
mrpt::math::TPoint3D(std::numeric_limits<double>::max(),std::numeric_limits<double>::max(), 0);
164 m_bb_max =
mrpt::math::TPoint3D(-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max(),0);
165 for (
size_t i=0;i<render_pts.size();i++)
166 for (
int k=0;k<DIM;k++)
172 m_pose.composePoint(m_bb_min, m_bb_min);
173 m_pose.composePoint(m_bb_max, m_bb_max);
176 mrpt::opengl::detail::renderGeneralizedEllipsoidTemplate<DIM>(render_pts,
178 m_numSegments,m_numSegments );
202 virtual void transformFromParameterSpace(
203 const std::vector<array_point_t> ¶ms_pts,
204 std::vector<array_point_t> & out_pts)
const = 0;
219 const uint8_t version = 0;
222 << m_quantiles << m_lineWidth << m_numSegments;
232 in >> m_cov >> m_mean
233 >> m_quantiles >> m_lineWidth >> m_numSegments;
234 m_needToRecomputeEigenVals =
true;
244 m_needToRecomputeEigenVals(true),
void OPENGL_IMPEXP generalizedEllipsoidPoints(const mrpt::math::CMatrixFixedNumeric< double, DIM, DIM > &U, const mrpt::math::CMatrixFixedNumeric< double, DIM, 1 > &mean, std::vector< mrpt::math::CMatrixFixedNumeric< float, DIM, 1 > > &out_params_pts, const uint32_t slices, const uint32_t stacks)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
float m_lineWidth
The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
void render_dl() const MRPT_OVERRIDE
Render If one of the eigen value of the covariance matrix of the ellipsoid is null, ellipsoid will not be rendered to ensure stability in the rendering process.
bool m_needToRecomputeEigenVals
virtual bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const MRPT_OVERRIDE
Ray tracing.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
void setNumberOfSegments(const uint32_t numSegments)
Set the number of segments of the surface/curve (higher means with greater resolution) ...
void thisclass_writeToStream(mrpt::utils::CStream &out) const
float m_quantiles
The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
float getLineWidth() const
#define THROW_EXCEPTION(msg)
mrpt::math::CMatrixFixedNumeric< double, DIM, DIM > cov_matrix_t
The type of fixed-size covariance matrices for this representation.
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
uint32_t m_numSegments
Number of segments in 2D/3D ellipsoids (default=10)
virtual 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...
void thisclass_readFromStream(mrpt::utils::CStream &in)
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void OPENGL_IMPEXP renderGeneralizedEllipsoidTemplate< 2 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 2, 1 > > &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
void setCovMatrixAndMean(const MATRIX &new_cov, const VECTOR &new_mean)
Set the NxN covariance matrix that will determine the aspect of the ellipsoid - Notice that the covar...
A renderizable object suitable for rendering with OpenGL's display lists.
mrpt::math::TPoint3D m_bb_min
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void setLineWidth(float w)
The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
uint32_t getNumberOfSegments()
void OPENGL_IMPEXP generalizedEllipsoidPoints< 2 >(const mrpt::math::CMatrixFixedNumeric< double, 2, 2 > &U, const mrpt::math::CMatrixFixedNumeric< double, 2, 1 > &mean, std::vector< mrpt::math::CMatrixFixedNumeric< float, 2, 1 > > &out_params_pts, const uint32_t slices, const uint32_t stacks)
mrpt::math::CMatrixFixedNumeric< float, DIM, 1 > array_parameter_t
mrpt::math::CMatrixFixedNumeric< float, DIM, 1 > array_point_t
A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of uncertainty sh...
CGeneralizedEllipsoidTemplate()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual ~CGeneralizedEllipsoidTemplate()
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
float getQuantiles() const
Refer to documentation of setQuantiles()
This file implements matrix/vector text and binary serialization.
const cov_matrix_t & getCovMatrix() const
Gets the current uncertainty covariance of parameter space.
void setQuantiles(float q)
Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the ...
void OPENGL_IMPEXP renderGeneralizedEllipsoidTemplate(const std::vector< mrpt::math::CMatrixFixedNumeric< float, DIM, 1 > > &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
void OPENGL_IMPEXP generalizedEllipsoidPoints< 3 >(const mrpt::math::CMatrixFixedNumeric< double, 3, 3 > &U, const mrpt::math::CMatrixFixedNumeric< double, 3, 1 > &mean, std::vector< mrpt::math::CMatrixFixedNumeric< float, 3, 1 > > &out_params_pts, const uint32_t slices, const uint32_t stacks)
cov_matrix_t m_U
Cholesky U triangular matrix cache. */.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
mrpt::math::CMatrixFixedNumeric< double, DIM, 1 > mean_vector_t
The type of fixed-size vector for this representation.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
void OPENGL_IMPEXP renderGeneralizedEllipsoidTemplate< 3 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 3, 1 > > &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)