From 4fbd98df3a23fa3c485fc649944cc8a3144ce9d5 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Tue, 8 Jun 2021 13:14:56 -0700 Subject: [PATCH] creating 18 point example --- examples/CreateSFMExampleData.cpp | 25 ++++++ examples/data/18pointExample1.txt | 131 ++++++++++++++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 examples/data/18pointExample1.txt diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index aa0bde8f6d..650b3c7317 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -99,11 +99,36 @@ void create5PointExample2() { createExampleBALFile(filename, P, 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 P; + P += Point3(-0.1, -0.5, 1), Point3(-0.1, 0, 1), Point3(-0.1, 0.5, 1), + Point3(0, -0.5, 1), Point3(0, 0, 1), Point3(0, 0.5, 1), + Point3(0.1, -0.5, 1), Point3(0.1, 0, 1), Point3(0.1, 0.5, 1), + Point3(-0.1, -0.5, 0.5), Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), + Point3(0, -0.5, 0.5), Point3(0, 0, 0.5), Point3(0, 0.5, 0.5), + Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/data/18pointExample1.txt"; + createExampleBALFile(filename, P, pose1, pose2); +} + /* ************************************************************************* */ int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); + create18PointExample1(); return 0; } diff --git a/examples/data/18pointExample1.txt b/examples/data/18pointExample1.txt new file mode 100644 index 0000000000..e7d1862941 --- /dev/null +++ b/examples/data/18pointExample1.txt @@ -0,0 +1,131 @@ +2 18 36 + +0 0 -0.10000000000000000555 0.5 +1 0 -0.5 -0.19999999999999998335 +0 1 -0.10000000000000000555 -0 +1 1 -1.2246467991473532688e-17 -0.2000000000000000111 +0 2 -0.10000000000000000555 -0.5 +1 2 0.5 -0.20000000000000003886 +0 3 0 0.5 +1 3 -0.5 -0.099999999999999977796 +0 4 0 -0 +1 4 -6.123233995736766344e-18 -0.10000000000000000555 +0 5 0 -0.5 +1 5 0.5 -0.10000000000000003331 +0 6 0.10000000000000000555 0.5 +1 6 -0.5 3.0616169978683830179e-17 +0 7 0.10000000000000000555 -0 +1 7 0 -0 +0 8 0.10000000000000000555 -0.5 +1 8 0.5 -3.0616169978683830179e-17 +0 9 -0.2000000000000000111 1 +1 9 -1 -0.39999999999999996669 +0 10 -0.2000000000000000111 -0 +1 10 -2.4492935982947065376e-17 -0.4000000000000000222 +0 11 -0.2000000000000000111 -1 +1 11 1 -0.40000000000000007772 +0 12 0 1 +1 12 -1 -0.19999999999999995559 +0 13 0 -0 +1 13 -1.2246467991473532688e-17 -0.2000000000000000111 +0 14 0 -1 +1 14 1 -0.20000000000000006661 +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.10000000000000000555 +-0.5 +1 + +-0.10000000000000000555 +0 +1 + +-0.10000000000000000555 +0.5 +1 + +0 +-0.5 +1 + +0 +0 +1 + +0 +0.5 +1 + +0.10000000000000000555 +-0.5 +1 + +0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0.5 +1 + +-0.10000000000000000555 +-0.5 +0.5 + +-0.10000000000000000555 +0 +0.5 + +-0.10000000000000000555 +0.5 +0.5 + +0 +-0.5 +0.5 + +0 +0 +0.5 + +0 +0.5 +0.5 + +0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +0 +0.5 + +0.10000000000000000555 +0.5 +0.5 +