1/**
2 * @file Pose3Upright.cpp
3 *
4 * @date Jan 24, 2012
5 * @author Alex Cunningham
6 */
7
8#include <iostream>
9#include <cassert>
10#include <gtsam/base/OptionalJacobian.h>
11#include <gtsam/base/Vector.h>
12
13#include <gtsam_unstable/geometry/Pose3Upright.h>
14
15using namespace std;
16
17namespace gtsam {
18
19/* ************************************************************************* */
20Pose3Upright::Pose3Upright(const Rot2& bearing, const Point3& t)
21: T_(bearing, Point2(t.x(), t.y())), z_(t.z())
22{
23}
24
25/* ************************************************************************* */
26Pose3Upright::Pose3Upright(double x, double y, double z, double theta)
27: T_(x, y, theta), z_(z)
28{
29}
30
31/* ************************************************************************* */
32Pose3Upright::Pose3Upright(const Pose2& pose, double z)
33: T_(pose), z_(z)
34{
35}
36
37/* ************************************************************************* */
38Pose3Upright::Pose3Upright(const Pose3& x)
39: T_(x.x(), x.y(), x.rotation().yaw()), z_(x.z())
40{
41}
42
43/* ************************************************************************* */
44void Pose3Upright::print(const std::string& s) const {
45 cout << s << "(" << T_.x() << ", " << T_.y() << ", " << z_ << ", " << T_.theta() << ")" << endl;
46}
47
48/* ************************************************************************* */
49bool Pose3Upright::equals(const Pose3Upright& x, double tol) const {
50 return T_.equals(pose: x.T_, tol) && std::abs(x: z_ - x.z_) < tol;
51}
52
53/* ************************************************************************* */
54Point3 Pose3Upright::translation() const {
55 return Point3(x(), y(), z());
56}
57
58/* ************************************************************************* */
59Point2 Pose3Upright::translation2() const {
60 return T_.t();
61}
62
63/* ************************************************************************* */
64Rot2 Pose3Upright::rotation2() const {
65 return T_.r();
66}
67
68/* ************************************************************************* */
69Rot3 Pose3Upright::rotation() const {
70 return Rot3::Yaw(t: theta());
71}
72
73/* ************************************************************************* */
74Pose2 Pose3Upright::pose2() const {
75 return T_;
76}
77
78/* ************************************************************************* */
79Pose3 Pose3Upright::pose() const {
80 return Pose3(rotation(), translation());
81}
82
83/* ************************************************************************* */
84Pose3Upright Pose3Upright::inverse(OptionalJacobian<4, 4> H1) const {
85 if (!H1) {
86 return Pose3Upright(T_.inverse(), -z_);
87 }
88 OptionalJacobian<3, 3>::Jacobian H3x3;
89 // TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
90 // Eigen::Ref<Eigen::Matrix<double, 3, 3>> H3x3 = H1->topLeftCorner(3,3);
91 Pose3Upright result(T_.inverse(H: H3x3), -z_);
92 Matrix H1_ = -I_4x4;
93 H1_.topLeftCorner(cRows: 2, cCols: 2) = H3x3.topLeftCorner(cRows: 2, cCols: 2);
94 H1_.topRightCorner(cRows: 2, cCols: 1) = H3x3.topRightCorner(cRows: 2, cCols: 1);
95 *H1 = H1_;
96 return result;
97}
98
99/* ************************************************************************* */
100Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
101 OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
102 if (!H1 && !H2)
103 return Pose3Upright(T_.compose(g: p2.T_), z_ + p2.z_);
104
105 // TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
106 OptionalJacobian<3, 3>::Jacobian H3x3;
107 Pose3Upright result(T_.compose(g: p2.T_, H1: H3x3), z_ + p2.z_);
108 if (H1) {
109 Matrix H1_ = I_4x4;
110 H1_.topLeftCorner(cRows: 2,cCols: 2) = H3x3.topLeftCorner(cRows: 2,cCols: 2);
111 H1_.topRightCorner(cRows: 2, cCols: 1) = H3x3.topRightCorner(cRows: 2, cCols: 1);
112 *H1 = H1_;
113 }
114 if (H2) *H2 = I_4x4;
115 return result;
116}
117
118/* ************************************************************************* */
119Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
120 OptionalJacobian<4,4> H1, OptionalJacobian<4,4> H2) const {
121 if (!H1 && !H2)
122 return Pose3Upright(T_.between(g: p2.T_), p2.z_ - z_);
123
124 // TODO(kartikarcot): Could not use reference to a view into H1 and H2 to reuse memory
125 OptionalJacobian<3, 3>::Jacobian H3x3_1, H3x3_2;
126 Pose3Upright result(T_.between(g: p2.T_, H1: H3x3_1, H2: H3x3_2), p2.z_ - z_);
127 if (H1) {
128 Matrix H1_ = -I_4x4;
129 H1_.topLeftCorner(cRows: 2,cCols: 2) = H3x3_1.topLeftCorner(cRows: 2,cCols: 2);
130 H1_.topRightCorner(cRows: 2, cCols: 1) = H3x3_1.topRightCorner(cRows: 2, cCols: 1);
131 *H1 = H1_;
132 }
133 if (H2) *H2 = I_4x4;
134 return result;
135}
136
137/* ************************************************************************* */
138Pose3Upright Pose3Upright::retract(const Vector& v) const {
139 assert(v.size() == 4);
140 Vector v1(3); v1 << v(0), v(1), v(3);
141 return Pose3Upright(T_.retract(v: v1), z_ + v(2));
142}
143
144/* ************************************************************************* */
145Vector Pose3Upright::localCoordinates(const Pose3Upright& p2) const {
146 Vector pose2 = T_.localCoordinates(g: p2.pose2());
147 Vector result(4);
148 result << pose2(0), pose2(1), p2.z() - z_, pose2(2);
149 return result;
150}
151
152/* ************************************************************************* */
153Pose3Upright Pose3Upright::Expmap(const Vector& xi) {
154 assert(xi.size() == 4);
155 Vector v1(3); v1 << xi(0), xi(1), xi(3);
156 return Pose3Upright(Pose2::Expmap(xi: v1), xi(2));
157}
158
159/* ************************************************************************* */
160Vector Pose3Upright::Logmap(const Pose3Upright& p) {
161 Vector pose2 = Pose2::Logmap(p: p.pose2());
162 Vector result(4);
163 result << pose2(0), pose2(1), p.z(), pose2(2);
164 return result;
165}
166/* ************************************************************************* */
167
168} // \namespace gtsam
169
170
171
172