Pose3Upright.h
Go to the documentation of this file.
1 
12 #pragma once
13 
14 #include <gtsam_unstable/dllexport.h>
15 #include <gtsam/geometry/Pose3.h>
16 #include <gtsam/geometry/Pose2.h>
17 
18 namespace gtsam {
19 
25 class GTSAM_UNSTABLE_EXPORT Pose3Upright {
26 public:
27  static const size_t dimension = 4;
28 
29 protected:
30 
32  double z_;
33 
34 public:
37 
39  Pose3Upright() : z_(0.0) {}
40 
42  Pose3Upright(const Pose3Upright& x) : T_(x.T_), z_(x.z_) {}
43  Pose3Upright(const Rot2& bearing, const Point3& t);
44  Pose3Upright(double x, double y, double z, double theta);
45  Pose3Upright(const Pose2& pose, double z);
46 
48  Pose3Upright(const Pose3& fullpose);
49 
53 
55  void print(const std::string& s = "") const;
56 
58  bool equals(const Pose3Upright& pose, double tol = 1e-9) const;
59 
63 
64  double x() const { return T_.x(); }
65  double y() const { return T_.y(); }
66  double z() const { return z_; }
67  double theta() const { return T_.theta(); }
68 
69  Point2 translation2() const;
70  Point3 translation() const;
71  Rot2 rotation2() const;
72  Rot3 rotation() const;
73  Pose2 pose2() const;
74  Pose3 pose() const;
75 
79 
81  inline static size_t Dim() { return dimension; }
82 
84  inline size_t dim() const { return dimension; }
85 
88  Pose3Upright retract(const Vector& v) const;
89 
91  Vector localCoordinates(const Pose3Upright& p2) const;
92 
96 
98  static Pose3Upright Identity() { return Pose3Upright(); }
99 
102 
104  Pose3Upright compose(const Pose3Upright& p2,
105  OptionalJacobian<4,4> H1={},
106  OptionalJacobian<4,4> H2={}) const;
107 
109  inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); }
110 
116  OptionalJacobian<4,4> H1={},
117  OptionalJacobian<4,4> H2={}) const;
118 
122 
124  static Pose3Upright Expmap(const Vector& xi);
125 
127  static Vector Logmap(const Pose3Upright& p);
128 
130 
131 private:
132 
133 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION //
134  // Serialization function
135  friend class boost::serialization::access;
136  template<class Archive>
137  void serialize(Archive & ar, const unsigned int /*version*/) {
138  ar & BOOST_SERIALIZATION_NVP(T_);
139  ar & BOOST_SERIALIZATION_NVP(z_);
140  }
141 #endif
142 
143 }; // \class Pose3Upright
144 
145 template<>
146 struct traits<Pose3Upright> : public internal::Manifold<Pose3Upright> {};
147 
148 
149 } // \namespace gtsam
gtsam::Pose3Upright::dim
size_t dim() const
Dimensionality of tangent space = 4 DOF.
Definition: Pose3Upright.h:84
Pose2.h
2D Pose
gtsam::translation
Point3_ translation(const Pose3_ &pose)
Definition: slam/expressions.h:93
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
gtsam::Pose3Upright::z
double z() const
Definition: Pose3Upright.h:66
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
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::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::Pose3Upright::theta
double theta() const
Definition: Pose3Upright.h:67
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Pose3Upright::T_
Pose2 T_
Definition: Pose3Upright.h:31
gtsam::Pose3Upright::operator*
Pose3Upright operator*(const Pose3Upright &T) const
compose syntactic sugar
Definition: Pose3Upright.h:109
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
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
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
Eigen::Triplet< double >
gtsam::equals
Definition: Testable.h:112
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::Pose2::theta
double theta() const
get theta
Definition: Pose2.h:252
gtsam::Rot2
Definition: Rot2.h:35
gtsam
traits
Definition: SFMdata.h:40
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:89
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Pose2::x
double x() const
get x
Definition: Pose2.h:246
gtsam::Pose3Upright::y
double y() const
Definition: Pose3Upright.h:65
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Pose3Upright::Identity
static Pose3Upright Identity()
identity for group operation
Definition: Pose3Upright.h:98
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::Pose3Upright
Definition: Pose3Upright.h:25
gtsam::Pose3Upright::Pose3Upright
Pose3Upright()
Default constructor initializes at origin.
Definition: Pose3Upright.h:39
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
Pose3.h
3D Pose
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Pose3Upright::Dim
static size_t Dim()
Dimensionality of tangent space = 4 DOF - used to autodetect sizes.
Definition: Pose3Upright.h:81
gtsam::Pose3Upright::Pose3Upright
Pose3Upright(const Pose3Upright &x)
Copy constructor.
Definition: Pose3Upright.h:42


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