Main MRPT website > C++ reference for MRPT 1.4.0
CGeneralizedEllipsoidTemplate.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, 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#ifndef opengl_CGeneralizedEllipsoidTemplate_H
10#define opengl_CGeneralizedEllipsoidTemplate_H
11
15#include <mrpt/utils/CStream.h> // for >> ops
16#include <mrpt/math/matrix_serialization.h> // for >> ops
18
19namespace mrpt
20{
21 namespace opengl
22 {
23 namespace detail
24 {
25 template <int DIM>
27 const std::vector<mrpt::math::CMatrixFixedNumeric<float,DIM,1> > & pts,
28 const float lineWidth,
29 const uint32_t slices,
30 const uint32_t stacks);
31 template <> 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);
32 template <> 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);
33
34 template <int DIM>
38 std::vector<mrpt::math::CMatrixFixedNumeric<float,DIM,1> > &out_params_pts,
39 const uint32_t slices,
40 const uint32_t stacks);
43 }
44
45 /** A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of
46 * uncertainty shapes in either 2D or 3D. See derived classes for examples.
47 *
48 * Please read the documentation of CGeneralizedEllipsoidTemplate::setQuantiles() for learning
49 * the mathematical details about setting the desired confidence interval.
50 *
51 * The main method to set the modeled uncertainty is \a setCovMatrixAndMean()
52 *
53 * \tparam DIM The dimensionality of the parameter space, which must coincide with that of the rendering space (2 or 3)
54 *
55 * \ingroup mrpt_opengl_grp
56 */
57 template <int DIM>
59 {
60 public:
61 typedef mrpt::math::CMatrixFixedNumeric<double,DIM,DIM> cov_matrix_t; //!< The type of fixed-size covariance matrices for this representation
62 typedef mrpt::math::CMatrixFixedNumeric<double,DIM,1> mean_vector_t; //!< The type of fixed-size vector for this representation
63
66
67 /** Set the NxN covariance matrix that will determine the aspect of the ellipsoid - Notice that the
68 * covariance determines the uncertainty in the parameter space, which would be transformed by derived function
69 */
70 template <typename MATRIX, typename VECTOR>
71 void setCovMatrixAndMean( const MATRIX &new_cov, const VECTOR &new_mean )
72 {
74 ASSERT_( new_cov.getColCount() == new_cov.getRowCount() && new_cov.getColCount() == DIM )
75 m_cov = new_cov;
76 m_mean = new_mean;
80 }
81
82 /** Gets the current uncertainty covariance of parameter space */
83 const cov_matrix_t &getCovMatrix() const { return m_cov; }
84
85 /** Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the exact mathematical meaning is:
86 * This value of "quantiles" \a q should be set to the square root of the chi-squared inverse cdf corresponding to
87 * the desired confidence interval.
88 * <b>Note that this value depends on the dimensionality</b>.
89 * Refer to the MATLAB functions \a chi2inv() and \a chi2cdf().
90 *
91 * Some common values follow here for the convenience of users:
92 * - Dimensionality=3 (3D ellipsoids):
93 * - 19.8748% CI -> q=1
94 * - 73.8536% CI -> q=2
95 * - 97.0709% CI -> q=3
96 * - 99.8866% CI -> q=4
97 * - Dimensionality=2 (2D ellipses):
98 * - 39.347% CI -> q=1
99 * - 86.466% CI -> q=2
100 * - 98.8891% CI -> q=3
101 * - 99.9664% CI -> q=4
102 * - Dimensionality=1 (Not aplicable to this class but provided for reference):
103 * - 68.27% CI -> q=1
104 * - 95.45% CI -> q=2
105 * - 99.73% CI -> q=3
106 * - 99.9937% CI -> q=4
107 *
108 */
110 /** Refer to documentation of \a setQuantiles() */
111 float getQuantiles() const { return m_quantiles; }
112
113 /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
115 float getLineWidth() const { return m_lineWidth; }
116
117 /** Set the number of segments of the surface/curve (higher means with greater resolution) */
120
121 /** Render
122 * If one of the eigen value of the covariance matrix of the ellipsoid is null, ellipsoid will not
123 * be rendered to ensure stability in the rendering process.
124 */
126 {
128 // 1) Update eigenvectors/values:
130 {
132 // Handle the special case of an ellipsoid of volume = 0
133 const double d=m_cov.det();
134 if (fabs(d)<1e-20 || d!=d) // Note: "d!=d" is a great test for invalid numbers, don't remove!
135 {
136 // All zeros:
137 m_U.setZero(DIM,DIM);
138 }
139 else{
140 // Not null matrix:
141 m_cov.chol(m_U);
142 }
143 }
144
145 // Only if all the eigenvalues are !=0
146 bool eig_ok = true;
147 for (int i=0;i<DIM;i++)
148 if (m_U.coeff(i,i)==0)
149 eig_ok=false;
150
151 if(eig_ok)
152 {
153 // 2) Generate "standard" ellipsoid:
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);
157
158 // 3) Transform into 2D/3D render space:
159 std::vector<array_point_t> render_pts;
160 this->transformFromParameterSpace(params_pts,render_pts);
161
162 // 3.5) Save bounding box:
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++)
167 {
168 mrpt::utils::keep_min(m_bb_min[k], render_pts[i][k] );
169 mrpt::utils::keep_max(m_bb_max[k], render_pts[i][k] );
170 }
171 // Convert to coordinates of my parent:
174
175 // 4) Render them:
176 mrpt::opengl::detail::renderGeneralizedEllipsoidTemplate<DIM>(render_pts,
179 }
180
182 }
183
184 /** Evaluates the bounding box of this object (including possible children) in the coordinate frame of the object parent. */
186 {
187 bb_min = m_bb_min;
188 bb_max = m_bb_max;
189 }
190
191 /** Ray tracing
192 */
193 virtual bool traceRay(const mrpt::poses::CPose3D &o,double &dist) const MRPT_OVERRIDE {
195 THROW_EXCEPTION("Not implemented ")
196 }
197
198 protected:
199 /** To be implemented by derived classes: maps, using some arbitrary space transformation, a list of points
200 * defining an ellipsoid in parameter space into their corresponding points in 2D/3D space.
201 */
203 const std::vector<array_point_t> &params_pts,
204 std::vector<array_point_t> & out_pts) const = 0;
205
209 float m_quantiles; //!< The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
210 float m_lineWidth; //!< The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
211 uint32_t m_numSegments; //!< Number of segments in 2D/3D ellipsoids (default=10)
213
214 mutable cov_matrix_t m_U; //!< Cholesky U triangular matrix cache. */
215
217 {
218 using namespace mrpt::math; using namespace mrpt::utils;
219 const uint8_t version = 0;
220 out << version
221 << m_cov << m_mean
223 }
225 {
226 uint8_t version;
227 in >> version;
228 switch (version)
229 {
230 case 0:
231 {
232 in >> m_cov >> m_mean
235 }
236 break;
237 default:
239 };
241 }
242
245 m_quantiles(3.f),
246 m_lineWidth(1.f),
247 m_numSegments(50),
248 m_bb_min(0,0,0),
249 m_bb_max(0,0,0)
250 {
251 }
253 };
254
255 } // end namespace
256
257} // End of namespace
258
259
260#endif
A numeric matrix of compile-time fixed size.
A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of uncertainty sh...
mrpt::math::CMatrixFixedNumeric< double, DIM, DIM > cov_matrix_t
The type of fixed-size covariance matrices for this representation.
const cov_matrix_t & getCovMatrix() const
Gets the current uncertainty covariance of parameter space.
virtual bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const MRPT_OVERRIDE
Ray tracing.
mrpt::math::CMatrixFixedNumeric< double, DIM, 1 > mean_vector_t
The type of fixed-size vector for this representation.
void render_dl() const MRPT_OVERRIDE
Render If one of the eigen value of the covariance matrix of the ellipsoid is null,...
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...
float getQuantiles() const
Refer to documentation of setQuantiles()
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_writeToStream(mrpt::utils::CStream &out) const
void setNumberOfSegments(const uint32_t numSegments)
Set the number of segments of the surface/curve (higher means with greater resolution)
uint32_t m_numSegments
Number of segments in 2D/3D ellipsoids (default=10)
virtual void transformFromParameterSpace(const std::vector< array_point_t > &params_pts, std::vector< array_point_t > &out_pts) const =0
To be implemented by derived classes: maps, using some arbitrary space transformation,...
cov_matrix_t m_U
Cholesky U triangular matrix cache. *‍/.
mrpt::math::CMatrixFixedNumeric< float, DIM, 1 > array_point_t
void setLineWidth(float w)
The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
float m_quantiles
The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
float m_lineWidth
The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
mrpt::math::CMatrixFixedNumeric< float, DIM, 1 > array_parameter_t
void setQuantiles(float q)
Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the ...
A renderizable object suitable for rendering with OpenGL's display lists.
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference. This class automatically holds the cached 3x3 rotation m...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition CPose3D.h:73
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition CStream.h:39
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
This file implements matrix/vector text and binary serialization.
#define MRPT_START
#define ASSERT_(f)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
#define MRPT_END
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define THROW_EXCEPTION(msg)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
This base provides a set of functions for maths stuff.
Definition CArray.h:19
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)
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 renderGeneralizedEllipsoidTemplate< 2 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 2, 1 > > &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
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)
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)
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)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition zip.h:16
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.
Definition bits.h:145
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.
Definition bits.h:140
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned long uint32_t
Definition pstdint.h:216
unsigned char uint8_t
Definition pstdint.h:143
Lightweight 3D point.



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 13:45:58 UTC 2023