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/InvDepthFactorVariant2.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( InvDepthFactorVariant2, 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), gtsam::Point3(0,0,1.0));
28 Pose3 pose2(Rot3::Ypr(y: -M_PI/2, p: 0., r: -M_PI/2), gtsam::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 Point3 referencePoint = pose1.translation();
42 Point3 ray = landmark - referencePoint;
43 double theta = atan2(y: ray.y(), x: ray.x());
44 double phi = atan2(y: ray.z(), x: sqrt(x: ray.x()*ray.x()+ray.y()*ray.y()));
45 double rho = 1./ray.norm();
46 Vector3 expected((Vector(3) << theta, phi, rho).finished());
47
48
49
50 // Create a factor graph with two inverse depth factors and two pose priors
51 Key poseKey1(1);
52 Key poseKey2(2);
53 Key landmarkKey(100);
54 SharedNoiseModel sigma(noiseModel::Unit::Create(dim: 2));
55 NonlinearFactor::shared_ptr factor1(new NonlinearEquality<Pose3>(poseKey1, pose1, 100000));
56 NonlinearFactor::shared_ptr factor2(new NonlinearEquality<Pose3>(poseKey2, pose2, 100000));
57 NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant2(poseKey1, landmarkKey, pixel1, K, referencePoint, sigma));
58 NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant2(poseKey2, landmarkKey, pixel2, K, referencePoint, sigma));
59 NonlinearFactorGraph graph;
60 graph.push_back(factor: factor1);
61 graph.push_back(factor: factor2);
62 graph.push_back(factor: factor3);
63 graph.push_back(factor: factor4);
64
65 // Create a values with slightly incorrect initial conditions
66 Values values;
67 values.insert(j: poseKey1, val: pose1.retract(v: (Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30).finished()));
68 values.insert(j: poseKey2, val: pose2.retract(v: (Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished()));
69 values.insert(j: landmarkKey, val: Vector3(expected + Vector3(+0.02, -0.04, +0.05)));
70
71 // Optimize the graph to recover the actual landmark position
72 LevenbergMarquardtParams params;
73 Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
74 Vector3 actual = result.at<Vector3>(j: landmarkKey);
75
76// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
77// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
78// pose1.print("Pose1 Truth:\n");
79// std::cout << std::endl << std::endl;
80// values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
81// result.at<Pose3>(poseKey2).print("Pose2 After:\n");
82// pose2.print("Pose2 Truth:\n");
83// std::cout << std::endl << std::endl;
84// values.at<Vector3>(landmarkKey).print("Landmark Before:\n");
85// result.at<Vector3>(landmarkKey).print("Landmark After:\n");
86// expected.print("Landmark Truth:\n");
87// std::cout << std::endl << std::endl;
88
89 // Calculate world coordinates of landmark versions
90 Point3 world_landmarkBefore(0,0,0);
91 {
92 Vector3 landmarkBefore = values.at<Vector3>(j: landmarkKey);
93 double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2);
94 world_landmarkBefore = referencePoint + Point3(cos(x: theta)*cos(x: phi)/rho, sin(x: theta)*cos(x: phi)/rho, sin(x: phi)/rho);
95 }
96 Point3 world_landmarkAfter(0,0,0);
97 {
98 Vector3 landmarkAfter = result.at<Vector3>(j: landmarkKey);
99 double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2);
100 world_landmarkAfter = referencePoint + Point3(cos(x: theta)*cos(x: phi)/rho, sin(x: theta)*cos(x: phi)/rho, sin(x: phi)/rho);
101 }
102
103// world_landmarkBefore.print("World Landmark Before:\n");
104// world_landmarkAfter.print("World Landmark After:\n");
105// landmark.print("World Landmark Truth:\n");
106// std::cout << std::endl << std::endl;
107
108 // Test that the correct landmark parameters have been recovered
109 EXPECT(assert_equal((Vector)expected, actual, 1e-9));
110}
111
112
113/* ************************************************************************* */
114int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
115/* ************************************************************************* */
116