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);
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));
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 /* ************************************************************************* */
bundlerPose::SmartFactor
SmartProjectionPoseFactor< Cal3Bundler > SmartFactor
Definition: smartFactorScenarios.h:125
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
sphericalCamera::Camera
SphericalCamera Camera
Definition: smartFactorScenarios.h:140
cam1
static PinholeCamera< Cal3_S2 > cam1(pose3, cal1)
bundler::pose1
static const Pose3 pose1
Definition: smartFactorScenarios.h:113
sphericalCamera::level_camera
static const Camera level_camera(level_pose)
fov
static double fov
Definition: testProjectionFactor.cpp:33
sphericalCamera::cam3
static const Camera cam3(pose_above)
bundler::K
static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0)
sphericalCamera::Cameras
CameraSet< Camera > Cameras
Definition: smartFactorScenarios.h:141
vanillaPose
Definition: smartFactorScenarios.h:74
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
SphericalCamera.h
Calibrated camera with spherical projection.
level_pose
Pose3 level_pose
Definition: testInvDepthCamera3.cpp:21
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::EmptyCal
Definition: SphericalCamera.h:42
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::SphericalCamera
Definition: SphericalCamera.h:74
vanillaPose2::sharedK2
static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480))
vanillaPose::sharedK
static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h))
GeneralSFMFactor.h
a general SFM factor with an unknown calibration
bundler::SFMFactor
GeneralSFMFactor< Camera, Point3 > SFMFactor
Definition: smartFactorScenarios.h:117
h
const double h
Definition: testSimpleHelicopter.cpp:19
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
sphericalCamera::SmartFactorP
SmartProjectionRigFactor< Camera > SmartFactorP
Definition: smartFactorScenarios.h:142
landmark2
Point2 landmark2(7.0, 1.5)
sphericalCamera::emptyK
static const EmptyCal::shared_ptr emptyK(new EmptyCal())
simulated2D::Measurement
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
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
bundler::level_uv_right
static const Point2 level_uv_right
Definition: smartFactorScenarios.h:112
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
vanilla::K2
static const Cal3_S2 K2(1500, 1200, 0, w, h)
gtsam::Pose3
Definition: Pose3.h:37
gtsam::SmartProjectionPoseFactor
Definition: SmartProjectionPoseFactor.h:45
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
sphericalCamera::level_camera_right
static const Camera level_camera_right(pose_right)
landmark
static Point3 landmark(0, 0, 5)
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
bundlerPose
Definition: smartFactorScenarios.h:122
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
bundler::level_uv
static const Point2 level_uv
Definition: smartFactorScenarios.h:111
SmartProjectionPoseFactor.h
Smart factor on poses, assuming camera calibration is fixed.
gtsam::SmartProjectionFactor
Definition: SmartProjectionFactor.h:44
cam2
static StereoCamera cam2(pose3, cal4ptr)
vanillaPose2
Definition: smartFactorScenarios.h:89
sphericalCamera
Definition: smartFactorScenarios.h:139
gtsam
traits
Definition: SFMdata.h:40
perturbCameraPose
CAMERA perturbCameraPose(const CAMERA &camera)
Definition: smartFactorScenarios.h:153
std
Definition: BFloat16.h:88
bundlerPose::SmartRigFactor
SmartProjectionRigFactor< Camera > SmartRigFactor
Definition: smartFactorScenarios.h:126
bundler
Definition: smartFactorScenarios.h:104
bundlerPose::sharedBundlerK
static const std::shared_ptr< Cal3Bundler > sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000))
gtsam::GeneralSFMFactor
Definition: GeneralSFMFactor.h:59
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
vanilla
Definition: smartFactorScenarios.h:56
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
camera
static const CalibratedCamera camera(kDefaultPose)
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::PinholePose
Definition: PinholePose.h:239
gtsam::EmptyCal::shared_ptr
std::shared_ptr< EmptyCal > shared_ptr
Definition: SphericalCamera.h:47
Cal3Bundler.h
Calibration used by Bundler.
projectToMultipleCameras
void projectToMultipleCameras(const CAMERA &cam1, const CAMERA &cam2, const CAMERA &cam3, Point3 landmark, typename CAMERA::MeasurementVector &measurements_cam)
Definition: smartFactorScenarios.h:162
gtsam::SmartProjectionRigFactor
Definition: SmartProjectionRigFactor.h:52


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:30