diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index a7cac5dbf..e6948cfab 100644 --- a/examples/EssentialViewGraphExample.cpp +++ b/examples/EssentialViewGraphExample.cpp @@ -17,28 +17,30 @@ */ #include +#include #include #include #include #include #include +#include #include #include #include -#include +#include // Contains EssentialTransferFactor #include -#include "SFMdata.h" -#include "gtsam/inference/Key.h" +#include "SFMdata.h" // For createPoints() and posesOnCircle() using namespace std; using namespace gtsam; +using namespace symbol_shorthand; // For K(symbol) -/* ************************************************************************* */ +// Main function int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); + Cal3_S2 K_initial(50.0, 50.0, 0.0, 50.0, 50.0); // Create the set of 8 ground-truth landmarks vector points = createPoints(); @@ -46,28 +48,22 @@ 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 = FundamentalMatrix(K, poses[0].between(poses[1]), K); - auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K); + // Calculate ground truth essential matrices, 1 and 2 poses apart + auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1])); + auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2])); // Simulate measurements from each camera pose std::array, 4> p; for (size_t i = 0; i < 4; ++i) { - PinholeCamera camera(poses[i], K); + PinholeCamera camera(poses[i], K_initial); for (size_t j = 0; j < 8; ++j) { p[i][j] = camera.project(points[j]); } } - // 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. + // Create the factor graph NonlinearFactorGraph graph; - using Factor = TransferFactor; + using Factor = EssentialTransferFactor; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera @@ -83,54 +79,60 @@ int main(int argc, char* argv[]) { tuples3.emplace_back(p[c][j], p[b][j], p[a][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. + // Add transfer factors between views a, b, and c. graph.emplace_shared(EdgeKey(a, c), EdgeKey(b, c), tuples1); graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), tuples2); graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), tuples3); } + // Formatter for printing keys auto formatter = [](Key key) { - EdgeKey edge(key); - return (std::string)edge; + if (Symbol(key).chr() == 'k') { + return (string)Symbol(key); + } else { + EdgeKey edge(key); + return (std::string)edge; + } }; 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 *= 1e-5; + // Create a delta vector to perturb the ground truth (small perturbation) + Vector5 delta; + delta << 1, 1, 1, 1, 1; + delta *= 1e-2; - // Create the data structure to hold the initial estimate to the solution + // Create the initial estimate for essential matrices Values initialEstimate; 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), F1.retract(delta)); - initialEstimate.insert(EdgeKey(a, c), F2.retract(delta)); + initialEstimate.insert(EdgeKey(a, b), E1.retract(delta)); + initialEstimate.insert(EdgeKey(a, c), E2.retract(delta)); } - initialEstimate.print("Initial Estimates:\n", formatter); - graph.printErrors(initialEstimate, "errors: ", formatter); - /* Optimize the graph and print results */ + // Insert initial calibrations (using K symbol) + for (size_t i = 0; i < 4; ++i) { + initialEstimate.insert(K(i), K_initial); + } + + initialEstimate.print("Initial Estimates:\n", formatter); + graph.printErrors(initialEstimate, "Initial Errors:\n", formatter); + + // Optimize the graph and print results LevenbergMarquardtParams params; params.setlambdaInitial(1000.0); // Initialize lambda to a high value params.setVerbosityLM("SUMMARY"); Values result = LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize(); - cout << "initial error = " << graph.error(initialEstimate) << endl; - cout << "final error = " << graph.error(result) << endl; + cout << "Initial error = " << graph.error(initialEstimate) << endl; + cout << "Final error = " << graph.error(result) << endl; - result.print("Final results:\n", formatter); + result.print("Final Results:\n", formatter); - cout << "Ground Truth F1:\n" << F1.matrix() << endl; - cout << "Ground Truth F2:\n" << F2.matrix() << endl; + cout << "Ground Truth E1:\n" << E1.matrix() << endl; + cout << "Ground Truth E2:\n" << E2.matrix() << endl; return 0; -} -/* ************************************************************************* */ +} \ No newline at end of file