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 "timeLinearize.h"
20#include <gtsam/3rdparty/ceres/example.h>
21#include <gtsam/nonlinear/AdaptAutoDiff.h>
22#include <gtsam/nonlinear/ExpressionFactor.h>
23#include <gtsam/slam/GeneralSFMFactor.h>
24#include <gtsam/geometry/PinholeCamera.h>
25#include <gtsam/geometry/Cal3Bundler.h>
26#include <gtsam/geometry/Point3.h>
27
28using namespace std;
29using namespace gtsam;
30
31#define time timeMultiThreaded
32
33int main() {
34
35 // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
36 typedef PinholeCamera<Cal3Bundler> Camera;
37 typedef Expression<Point2> Point2_;
38 typedef Expression<Camera> Camera_;
39 typedef Expression<Point3> Point3_;
40
41 // Create leaves
42 Camera_ camera(1);
43 Point3_ point(2);
44
45 // Some parameters needed
46 Point2 z(-17, 30);
47 SharedNoiseModel model = noiseModel::Unit::Create(dim: 2);
48
49 // Create values
50 Values values;
51 values.insert(j: 1, val: Camera());
52 values.insert(j: 2, val: Point3(0, 0, 1));
53
54 NonlinearFactor::shared_ptr f1, f2, f3;
55
56 // Dedicated factor
57 f1 = std::make_shared<GeneralSFMFactor<Camera, Point3> >(args&: z, args&: model, args: 1, args: 2);
58 time(str: "GeneralSFMFactor<Camera> : ", f: f1, values);
59
60 // ExpressionFactor
61 Point2_ expression2(camera, &Camera::project2, point);
62 f3 = std::make_shared<ExpressionFactor<Point2> >(args&: model, args&: z, args&: expression2);
63 time(str: "Point2_(camera, &Camera::project, point): ", f: f3, values);
64
65 // AdaptAutoDiff
66 values.clear();
67 values.insert(j: 1,val: Vector9(Vector9::Zero()));
68 values.insert(j: 2,val: Vector3(0,0,1));
69 typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> AdaptedSnavely;
70 Expression<Vector2> expression(AdaptedSnavely(), Expression<Vector9>(1), Expression<Vector3>(2));
71 f2 = std::make_shared<ExpressionFactor<Vector2> >(args&: model, args&: z, args&: expression);
72 time(str: "Point2_(AdaptedSnavely(), camera, point): ", f: f2, values);
73
74 return 0;
75}
76