1/**
2 * @file BearingS2.cpp
3 *
4 * @date Jan 26, 2012
5 * @author Alex Cunningham
6 */
7
8#include <iostream>
9
10#include <gtsam_unstable/geometry/BearingS2.h>
11
12#include <cassert>
13
14namespace gtsam {
15
16using namespace std;
17
18/* ************************************************************************* */
19void BearingS2::print(const std::string& s) const {
20 cout << s << " azimuth: " << azimuth_.theta() << " elevation: " << elevation_.theta() << endl;
21}
22
23/* ************************************************************************* */
24bool BearingS2::equals(const BearingS2& x, double tol) const {
25 return azimuth_.equals(R: x.azimuth_, tol) && elevation_.equals(R: x.elevation_, tol);
26}
27
28/* ************************************************************************* */
29BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) {
30 // Cnb = DCMnb(Att);
31 Matrix Cnb = A.rotation().matrix().transpose();
32
33 // Cbc = [0,0,1;0,1,0;-1,0,0];
34 Matrix Cbc = (Matrix(3,3) <<
35 0.,0.,1.,
36 0.,1.,0.,
37 -1.,0.,0.).finished();
38 // p_rel_c = Cbc*Cnb*(PosObj - Pos);
39 Vector p_rel_c = Cbc*Cnb*(B - A.translation());
40
41 // FIXME: the matlab code checks for p_rel_c(0) greater than
42
43 // azi = atan2(p_rel_c(2),p_rel_c(1));
44 double azimuth = atan2(y: p_rel_c(1),x: p_rel_c(0));
45 // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2));
46 double elevation = atan2(y: p_rel_c(2),x: sqrt(x: p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1)));
47 return BearingS2(azimuth, elevation);
48}
49
50/* ************************************************************************* */
51BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) {
52 // Cnb = DCMnb(Att);
53 Matrix Cnb = A.rotation().matrix().transpose();
54
55 Vector p_rel_c = Cnb*(B - A.translation());
56
57 // FIXME: the matlab code checks for p_rel_c(0) greater than
58
59 // azi = atan2(p_rel_c(2),p_rel_c(1));
60 double azimuth = atan2(y: p_rel_c(1),x: p_rel_c(0));
61 // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2));
62 double elevation = atan2(y: p_rel_c(2),x: sqrt(x: p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1)));
63 return BearingS2(azimuth, elevation);
64}
65
66/* ************************************************************************* */
67BearingS2 BearingS2::retract(const Vector& v) const {
68 assert(v.size() == 2);
69 return BearingS2(azimuth_.retract(v: v.head(n: 1)), elevation_.retract(v: v.tail(n: 1)));
70}
71
72/* ************************************************************************* */
73Vector BearingS2::localCoordinates(const BearingS2& x) const {
74 return (Vector(2) << azimuth_.localCoordinates(g: x.azimuth_)(0),
75 elevation_.localCoordinates(g: x.elevation_)(0)).finished();
76}
77
78} // \namespace gtsam
79