Pose3Upright.cpp
Go to the documentation of this file.
1 
8 #include <iostream>
10 #include "gtsam/base/Vector.h"
11 
13 
14 using namespace std;
15 
16 namespace gtsam {
17 
18 /* ************************************************************************* */
19 Pose3Upright::Pose3Upright(const Rot2& bearing, const Point3& t)
20 : T_(bearing, Point2(t.x(), t.y())), z_(t.z())
21 {
22 }
23 
24 /* ************************************************************************* */
25 Pose3Upright::Pose3Upright(double x, double y, double z, double theta)
26 : T_(x, y, theta), z_(z)
27 {
28 }
29 
30 /* ************************************************************************* */
32 : T_(pose), z_(z)
33 {
34 }
35 
36 /* ************************************************************************* */
38 : T_(x.x(), x.y(), x.rotation().yaw()), z_(x.z())
39 {
40 }
41 
42 /* ************************************************************************* */
43 void Pose3Upright::print(const std::string& s) const {
44  cout << s << "(" << T_.x() << ", " << T_.y() << ", " << z_ << ", " << T_.theta() << ")" << endl;
45 }
46 
47 /* ************************************************************************* */
48 bool Pose3Upright::equals(const Pose3Upright& x, double tol) const {
49  return T_.equals(x.T_, tol) && std::abs(z_ - x.z_) < tol;
50 }
51 
52 /* ************************************************************************* */
54  return Point3(x(), y(), z());
55 }
56 
57 /* ************************************************************************* */
59  return T_.t();
60 }
61 
62 /* ************************************************************************* */
64  return T_.r();
65 }
66 
67 /* ************************************************************************* */
69  return Rot3::Yaw(theta());
70 }
71 
72 /* ************************************************************************* */
74  return T_;
75 }
76 
77 /* ************************************************************************* */
79  return Pose3(rotation(), translation());
80 }
81 
82 /* ************************************************************************* */
84  if (!H1) {
85  return Pose3Upright(T_.inverse(), -z_);
86  }
88  // TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
89  // Eigen::Ref<Eigen::Matrix<double, 3, 3>> H3x3 = H1->topLeftCorner(3,3);
90  Pose3Upright result(T_.inverse(H3x3), -z_);
91  Matrix H1_ = -I_4x4;
92  H1_.topLeftCorner(2, 2) = H3x3.topLeftCorner(2, 2);
93  H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
94  *H1 = H1_;
95  return result;
96 }
97 
98 /* ************************************************************************* */
101  if (!H1 && !H2)
102  return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
103 
104  // TODO(kartikarcot): Could not use reference to a view into H1 and reuse memory
106  Pose3Upright result(T_.compose(p2.T_, H3x3), z_ + p2.z_);
107  if (H1) {
108  Matrix H1_ = I_4x4;
109  H1_.topLeftCorner(2,2) = H3x3.topLeftCorner(2,2);
110  H1_.topRightCorner(2, 1) = H3x3.topRightCorner(2, 1);
111  *H1 = H1_;
112  }
113  if (H2) *H2 = I_4x4;
114  return result;
115 }
116 
117 /* ************************************************************************* */
120  if (!H1 && !H2)
121  return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
122 
123  // TODO(kartikarcot): Could not use reference to a view into H1 and H2 to reuse memory
124  OptionalJacobian<3, 3>::Jacobian H3x3_1, H3x3_2;
125  Pose3Upright result(T_.between(p2.T_, H3x3_1, H3x3_2), p2.z_ - z_);
126  if (H1) {
127  Matrix H1_ = -I_4x4;
128  H1_.topLeftCorner(2,2) = H3x3_1.topLeftCorner(2,2);
129  H1_.topRightCorner(2, 1) = H3x3_1.topRightCorner(2, 1);
130  *H1 = H1_;
131  }
132  if (H2) *H2 = I_4x4;
133  return result;
134 }
135 
136 /* ************************************************************************* */
138  assert(v.size() == 4);
139  Vector v1(3); v1 << v(0), v(1), v(3);
140  return Pose3Upright(T_.retract(v1), z_ + v(2));
141 }
142 
143 /* ************************************************************************* */
145  Vector pose2 = T_.localCoordinates(p2.pose2());
146  Vector result(4);
147  result << pose2(0), pose2(1), p2.z() - z_, pose2(2);
148  return result;
149 }
150 
151 /* ************************************************************************* */
153  assert(xi.size() == 4);
154  Vector v1(3); v1 << xi(0), xi(1), xi(3);
155  return Pose3Upright(Pose2::Expmap(v1), xi(2));
156 }
157 
158 /* ************************************************************************* */
160  Vector pose2 = Pose2::Logmap(p.pose2());
161  Vector result(4);
162  result << pose2(0), pose2(1), p.z(), pose2(2);
163  return result;
164 }
165 /* ************************************************************************* */
166 
167 } // \namespace gtsam
168 
169 
170 
gtsam::Pose2::inverse
Pose2 inverse() const
inverse
Definition: Pose2.cpp:201
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
gtsam::Pose3Upright::pose2
Pose2 pose2() const
Definition: Pose3Upright.cpp:73
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::Pose3Upright::z
double z() const
Definition: Pose3Upright.h:66
s
RealScalar s
Definition: level1_cplx_impl.h:126
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::Pose2::equals
bool equals(const Pose2 &pose, double tol=1e-9) const
Definition: Pose2.cpp:61
gtsam::Pose3Upright::compose
Pose3Upright compose(const Pose3Upright &p2, OptionalJacobian< 4, 4 > H1={}, OptionalJacobian< 4, 4 > H2={}) const
compose this transformation onto another (first *this and then p2)
Definition: Pose3Upright.cpp:99
gtsam::Pose2::t
const Point2 & t() const
translation
Definition: Pose2.h:255
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
gtsam::Pose3Upright::between
Pose3Upright between(const Pose3Upright &p2, OptionalJacobian< 4, 4 > H1={}, OptionalJacobian< 4, 4 > H2={}) const
Definition: Pose3Upright.cpp:118
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Pose3Upright::theta
double theta() const
Definition: Pose3Upright.h:67
gtsam::Pose2::Expmap
static Pose2 Expmap(const Vector3 &xi, ChartJacobian H={})
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose2.cpp:66
gtsam::Pose3Upright::Expmap
static Pose3Upright Expmap(const Vector &xi)
Exponential map at identity - create a rotation from canonical coordinates.
Definition: Pose3Upright.cpp:152
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Pose3Upright::T_
Pose2 T_
Definition: Pose3Upright.h:31
gtsam::Pose3Upright::pose
Pose3 pose() const
Definition: Pose3Upright.cpp:78
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::Pose2::Logmap
static Vector3 Logmap(const Pose2 &p, ChartJacobian H={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose2.cpp:82
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Pose3Upright::Logmap
static Vector Logmap(const Pose3Upright &p)
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3Upright.cpp:159
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition: Pose2.h:258
Pose3Upright.h
Variation of a Pose3 in which the rotation is constained to purely yaw This state is essentially a Po...
OptionalJacobian.h
Special class for optional Jacobian arguments.
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
gtsam::Pose3Upright::rotation
Rot3 rotation() const
Definition: Pose3Upright.cpp:68
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::Pose2::theta
double theta() const
get theta
Definition: Pose2.h:252
gtsam::Pose3Upright::translation2
Point2 translation2() const
Definition: Pose3Upright.cpp:58
gtsam::Rot2
Definition: Rot2.h:35
gtsam::Pose3Upright::rotation2
Rot2 rotation2() const
Definition: Pose3Upright.cpp:63
gtsam
traits
Definition: SFMdata.h:40
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:89
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Pose3Upright::translation
Point3 translation() const
Definition: Pose3Upright.cpp:53
gtsam::Pose2::x
double x() const
get x
Definition: Pose2.h:246
gtsam::Pose3Upright::y
double y() const
Definition: Pose3Upright.h:65
std
Definition: BFloat16.h:88
gtsam::Pose3Upright::equals
bool equals(const Pose3Upright &pose, double tol=1e-9) const
Definition: Pose3Upright.cpp:48
gtsam::Pose3Upright::print
void print(const std::string &s="") const
Definition: Pose3Upright.cpp:43
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Pose3Upright::x
double x() const
Definition: Pose3Upright.h:64
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::Pose3Upright::retract
Pose3Upright retract(const Vector &v) const
Definition: Pose3Upright.cpp:137
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::Pose3Upright
Definition: Pose3Upright.h:25
gtsam::Pose3Upright::Pose3Upright
Pose3Upright()
Default constructor initializes at origin.
Definition: Pose3Upright.h:39
gtsam::Pose3Upright::localCoordinates
Vector localCoordinates(const Pose3Upright &p2) const
Local 3D coordinates of Pose3Upright manifold neighborhood around current pose.
Definition: Pose3Upright.cpp:144
gtsam::Pose3Upright::z_
double z_
Definition: Pose3Upright.h:32
align_3::t
Point2 t(10, 10)
gtsam::Pose2::y
double y() const
get y
Definition: Pose2.h:249
gtsam::Pose3Upright::inverse
Pose3Upright inverse(OptionalJacobian< 4, 4 > H1={}) const
inverse transformation with derivatives
Definition: Pose3Upright.cpp:83
v1
Vector v1
Definition: testSerializationBase.cpp:38
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24