PoseTransform.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #pragma once
19 
20 #ifndef __OpenKarto__PoseTransform_h__
21 #define __OpenKarto__PoseTransform_h__
22 
23 #include <OpenKarto/Geometry.h>
24 
25 namespace karto
26 {
27 
29 
30 
35  {
36  public:
40  Transform();
41 
46  Transform(const Pose2& rPose);
47 
53  Transform(const Pose2& rPose1, const Pose2& rPose2);
54 
55  public:
61  inline Pose2 TransformPose(const Pose2& rSourcePose) const
62  {
63  Pose2 newPosition = m_Transform + m_Rotation * rSourcePose;
64  kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading());
65 
66  return Pose2(newPosition.GetPosition(), angle);
67  }
68 
74  inline Pose2 InverseTransformPose(const Pose2& rSourcePose) const
75  {
76  Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform);
77  kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading());
78 
79  // components of transform
80  return Pose2(newPosition.GetPosition(), angle);
81  }
82 
83  private:
89  void SetTransform(const Pose2& rPose1, const Pose2& rPose2);
90 
91  private:
92  // pose transformation
94 
97  }; // Transform
98 
100 
101 }
102 
103 #endif // __OpenKarto__PoseTransform_h__
Pose2 InverseTransformPose(const Pose2 &rSourcePose) const
Definition: PoseTransform.h:74
kt_double GetHeading() const
Definition: Geometry.h:2274
#define KARTO_EXPORT
Definition: Macros.h:78
Pose2 TransformPose(const Pose2 &rSourcePose) const
Definition: PoseTransform.h:61
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
const Vector2d & GetPosition() const
Definition: Geometry.h:2256
double kt_double
Definition: Types.h:160
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:213
Matrix3 m_InverseRotation
Definition: PoseTransform.h:96
Definition: Any.cpp:20


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24