BearingS2.cpp
Go to the documentation of this file.
1 
8 #include <iostream>
9 
11 
12 namespace gtsam {
13 
14 using namespace std;
15 
16 /* ************************************************************************* */
17 void BearingS2::print(const std::string& s) const {
18  cout << s << " azimuth: " << azimuth_.theta() << " elevation: " << elevation_.theta() << endl;
19 }
20 
21 /* ************************************************************************* */
22 bool BearingS2::equals(const BearingS2& x, double tol) const {
23  return azimuth_.equals(x.azimuth_, tol) && elevation_.equals(x.elevation_, tol);
24 }
25 
26 /* ************************************************************************* */
28  // Cnb = DCMnb(Att);
29  Matrix Cnb = A.rotation().matrix().transpose();
30 
31  // Cbc = [0,0,1;0,1,0;-1,0,0];
32  Matrix Cbc = (Matrix(3,3) <<
33  0.,0.,1.,
34  0.,1.,0.,
35  -1.,0.,0.).finished();
36  // p_rel_c = Cbc*Cnb*(PosObj - Pos);
37  Vector p_rel_c = Cbc*Cnb*(B - A.translation());
38 
39  // FIXME: the matlab code checks for p_rel_c(0) greater than
40 
41  // azi = atan2(p_rel_c(2),p_rel_c(1));
42  double azimuth = atan2(p_rel_c(1),p_rel_c(0));
43  // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2));
44  double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1)));
45  return BearingS2(azimuth, elevation);
46 }
47 
48 /* ************************************************************************* */
50  // Cnb = DCMnb(Att);
51  Matrix Cnb = A.rotation().matrix().transpose();
52 
53  Vector p_rel_c = Cnb*(B - A.translation());
54 
55  // FIXME: the matlab code checks for p_rel_c(0) greater than
56 
57  // azi = atan2(p_rel_c(2),p_rel_c(1));
58  double azimuth = atan2(p_rel_c(1),p_rel_c(0));
59  // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2));
60  double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1)));
61  return BearingS2(azimuth, elevation);
62 }
63 
64 /* ************************************************************************* */
66  assert(v.size() == 2);
67  return BearingS2(azimuth_.retract(v.head(1)), elevation_.retract(v.tail(1)));
68 }
69 
70 /* ************************************************************************* */
72  return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0),
73  elevation_.localCoordinates(x.elevation_)(0)).finished();
74 }
75 
76 } // \namespace gtsam
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
bool equals(const BearingS2 &x, double tol=1e-9) const
Definition: BearingS2.cpp:22
Definition: BFloat16.h:88
void print(const std::string &s="") const
Definition: BearingS2.cpp:17
static BearingS2 fromDownwardsObservation(const POSE &A, const TARGET &B)
Definition: BearingS2.h:51
Eigen::VectorXd Vector
Definition: Vector.h:38
BearingS2 retract(const Vector &v) const
Definition: BearingS2.cpp:65
Vector localCoordinates(const BearingS2 &p2) const
Local coordinates of BearingS2 manifold neighborhood around current pose.
Definition: BearingS2.cpp:71
Array< int, Dynamic, 1 > v
RealScalar s
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
traits
Definition: chartTesting.h:28
Matrix3 matrix() const
Definition: Rot3M.cpp:218
static BearingS2 fromForwardObservation(const Pose3 &A, const Point3 &B)
Definition: BearingS2.cpp:49
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
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
Manifold measurement between two points on a unit sphere.


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