Main MRPT website > C++ reference for MRPT 1.5.7
CPoses3DSequence.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  m_poses()
27 {
28 
29 }
30 
31 /*---------------------------------------------------------------
32  Returns the poses count in the sequence:
33  ---------------------------------------------------------------*/
35 {
36  return m_poses.size();
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 = m_poses.size();
53  out << n;
54  for (i=0;i<n;i++) out << m_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  m_poses.resize(n);
73  for (i=0;i<n;i++) in >> m_poses[i];
74  } break;
75  default:
77 
78  };
79 }
80 
81 /*---------------------------------------------------------------
82 Reads the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"
83  ---------------------------------------------------------------*/
84 void CPoses3DSequence::getPose(unsigned int ind, CPose3D &outPose)
85 {
86  if (ind>=m_poses.size()) THROW_EXCEPTION("getPose: Index out of range!!");
87 
88  outPose = CPose3D(m_poses[ind]);
89 }
90 
91 /*---------------------------------------------------------------
92 Changes the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"
93  ---------------------------------------------------------------*/
94 void CPoses3DSequence::changePose(unsigned int ind, CPose3D &inPose)
95 {
96  if (ind>=m_poses.size()) THROW_EXCEPTION("getPose: Index out of range!!");
97  m_poses[ind] = inPose;
98 }
99 
100 /*---------------------------------------------------------------
101  Appends a new pose at the end of sequence. Remember that poses are relative, incremental to the last one.
102  ---------------------------------------------------------------*/
104 {
105  m_poses.push_back( newPose );
106 }
107 
108 /*---------------------------------------------------------------
109  Clears the sequence.
110  ---------------------------------------------------------------*/
112 {
113  m_poses.clear();
114 }
115 
116 /*---------------------------------------------------------------
117  ---------------------------------------------------------------*/
118 /** 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
119  * of robot after moving ALL poses is returned, all of them relative to the starting pose.
120  */
122 {
123  CPose3D ret(0,0,0);
124  unsigned int i;
125 
126  if (n>m_poses.size()) THROW_EXCEPTION("absolutePoseOf: Index out of range!!");
127 
128  for (i=0;i<n;i++) ret = ret + CPose3D(m_poses[i]);
129 
130  return ret;
131 }
132 
133 
134 /*---------------------------------------------------------------
135  A shortcut for "absolutePoseOf( posesCount() )".
136  ---------------------------------------------------------------*/
138 {
139  return absolutePoseOf( posesCount() );
140 }
141 
142 /*---------------------------------------------------------------
143  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
144  distance after ALL movements.
145  ---------------------------------------------------------------*/
147 {
148  unsigned int i;
149  float dist = 0;
150 
151  if (n>m_poses.size()) THROW_EXCEPTION("computeTraveledDistanceAfter: Index out of range!!");
152 
153  for (i=0;i<n;i++) dist+= m_poses[i].norm();
154  return dist;
155 }
156 
157 /*---------------------------------------------------------------
158  Returns the traveled distance after ALL movements.
159  A shortcut for "computeTraveledDistanceAfter( posesCount() )".
160  ---------------------------------------------------------------*/
162 {
163  return computeTraveledDistanceAfter( posesCount() );
164 }
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 ...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
void clear()
Clears the sequence.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
float computeTraveledDistanceAfterAll()
Returns the traveled distance after ALL movements.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
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
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CPose3D absolutePoseOf(unsigned int n)
Returns the absolute pose of a robot after moving "n" poses, so for "n=0" the origin pose (0...
int version
Definition: mrpt_jpeglib.h:898
size_t posesCount()
Returns the poses count in the sequence:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
void appendPose(CPose3D &newPose)
Appends a new pose at the end of sequence.
This class stores a sequence of relative, incremental 3D poses.
void getPose(unsigned int ind, CPose3D &outPose)
Reads the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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...
GLuint in
Definition: glext.h:6301
CPose3D absolutePoseAfterAll()
A shortcut for "absolutePoseOf( posesCount() )".
void changePose(unsigned int ind, CPose3D &inPose)
Changes the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"...
unsigned __int32 uint32_t
Definition: rptypes.h:49
CONTAINER::Scalar norm(const CONTAINER &v)
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.



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