MRPT  1.9.9
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-2018, 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 "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 CPoses3DSequence::posesCount() { return m_poses.size(); }
24 {
25  out.WriteAs<uint32_t>(m_poses.size());
26  for (const auto& p : m_poses) out << p;
27 }
30 {
31  switch (version)
32  {
33  case 0:
34  {
35  m_poses.resize(in.ReadAs<uint32_t>());
36  for (auto& p : m_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 CPoses3DSequence::getPose(unsigned int ind, CPose3D& outPose)
49 {
50  if (ind >= m_poses.size()) THROW_EXCEPTION("getPose: Index out of range!!");
51 
52  outPose = CPose3D(m_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 CPoses3DSequence::changePose(unsigned int ind, CPose3D& inPose)
60 {
61  if (ind >= m_poses.size()) THROW_EXCEPTION("getPose: Index out of range!!");
62  m_poses[ind] = inPose.asTPose();
63 }
64 
65 /*---------------------------------------------------------------
66  Appends a new pose at the end of sequence. Remember that poses are relative,
67  incremental to the last one.
68  ---------------------------------------------------------------*/
70 {
71  m_poses.push_back(newPose.asTPose());
72 }
73 
74 /*---------------------------------------------------------------
75  Clears the sequence.
76  ---------------------------------------------------------------*/
77 void CPoses3DSequence::clear() { m_poses.clear(); }
78 /*---------------------------------------------------------------
79  ---------------------------------------------------------------*/
80 /** Returns the absolute pose of a robot after moving "n" poses, so for "n=0"
81  * the origin pose (0,0,0deg) is returned, for "n=1" the first pose is returned,
82  * and for "n=posesCount()", the pose
83  * of robot after moving ALL poses is returned, all of them relative to the
84  * starting pose.
85  */
87 {
88  CPose3D ret(0, 0, 0);
89  unsigned int i;
90 
91  if (n > m_poses.size())
92  THROW_EXCEPTION("absolutePoseOf: Index out of range!!");
93 
94  for (i = 0; i < n; i++) ret = ret + CPose3D(m_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 > m_poses.size())
119  THROW_EXCEPTION("computeTraveledDistanceAfter: Index out of range!!");
120 
121  for (i = 0; i < n; i++) dist += m_poses[i].norm();
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 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1043
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 ...
void clear()
Clears the sequence.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
GLenum GLsizei n
Definition: glext.h:5074
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:156
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
This base provides a set of functions for maths stuff.
CPose3D absolutePoseOf(unsigned int n)
Returns the absolute pose of a robot after moving "n" poses, so for "n=0" the origin pose (0...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
GLuint in
Definition: glext.h:7274
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:47
GLfloat GLfloat p
Definition: glext.h:6305
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020