1/*
2 * testInvDepthFactorVariant1.cpp
3 *
4 * Created on: Apr 13, 2012
5 * Author: cbeall3
6 */
7
8#include <CppUnitLite/TestHarness.h>
9#include <gtsam_unstable/slam/InvDepthFactorVariant1.h>
10#include <gtsam/nonlinear/NonlinearEquality.h>
11#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
12#include <gtsam/nonlinear/NonlinearFactorGraph.h>
13#include <gtsam/inference/Symbol.h>
14#include <gtsam/geometry/PinholeCamera.h>
15#include <gtsam/geometry/Cal3_S2.h>
16#include <gtsam/geometry/Pose3.h>
17#include <gtsam/geometry/Point3.h>
18#include <gtsam/geometry/Point2.h>
19
20using namespace std;
21using namespace gtsam;
22
23/* ************************************************************************* */
24TEST( InvDepthFactorVariant1, optimize) {
25
26 // Create two poses looking in the x-direction
27 Pose3 pose1(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), Point3(0,0,1.0));
28 Pose3 pose2(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), Point3(0,0,1.5));
29
30 // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
31 Point3 landmark(5, 0, 1);
32
33 // Create image observations
34 Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
35 PinholeCamera<Cal3_S2> camera1(pose1, *K);
36 PinholeCamera<Cal3_S2> camera2(pose2, *K);
37 Point2 pixel1 = camera1.project(pw: landmark);
38 Point2 pixel2 = camera2.project(pw: landmark);
39
40 // Create expected landmark
41 double x = pose1.translation().x();
42 double y = pose1.translation().y();
43 double z = pose1.translation().z();
44 Point3 ray = landmark - pose1.translation();
45 double theta = atan2(y: ray.y(), x: ray.x());
46 double phi = atan2(y: ray.z(), x: sqrt(x: ray.x()*ray.x()+ray.y()*ray.y()));
47 double rho = 1./ray.norm();
48 Vector6 expected((Vector(6) << x, y, z, theta, phi, rho).finished());
49
50
51
52 // Create a factor graph with two inverse depth factors and two pose priors
53 Key poseKey1(1);
54 Key poseKey2(2);
55 Key landmarkKey(100);
56 SharedNoiseModel sigma(noiseModel::Unit::Create(dim: 2));
57 NonlinearFactor::shared_ptr factor1(new NonlinearEquality<Pose3>(poseKey1, pose1, 100000));
58 NonlinearFactor::shared_ptr factor2(new NonlinearEquality<Pose3>(poseKey2, pose2, 100000));
59 NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant1(poseKey1, landmarkKey, pixel1, K, sigma));
60 NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant1(poseKey2, landmarkKey, pixel2, K, sigma));
61 NonlinearFactorGraph graph;
62 graph.push_back(factor: factor1);
63 graph.push_back(factor: factor2);
64 graph.push_back(factor: factor3);
65 graph.push_back(factor: factor4);
66
67 // Create a values with slightly incorrect initial conditions
68 Values values;
69 values.insert(j: poseKey1, val: pose1.retract(v: (Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30).finished()));
70 values.insert(j: poseKey2, val: pose2.retract(v: (Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished()));
71 values.insert(j: landmarkKey, val: Vector6(expected + (Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05).finished()));
72
73 // Optimize the graph to recover the actual landmark position
74 LevenbergMarquardtParams params;
75 Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
76
77// Vector6 actual = result.at<Vector6>(landmarkKey);
78// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
79// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
80// pose1.print("Pose1 Truth:\n");
81// cout << endl << endl;
82// values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
83// result.at<Pose3>(poseKey2).print("Pose2 After:\n");
84// pose2.print("Pose2 Truth:\n");
85// cout << endl << endl;
86// values.at<Vector6>(landmarkKey).print("Landmark Before:\n");
87// result.at<Vector6>(landmarkKey).print("Landmark After:\n");
88// expected.print("Landmark Truth:\n");
89// cout << endl << endl;
90
91 // Calculate world coordinates of landmark versions
92 Point3 world_landmarkBefore(0,0,0);
93 {
94 Vector6 landmarkBefore = values.at<Vector6>(j: landmarkKey);
95 double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2);
96 double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5);
97 world_landmarkBefore = Point3(x, y, z) + Point3(cos(x: theta)*cos(x: phi)/rho, sin(x: theta)*cos(x: phi)/rho, sin(x: phi)/rho);
98 }
99 Point3 world_landmarkAfter(0,0,0);
100 {
101 Vector6 landmarkAfter = result.at<Vector6>(j: landmarkKey);
102 double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2);
103 double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5);
104 world_landmarkAfter = Point3(x, y, z) + Point3(cos(x: theta)*cos(x: phi)/rho, sin(x: theta)*cos(x: phi)/rho, sin(x: phi)/rho);
105 }
106
107// world_landmarkBefore.print("World Landmark Before:\n");
108// world_landmarkAfter.print("World Landmark After:\n");
109// landmark.print("World Landmark Truth:\n");
110// cout << endl << endl;
111
112
113
114 // Test that the correct landmark parameters have been recovered
115 // However, since this is an over-parameterization, there can be
116 // many 6D landmark values that equate to the same 3D world position
117 // Instead, directly test the recovered 3D landmark position
118 EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9));
119
120 // Frank asks: why commented out?
121 //Vector6 actual = result.at<Vector6>(landmarkKey);
122 //EXPECT(assert_equal(expected, actual, 1e-9));
123}
124
125
126/* ************************************************************************* */
127int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
128/* ************************************************************************* */
129