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
20 #include <gtsam/geometry/Cal3Bundler.h>
21 #include <gtsam/geometry/Cal3_S2.h>
22 #include <gtsam/geometry/SphericalCamera.h>
23 #include <gtsam/slam/GeneralSFMFactor.h>
24 #include <gtsam/slam/SmartProjectionFactor.h>
25 #include <gtsam/slam/SmartProjectionPoseFactor.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 {
57 typedef PinholeCamera<Cal3_S2> Camera;
58 typedef SmartProjectionFactor<Camera> SmartFactor;
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);
63 static const Point2 level_uv = level_camera.project(landmark1);
64 static const Point2 level_uv_right = level_camera_right.project(landmark1);
65 static const Camera cam1(level_pose, K2);
66 static const Camera cam2(pose_right, K2);
67 static const Camera cam3(pose_above, K2);
68 typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
69 static const SmartProjectionParams params;
70 } // namespace vanilla
71 
72 /* ************************************************************************* */
73 // default Cal3_S2 poses
74 namespace vanillaPose {
75 typedef PinholePose<Cal3_S2> Camera;
76 typedef CameraSet<Camera> Cameras;
77 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
78 typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
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 {
90 typedef PinholePose<Cal3_S2> Camera;
91 typedef CameraSet<Camera> Cameras;
92 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
93 typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
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 {
105 typedef PinholeCamera<Cal3Bundler> Camera;
106 typedef CameraSet<Camera> Cameras;
107 typedef SmartProjectionFactor<Camera> SmartFactor;
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);
117 typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
118 } // namespace bundler
119 
120 /* *************************************************************************/
121 // Cal3Bundler poses
122 namespace bundlerPose {
123 typedef PinholePose<Cal3Bundler> Camera;
124 typedef CameraSet<Camera> Cameras;
125 typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
126 typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
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 {
140 typedef SphericalCamera Camera;
141 typedef CameraSet<Camera> Cameras;
142 typedef SmartProjectionRigFactor<Camera> SmartFactorP;
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>
162 void projectToMultipleCameras(
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 /* ************************************************************************* */
SmartProjectionPoseFactor.h
Smart factor on poses, assuming camera calibration is fixed.
cam1
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
cam2
static StereoCamera cam2(pose3, cal4ptr)
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
bundler::level_uv_right
static const Point2 level_uv_right
Definition: smartFactorScenarios.h:112
sphericalCamera::SmartFactorP
SmartProjectionRigFactor< Camera > SmartFactorP
Definition: smartFactorScenarios.h:142
bundler::pose1
static const Pose3 pose1
Definition: smartFactorScenarios.h:113
M_PI
#define M_PI
Definition: main.h:106
sphericalCamera::Camera
SphericalCamera Camera
Definition: smartFactorScenarios.h:140
sphericalCamera::level_camera
static const Camera level_camera(level_pose)
perturbCameraPose
CAMERA perturbCameraPose(const CAMERA &camera)
Definition: smartFactorScenarios.h:153
bundlerPose::sharedBundlerK
static const std::shared_ptr< Cal3Bundler > sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000))
SphericalCamera.h
Calibrated camera with spherical projection.
std
Definition: BFloat16.h:88
sphericalCamera::Cameras
CameraSet< Camera > Cameras
Definition: smartFactorScenarios.h:141
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
landmark1
Point2 landmark1(5.0, 1.5)
gtsam::SphericalCamera::project
Unit3 project(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: SphericalCamera.cpp:83
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
bundlerPose::SmartFactor
SmartProjectionPoseFactor< Cal3Bundler > SmartFactor
Definition: smartFactorScenarios.h:125
bundler::K
static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0)
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
landmark
static Point3 landmark(0, 0, 5)
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
landmark2
Point2 landmark2(7.0, 1.5)
vanilla::K2
static const Cal3_S2 K2(1500, 1200, 0, w, h)
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
simulated2D::Measurement
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
bundler::SFMFactor
GeneralSFMFactor< Camera, Point3 > SFMFactor
Definition: smartFactorScenarios.h:117
level_pose
Pose3 level_pose
Definition: testInvDepthCamera3.cpp:21
sphericalCamera::level_camera_right
static const Camera level_camera_right(pose_right)
projectToMultipleCameras
void projectToMultipleCameras(const CAMERA &cam1, const CAMERA &cam2, const CAMERA &cam3, Point3 landmark, typename CAMERA::MeasurementVector &measurements_cam)
Definition: smartFactorScenarios.h:162
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
vanillaPose::sharedK
static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h))
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam
traits
Definition: chartTesting.h:28
sphericalCamera::cam3
static const Camera cam3(pose_above)
h
const double h
Definition: testSimpleHelicopter.cpp:19
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
bundlerPose::SmartRigFactor
SmartProjectionRigFactor< Camera > SmartRigFactor
Definition: smartFactorScenarios.h:126
bundler::level_uv
static const Point2 level_uv
Definition: smartFactorScenarios.h:111
gtsam::EmptyCal::shared_ptr
std::shared_ptr< EmptyCal > shared_ptr
Definition: SphericalCamera.h:47
vanillaPose2::sharedK2
static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480))
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
camera
static const CalibratedCamera camera(kDefaultPose)
GeneralSFMFactor.h
a general SFM factor with an unknown calibration
sphericalCamera::emptyK
static const EmptyCal::shared_ptr emptyK(new EmptyCal())
Cal3Bundler.h
Calibration used by Bundler.
fov
static double fov
Definition: testProjectionFactor.cpp:33
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
Cal3_S2.h
The most common 5DOF 3D->2D calibration.

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