MRPT  2.0.4
CPoses3DSequence.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 
12 #include <mrpt/math/TPose3D.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 
21 
22 size_t CPoses3DSequence::posesCount() { return m_poses.size(); }
23 uint8_t CPoses3DSequence::serializeGetVersion() const { return 0; }
25 {
26  out.WriteAs<uint32_t>(m_poses.size());
27  for (const auto& p : m_poses) out << p;
28 }
30  mrpt::serialization::CArchive& in, uint8_t version)
31 {
32  switch (version)
33  {
34  case 0:
35  {
36  m_poses.resize(in.ReadAs<uint32_t>());
37  for (auto& p : m_poses) in >> p;
38  }
39  break;
40  default:
42  };
43 }
44 
45 /*---------------------------------------------------------------
46 Reads the stored pose at index "ind", where the first one is 0, the last
47 "posesCount() - 1"
48  ---------------------------------------------------------------*/
49 void CPoses3DSequence::getPose(unsigned int ind, CPose3D& outPose)
50 {
51  if (ind >= m_poses.size()) THROW_EXCEPTION("getPose: Index out of range!!");
52 
53  outPose = CPose3D(m_poses[ind]);
54 }
55 
56 /*---------------------------------------------------------------
57 Changes the stored pose at index "ind", where the first one is 0, the last
58 "posesCount() - 1"
59  ---------------------------------------------------------------*/
60 void CPoses3DSequence::changePose(unsigned int ind, CPose3D& inPose)
61 {
62  if (ind >= m_poses.size()) THROW_EXCEPTION("getPose: Index out of range!!");
63  m_poses[ind] = inPose.asTPose();
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  m_poses.push_back(newPose.asTPose());
73 }
74 
75 /*---------------------------------------------------------------
76  Clears the sequence.
77  ---------------------------------------------------------------*/
78 void CPoses3DSequence::clear() { m_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  CPose3D ret(0, 0, 0);
90  unsigned int i;
91 
92  if (n > m_poses.size())
93  THROW_EXCEPTION("absolutePoseOf: Index out of range!!");
94 
95  for (i = 0; i < n; i++) ret = ret + CPose3D(m_poses[i]);
96 
97  return ret;
98 }
99 
100 /*---------------------------------------------------------------
101  A shortcut for "absolutePoseOf( posesCount() )".
102  ---------------------------------------------------------------*/
104 {
105  return absolutePoseOf(posesCount());
106 }
107 
109 {
110  double dist = 0;
111 
112  if (n > m_poses.size())
113  THROW_EXCEPTION("computeTraveledDistanceAfter: Index out of range!!");
114 
115  for (size_t i = 0; i < n; i++) dist += m_poses[i].norm();
116  return dist;
117 }
118 
120 {
121  return computeTraveledDistanceAfter(posesCount());
122 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
void clear()
Clears the sequence.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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 ...
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.
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
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:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
double computeTraveledDistanceAfterAll()
Returns the traveled distance after ALL movements.
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"...
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020