Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding factor which considers the essential matrix and camera calibration as variable #753

Merged
merged 37 commits into from
Jun 21, 2021
Merged
Changes from all commits
Commits
Show all changes
37 commits
Select commit Hold shift + click to select a range
8ca7f1f
Adding factor with shared calibration as a variable
ayushbaid Apr 23, 2021
f60e9e9
fixing tests by moving to Cal3_S2
ayushbaid Apr 24, 2021
64ff24b
using fixed size matrix, and adding jacobian in homogeneous conversion
ayushbaid Apr 27, 2021
b00046c
Merge branch 'develop' into feature/essential-mat-with-approx-k
ayushbaid Apr 27, 2021
b0fb6a3
renaming key variable
ayushbaid Apr 27, 2021
bd0838c
fixing docstring
ayushbaid Apr 27, 2021
2cf76da
reverting jacobian computation from homogeneous function
ayushbaid Apr 27, 2021
4572282
adding prior on calibrations
ayushbaid Apr 28, 2021
71b9004
Merge branch 'develop' into feature/essential-mat-with-approx-k
ayushbaid May 25, 2021
bf93f17
Merge branch 'develop' of github.com:borglab/gtsam into feature/essen…
akshay-krishnan Jun 8, 2021
b3601ef
updating tests
ayushbaid Jun 8, 2021
4fbd98d
creating 18 point example
ayushbaid Jun 8, 2021
8a2ce7e
switching to sampson point line error
ayushbaid Jun 9, 2021
0ecd8ab
fixing jacobians and reformatting
ayushbaid Jun 9, 2021
ae69e5f
changing error values in test
ayushbaid Jun 9, 2021
582afe1
Merge branch 'feature/sampson-epipolar-error' into feature/essential-…
ayushbaid Jun 9, 2021
15f8b41
adding jacobians on input points
ayushbaid Jun 9, 2021
e9738c7
adding jacobians on input points
ayushbaid Jun 9, 2021
65211f8
moving to squared sampson error
ayushbaid Jun 9, 2021
2e69d09
Merge branch 'feature/sampson-epipolar-error' into feature/essential-…
ayushbaid Jun 9, 2021
2e8bfd6
using correct jacobian computation for calibration
ayushbaid Jun 10, 2021
91e58f5
fixing unit tests
ayushbaid Jun 10, 2021
bce9050
adding 11 point example for cal3bundler
ayushbaid Jun 10, 2021
620bcf7
fixing test cases
ayushbaid Jun 10, 2021
f1ea57a
Merge branch 'feature/essential-mat-with-approx-k' of github.com:borg…
akshay-krishnan Jun 12, 2021
14f8b8a
removing Sampson error + some tests cleanup
akshay-krishnan Jun 14, 2021
285f041
increasing calibrate() tolerance
akshay-krishnan Jun 14, 2021
373b109
small covariance change
akshay-krishnan Jun 14, 2021
01515d1
formatting changes
akshay-krishnan Jun 14, 2021
c458294
removing duplicate data file
akshay-krishnan Jun 14, 2021
545dfd0
removing failing test and unused data
akshay-krishnan Jun 17, 2021
d9a8111
resolving merge conflict
akshay-krishnan Jun 17, 2021
47f9f30
updating documentation for factor
akshay-krishnan Jun 17, 2021
e3b6c83
updating points name, constexpr
akshay-krishnan Jun 21, 2021
a69f9e5
changing to macro EssenstialMatrixfactor4
akshay-krishnan Jun 21, 2021
119e43a
all jacobian tests for essential matrix use macro
akshay-krishnan Jun 21, 2021
01561bc
formatting example
akshay-krishnan Jun 21, 2021
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 37 additions & 26 deletions examples/CreateSFMExampleData.cpp
Original file line number Diff line number Diff line change
@@ -15,8 +15,8 @@
* @author Frank Dellaert
*/

#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/slam/dataset.h>

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

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

/* ************************************************************************* */

void createExampleBALFile(const string& filename, const vector<Point3>& P,
const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
Cal3Bundler()) {

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
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 identity, aPb(aRb, aTb);
// 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: P) {

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

// 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)));
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));

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

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> P;
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
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, P, pose1, pose2);
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> P;
P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
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";
const string filename = "../../examples/Data/5pointExample2.txt";
Cal3Bundler K(500, 0, 0);
createExampleBALFile(filename, P, pose1, pose2,K);
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;
}

/* ************************************************************************* */

131 changes: 131 additions & 0 deletions examples/Data/18pointExample1.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
2 18 36

0 0 0 -0
1 0 -6.123233995736766344e-18 -0.10000000000000000555
0 1 -0.10000000000000000555 -0
1 1 -1.2246467991473532688e-17 -0.2000000000000000111
0 2 0.10000000000000000555 -0
1 2 0 -0
0 3 0 -1
1 3 1 -0.20000000000000006661
0 4 0 1
1 4 -1 -0.19999999999999995559
0 5 -0.5 0.25
1 5 -0.25000000000000005551 -0.55000000000000004441
0 6 -0.5 -0.25
1 6 0.24999999999999997224 -0.55000000000000004441
0 7 0.16666666666666665741 0.33333333333333331483
1 7 -0.33333333333333331483 0.10000000000000000555
0 8 0.16666666666666665741 -0.33333333333333331483
1 8 0.33333333333333331483 0.099999999999999977796
0 9 -0.2000000000000000111 1
1 9 -1 -0.39999999999999996669
0 10 0.10000000000000000555 0.5
1 10 -0.5 3.0616169978683830179e-17
0 11 0.10000000000000000555 -0.5
1 11 0.5 -3.0616169978683830179e-17
0 12 -0.2000000000000000111 -0
1 12 -2.4492935982947065376e-17 -0.4000000000000000222
0 13 -0.2000000000000000111 -1
1 13 1 -0.40000000000000007772
0 14 0 -0
1 14 -1.2246467991473532688e-17 -0.2000000000000000111
0 15 0.2000000000000000111 1
1 15 -1 6.1232339957367660359e-17
0 16 0.2000000000000000111 -0
1 16 0 -0
0 17 0.2000000000000000111 -1
1 17 1 -6.1232339957367660359e-17

3.141592653589793116
0
0
-0
0
0
1
0
0

2.2214414690791830509
2.2214414690791826068
0
-6.123233995736766344e-18
-0.10000000000000000555
0
1
0
0

0
0
1

-0.10000000000000000555
0
1

0.10000000000000000555
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.10000000000000000555
-0.5
0.5

0.10000000000000000555
-0.5
1

0.10000000000000000555
0.5
1

-0.10000000000000000555
0
0.5

-0.10000000000000000555
0.5
0.5

0
0
0.5

0.10000000000000000555
-0.5
0.5

0.10000000000000000555
0
0.5

0.10000000000000000555
0.5
0.5

Loading