gtsam 4.1.1 gtsam
Line3.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 * Atlanta, Georgia 30332-0415
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 * -------------------------------------------------------------------------- */
8
15// \callgraph
16
17#pragma once
18
19#include <gtsam/geometry/Rot3.h>
21
22namespace gtsam {
23
29class Line3 {
30 private:
31 Rot3 R_; // Rotation of line about x and y in world frame
32 double a_, b_; // Intersection of line with the world x-y plane rotated by R_
33 // Also the closest point on line to origin
34 public:
35 enum { dimension = 4 };
36
39 a_(0), b_(0) {}
40
42 Line3(const Rot3 &R, const double a, const double b) :
43 R_(R), a_(a), b_(b) {}
44
56 Line3 retract(const Vector4 &v,
57 OptionalJacobian<4, 4> Dp = boost::none,
58 OptionalJacobian<4, 4> Dv = boost::none) const;
59
69 Vector4 localCoordinates(const Line3 &q,
70 OptionalJacobian<4, 4> Dp = boost::none,
71 OptionalJacobian<4, 4> Dq = boost::none) const;
72
77 void print(const std::string &s = "") const;
78
85 bool equals(const Line3 &l2, double tol = 10e-9) const;
86
93 Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const;
94
103 Point3 point(double distance = 0) const;
104
108 inline Rot3 R() const {
109 return R_;
110 }
111
115 inline double a() const {
116 return a_;
117 }
118
122 inline double b() const {
123 return b_;
124 }
125
134 friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
137};
138
147Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
148 OptionalJacobian<4, 6> Dpose = boost::none,
149 OptionalJacobian<4, 4> Dline = boost::none);
150
151template<>
152struct traits<Line3> : public internal::Manifold<Line3> {};
153
154template<>
155struct traits<const Line3> : public internal::Manifold<Line3> {};
156}
3D Pose
3D rotation represented as a rotation matrix or quaternion
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition: Line3.cpp:94
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
Definition: Line3.h:29
Unit3 project(OptionalJacobian< 2, 4 > Dline=boost::none) const
Projecting a line to the image plane.
Definition: Line3.cpp:61
Vector4 localCoordinates(const Line3 &q, OptionalJacobian< 4, 4 > Dp=boost::none, OptionalJacobian< 4, 4 > Dq=boost::none) const
The localCoordinates method is the inverse of retract and finds the difference between two lines in t...
Definition: Line3.cpp:27
Rot3 R() const
Return the rotation of the line.
Definition: Line3.h:108
Line3 retract(const Vector4 &v, OptionalJacobian< 4, 4 > Dp=boost::none, OptionalJacobian< 4, 4 > Dv=boost::none) const
The retract method maps from the tangent space back to the manifold.
Definition: Line3.cpp:5
void print(const std::string &s="") const
Print R, a, b.
Definition: Line3.cpp:49
Line3()
Default constructor is the Z axis.
Definition: Line3.h:38
bool equals(const Line3 &l2, double tol=10e-9) const
Check if two lines are equal.
Definition: Line3.cpp:55
Line3(const Rot3 &R, const double a, const double b)
Constructor for general line from (R, a, b)
Definition: Line3.h:42
Point3 point(double distance=0) const
Returns point on the line that is at a certain distance starting from the point where the rotated XY ...
Definition: Line3.cpp:86
double b() const
Return the y-coordinate of the intersection of the line with the xy plane.
Definition: Line3.h:122
double a() const
Return the x-coordinate of the intersection of the line with the xy plane.
Definition: Line3.h:115
friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition: Line3.cpp:94
Definition: Pose3.h:37
Definition: Rot3.h:59
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42