timeAdaptAutoDiff.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 "timeLinearize.h"
26 #include <gtsam/geometry/Point3.h>
27 
28 using namespace std;
29 using namespace gtsam;
30 
31 #define time timeMultiThreaded
32 
33 int main() {
34 
35  // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
38  typedef Expression<Camera> Camera_;
40 
41  // Create leaves
42  Camera_ camera(1);
43  Point3_ point(2);
44 
45  // Some parameters needed
46  Point2 z(-17, 30);
47  SharedNoiseModel model = noiseModel::Unit::Create(2);
48 
49  // Create values
50  Values values;
51  values.insert(1, Camera());
52  values.insert(2, Point3(0, 0, 1));
53 
54  NonlinearFactor::shared_ptr f1, f2, f3;
55 
56  // Dedicated factor
57  f1 = std::make_shared<GeneralSFMFactor<Camera, Point3> >(z, model, 1, 2);
58  time("GeneralSFMFactor<Camera> : ", f1, values);
59 
60  // ExpressionFactor
61  Point2_ expression2(camera, &Camera::project2, point);
62  f3 = std::make_shared<ExpressionFactor<Point2> >(model, z, expression2);
63  time("Point2_(camera, &Camera::project, point): ", f3, values);
64 
65  // AdaptAutoDiff
66  values.clear();
67  values.insert(1,Vector9(Vector9::Zero()));
68  values.insert(2,Vector3(0,0,1));
69  typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> AdaptedSnavely;
70  Expression<Vector2> expression(AdaptedSnavely(), Expression<Vector9>(1), Expression<Vector3>(2));
71  f2 = std::make_shared<ExpressionFactor<Vector2> >(model, z, expression);
72  time("Point2_(AdaptedSnavely(), camera, point): ", f2, values);
73 
74  return 0;
75 }
Expression< Point3 > Point3_
static Point2 project2(const CalibratedCamera &camera, const Point3 &point)
void clear()
Definition: Values.h:298
Adaptor for Ceres style auto-differentiated functions.
Eigen::Vector3d Vector3
Definition: Vector.h:43
noiseModel::Diagonal::shared_ptr model
Vector2 Point2
Definition: Point2.h:32
leaf::MyValues values
Expression< Point2 > Point2_
Definition: BFloat16.h:88
double f2(const Vector2 &x)
Base class for all pinhole cameras.
int main()
#define time
time linearize
traits
Definition: chartTesting.h:28
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
3D Point
double f3(double x1, double x2)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
PinholePose< Cal3_S2 > Camera
Vector3 Point3
Definition: Point3.h:38
static const CalibratedCamera camera(kDefaultPose)
a general SFM factor with an unknown calibration
Calibration used by Bundler.
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:59