SFMdata.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 
26 #pragma once
27 
28 // As this is a full 3D problem, we will use Pose3 variables to represent the
29 // camera positions and Point3 variables (x, y, z) to represent the landmark
30 // coordinates. Camera observations of landmarks (i.e. pixel coordinates) will
31 // be stored as Point2 (x, y).
32 #include <gtsam/geometry/Point3.h>
33 #include <gtsam/geometry/Pose3.h>
34 
35 // We will also need a camera object to hold calibration information and perform
36 // projections.
37 #include <gtsam/geometry/Cal3_S2.h>
39 
40 namespace gtsam {
41 
43 std::vector<Point3> createPoints() {
44  std::vector<Point3> points;
45  points.push_back(Point3(10.0, 10.0, 10.0));
46  points.push_back(Point3(-10.0, 10.0, 10.0));
47  points.push_back(Point3(-10.0, -10.0, 10.0));
48  points.push_back(Point3(10.0, -10.0, 10.0));
49  points.push_back(Point3(10.0, 10.0, -10.0));
50  points.push_back(Point3(-10.0, 10.0, -10.0));
51  points.push_back(Point3(-10.0, -10.0, -10.0));
52  points.push_back(Point3(10.0, -10.0, -10.0));
53 
54  return points;
55 }
56 
62 std::vector<Pose3> createPoses(
63  const Pose3& init = Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}),
64  const Pose3& delta = Pose3(Rot3::Ypr(0, -M_PI_4, 0),
65  {sin(M_PI_4) * 30, 0, 30 * (1 - sin(M_PI_4))}),
66  int steps = 8) {
67  std::vector<Pose3> poses;
68  poses.reserve(steps);
69 
70  poses.push_back(init);
71  for (int i = 1; i < steps; ++i) {
72  poses.push_back(poses[i - 1].compose(delta));
73  }
74 
75  return poses;
76 }
77 
81 std::vector<Pose3> posesOnCircle(int num_cameras = 8, double R = 30) {
82  const double theta = 2 * M_PI / num_cameras;
83 
84  // Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis
85  // pointing down
86  const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {R, 0, 0});
87 
88  // Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
89  Rot3 delta_rotation = Rot3::Ypr(0, -theta, 0);
90 
91  // Delta translation in world frame
92  Vector3 delta_translation_world(R * (cos(theta) - 1), R * sin(theta), 0);
93 
94  // Transform delta translation to local frame of the camera
95  Vector3 delta_translation_local =
96  init.rotation().inverse() * delta_translation_world;
97 
98  // Define delta pose
99  const Pose3 delta(delta_rotation, delta_translation_local);
100 
101  // Generate poses using createPoses
102  return createPoses(init, delta, num_cameras);
103 }
104 } // namespace gtsam
gtsam::createPoints
std::vector< Point3 > createPoints()
Create a set of ground-truth landmarks.
Definition: SFMdata.h:43
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Rot3::Ypr
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
Definition: Rot3.h:196
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
M_PI_4
#define M_PI_4
Definition: mconf.h:119
PinholeCamera.h
Base class for all pinhole cameras.
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:2006
M_PI_2
#define M_PI_2
Definition: mconf.h:118
gtsam::posesOnCircle
std::vector< Pose3 > posesOnCircle(int num_cameras=8, double R=30)
Definition: SFMdata.h:81
gtsam::createPoses
std::vector< Pose3 > createPoses(const Pose3 &init=Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}), const Pose3 &delta=Pose3(Rot3::Ypr(0, -M_PI_4, 0), {sin(M_PI_4) *30, 0, 30 *(1 - sin(M_PI_4))}), int steps=8)
Definition: SFMdata.h:62
gtsam
traits
Definition: SFMdata.h:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
M_PI
#define M_PI
Definition: mconf.h:117
Pose3
Definition: testDependencies.h:3
init
Definition: TutorialInplaceLU.cpp:2
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
Pose3.h
3D Pose


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