Line3.cpp
Go to the documentation of this file.
1 #include <gtsam/geometry/Line3.h>
2 
3 namespace gtsam {
4 
6  Vector3 w;
7  w << v[0], v[1], 0;
8  Rot3 incR;
9 
10  if (Dp) {
11  Dp->setIdentity();
12  incR = Rot3::Expmap(w);
13  Dp->block<2, 2>(0, 0) = ((incR.matrix()).transpose()).block<2, 2>(0, 0);
14  }
15  if (Dv) {
16  Matrix3 Dw;
17  incR = Rot3::Expmap(w, Dw);
18  Dv->setIdentity();
19  Dv->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0);
20  } else {
21  incR = Rot3::Expmap(w);
22  }
23  Rot3 Rt = R_ * incR;
24  return Line3(Rt, a_ + v[2], b_ + v[3]);
25 }
26 
28  OptionalJacobian<4, 4> Dq) const {
29  Vector3 omega;
30  Matrix3 D_log;
31  omega = Rot3::Logmap(R_.inverse() * q.R_, D_log);
32  if (Dp) {
33  Matrix3 D_log_wp = -((q.R_).matrix()).transpose() * R_.matrix();
34  Matrix3 Dwp = D_log * D_log_wp;
35  Dp->setIdentity();
36  Dp->block<2, 2>(0, 0) = Dwp.block<2, 2>(0, 0);
37  (*Dp)(2, 2) = -1;
38  (*Dp)(3, 3) = -1;
39  }
40  if (Dq) {
41  Dq->setIdentity();
42  Dq->block<2, 2>(0, 0) = D_log.block<2, 2>(0, 0);
43  }
44  Vector4 local;
45  local << omega[0], omega[1], q.a_ - a_, q.b_ - b_;
46  return local;
47 }
48 
49 void Line3::print(const std::string &s) const {
50  std::cout << s << std::endl;
51  R_.print("R:\n");
52  std::cout << "a: " << a_ << ", b: " << b_ << std::endl;
53 }
54 
55 bool Line3::equals(const Line3 &l2, double tol) const {
56  Vector4 diff = localCoordinates(l2);
57  return fabs(diff[0]) < tol && fabs(diff[1]) < tol
58  && fabs(diff[2]) < tol && fabs(diff[3]) < tol;
59 }
60 
62  Vector3 V_0;
63  V_0 << -b_, a_, 0.0;
64 
65  Unit3 l;
66  if (Dline) {
67  // Jacobian of the normalized Unit3 projected line with respect to
68  // un-normalized Vector3 projected line in homogeneous coordinates.
69  Matrix23 D_unit_line;
70  l = Unit3::FromPoint3(Point3(R_ * V_0), D_unit_line);
71  // Jacobian of the un-normalized Vector3 line with respect to
72  // input 3D line
73  Matrix34 D_vec_line = Matrix34::Zero();
74  D_vec_line.col(0) = a_ * R_.r3();
75  D_vec_line.col(1) = b_ * R_.r3();
76  D_vec_line.col(2) = R_.r2();
77  D_vec_line.col(3) = -R_.r1();
78  // Jacobian of output wrt input is the product of the two.
79  *Dline = D_unit_line * D_vec_line;
80  } else {
81  l = Unit3::FromPoint3(Point3(R_ * V_0));
82  }
83  return l;
84 }
85 
86 Point3 Line3::point(double distance) const {
87  // defining "center" of the line to be the point where it
88  // intersects rotated XY axis
89  Point3 center(a_, b_, 0);
90  Point3 rotated_center = R_ * center;
91  return rotated_center + distance * R_.r3();
92 }
93 
94 Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
96  Rot3 wRc = wTc.rotation();
97  Rot3 cRw = wRc.inverse();
98  Rot3 cRl = cRw * wL.R_;
99 
100  Vector2 w_ab;
101  Vector3 t = ((wL.R_).transpose() * wTc.translation());
102  Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]);
103 
104  if (Dpose) {
105  Matrix3 lRc = (cRl.matrix()).transpose();
106  Dpose->setZero();
107  // rotation
108  Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0);
109  // translation
110  Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0);
111  }
112  if (Dline) {
113  Dline->setIdentity();
114  (*Dline)(3, 0) = -t[2];
115  (*Dline)(2, 1) = t[2];
116  }
117  return Line3(cRl, c_ab[0], c_ab[1]);
118 }
119 
120 } // namespace gtsam
void print(const std::string &s="") const
Definition: Line3.cpp:49
Point3 r1() const
first column
Definition: Rot3M.cpp:223
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Eigen::Vector3d Vector3
Definition: Vector.h:43
friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
gtsam::Key l2
Double_ distance(const OrientedPlane3_ &p)
Real fabs(const Real &a)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Point3 point(double distance=0) const
Definition: Line3.cpp:86
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
Point3 r2() const
second column
Definition: Rot3M.cpp:226
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
static const Line3 l(Rot3(), 1, 1)
Vector4 localCoordinates(const Line3 &q, OptionalJacobian< 4, 4 > Dp={}, OptionalJacobian< 4, 4 > Dq={}) const
Definition: Line3.cpp:27
bool equals(const Line3 &l2, double tol=10e-9) const
Definition: Line3.cpp:55
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
double a_
Definition: Line3.h:47
Array< int, Dynamic, 1 > v
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
RowVector3d w
4 dimensional manifold of 3D lines
traits
Definition: chartTesting.h:28
static Unit3 FromPoint3(const Point3 &point, OptionalJacobian< 2, 3 > H={})
Named constructor from Point3 with optional Jacobian.
Definition: Unit3.cpp:44
Matrix3 matrix() const
Definition: Rot3M.cpp:218
double b_
Definition: Line3.h:47
Rot3 R_
Definition: Line3.h:46
void print(const std::string &s="") const
Definition: Rot3.cpp:33
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Line3 retract(const Vector4 &v, OptionalJacobian< 4, 4 > Dp={}, OptionalJacobian< 4, 4 > Dv={}) const
Definition: Line3.cpp:5
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Unit3 project(OptionalJacobian< 2, 4 > Dline={}) const
Definition: Line3.cpp:61
Point2 t(10, 10)
Point3 r3() const
third column
Definition: Rot3M.cpp:229


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