smartFactorScenarios.h
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 #pragma once
21 #include <gtsam/geometry/Cal3_S2.h>
26 
27 #include "../SmartProjectionRigFactor.h"
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 namespace {
33 // three landmarks ~5 meters infront of camera
34 Point3 landmark1(5, 0.5, 1.2);
35 Point3 landmark2(5, -0.5, 1.2);
36 Point3 landmark3(3, 0, 3.0);
37 Point3 landmark4(10, 0.5, 1.2);
38 Point3 landmark5(10, -0.5, 1.2);
39 
40 // First camera pose, looking along X-axis, 1 meter above ground plane (x-y)
41 Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
42 // Second camera 1 meter to the right of first camera
43 Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
44 // Third camera 1 meter above the first camera
45 Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
46 
47 // Create a noise unit2 for the pixel error
48 static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
49 
50 static double fov = 60; // degrees
51 static size_t w = 640, h = 480;
52 }
53 
54 /* ************************************************************************* */
55 // default Cal3_S2 cameras
56 namespace vanilla {
59 static const Cal3_S2 K(fov, w, h);
60 static const Cal3_S2 K2(1500, 1200, 0, w, h);
61 static const Camera level_camera(level_pose, K2);
62 static const Camera level_camera_right(pose_right, K2);
65 static const Camera cam1(level_pose, K2);
66 static const Camera cam2(pose_right, K2);
67 static const Camera cam3(pose_above, K2);
70 } // namespace vanilla
71 
72 /* ************************************************************************* */
73 // default Cal3_S2 poses
74 namespace vanillaPose {
79 static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
80 static const Camera level_camera(level_pose, sharedK);
81 static const Camera level_camera_right(pose_right, sharedK);
82 static const Camera cam1(level_pose, sharedK);
83 static const Camera cam2(pose_right, sharedK);
84 static const Camera cam3(pose_above, sharedK);
85 } // namespace vanillaPose
86 
87 /* ************************************************************************* */
88 // default Cal3_S2 poses
89 namespace vanillaPose2 {
94 static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
95 static const Camera level_camera(level_pose, sharedK2);
96 static const Camera level_camera_right(pose_right, sharedK2);
97 static const Camera cam1(level_pose, sharedK2);
98 static const Camera cam2(pose_right, sharedK2);
99 static const Camera cam3(pose_above, sharedK2);
100 } // namespace vanillaPose2
101 
102 /* *************************************************************************/
103 // Cal3Bundler cameras
104 namespace bundler {
108 static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
109 static const Camera level_camera(level_pose, K);
110 static const Camera level_camera_right(pose_right, K);
111 static const Point2 level_uv = level_camera.project(landmark1);
112 static const Point2 level_uv_right = level_camera_right.project(landmark1);
113 static const Pose3 pose1 = level_pose;
114 static const Camera cam1(level_pose, K);
115 static const Camera cam2(pose_right, K);
116 static const Camera cam3(pose_above, K);
118 } // namespace bundler
119 
120 /* *************************************************************************/
121 // Cal3Bundler poses
122 namespace bundlerPose {
127 static const std::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
128  1e-3, 1000,
129  2000));
130 static const Camera level_camera(level_pose, sharedBundlerK);
131 static const Camera level_camera_right(pose_right, sharedBundlerK);
132 static const Camera cam1(level_pose, sharedBundlerK);
133 static const Camera cam2(pose_right, sharedBundlerK);
134 static const Camera cam3(pose_above, sharedBundlerK);
135 } // namespace bundlerPose
136 
137 /* ************************************************************************* */
138 // sphericalCamera
139 namespace sphericalCamera {
143 static const EmptyCal::shared_ptr emptyK(new EmptyCal());
144 static const Camera level_camera(level_pose);
145 static const Camera level_camera_right(pose_right);
146 static const Camera cam1(level_pose);
147 static const Camera cam2(pose_right);
148 static const Camera cam3(pose_above);
149 } // namespace sphericalCamera
150 /* *************************************************************************/
151 
152 template <class CAMERA>
153 CAMERA perturbCameraPose(const CAMERA& camera) {
154  Pose3 noise_pose =
155  Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
156  Pose3 cameraPose = camera.pose();
157  Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
158  return CAMERA(perturbedCameraPose, camera.calibration());
159 }
160 
161 template <class CAMERA>
163  const CAMERA& cam1, const CAMERA& cam2, const CAMERA& cam3, Point3 landmark,
164  typename CAMERA::MeasurementVector& measurements_cam) {
165  typename CAMERA::Measurement cam1_uv1 = cam1.project(landmark);
166  typename CAMERA::Measurement cam2_uv1 = cam2.project(landmark);
167  typename CAMERA::Measurement cam3_uv1 = cam3.project(landmark);
168  measurements_cam.push_back(cam1_uv1);
169  measurements_cam.push_back(cam2_uv1);
170  measurements_cam.push_back(cam3_uv1);
171 }
172 
173 /* ************************************************************************* */
Smart factor on poses, assuming camera calibration is fixed.
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
static StereoCamera cam2(pose3, cal4ptr)
Vector2 Point2
Definition: Point2.h:32
static const Point2 level_uv_right
SmartProjectionRigFactor< Camera > SmartFactorP
static const Pose3 pose1
#define M_PI
Definition: main.h:106
SphericalCamera Camera
static const Camera level_camera(level_pose)
CAMERA perturbCameraPose(const CAMERA &camera)
static const std::shared_ptr< Cal3Bundler > sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000))
Calibrated camera with spherical projection.
Definition: BFloat16.h:88
CameraSet< Camera > Cameras
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
Point2 landmark1(5.0, 1.5)
Unit3 project(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
SmartProjectionPoseFactor< Cal3Bundler > SmartFactor
static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0)
static const SmartProjectionParams params
static Point3 landmark(0, 0, 5)
Smart factor on cameras (pose + calibration)
Point2 landmark2(7.0, 1.5)
static const Cal3_S2 K2(1500, 1200, 0, w, h)
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
GeneralSFMFactor< Camera, Point3 > SFMFactor
Pose3 level_pose
static const Camera level_camera_right(pose_right)
void projectToMultipleCameras(const CAMERA &cam1, const CAMERA &cam2, const CAMERA &cam3, Point3 landmark, typename CAMERA::MeasurementVector &measurements_cam)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
Class compose(const Class &g) const
Definition: Lie.h:48
static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h))
RowVector3d w
traits
Definition: chartTesting.h:28
static const Camera cam3(pose_above)
const double h
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
SmartProjectionRigFactor< Camera > SmartRigFactor
static const Point2 level_uv
std::shared_ptr< EmptyCal > shared_ptr
static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480))
Vector3 Point3
Definition: Point3.h:38
static const CalibratedCamera camera(kDefaultPose)
a general SFM factor with an unknown calibration
static const EmptyCal::shared_ptr emptyK(new EmptyCal())
Calibration used by Bundler.
static double fov
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
The most common 5DOF 3D->2D calibration.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:51