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) ...
float m_quantiles
The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
#define THROW_EXCEPTION(msg)
float getLineWidth() const
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) ...
float getQuantiles() const
Refer to documentation of setQuantiles()
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.
void thisclass_writeToStream(mrpt::utils::CStream &out) const
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).
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. */.
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...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
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)