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-2019, 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 
99 /*---------------------------------------------------------------
100  A shortcut for "absolutePoseOf( posesCount() )".
101  ---------------------------------------------------------------*/
103 {
104  return absolutePoseOf(posesCount());
105 }
106 
107 /*---------------------------------------------------------------
108  Returns the traveled distance after moving "n" poses, so for "n=0" it
109  returns 0, for "n=1" the first traveled distance, and for "n=posesCount()", the
110  total
111  distance after ALL movements.
112  ---------------------------------------------------------------*/
114 {
115  unsigned int i;
116  float dist = 0;
117 
118  if (n > poses.size()) THROW_EXCEPTION("Index out of range!!");
119 
120  for (i = 0; i < n; i++) dist += poses[i].norm();
121 
122  return dist;
123 }
124 
125 /*---------------------------------------------------------------
126  Returns the traveled distance after ALL movements.
127  A shortcut for "computeTraveledDistanceAfter( posesCount() )".
128  ---------------------------------------------------------------*/
130 {
131  return computeTraveledDistanceAfter(posesCount());
132 }
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
#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...
GLenum GLsizei n
Definition: glext.h:5136
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.
float computeTraveledDistanceAfterAll()
Returns the traveled distance after ALL movements.
void clear()
Clears the sequence.
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...
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
GLuint in
Definition: glext.h:7391
void appendPose(CPose2D &newPose)
Appends a new pose at the end of sequence.
GLfloat GLfloat p
Definition: glext.h:6398
CONTAINER::Scalar norm(const CONTAINER &v)
float computeTraveledDistanceAfter(unsigned int n)
Returns the traveled distance after moving "n" poses, so for "n=0" it returns 0, for "n=1" the first ...
CPose2D absolutePoseAfterAll()
A shortcut for "absolutePoseOf( posesCount() )".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019