Pose3Upright.cpp
Go to the documentation of this file.
1 
8 #include <iostream>
9 
11 
12 using namespace std;
13 
14 namespace gtsam {
15 
16 /* ************************************************************************* */
17 Pose3Upright::Pose3Upright(const Rot2& bearing, const Point3& t)
18 : T_(bearing, Point2(t.x(), t.y())), z_(t.z())
19 {
20 }
21 
22 /* ************************************************************************* */
23 Pose3Upright::Pose3Upright(double x, double y, double z, double theta)
24 : T_(x, y, theta), z_(z)
25 {
26 }
27 
28 /* ************************************************************************* */
30 : T_(pose), z_(z)
31 {
32 }
33 
34 /* ************************************************************************* */
36 : T_(x.x(), x.y(), x.rotation().yaw()), z_(x.z())
37 {
38 }
39 
40 /* ************************************************************************* */
41 void Pose3Upright::print(const std::string& s) const {
42  cout << s << "(" << T_.x() << ", " << T_.y() << ", " << z_ << ", " << T_.theta() << ")" << endl;
43 }
44 
45 /* ************************************************************************* */
46 bool Pose3Upright::equals(const Pose3Upright& x, double tol) const {
47  return T_.equals(x.T_, tol) && std::abs(z_ - x.z_) < tol;
48 }
49 
50 /* ************************************************************************* */
52  return Point3(x(), y(), z());
53 }
54 
55 /* ************************************************************************* */
57  return T_.t();
58 }
59 
60 /* ************************************************************************* */
62  return T_.r();
63 }
64 
65 /* ************************************************************************* */
67  return Rot3::Yaw(theta());
68 }
69 
70 /* ************************************************************************* */
72  return T_;
73 }
74 
75 /* ************************************************************************* */
77  return Pose3(rotation(), translation());
78 }
79 
80 /* ************************************************************************* */
81 Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const {
83  if (H1) {
84  Matrix H1_ = -I_4x4;
85  H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
86  H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
87  *H1 = H1_;
88  }
89  return result;
90 }
91 
92 /* ************************************************************************* */
94  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
95  if (!H1 && !H2)
96  return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
97  Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_);
98  if (H1) {
99  Matrix H1_ = I_4x4;
100  H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
101  H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
102  *H1 = H1_;
103  }
104  if (H2) *H2 = I_4x4;
105  return result;
106 }
107 
108 /* ************************************************************************* */
110  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
111  if (!H1 && !H2)
112  return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
113  Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_);
114  if (H1) {
115  Matrix H1_ = -I_4x4;
116  H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
117  H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
118  *H1 = H1_;
119  }
120  if (H2) *H2 = I_4x4;
121  return result;
122 }
123 
124 /* ************************************************************************* */
126  assert(v.size() == 4);
127  Vector v1(3); v1 << v(0), v(1), v(3);
128  return Pose3Upright(T_.retract(v1), z_ + v(2));
129 }
130 
131 /* ************************************************************************* */
134  Vector result(4);
135  result << pose2(0), pose2(1), p2.z() - z_, pose2(2);
136  return result;
137 }
138 
139 /* ************************************************************************* */
141  assert(xi.size() == 4);
142  Vector v1(3); v1 << xi(0), xi(1), xi(3);
143  return Pose3Upright(Pose2::Expmap(v1), xi(2));
144 }
145 
146 /* ************************************************************************* */
149  Vector result(4);
150  result << pose2(0), pose2(1), p.z(), pose2(2);
151  return result;
152 }
153 /* ************************************************************************* */
154 
155 } // \namespace gtsam
156 
157 
158 
static Pose3Upright Expmap(const Vector &xi)
Exponential map at identity - create a rotation from canonical coordinates.
Scalar * y
double y() const
get y
Definition: Pose2.h:218
Vector v1
Rot2 rotation2() const
const Point2 & t() const
translation
Definition: Pose2.h:224
Pose3Upright()
Default constructor initializes at origin.
Definition: Pose3Upright.h:39
Pose3Upright between(const Pose3Upright &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Vector2 Point2
Definition: Point2.h:27
ArrayXcf v
Definition: Cwise_arg.cpp:1
Pose3Upright compose(const Pose3Upright &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
compose this transformation onto another (first *this and then p2)
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
static Vector Logmap(const Pose3Upright &p)
Log map at identity - return the canonical coordinates of this rotation.
bool equals(const Pose3Upright &pose, double tol=1e-9) const
Definition: Half.h:150
static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose2.cpp:82
const Rot2 & r() const
rotation
Definition: Pose2.h:227
Point2 translation2() const
Pose3Upright retract(const Vector &v) const
double x() const
Definition: Pose3Upright.h:64
Pose3 pose() const
Class compose(const Class &g) const
Definition: Lie.h:47
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:177
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
double theta() const
get theta
Definition: Pose2.h:221
void print(const std::string &s="") const
Vector localCoordinates(const Pose3Upright &p2) const
Local 3D coordinates of Pose3Upright manifold neighborhood around current pose.
RealScalar s
double theta() const
Definition: Pose3Upright.h:67
double z() const
Definition: Pose3Upright.h:66
Rot3 rotation() const
Vector xi
Definition: testPose2.cpp:150
traits
Definition: chartTesting.h:28
Point3 translation() const
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
double x() const
get x
Definition: Pose2.h:215
double y() const
Definition: Pose3Upright.h:65
float * p
Class between(const Class &g) const
Definition: Lie.h:51
static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose2.cpp:66
static Point3 p2
Pose2 pose2() const
const G double tol
Definition: Group.h:83
Vector3 Point3
Definition: Point3.h:35
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition: Pose2.cpp:201
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
#define abs(x)
Definition: datatypes.h:17
Pose3Upright inverse(boost::optional< Matrix & > H1=boost::none) const
inverse transformation with derivatives
GTSAM_EXPORT bool equals(const Pose2 &pose, double tol=1e-9) const
Definition: Pose2.cpp:61
Point2 t(10, 10)
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:135
Variation of a Pose3 in which the rotation is constained to purely yaw This state is essentially a Po...


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:27