MRPT  2.0.5
Twist3D.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 "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/core/bits_math.h>
13 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/TPose3D.h>
15 #include <mrpt/math/TTwist3D.h>
17 
18 using namespace mrpt::math;
19 using mrpt::DEG2RAD;
20 using mrpt::RAD2DEG;
21 
22 static_assert(std::is_trivially_copyable_v<TTwist3D>);
23 
24 void TTwist3D::asString(std::string& s) const
25 {
26  s = mrpt::format(
27  "[%f %f %f %f %f %f]", vx, vy, vz, RAD2DEG(wx), RAD2DEG(wy),
28  RAD2DEG(wz));
29 }
30 void TTwist3D::fromString(const std::string& s)
31 {
32  CMatrixDouble m;
33  if (!m.fromMatlabStringFormat(s))
34  THROW_EXCEPTION("Malformed expression in ::fromString");
35  ASSERTMSG_(
36  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
37  for (int i = 0; i < 3; i++) (*this)[i] = m(0, i);
38  for (int i = 0; i < 3; i++) (*this)[3 + i] = DEG2RAD(m(0, 3 + i));
39 }
40 // Transform all 6 components for a change of reference frame from "A" to
41 // another frame "B" whose rotation with respect to "A" is given by `rot`. The
42 // translational part of the pose is ignored
43 void TTwist3D::rotate(const TPose3D& rot)
44 {
45  const TTwist3D t = *this;
47  rot.getRotationMatrix(R);
48  vx = R(0, 0) * t.vx + R(0, 1) * t.vy + R(0, 2) * t.vz;
49  vy = R(1, 0) * t.vx + R(1, 1) * t.vy + R(1, 2) * t.vz;
50  vz = R(2, 0) * t.vx + R(2, 1) * t.vy + R(2, 2) * t.vz;
51 
52  wx = R(0, 0) * t.wx + R(0, 1) * t.wy + R(0, 2) * t.wz;
53  wy = R(1, 0) * t.wx + R(1, 1) * t.wy + R(1, 2) * t.wz;
54  wz = R(2, 0) * t.wx + R(2, 1) * t.wy + R(2, 2) * t.wz;
55 }
56 bool TTwist3D::operator==(const TTwist3D& o) const
57 {
58  return vx == o.vx && vy == o.vy && vz == o.vz && wx == o.wx && wy == o.wy &&
59  wz == o.wz;
60 }
61 bool TTwist3D::operator!=(const TTwist3D& o) const { return !(*this == o); }
62 
65 {
66  for (size_t i = 0; i < o.size(); i++) in >> o[i];
67  return in;
68 }
71 {
72  for (size_t i = 0; i < o.size(); i++) out << o[i];
73  return out;
74 }
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const
Definition: TPose3D.cpp:116
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
Definition: TTwist3D.h:18
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
This base provides a set of functions for maths stuff.
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[vx vy vz wx wy wz]" ) ...
Definition: Twist3D.cpp:30
bool operator!=(const TTwist3D &o) const
Definition: Twist3D.cpp:61
constexpr double DEG2RAD(const double x)
Degrees to radians.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:67
double wx
Angular velocity (rad/s)
Definition: TTwist3D.h:28
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
double vx
Velocity components: X,Y (m/s)
Definition: TTwist3D.h:26
const float R
mrpt::vision::TStereoCalibResults out
void rotate(const mrpt::math::TPose3D &rot)
Transform all 6 components for a change of reference frame from "A" to another frame "B" whose rotati...
Definition: Twist3D.cpp:43
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:626
constexpr double RAD2DEG(const double x)
Radians to degrees.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
std::string asString() const
Definition: TTwist3D.h:135
bool operator==(const TTwist3D &o) const
Definition: Twist3D.cpp:56



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020