Main MRPT website > C++ reference for MRPT 1.9.9
pose_pdfs.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "opengl-precomp.h" // Precompiled header
11 
12 #include <mrpt/opengl/CEllipsoid.h>
17 
18 #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  CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();
46 
47  if (IS_CLASS(&o, CPosePDFSOG))
48  {
49  const CPosePDFSOG* p = static_cast<const CPosePDFSOG*>(&o);
50 
52  mrpt::make_aligned_shared<opengl::CSetOfLines>();
53  lins->setColor(0, 0, 1, 0.6);
54  lins->setLineWidth(POSE_TAIL_WIDTH);
55 
56  for (CPosePDFSOG::const_iterator it = p->begin(); it != p->end(); ++it)
57  {
59  mrpt::make_aligned_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 CPosePDFGaussian* p = static_cast<const CPosePDFGaussian*>(&o);
79 
81  mrpt::make_aligned_shared<opengl::CSetOfLines>();
82  lins->setColor(POSE_COLOR, 0.6);
83  lins->setLineWidth(POSE_TAIL_WIDTH);
84 
86  mrpt::make_aligned_shared<opengl::CEllipsoid>();
87 
88  ellip->setPose(CPose3D(p->mean.x(), p->mean.y(), 0));
89  ellip->setCovMatrix(p->cov, 2 /* x y */);
90  ellip->setColor(POSE_COLOR, 0.6);
91 
92  ellip->setQuantiles(3);
93  ellip->enableDrawSolid3D(false);
94 
95  outObj->insert(ellip);
96 
97  lins->appendLine(
98  p->mean.x(), p->mean.y(), 0,
99  p->mean.x() + POSE_TAIL_LENGTH * cos(p->mean.phi()),
100  p->mean.y() + POSE_TAIL_LENGTH * sin(p->mean.phi()), 0);
101 
102  outObj->insert(lins);
103  }
104  else if (IS_CLASS(&o, CPosePDFParticles))
105  {
106  const CPosePDFParticles* p = static_cast<const CPosePDFParticles*>(&o);
107 
109  mrpt::make_aligned_shared<opengl::CPointCloud>();
110  pnts->setColor(POSE_COLOR, 0.6);
111  pnts->setPointSize(POSE_POINT_SIZE);
112 
114  mrpt::make_aligned_shared<opengl::CSetOfLines>();
115  lins->setColor(POSE_COLOR, 0.6);
116  lins->setLineWidth(POSE_TAIL_WIDTH);
117 
118  for (size_t i = 0; i < p->size(); ++i)
119  {
120  const auto po = p->m_particles[i].d;
121  pnts->insertPoint(po.x, po.y, 0);
122  lins->appendLine(
123  po.x, po.y, 0,
124  po.x + POSE_TAIL_LENGTH * cos(po.phi),
125  po.y + POSE_TAIL_LENGTH * sin(po.phi), 0);
126  }
127  outObj->insert(pnts);
128  outObj->insert(lins);
129  }
130 
131  return outObj;
132 }
133 
134 /** Returns a representation of a the PDF - this is just an auxiliary function,
135  * it's more natural to call
136  * mrpt::poses::CPointPDF::getAs3DObject */
138 {
139  CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();
140 
141  if (IS_CLASS(&o, CPointPDFSOG))
142  {
143  const CPointPDFSOG* p = static_cast<const CPointPDFSOG*>(&o);
144 
145  // For each gaussian node
147  it != p->end(); ++it)
148  {
150  mrpt::make_aligned_shared<opengl::CEllipsoid>();
151 
152  obj->setPose(it->val.mean);
153  obj->setCovMatrix(it->val.cov, it->val.cov(2, 2) == 0 ? 2 : 3);
154 
155  obj->setQuantiles(3);
156  obj->enableDrawSolid3D(false);
157  obj->setColor(POINT_COLOR);
158 
159  outObj->insert(obj);
160  } // end for each gaussian node
161  }
162  else if (IS_CLASS(&o, CPointPDFGaussian))
163  {
164  const CPointPDFGaussian* p = static_cast<const CPointPDFGaussian*>(&o);
165 
166  CEllipsoid::Ptr obj = mrpt::make_aligned_shared<CEllipsoid>();
167  obj->setLocation(p->mean.x(), p->mean.y(), p->mean.z());
168  obj->setCovMatrix(p->cov, p->cov(2, 2) == 0 ? 2 : 3);
169  obj->setColor(POINT_COLOR);
170  obj->setQuantiles(3);
171  obj->enableDrawSolid3D(false);
172  outObj->insert(obj);
173  }
174  else if (IS_CLASS(&o, CPointPDFParticles))
175  {
176  const CPointPDFParticles* p =
177  static_cast<const CPointPDFParticles*>(&o);
178 
180  mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
181  const size_t N = p->size();
182 
183  obj->resize(N);
184  obj->setColor(POINT_COLOR);
185  for (size_t i = 0; i < N; i++)
186  obj->setPoint(
187  i, p->m_particles[i].d->x, p->m_particles[i].d->y,
188  p->m_particles[i].d->z);
189  outObj->insert(obj);
190  }
191 
192  return outObj;
193 }
194 
195 /** Returns a representation of a the PDF - this is just an auxiliary function,
196  * it's more natural to call
197  * mrpt::poses::CPose3DPDF::getAs3DObject */
199 {
200  CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();
201 
202  if (IS_CLASS(&o, CPose3DPDFSOG))
203  {
204  const CPose3DPDFSOG* p = static_cast<const CPose3DPDFSOG*>(&o);
205 
206  // For each gaussian node
207  for (CPose3DPDFSOG::const_iterator it = p->begin(); it != p->end();
208  ++it)
209  {
211  mrpt::make_aligned_shared<opengl::CEllipsoid>();
212 
213  obj->setPose(it->val.mean);
214  obj->setCovMatrix(
215  CMatrixDouble(it->val.cov), it->val.cov(2, 2) == 0 ? 2 : 3);
216 
217  obj->setQuantiles(3);
218  obj->enableDrawSolid3D(false);
219  obj->setColor(POSE_COLOR);
220 
221  outObj->insert(obj);
222 
225  axes->setPose(it->val.mean);
226  axes->setScale(POSE_AXIS_SCALE);
227  outObj->insert(axes);
228  } // end for each gaussian node
229  }
230  else if (IS_CLASS(&o, CPose3DPDFGaussian))
231  {
232  const CPose3DPDFGaussian* p =
233  static_cast<const CPose3DPDFGaussian*>(&o);
234 
236  mrpt::make_aligned_shared<opengl::CEllipsoid>();
237 
238  obj->setPose(p->mean);
239  obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2, 2) == 0 ? 2 : 3);
240 
241  obj->setQuantiles(3);
242  obj->enableDrawSolid3D(false);
243  obj->setColor(POSE_COLOR);
244 
245  outObj->insert(obj);
246 
248  axes->setPose(p->mean);
249  axes->setScale(POSE_AXIS_SCALE);
250  outObj->insert(axes);
251  }
252  else if (IS_CLASS(&o, CPose3DPDFParticles))
253  {
254  const CPose3DPDFParticles* p =
255  static_cast<const CPose3DPDFParticles*>(&o);
256 
257  for (size_t i = 0; i < p->size(); i++)
258  {
261  axes->setPose(p->m_particles[i].d);
262  outObj->insert(axes);
263  }
264  }
265 
266  return outObj;
267 }
268 
269 /** Returns a representation of a the PDF - this is just an auxiliary function,
270  * it's more natural to call
271  * mrpt::poses::CPose3DQuatPDF::getAs3DObject */
273 {
274  CSetOfObjects::Ptr outObj = mrpt::make_aligned_shared<CSetOfObjects>();
275 
277  {
278  const CPose3DQuatPDFGaussian* p =
279  static_cast<const CPose3DQuatPDFGaussian*>(&o);
280 
282  mrpt::make_aligned_shared<opengl::CEllipsoid>();
283 
284  obj->setPose(CPose3D(p->mean));
285  obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2, 2) == 0 ? 2 : 3);
286 
287  obj->setQuantiles(3);
288  obj->enableDrawSolid3D(false);
289  obj->setColor(POSE_COLOR);
290 
291  outObj->insert(obj);
292 
294  axes->setPose(CPose3D(p->mean));
295  axes->setScale(POSE_AXIS_SCALE);
296  outObj->insert(axes);
297  }
298 
299  return outObj;
300 }
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:37
CListGaussianModes::const_iterator const_iterator
Definition: CPosePDFSOG.h:69
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:36
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
#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:4070
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
const double POSE_AXIS_SCALE
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 ...
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
const double POSE_POINT_SIZE
Definition: pose_pdfs.cpp:34
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:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:54
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:24
#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:88
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
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
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:39
GLfloat GLfloat p
Definition: glext.h:6305
A probability distribution of a 2D/3D point, represented as a set of random samples (particles)...
const Scalar * const_iterator
Definition: eigen_plugins.h:27
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:42
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 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019