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