1/*
2 * testInvDepthFactorVariant3.cpp
3 *
4 * Created on: Apr 13, 2012
5 * Author: cbeall3
6 */
7
8#include <CppUnitLite/TestHarness.h>
9#include <gtsam_unstable/slam/InvDepthFactorVariant3.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( InvDepthFactorVariant3, 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 landmark_p1 = pose1.transformTo(point: landmark);
42 // landmark_p1.print("Landmark in Pose1 Frame:\n");
43 double theta = atan2(y: landmark_p1.x(), x: landmark_p1.z());
44 double phi = atan2(y: landmark_p1.y(), x: sqrt(x: landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
45 double rho = 1./landmark_p1.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 InvDepthFactorVariant3a(poseKey1, landmarkKey, pixel1, K, sigma));
58 NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant3b(poseKey1, poseKey2, landmarkKey, pixel2, K, 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
77
78 // Test that the correct landmark parameters have been recovered
79 EXPECT(assert_equal((Vector)expected, actual, 1e-9));
80}
81
82
83/* ************************************************************************* */
84int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
85/* ************************************************************************* */
86