MRPT  1.9.9
CGeneralizedEllipsoidTemplate.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/CMatrixFixed.h>
12 #include <mrpt/math/matrix_serialization.h> // for >> ops
13 //#include <mrpt/math/types_math.h>
15 #include <mrpt/serialization/CArchive.h> // for >> ops
16 
17 namespace mrpt
18 {
19 namespace opengl
20 {
21 namespace detail
22 {
23 template <int DIM>
25  const std::vector<mrpt::math::CMatrixFixed<float, DIM, 1>>& pts,
26  const float lineWidth, const uint32_t slices, const uint32_t stacks);
27 template <>
29  const std::vector<mrpt::math::CMatrixFixed<float, 2, 1>>& pts,
30  const float lineWidth, const uint32_t slices, const uint32_t stacks);
31 template <>
33  const std::vector<mrpt::math::CMatrixFixed<float, 3, 1>>& pts,
34  const float lineWidth, const uint32_t slices, const uint32_t stacks);
35 
36 template <int DIM>
40  std::vector<mrpt::math::CMatrixFixed<float, DIM, 1>>& out_params_pts,
41  const uint32_t slices, const uint32_t stacks);
42 template <>
46  std::vector<mrpt::math::CMatrixFixed<float, 2, 1>>& out_params_pts,
47  const uint32_t slices, const uint32_t stacks);
48 template <>
52  std::vector<mrpt::math::CMatrixFixed<float, 3, 1>>& out_params_pts,
53  const uint32_t slices, const uint32_t stacks);
54 } // namespace detail
55 
56 /** A class that generalizes the concept of an ellipsoid to arbitrary
57  * parameterizations of
58  * uncertainty shapes in either 2D or 3D. See derived classes for examples.
59  *
60  * Please read the documentation of
61  * CGeneralizedEllipsoidTemplate::setQuantiles() for learning
62  * the mathematical details about setting the desired confidence interval.
63  *
64  * The main method to set the modeled uncertainty is \a setCovMatrixAndMean()
65  *
66  * \tparam DIM The dimensionality of the parameter space, which must coincide
67  * with that of the rendering space (2 or 3)
68  *
69  * \ingroup mrpt_opengl_grp
70  */
71 template <int DIM>
73 {
74  public:
75  /** The type of fixed-size covariance matrices for this representation */
77  /** The type of fixed-size vector for this representation */
79 
82 
83  /** Set the NxN covariance matrix that will determine the aspect of the
84  * ellipsoid - Notice that the
85  * covariance determines the uncertainty in the parameter space, which
86  * would be transformed by derived function
87  */
88  template <typename MATRIX, typename VECTOR>
89  void setCovMatrixAndMean(const MATRIX& new_cov, const VECTOR& new_mean)
90  {
92  ASSERT_(new_cov.cols() == new_cov.rows() && new_cov.cols() == DIM);
93  m_cov = new_cov;
94  m_mean = new_mean;
97  MRPT_END
98  }
99 
100  /** Gets the current uncertainty covariance of parameter space */
101  const cov_matrix_t& getCovMatrix() const { return m_cov; }
102  /** Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid
103  *(default=3, ~97 or ~98% CI); the exact mathematical meaning is:
104  * This value of "quantiles" \a q should be set to the square root of the
105  *chi-squared inverse cdf corresponding to
106  * the desired confidence interval.
107  * <b>Note that this value depends on the dimensionality</b>.
108  * Refer to the MATLAB functions \a chi2inv() and \a chi2cdf().
109  *
110  * Some common values follow here for the convenience of users:
111  * - Dimensionality=3 (3D ellipsoids):
112  * - 19.8748% CI -> q=1
113  * - 73.8536% CI -> q=2
114  * - 97.0709% CI -> q=3
115  * - 99.8866% CI -> q=4
116  * - Dimensionality=2 (2D ellipses):
117  * - 39.347% CI -> q=1
118  * - 86.466% CI -> q=2
119  * - 98.8891% CI -> q=3
120  * - 99.9664% CI -> q=4
121  * - Dimensionality=1 (Not aplicable to this class but provided for
122  *reference):
123  * - 68.27% CI -> q=1
124  * - 95.45% CI -> q=2
125  * - 99.73% CI -> q=3
126  * - 99.9937% CI -> q=4
127  *
128  */
129  void setQuantiles(float q)
130  {
131  m_quantiles = q;
133  }
134  /** Refer to documentation of \a setQuantiles() */
135  float getQuantiles() const { return m_quantiles; }
136  /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
137  void setLineWidth(float w)
138  {
139  m_lineWidth = w;
141  }
142  float getLineWidth() const { return m_lineWidth; }
143  /** Set the number of segments of the surface/curve (higher means with
144  * greater resolution) */
145  void setNumberOfSegments(const uint32_t numSegments)
146  {
147  m_numSegments = numSegments;
149  }
150  uint32_t getNumberOfSegments() { return m_numSegments; }
151  /** Render
152  * If one of the eigen value of the covariance matrix of the ellipsoid is
153  *null, ellipsoid will not
154  * be rendered to ensure stability in the rendering process.
155  */
156  void render_dl() const override
157  {
158  MRPT_START
159  // 1) Update eigenvectors/values:
161  {
163  // Handle the special case of an ellipsoid of volume = 0
164  const double d = m_cov.det();
165  if (fabs(d) < 1e-20 || d != d) // Note: "d!=d" is a great test for
166  // invalid numbers, don't remove!
167  {
168  // All zeros:
169  m_U.setZero(DIM, DIM);
170  }
171  else
172  {
173  // Not null matrix:
174  m_cov.chol(m_U);
175  }
176  }
177 
178  // Only if all the eigenvalues are !=0
179  bool eig_ok = true;
180  for (int i = 0; i < DIM; i++)
181  if (m_U.coeff(i, i) == 0) eig_ok = false;
182 
183  if (eig_ok)
184  {
185  // 2) Generate "standard" ellipsoid:
186  std::vector<array_parameter_t> params_pts;
187  cov_matrix_t Uscaled = m_U;
188  Uscaled *= static_cast<double>(m_quantiles);
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. */
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  bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override
234  {
236  MRPT_UNUSED_PARAM(dist);
237  THROW_EXCEPTION("Not implemented ");
238  }
239 
240  protected:
241  /** To be implemented by derived classes: maps, using some arbitrary space
242  * transformation, a list of points
243  * defining an ellipsoid in parameter space into their corresponding
244  * points in 2D/3D space.
245  */
246  virtual void transformFromParameterSpace(
247  const std::vector<array_point_t>& params_pts,
248  std::vector<array_point_t>& out_pts) const = 0;
249 
252  mutable bool m_needToRecomputeEigenVals{true};
253  /** The number of "sigmas" for drawing the ellipse/ellipsoid (default=3) */
254  float m_quantiles{3.f};
255  /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
256  float m_lineWidth{1.f};
257  /** Number of segments in 2D/3D ellipsoids (default=10) */
258  uint32_t m_numSegments{50};
260 
261  /** Cholesky U triangular matrix cache. */
262  mutable cov_matrix_t m_U;
263 
265  {
266  using namespace mrpt::math;
267 
268  const uint8_t version = 0;
269  out << version << m_cov << m_mean << m_quantiles << m_lineWidth
270  << m_numSegments;
271  }
273  {
274  uint8_t version;
275  in >> version;
276  switch (version)
277  {
278  case 0:
279  {
280  in >> m_cov >> m_mean >> m_quantiles >> m_lineWidth >>
283  }
284  break;
285  default:
287  };
289  }
290 
292  ~CGeneralizedEllipsoidTemplate() override = default;
293 };
294 
295 } // namespace opengl
296 
297 } // namespace mrpt
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
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...
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
float m_lineWidth
The line width for 2D ellipses or 3D wireframe ellipsoids (default=1)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void thisclass_writeToStream(mrpt::serialization::CArchive &out) const
bool chol(Derived &U) const
Cholesky M=UT * U decomposition for symmetric matrix (upper-half of the matrix is actually ignored...
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:3727
void renderGeneralizedEllipsoidTemplate< 3 >(const std::vector< mrpt::math::CMatrixFixed< float, 3, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
float getQuantiles() const
Refer to documentation of setQuantiles()
uint32_t m_numSegments
Number of segments in 2D/3D ellipsoids (default=10)
void generalizedEllipsoidPoints< 2 >(const mrpt::math::CMatrixFixed< double, 2, 2 > &U, const mrpt::math::CMatrixFixed< double, 2, 1 > &mean, std::vector< mrpt::math::CMatrixFixed< float, 2, 1 >> &out_params_pts, const uint32_t slices, const uint32_t stacks)
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:54
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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&#39;s display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
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.
Scalar det() const
Determinant of matrix.
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...
const Scalar & coeff(int r, int c) const
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of uncertainty sh...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void generalizedEllipsoidPoints< 3 >(const mrpt::math::CMatrixFixed< double, 3, 3 > &U, const mrpt::math::CMatrixFixed< double, 3, 1 > &mean, std::vector< mrpt::math::CMatrixFixed< float, 3, 1 >> &out_params_pts, const uint32_t slices, const uint32_t stacks)
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:54
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 thisclass_readFromStream(mrpt::serialization::CArchive &in)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void renderGeneralizedEllipsoidTemplate(const std::vector< mrpt::math::CMatrixFixed< float, DIM, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
This file implements matrix/vector text and binary serialization.
const cov_matrix_t & getCovMatrix() const
Gets the current uncertainty covariance of parameter space.
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
void setQuantiles(float q)
Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the ...
double mean(const CONTAINER &v)
Computes the mean value of a vector.
const auto bb_max
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, 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...
cov_matrix_t m_U
Cholesky U triangular matrix cache.
const auto bb_min
Lightweight 3D point.
Definition: TPoint3D.h:90
void generalizedEllipsoidPoints(const mrpt::math::CMatrixFixed< double, DIM, DIM > &U, const mrpt::math::CMatrixFixed< double, DIM, 1 > &mean, std::vector< mrpt::math::CMatrixFixed< float, DIM, 1 >> &out_params_pts, const uint32_t slices, const uint32_t stacks)
void renderGeneralizedEllipsoidTemplate< 2 >(const std::vector< mrpt::math::CMatrixFixed< float, 2, 1 >> &pts, const float lineWidth, const uint32_t slices, const uint32_t stacks)
#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: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019