Skip to content

Commit aec2cf0

Browse files
akshay-krishnanakshay-krishnan
authored
Merge pull request #753 from borglab/feature/essential-mat-with-approx-k
Adding factor which considers the essential matrix and camera calibration as variable
2 parents 3b4eeeb + 01561bc commit aec2cf0

File tree

4 files changed

+632
-231
lines changed

4 files changed

+632
-231
lines changed

‎examples/CreateSFMExampleData.cpp‎

Lines changed: 37 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,8 @@
1515
* @author Frank Dellaert
1616
*/
1717

18-
#include <gtsam/slam/dataset.h>
1918
#include <gtsam/geometry/CalibratedCamera.h>
19+
#include <gtsam/slam/dataset.h>
2020

2121
#include <boost/assign/std/vector.hpp>
2222

@@ -26,22 +26,16 @@ using namespace gtsam;
2626

2727
/* ************************************************************************* */
2828

29-
void createExampleBALFile(const string& filename, const vector<Point3>& P,
30-
const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
31-
Cal3Bundler()) {
32-
29+
void createExampleBALFile(const string& filename, const vector<Point3>& points,
30+
const Pose3& pose1, const Pose3& pose2,
31+
const Cal3Bundler& K = Cal3Bundler()) {
3332
// Class that will gather all data
3433
SfmData data;
35-
36-
// Create two cameras
37-
Rot3 aRb = Rot3::Yaw(M_PI_2);
38-
Point3 aTb(0.1, 0, 0);
39-
Pose3 identity, aPb(aRb, aTb);
34+
// Create two cameras and add them to data
4035
data.cameras.push_back(SfmCamera(pose1, K));
4136
data.cameras.push_back(SfmCamera(pose2, K));
4237

43-
for(const Point3& p: P) {
44-
38+
for (const Point3& p : points) {
4539
// Create the track
4640
SfmTrack track;
4741
track.p = p;
@@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
5145

5246
// Project points in both cameras
5347
for (size_t i = 0; i < 2; i++)
54-
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
48+
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
5549

5650
// Add track to data
5751
data.tracks.push_back(track);
@@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
6357
/* ************************************************************************* */
6458

6559
void create5PointExample1() {
66-
6760
// Create two cameras poses
6861
Rot3 aRb = Rot3::Yaw(M_PI_2);
6962
Point3 aTb(0.1, 0, 0);
7063
Pose3 pose1, pose2(aRb, aTb);
7164

7265
// Create test data, we need at least 5 points
73-
vector<Point3> P;
74-
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
75-
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
66+
vector<Point3> points = {
67+
{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}};
7668

7769
// Assumes example is run in ${GTSAM_TOP}/build/examples
78-
const string filename = "../../examples/data/5pointExample1.txt";
79-
createExampleBALFile(filename, P, pose1, pose2);
70+
const string filename = "../../examples/Data/5pointExample1.txt";
71+
createExampleBALFile(filename, points, pose1, pose2);
8072
}
8173

8274
/* ************************************************************************* */
8375

8476
void create5PointExample2() {
85-
8677
// Create two cameras poses
8778
Rot3 aRb = Rot3::Yaw(M_PI_2);
8879
Point3 aTb(10, 0, 0);
8980
Pose3 pose1, pose2(aRb, aTb);
9081

9182
// Create test data, we need at least 5 points
92-
vector<Point3> P;
93-
P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
94-
Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
83+
vector<Point3> points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, //
84+
{0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, //
85+
{20, -50, 80}};
9586

9687
// Assumes example is run in ${GTSAM_TOP}/build/examples
97-
const string filename = "../../examples/data/5pointExample2.txt";
88+
const string filename = "../../examples/Data/5pointExample2.txt";
9889
Cal3Bundler K(500, 0, 0);
99-
createExampleBALFile(filename, P, pose1, pose2,K);
90+
createExampleBALFile(filename, points, pose1, pose2, K);
10091
}
10192

10293
/* ************************************************************************* */
10394

95+
void create18PointExample1() {
96+
// Create two cameras poses
97+
Rot3 aRb = Rot3::Yaw(M_PI_2);
98+
Point3 aTb(0.1, 0, 0);
99+
Pose3 pose1, pose2(aRb, aTb);
100+
101+
// Create test data, we need 15 points
102+
vector<Point3> points = {
103+
{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, //
104+
{0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, //
105+
{-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, //
106+
{-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, //
107+
{-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, //
108+
{0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}};
109+
110+
// Assumes example is run in ${GTSAM_TOP}/build/examples
111+
const string filename = "../../examples/Data/18pointExample1.txt";
112+
createExampleBALFile(filename, points, pose1, pose2);
113+
}
114+
104115
int main(int argc, char* argv[]) {
105116
create5PointExample1();
106117
create5PointExample2();
118+
create18PointExample1();
107119
return 0;
108120
}
109121

110122
/* ************************************************************************* */
111-

‎examples/Data/18pointExample1.txt‎

Lines changed: 131 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,131 @@
1+
2 18 36
2+
3+
0 0 0 -0
4+
1 0 -6.123233995736766344e-18 -0.10000000000000000555
5+
0 1 -0.10000000000000000555 -0
6+
1 1 -1.2246467991473532688e-17 -0.2000000000000000111
7+
0 2 0.10000000000000000555 -0
8+
1 2 0 -0
9+
0 3 0 -1
10+
1 3 1 -0.20000000000000006661
11+
0 4 0 1
12+
1 4 -1 -0.19999999999999995559
13+
0 5 -0.5 0.25
14+
1 5 -0.25000000000000005551 -0.55000000000000004441
15+
0 6 -0.5 -0.25
16+
1 6 0.24999999999999997224 -0.55000000000000004441
17+
0 7 0.16666666666666665741 0.33333333333333331483
18+
1 7 -0.33333333333333331483 0.10000000000000000555
19+
0 8 0.16666666666666665741 -0.33333333333333331483
20+
1 8 0.33333333333333331483 0.099999999999999977796
21+
0 9 -0.2000000000000000111 1
22+
1 9 -1 -0.39999999999999996669
23+
0 10 0.10000000000000000555 0.5
24+
1 10 -0.5 3.0616169978683830179e-17
25+
0 11 0.10000000000000000555 -0.5
26+
1 11 0.5 -3.0616169978683830179e-17
27+
0 12 -0.2000000000000000111 -0
28+
1 12 -2.4492935982947065376e-17 -0.4000000000000000222
29+
0 13 -0.2000000000000000111 -1
30+
1 13 1 -0.40000000000000007772
31+
0 14 0 -0
32+
1 14 -1.2246467991473532688e-17 -0.2000000000000000111
33+
0 15 0.2000000000000000111 1
34+
1 15 -1 6.1232339957367660359e-17
35+
0 16 0.2000000000000000111 -0
36+
1 16 0 -0
37+
0 17 0.2000000000000000111 -1
38+
1 17 1 -6.1232339957367660359e-17
39+
40+
3.141592653589793116
41+
0
42+
0
43+
-0
44+
0
45+
0
46+
1
47+
0
48+
0
49+
50+
2.2214414690791830509
51+
2.2214414690791826068
52+
0
53+
-6.123233995736766344e-18
54+
-0.10000000000000000555
55+
0
56+
1
57+
0
58+
0
59+
60+
0
61+
0
62+
1
63+
64+
-0.10000000000000000555
65+
0
66+
1
67+
68+
0.10000000000000000555
69+
0
70+
1
71+
72+
0
73+
0.5
74+
0.5
75+
76+
0
77+
-0.5
78+
0.5
79+
80+
-1
81+
-0.5
82+
2
83+
84+
-1
85+
0.5
86+
2
87+
88+
0.25
89+
-0.5
90+
1.5
91+
92+
0.25
93+
0.5
94+
1.5
95+
96+
-0.10000000000000000555
97+
-0.5
98+
0.5
99+
100+
0.10000000000000000555
101+
-0.5
102+
1
103+
104+
0.10000000000000000555
105+
0.5
106+
1
107+
108+
-0.10000000000000000555
109+
0
110+
0.5
111+
112+
-0.10000000000000000555
113+
0.5
114+
0.5
115+
116+
0
117+
0
118+
0.5
119+
120+
0.10000000000000000555
121+
-0.5
122+
0.5
123+
124+
0.10000000000000000555
125+
0
126+
0.5
127+
128+
0.10000000000000000555
129+
0.5
130+
0.5
131+

0 commit comments

Comments
 (0)