timeOneCameraExpression.cpp
Go to the documentation of this file.
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 
19 #include <gtsam/slam/expressions.h>
21 #include "timeLinearize.h"
22 
23 using namespace std;
24 using namespace gtsam;
25 
26 #define time timeSingleThreaded
27 
28 int 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(2);
38 
39  // Create values
40  Values values;
41  values.insert(1, Pose3());
42  values.insert(2, Point3(0, 0, 1));
43  values.insert(3, 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  (model, z, uncalibrate(K, project(transformTo(x, p))));
54 #endif
55  time("timing:", f, values);
56 
57  return 0;
58 }
#define time
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
noiseModel::Diagonal::shared_ptr model
Vector2 Point2
Definition: Point2.h:32
leaf::MyValues values
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: BFloat16.h:88
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
time linearize
traits
Definition: chartTesting.h:28
float * p
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
Vector3 Point3
Definition: Point3.h:38
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:03