34 using namespace std::placeholders;
35 using namespace gtsam;
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;
47 Point3(3.5, -8.2, 4.2), 1);
88 delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
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);
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);
128 Vector7
v = Vector7::Zero();
137 v3 << 0, 0, 0, 1, 2, 3, 0;
147 Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
170 Vector7 d12 = Vector7::Constant(0.1);
191 expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5;
200 delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
205 zeros << 0, 0, 0, 0, 0, 0, 0;
206 Vector7 logIdentity = Similarity3::Logmap(Similarity3::Identity());
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);
273 Pose3 eTo2(
Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1),
Point3(4, 0, 0));
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));
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));
318 vector<Point3Pair> correspondences{ab1, ab2, ab3};
333 vector<Point3Pair> correspondences{ab1, ab2, ab3};
348 vector<Point3Pair> correspondences{ab1, ab2, ab3};
362 Pose3 eTo2(
Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1),
Point3(4, 0, 0));
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));
371 vector<Pose3Pair> correspondences{wTe1, wTe2};
375 #ifdef GTSAM_ROT3_EXPMAP
401 params.setVerbosityLM(
"TRYCONFIG");
419 Point3(6 * 0.7, 0, 0), 1.0);
426 (
Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
428 (
Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
482 Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0);
509 EXPECT(check_group_invariants(
id,
id));
510 EXPECT(check_group_invariants(
id,
T3));
511 EXPECT(check_group_invariants(
T2,
id));
514 EXPECT(check_manifold_invariants(
id,
id));
515 EXPECT(check_manifold_invariants(
id,
T3));
516 EXPECT(check_manifold_invariants(
T2,
id));