Unit3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * Atlanta, Georgia 30332-0415
4  * All Rights Reserved
5  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
12 /*
13  * @file Unit3.h
14  * @date Feb 02, 2011
15  * @author Can Erdogan
16  * @author Frank Dellaert
17  * @author Alex Trevor
18  * @author Zhaoyang Lv
19  * @brief The Unit3 class - basically a point on a unit sphere
20  */
21 
22 #include <gtsam/geometry/Unit3.h>
23 #include <gtsam/geometry/Point2.h>
24 #include <gtsam/config.h> // for GTSAM_USE_TBB
25 
26 #include <iostream>
27 #include <limits>
28 #include <cmath>
29 #include <vector>
30 
31 using namespace std;
32 
33 namespace gtsam {
34 
35 /* ************************************************************************* */
36 Unit3::Unit3(const Vector3& p) : p_(p.normalized()) {}
37 
38 Unit3::Unit3(double x, double y, double z) : p_(x, y, z) { p_.normalize(); }
39 
40 Unit3::Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
41  p_.normalize();
42 }
43 /* ************************************************************************* */
45  // 3*3 Derivative of representation with respect to point is 3*3:
46  Matrix3 D_p_point;
47  Unit3 direction;
48  direction.p_ = normalize(point, H ? &D_p_point : 0);
49  if (H)
50  *H << direction.basis().transpose() * D_p_point;
51  return direction;
52 }
53 
54 /* ************************************************************************* */
55 Unit3 Unit3::Random(std::mt19937& rng) {
56  // http://mathworld.wolfram.com/SpherePointPicking.html
57  // Adapted from implementation in boost, but using std <random>
58  std::uniform_real_distribution<double> uniform(-1.0, 1.0);
59  double sqsum;
60  double x, y;
61  do {
62  x = uniform(rng);
63  y = uniform(rng);
64  sqsum = x * x + y * y;
65  } while (sqsum > 1);
66  const double mult = 2 * sqrt(1 - sqsum);
67  return Unit3(x * mult, y * mult, 2 * sqsum - 1);
68 }
69 
70 /* ************************************************************************* */
71 // Get the axis of rotation with the minimum projected length of the point
72 static Point3 CalculateBestAxis(const Point3& n) {
73  double mx = std::abs(n.x()), my = std::abs(n.y()), mz = std::abs(n.z());
74  if ((mx <= my) && (mx <= mz)) {
75  return Point3(1.0, 0.0, 0.0);
76  } else if ((my <= mx) && (my <= mz)) {
77  return Point3(0.0, 1.0, 0.0);
78  } else {
79  return Point3(0, 0, 1);
80  }
81 }
82 
83 /* ************************************************************************* */
84 const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
85 #ifdef GTSAM_USE_TBB
86  // NOTE(hayk): At some point it seemed like this reproducably resulted in
87  // deadlock. However, I don't know why and I can no longer reproduce it.
88  // It either was a red herring or there is still a latent bug left to debug.
89  std::unique_lock<std::mutex> lock(B_mutex_);
90 #endif
91 
92  const bool cachedBasis = static_cast<bool>(B_);
93  const bool cachedJacobian = static_cast<bool>(H_B_);
94 
95  if (H) {
96  if (!cachedJacobian) {
97  // Compute Jacobian. Recomputes B_
98  Matrix32 B;
99  Matrix62 jacobian;
100  Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1;
101 
102  // Choose the direction of the first basis vector b1 in the tangent plane
103  // by crossing n with the chosen axis.
104  const Point3 n(p_), axis = CalculateBestAxis(n);
105  const Point3 B1 = gtsam::cross(n, axis, &H_B1_n);
106 
107  // Normalize result to get a unit vector: b1 = B1 / |B1|.
108  B.col(0) = normalize(B1, &H_b1_B1);
109 
110  // Get the second basis vector b2, which is orthogonal to n and b1.
111  B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1);
112 
113  // Chain rule tomfoolery to compute the jacobian.
114  const Matrix32& H_n_p = B;
115  jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p;
116  auto H_b1_p = jacobian.block<3, 2>(0, 0);
117  jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
118 
119  // Cache the result and jacobian
120  H_B_ = (jacobian);
121  B_ = (B);
122  }
123 
124  // Return cached jacobian, possibly computed just above
125  *H = *H_B_;
126  }
127 
128  if (!cachedBasis) {
129  // Same calculation as above, without derivatives.
130  // Done after H block, as that possibly computes B_ for the first time
131  Matrix32 B;
132 
133  const Point3 n(p_), axis = CalculateBestAxis(n);
134  const Point3 B1 = gtsam::cross(n, axis);
135  B.col(0) = normalize(B1);
136  B.col(1) = gtsam::cross(n, B.col(0));
137  B_ = (B);
138  }
139 
140  return *B_;
141 }
142 
143 /* ************************************************************************* */
145  if (H)
146  *H = basis();
147  return Point3(p_);
148 }
149 
150 /* ************************************************************************* */
152  if (H)
153  *H = basis();
154  return p_;
155 }
156 
157 /* ************************************************************************* */
158 std::ostream& operator<<(std::ostream& os, const Unit3& pair) {
159  os << pair.p_ << endl;
160  return os;
161 }
162 
163 /* ************************************************************************* */
164 void Unit3::print(const std::string& s) const {
165  cout << s << ":" << p_ << endl;
166 }
167 
168 /* ************************************************************************* */
169 Matrix3 Unit3::skew() const {
170  return skewSymmetric(p_.x(), p_.y(), p_.z());
171 }
172 
173 /* ************************************************************************* */
175  OptionalJacobian<1, 2> H_q) const {
176  // Get the unit vectors of each, and the derivative.
177  Matrix32 H_pn_p;
178  Point3 pn = point3(H_p ? &H_pn_p : nullptr);
179 
180  Matrix32 H_qn_q;
181  const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);
182 
183  // Compute the dot product of the Point3s.
184  Matrix13 H_dot_pn, H_dot_qn;
185  double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr);
186 
187  if (H_p) {
188  (*H_p) << H_dot_pn * H_pn_p;
189  }
190 
191  if (H_q) {
192  (*H_q) = H_dot_qn * H_qn_q;
193  }
194 
195  return d;
196 }
197 
198 /* ************************************************************************* */
200  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
201  const Vector2 xi = basis().transpose() * q.p_;
202  if (H_q) {
203  *H_q = basis().transpose() * q.basis();
204  }
205  return xi;
206 }
207 
208 /* ************************************************************************* */
210  OptionalJacobian<2, 2> H_q) const {
211  // Get the point3 of this, and the derivative.
212  Matrix32 H_qn_q;
213  const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);
214 
215  // 2D error here is projecting q into the tangent plane of this (p).
216  Matrix62 H_B_p;
217  Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose();
218  Vector2 xi = Bt * qn;
219 
220  if (H_p) {
221  // Derivatives of each basis vector.
222  const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0);
223  const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
224 
225  // Derivatives of the two entries of xi wrt the basis vectors.
226  const Matrix13 H_xi1_b1 = qn.transpose();
227  const Matrix13 H_xi2_b2 = qn.transpose();
228 
229  // Assemble dxi/dp = dxi/dB * dB/dp.
230  const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
231  const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
232  *H_p << H_xi1_p, H_xi2_p;
233  }
234 
235  if (H_q) {
236  // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
237  const Matrix23 H_xi_qu = Bt;
238  *H_q = H_xi_qu * H_qn_q;
239  }
240 
241  return xi;
242 }
243 
244 /* ************************************************************************* */
246  Matrix2 H_xi_q;
247  const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
248  const double theta = xi.norm();
249  if (H)
250  *H = (xi.transpose() / theta) * H_xi_q;
251  return theta;
252 }
253 
254 /* ************************************************************************* */
256  // Compute the 3D xi_hat vector
257  const Vector3 xi_hat = basis() * v;
258  const double theta = xi_hat.norm();
259  const double c = std::cos(theta);
260 
261  // Treat case of very small v differently.
262  Matrix23 H_from_point;
263  if (theta < std::numeric_limits<double>::epsilon()) {
264  const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat,
265  H? &H_from_point : nullptr);
266  if (H) { // Jacobian
267  *H = H_from_point *
268  (-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis();
269  }
270  return exp_p_xi_hat;
271  }
272 
273  const double st = std::sin(theta) / theta;
274  const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st,
275  H? &H_from_point : nullptr);
276  if (H) { // Jacobian
277  *H = H_from_point *
278  (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() +
279  xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis();
280  }
281  return exp_p_xi_hat;
282 }
283 
284 /* ************************************************************************* */
286  const double x = p_.dot(other.p_);
287  // Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
288  // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
289  // We treat the special case 1 and -1 below
290  const double x2 = x * x;
291  const double z = 1 - x2;
292  double y;
294  if (x > 0) // first order expansion at x=1
295  y = 1.0 - (x - 1.0) / 3.0;
296  else // cop out
297  return Vector2(M_PI, 0.0);
298  } else {
299  // no special case
300  y = acos(x) / sqrt(z);
301  }
302  return basis().transpose() * y * (other.p_ - x * p_);
303 }
304 /* ************************************************************************* */
305 
306 } // namespace gtsam
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
static Point3 CalculateBestAxis(const Point3 &n)
Definition: Unit3.cpp:72
Vector3 p_
The location of the point on the unit sphere.
Definition: Unit3.h:46
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Scalar * y
friend std::ostream & operator<<(std::ostream &os, const Unit3 &pair)
Definition: Unit3.cpp:158
Eigen::Vector3d Vector3
Definition: Vector.h:43
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
static std::uniform_real_distribution uniform(0.0, 1.0)
std::optional< Matrix32 > B_
Cached basis.
Definition: Unit3.h:47
double dot(const Unit3 &q, OptionalJacobian< 1, 2 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Return dot product with q.
Definition: Unit3.cpp:174
Vector2 Point2
Definition: Point2.h:32
static std::mt19937 rng
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
Definition: Unit3.cpp:151
int n
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
#define M_PI
Definition: main.h:106
Jet< T, N > acos(const Jet< T, N > &f)
Definition: jet.h:432
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
std::optional< Matrix62 > H_B_
Cached basis derivative.
Definition: Unit3.h:48
Definition: BFloat16.h:88
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Vector2 localCoordinates(const Unit3 &s) const
The local coordinates function.
Definition: Unit3.cpp:285
static Unit3 Random(std::mt19937 &rng)
Definition: Unit3.cpp:55
static double epsilon
Definition: testRot3.cpp:37
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
void print(const std::string &s=std::string()) const
The print fuction.
Definition: Unit3.cpp:164
Matrix3 skew() const
Return skew-symmetric associated with 3D point on unit sphere.
Definition: Unit3.cpp:169
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
Array< int, Dynamic, 1 > v
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
Unit3()
Default constructor.
Definition: Unit3.h:64
Vector xi
Definition: testPose2.cpp:148
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 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
Eigen::Vector2d Vector2
Definition: Vector.h:42
ofstream os("timeSchurFactors.csv")
const Matrix32 & basis(OptionalJacobian< 6, 2 > H={}) const
Definition: Unit3.cpp:84
float * p
Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H={}) const
The retract function.
Definition: Unit3.cpp:255
Point3 point3(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector3 Point3
Definition: Point3.h:38
Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q={}) const
Definition: Unit3.cpp:199
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
double distance(const Unit3 &q, OptionalJacobian< 1, 2 > H={}) const
Distance between two directions.
Definition: Unit3.cpp:245
Vector2 errorVector(const Unit3 &q, OptionalJacobian< 2, 2 > H_p={}, OptionalJacobian< 2, 2 > H_q={}) const
Definition: Unit3.cpp:209
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
#define abs(x)
Definition: datatypes.h:17
2D Point


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:41