Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
14 #include <mrpt/utils/types_math.h>
15 #include <mrpt/utils/CStream.h> // for >> ops
16 #include <mrpt/math/matrix_serialization.h> // for >> ops
18 
19 namespace 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);
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);
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;
79  MRPT_END
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  {
127  MRPT_START
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,
177  m_lineWidth,
179  }
180 
181  MRPT_END
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  */
202  virtual void transformFromParameterSpace(
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  {
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
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.
Definition: zip.h:16
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.
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)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
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...
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference. This class automatically holds the cached 3x3 rotation m...
Definition: CRenderizable.h:55
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)
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
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...
unsigned char uint8_t
Definition: rptypes.h:43
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
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...
Definition: CPose3D.cpp:427
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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.
Definition: CArrayNumeric.h:19
#define MRPT_END
#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)
int version
Definition: mrpt_jpeglib.h:898
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...
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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, a list of points defining an ellipsoid in parameter space into their corresponding points in 2D/3D space.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
This file implements matrix/vector text and binary serialization.
const cov_matrix_t & getCovMatrix() const
Gets the current uncertainty covariance of parameter space.
GLuint in
Definition: glext.h:6301
void setQuantiles(float q)
Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the ...
#define ASSERT_(f)
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. */.
Lightweight 3D point.
unsigned __int32 uint32_t
Definition: rptypes.h:49
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)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019