Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::utils;
22 using namespace mrpt::math;
23 using namespace mrpt::opengl;
24 using namespace mrpt::poses;
25 
26 
27 const double POSE_TAIL_LENGTH = 0.1;
28 const double POSE_TAIL_WIDTH = 3.0;
29 const double POSE_POINT_SIZE = 4.0;
30 const double POSE_AXIS_SCALE = 0.1;
31 
32 #define POSE_COLOR 0,0,1
33 #define POINT_COLOR 1,0,0
34 
35 
36 /** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
37  * mrpt::poses::CPosePDF::getAs3DObject */
38 CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPosePDF &o)
39 {
40  CSetOfObjectsPtr outObj = CSetOfObjects::Create();
41 
42  if (IS_CLASS(&o,CPosePDFSOG))
43  {
44  const CPosePDFSOG *p = static_cast<const CPosePDFSOG*>(&o);
45 
46  opengl::CSetOfLinesPtr lins = opengl::CSetOfLines::Create();
47  lins->setColor(0,0,1,0.6);
48  lins->setLineWidth(POSE_TAIL_WIDTH);
49 
50  for (CPosePDFSOG::const_iterator it=p->begin();it!=p->end();++it)
51  {
52  opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
53 
54  ellip->setPose( CPose3D((it)->mean.x(), (it)->mean.y(), 0) );
55  ellip->setCovMatrix((it)->cov, 2 /* x y */ );
56  ellip->setColor(POSE_COLOR,0.6);
57  ellip->setQuantiles(3);
58  ellip->enableDrawSolid3D(false);
59 
60  outObj->insert(ellip);
61 
62  lins->appendLine(
63  (it)->mean.x(), (it)->mean.y(), 0,
64  (it)->mean.x() + POSE_TAIL_LENGTH * cos((it)->mean.phi()) , (it)->mean.y() + POSE_TAIL_LENGTH * sin((it)->mean.phi()) , 0
65  );
66  }
67  outObj->insert(lins);
68  } else
69  if (IS_CLASS(&o,CPosePDFGaussian))
70  {
71  const CPosePDFGaussian *p = static_cast<const CPosePDFGaussian*>(&o);
72 
73  opengl::CSetOfLinesPtr lins = opengl::CSetOfLines::Create();
74  lins->setColor(POSE_COLOR,0.6);
75  lins->setLineWidth(POSE_TAIL_WIDTH);
76 
77  opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
78 
79  ellip->setPose( CPose3D(p->mean.x(), p->mean.y(), 0) );
80  ellip->setCovMatrix(p->cov, 2 /* x y */ );
81  ellip->setColor(POSE_COLOR,0.6);
82 
83  ellip->setQuantiles(3);
84  ellip->enableDrawSolid3D(false);
85 
86  outObj->insert(ellip);
87 
88  lins->appendLine(
89  p->mean.x(), p->mean.y(), 0,
90  p->mean.x() + POSE_TAIL_LENGTH * cos(p->mean.phi()) , p->mean.y() + POSE_TAIL_LENGTH * sin(p->mean.phi()) , 0
91  );
92 
93  outObj->insert(lins);
94  } else
96  {
97  const CPosePDFParticles *p = static_cast<const CPosePDFParticles*>(&o);
98 
99  opengl::CPointCloudPtr pnts = opengl::CPointCloud::Create();
100  pnts->setColor(POSE_COLOR,0.6);
101  pnts->setPointSize(POSE_POINT_SIZE);
102 
103  opengl::CSetOfLinesPtr lins = opengl::CSetOfLines::Create();
104  lins->setColor(POSE_COLOR,0.6);
105  lins->setLineWidth(POSE_TAIL_WIDTH);
106 
107  for (size_t i=0;i<p->size();++i)
108  {
109  const mrpt::poses::CPose2D *po = p->m_particles[i].d.get();
110  pnts->insertPoint(po->x(), po->y(), 0);
111  lins->appendLine(
112  po->x(), po->y(), 0,
113  po->x() + POSE_TAIL_LENGTH * cos(po->phi()), po->y() + POSE_TAIL_LENGTH * sin(po->phi()), 0
114  );
115  }
116  outObj->insert(pnts);
117  outObj->insert(lins);
118  }
119 
120 
121  return outObj;
122 }
123 
124 /** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
125  * mrpt::poses::CPointPDF::getAs3DObject */
126 CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPointPDF &o)
127 {
128  CSetOfObjectsPtr outObj = CSetOfObjects::Create();
129 
130  if (IS_CLASS(&o,CPointPDFSOG))
131  {
132  const CPointPDFSOG *p = static_cast<const CPointPDFSOG*>(&o);
133 
134  // For each gaussian node
135  for (CPointPDFSOG::CListGaussianModes::const_iterator it = p->begin(); it!= p->end();++it)
136  {
137  opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
138 
139  obj->setPose( it->val.mean);
140  obj->setCovMatrix(it->val.cov, it->val.cov(2,2)==0 ? 2:3);
141 
142  obj->setQuantiles(3);
143  obj->enableDrawSolid3D(false);
144  obj->setColor(POINT_COLOR);
145 
146  outObj->insert( obj );
147  } // end for each gaussian node
148  }
149  else if (IS_CLASS(&o, CPointPDFGaussian))
150  {
151  const CPointPDFGaussian *p = static_cast<const CPointPDFGaussian*>(&o);
152 
153  CEllipsoidPtr obj = CEllipsoid::Create();
154  obj->setLocation(p->mean);
155  obj->setCovMatrix(p->cov, p->cov(2,2)==0 ? 2:3);
156  obj->setColor(POINT_COLOR);
157  obj->setQuantiles(3);
158  obj->enableDrawSolid3D(false);
159  outObj->insert( obj );
160  }
161  else if (IS_CLASS(&o, CPointPDFParticles))
162  {
163  const CPointPDFParticles *p = static_cast<const CPointPDFParticles*>(&o);
164 
165  mrpt::opengl::CPointCloudPtr obj = mrpt::opengl::CPointCloud::Create();
166  const size_t N=p->size();
167 
168  obj->resize(N);
169  obj->setColor(POINT_COLOR);
170  for (size_t i=0;i<N;i++)
171  obj->setPoint(
172  i,
173  p->m_particles[i].d->x,
174  p->m_particles[i].d->y,
175  p->m_particles[i].d->z );
176  outObj->insert( obj );
177  }
178 
179  return outObj;
180 }
181 
182 /** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
183  * mrpt::poses::CPose3DPDF::getAs3DObject */
184 CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPose3DPDF &o)
185 {
186  CSetOfObjectsPtr outObj = CSetOfObjects::Create();
187 
188  if (IS_CLASS(&o,CPose3DPDFSOG))
189  {
190  const CPose3DPDFSOG *p = static_cast<const CPose3DPDFSOG*>(&o);
191 
192  // For each gaussian node
193  for (CPose3DPDFSOG::const_iterator it = p->begin(); it!= p->end();++it)
194  {
195  opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
196 
197  obj->setPose( it->val.mean);
198  obj->setCovMatrix(CMatrixDouble(it->val.cov), it->val.cov(2,2)==0 ? 2:3);
199 
200  obj->setQuantiles(3);
201  obj->enableDrawSolid3D(false);
202  obj->setColor(POSE_COLOR);
203 
204  outObj->insert( obj );
205 
206  opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZ();
207  axes->setPose(it->val.mean);
208  axes->setScale(POSE_AXIS_SCALE);
209  outObj->insert(axes);
210  } // end for each gaussian node
211  }
212  else
214  {
215  const CPose3DPDFGaussian *p = static_cast<const CPose3DPDFGaussian*>(&o);
216 
217  opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
218 
219  obj->setPose( p->mean);
220  obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2,2)==0 ? 2:3);
221 
222  obj->setQuantiles(3);
223  obj->enableDrawSolid3D(false);
224  obj->setColor(POSE_COLOR);
225 
226  outObj->insert( obj );
227 
228  opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZ();
229  axes->setPose(p->mean);
230  axes->setScale(POSE_AXIS_SCALE);
231  outObj->insert(axes);
232  }
233  else
235  {
236  const CPose3DPDFParticles *p = static_cast<const CPose3DPDFParticles*>(&o);
237 
238  for (size_t i=0;i<p->size();i++)
239  {
240  opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZSimple(POSE_AXIS_SCALE);
241  axes->setPose(*p->m_particles[i].d);
242  outObj->insert(axes);
243  }
244 
245  }
246 
247  return outObj;
248 }
249 
250 /** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
251  * mrpt::poses::CPose3DQuatPDF::getAs3DObject */
253 {
254  CSetOfObjectsPtr outObj = CSetOfObjects::Create();
255 
257  {
258  const CPose3DQuatPDFGaussian *p = static_cast<const CPose3DQuatPDFGaussian*>(&o);
259 
260  opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
261 
262  obj->setPose( CPose3D(p->mean));
263  obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2,2)==0 ? 2:3);
264 
265  obj->setQuantiles(3);
266  obj->enableDrawSolid3D(false);
267  obj->setColor(POSE_COLOR);
268 
269  outObj->insert( obj );
270 
271  opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZ();
272  axes->setPose(CPose3D(p->mean));
273  axes->setScale(POSE_AXIS_SCALE);
274  outObj->insert(axes);
275  }
276 
277  return outObj;
278 }
279 
static CEllipsoidPtr Create()
CPose2D mean
The mean value.
CPose3D mean
The mean value.
const double POSE_TAIL_WIDTH
Definition: pose_pdfs.cpp:28
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:37
CPoint3D mean
The mean value.
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:35
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
#define POSE_COLOR
Definition: pose_pdfs.cpp:32
const double POSE_TAIL_LENGTH
Definition: pose_pdfs.cpp:27
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
static CSetOfLinesPtr Create()
const double POSE_AXIS_SCALE
Definition: pose_pdfs.cpp:30
CParticleList m_particles
The array of particles.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Declares a class that represents a Probability Density function (PDF) of a 2D 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:135
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
const double POSE_POINT_SIZE
Definition: pose_pdfs.cpp:29
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:57
CSetOfObjectsPtr OPENGL_IMPEXP 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...
GLhandleARB obj
Definition: glew.h:3276
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLfloat GLfloat p
Definition: glew.h:10113
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
#define POINT_COLOR
Definition: pose_pdfs.cpp:33
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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:72
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
static CPointCloudPtr Create()
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:38
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:40
CListGaussianModes::const_iterator const_iterator
Definition: CPosePDFSOG.h:72
CSetOfObjectsPtr posePDF2opengl(const POSE_PDF &o)
Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call...
Definition: pose_pdfs.h:23
A gaussian distribution for 3D points.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
size_t size() const
Returns the number of particles.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018