MRPT  2.0.5
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-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 
18 
26 #include <mrpt/poses/CPosePDFSOG.h>
27 
28 using namespace mrpt;
29 using namespace mrpt::math;
30 using namespace mrpt::opengl;
31 using namespace mrpt::poses;
32 
33 const double POSE_TAIL_LENGTH = 0.1;
34 const double POSE_TAIL_WIDTH = 3.0;
35 const float POSE_POINT_SIZE = 4.0f;
36 const float POSE_AXIS_SCALE = 0.1f;
37 
38 #define POSE_COLOR 0, 0, 1
39 #define POINT_COLOR 1, 0, 0
40 
41 /** Returns a representation of a the PDF - this is just an auxiliary function,
42  * it's more natural to call
43  * mrpt::poses::CPosePDF::getAs3DObject */
45 {
46  auto outObj = CSetOfObjects::Create();
47 
48  if (IS_CLASS(o, CPosePDFSOG))
49  {
50  const auto* p = dynamic_cast<const CPosePDFSOG*>(&o);
51  ASSERT_(p != nullptr);
52 
53  opengl::CSetOfLines::Ptr lins = std::make_shared<opengl::CSetOfLines>();
54  lins->setColor(0.0f, 0.0f, 1.0f, 0.6f);
55  lins->setLineWidth(POSE_TAIL_WIDTH);
56 
57  for (const auto& it : *p)
58  {
60 
61  ellip->setPose(CPose3D(it.mean.x(), it.mean.y(), 0));
62  ellip->setCovMatrix(it.cov.blockCopy<2, 2>());
63  ellip->setColor(POSE_COLOR, 0.6f);
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.6f);
83  lins->setLineWidth(POSE_TAIL_WIDTH);
84 
86 
87  ellip->setPose(CPose3D(p->mean.x(), p->mean.y(), 0));
88  ellip->setCovMatrix(p->cov.blockCopy<2, 2>());
89  ellip->setColor(POSE_COLOR, 0.6f);
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  auto pnts = opengl::CPointCloud::Create();
109  pnts->setColor(POSE_COLOR, 0.6f);
110  pnts->setPointSize(POSE_POINT_SIZE);
111 
112  auto lins = opengl::CSetOfLines::Create();
113  lins->setColor(POSE_COLOR, 0.6f);
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(d2f(po.x), d2f(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  auto 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  if (it.val.cov(2, 2) == 0)
148  {
149  auto ellip = mrpt::opengl::CEllipsoid2D::Create();
150  ellip->setCovMatrix(it.val.cov.blockCopy<2, 2>());
151  ellip->setQuantiles(3);
152  ellip->enableDrawSolid3D(false);
153  obj = ellip;
154  }
155  else
156  {
157  auto ellip = mrpt::opengl::CEllipsoid3D::Create();
158  ellip->setCovMatrix(it.val.cov);
159  ellip->setQuantiles(3);
160  ellip->enableDrawSolid3D(false);
161  obj = ellip;
162  }
163  obj->setPose(it.val.mean);
164  obj->setColor(POINT_COLOR);
165 
166  outObj->insert(obj);
167  } // end for each gaussian node
168  }
169  else if (IS_CLASS(o, CPointPDFGaussian))
170  {
171  const auto* p = dynamic_cast<const CPointPDFGaussian*>(&o);
172  ASSERT_(p != nullptr);
173 
175  if (p->cov(2, 2) == 0)
176  {
177  auto ellip = mrpt::opengl::CEllipsoid2D::Create();
178  ellip->setCovMatrix(p->cov.blockCopy<2, 2>());
179  ellip->setQuantiles(3);
180  ellip->enableDrawSolid3D(false);
181  obj = ellip;
182  }
183  else
184  {
185  auto ellip = mrpt::opengl::CEllipsoid3D::Create();
186  ellip->setCovMatrix(p->cov);
187  ellip->setQuantiles(3);
188  ellip->enableDrawSolid3D(false);
189  obj = ellip;
190  }
191  obj->setLocation(p->mean.x(), p->mean.y(), p->mean.z());
192  obj->setColor(POINT_COLOR);
193  outObj->insert(obj);
194  }
195  else if (IS_CLASS(o, CPointPDFParticles))
196  {
197  const auto* p = dynamic_cast<const CPointPDFParticles*>(&o);
198  ASSERT_(p != nullptr);
199 
202  const size_t N = p->size();
203 
204  obj->resize(N);
205  obj->setColor(POINT_COLOR);
206  for (size_t i = 0; i < N; i++)
207  obj->setPoint(
208  i, p->m_particles[i].d->x, p->m_particles[i].d->y,
209  p->m_particles[i].d->z);
210  outObj->insert(obj);
211  }
212 
213  return outObj;
214 }
215 
216 /** Returns a representation of a the PDF - this is just an auxiliary function,
217  * it's more natural to call
218  * mrpt::poses::CPose3DPDF::getAs3DObject */
220 {
221  CSetOfObjects::Ptr outObj = std::make_shared<CSetOfObjects>();
222 
223  if (IS_CLASS(o, CPose3DPDFSOG))
224  {
225  const auto* p = dynamic_cast<const CPose3DPDFSOG*>(&o);
226  ASSERT_(p != nullptr);
227 
228  // For each gaussian node
229  for (const auto& it : *p)
230  {
232  std::make_shared<opengl::CEllipsoid3D>();
233 
234  obj->setPose(it.val.mean);
235  obj->setCovMatrix(it.val.cov.blockCopy<3, 3>());
236 
237  obj->setQuantiles(3);
238  obj->enableDrawSolid3D(false);
239  obj->setColor(POSE_COLOR);
240 
241  outObj->insert(obj);
242 
245  axes->setPose(it.val.mean);
246  axes->setScale(POSE_AXIS_SCALE);
247  outObj->insert(axes);
248  } // end for each gaussian node
249  }
250  else if (IS_CLASS(o, CPose3DPDFGaussian))
251  {
252  const auto* p = dynamic_cast<const CPose3DPDFGaussian*>(&o);
253  ASSERT_(p != nullptr);
254 
255  auto obj = std::make_shared<opengl::CEllipsoid3D>();
256 
257  obj->setPose(p->mean);
258  obj->setCovMatrix(p->cov.blockCopy<3, 3>());
259 
260  obj->setQuantiles(3);
261  obj->enableDrawSolid3D(false);
262  obj->setColor(POSE_COLOR);
263 
264  outObj->insert(obj);
265 
267  axes->setPose(p->mean);
268  axes->setScale(POSE_AXIS_SCALE);
269  outObj->insert(axes);
270  }
271  else if (IS_CLASS(o, CPose3DPDFParticles))
272  {
273  const auto* p = dynamic_cast<const CPose3DPDFParticles*>(&o);
274  ASSERT_(p != nullptr);
275 
276  for (size_t i = 0; i < p->size(); i++)
277  {
280  axes->setPose(p->m_particles[i].d);
281  outObj->insert(axes);
282  }
283  }
284 
285  return outObj;
286 }
287 
288 /** Returns a representation of a the PDF - this is just an auxiliary function,
289  * it's more natural to call
290  * mrpt::poses::CPose3DQuatPDF::getAs3DObject */
292 {
293  CSetOfObjects::Ptr outObj = std::make_shared<CSetOfObjects>();
294 
296  {
297  const auto* p = dynamic_cast<const CPose3DQuatPDFGaussian*>(&o);
298  ASSERT_(p != nullptr);
299 
301 
302  obj->setPose(CPose3D(p->mean));
303  obj->setCovMatrix(p->cov.blockCopy<3, 3>());
304 
305  obj->setQuantiles(3);
306  obj->enableDrawSolid3D(false);
307  obj->setColor(POSE_COLOR);
308 
309  outObj->insert(obj);
310 
312  axes->setPose(CPose3D(p->mean));
313  axes->setScale(POSE_AXIS_SCALE);
314  outObj->insert(axes);
315  }
316 
317  return outObj;
318 }
const double POSE_TAIL_WIDTH
Definition: pose_pdfs.cpp:34
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:38
const double POSE_TAIL_LENGTH
Definition: pose_pdfs.cpp:33
static Ptr Create(Args &&... args)
Definition: CEllipsoid2D.h:43
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:49
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
const float POSE_POINT_SIZE
Definition: pose_pdfs.cpp:35
This base provides a set of functions for maths stuff.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#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:39
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
static Ptr Create(Args &&... args)
Definition: CEllipsoid3D.h:42
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:13
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
A probability distribution of a 2D/3D point, represented as a set of random samples (particles)...
const float POSE_AXIS_SCALE
Definition: pose_pdfs.cpp:36
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
static Ptr Create(Args &&... args)
Definition: CSetOfLines.h:35
A gaussian distribution for 3D points.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: b26ce1112 Mon Jul 6 09:39:08 2020 +0200 at lun jul 6 09:45:13 CEST 2020