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 timeSFMExpressions.cpp
14 * @brief time CalibratedCamera derivatives, realistic scenario
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/nonlinear/NonlinearFactorGraph.h>
22#include <gtsam/linear/GaussianFactorGraph.h>
23
24#include <time.h>
25#include <iostream>
26#include <iomanip> // std::setprecision
27
28using namespace std;
29using namespace gtsam;
30
31//#define TERNARY
32
33int main() {
34
35 // number of cameras, and points
36 static const size_t M=100, N = 10000, n = M*N;
37
38 // Create leaves
39 Cal3_S2_ K('K', 0);
40 std::vector<Expression<Pose3> > x = createUnknowns<Pose3>(n: M, c: 'x');
41 std::vector<Expression<Point3> > p = createUnknowns<Point3>(n: N, c: 'p');
42
43 // Some parameters needed
44 Point2 z(-17, 30);
45 SharedNoiseModel model = noiseModel::Unit::Create(dim: 2);
46
47 // Create values
48 Values values;
49 values.insert(j: Symbol('K', 0), val: Cal3_S2());
50 for (size_t i = 0; i < M; i++)
51 values.insert(j: Symbol('x', i), val: Pose3());
52 for (size_t j = 0; j < N; j++)
53 values.insert(j: Symbol('p', j), val: Point3(0, 0, 1));
54
55 long timeLog = clock();
56 NonlinearFactorGraph graph;
57 for (size_t i = 0; i < M; i++) {
58 for (size_t j = 0; j < N; j++) {
59 NonlinearFactor::shared_ptr f = std::make_shared<
60 ExpressionFactor<Point2> >
61#ifdef TERNARY
62 (model, z, project3(x[i], p[j], K));
63#else
64 (args&: model, args&: z, args: uncalibrate(K, xy_hat: project(p_cam: transformTo(x: x[i], p: p[j]))));
65#endif
66 graph.push_back(factor: f);
67 }
68 }
69 long timeLog2 = clock();
70 cout << setprecision(3);
71 double seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC;
72 cout << seconds << " seconds to build" << endl;
73
74 timeLog = clock();
75 GaussianFactorGraph::shared_ptr gfg = graph.linearize(linearizationPoint: values);
76 timeLog2 = clock();
77 seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC;
78 cout << seconds << " seconds to linearize" << endl;
79 cout << ((double) seconds * 1000000 / n) << " musecs/call" << endl;
80
81 return 0;
82}
83