Main MRPT website > C++ reference for MRPT 1.5.6
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)
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) ...
void thisclass_writeToStream(mrpt::utils::CStream &out) const
float m_quantiles
The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
#define THROW_EXCEPTION(msg)
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
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...
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)
GLuint in
Definition: glew.h:7146
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's display lists.
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.
#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
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
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
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 ...
#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. */.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
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...
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)
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018