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
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
70  std::make_shared<ExpressionFactor<Point2> >(model, z,
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
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
89  time("GenericProjectionFactor<P,P>: ", g1, values);
90 
91  // ExpressionFactor
92  // Oct 3, 2014, Macbook Air
93  // 16.0 musecs/call
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
103  std::make_shared<ExpressionFactor<Point2> >(model, z,
104  Point2_(myProject, x, p));
105  time("Binary(Leaf,Leaf) : ", g3, values);
106  return 0;
107 }
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::Point2_
Expression< Point2 > Point2_
Definition: slam/expressions.h:22
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
GeneralSFMFactor.h
a general SFM factor with an unknown calibration
gtsam::Cal3_S2_
Expression< Cal3_S2 > Cal3_S2_
Definition: slam/expressions.h:127
uncalibrate
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
Definition: testExpression.cpp:37
gtsam::Expression
Definition: Expression.h:47
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
time
#define time
Definition: timeCameraExpression.cpp:30
project3
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
Definition: testPinholeCamera.cpp:198
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
gtsam::Pose3
Definition: Pose3.h:37
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:40
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
main
int main()
Definition: timeCameraExpression.cpp:40
myProject
Point2 myProject(const Pose3 &pose, const Point3 &point, OptionalJacobian< 2, 6 > H1, OptionalJacobian< 2, 3 > H2)
Definition: timeCameraExpression.cpp:34
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
timeLinearize.h
time linearize
project
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Definition: testPinholePose.cpp:188
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
f3
double f3(double x1, double x2)
Definition: testNumericalDerivative.cpp:76
gtsam::Values
Definition: Values.h:65
expressions.h
Common expressions for solving geometry/slam/sfm problems.
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
fixedK
std::shared_ptr< Cal3_S2 > fixedK(new Cal3_S2())
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
ExpressionFactor.h
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
camera
static const CalibratedCamera camera(kDefaultPose)
Pose3.h
3D Pose
gtsam::CalibratedCamera::project
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:188


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:59