MRPT  1.9.9
CPoses2DSequence.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 "poses-precomp.h" // Precompiled headers
11 
14 
15 using namespace mrpt;
16 using namespace mrpt::poses;
17 using namespace mrpt::math;
18 
20 
21 size_t CPoses2DSequence::posesCount() { return poses.size(); }
22 uint8_t CPoses2DSequence::serializeGetVersion() const { return 0; }
24 {
25  out.WriteAs<uint32_t>(poses.size());
26  for (const auto& p : poses) out << p;
27 }
29  mrpt::serialization::CArchive& in, uint8_t version)
30 {
31  switch (version)
32  {
33  case 0:
34  {
35  poses.resize(in.ReadAs<uint32_t>());
36  for (auto& p : poses) in >> p;
37  }
38  break;
39  default:
41  };
42 }
43 
44 /*---------------------------------------------------------------
45 Reads the stored pose at index "ind", where the first one is 0, the last
46 "posesCount() - 1"
47  ---------------------------------------------------------------*/
48 void CPoses2DSequence::getPose(unsigned int ind, CPose2D& outPose)
49 {
50  if (ind >= poses.size()) THROW_EXCEPTION("Index out of range!!");
51 
52  outPose = poses[ind];
53 }
54 
55 /*---------------------------------------------------------------
56 Changes the stored pose at index "ind", where the first one is 0, the last
57 "posesCount() - 1"
58  ---------------------------------------------------------------*/
59 void CPoses2DSequence::changePose(unsigned int ind, CPose2D& inPose)
60 {
61  if (ind >= poses.size()) THROW_EXCEPTION("Index out of range!!");
62 
63  *((&(poses[ind])) + ind) = inPose;
64 }
65 
66 /*---------------------------------------------------------------
67  Appends a new pose at the end of sequence. Remember that poses are relative,
68  incremental to the last one.
69  ---------------------------------------------------------------*/
71 {
72  poses.push_back(newPose);
73 }
74 
75 /*---------------------------------------------------------------
76  Clears the sequence.
77  ---------------------------------------------------------------*/
78 void CPoses2DSequence::clear() { poses.clear(); }
79 /*---------------------------------------------------------------
80  ---------------------------------------------------------------*/
81 /** Returns the absolute pose of a robot after moving "n" poses, so for "n=0"
82  * the origin pose (0,0,0deg) is returned, for "n=1" the first pose is returned,
83  * and for "n=posesCount()", the pose
84  * of robot after moving ALL poses is returned, all of them relative to the
85  * starting pose.
86  */
88 {
89  CPose2D ret(0, 0, 0);
90  unsigned int i;
91 
92  if (n > poses.size()) THROW_EXCEPTION("Index out of range!!");
93 
94  for (i = 0; i < n; i++) ret = ret + poses[i];
95 
96  return ret;
97 }
98 
100 {
101  return absolutePoseOf(posesCount());
102 }
103 
105 {
106  double dist = 0;
107 
108  if (n > poses.size()) THROW_EXCEPTION("Index out of range!!");
109 
110  for (size_t i = 0; i < n; i++) dist += poses[i].norm();
111 
112  return dist;
113 }
114 
116 {
117  return computeTraveledDistanceAfter(posesCount());
118 }
This class stores a sequence of relative, incremental 2D poses.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double computeTraveledDistanceAfterAll()
Returns the traveled distance after ALL movements.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
CPose2D absolutePoseOf(unsigned int n)
Returns the absolute pose of a robot after moving "n" poses, so for "n=0" the origin pose (0...
void getPose(unsigned int ind, CPose2D &outPose)
Reads the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
This base provides a set of functions for maths stuff.
void clear()
Clears the sequence.
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
void changePose(unsigned int ind, CPose2D &inPose)
Changes the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double computeTraveledDistanceAfter(size_t n)
Returns the traveled distance after moving "n" poses, so for "n=0" it returns 0, for "n=1" the first ...
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:54
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
mrpt::vision::TStereoCalibResults out
void appendPose(CPose2D &newPose)
Appends a new pose at the end of sequence.
CONTAINER::Scalar norm(const CONTAINER &v)
CPose2D absolutePoseAfterAll()
A shortcut for "absolutePoseOf( posesCount() )".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020