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

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:50:25