MRPT  2.0.1
CEllipsoid3D.cpp
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 
10 #include "opengl-precomp.h" // Precompiled header
11 
12 #include <mrpt/math/TLine3D.h>
13 #include <mrpt/math/geometry.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::opengl;
20 using namespace mrpt::math;
21 using namespace std;
22 
25 
27  const std::vector<BASE::array_parameter_t>& in_pts,
28  std::vector<BASE::array_point_t>& out_pts) const
29 {
30  // Euclidean space:
31  out_pts = in_pts;
32 }
33 
34 uint8_t CEllipsoid3D::serializeGetVersion() const { return 0; }
36 {
37  writeToStreamRender(out);
38  out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_numSegments
39  << m_lineWidth;
40 }
41 
43  mrpt::serialization::CArchive& in, uint8_t version)
44 {
45  switch (version)
46  {
47  case 0:
48  {
49  readFromStreamRender(in);
50  in >> m_cov;
51  in >> m_drawSolid3D >> m_quantiles;
52  m_numSegments = in.ReadAs<uint32_t>();
53  in >> m_lineWidth;
54 
55  // Update cov. matrix cache:
56  setCovMatrix(m_cov);
57  }
58  break;
59  default:
61  };
63 }
64 
65 #if 0
66 static bool quickSolveEqn(double a, double b_2, double c, double& t)
67 {
68  double delta = square(b_2) - a * c;
69  if (delta == 0)
70  return (t = -b_2 / a) >= 0;
71  else if (delta > 0)
72  {
73  delta = sqrt(delta);
74  if ((t = (-b_2 - delta) / a) >= 0)
75  return true;
76  else
77  return (t = (-b_2 + delta) / a) >= 0;
78  }
79  else
80  return false;
81 }
82 #endif
83 
84 bool CEllipsoid3D::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
85 {
86 #if 0 // Update, someday...
87  if (m_cov.rows() != 3) return false;
88  TLine3D lin, lin2;
89  createFromPoseX((o - this->m_pose).asTPose(), lin);
90  lin.unitarize(); // By adding this line, distance from any point of the
91  // line to its base is exactly equal to the "t".
92  for (size_t i = 0; i < 3; i++)
93  {
94  lin2.pBase[i] = 0;
95  lin2.director[i] = 0;
96  for (size_t j = 0; j < 3; j++)
97  {
98  double vji = m_eigVec(j, i);
99  lin2.pBase[i] += vji * lin.pBase[j];
100  lin2.director[i] += vji * lin.director[j];
101  }
102  }
103  double a = 0, b_2 = 0, c = -square(m_quantiles);
104  for (size_t i = 0; i < 3; i++)
105  {
106  double ev = m_eigVal(i, i);
107  a += square(lin2.director[i] / ev);
108  b_2 += lin2.director[i] * lin2.pBase[i] / square(ev);
109  c += square(lin2.pBase[i] / ev);
110  }
111  return quickSolveEqn(a, b_2, c, dist);
112 #endif
113  return false;
114 }
IMPLEMENTS_SERIALIZABLE(CEllipsoid3D, CRenderizableShaderWireFrame, mrpt::opengl) void CEllipsoid3D
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
bool traceRay(const mrpt::poses::CPose3D &o, double &dist) const override
Ray tracing.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
STL namespace.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
void transformFromParameterSpace(const std::vector< BASE::array_parameter_t > &in_pts, std::vector< BASE::array_point_t > &out_pts) const override
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.
This base provides a set of functions for maths stuff.
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
return_t square(const num_t x)
Inline function for the square of a number.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
A 3D ellipsoid, centered at zero with respect to this object pose.
Definition: CEllipsoid3D.h:38
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
Definition: geometry.cpp:917
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void unitarize()
Unitarize director vector.
Definition: TLine3D.cpp:56
This file implements matrix/vector text and binary serialization.
Renderizable generic renderer for objects using the wireframe shader.
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19



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