MRPT  1.9.9
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-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 "obs-precomp.h" // Precompiled headers
11 
15 #include <mrpt/poses/CPosePDF.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::obs;
21 using namespace mrpt::poses;
22 
24 
26 {
27  m_actions.push_back(CAction::Ptr(static_cast<CAction*>(a.clone())));
28 }
29 
32 {
33  out.WriteAs<uint32_t>(m_actions.size());
34  for (const auto& a : *this) out << *a;
35 }
36 
39 {
40  switch (version)
41  {
42  case 0:
43  {
44  clear();
45  m_actions.resize(in.ReadAs<uint32_t>());
46  for_each(
47  begin(), end(),
49  ObjectReadFromStreamToPtrs<CAction::Ptr>(&in));
50  }
51  break;
52  default:
54  };
55 }
56 
57 /*---------------------------------------------------------------
58  clear
59  ---------------------------------------------------------------*/
60 void CActionCollection::clear() { m_actions.clear(); }
61 /*---------------------------------------------------------------
62  get
63  ---------------------------------------------------------------*/
65 {
66  if (index >= m_actions.size()) THROW_EXCEPTION("Index out of bounds");
67 
68  return m_actions.at(index).get_ptr();
69 }
70 
71 const CAction& CActionCollection::get(size_t index) const
72 {
73  if (index >= m_actions.size()) THROW_EXCEPTION("Index out of bounds");
74 
75  return *(m_actions.at(index).get_ptr());
76 }
77 
78 /*---------------------------------------------------------------
79  size
80  ---------------------------------------------------------------*/
81 size_t CActionCollection::size() { return m_actions.size(); }
82 /*---------------------------------------------------------------
83  insert
84  ---------------------------------------------------------------*/
86 {
87  m_actions.push_back(CAction::Ptr(static_cast<CAction*>(action.clone())));
88 }
89 
90 /*---------------------------------------------------------------
91  getBestMovementEstimation
92  ---------------------------------------------------------------*/
94 {
96  double bestDet = 1e3;
97 
98  // Find the best
99  for (const_iterator it = begin(); it != end(); ++it)
100  {
101  if ((*it)->GetRuntimeClass()->derivedFrom(
103  {
105  std::dynamic_pointer_cast<CActionRobotMovement2D>(
106  it->get_ptr());
107 
108  if (temp->estimationMethod ==
110  {
111  return temp;
112  }
113 
114  double det = temp->poseChange->getCovariance().det();
115 
116  // If it is the best until now, save it:
117  if (det < bestDet)
118  {
119  bestEst = temp;
120  bestDet = det;
121  }
122  }
123  }
124 
125  return bestEst;
126 }
127 
128 /*---------------------------------------------------------------
129  eraseByIndex
130  ---------------------------------------------------------------*/
132 {
133  if (index >= m_actions.size()) THROW_EXCEPTION("Index out of bounds");
134 
135  iterator it = m_actions.begin() + index;
136  m_actions.erase(it);
137 }
138 
139 /*---------------------------------------------------------------
140  eraseByIndex
141  ---------------------------------------------------------------*/
144 {
145  // Find it:
146  for (iterator it = begin(); it != end(); ++it)
147  {
148  if ((*it)->GetRuntimeClass()->derivedFrom(
150  {
152  std::dynamic_pointer_cast<CActionRobotMovement2D>(
153  it->get_ptr());
154 
155  // Is it of the required type?
156  if (temp->estimationMethod == method)
157  {
158  // Yes!:
159  return temp;
160  }
161  }
162  }
163 
164  // Not found:
166 }
167 
168 /*---------------------------------------------------------------
169  erase
170  ---------------------------------------------------------------*/
172 {
173  MRPT_START
174  ASSERT_(it != end());
175 
176  return m_actions.erase(it);
177  MRPT_END
178 }
179 
180 /*---------------------------------------------------------------
181  getFirstMovementEstimationMean
182  ---------------------------------------------------------------*/
184  CPose3D& out_pose_increment) const
185 {
187  getActionByClass<CActionRobotMovement3D>();
188  if (act3D)
189  {
190  out_pose_increment = act3D->poseChange.mean;
191  return true;
192  }
194  getActionByClass<CActionRobotMovement2D>();
195  if (act2D)
196  {
197  out_pose_increment = CPose3D(act2D->poseChange->getMeanVal());
198  return true;
199  }
200  return false;
201 }
202 
203 /*---------------------------------------------------------------
204  getFirstMovementEstimation
205  ---------------------------------------------------------------*/
207  CPose3DPDFGaussian& out_pose_increment) const
208 {
210  getActionByClass<CActionRobotMovement3D>();
211  if (act3D)
212  {
213  out_pose_increment = act3D->poseChange;
214  return true;
215  }
217  getActionByClass<CActionRobotMovement2D>();
218  if (act2D)
219  {
220  out_pose_increment.copyFrom(*act2D->poseChange);
221  return true;
222  }
223  return false;
224 }
EIGEN_STRONG_INLINE Scalar det() const
#define MRPT_START
Definition: exceptions.h:262
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...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
CActionRobotMovement2D::Ptr getMovementEstimationByType(CActionRobotMovement2D::TEstimationMethod method)
Returns the pose increment estimator in the collection having the specified type. ...
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:156
Declares a class for storing a collection of robot actions.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
Represents a probabilistic 2D movement of the robot mobile base.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:84
std::deque< mrpt::containers::deepcopy_poly_ptr< CAction::Ptr > >::iterator iterator
You can use CActionCollection::begin to get a iterator to the first element.
std::shared_ptr< CActionRobotMovement2D > Ptr
virtual CObject * clone() const =0
Returns a deep copy (clone) of the object, indepently of its class.
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
CAction::Ptr get(size_t index)
Access the i&#39;th action.DO NOT MODIFY the returned object, make a copy of ir with "CSerializable::dupl...
GLuint index
Definition: glext.h:4054
void clear()
Erase all actions from the list.
GLuint GLuint end
Definition: glext.h:3528
This namespace contains representation of robot actions and observations.
TEstimationMethod
A list of posible ways for estimating the content of a CActionRobotMovement2D object.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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: CAction.h:27
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
size_t size()
Returns the actions count in the collection.
void eraseByIndex(const size_t &index)
Remove an action from the list by its index.
std::deque< mrpt::containers::deepcopy_poly_ptr< CAction::Ptr > >::const_iterator const_iterator
You can use CActionCollection::begin to get a iterator to the first element.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:186
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.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020