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 CreateSFMExampleData.cpp
14 * @brief Create some example data that for inclusion in the data folder
15 * @author Frank Dellaert
16 */
17
18#include <gtsam/geometry/CalibratedCamera.h>
19#include <gtsam/slam/dataset.h>
20
21using namespace std;
22using namespace gtsam;
23
24/* ************************************************************************* */
25
26void createExampleBALFile(const string& filename, const vector<Point3>& points,
27 const Pose3& pose1, const Pose3& pose2,
28 const Cal3Bundler& K = Cal3Bundler()) {
29 // Class that will gather all data
30 SfmData data;
31 // Create two cameras and add them to data
32 data.cameras.push_back(x: SfmCamera(pose1, K));
33 data.cameras.push_back(x: SfmCamera(pose2, K));
34
35 for (const Point3& p : points) {
36 // Create the track
37 SfmTrack track;
38 track.p = p;
39 track.r = 1;
40 track.g = 1;
41 track.b = 1;
42
43 // Project points in both cameras
44 for (size_t i = 0; i < 2; i++)
45 track.measurements.push_back(x: make_pair(x&: i, y: data.cameras[i].project(pw: p)));
46
47 // Add track to data
48 data.tracks.push_back(x: track);
49 }
50
51 writeBAL(filename, data);
52}
53
54/* ************************************************************************* */
55
56void create5PointExample1() {
57 // Create two cameras poses
58 Rot3 aRb = Rot3::Yaw(M_PI_2);
59 Point3 aTb(0.1, 0, 0);
60 Pose3 pose1, pose2(aRb, aTb);
61
62 // Create test data, we need at least 5 points
63 vector<Point3> points = {
64 {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}};
65
66 // Assumes example is run in ${GTSAM_TOP}/build/examples
67 const string filename = "../../examples/Data/5pointExample1.txt";
68 createExampleBALFile(filename, points, pose1, pose2);
69}
70
71/* ************************************************************************* */
72
73void create5PointExample2() {
74 // Create two cameras poses
75 Rot3 aRb = Rot3::Yaw(M_PI_2);
76 Point3 aTb(10, 0, 0);
77 Pose3 pose1, pose2(aRb, aTb);
78
79 // Create test data, we need at least 5 points
80 vector<Point3> points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, //
81 {0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, //
82 {20, -50, 80}};
83
84 // Assumes example is run in ${GTSAM_TOP}/build/examples
85 const string filename = "../../examples/Data/5pointExample2.txt";
86 Cal3Bundler K(500, 0, 0);
87 createExampleBALFile(filename, points, pose1, pose2, K);
88}
89
90/* ************************************************************************* */
91
92void create18PointExample1() {
93 // Create two cameras poses
94 Rot3 aRb = Rot3::Yaw(M_PI_2);
95 Point3 aTb(0.1, 0, 0);
96 Pose3 pose1, pose2(aRb, aTb);
97
98 // Create test data, we need 15 points
99 vector<Point3> points = {
100 {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, //
101 {0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, //
102 {-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, //
103 {-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, //
104 {-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, //
105 {0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}};
106
107 // Assumes example is run in ${GTSAM_TOP}/build/examples
108 const string filename = "../../examples/Data/18pointExample1.txt";
109 createExampleBALFile(filename, points, pose1, pose2);
110}
111
112int main(int argc, char* argv[]) {
113 create5PointExample1();
114 create5PointExample2();
115 create18PointExample1();
116 return 0;
117}
118
119/* ************************************************************************* */
120