Main MRPT website > C++ reference for MRPT 1.5.7
CPoses2DSequence.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 "base-precomp.h" // Precompiled headers
11 
13 #include <mrpt/utils/CStream.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::poses;
17 using namespace mrpt::math;
18 using namespace mrpt::utils;
19 
21 
22 /*---------------------------------------------------------------
23  Default constructor
24  ---------------------------------------------------------------*/
26 {
27 
28 }
29 
30 /*---------------------------------------------------------------
31  Returns the poses count in the sequence:
32  ---------------------------------------------------------------*/
34 {
35  return poses.size();
36 }
37 
38 
39 /*---------------------------------------------------------------
40  Implements the writing to a CStream capability of
41  CSerializable objects
42  ---------------------------------------------------------------*/
44 {
45  if (version)
46  *version = 0;
47  else
48  {
49  uint32_t i,n;
50 
51  // The poses:
52  n = poses.size();
53  out << n;
54  for (i=0;i<n;i++) out << poses[i];
55  }
56 }
57 
58 /*---------------------------------------------------------------
59  Implements the reading from a CStream capability of
60  CSerializable objects
61  ---------------------------------------------------------------*/
63 {
64  switch(version)
65  {
66  case 0:
67  {
68  uint32_t i,n;
69 
70  // The poses:
71  in >> n;
72  poses.resize(n);
73  for (i=0;i<n;i++) in >> poses[i];
74  } break;
75  default:
77 
78  };
79 }
80 
81 
82 /*---------------------------------------------------------------
83 Reads the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"
84  ---------------------------------------------------------------*/
85 void CPoses2DSequence::getPose(unsigned int ind, CPose2D &outPose)
86 {
87  if (ind>=poses.size()) THROW_EXCEPTION("Index out of range!!");
88 
89  outPose = poses[ind];
90 }
91 
92 /*---------------------------------------------------------------
93 Changes the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"
94  ---------------------------------------------------------------*/
95 void CPoses2DSequence::changePose(unsigned int ind, CPose2D &inPose)
96 {
97  if (ind>=poses.size()) THROW_EXCEPTION("Index out of range!!");
98 
99  *((&(poses[ind])) + ind) = inPose;
100 }
101 
102 /*---------------------------------------------------------------
103  Appends a new pose at the end of sequence. Remember that poses are relative, incremental to the last one.
104  ---------------------------------------------------------------*/
106 {
107  poses.push_back( newPose );
108 }
109 
110 /*---------------------------------------------------------------
111  Clears the sequence.
112  ---------------------------------------------------------------*/
114 {
115  poses.clear();
116 }
117 
118 /*---------------------------------------------------------------
119  ---------------------------------------------------------------*/
120 /** Returns the absolute pose of a robot after moving "n" poses, so for "n=0" the origin pose (0,0,0deg) is returned, for "n=1" the first pose is returned, and for "n=posesCount()", the pose
121  * of robot after moving ALL poses is returned, all of them relative to the starting pose.
122  */
124 {
125  CPose2D ret(0,0,0);
126  unsigned int i;
127 
128  if (n>poses.size()) THROW_EXCEPTION("Index out of range!!");
129 
130  for (i=0;i<n;i++) ret = ret + poses[i];
131 
132  return ret;
133 }
134 
135 
136 /*---------------------------------------------------------------
137  A shortcut for "absolutePoseOf( posesCount() )".
138  ---------------------------------------------------------------*/
140 {
141  return absolutePoseOf( posesCount() );
142 }
143 
144 /*---------------------------------------------------------------
145  Returns the traveled distance after moving "n" poses, so for "n=0" it returns 0, for "n=1" the first traveled distance, and for "n=posesCount()", the total
146  distance after ALL movements.
147  ---------------------------------------------------------------*/
149 {
150  unsigned int i;
151  float dist = 0;
152 
153  if (n>poses.size()) THROW_EXCEPTION("Index out of range!!");
154 
155  for (i=0;i<n;i++) dist+= poses[i].norm();
156 
157  return dist;
158 }
159 
160 /*---------------------------------------------------------------
161  Returns the traveled distance after ALL movements.
162  A shortcut for "computeTraveledDistanceAfter( posesCount() )".
163  ---------------------------------------------------------------*/
165 {
166  return computeTraveledDistanceAfter( posesCount() );
167 }
This class stores a sequence of relative, incremental 2D poses.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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.
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 readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
void getPose(unsigned int ind, CPose2D &outPose)
Reads the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"...
size_t posesCount()
Returns the poses count in the sequence:
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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"...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
GLuint in
Definition: glext.h:6301
unsigned __int32 uint32_t
Definition: rptypes.h:49
void appendPose(CPose2D &newPose)
Appends a new pose at the end of sequence.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019