Line3.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3  * Atlanta, Georgia 30332-0415
4  * All Rights Reserved
5  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6  * See LICENSE for the license information
7  * -------------------------------------------------------------------------- */
8 
15 // \callgraph
16 
17 #pragma once
18 
19 #include <gtsam/geometry/Rot3.h>
20 #include <gtsam/geometry/Pose3.h>
21 
22 namespace gtsam {
23 
24 class Line3;
25 
34 GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
35  OptionalJacobian<4, 6> Dpose = {},
36  OptionalJacobian<4, 4> Dline = {});
37 
38 
44 class GTSAM_EXPORT Line3 {
45  private:
46  Rot3 R_; // Rotation of line about x and y in world frame
47  double a_, b_; // Intersection of line with the world x-y plane rotated by R_
48  // Also the closest point on line to origin
49  public:
50  enum { dimension = 4 };
51 
53  Line3() :
54  a_(0), b_(0) {}
55 
57  Line3(const Rot3 &R, const double a, const double b) :
58  R_(R), a_(a), b_(b) {}
59 
71  Line3 retract(const Vector4 &v,
72  OptionalJacobian<4, 4> Dp = {},
73  OptionalJacobian<4, 4> Dv = {}) const;
74 
84  Vector4 localCoordinates(const Line3 &q,
85  OptionalJacobian<4, 4> Dp = {},
86  OptionalJacobian<4, 4> Dq = {}) const;
87 
92  void print(const std::string &s = "") const;
93 
100  bool equals(const Line3 &l2, double tol = 10e-9) const;
101 
108  Unit3 project(OptionalJacobian<2, 4> Dline = {}) const;
109 
118  Point3 point(double distance = 0) const;
119 
123  inline Rot3 R() const {
124  return R_;
125  }
126 
130  inline double a() const {
131  return a_;
132  }
133 
137  inline double b() const {
138  return b_;
139  }
140 
149  friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
151  OptionalJacobian<4, 4> Dline);
152 };
153 
154 template<>
155 struct traits<Line3> : public internal::Manifold<Line3> {};
156 
157 template<>
158 struct traits<const Line3> : public internal::Manifold<Line3> {};
159 }
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Rot2 R(Rot2::fromAngle(0.1))
gtsam::Key l2
Double_ distance(const OrientedPlane3_ &p)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Rot3 R() const
Definition: Line3.h:123
double a() const
Definition: Line3.h:130
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Array< int, Dynamic, 1 > v
double b() const
Definition: Line3.h:137
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
const G & b
Definition: Group.h:86
traits
Definition: chartTesting.h:28
Line3(const Rot3 &R, const double a, const double b)
Definition: Line3.h:57
double b_
Definition: Line3.h:47
Rot3 R_
Definition: Line3.h:46
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
3D Pose
3D rotation represented as a rotation matrix or quaternion


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33