timeCameraExpression.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>
23 #include <gtsam/geometry/Pose3.h>
24 #include <gtsam/geometry/Cal3_S2.h>
25 #include "timeLinearize.h"
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 #define time timeSingleThreaded
31 
32 std::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
33 
37  return camera.project(point, H1, H2, {});
38 }
39 
40 int 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(2);
50 
51  // Create values
52  Values values;
53  values.insert(1, Pose3());
54  values.insert(2, Point3(0, 0, 1));
55  values.insert(3, 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> >(z, model, 1, 2, 3);
64  time("GeneralSFMFactor2<Cal3_S2> : ", 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> >(model, z,
71  uncalibrate(K, project(transformTo(x, p))));
72  time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", 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> >(model, z,
79  project3(x, p, K));
80  time("Ternary(Leaf,Leaf,Leaf) : ", 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<
89  time("GenericProjectionFactor<P,P>: ", 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> >(model, z,
97  time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", 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> >(model, z,
104  Point2_(myProject, x, p));
105  time("Binary(Leaf,Leaf) : ", g3, values);
106  return 0;
107 }
#define time
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
noiseModel::Diagonal::shared_ptr model
Point2 myProject(const Pose3 &pose, const Point3 &point, OptionalJacobian< 2, 6 > H1, OptionalJacobian< 2, 3 > H2)
Vector2 Point2
Definition: Point2.h:32
int main()
leaf::MyValues values
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Expression< Point2 > Point2_
Definition: BFloat16.h:88
double f2(const Vector2 &x)
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:112
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
time linearize
Reprojection of a LANDMARK to a 2D point.
std::shared_ptr< Cal3_S2 > fixedK(new Cal3_S2())
traits
Definition: chartTesting.h:28
Expression< Cal3_S2 > Cal3_S2_
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
float * p
double f3(double x1, double x2)
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
static const CalibratedCamera camera(kDefaultPose)
a general SFM factor with an unknown calibration
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
Pose3 g2(g1.expmap(h *V1_g1))
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
3D Pose
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
The most common 5DOF 3D->2D calibration.
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:01