From 77754fd69b9a984f4419089d2975f6ebe825cce0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 16:40:53 -0700 Subject: [PATCH] Example with SimpleF --- examples/SFMdata.h | 27 ++++++-- examples/ViewGraphExample.cpp | 113 ++++++++++++++++++++++++++++++++++ 2 files changed, 134 insertions(+), 6 deletions(-) create mode 100644 examples/ViewGraphExample.cpp diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 112bd927c..f2561b490 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -78,12 +78,27 @@ std::vector createPoses( /** * Create regularly spaced poses with specified radius and number of cameras */ -std::vector posesOnCircle(int num_cameras = 8, double radius = 30) { - const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {radius, 0, 0}); - const double theta = M_PI / num_cameras; - const Pose3 delta( - Rot3::Ypr(0, -2 * theta, 0), - {sin(2 * theta) * radius, 0, radius * (1 - sin(2 * theta))}); +std::vector posesOnCircle(int num_cameras = 8, double R = 30) { + const double theta = 2 * M_PI / num_cameras; + + // Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis + // pointing down + const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {R, 0, 0}); + + // Delta rotation: rotate by -theta around Z-axis (counterclockwise movement) + Rot3 delta_rotation = Rot3::Ypr(0, -theta, 0); + + // Delta translation in world frame + Vector3 delta_translation_world(R * (cos(theta) - 1), R * sin(theta), 0); + + // Transform delta translation to local frame of the camera + Vector3 delta_translation_local = + init.rotation().inverse() * delta_translation_world; + + // Define delta pose + const Pose3 delta(delta_rotation, delta_translation_local); + + // Generate poses using createPoses return createPoses(init, delta, num_cameras); } } // namespace gtsam \ No newline at end of file diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp new file mode 100644 index 000000000..6d5131942 --- /dev/null +++ b/examples/ViewGraphExample.cpp @@ -0,0 +1,113 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ViewGraphExample.cpp + * @brief View-graph calibration on a simulated dataset, a la Sweeney 2015 + * @author Frank Dellaert + * @author October 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "SFMdata.h" +#include "gtsam/geometry/EssentialMatrix.h" +#include "gtsam/inference/Key.h" + +using namespace std; +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)); + + // Create the set of 8 ground-truth landmarks + vector points = createPoints(); + + // Create the set of 4 ground-truth poses + vector poses = posesOnCircle(4, 30); + + // 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); + 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 + NonlinearFactorGraph graph; + + 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) { + 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], + p[c][j], p[b][j]); + graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), p[c][j], + p[b][j], p[a][j]); + } + } + + auto formatter = [](Key key) { + EdgeKey edge(key); + return (std::string)edge; + }; + + graph.print("Factor Graph:\n", formatter); + + // 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.print("Initial Estimates:\n", formatter); + // graph.printErrors(initialEstimate, "errors: ", 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(); + result.print("Final results:\n", formatter); + cout << "initial error = " << graph.error(initialEstimate) << endl; + cout << "final error = " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */