MRPT  1.9.9
pose_pdfs.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-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 
10 #include "opengl-precomp.h" // Precompiled header
11 
12 #include <mrpt/opengl/CEllipsoid.h>
17 
25 #include <mrpt/poses/CPosePDFSOG.h>
26 
27 using namespace mrpt;
28 using namespace mrpt::math;
29 using namespace mrpt::opengl;
30 using namespace mrpt::poses;
31 
32 const double POSE_TAIL_LENGTH = 0.1;
33 const double POSE_TAIL_WIDTH = 3.0;
34 const double POSE_POINT_SIZE = 4.0;
35 const double POSE_AXIS_SCALE = 0.1;
36 
37 #define POSE_COLOR 0, 0, 1
38 #define POINT_COLOR 1, 0, 0
39 
40 /** Returns a representation of a the PDF - this is just an auxiliary function,
41  * it's more natural to call
42  * mrpt::poses::CPosePDF::getAs3DObject */
44 {
45  auto outObj = CSetOfObjects::Create();
46 
47  if (IS_CLASS(o, CPosePDFSOG))
48  {
49  const auto* p = dynamic_cast<const CPosePDFSOG*>(&o);
50  ASSERT_(p != nullptr);
51 
52  opengl::CSetOfLines::Ptr lins = std::make_shared<opengl::CSetOfLines>();
53  lins->setColor(0, 0, 1, 0.6);
54  lins->setLineWidth(POSE_TAIL_WIDTH);
55 
56  for (const auto& it : *p)
57  {
59  std::make_shared<opengl::CEllipsoid>();
60 
61  ellip->setPose(CPose3D(it.mean.x(), it.mean.y(), 0));
62  ellip->setCovMatrix(it.cov, 2 /* x y */);
63  ellip->setColor(POSE_COLOR, 0.6);
64  ellip->setQuantiles(3);
65  ellip->enableDrawSolid3D(false);
66 
67  outObj->insert(ellip);
68 
69  lins->appendLine(
70  it.mean.x(), it.mean.y(), 0,
71  it.mean.x() + POSE_TAIL_LENGTH * cos(it.mean.phi()),
72  it.mean.y() + POSE_TAIL_LENGTH * sin(it.mean.phi()), 0);
73  }
74  outObj->insert(lins);
75  }
76  else if (IS_CLASS(o, CPosePDFGaussian))
77  {
78  const auto* p = dynamic_cast<const CPosePDFGaussian*>(&o);
79  ASSERT_(p != nullptr);
80 
81  opengl::CSetOfLines::Ptr lins = std::make_shared<opengl::CSetOfLines>();
82  lins->setColor(POSE_COLOR, 0.6);
83  lins->setLineWidth(POSE_TAIL_WIDTH);
84 
85  opengl::CEllipsoid::Ptr ellip = std::make_shared<opengl::CEllipsoid>();
86 
87  ellip->setPose(CPose3D(p->mean.x(), p->mean.y(), 0));
88  ellip->setCovMatrix(p->cov, 2 /* x y */);
89  ellip->setColor(POSE_COLOR, 0.6);
90 
91  ellip->setQuantiles(3);
92  ellip->enableDrawSolid3D(false);
93 
94  outObj->insert(ellip);
95 
96  lins->appendLine(
97  p->mean.x(), p->mean.y(), 0,
98  p->mean.x() + POSE_TAIL_LENGTH * cos(p->mean.phi()),
99  p->mean.y() + POSE_TAIL_LENGTH * sin(p->mean.phi()), 0);
100 
101  outObj->insert(lins);
102  }
103  else if (IS_CLASS(o, CPosePDFParticles))
104  {
105  const auto* p = dynamic_cast<const CPosePDFParticles*>(&o);
106  ASSERT_(p != nullptr);
107 
108  opengl::CPointCloud::Ptr pnts = std::make_shared<opengl::CPointCloud>();
109  pnts->setColor(POSE_COLOR, 0.6);
110  pnts->setPointSize(POSE_POINT_SIZE);
111 
112  opengl::CSetOfLines::Ptr lins = std::make_shared<opengl::CSetOfLines>();
113  lins->setColor(POSE_COLOR, 0.6);
114  lins->setLineWidth(POSE_TAIL_WIDTH);
115 
116  for (size_t i = 0; i < p->size(); ++i)
117  {
118  const auto po = p->m_particles[i].d;
119  pnts->insertPoint(po.x, po.y, 0);
120  lins->appendLine(
121  po.x, po.y, 0, po.x + POSE_TAIL_LENGTH * cos(po.phi),
122  po.y + POSE_TAIL_LENGTH * sin(po.phi), 0);
123  }
124  outObj->insert(pnts);
125  outObj->insert(lins);
126  }
127 
128  return outObj;
129 }
130 
131 /** Returns a representation of a the PDF - this is just an auxiliary function,
132  * it's more natural to call
133  * mrpt::poses::CPointPDF::getAs3DObject */
135 {
136  CSetOfObjects::Ptr outObj = std::make_shared<CSetOfObjects>();
137 
138  if (IS_CLASS(o, CPointPDFSOG))
139  {
140  const auto* p = dynamic_cast<const CPointPDFSOG*>(&o);
141  ASSERT_(p != nullptr);
142 
143  // For each gaussian node
144  for (const auto& it : *p)
145  {
147  std::make_shared<opengl::CEllipsoid>();
148 
149  obj->setPose(it.val.mean);
150  obj->setCovMatrix(it.val.cov, it.val.cov(2, 2) == 0 ? 2 : 3);
151 
152  obj->setQuantiles(3);
153  obj->enableDrawSolid3D(false);
154  obj->setColor(POINT_COLOR);
155 
156  outObj->insert(obj);
157  } // end for each gaussian node
158  }
159  else if (IS_CLASS(o, CPointPDFGaussian))
160  {
161  const auto* p = dynamic_cast<const CPointPDFGaussian*>(&o);
162  ASSERT_(p != nullptr);
163 
164  CEllipsoid::Ptr obj = std::make_shared<CEllipsoid>();
165  obj->setLocation(p->mean.x(), p->mean.y(), p->mean.z());
166  obj->setCovMatrix(p->cov, p->cov(2, 2) == 0 ? 2 : 3);
167  obj->setColor(POINT_COLOR);
168  obj->setQuantiles(3);
169  obj->enableDrawSolid3D(false);
170  outObj->insert(obj);
171  }
172  else if (IS_CLASS(o, CPointPDFParticles))
173  {
174  const auto* p = dynamic_cast<const CPointPDFParticles*>(&o);
175  ASSERT_(p != nullptr);
176 
179  const size_t N = p->size();
180 
181  obj->resize(N);
182  obj->setColor(POINT_COLOR);
183  for (size_t i = 0; i < N; i++)
184  obj->setPoint(
185  i, p->m_particles[i].d->x, p->m_particles[i].d->y,
186  p->m_particles[i].d->z);
187  outObj->insert(obj);
188  }
189 
190  return outObj;
191 }
192 
193 /** Returns a representation of a the PDF - this is just an auxiliary function,
194  * it's more natural to call
195  * mrpt::poses::CPose3DPDF::getAs3DObject */
197 {
198  CSetOfObjects::Ptr outObj = std::make_shared<CSetOfObjects>();
199 
200  if (IS_CLASS(o, CPose3DPDFSOG))
201  {
202  const auto* p = dynamic_cast<const CPose3DPDFSOG*>(&o);
203  ASSERT_(p != nullptr);
204 
205  // For each gaussian node
206  for (const auto& it : *p)
207  {
209  std::make_shared<opengl::CEllipsoid>();
210 
211  obj->setPose(it.val.mean);
212  obj->setCovMatrix(
213  CMatrixDouble(it.val.cov), it.val.cov(2, 2) == 0 ? 2 : 3);
214 
215  obj->setQuantiles(3);
216  obj->enableDrawSolid3D(false);
217  obj->setColor(POSE_COLOR);
218 
219  outObj->insert(obj);
220 
223  axes->setPose(it.val.mean);
224  axes->setScale(POSE_AXIS_SCALE);
225  outObj->insert(axes);
226  } // end for each gaussian node
227  }
228  else if (IS_CLASS(o, CPose3DPDFGaussian))
229  {
230  const auto* p = dynamic_cast<const CPose3DPDFGaussian*>(&o);
231  ASSERT_(p != nullptr);
232 
233  opengl::CEllipsoid::Ptr obj = std::make_shared<opengl::CEllipsoid>();
234 
235  obj->setPose(p->mean);
236  obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2, 2) == 0 ? 2 : 3);
237 
238  obj->setQuantiles(3);
239  obj->enableDrawSolid3D(false);
240  obj->setColor(POSE_COLOR);
241 
242  outObj->insert(obj);
243 
245  axes->setPose(p->mean);
246  axes->setScale(POSE_AXIS_SCALE);
247  outObj->insert(axes);
248  }
249  else if (IS_CLASS(o, CPose3DPDFParticles))
250  {
251  const auto* p = dynamic_cast<const CPose3DPDFParticles*>(&o);
252  ASSERT_(p != nullptr);
253 
254  for (size_t i = 0; i < p->size(); i++)
255  {
258  axes->setPose(p->m_particles[i].d);
259  outObj->insert(axes);
260  }
261  }
262 
263  return outObj;
264 }
265 
266 /** Returns a representation of a the PDF - this is just an auxiliary function,
267  * it's more natural to call
268  * mrpt::poses::CPose3DQuatPDF::getAs3DObject */
270 {
271  CSetOfObjects::Ptr outObj = std::make_shared<CSetOfObjects>();
272 
274  {
275  const auto* p = dynamic_cast<const CPose3DQuatPDFGaussian*>(&o);
276  ASSERT_(p != nullptr);
277 
278  opengl::CEllipsoid::Ptr obj = std::make_shared<opengl::CEllipsoid>();
279 
280  obj->setPose(CPose3D(p->mean));
281  obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2, 2) == 0 ? 2 : 3);
282 
283  obj->setQuantiles(3);
284  obj->enableDrawSolid3D(false);
285  obj->setColor(POSE_COLOR);
286 
287  outObj->insert(obj);
288 
290  axes->setPose(CPose3D(p->mean));
291  axes->setScale(POSE_AXIS_SCALE);
292  outObj->insert(axes);
293  }
294 
295  return outObj;
296 }
const double POSE_TAIL_WIDTH
Definition: pose_pdfs.cpp:33
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:34
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:33
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
#define POSE_COLOR
Definition: pose_pdfs.cpp:37
const double POSE_TAIL_LENGTH
Definition: pose_pdfs.cpp:32
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
static Ptr Create(Args &&... args)
Definition: CPointCloud.h:48
const double POSE_AXIS_SCALE
Definition: pose_pdfs.cpp:35
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
const double POSE_POINT_SIZE
Definition: pose_pdfs.cpp:34
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CSetOfObjects::Ptr posePDF2opengl(const POSE_PDF &o)
Returns a representation of a the PDF - this is just an auxiliary function, it&#39;s more natural to call...
Definition: pose_pdfs.h:21
#define POINT_COLOR
Definition: pose_pdfs.cpp:38
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
CSetOfObjects::Ptr CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
Returns three arrows representing a X,Y,Z 3D corner (just thick lines instead of complex arrows for f...
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:36
GLfloat GLfloat p
Definition: glext.h:6398
A probability distribution of a 2D/3D point, represented as a set of random samples (particles)...
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
A gaussian distribution for 3D points.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019