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
12/**
13 * @file timeCameraExpression.cpp
14 * @brief time CalibratedCamera derivatives
15 * @author Frank Dellaert
16 * @date October 3, 2014
17 */
18
19#include <gtsam/slam/expressions.h>
20#include <gtsam/nonlinear/ExpressionFactor.h>
21#include <gtsam/slam/ProjectionFactor.h>
22#include <gtsam/slam/GeneralSFMFactor.h>
23#include <gtsam/geometry/Pose3.h>
24#include <gtsam/geometry/Cal3_S2.h>
25#include "timeLinearize.h"
26
27using namespace std;
28using namespace gtsam;
29
30#define time timeSingleThreaded
31
32std::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
33
34Point2 myProject(const Pose3& pose, const Point3& point,
35 OptionalJacobian<2,6> H1, OptionalJacobian<2,3> H2) {
36 PinholeCamera<Cal3_S2> camera(pose, *fixedK);
37 return camera.project(pw: point, Dpose: H1, Dpoint: H2, Dcal: {});
38}
39
40int main() {
41
42 // Create leaves
43 Pose3_ x(1);
44 Point3_ p(2);
45 Cal3_S2_ K(3);
46
47 // Some parameters needed
48 Point2 z(-17, 30);
49 SharedNoiseModel model = noiseModel::Unit::Create(dim: 2);
50
51 // Create values
52 Values values;
53 values.insert(j: 1, val: Pose3());
54 values.insert(j: 2, val: Point3(0, 0, 1));
55 values.insert(j: 3, val: Cal3_S2());
56
57 // UNCALIBRATED
58
59 // Dedicated factor
60 // Oct 3, 2014, Macbook Air
61 // 4.2 musecs/call
62 NonlinearFactor::shared_ptr f1 =
63 std::make_shared<GeneralSFMFactor2<Cal3_S2> >(args&: z, args&: model, args: 1, args: 2, args: 3);
64 time(str: "GeneralSFMFactor2<Cal3_S2> : ", f: f1, values);
65
66 // ExpressionFactor
67 // Oct 3, 2014, Macbook Air
68 // 20.3 musecs/call
69 NonlinearFactor::shared_ptr f2 =
70 std::make_shared<ExpressionFactor<Point2> >(args&: model, args&: z,
71 args: uncalibrate(K, xy_hat: project(p_cam: transformTo(x, p))));
72 time(str: "Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f: f2, values);
73
74 // ExpressionFactor ternary
75 // Oct 3, 2014, Macbook Air
76 // 20.3 musecs/call
77 NonlinearFactor::shared_ptr f3 =
78 std::make_shared<ExpressionFactor<Point2> >(args&: model, args&: z,
79 args: project3(x, p, K));
80 time(str: "Ternary(Leaf,Leaf,Leaf) : ", f: f3, values);
81
82 // CALIBRATED
83
84 // Dedicated factor
85 // Oct 3, 2014, Macbook Air
86 // 3.4 musecs/call
87 NonlinearFactor::shared_ptr g1 = std::make_shared<
88 GenericProjectionFactor<Pose3, Point3> >(args&: z, args&: model, args: 1, args: 2, args&: fixedK);
89 time(str: "GenericProjectionFactor<P,P>: ", f: g1, values);
90
91 // ExpressionFactor
92 // Oct 3, 2014, Macbook Air
93 // 16.0 musecs/call
94 NonlinearFactor::shared_ptr g2 =
95 std::make_shared<ExpressionFactor<Point2> >(args&: model, args&: z,
96 args: uncalibrate(K: Cal3_S2_(*fixedK), xy_hat: project(p_cam: transformTo(x, p))));
97 time(str: "Bin(Cnst,Un(Bin(Leaf,Leaf))): ", f: g2, values);
98
99 // ExpressionFactor, optimized
100 // Oct 3, 2014, Macbook Air
101 // 9.0 musecs/call
102 NonlinearFactor::shared_ptr g3 =
103 std::make_shared<ExpressionFactor<Point2> >(args&: model, args&: z,
104 args: Point2_(myProject, x, p));
105 time(str: "Binary(Leaf,Leaf) : ", f: g3, values);
106 return 0;
107}
108