Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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) */
118  void setNumberOfSegments(const uint32_t numSegments) { m_numSegments=numSegments; CRenderizableDisplayList::notifyChange(); }
119  uint32_t getNumberOfSegments() { return m_numSegments; }
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  */
125  void render_dl() const
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. */
185  virtual void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
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 {
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 
206  mutable cov_matrix_t m_cov;
207  mean_vector_t m_mean;
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
222  << m_quantiles << m_lineWidth << m_numSegments;
223  }
225  {
226  uint8_t version;
227  in >> version;
228  switch (version)
229  {
230  case 0:
231  {
232  in >> m_cov >> m_mean
233  >> m_quantiles >> m_lineWidth >> m_numSegments;
234  m_needToRecomputeEigenVals = true;
235  }
236  break;
237  default:
239  };
241  }
242 
244  m_needToRecomputeEigenVals(true),
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)
virtual void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
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)
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)
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)
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.
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.
This base provides a set of functions for maths stuff.
Definition: CArray.h:18
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...
#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)
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)
void render_dl() const
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.
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
float getQuantiles() const
Refer to documentation of setQuantiles()
This file implements matrix/vector text and binary serialization.
virtual bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const
Ray tracing.
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.
Lightweight 3D point.
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)



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Tue Dec 8 09:49:21 UTC 2015