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
392  graph.addPrior(key, prior, model);
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");
402  result = LevenbergMarquardtOptimizer(graph, initial).optimize();
403 
404  // After optimization, result should be prior
405  EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
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;
459  result = LevenbergMarquardtOptimizer(graph, initial).optimize();
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);
475  EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));
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 
static const Point3 P(0.2, 0.7, -2)
const gtsam::Symbol key('X', 0)
static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1)
static const Rot3 R
virtual const Values & optimize()
Rot3 rotation() const
Return a GTSAM rotation.
Definition: Similarity3.h:185
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
static Point3 p3
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
A non-templated config holding any types of Manifold-group elements.
Vector3f p1
Factor Graph consisting of non-linear factors.
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
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:971
KeyInt p4(x4, 4)
MatrixType m2(n_dims)
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
TEST(Similarity3, Concepts)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define M_PI
Definition: main.h:106
Vector2 b2(4, -5)
Pose2_ Expmap(const Vector3_ &xi)
Similarity3 inverse() const
Return the inverse.
Values initial
Definition: BFloat16.h:88
Key W(std::uint64_t j)
Some functions to compute numerical derivatives.
DiscreteKey S(1, 2)
const double degree
NonlinearFactorGraph graph
void test2(OptionalJacobian<-1,-1 > H={})
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
int main()
static const Similarity3 id
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Matrix7 AdjointMap() const
Project from one tangent space to another.
static const Similarity3 T4(R, P, s)
static const SmartProjectionParams params
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
Definition: testLie.h:82
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:136
static const double s
Vector v3
Matrix wedge(const Vector &x)
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
Factor graph that supports adding ExpressionFactors directly.
Matrix3d m1
Definition: IOFormat.cpp:2
Point3 translation() const
Return a GTSAM translation.
Definition: Similarity3.h:188
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2)
Eigen::Triplet< double > T
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EIGEN_DEVICE_FUNC const Scalar & q
KeyInt p5(x5, 5)
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:542
Matrix4 matrix() const
Calculate 4*4 matrix group equivalent.
Point3 transformFrom(const Point3 &p, OptionalJacobian< 3, 7 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Action on a point p is s*(R*p+t)
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
Vector2 b3(3, -6)
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
Matrix3 matrix() const
Definition: Rot3M.cpp:218
float * p
void setVerbosityLM(const std::string &s)
static const Similarity3 T5(R, P, 10)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
static Point3 p2
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector3 Point3
Definition: Point3.h:38
std::pair< Pose3, Pose3 > Pose3Pair
Definition: Pose3.h:424
static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1)
#define X
Definition: icosphere.cpp:20
Implementation of Similarity3 transform.
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
double scale() const
Return the scale.
Definition: Similarity3.h:191
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
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
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42
Vector2 b1(2, -1)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:31