Main MRPT website > C++ reference for MRPT 1.5.7
CActionCollection.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 "obs-precomp.h" // Precompiled headers
11 
15 #include <mrpt/poses/CPosePDF.h>
16 #include <mrpt/utils/CStream.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::obs;
20 using namespace mrpt::poses;
21 using namespace mrpt::utils;
22 
24 using namespace mrpt::utils::metaprogramming;
25 
27 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
33 {
34 }
35 
36 /*---------------------------------------------------------------
37  Constructor
38  ---------------------------------------------------------------*/
40 {
41  m_actions.push_back( CActionPtr( static_cast<CAction*>(a.duplicate()) ) );
42 }
43 
44 /*---------------------------------------------------------------
45  Implements the writing to a CStream capability of CSerializable objects
46  ---------------------------------------------------------------*/
48 {
49  if (version)
50  *version = 0;
51  else
52  {
53  uint32_t n;
54 
55  n = static_cast<uint32_t> ( m_actions.size() );
56  out << n;
57  for (const_iterator it=begin();it!=end();++it)
58  out << *(*it);
59  }
60 }
61 
62 /*---------------------------------------------------------------
63  Implements the reading from a CStream capability of CSerializable objects
64  ---------------------------------------------------------------*/
66 {
67  switch(version)
68  {
69  case 0:
70  {
71  uint32_t n;
72 
73  clear();
74 
75  in >> n;
76  m_actions.resize(n);
78 
79  } break;
80  default:
82  };
83 }
84 
85 /*---------------------------------------------------------------
86  clear
87  ---------------------------------------------------------------*/
89 {
90  m_actions.clear();
91 }
92 
93 /*---------------------------------------------------------------
94  get
95  ---------------------------------------------------------------*/
96 CActionPtr CActionCollection::get(size_t index)
97 {
98  if (index>=m_actions.size())
99  THROW_EXCEPTION("Index out of bounds");
100 
101  return m_actions.at(index).get_ptr();
102 }
103 
104 const CAction& CActionCollection::get(size_t index) const {
105  if (index>=m_actions.size())
106  THROW_EXCEPTION("Index out of bounds");
107 
108  return *(m_actions.at(index).get_ptr());
109 }
110 
111 
112 
113 /*---------------------------------------------------------------
114  size
115  ---------------------------------------------------------------*/
117 {
118  return m_actions.size();
119 }
120 
121 /*---------------------------------------------------------------
122  insert
123  ---------------------------------------------------------------*/
125 {
126  m_actions.push_back( CActionPtr( static_cast<CAction*>( action.duplicate() )) );
127 }
128 
129 
130 /*---------------------------------------------------------------
131  getBestMovementEstimation
132  ---------------------------------------------------------------*/
133 CActionRobotMovement2DPtr CActionCollection::getBestMovementEstimation() const
134 {
135  CActionRobotMovement2DPtr bestEst;
136  double bestDet = 1e3;
137 
138  // Find the best
139  for (const_iterator it=begin();it!=end();++it)
140  {
141  if ((*it)->GetRuntimeClass()->derivedFrom( CLASS_ID( CActionRobotMovement2D ) ) )
142  {
143  CActionRobotMovement2DPtr temp = CActionRobotMovement2DPtr( it->get_ptr() );
144 
145  if (temp->estimationMethod == CActionRobotMovement2D::emScan2DMatching )
146  {
147  return temp;
148  }
149 
150  double det = temp->poseChange->getCovariance().det();
151 
152  // If it is the best until now, save it:
153  if ( det<bestDet )
154  {
155  bestEst = temp;
156  bestDet = det;
157  }
158  }
159  }
160 
161  return bestEst;
162 }
163 
164 
165 /*---------------------------------------------------------------
166  eraseByIndex
167  ---------------------------------------------------------------*/
169 {
170  if (index>=m_actions.size())
171  THROW_EXCEPTION("Index out of bounds");
172 
173  iterator it = m_actions.begin() + index;
174  m_actions.erase( it );
175 }
176 
177 /*---------------------------------------------------------------
178  eraseByIndex
179  ---------------------------------------------------------------*/
181 {
182  // Find it:
183  for (iterator it=begin();it!=end();++it)
184  {
185  if ((*it)->GetRuntimeClass()->derivedFrom( CLASS_ID( CActionRobotMovement2D ) ) )
186  {
187  CActionRobotMovement2DPtr temp = CActionRobotMovement2DPtr( it->get_ptr() );
188 
189  // Is it of the required type?
190  if ( temp->estimationMethod == method )
191  {
192  // Yes!:
193  return temp;
194  }
195  }
196  }
197 
198  // Not found:
199  return CActionRobotMovement2DPtr();
200 }
201 
202 /*---------------------------------------------------------------
203  erase
204  ---------------------------------------------------------------*/
206 {
207  MRPT_START
208  ASSERT_(it!=end());
209 
210  return m_actions.erase(it);
211  MRPT_END
212 
213 }
214 
215 /*---------------------------------------------------------------
216  getFirstMovementEstimationMean
217  ---------------------------------------------------------------*/
219 {
220  CActionRobotMovement3DPtr act3D = getActionByClass<CActionRobotMovement3D>();
221  if (act3D)
222  {
223  out_pose_increment = act3D->poseChange.mean;
224  return true;
225  }
226  CActionRobotMovement2DPtr act2D = getActionByClass<CActionRobotMovement2D>();
227  if (act2D)
228  {
229  out_pose_increment = CPose3D(act2D->poseChange->getMeanVal());
230  return true;
231  }
232  return false;
233 }
234 
235 /*---------------------------------------------------------------
236  getFirstMovementEstimation
237  ---------------------------------------------------------------*/
239 {
240  CActionRobotMovement3DPtr act3D = getActionByClass<CActionRobotMovement3D>();
241  if (act3D)
242  {
243  out_pose_increment = act3D->poseChange;
244  return true;
245  }
246  CActionRobotMovement2DPtr act2D = getActionByClass<CActionRobotMovement2D>();
247  if (act2D)
248  {
249  out_pose_increment.copyFrom( *act2D->poseChange );
250  return true;
251  }
252  return false;
253 }
EIGEN_STRONG_INLINE Scalar det() const
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
bool getFirstMovementEstimationMean(mrpt::poses::CPose3D &out_pose_increment) const
Look for the first 2D or 3D "odometry" found in this collection of actions, and return the "mean" inc...
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
const_iterator end() const
Returns a iterator pointing to the end of the list: this is an example of usage:
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
std::deque< mrpt::utils::poly_ptr_ptr< CActionPtr > >::const_iterator const_iterator
You can use CActionCollection::begin to get a iterator to the first element.
Declares a class for storing a collection of robot actions.
std::deque< mrpt::utils::poly_ptr_ptr< CActionPtr > >::iterator iterator
You can use CActionCollection::begin to get a iterator to the first element.
Represents a probabilistic 2D movement of the robot mobile base.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#define MRPT_END
GLuint index
Definition: glext.h:3891
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void clear()
Erase all actions from the list.
This namespace contains representation of robot actions and observations.
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement2D object.
A set of utility objects for metaprogramming with STL algorithms.
int version
Definition: mrpt_jpeglib.h:898
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
iterator erase(const iterator &it)
Removes the given action in the list, and return an iterator to the next element (or this->end() if i...
Declares a class for storing a robot action.
Definition: obs/CAction.h:33
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:82
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual CObject * duplicate() const =0
Returns a copy of the object, indepently of its class.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
CActionPtr get(size_t index)
Access the i&#39;th action.DO NOT MODIFY the returned object, make a copy of ir with "CSerializable::dupl...
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
size_t size()
Returns the actions count in the collection.
std::deque< mrpt::utils::poly_ptr_ptr< CActionPtr > > m_actions
The robot "actionss".
void eraseByIndex(const size_t &index)
Remove an action from the list by its index.
CActionRobotMovement2DPtr getMovementEstimationByType(CActionRobotMovement2D::TEstimationMethod method)
Returns the pose increment estimator in the collection having the specified type. ...
unsigned __int32 uint32_t
Definition: rptypes.h:49
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
const_iterator begin() const
Returns a iterator to the first action: this is an example of usage:
bool getFirstMovementEstimation(mrpt::poses::CPose3DPDFGaussian &out_pose_increment) const
Look for the first 2D or 3D "odometry" found in this collection of actions, and return the "mean" inc...
void insert(CAction &action)
Add a new object to the list.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019