MRPT  1.9.9
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-2018, 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/math/types_math.h>
15 #include <mrpt/serialization/CArchive.h> // for >> ops
16 #include <mrpt/math/matrix_serialization.h> // for >> ops
17 
18 namespace mrpt
19 {
20 namespace opengl
21 {
22 namespace detail
23 {
24 template <int DIM>
26  const std::vector<mrpt::math::CMatrixFixedNumeric<float, DIM, 1>>& pts,
27  const float lineWidth, const uint32_t slices, const uint32_t stacks);
28 template <>
30  const std::vector<mrpt::math::CMatrixFixedNumeric<float, 2, 1>>& pts,
31  const float lineWidth, const uint32_t slices, const uint32_t stacks);
32 template <>
34  const std::vector<mrpt::math::CMatrixFixedNumeric<float, 3, 1>>& pts,
35  const float lineWidth, const uint32_t slices, const uint32_t stacks);
36 
37 template <int DIM>
41  std::vector<mrpt::math::CMatrixFixedNumeric<float, DIM, 1>>& out_params_pts,
42  const uint32_t slices, const uint32_t stacks);
43 template <>
47  std::vector<mrpt::math::CMatrixFixedNumeric<float, 2, 1>>& out_params_pts,
48  const uint32_t slices, const uint32_t stacks);
49 template <>
53  std::vector<mrpt::math::CMatrixFixedNumeric<float, 3, 1>>& out_params_pts,
54  const uint32_t slices, const uint32_t stacks);
55 } // namespace detail
56 
57 /** A class that generalizes the concept of an ellipsoid to arbitrary
58  * parameterizations of
59  * uncertainty shapes in either 2D or 3D. See derived classes for examples.
60  *
61  * Please read the documentation of
62  * CGeneralizedEllipsoidTemplate::setQuantiles() for learning
63  * the mathematical details about setting the desired confidence interval.
64  *
65  * The main method to set the modeled uncertainty is \a setCovMatrixAndMean()
66  *
67  * \tparam DIM The dimensionality of the parameter space, which must coincide
68  * with that of the rendering space (2 or 3)
69  *
70  * \ingroup mrpt_opengl_grp
71  */
72 template <int DIM>
74 {
75  public:
76  /** The type of fixed-size covariance matrices for this representation */
78  /** The type of fixed-size vector for this representation */
80 
83 
84  /** Set the NxN covariance matrix that will determine the aspect of the
85  * ellipsoid - Notice that the
86  * covariance determines the uncertainty in the parameter space, which
87  * would be transformed by derived function
88  */
89  template <typename MATRIX, typename VECTOR>
90  void setCovMatrixAndMean(const MATRIX& new_cov, const VECTOR& new_mean)
91  {
93  ASSERT_(new_cov.cols() == new_cov.rows() && new_cov.cols() == DIM);
94  m_cov = new_cov;
95  m_mean = new_mean;
98  MRPT_END
99  }
100 
101  /** Gets the current uncertainty covariance of parameter space */
102  const cov_matrix_t& getCovMatrix() const { return m_cov; }
103  /** Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid
104  *(default=3, ~97 or ~98% CI); the exact mathematical meaning is:
105  * This value of "quantiles" \a q should be set to the square root of the
106  *chi-squared inverse cdf corresponding to
107  * the desired confidence interval.
108  * <b>Note that this value depends on the dimensionality</b>.
109  * Refer to the MATLAB functions \a chi2inv() and \a chi2cdf().
110  *
111  * Some common values follow here for the convenience of users:
112  * - Dimensionality=3 (3D ellipsoids):
113  * - 19.8748% CI -> q=1
114  * - 73.8536% CI -> q=2
115  * - 97.0709% CI -> q=3
116  * - 99.8866% CI -> q=4
117  * - Dimensionality=2 (2D ellipses):
118  * - 39.347% CI -> q=1
119  * - 86.466% CI -> q=2
120  * - 98.8891% CI -> q=3
121  * - 99.9664% CI -> q=4
122  * - Dimensionality=1 (Not aplicable to this class but provided for
123  *reference):
124  * - 68.27% CI -> q=1
125  * - 95.45% CI -> q=2
126  * - 99.73% CI -> q=3
127  * - 99.9937% CI -> q=4
128  *
129  */
130  void setQuantiles(float q)
131  {
132  m_quantiles = q;
134  }
135  /** Refer to documentation of \a setQuantiles() */
136  float getQuantiles() const { return m_quantiles; }
137  /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
138  void setLineWidth(float w)
139  {
140  m_lineWidth = w;
142  }
143  float getLineWidth() const { return m_lineWidth; }
144  /** Set the number of segments of the surface/curve (higher means with
145  * greater resolution) */
146  void setNumberOfSegments(const uint32_t numSegments)
147  {
148  m_numSegments = numSegments;
150  }
152  /** Render
153  * If one of the eigen value of the covariance matrix of the ellipsoid is
154  *null, ellipsoid will not
155  * be rendered to ensure stability in the rendering process.
156  */
157  void render_dl() const override
158  {
159  MRPT_START
160  // 1) Update eigenvectors/values:
162  {
164  // Handle the special case of an ellipsoid of volume = 0
165  const double d = m_cov.det();
166  if (fabs(d) < 1e-20 || d != d) // Note: "d!=d" is a great test for
167  // invalid numbers, don't remove!
168  {
169  // All zeros:
170  m_U.setZero(DIM, DIM);
171  }
172  else
173  {
174  // Not null matrix:
175  m_cov.chol(m_U);
176  }
177  }
178 
179  // Only if all the eigenvalues are !=0
180  bool eig_ok = true;
181  for (int i = 0; i < DIM; i++)
182  if (m_U.coeff(i, i) == 0) eig_ok = false;
183 
184  if (eig_ok)
185  {
186  // 2) Generate "standard" ellipsoid:
187  std::vector<array_parameter_t> params_pts;
188  const cov_matrix_t Uscaled = static_cast<double>(m_quantiles) * m_U;
189  detail::generalizedEllipsoidPoints<DIM>(
190  Uscaled, m_mean, params_pts, m_numSegments, m_numSegments);
191 
192  // 3) Transform into 2D/3D render space:
193  std::vector<array_point_t> render_pts;
194  this->transformFromParameterSpace(params_pts, render_pts);
195 
196  // 3.5) Save bounding box:
198  std::numeric_limits<double>::max(),
199  std::numeric_limits<double>::max(), 0);
201  -std::numeric_limits<double>::max(),
202  -std::numeric_limits<double>::max(), 0);
203  for (size_t i = 0; i < render_pts.size(); i++)
204  for (int k = 0; k < DIM; k++)
205  {
206  mrpt::keep_min(m_bb_min[k], render_pts[i][k]);
207  mrpt::keep_max(m_bb_max[k], render_pts[i][k]);
208  }
209  // Convert to coordinates of my parent:
212 
213  // 4) Render them:
214  mrpt::opengl::detail::renderGeneralizedEllipsoidTemplate<DIM>(
215  render_pts, m_lineWidth, m_numSegments, m_numSegments);
216  }
217 
218  MRPT_END
219  }
220 
221  /** Evaluates the bounding box of this object (including possible children)
222  * in the coordinate frame of the object parent. */
223  virtual void getBoundingBox(
224  mrpt::math::TPoint3D& bb_min,
225  mrpt::math::TPoint3D& bb_max) const override
226  {
227  bb_min = m_bb_min;
228  bb_max = m_bb_max;
229  }
230 
231  /** Ray tracing
232  */
233  virtual bool traceRay(
234  const mrpt::poses::CPose3D& o, double& dist) const override
235  {
237  MRPT_UNUSED_PARAM(dist);
238  THROW_EXCEPTION("Not implemented ");
239  }
240 
241  protected:
242  /** To be implemented by derived classes: maps, using some arbitrary space
243  * transformation, a list of points
244  * defining an ellipsoid in parameter space into their corresponding
245  * points in 2D/3D space.
246  */
247  virtual void transformFromParameterSpace(
248  const std::vector<array_point_t>& params_pts,
249  std::vector<array_point_t>& out_pts) const = 0;
250 
254  /** The number of "sigmas" for drawing the ellipse/ellipsoid (default=3) */
255  float m_quantiles;
256  /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
257  float m_lineWidth;
258  /** Number of segments in 2D/3D ellipsoids (default=10) */
261 
262  /** Cholesky U triangular matrix cache. */
263  mutable cov_matrix_t m_U;
264 
266  {
267  using namespace mrpt::math;
268 
269  const uint8_t version = 0;
270  out << version << m_cov << m_mean << m_quantiles << m_lineWidth
271  << m_numSegments;
272  }
274  {
275  uint8_t version;
276  in >> version;
277  switch (version)
278  {
279  case 0:
280  {
281  in >> m_cov >> m_mean >> m_quantiles >> m_lineWidth >>
284  }
285  break;
286  default:
288  };
290  }
291 
294  m_quantiles(3.f),
295  m_lineWidth(1.f),
296  m_numSegments(50),
297  m_bb_min(0, 0, 0),
298  m_bb_max(0, 0, 0)
299  {
300  }
302 };
303 
304 } // namespace opengl
305 
306 } // namespace mrpt
307 
308 #endif
#define MRPT_START
Definition: exceptions.h:262
float m_lineWidth
The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
void thisclass_writeToStream(mrpt::serialization::CArchive &out) const
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)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
void 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)
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
virtual void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
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...
float getQuantiles() const
Refer to documentation of setQuantiles()
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.
Definition: CRenderizable.h:55
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
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:41
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
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 renderGeneralizedEllipsoidTemplate(const std::vector< mrpt::math::CMatrixFixedNumeric< float, DIM, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
void setLineWidth(float w)
The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
void render_dl() const 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.
void renderGeneralizedEllipsoidTemplate< 3 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 3, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of uncertainty sh...
void renderGeneralizedEllipsoidTemplate< 2 >(const std::vector< mrpt::math::CMatrixFixedNumeric< float, 2, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
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.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
void thisclass_readFromStream(mrpt::serialization::CArchive &in)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
This file implements matrix/vector text and binary serialization.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, 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:379
const cov_matrix_t & getCovMatrix() const
Gets the current uncertainty covariance of parameter space.
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
void setQuantiles(float q)
Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the ...
virtual bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
cov_matrix_t m_U
Cholesky U triangular matrix cache.
void 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 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)
Lightweight 3D point.
unsigned __int32 uint32_t
Definition: rptypes.h:47
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020