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
12/**
13 * @file SFMdata.h
14 * @brief Simple example for the structure-from-motion problems
15 * @author Duy-Nguyen Ta
16 */
17
18/**
19 * A structure-from-motion example with landmarks, default arguments give:
20 * - The landmarks form a 10 meter cube
21 * - The robot rotates around the landmarks, always facing towards the cube
22 * Passing function argument allows to specify an initial position, a pose
23 * increment and step count.
24 */
25
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>
38#include <gtsam/geometry/PinholeCamera.h>
39
40namespace gtsam {
41
42/// Create a set of ground-truth landmarks
43[[maybe_unused]] static std::vector<Point3> createPoints() {
44 std::vector<Point3> points;
45 points.push_back(x: Point3(10.0, 10.0, 10.0));
46 points.push_back(x: Point3(-10.0, 10.0, 10.0));
47 points.push_back(x: Point3(-10.0, -10.0, 10.0));
48 points.push_back(x: Point3(10.0, -10.0, 10.0));
49 points.push_back(x: Point3(10.0, 10.0, -10.0));
50 points.push_back(x: Point3(-10.0, 10.0, -10.0));
51 points.push_back(x: Point3(-10.0, -10.0, -10.0));
52 points.push_back(x: Point3(10.0, -10.0, -10.0));
53
54 return points;
55}
56
57/**
58 * Create a set of ground-truth poses
59 * Default values give a circular trajectory, radius 30 at pi/4 intervals,
60 * always facing the circle center
61 */
62[[maybe_unused]] static std::vector<Pose3> createPoses(
63 const Pose3& init = Pose3(Rot3::Ypr(M_PI_2, p: 0, r: -M_PI_2), {30, 0, 0}),
64 const Pose3& delta = Pose3(Rot3::Ypr(y: 0, p: -M_PI_4, r: 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(n: steps);
69
70 poses.push_back(x: init);
71 for (int i = 1; i < steps; ++i) {
72 poses.push_back(x: poses[i - 1].compose(g: delta));
73 }
74
75 return poses;
76}
77
78/**
79 * Create regularly spaced poses with specified radius and number of cameras
80 */
81[[maybe_unused]] static 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, p: 0, r: -M_PI_2), {R, 0, 0});
87
88 // Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
89 Rot3 delta_rotation = Rot3::Ypr(y: 0, p: -theta, r: 0);
90
91 // Delta translation in world frame
92 Vector3 delta_translation_world(R * (cos(x: theta) - 1), R * sin(x: 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, steps: num_cameras);
103}
104} // namespace gtsam