MRPT  2.0.1
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-2020, 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
15 #include <mrpt/serialization/CArchive.h> // for >> ops
16 
17 namespace mrpt::opengl
18 {
19 /** A class that generalizes the concept of an ellipsoid to arbitrary
20  * parameterizations of
21  * uncertainty shapes in either 2D or 3D. See derived classes for examples.
22  *
23  * Please read the documentation of
24  * CGeneralizedEllipsoidTemplate::setQuantiles() for learning
25  * the mathematical details about setting the desired confidence interval.
26  *
27  * The main method to set the modeled uncertainty is \a setCovMatrixAndMean()
28  *
29  * \tparam DIM The dimensionality of the parameter space, which must coincide
30  * with that of the rendering space (2 or 3)
31  *
32  * \ingroup mrpt_opengl_grp
33  */
34 template <int DIM>
36  : virtual public CRenderizableShaderTriangles,
37  virtual public CRenderizableShaderWireFrame
38 {
39  public:
40  /** @name Renderizable shader API virtual methods
41  * @{ */
42  void render(const RenderContext& rc) const override
43  {
44  switch (rc.shader_id)
45  {
48  break;
51  break;
52  };
53  }
54  void freeOpenGLResources() override
55  {
58  }
59 
60  void renderUpdateBuffers() const override
61  {
62  // 1) Update eigenvectors/values:
64  {
66  // Handle the special case of an ellipsoid of volume = 0
67  const double d = m_cov.det();
68  // Note: "d!=d" is a great test for invalid numbers, don't remove!
69  if (std::abs(d) < 1e-20 || d != d)
70  {
71  // All zeros:
72  m_U.setZero(DIM, DIM);
73  }
74  else
75  {
76  // A valid matrix:
77  m_cov.chol(m_U);
78  }
79  }
80 
81  // 2) Generate "standard" ellipsoid:
82  std::vector<array_parameter_t> params_pts;
83  cov_matrix_t Uscaled = m_U;
84  Uscaled *= static_cast<double>(m_quantiles);
85  generatePoints(Uscaled, params_pts);
86 
87  // 3) Transform into 2D/3D render space:
88  this->transformFromParameterSpace(params_pts, m_render_pts);
89 
90  // 3.5) Save bounding box:
92  std::numeric_limits<double>::max(),
93  std::numeric_limits<double>::max(), 0);
95  -std::numeric_limits<double>::max(),
96  -std::numeric_limits<double>::max(), 0);
97  for (size_t i = 0; i < m_render_pts.size(); i++)
98  for (int k = 0; k < DIM; k++)
99  {
102  }
103  // Convert to coordinates of my parent:
106 
109  }
110  virtual shader_list_t requiredShaders() const override
111  {
112  // May use up to two shaders (triangles and lines):
114  }
115  // Render precomputed points in m_render_pts:
117  // Render precomputed points in m_render_pts:
119  /** @} */
120 
121  /** The type of fixed-size covariance matrices for this representation
122  */
124  /** The type of fixed-size vector for this representation */
126 
129 
130  /** Set the NxN covariance matrix that will determine the aspect of the
131  * ellipsoid - Notice that the
132  * covariance determines the uncertainty in the parameter space, which
133  * would be transformed by derived function
134  */
135  template <typename MATRIX, typename VECTOR>
136  void setCovMatrixAndMean(const MATRIX& new_cov, const VECTOR& new_mean)
137  {
138  MRPT_START
139  ASSERT_EQUAL_(new_cov.cols(), new_cov.rows());
140  ASSERT_EQUAL_(new_cov.cols(), DIM);
141  m_cov = new_cov;
142  m_mean = new_mean;
145  MRPT_END
146  }
147 
148  /** Gets the current uncertainty covariance of parameter space */
149  const cov_matrix_t& getCovMatrix() const { return m_cov; }
150 
151  /** Like setCovMatrixAndMean(), for mean=zero.
152  */
153  template <typename MATRIX>
154  void setCovMatrix(const MATRIX& new_cov)
155  {
157  }
158 
159  /** Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid
160  *(default=3, ~97 or ~98% CI); the exact mathematical meaning is:
161  * This value of "quantiles" \a q should be set to the square root of
162  *the chi-squared inverse cdf corresponding to the desired confidence
163  *interval. <b>Note that this value depends on the dimensionality</b>.
164  * Refer to the MATLAB functions \a chi2inv() and \a chi2cdf().
165  *
166  * Some common values follow here for the convenience of users:
167  * - Dimensionality=3 (3D ellipsoids):
168  * - 19.8748% CI -> q=1
169  * - 73.8536% CI -> q=2
170  * - 97.0709% CI -> q=3
171  * - 99.8866% CI -> q=4
172  * - Dimensionality=2 (2D ellipses):
173  * - 39.347% CI -> q=1
174  * - 86.466% CI -> q=2
175  * - 98.8891% CI -> q=3
176  * - 99.9664% CI -> q=4
177  * - Dimensionality=1 (Not aplicable to this class but provided for
178  *reference):
179  * - 68.27% CI -> q=1
180  * - 95.45% CI -> q=2
181  * - 99.73% CI -> q=3
182  * - 99.9937% CI -> q=4
183  *
184  */
185  void setQuantiles(float q)
186  {
187  m_quantiles = q;
189  }
190  /** Refer to documentation of \a setQuantiles() */
191  float getQuantiles() const { return m_quantiles; }
192 
193  /** Set the number of segments of the surface/curve (higher means with
194  * greater resolution) */
195  void setNumberOfSegments(const uint32_t numSegments)
196  {
197  m_numSegments = numSegments;
199  }
200  uint32_t getNumberOfSegments() { return m_numSegments; }
201 
202  /** If set to "true", a whole ellipsoid surface will be drawn, or if
203  * set to "false" (default) it will be drawn as a "wireframe". */
204  void enableDrawSolid3D(bool v)
205  {
206  m_drawSolid3D = v;
208  }
209 
210  /** Evaluates the bounding box of this object (including possible
211  * children) in the coordinate frame of the object parent. */
214  mrpt::math::TPoint3D& bb_max) const override
215  {
216  bb_min = m_bb_min;
217  bb_max = m_bb_max;
218  }
219 
220  /** Ray tracing */
221  virtual bool traceRay(
222  [[maybe_unused]] const mrpt::poses::CPose3D& o,
223  [[maybe_unused]] double& dist) const override
224  {
225  THROW_EXCEPTION("Not implemented ");
226  }
227 
228  protected:
229  /** To be implemented by derived classes: maps, using some arbitrary
230  * space transformation, a list of points defining an ellipsoid in
231  * parameter space into their corresponding points in 2D/3D space.
232  */
233  virtual void transformFromParameterSpace(
234  const std::vector<array_point_t>& params_pts,
235  std::vector<array_point_t>& out_pts) const = 0;
236 
239  mutable bool m_needToRecomputeEigenVals{true};
240  /** The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
241  */
242  float m_quantiles{3.f};
243  /** Number of segments in 2D/3D ellipsoids (default=20) */
244  uint32_t m_numSegments{20};
245  mutable mrpt::math::TPoint3D m_bb_min{0, 0, 0}, m_bb_max{0, 0, 0};
246 
247  /** If set to "true", a whole ellipsoid surface will be drawn, or
248  * if set to "false" (default)it will be drawn as a "wireframe". */
249  bool m_drawSolid3D{false};
250 
251  /** Cholesky U triangular matrix cache. */
252  mutable cov_matrix_t m_U;
253  mutable std::vector<array_point_t> m_render_pts;
254 
256  {
257  using namespace mrpt::math;
258 
259  const uint8_t version = 0;
260  out << version << m_cov << m_mean << m_quantiles << m_lineWidth
261  << m_numSegments;
262  }
264  {
265  uint8_t version;
266  in >> version;
267  switch (version)
268  {
269  case 0:
270  {
271  in >> m_cov >> m_mean >> m_quantiles >> m_lineWidth >>
274  }
275  break;
276  default:
278  };
280  }
281 
282  CGeneralizedEllipsoidTemplate() = default;
283  virtual ~CGeneralizedEllipsoidTemplate() override = default;
284 
285  // Uscaled, m_mean, params_pts, m_numSegments, m_numSegments
286  void generatePoints(
287  const cov_matrix_t& U,
288  std::vector<array_parameter_t>& out_params_pts) const;
289 
290  void implUpdate_Wireframe();
291  void implUpdate_Triangles();
292 
293  public:
294  // Solve virtual public inheritance ambiguity:
295  virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const override
296  {
298  }
299 };
300 
301 } // namespace mrpt::opengl
void generatePoints(const cov_matrix_t &U, std::vector< array_parameter_t > &out_params_pts) const
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...
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void thisclass_writeToStream(mrpt::serialization::CArchive &out) const
void freeOpenGLResources() override
Free opengl buffers.
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) ...
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
float m_quantiles
The number of "sigmas" for drawing the ellipse/ellipsoid (default=3)
A structure that holds runtime class type information.
Definition: CObject.h:31
Renderizable generic renderer for objects using the triangles shader.
float getQuantiles() const
Refer to documentation of setQuantiles()
uint32_t m_numSegments
Number of segments in 2D/3D ellipsoids (default=20)
void freeOpenGLResources() override
Free opengl buffers.
mrpt::poses::CPose3D m_pose
6D pose wrt the parent coordinate reference.
Definition: CRenderizable.h:62
std::vector< shader_id_t > shader_list_t
A list of shader IDs.
Definition: Shader.h:26
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
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...
Context for calls to render()
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
This base provides a set of functions for maths stuff.
virtual ~CGeneralizedEllipsoidTemplate() override=default
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
static constexpr shader_id_t WIREFRAME
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
Scalar det() const
Determinant of matrix.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
void setCovMatrix(const MATRIX &new_cov)
Like setCovMatrixAndMean(), for mean=zero.
void onUpdateBuffers_Triangles() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
static constexpr shader_id_t TRIANGLES
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...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
A class that generalizes the concept of an ellipsoid to arbitrary parameterizations of uncertainty sh...
void onUpdateBuffers_Wireframe() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
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
This file implements matrix/vector text and binary serialization.
const cov_matrix_t & getCovMatrix() const
Gets the current uncertainty covariance of parameter space.
Renderizable generic renderer for objects using the wireframe shader.
#define MRPT_END
Definition: exceptions.h:245
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
void setQuantiles(float q)
Changes the scale of the "sigmas" for drawing the ellipse/ellipsoid (default=3, ~97 or ~98% CI); the ...
const auto bb_max
void enableDrawSolid3D(bool v)
If set to "true", a whole ellipsoid surface will be drawn, or if set to "false" (default) it will be ...
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
virtual bool traceRay([[maybe_unused]] const mrpt::poses::CPose3D &o, [[maybe_unused]] double &dist) const override
Ray tracing.
void freeOpenGLResources() override
Free opengl buffers.
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
bool m_drawSolid3D
If set to "true", a whole ellipsoid surface will be drawn, or if set to "false" (default)it will be d...
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
mrpt::math::CMatrixFixed< double, DIM, DIM > cov_matrix_t
The type of fixed-size covariance matrices for this representation.
virtual shader_list_t requiredShaders() const override
Returns the ID of the OpenGL shader program required to render this class.
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020