MRPT  1.9.9
CPoint3D.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 
12 #include <mrpt/poses/CPoint3D.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPoint2D.h>
15 #include <mrpt/poses/CPose2D.h>
17 #include <limits>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 
24 
25 /** Constructor from an CPoint2D object. */ // Here instead of in the .h to
26 // avoid headers include loops.
28 {
29  m_coords[0] = p.x();
30  m_coords[1] = p.y();
31  m_coords[2] = 0;
32 }
33 /** Constructor from an CPose2D object. */
35 {
36  m_coords[0] = p.x();
37  m_coords[1] = p.y();
38  m_coords[2] = 0;
39 }
40 
41 /** Constructor from an CPose3D object. */
43 {
44  m_coords[0] = p.x();
45  m_coords[1] = p.y();
46  m_coords[2] = p.z();
47 }
48 
51 {
52  out << m_coords[0] << m_coords[1] << m_coords[2];
53 }
55 {
56  switch (version)
57  {
58  case 0:
59  {
60  float f;
61  in >> f;
62  m_coords[0] = f;
63  in >> f;
64  m_coords[1] = f;
65  in >> f;
66  m_coords[2] = f;
67  }
68  break;
69  case 1:
70  {
71  // The coordinates:
72  in >> m_coords[0] >> m_coords[1] >> m_coords[2];
73  }
74  break;
75  default:
77  };
78 }
79 
80 /*---------------------------------------------------------------
81  point3D = point3D - pose3D
82  ---------------------------------------------------------------*/
84 {
85  // JLBC: 7-FEB-2008: Why computing the whole matrix multiplication?? ;-)
86  // 5.7us -> 4.1us -> 3.1us (with optimization of HM matrices by reference)
87  // JLBC: 10-APR-2009: Usage of fixed-size 4x4 matrix, should be even faster
88  // now.
90  b.getInverseHomogeneousMatrix(B_INV);
91 
92  return CPoint3D(
93  B_INV.get_unsafe(0, 0) * m_coords[0] +
94  B_INV.get_unsafe(0, 1) * m_coords[1] +
95  B_INV.get_unsafe(0, 2) * m_coords[2] + B_INV.get_unsafe(0, 3),
96  B_INV.get_unsafe(1, 0) * m_coords[0] +
97  B_INV.get_unsafe(1, 1) * m_coords[1] +
98  B_INV.get_unsafe(1, 2) * m_coords[2] + B_INV.get_unsafe(1, 3),
99  B_INV.get_unsafe(2, 0) * m_coords[0] +
100  B_INV.get_unsafe(2, 1) * m_coords[1] +
101  B_INV.get_unsafe(2, 2) * m_coords[2] + B_INV.get_unsafe(2, 3));
102 }
103 
104 /*---------------------------------------------------------------
105  point3D = point3D - point3D
106  ---------------------------------------------------------------*/
108 {
109  return CPoint3D(
110  m_coords[0] - b.m_coords[0], m_coords[1] - b.m_coords[1],
111  m_coords[2] - b.m_coords[2]);
112 }
113 
114 /*---------------------------------------------------------------
115  point3D = point3D + point3D
116  ---------------------------------------------------------------*/
118 {
119  return CPoint3D(
120  m_coords[0] + b.m_coords[0], m_coords[1] + b.m_coords[1],
121  m_coords[2] + b.m_coords[2]);
122 }
123 
124 /*---------------------------------------------------------------
125  pose3D = point3D + pose3D
126  ---------------------------------------------------------------*/
128 {
129  return CPose3D(
130  m_coords[0] + b.x(), m_coords[1] + b.y(), m_coords[2] + b.z(), b.yaw(),
131  b.pitch(), b.roll());
132 }
133 
135 {
136  for (int i = 0; i < 3; i++)
137  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
138 }
139 
141 {
142  return mrpt::math::TPoint3D(x(), y(), z());
143 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
GLdouble GLdouble z
Definition: glext.h:3872
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPoint3D.cpp:134
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPoint3D.cpp:54
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPoint3D.cpp:50
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPoint3D.cpp:49
GLubyte GLubyte b
Definition: glext.h:6279
A class used to store a 2D point.
Definition: CPoint2D.h:33
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::math::TPoint3D asTPoint() const
Definition: CPoint3D.cpp:140
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 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
CPoint3D operator-(const CPose3D &b) const
Returns this point as seen from "b", i.e.
Definition: CPoint3D.cpp:83
CPoint3D(const double x=0, const double y=0, const double z=0)
Constructor for initializing point coordinates.
Definition: CPoint3D.h:42
GLuint in
Definition: glext.h:7274
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
CPoint3D operator+(const CPoint3D &b) const
Returns this point plus point "b", i.e.
Definition: CPoint3D.cpp:117
GLfloat GLfloat p
Definition: glext.h:6305



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