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 timeOneCameraExpression.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 "timeLinearize.h"
22
23using namespace std;
24using namespace gtsam;
25
26#define time timeSingleThreaded
27
28int main() {
29
30 // Create leaves
31 Pose3_ x(1);
32 Point3_ p(2);
33 Cal3_S2_ K(3);
34
35 // Some parameters needed
36 Point2 z(-17, 30);
37 SharedNoiseModel model = noiseModel::Unit::Create(dim: 2);
38
39 // Create values
40 Values values;
41 values.insert(j: 1, val: Pose3());
42 values.insert(j: 2, val: Point3(0, 0, 1));
43 values.insert(j: 3, val: Cal3_S2());
44
45 // ExpressionFactor
46 // Oct 3, 2014, Macbook Air
47 // 20.3 musecs/call
48//#define TERNARY
49 NonlinearFactor::shared_ptr f = std::make_shared<ExpressionFactor<Point2> >
50#ifdef TERNARY
51 (model, z, project3(x, p, K));
52#else
53 (args&: model, args&: z, args: uncalibrate(K, xy_hat: project(p_cam: transformTo(x, p))));
54#endif
55 time(str: "timing:", f, values);
56
57 return 0;
58}
59