testSimilarity3.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 
20 #include <gtsam/base/Testable.h>
22 #include <gtsam/base/testLie.h>
23 #include <gtsam/geometry/Pose3.h>
25 #include <gtsam/inference/Symbol.h>
29 #include <gtsam/nonlinear/Values.h>
31 
32 #include <functional>
33 
34 using namespace std::placeholders;
35 using namespace gtsam;
36 using namespace std;
38 
40 
41 static const Point3 P(0.2, 0.7, -2);
42 static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
43 static const double s = 4;
44 static const Similarity3 id;
45 static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
46 static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1),
47  Point3(3.5, -8.2, 4.2), 1);
48 static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
49 static const Similarity3 T4(R, P, s);
50 static const Similarity3 T5(R, P, 10);
51 static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
52 
53 const double degree = M_PI / 180;
54 
55 //******************************************************************************
56 TEST(Similarity3, Concepts) {
58  GTSAM_CONCEPT_ASSERT(IsManifold<Similarity3 >);
60 }
61 
62 //******************************************************************************
63 TEST(Similarity3, Constructors) {
64  Similarity3 sim3_Construct1;
65  Similarity3 sim3_Construct2(s);
66  Similarity3 sim3_Construct3(R, P, s);
67  Similarity3 sim4_Construct4(R.matrix(), P, s);
68 }
69 
70 //******************************************************************************
71 TEST(Similarity3, Getters) {
72  Similarity3 sim3_default;
73  EXPECT(assert_equal(Rot3(), sim3_default.rotation()));
74  EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation()));
75  EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9);
76 
77  Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
78  EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation()));
79  EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation()));
80  EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9);
81 }
82 
83 //******************************************************************************
84 TEST(Similarity3, AdjointMap) {
85  const Matrix4 T = T2.matrix();
86  // Check Ad with actual definition
87  Vector7 delta;
88  delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
89  Matrix4 W = Similarity3::wedge(delta);
90  Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta);
91  EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9));
92 }
93 
94 //******************************************************************************
96  Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
97  Matrix3 Re; // some values from matlab
98  Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120;
99  Vector3 te(-9.8472, 59.7640, 10.2125);
100  Similarity3 expected(Re, te, 1.0 / 7.0);
101  EXPECT(assert_equal(expected, sim3.inverse(), 1e-4));
102  EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8));
103 
104  // test lie group inverse
105  Matrix H1, H2;
106  EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4));
107  EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8));
108 }
109 
110 //******************************************************************************
111 TEST(Similarity3, Multiplication) {
112  Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
113  Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11);
114  Matrix3 re;
115  re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530;
116  Vector3 te(-13.6797, 3.2441, -5.7794);
117  Similarity3 expected(re, te, 77);
118  EXPECT(assert_equal(expected, test1 * test2, 1e-2));
119 }
120 
121 //******************************************************************************
122 TEST(Similarity3, Manifold) {
123  EXPECT_LONGS_EQUAL(7, Similarity3::Dim());
124  Vector z = Vector7::Zero();
125  Similarity3 sim;
126  EXPECT(sim.retract(z) == sim);
127 
128  Vector7 v = Vector7::Zero();
129  v(6) = 2;
130  Similarity3 sim2;
131  EXPECT(sim2.retract(z) == sim2);
132 
133  EXPECT(assert_equal(z, sim2.localCoordinates(sim)));
134 
135  Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1);
136  Vector v3(7);
137  v3 << 0, 0, 0, 1, 2, 3, 0;
138  EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
139 
140  Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1);
141 
142  Vector vlocal = sim.localCoordinates(other);
143 
144  EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
145 
146  Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1);
147  Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
148 
149  Vector vlocal2 = sim.localCoordinates(other2);
150 
151  EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2));
152 
153  // TODO add unit tests for retract and localCoordinates
154 }
155 
156 //******************************************************************************
157 TEST( Similarity3, retract_first_order) {
158  Similarity3 id;
159  Vector v = Z_7x1;
160  v(0) = 0.3;
161  EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2));
162 // v(3) = 0.2;
163 // v(4) = 0.7;
164 // v(5) = -2;
165 // EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2));
166 }
167 
168 //******************************************************************************
169 TEST(Similarity3, localCoordinates_first_order) {
170  Vector7 d12 = Vector7::Constant(0.1);
171  d12(6) = 1.0;
172  Similarity3 t1 = T1, t2 = t1.retract(d12);
173  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
174 }
175 
176 //******************************************************************************
177 TEST(Similarity3, manifold_first_order) {
178  Similarity3 t1 = T1;
179  Similarity3 t2 = T3;
181  Vector d12 = t1.localCoordinates(t2);
182  EXPECT(assert_equal(t2, t1.retract(d12)));
183  Vector d21 = t2.localCoordinates(t1);
184  EXPECT(assert_equal(t1, t2.retract(d21)));
185 }
186 
187 //******************************************************************************
188 // Return as a 4*4 Matrix
190  Matrix4 expected;
191  expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5;
192  Matrix4 actual = T6.matrix();
193  EXPECT(assert_equal(expected, actual));
194 }
195 
196 //*****************************************************************************
197 // Exponential and log maps
198 TEST(Similarity3, ExpLogMap) {
199  Vector7 delta;
200  delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
201  Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta));
202  EXPECT(assert_equal(delta, actual));
203 
204  Vector7 zeros;
205  zeros << 0, 0, 0, 0, 0, 0, 0;
206  Vector7 logIdentity = Similarity3::Logmap(Similarity3::Identity());
207  EXPECT(assert_equal(zeros, logIdentity));
208 
209  Similarity3 expZero = Similarity3::Expmap(zeros);
210  Similarity3 ident = Similarity3::Identity();
211  EXPECT(assert_equal(expZero, ident));
212 
213  // Compare to matrix exponential, using expm in Lie.h
214  EXPECT(
215  assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
216 }
217 
218 //******************************************************************************
219 // Group action on Point3 (with simpler transform)
220 TEST(Similarity3, GroupAction) {
221  EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0)));
222  EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0)));
223 
224  // Test group action on R^4 via matrix representation
225  Vector4 qh;
226  qh << 1, 0, 0, 1;
227  Vector4 ph;
228  ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0)
229  EXPECT(assert_equal((Vector )ph, T6.matrix() * qh));
230 
231  // Test some more...
232  Point3 pa = Point3(1, 0, 0);
233  Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0);
234  Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0);
235  EXPECT(assert_equal(Point3(2, 2, 3), Ta.transformFrom(pa)));
236  EXPECT(assert_equal(Point3(4, 4, 6), Tb.transformFrom(pa)));
237 
238  Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0);
239  Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0);
240  EXPECT(assert_equal(Point3(1, 3, 3), Tc.transformFrom(pa)));
241  EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa)));
242 
243  // Test derivative
244  // Use lambda to resolve overloaded method
245  std::function<Point3(const Similarity3&, const Point3&)>
246  f = [](const Similarity3& S, const Point3& p){ return S.transformFrom(p); };
247 
248  Point3 q(1, 2, 3);
249  for (const auto& T : { T1, T2, T3, T4, T5, T6 }) {
250  Point3 q(1, 0, 0);
251  Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q);
252  Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T, q);
253  Matrix actualH1, actualH2;
254  T.transformFrom(q, actualH1, actualH2);
255  EXPECT(assert_equal(H1, actualH1));
256  EXPECT(assert_equal(H2, actualH2));
257  }
258 }
259 
260 //******************************************************************************
261 // Group action on Pose3
262 // Estimate Sim(3) object "aSb" from pose pairs {(aTi, bTi)}
263 // In the example below, let the "a" frame be the "world" frame below,
264 // and let the "b" frame be the "egovehicle" frame.
265 // Suppose within the egovehicle frame, we know the poses of two objects "o1" and "o2"
266 TEST(Similarity3, GroupActionPose3) {
267  // Suppose we know the pose of the egovehicle in the world frame
268  Similarity3 wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
269 
270  // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
271  // Suppose they are 3d cuboids detected by onboard sensor, in the egovehicle frame
272  Pose3 eTo1(Rot3(), Point3(0, 0, 0));
273  Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
274 
275  // Create destination poses (two objects now living in the world/city "w" frame)
276  // Desired to place the objects into the world frame for tracking
277  Pose3 expected_wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
278  Pose3 expected_wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
279 
280  // objects now live in the world frame, instead of in the egovehicle frame
281  EXPECT(assert_equal(expected_wTo1, wSe.transformFrom(eTo1)));
282  EXPECT(assert_equal(expected_wTo2, wSe.transformFrom(eTo2)));
283 }
284 
285 // Test left group action compatibility.
286 // cSa*Ta = cSb*bSa*Ta
287 TEST(Similarity3, GroupActionPose3_Compatibility) {
288  Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
289  Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0);
290  Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0);
291 
292  // Create poses
293  Pose3 Ta1(Rot3(), Point3(0, 0, 0));
294  Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
295  Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
296  Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
297  Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12));
298  Pose3 Tc2(Rot3(0, 0, -1, 0, -1, 0, -1, 0, 0), Point3(0, 6, 12));
299 
300  EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1)));
301  EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2)));
302 
303  EXPECT(assert_equal(cSa.transformFrom(Ta1), cSb.transformFrom(Tb1)));
304  EXPECT(assert_equal(cSa.transformFrom(Ta2), cSb.transformFrom(Tb2)));
305 }
306 
307 //******************************************************************************
308 // Align with Point3 Pairs
309 TEST(Similarity3, AlignPoint3_1) {
310  Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);
311 
312  Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4);
313 
314  Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
315  Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
316  Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
317 
318  vector<Point3Pair> correspondences{ab1, ab2, ab3};
319 
320  Similarity3 actual_aSb = Similarity3::Align(correspondences);
321  EXPECT(assert_equal(expected_aSb, actual_aSb));
322 }
323 
324 TEST(Similarity3, AlignPoint3_2) {
325  Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0);
326 
327  Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0);
328 
329  Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
330  Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
331  Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
332 
333  vector<Point3Pair> correspondences{ab1, ab2, ab3};
334 
335  Similarity3 actual_aSb = Similarity3::Align(correspondences);
336  EXPECT(assert_equal(expected_aSb, actual_aSb));
337 }
338 
339 TEST(Similarity3, AlignPoint3_3) {
340  Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);
341 
342  Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30);
343 
344  Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
345  Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
346  Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
347 
348  vector<Point3Pair> correspondences{ab1, ab2, ab3};
349 
350  Similarity3 actual_aSb = Similarity3::Align(correspondences);
351  EXPECT(assert_equal(expected_aSb, actual_aSb));
352 }
353 
354 //******************************************************************************
355 // Align with Pose3 Pairs
356 TEST(Similarity3, AlignPose3) {
357  Similarity3 expected_wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
358 
359  // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
360  // Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
361  Pose3 eTo1(Rot3(), Point3(0, 0, 0));
362  Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
363 
364  // Create destination poses (same two objects, but instead living in the world/city "w" frame)
365  Pose3 wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
366  Pose3 wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
367 
368  Pose3Pair wTe1(make_pair(wTo1, eTo1));
369  Pose3Pair wTe2(make_pair(wTo2, eTo2));
370 
371  vector<Pose3Pair> correspondences{wTe1, wTe2};
372 
373  // Cayley transform cannot accommodate 180 degree rotations,
374  // hence we only test for Expmap
375 #ifdef GTSAM_ROT3_EXPMAP
376  // Recover the transformation wSe (i.e. world_S_egovehicle)
377  Similarity3 actual_wSe = Similarity3::Align(correspondences);
378  EXPECT(assert_equal(expected_wSe, actual_wSe));
379 #endif
380 }
381 
382 //******************************************************************************
383 // Test very simple prior optimization example
384 TEST(Similarity3, Optimization) {
385  // Create a PriorFactor with a Sim3 prior
386  Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
387  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
388  Symbol key('x', 1);
389 
390  // Create graph
393 
394  // Create initial estimate with Identity transform
395  Values initial;
396  initial.insert<Similarity3>(key, Similarity3());
397 
398  // Optimize
399  Values result;
401  params.setVerbosityLM("TRYCONFIG");
403 
404  // After optimization, result should be prior
406 }
407 
408 //******************************************************************************
409 // Test optimization with both Prior and BetweenFactors
410 TEST(Similarity3, Optimization2) {
412  Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0),
413  1.0);
414  Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0),
415  Point3(sqrt(8) * 0.9, 0, 0), 1.0);
416  Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0),
417  Point3(sqrt(32) * 0.8, 0, 0), 1.0);
418  Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0),
419  Point3(6 * 0.7, 0, 0), 1.0);
420  Similarity3 loop = Similarity3(1.42);
421 
422  //prior.print("Goal Transform");
423  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7,
424  0.01);
425  SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
426  (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
427  SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
428  (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
429  BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
430  BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
431  BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
432  BetweenFactor<Similarity3> b4(X(4), X(5), m4, betweenNoise);
433  BetweenFactor<Similarity3> lc(X(5), X(1), loop, betweenNoise2);
434 
435  // Create graph
437  graph.addPrior(X(1), prior, model); // Prior !
438  graph.push_back(b1);
439  graph.push_back(b2);
440  graph.push_back(b3);
441  graph.push_back(b4);
442  graph.push_back(lc);
443 
444  //graph.print("Full Graph\n");
445  Values initial;
446  initial.insert<Similarity3>(X(1), Similarity3());
447  initial.insert<Similarity3>(X(2),
448  Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1));
449  initial.insert<Similarity3>(X(3),
450  Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
451  initial.insert<Similarity3>(X(4),
452  Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3));
453  initial.insert<Similarity3>(X(5),
454  Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0));
455 
456  //initial.print("Initial Estimate\n");
457 
458  Values result;
460  //result.print("Optimized Estimate\n");
461  Similarity3 p1, p2, p3, p4, p5;
462  p1 = result.at<Similarity3>(X(1));
463  p2 = result.at<Similarity3>(X(2));
464  p3 = result.at<Similarity3>(X(3));
465  p4 = result.at<Similarity3>(X(4));
466  p5 = result.at<Similarity3>(X(5));
467 
468  //p1.print("Similarity1");
469  //p2.print("Similarity2");
470  //p3.print("Similarity3");
471  //p4.print("Similarity4");
472  //p5.print("Similarity5");
473 
474  Similarity3 expected(0.7);
476 }
477 
478 //******************************************************************************
479 // Align points (p,q) assuming that p = T*q + noise
480 TEST(Similarity3, AlignScaledPointClouds) {
481 // Create ground truth
482  Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0);
483 
484  // Create transformed cloud (noiseless)
485 // Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3;
486 
487  // Create an unknown expression
488  Expression<Similarity3> unknownT(0); // use key 0
489 
490  // Create constant expressions for the ground truth points
491  Expression<Point3> q1_(q1), q2_(q2), q3_(q3);
492 
493  // Create prediction expressions
494  Expression<Point3> predict1(unknownT, &Similarity3::transformFrom, q1_);
495  Expression<Point3> predict2(unknownT, &Similarity3::transformFrom, q2_);
496  Expression<Point3> predict3(unknownT, &Similarity3::transformFrom, q3_);
497 
499 // ExpressionFactorGraph graph;
500 // graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1|
501 // graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2|
502 // graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3|
503 }
504 
505 //******************************************************************************
506 TEST(Similarity3 , Invariants) {
507  Similarity3 id;
508 
509  EXPECT(check_group_invariants(id, id));
510  EXPECT(check_group_invariants(id, T3));
511  EXPECT(check_group_invariants(T2, id));
512  EXPECT(check_group_invariants(T2, T3));
513 
514  EXPECT(check_manifold_invariants(id, id));
515  EXPECT(check_manifold_invariants(id, T3));
516  EXPECT(check_manifold_invariants(T2, id));
517  EXPECT(check_manifold_invariants(T2, T3));
518 }
519 
520 //******************************************************************************
521 TEST(Similarity3 , LieGroupDerivatives) {
522  Similarity3 id;
523 
528 }
529 
530 //******************************************************************************
531 int main() {
532  TestResult tr;
533  return TestRegistry::runAllTests(tr);
534 }
535 //******************************************************************************
536 
gtsam::Similarity3::scale
double scale() const
Return the scale.
Definition: Similarity3.h:192
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Pose3Pair
std::pair< Pose3, Pose3 > Pose3Pair
Definition: Pose3.h:424
gtsam::wedge
Matrix wedge(const Vector &x)
gtsam::Point3Pair
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
Similarity3.h
Implementation of Similarity3 transform.
simple_graph::b1
Vector2 b1(2, -1)
P
static const Point3 P(0.2, 0.7, -2)
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
p5
KeyInt p5(x5, 5)
TestHarness.h
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
simple_graph::b2
Vector2 b2(4, -5)
gtsam::Similarity3::AdjointMap
Matrix7 AdjointMap() const
Project from one tangent space to another.
Definition: Similarity3.cpp:206
m1
Matrix3d m1
Definition: IOFormat.cpp:2
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
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
testLie.h
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::IsGroup
Definition: Group.h:42
gtsam::Similarity3::inverse
Similarity3 inverse() const
Return the inverse.
Definition: Similarity3.cpp:131
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
s
static const double s
Definition: testSimilarity3.cpp:43
gtsam::Expression
Definition: Expression.h:47
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
m2
MatrixType m2(n_dims)
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
id
static const Similarity3 id
Definition: testSimilarity3.cpp:44
gtsam::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
numericalDerivative.h
Some functions to compute numerical derivatives.
T4
static const Similarity3 T4(R, P, s)
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::Similarity3::translation
Point3 translation() const
Return a GTSAM translation.
Definition: Similarity3.h:189
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
BetweenFactor.h
TEST
TEST(Similarity3, Concepts)
Definition: testSimilarity3.cpp:56
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
T6
static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2)
gtsam::Pose3
Definition: Pose3.h:37
gtsam::IsLieGroup
Definition: Lie.h:260
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
ExpressionFactorGraph.h
Factor graph that supports adding ExpressionFactors directly.
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
CHECK_LIE_GROUP_DERIVATIVES
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
Definition: testLie.h:82
T5
static const Similarity3 T5(R, P, 10)
Symbol.h
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Similarity3::transformFrom
Point3 transformFrom(const Point3 &p, OptionalJacobian< 3, 7 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Action on a point p is s*(R*p+t)
Definition: Similarity3.cpp:137
Eigen::Triplet< double >
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
simple_graph::b3
Vector2 b3(3, -6)
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
p4
KeyInt p4(x4, 4)
main
int main()
Definition: testSimilarity3.cpp:531
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
m3
static const DiscreteKey m3(M(3), 2)
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
gtsam::symbol_shorthand::W
Key W(std::uint64_t j)
Definition: inference/Symbol.h:170
gtsam::Similarity3::rotation
Rot3 rotation() const
Return a GTSAM rotation.
Definition: Similarity3.h:186
std
Definition: BFloat16.h:88
T2
static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1)
origin
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 origin
Definition: gnuplot_common_settings.hh:45
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Similarity3::matrix
Matrix4 matrix() const
Calculate 4*4 matrix group equivalent.
Definition: Similarity3.cpp:284
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
initial
Definition: testScenarioRunner.cpp:148
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
different_sigmas::prior
const auto prior
Definition: testHybridBayesNet.cpp:238
gtsam::noiseModel::Isotropic::shared_ptr
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:559
test2
void test2(OptionalJacobian<-1,-1 > H={})
Definition: testOptionalJacobian.cpp:123
v3
Vector v3
Definition: testSerializationBase.cpp:40
gtsam::internal::Align
static Similarity2 Align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, const Point2Pair &centroids)
This method estimates the similarity transform from differences point pairs, given a known or estimat...
Definition: Similarity2.cpp:74
M_PI
#define M_PI
Definition: mconf.h:117
T1
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
degree
const double degree
Definition: testSimilarity3.cpp:53
R
static const Rot3 R
Definition: testSimilarity3.cpp:42
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
T3
static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1)
gtsam::Similarity3
Definition: Similarity3.h:36
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose
S
DiscreteKey S(1, 2)
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:36