MRPT  1.9.9
CPoint3D.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/poses/CPoint2D.h>
13 #include <mrpt/poses/CPoint3D.h>
14 #include <mrpt/poses/CPose2D.h>
15 #include <mrpt/poses/CPose3D.h>
18 #include <iostream>
19 #include <limits>
20 
21 using namespace mrpt;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 
26 
27 /** Constructor from an CPoint2D object. */ // Here instead of in the .h to
28 // avoid headers include loops.
30 {
31  m_coords[0] = p.x();
32  m_coords[1] = p.y();
33  m_coords[2] = 0;
34 }
35 /** Constructor from an CPose2D object. */
37 {
38  m_coords[0] = p.x();
39  m_coords[1] = p.y();
40  m_coords[2] = 0;
41 }
42 
43 /** Constructor from an CPose3D object. */
45 {
46  m_coords[0] = p.x();
47  m_coords[1] = p.y();
48  m_coords[2] = p.z();
49 }
50 
51 uint8_t CPoint3D::serializeGetVersion() const { return 1; }
53 {
54  out << m_coords[0] << m_coords[1] << m_coords[2];
55 }
57 {
58  switch (version)
59  {
60  case 0:
61  {
62  float f;
63  in >> f;
64  m_coords[0] = f;
65  in >> f;
66  m_coords[1] = f;
67  in >> f;
68  m_coords[2] = f;
69  }
70  break;
71  case 1:
72  {
73  // The coordinates:
74  in >> m_coords[0] >> m_coords[1] >> m_coords[2];
75  }
76  break;
77  default:
79  };
80 }
81 /** Serialize CSerializable Object to CSchemeArchiveBase derived object*/
83 {
85  out["x"] = m_coords[0];
86  out["y"] = m_coords[1];
87  out["z"] = m_coords[2];
88 }
89 /** Serialize CSchemeArchiveBase derived object to CSerializable Object*/
91 {
92  uint8_t version;
94  switch (version)
95  {
96  case 1:
97  {
98  m_coords[0] = static_cast<double>(in["x"]);
99  m_coords[1] = static_cast<double>(in["y"]);
100  m_coords[2] = static_cast<double>(in["z"]);
101  }
102  break;
103  default:
105  }
106 }
107 /*---------------------------------------------------------------
108  point3D = point3D - pose3D
109  ---------------------------------------------------------------*/
111 {
112  // JLBC: 7-FEB-2008: Why computing the whole matrix multiplication?? ;-)
113  // 5.7us -> 4.1us -> 3.1us (with optimization of HM matrices by reference)
114  // JLBC: 10-APR-2009: Usage of fixed-size 4x4 matrix, should be even faster
115  // now.
118 
119  return CPoint3D(
120  B_INV(0, 0) * m_coords[0] + B_INV(0, 1) * m_coords[1] +
121  B_INV(0, 2) * m_coords[2] + B_INV(0, 3),
122  B_INV(1, 0) * m_coords[0] + B_INV(1, 1) * m_coords[1] +
123  B_INV(1, 2) * m_coords[2] + B_INV(1, 3),
124  B_INV(2, 0) * m_coords[0] + B_INV(2, 1) * m_coords[1] +
125  B_INV(2, 2) * m_coords[2] + B_INV(2, 3));
126 }
127 
128 /*---------------------------------------------------------------
129  point3D = point3D - point3D
130  ---------------------------------------------------------------*/
132 {
133  return CPoint3D(
134  m_coords[0] - b.m_coords[0], m_coords[1] - b.m_coords[1],
135  m_coords[2] - b.m_coords[2]);
136 }
137 
138 /*---------------------------------------------------------------
139  point3D = point3D + point3D
140  ---------------------------------------------------------------*/
142 {
143  return CPoint3D(
144  m_coords[0] + b.m_coords[0], m_coords[1] + b.m_coords[1],
145  m_coords[2] + b.m_coords[2]);
146 }
147 
148 /*---------------------------------------------------------------
149  pose3D = point3D + pose3D
150  ---------------------------------------------------------------*/
152 {
153  return CPose3D(
154  m_coords[0] + b.x(), m_coords[1] + b.y(), m_coords[2] + b.z(), b.yaw(),
155  b.pitch(), b.roll());
156 }
157 
159 {
160  for (int i = 0; i < 3; i++)
161  m_coords[i] = std::numeric_limits<double>::quiet_NaN();
162 }
163 
165 {
166  return mrpt::math::TPoint3D(x(), y(), z());
167 }
168 
169 std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPoint3D& p)
170 {
171  o << "(" << p[0] << "," << p[1] << "," << p[2] << ")";
172  return o;
173 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void setToNaN() override
Set all data fields to quiet NaN.
Definition: CPoint3D.cpp:158
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
mrpt::math::CVectorFixedDouble< 3 > m_coords
[x,y,z]
Definition: CPoint3D.h:38
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
Definition: CPoint2D.cpp:102
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Serialize CSchemeArchiveBase derived object to CSerializable Object.
Definition: CPoint3D.cpp:56
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
void serializeTo(mrpt::serialization::CArchive &out) const override
Serialize CSerializable Object to CSchemeArchiveBase derived object.
Definition: CPoint3D.cpp:52
Virtual base class for "schematic archives" (JSON, XML,...)
#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.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPoint3D.cpp:51
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:290
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 2D point.
Definition: CPoint2D.h:32
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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
mrpt::math::TPoint3D asTPoint() const
Definition: CPoint3D.cpp:164
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 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
CPoint3D operator-(const CPose3D &b) const
Returns this point as seen from "b", i.e.
Definition: CPoint3D.cpp:110
CPoint3D(const double x=0, const double y=0, const double z=0)
Constructor for initializing point coordinates.
Definition: CPoint3D.h:42
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
CPoint3D operator+(const CPoint3D &b) const
Returns this point plus point "b", i.e.
Definition: CPoint3D.cpp:141



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020