From 02bfd20a599ae1b4062da3d36d90ce4941f3e21d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 17:04:24 -0700 Subject: [PATCH] Switch to general F --- examples/SFMExample.cpp | 4 +-- examples/ViewGraphExample.cpp | 48 +++++++++++++++++++++++------------ 2 files changed, 34 insertions(+), 18 deletions(-) diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index fddca81694..0b4dc4036f 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -39,7 +39,7 @@ // Finally, once all of the factors have been added to our factor graph, we will want to // solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. // GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg +// trust-region method known as Powell's Dogleg #include // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the @@ -57,7 +57,7 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + auto K = std::make_shared(50.0, 50.0, 0.0, 50.0, 50.0); // Define the camera observation noise model auto measurementNoise = diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index 6d51319424..7b5ed0fc6e 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -30,7 +30,6 @@ #include #include "SFMdata.h" -#include "gtsam/geometry/EssentialMatrix.h" #include "gtsam/inference/Key.h" using namespace std; @@ -39,7 +38,7 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); // Create the set of 8 ground-truth landmarks vector points = createPoints(); @@ -47,25 +46,36 @@ int main(int argc, char* argv[]) { // Create the set of 4 ground-truth poses vector poses = posesOnCircle(4, 30); + // Calculate ground truth fundamental matrices, 1 and 2 poses apart + auto F1 = GeneralFundamentalMatrix(K, poses[0].between(poses[1]), K); + auto F2 = GeneralFundamentalMatrix(K, poses[0].between(poses[2]), K); + // Simulate measurements from each camera pose std::array, 4> p; for (size_t i = 0; i < 4; ++i) { - GTSAM_PRINT(poses[i]); - PinholeCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], K); for (size_t j = 0; j < 8; ++j) { - cout << "Camera index: " << i << ", Landmark index: " << j << endl; p[i][j] = camera.project(points[j]); } } - // Create a factor graph + // This section of the code is inspired by the work of Sweeney et al. + // [link](sites.cs.ucsb.edu/~holl/pubs/Sweeney-2015-ICCV.pdf) on view-graph + // calibration. The graph is made up of transfer factors that enforce the + // epipolar constraint between corresponding points across three views, as + // described in the paper. Rather than adding one ternary error term per point + // in a triplet, we add three binary factors for sparsity during optimization. + // In this version, we only include triplets between 3 successive cameras. NonlinearFactorGraph graph; - - using Factor = TransferFactor; + using Factor = TransferFactor; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera size_t c = (a + 2) % 4; // Camera after next for (size_t j = 0; j < 4; ++j) { + // Add transfer factors between views a, b, and c. Note that the EdgeKeys + // are crucial in performing the transfer in the right direction. We use + // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental + // matrices we will optimize for. graph.emplace_shared(EdgeKey(a, c), EdgeKey(b, c), p[a][j], p[b][j], p[c][j]); graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), p[a][j], @@ -82,18 +92,19 @@ int main(int argc, char* argv[]) { graph.print("Factor Graph:\n", formatter); + // Create a delta vector to perturb the ground truth + // We can't really go far before convergence becomes problematic :-( + Vector7 delta; + delta << 1, 2, 3, 4, 5, 6, 7; + delta *= 5e-5; + // Create the data structure to hold the initial estimate to the solution Values initialEstimate; - const Point2 center(50, 50); - auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1])); - auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2])); for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera size_t c = (a + 2) % 4; // Camera after next - initialEstimate.insert(EdgeKey(a, b), - SimpleFundamentalMatrix(E1, 50, 50, center, center)); - initialEstimate.insert(EdgeKey(a, c), - SimpleFundamentalMatrix(E2, 50, 50, center, center)); + initialEstimate.insert(EdgeKey(a, b), F1.retract(delta)); + initialEstimate.insert(EdgeKey(a, c), F2.retract(delta)); } initialEstimate.print("Initial Estimates:\n", formatter); // graph.printErrors(initialEstimate, "errors: ", formatter); @@ -104,10 +115,15 @@ int main(int argc, char* argv[]) { params.setVerbosityLM("SUMMARY"); Values result = LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize(); - result.print("Final results:\n", formatter); + cout << "initial error = " << graph.error(initialEstimate) << endl; cout << "final error = " << graph.error(result) << endl; + result.print("Final results:\n", formatter); + + cout << "Ground Truth F1:\n" << F1.matrix() << endl; + cout << "Ground Truth F2:\n" << F2.matrix() << endl; + return 0; } /* ************************************************************************* */