Main MRPT website > C++ reference
MRPT logo
CGeneralizedEllipsoidTemplate.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef opengl_CGeneralizedEllipsoidTemplate_H
29 #define opengl_CGeneralizedEllipsoidTemplate_H
30 
31 #include <mrpt/opengl/CRenderizableDisplayList.h>
32 #include <mrpt/math/CMatrixFixedNumeric.h>
33 
34 namespace mrpt
35 {
36  namespace opengl
37  {
38  namespace detail
39  {
40  template <int DIM>
41  void OPENGL_IMPEXP renderGeneralizedEllipsoidTemplate(
42  const std::vector<mrpt::math::CArray<float,DIM> > & pts,
43  const float lineWidth,
44  const uint32_t slices,
45  const uint32_t stacks);
46  template <int DIM>
47  void OPENGL_IMPEXP generalizedEllipsoidPoints(
48  const mrpt::math::CMatrixFixedNumeric<double,DIM,DIM> & U,
49  const mrpt::math::CMatrixFixedNumeric<double,DIM,1> & mean,
50  std::vector<mrpt::math::CArray<float,DIM> > &out_params_pts,
51  const uint32_t slices,
52  const uint32_t stacks);
53  }
54 
55  /** A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of
56  * uncertainty shapes in either 2D or 3D. See derived classes for examples.
57  *
58  * Please read the documentation of CGeneralizedEllipsoidTemplate::setQuantiles() for learning
59  * the mathematical details about setting the desired confidence interval.
60  *
61  * The main method to set the modeled uncertainty is \a setCovMatrixAndMean()
62  *
63  * \tparam DIM The dimensionality of the parameter space, which must coincide with that of the rendering space (2 or 3)
64  *
65  * \ingroup mrpt_opengl_grp
66  */
67  template <int DIM>
68  class CGeneralizedEllipsoidTemplate : public CRenderizableDisplayList
69  {
70  public:
71  typedef mrpt::math::CMatrixFixedNumeric<double,DIM,DIM> cov_matrix_t; //!< The type of fixed-size covariance matrices for this representation
72  typedef mrpt::math::CMatrixFixedNumeric<double,DIM,1> mean_vector_t; //!< The type of fixed-size vector for this representation
73 
74  typedef mrpt::math::CArray<float,DIM> array_parameter_t;
75  typedef mrpt::math::CArray<float,DIM> array_point_t;
76 
77  /** Set the NxN covariance matrix that will determine the aspect of the ellipsoid - Notice that the
78  * covariance determines the uncertainty in the parameter space, which would be transformed by derived function
79  */
80  template <typename MATRIX, typename VECTOR>
81  void setCovMatrixAndMean( const MATRIX &new_cov, const VECTOR &new_mean )
82  {
83  MRPT_START
84  ASSERT_( new_cov.getColCount() == new_cov.getRowCount() && new_cov.getColCount() == DIM )
85  m_cov = new_cov;
86  m_mean = new_mean;
87  m_needToRecomputeEigenVals = true;
88  CRenderizableDisplayList::notifyChange();
89  MRPT_END
90  }
91 
92  /** Gets the current uncertainty covariance of parameter space */
93  const cov_matrix_t &getCovMatrix() const { return m_cov; }
94 
95  /** Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the exact mathematical meaning is:
96  * This value of "quantiles" \a q should be set to the square root of the chi-squared inverse cdf corresponding to
97  * the desired confidence interval.
98  * <b>Note that this value depends on the dimensionality</b>.
99  * Refer to the MATLAB functions \a chi2inv() and \a chi2cdf().
100  *
101  * Some common values follow here for the convenience of users:
102  * - Dimensionality=3 (3D ellipsoids):
103  * - 19.8748% CI -> q=1
104  * - 73.8536% CI -> q=2
105  * - 97.0709% CI -> q=3
106  * - 99.8866% CI -> q=4
107  * - Dimensionality=2 (2D ellipses):
108  * - 39.347% CI -> q=1
109  * - 86.466% CI -> q=2
110  * - 98.8891% CI -> q=3
111  * - 99.9664% CI -> q=4
112  * - Dimensionality=1 (Not aplicable to this class but provided for reference):
113  * - 68.27% CI -> q=1
114  * - 95.45% CI -> q=2
115  * - 99.73% CI -> q=3
116  * - 99.9937% CI -> q=4
117  *
118  */
119  void setQuantiles(float q) { m_quantiles=q; CRenderizableDisplayList::notifyChange(); }
120  /** Refer to documentation of \a setQuantiles() */
121  float getQuantiles() const { return m_quantiles; }
122 
123  /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
124  void setLineWidth(float w) { m_lineWidth=w; CRenderizableDisplayList::notifyChange(); }
125  float getLineWidth() const { return m_lineWidth; }
126 
127  /** Set the number of segments of the surface/curve (higher means with greater resolution) */
128  void setNumberOfSegments(const uint32_t numSegments) { m_numSegments=numSegments; CRenderizableDisplayList::notifyChange(); }
129  uint32_t getNumberOfSegments() { return m_numSegments; }
130 
131  /** Render
132  * If one of the eigen value of the covariance matrix of the ellipsoid is null, ellipsoid will not
133  * be rendered to ensure stability in the rendering process.
134  */
135  void render_dl() const
136  {
137  MRPT_START
138  // 1) Update eigenvectors/values:
139  if (m_needToRecomputeEigenVals)
140  {
141  m_needToRecomputeEigenVals = false;
142  // Handle the special case of an ellipsoid of volume = 0
143  const double d=m_cov.det();
144  if (d==0 || d!=d) // Note: "d!=d" is a great test for invalid numbers, don't remove!
145  {
146  // All zeros:
147  m_U.setZero(DIM,DIM);
148  }
149  else{
150  // Not null matrix:
151  m_cov.chol(m_U);
152  }
153  }
154 
155  // Only if all the eigenvalues are !=0
156  bool eig_ok = true;
157  for (int i=0;i<DIM;i++)
158  if (m_U.coeff(i,i)==0)
159  eig_ok=false;
160 
161  if(eig_ok)
162  {
163  // 2) Generate "standard" ellipsoid:
164  std::vector<array_parameter_t> params_pts;
165  const cov_matrix_t Uscaled = static_cast<double>(m_quantiles) * m_U;
166  detail::generalizedEllipsoidPoints<DIM>(Uscaled,m_mean, params_pts,m_numSegments,m_numSegments);
167 
168  // 3) Transform into 2D/3D render space:
169  std::vector<array_point_t> render_pts;
170  this->transformFromParameterSpace(params_pts,render_pts);
171 
172  // 4) Render them:
173  mrpt::opengl::detail::renderGeneralizedEllipsoidTemplate<DIM>(render_pts,
174  m_lineWidth,
175  m_numSegments,m_numSegments );
176  }
177 
178  MRPT_END
179  }
180 
181  /** Ray tracing
182  */
183  virtual bool traceRay(const mrpt::poses::CPose3D &o,double &dist) const { THROW_EXCEPTION("Not implemented ") }
184 
185  protected:
186  /** To be implemented by derived classes: maps, using some arbitrary space transformation, a list of points
187  * defining an ellipsoid in parameter space into their corresponding points in 2D/3D space.
188  */
189  virtual void transformFromParameterSpace(
190  const std::vector<array_point_t> &params_pts,
191  std::vector<array_point_t> & out_pts) const = 0;
192 
193  mutable cov_matrix_t m_cov;
194  mean_vector_t m_mean;
195  mutable bool m_needToRecomputeEigenVals;
196  float m_quantiles; //!< The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
197  float m_lineWidth; //!< The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
198  uint32_t m_numSegments; //!< Number of segments in 2D/3D ellipsoids (default=10)
199 
200  mutable cov_matrix_t m_U; //!< Cholesky U triangular matrix cache. */
201 
202  void thisclass_writeToStream(mrpt::utils::CStream &out) const
203  {
204  const uint8_t version = 0;
205  out << version
206  << m_cov << m_mean
207  << m_quantiles << m_lineWidth << m_numSegments;
208  }
209  void thisclass_readFromStream(mrpt::utils::CStream &in)
210  {
211  uint8_t version;
212  in >> version;
213  switch (version)
214  {
215  case 0:
216  {
217  in >> m_cov >> m_mean
218  >> m_quantiles >> m_lineWidth >> m_numSegments;
219  m_needToRecomputeEigenVals = true;
220  }
221  break;
222  default:
223  MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
224  };
225  }
226 
227  CGeneralizedEllipsoidTemplate() :
228  m_needToRecomputeEigenVals(true),
229  m_quantiles(3.f),
230  m_lineWidth(1.f),
231  m_numSegments(50)
232  {
233  }
234  virtual ~CGeneralizedEllipsoidTemplate() { }
235  };
236 
237  } // end namespace
238 
239 } // End of namespace
240 
241 
242 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013