MRPT  2.0.4
CActionCollection.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 "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.emplace_back(CAction::Ptr(dynamic_cast<CAction*>(a.clone())));
28 }
29 
30 uint8_t CActionCollection::serializeGetVersion() const { return 0; }
32 {
33  out.WriteAs<uint32_t>(m_actions.size());
34  for (const auto& a : *this) out << *a;
35 }
36 
38  mrpt::serialization::CArchive& in, uint8_t version)
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.emplace_back(
88  CAction::Ptr(dynamic_cast<CAction*>(action.clone())));
89 }
90 
91 /*---------------------------------------------------------------
92  getBestMovementEstimation
93  ---------------------------------------------------------------*/
95 {
97  double bestDet = 1e3;
98 
99  // Find the best
100  for (const auto& it : *this)
101  {
102  if (it->GetRuntimeClass()->derivedFrom(
104  {
106  std::dynamic_pointer_cast<CActionRobotMovement2D>(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  auto it = m_actions.begin() + index;
136  m_actions.erase(it);
137 }
138 
139 /*---------------------------------------------------------------
140  eraseByIndex
141  ---------------------------------------------------------------*/
144 {
145  // Find it:
146  for (auto& it : *this)
147  {
148  if (it->GetRuntimeClass()->derivedFrom(
150  {
152  std::dynamic_pointer_cast<CActionRobotMovement2D>(it.get_ptr());
153 
154  // Is it of the required type?
155  if (temp->estimationMethod == method)
156  {
157  // Yes!:
158  return temp;
159  }
160  }
161  }
162 
163  // Not found:
165 }
166 
167 /*---------------------------------------------------------------
168  erase
169  ---------------------------------------------------------------*/
171 {
172  MRPT_START
173  ASSERT_(it != end());
174 
175  return m_actions.erase(it);
176  MRPT_END
177 }
178 
179 /*---------------------------------------------------------------
180  getFirstMovementEstimationMean
181  ---------------------------------------------------------------*/
183  CPose3D& out_pose_increment) const
184 {
186  getActionByClass<CActionRobotMovement3D>();
187  if (act3D)
188  {
189  out_pose_increment = act3D->poseChange.mean;
190  return true;
191  }
193  getActionByClass<CActionRobotMovement2D>();
194  if (act2D)
195  {
196  out_pose_increment = CPose3D(act2D->poseChange->getMeanVal());
197  return true;
198  }
199  return false;
200 }
201 
202 /*---------------------------------------------------------------
203  getFirstMovementEstimation
204  ---------------------------------------------------------------*/
206  CPose3DPDFGaussian& out_pose_increment) const
207 {
209  getActionByClass<CActionRobotMovement3D>();
210  if (act3D)
211  {
212  out_pose_increment = act3D->poseChange;
213  return true;
214  }
216  getActionByClass<CActionRobotMovement2D>();
217  if (act2D)
218  {
219  out_pose_increment.copyFrom(*act2D->poseChange);
220  return true;
221  }
222  return false;
223 }
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
#define MRPT_START
Definition: exceptions.h:241
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:67
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)
To be added to all CSerializable-classes implementation files.
CActionRobotMovement2D::Ptr getMovementEstimationByType(CActionRobotMovement2D::TEstimationMethod method)
Returns the pose increment estimator in the collection having the specified type. ...
void eraseByIndex(size_t index)
Remove an action from the list by its index.
Declares a class for storing a collection of robot actions.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:120
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:102
std::deque< mrpt::containers::deepcopy_poly_ptr< CAction::Ptr > >::iterator iterator
You can use CActionCollection::begin to get a iterator to the first element.
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...
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
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.
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:24
const_iterator end() const
Definition: ts_hash_map.h:246
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const_iterator begin() const
Definition: ts_hash_map.h:240
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
size_t size()
Returns the actions count in the collection.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020