testUnit3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
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 testUnit3.cpp
14  * @date Feb 03, 2012
15  * @author Can Erdogan
16  * @author Frank Dellaert
17  * @author Alex Trevor
18  * @brief Tests the Unit3 class
19  */
20 
21 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/numericalDerivative.h>
23 #include <gtsam/base/serializationTestHelpers.h>
24 #include <gtsam/geometry/Unit3.h>
25 #include <gtsam/geometry/Rot3.h>
26 #include <gtsam/inference/Symbol.h>
27 #include <gtsam/nonlinear/ExpressionFactor.h>
28 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
29 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
30 
31 
32 #include <CppUnitLite/TestHarness.h>
33 
34 #include <cmath>
35 #include <random>
36 
37 using namespace std::placeholders;
38 using namespace gtsam;
39 using namespace std;
40 using gtsam::symbol_shorthand::U;
41 
42 GTSAM_CONCEPT_TESTABLE_INST(Unit3)
43 GTSAM_CONCEPT_MANIFOLD_INST(Unit3)
44 
45 //*******************************************************************************
46 Point3 point3_(const Unit3& p) {
47  return p.point3();
48 }
49 
50 TEST(Unit3, point3) {
51  const vector<Point3> ps{Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1),
52  Point3(1, 1, 0) / sqrt(2.0)};
53  Matrix actualH, expectedH;
54  for(Point3 p: ps) {
55  Unit3 s(p);
56  expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
57  EXPECT(assert_equal(p, s.point3(actualH), 1e-5));
58  EXPECT(assert_equal(expectedH, actualH, 1e-5));
59  }
60 }
61 
62 //*******************************************************************************
63 static Unit3 rotate_(const Rot3& R, const Unit3& p) {
64  return R * p;
65 }
66 
67 TEST(Unit3, rotate) {
68  Rot3 R = Rot3::Yaw(0.5);
69  Unit3 p(1, 0, 0);
70  Unit3 expected = Unit3(R.matrix().col(0));
71  Unit3 actual = R * p;
72  EXPECT(assert_equal(expected, actual, 1e-5));
73  Matrix actualH, expectedH;
74  // Use numerical derivatives to calculate the expected Jacobian
75  {
76  expectedH = numericalDerivative21(rotate_, R, p);
77  R.rotate(p, actualH, {});
78  EXPECT(assert_equal(expectedH, actualH, 1e-5));
79  }
80  {
81  expectedH = numericalDerivative22(rotate_, R, p);
82  R.rotate(p, {}, actualH);
83  EXPECT(assert_equal(expectedH, actualH, 1e-5));
84  }
85 }
86 
87 //*******************************************************************************
88 static Unit3 unrotate_(const Rot3& R, const Unit3& p) {
89  return R.unrotate(p);
90 }
91 
92 TEST(Unit3, unrotate) {
93  Rot3 R = Rot3::Yaw(-M_PI / 4.0);
94  Unit3 p(1, 0, 0);
95  Unit3 expected = Unit3(1, 1, 0);
96  Unit3 actual = R.unrotate(p);
97  EXPECT(assert_equal(expected, actual, 1e-5));
98 
99  Matrix actualH, expectedH;
100  // Use numerical derivatives to calculate the expected Jacobian
101  {
102  expectedH = numericalDerivative21(unrotate_, R, p);
103  R.unrotate(p, actualH, {});
104  EXPECT(assert_equal(expectedH, actualH, 1e-5));
105  }
106  {
107  expectedH = numericalDerivative22(unrotate_, R, p);
108  R.unrotate(p, {}, actualH);
109  EXPECT(assert_equal(expectedH, actualH, 1e-5));
110  }
111 }
112 
113 TEST(Unit3, dot) {
114  Unit3 p(1, 0.2, 0.3);
115  Unit3 q = p.retract(Vector2(0.5, 0));
116  Unit3 r = p.retract(Vector2(0.8, 0));
117  Unit3 t = p.retract(Vector2(0, 0.3));
118  EXPECT(assert_equal(1.0, p.dot(p), 1e-5));
119  EXPECT(assert_equal(0.877583, p.dot(q), 1e-5));
120  EXPECT(assert_equal(0.696707, p.dot(r), 1e-5));
121  EXPECT(assert_equal(0.955336, p.dot(t), 1e-5));
122 
123  // Use numerical derivatives to calculate the expected Jacobians
124  Matrix H1, H2;
125  std::function<double(const Unit3&, const Unit3&)> f =
126  std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, //
127  nullptr, nullptr);
128  {
129  p.dot(q, H1, H2);
130  EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-5));
131  EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-5));
132  }
133  {
134  p.dot(r, H1, H2);
135  EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-5));
136  EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-5));
137  }
138  {
139  p.dot(t, H1, H2);
140  EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-5));
141  EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-5));
142  }
143 }
144 
145 //*******************************************************************************
146 TEST(Unit3, error) {
147  Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
148  r = p.retract(Vector2(0.8, 0));
149  EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5));
150  EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
151  EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
152 
153  Matrix actual, expected;
154  // Use numerical derivatives to calculate the expected Jacobian
155  {
156  expected = numericalDerivative11<Vector2,Unit3>(
157  std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), q);
158  p.error(q, actual);
159  EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
160  }
161  {
162  expected = numericalDerivative11<Vector2,Unit3>(
163  std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), r);
164  p.error(r, actual);
165  EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
166  }
167 }
168 
169 //*******************************************************************************
170 TEST(Unit3, error2) {
171  Unit3 p(0.1, -0.2, 0.8);
172  Unit3 q = p.retract(Vector2(0.2, -0.1));
173  Unit3 r = p.retract(Vector2(0.8, 0));
174 
175  // Hard-coded as simple regression values
176  EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-5));
177  EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5));
178  EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));
179 
180  Matrix actual, expected;
181  // Use numerical derivatives to calculate the expected Jacobian
182  {
183  expected = numericalDerivative21<Vector2, Unit3, Unit3>(
184  std::bind(&Unit3::errorVector, std::placeholders::_1,
185  std::placeholders::_2, nullptr, nullptr),
186  p, q);
187  p.errorVector(q, actual, {});
188  EXPECT(assert_equal(expected, actual, 1e-5));
189  }
190  {
191  expected = numericalDerivative21<Vector2, Unit3, Unit3>(
192  std::bind(&Unit3::errorVector, std::placeholders::_1,
193  std::placeholders::_2, nullptr, nullptr),
194  p, r);
195  p.errorVector(r, actual, {});
196  EXPECT(assert_equal(expected, actual, 1e-5));
197  }
198  {
199  expected = numericalDerivative22<Vector2, Unit3, Unit3>(
200  std::bind(&Unit3::errorVector, std::placeholders::_1,
201  std::placeholders::_2, nullptr, nullptr),
202  p, q);
203  p.errorVector(q, {}, actual);
204  EXPECT(assert_equal(expected, actual, 1e-5));
205  }
206  {
207  expected = numericalDerivative22<Vector2, Unit3, Unit3>(
208  std::bind(&Unit3::errorVector, std::placeholders::_1,
209  std::placeholders::_2, nullptr, nullptr),
210  p, r);
211  p.errorVector(r, {}, actual);
212  EXPECT(assert_equal(expected, actual, 1e-5));
213  }
214 }
215 
216 //*******************************************************************************
217 TEST(Unit3, distance) {
218  Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
219  r = p.retract(Vector2(0.8, 0));
220  EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-5);
221  EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-5);
222  EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-5);
223 
224  Matrix actual, expected;
225  // Use numerical derivatives to calculate the expected Jacobian
226  {
227  expected = numericalGradient<Unit3>(
228  std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), q);
229  p.distance(q, actual);
230  EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
231  }
232  {
233  expected = numericalGradient<Unit3>(
234  std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), r);
235  p.distance(r, actual);
236  EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
237  }
238 }
239 
240 //*******************************************************************************
241 TEST(Unit3, localCoordinates0) {
242  Unit3 p;
243  Vector actual = p.localCoordinates(p);
244  EXPECT(assert_equal(Z_2x1, actual, 1e-5));
245 }
246 
247 TEST(Unit3, localCoordinates) {
248  {
249  Unit3 p, q;
250  Vector2 expected = Vector2::Zero();
251  Vector2 actual = p.localCoordinates(q);
252  EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5));
253  EXPECT(assert_equal(q, p.retract(expected), 1e-5));
254  }
255  {
256  Unit3 p, q(1, 6.12385e-21, 0);
257  Vector2 expected = Vector2::Zero();
258  Vector2 actual = p.localCoordinates(q);
259  EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-5));
260  EXPECT(assert_equal(q, p.retract(expected), 1e-5));
261  }
262  {
263  Unit3 p, q(-1, 0, 0);
264  Vector2 expected(M_PI, 0);
265  Vector2 actual = p.localCoordinates(q);
266  EXPECT(assert_equal(expected, actual, 1e-5));
267  EXPECT(assert_equal(q, p.retract(expected), 1e-5));
268  }
269  {
270  Unit3 p, q(0, 1, 0);
271  Vector2 expected(0,-M_PI_2);
272  Vector2 actual = p.localCoordinates(q);
273  EXPECT(assert_equal(expected, actual, 1e-5));
274  EXPECT(assert_equal(q, p.retract(expected), 1e-5));
275  }
276  {
277  Unit3 p, q(0, -1, 0);
278  Vector2 expected(0, M_PI_2);
279  Vector2 actual = p.localCoordinates(q);
280  EXPECT(assert_equal(expected, actual, 1e-5));
281  EXPECT(assert_equal(q, p.retract(expected), 1e-5));
282  }
283  {
284  Unit3 p(0,1,0), q(0,-1,0);
285  Vector2 actual = p.localCoordinates(q);
286  EXPECT(assert_equal(q, p.retract(actual), 1e-5));
287  }
288  {
289  Unit3 p(0,0,1), q(0,0,-1);
290  Vector2 actual = p.localCoordinates(q);
291  EXPECT(assert_equal(q, p.retract(actual), 1e-5));
292  }
293 
294  double twist = 1e-4;
295  {
296  Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0);
297  Vector2 actual = p.localCoordinates(q);
298  EXPECT(actual(0) < 1e-2);
299  EXPECT(actual(1) > M_PI - 1e-2)
300  }
301  {
302  Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0);
303  Vector2 actual = p.localCoordinates(q);
304  EXPECT(actual(0) < 1e-2);
305  EXPECT(actual(1) < -M_PI + 1e-2)
306  }
307 }
308 
309 //*******************************************************************************
310 // Wrapper to make basis return a Vector6 so we can test numerical derivatives.
311 Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) {
312  Matrix32 B = p.basis(H);
313  Vector6 B_vec;
314  B_vec << B.col(0), B.col(1);
315  return B_vec;
316 }
317 
318 TEST(Unit3, basis) {
319  Unit3 p(0.1, -0.2, 0.9);
320 
321  Matrix expected(3, 2);
322  expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958;
323 
324  Matrix62 actualH;
325  Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
326  std::bind(BasisTest, std::placeholders::_1, nullptr), p);
327 
328  // without H, first time
329  EXPECT(assert_equal(expected, p.basis(), 1e-6));
330 
331  // without H, cached
332  EXPECT(assert_equal(expected, p.basis(), 1e-6));
333 
334  // with H, first time
335  EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
336  EXPECT(assert_equal(expectedH, actualH, 1e-5));
337 
338  // with H, cached
339  EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
340  EXPECT(assert_equal(expectedH, actualH, 1e-5));
341 }
342 
343 //*******************************************************************************
345 TEST(Unit3, basis_derivatives) {
346  int num_tests = 100;
347  std::mt19937 rng(42);
348  for (int i = 0; i < num_tests; i++) {
349  Unit3 p = Unit3::Random(rng);
350 
351  Matrix62 actualH;
352  p.basis(actualH);
353 
354  Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
355  std::bind(BasisTest, std::placeholders::_1, nullptr), p);
356  EXPECT(assert_equal(expectedH, actualH, 1e-5));
357  }
358 }
359 
360 //*******************************************************************************
361 TEST(Unit3, retract) {
362  {
363  Unit3 p;
364  Vector2 v(0.5, 0);
365  Unit3 expected(0.877583, 0, 0.479426);
366  Unit3 actual = p.retract(v);
367  EXPECT(assert_equal(expected, actual, 1e-6));
368  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
369  }
370  {
371  Unit3 p;
372  Vector2 v(0, 0);
373  Unit3 actual = p.retract(v);
374  EXPECT(assert_equal(p, actual, 1e-6));
375  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
376  }
377 }
378 
379 //*******************************************************************************
380 TEST (Unit3, jacobian_retract) {
381  Matrix22 H;
382  Unit3 p;
383  std::function<Unit3(const Vector2&)> f =
384  std::bind(&Unit3::retract, p, std::placeholders::_1, nullptr);
385  {
386  Vector2 v (-0.2, 0.1);
387  p.retract(v, H);
388  Matrix H_expected_numerical = numericalDerivative11(f, v);
389  EXPECT(assert_equal(H_expected_numerical, H, 1e-5));
390  }
391  {
392  Vector2 v (0, 0);
393  p.retract(v, H);
394  Matrix H_expected_numerical = numericalDerivative11(f, v);
395  EXPECT(assert_equal(H_expected_numerical, H, 1e-5));
396  }
397 }
398 
399 //*******************************************************************************
400 TEST(Unit3, retract_expmap) {
401  Unit3 p;
402  Vector2 v((M_PI / 2.0), 0);
403  Unit3 expected(Point3(0, 0, 1));
404  Unit3 actual = p.retract(v);
405  EXPECT(assert_equal(expected, actual, 1e-5));
406  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-5));
407 }
408 
409 //*******************************************************************************
410 TEST(Unit3, Random) {
411  std::mt19937 rng(42);
412  // Check that means are all zero at least
413  Point3 expectedMean(0,0,0), actualMean(0,0,0);
414  for (size_t i = 0; i < 100; i++)
415  actualMean = actualMean + Unit3::Random(rng).point3();
416  actualMean = actualMean / 100;
417  EXPECT(assert_equal(expectedMean,actualMean,0.1));
418 }
419 
420 //*******************************************************************************
421 // New test that uses Unit3::Random
422 TEST(Unit3, localCoordinates_retract) {
423  std::mt19937 rng(42);
424  size_t numIterations = 10000;
425 
426  for (size_t i = 0; i < numIterations; i++) {
427  // Create two random Unit3s
428  const Unit3 s1 = Unit3::Random(rng);
429  const Unit3 s2 = Unit3::Random(rng);
430  // Check that they are not at opposite ends of the sphere, which is ill defined
431  if (s1.unitVector().dot(s2.unitVector())<-0.9) continue;
432 
433  // Check if the local coordinates and retract return consistent results.
434  Vector v12 = s1.localCoordinates(s2);
435  Unit3 actual_s2 = s1.retract(v12);
436  EXPECT(assert_equal(s2, actual_s2, 1e-5));
437  }
438 }
439 
440 //*************************************************************************
441 TEST (Unit3, FromPoint3) {
442  Matrix actualH;
443  Point3 point(1, -2, 3); // arbitrary point
444  Unit3 expected(point);
445  EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5));
446  Matrix expectedH = numericalDerivative11<Unit3, Point3>(
447  std::bind(Unit3::FromPoint3, std::placeholders::_1, nullptr), point);
448  EXPECT(assert_equal(expectedH, actualH, 1e-5));
449 }
450 
451 //*******************************************************************************
452 TEST(Unit3, ErrorBetweenFactor) {
453  std::vector<Unit3> data;
454  data.push_back(Unit3(1.0, 0.0, 0.0));
455  data.push_back(Unit3(0.0, 0.0, 1.0));
456 
457  NonlinearFactorGraph graph;
458  Values initial_values;
459 
460  // Add prior factors.
461  SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
462  for (size_t i = 0; i < data.size(); i++) {
463  graph.addPrior(U(i), data[i], R_prior);
464  }
465 
466  // Add process factors using the dot product error function.
467  SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01);
468  for (size_t i = 0; i < data.size() - 1; i++) {
469  Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector,
470  Expression<Unit3>(U(i + 1)));
471  graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
472  }
473 
474  // Add initial values. Since there is no identity, just pick something.
475  for (size_t i = 0; i < data.size(); i++) {
476  initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0));
477  }
478 
479  Values values = GaussNewtonOptimizer(graph, initial_values).optimize();
480 
481  // Check that the y-value is very small for each.
482  for (size_t i = 0; i < data.size(); i++) {
483  EXPECT(assert_equal(0.0, values.at<Unit3>(U(i)).unitVector().y(), 1e-3));
484  }
485 
486  // Check that the dot product between variables is close to 1.
487  for (size_t i = 0; i < data.size() - 1; i++) {
488  EXPECT(assert_equal(1.0, values.at<Unit3>(U(i)).dot(values.at<Unit3>(U(i + 1))), 1e-2));
489  }
490 }
491 
492 TEST(Unit3, CopyAssign) {
493  Unit3 p{1, 0.2, 0.3};
494 
495  EXPECT(p.error(p).isZero());
496 
497  p = Unit3{-1, 2, 8};
498  EXPECT(p.error(p).isZero());
499 }
500 
501 /* ************************************************************************* */
502 #if GTSAM_ENABLE_BOOST_SERIALIZATION
503 TEST(actualH, Serialization) {
504  Unit3 p(0, 1, 0);
505  EXPECT(serializationTestHelpers::equalsObj(p));
506  EXPECT(serializationTestHelpers::equalsXML(p));
507  EXPECT(serializationTestHelpers::equalsBinary(p));
508 }
509 #endif
510 
511 
512 /* ************************************************************************* */
513 int main() {
514  srand(time(nullptr));
515  TestResult tr;
516  return TestRegistry::runAllTests(tr);
517 }
518 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Rot2::unrotate
Point2 unrotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
Definition: Rot2.cpp:125
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
TestHarness.h
point3_
Point3 point3_(const Unit3 &p)
Definition: testUnit3.cpp:46
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:110
point3
static const Point3 point3(0.08, 0.08, 0.0)
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
rotate_
static Unit3 rotate_(const Rot3 &R, const Unit3 &p)
Definition: testUnit3.cpp:63
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:247
gtsam::Rot2::matrix
Matrix2 matrix() const
Definition: Rot2.cpp:100
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
Unit3.h
Rot3.h
3D rotation represented as a rotation matrix or quaternion
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:49
BasisTest
Vector6 BasisTest(const Unit3 &p, OptionalJacobian< 6, 2 > H)
Definition: testUnit3.cpp:311
gtsam::Expression
Definition: Expression.h:49
GaussNewtonOptimizer.h
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
numericalDerivative.h
Some functions to compute numerical derivatives.
data
int data[]
Definition: Map_placement_new.cpp:1
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:916
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
Symbol.h
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
time
#define time
Definition: timeAdaptAutoDiff.cpp:31
gtsam::Unit3::retract
Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H={}) const
The retract function.
Definition: Unit3.cpp:255
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
serializationTestHelpers.h
TestResult
Definition: TestResult.h:26
y
Scalar * y
Definition: level1_cplx_impl.h:124
M_PI_2
#define M_PI_2
Definition: mconf.h:118
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::Rot2::rotate
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
Definition: Rot2.cpp:115
gtsam::HybridValues::at
Vector & at(Key j)
Definition: HybridValues.cpp:129
gtsam
traits
Definition: ABC.h:17
error
static double error
Definition: testRot3.cpp:37
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
unrotate_
static Unit3 unrotate_(const Rot3 &R, const Unit3 &p)
Definition: testUnit3.cpp:88
gtsam::Values
Definition: Values.h:65
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
main
int main()
Definition: testUnit3.cpp:513
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:156
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:46
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
ps
int RealScalar int RealScalar int RealScalar RealScalar * ps
Definition: level1_cplx_impl.h:120
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
GTSAM_CONCEPT_MANIFOLD_INST
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
‍**
Definition: Manifold.h:180
ExpressionFactor.h
gtsam::testing::unrotate
P unrotate(const T &r, const P &pt)
Definition: lieProxies.h:50
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
U
@ U
Definition: testDecisionTree.cpp:342
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:121
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
Definition: Unit3.cpp:151
gtsam::symbol_shorthand::U
Key U(std::uint64_t j)
Definition: inference/Symbol.h:168
TEST
TEST(Unit3, point3)
Definition: testUnit3.cpp:50
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
align_3::t
Point2 t(10, 10)
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
Definition: lieProxies.h:47
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::Unit3::localCoordinates
Vector2 localCoordinates(const Unit3 &s) const
The local coordinates function.
Definition: Unit3.cpp:285
gtsam::NonlinearFactorGraph::addExpressionFactor
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Definition: NonlinearFactorGraph.h:187

gtsam
Author(s):
autogenerated on Wed May 28 2025 03:07:56