12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1887 from borglab/feature/essential_transfer 

EssentialTransferFactor, EssentialTransferFactorK, and python wrapping
release/4.3a0
Frank Dellaert 2024-11-05 17:20:40 -05:00 committed by GitHub
parent 2bd2d82f19 be9195465e
commit a0f4955431
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
22 changed files with 1720 additions and 249 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
examples/EssentialViewGraphExample.cpp Normal file
View File

@ -0,0 +1,138 @@
/* ----------------------------------------------------------------------------
* 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 EssentialViewGraphExample.cpp
* @brief View-graph calibration with essential matrices.
* @author Frank Dellaert
* @date October 2024
*/
#include <gtsam/geometry/Cal3f.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/EdgeKey.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/TransferFactor.h> // Contains EssentialTransferFactorK
#include <vector>
#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
Cal3f cal(50.0, 50.0, 50.0);
// Create the set of 8 ground-truth landmarks
vector<Point3> points = createPoints();
// Create the set of 4 ground-truth poses
vector<Pose3> poses = posesOnCircle(4, 30);
// 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<std::array<Point2, 8>, 4> p;
for (size_t i = 0; i < 4; ++i) {
PinholeCamera<Cal3f> camera(poses[i], cal);
for (size_t j = 0; j < 8; ++j) {
p[i][j] = camera.project(points[j]);
}
}
// Create the factor graph
NonlinearFactorGraph graph;
using Factor = EssentialTransferFactorK<Cal3f>;
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
// Vectors to collect tuples for each factor
std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;
// Collect data for the three factors
for (size_t j = 0; j < 8; ++j) {
tuples1.emplace_back(p[a][j], p[b][j], p[c][j]);
tuples2.emplace_back(p[a][j], p[c][j], p[b][j]);
tuples3.emplace_back(p[c][j], p[b][j], p[a][j]);
}
// Add transfer factors between views a, b, and c.
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
}
// Formatter for printing keys
auto formatter = [](Key key) {
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 (small perturbation)
Vector5 delta;
delta << 1, 1, 1, 1, 1;
delta *= 1e-2;
// 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), E1.retract(delta));
initialEstimate.insert(EdgeKey(a, c), E2.retract(delta));
}
// Insert initial calibrations (using K symbol)
for (size_t i = 0; i < 4; ++i) {
initialEstimate.insert(K(i), cal);
}
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;
result.print("Final Results:\n", formatter);
E1.print("Ground Truth E1:\n");
E2.print("Ground Truth E2:\n");
return 0;
}

2
examples/ViewGraphExample.cpp
View File

@ -114,7 +114,7 @@ int main(int argc, char* argv[]) {
initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
}
initialEstimate.print("Initial Estimates:\n", formatter);
graph.printErrors(initialEstimate, "errors: ", formatter);
graph.printErrors(initialEstimate, "errors: ", formatter);
/* Optimize the graph and print results */
LevenbergMarquardtParams params;

6
gtsam/geometry/Cal3Bundler.cpp
View File

@ -56,10 +56,8 @@ void Cal3Bundler::print(const std::string& s) const {
/* ************************************************************************* */
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
const Cal3* base = dynamic_cast<const Cal3*>(&K);
return (Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
std::fabs(v0_ - K.v0_) < tol);
return Cal3f::equals(static_cast<const Cal3f&>(K), tol) &&
std::fabs(k1_ - K.k1_) < tol && std::fabs(k2_ - K.k2_) < tol;
}
/* ************************************************************************* */

41
gtsam/geometry/Cal3Bundler.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Cal3f.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -29,22 +29,18 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3Bundler : public Cal3 {
class GTSAM_EXPORT Cal3Bundler : public Cal3f {
private:
double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion
double tol_ = 1e-5; ///< tolerance value when calibrating
double k1_ = 0.0, k2_ = 0.0; ///< radial distortion coefficients
double tol_ = 1e-5; ///< tolerance value when calibrating
// NOTE: We use the base class fx to represent the common focal length.
// Also, image center parameters (u0, v0) are not optimized
// but are treated as constants.
// Note: u0 and v0 are constants and not optimized.
public:
enum { dimension = 3 };
///< shared pointer to stereo calibration object
using shared_ptr = std::shared_ptr<Cal3Bundler>;
/// @name Standard Constructors
/// @name Constructors
/// @{
/// Default constructor
@ -61,9 +57,9 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
*/
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
double tol = 1e-5)
: Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
: Cal3f(f, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
~Cal3Bundler() override {}
~Cal3Bundler() override = default;
/// @}
/// @name Testable
@ -77,24 +73,18 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
void print(const std::string& s = "") const override;
/// assert equality up to a tolerance
bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
bool equals(const Cal3Bundler& K, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// distorsion parameter k1
/// distortion parameter k1
inline double k1() const { return k1_; }
/// distorsion parameter k2
/// distortion parameter k2
inline double k2() const { return k2_; }
/// image center in x
inline double px() const { return u0_; }
/// image center in y
inline double py() const { return v0_; }
Matrix3 K() const override; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
@ -113,8 +103,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
/**
* Convert a pixel coordinate to ideal coordinate xy
* @param p point in image coordinates
* @param tol optional tolerance threshold value for iterative minimization
* @param pi point in image coordinates
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
@ -135,14 +124,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
/// @name Manifold
/// @{
/// return DOF, dimensionality of tangent space
/// Return DOF, dimensionality of tangent space
size_t dim() const override { return Dim(); }
/// return DOF, dimensionality of tangent space
/// Return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// Update calibration with tangent space delta
inline Cal3Bundler retract(const Vector& d) const {
Cal3Bundler retract(const Vector& d) const {
return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
}

106
gtsam/geometry/Cal3f.cpp Normal file
View File

@ -0,0 +1,106 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Cal3f.cpp
* @brief Implementation file for Cal3f class
* @author Frank Dellaert
* @date October 2024
*/
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3f.h>
#include <iostream>
namespace gtsam {
/* ************************************************************************* */
Cal3f::Cal3f(double f, double u0, double v0) : Cal3(f, f, 0.0, u0, v0) {}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3f& cal) {
os << "f: " << cal.fx_ << ", px: " << cal.u0_ << ", py: " << cal.v0_;
return os;
}
/* ************************************************************************* */
void Cal3f::print(const std::string& s) const {
if (!s.empty()) std::cout << s << " ";
std::cout << *this << std::endl;
}
/* ************************************************************************* */
bool Cal3f::equals(const Cal3f& K, double tol) const {
return Cal3::equals(static_cast<const Cal3&>(K), tol);
}
/* ************************************************************************* */
Vector1 Cal3f::vector() const {
Vector1 v;
v << fx_;
return v;
}
/* ************************************************************************* */
Point2 Cal3f::uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal,
OptionalJacobian<2, 2> Dp) const {
assert(fx_ == fy_ && s_ == 0.0);
const double x = p.x(), y = p.y();
const double u = fx_ * x + u0_;
const double v = fx_ * y + v0_;
if (Dcal) {
Dcal->resize(2, 1);
(*Dcal) << x, y;
}
if (Dp) {
Dp->resize(2, 2);
(*Dp) << fx_, 0.0, //
0.0, fx_;
}
return Point2(u, v);
}
/* ************************************************************************* */
Point2 Cal3f::calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal,
OptionalJacobian<2, 2> Dp) const {
assert(fx_ == fy_ && s_ == 0.0);
const double u = pi.x(), v = pi.y();
double delta_u = u - u0_, delta_v = v - v0_;
double inv_f = 1.0 / fx_;
Point2 point(inv_f * delta_u, inv_f * delta_v);
if (Dcal) {
Dcal->resize(2, 1);
(*Dcal) << -inv_f * point.x(), -inv_f * point.y();
}
if (Dp) {
Dp->resize(2, 2);
(*Dp) << inv_f, 0.0, //
0.0, inv_f;
}
return point;
}
} // namespace gtsam

141
gtsam/geometry/Cal3f.h Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Cal3f.h
* @brief Calibration model with a single focal length and zero skew.
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/Cal3.h>
namespace gtsam {
/**
* @brief Calibration model with a single focal length and zero skew.
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3f : public Cal3 {
public:
enum { dimension = 1 };
using shared_ptr = std::shared_ptr<Cal3f>;
/// @name Constructors
/// @{
/// Default constructor
Cal3f() = default;
/**
* Constructor
* @param f focal length
* @param u0 image center x-coordinate (considered a constant)
* @param v0 image center y-coordinate (considered a constant)
*/
Cal3f(double f, double u0, double v0);
~Cal3f() override = default;
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const Cal3f& cal);
/// print with optional string
void print(const std::string& s = "") const override;
/// assert equality up to a tolerance
bool equals(const Cal3f& K, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length
inline double f() const { return fx_; }
/// vectorized form (column-wise)
Vector1 vector() const;
/**
* @brief: convert intrinsic coordinates xy to image coordinates uv
* Version of uncalibrate with derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*1 Jacobian wrpt focal length
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/**
* Convert a pixel coordinate to ideal coordinate xy
* @param pi point in image coordinates
* @param Dcal optional 2*1 Jacobian wrpt focal length
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal = {},
OptionalJacobian<2, 2> Dp = {}) const;
/// @}
/// @name Manifold
/// @{
/// Return DOF, dimensionality of tangent space
size_t dim() const override { return Dim(); }
/// Return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// Update calibration with tangent space delta
Cal3f retract(const Vector& d) const { return Cal3f(fx_ + d(0), u0_, v0_); }
/// Calculate local coordinates to another calibration
Vector1 localCoordinates(const Cal3f& T2) const {
return Vector1(T2.fx_ - fx_);
}
/// @}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(fx_);
ar& BOOST_SERIALIZATION_NVP(u0_);
ar& BOOST_SERIALIZATION_NVP(v0_);
}
#endif
/// @}
};
template <>
struct traits<Cal3f> : public internal::Manifold<Cal3f> {};
template <>
struct traits<const Cal3f> : public internal::Manifold<Cal3f> {};
} // namespace gtsam

125
gtsam/geometry/geometry.i
View File

@ -644,8 +644,36 @@ class EssentialMatrix {
double error(gtsam::Vector vA, gtsam::Vector vB);
};
#include <gtsam/geometry/Cal3.h>
virtual class Cal3 {
// Standard Constructors
Cal3();
Cal3(double fx, double fy, double s, double u0, double v0);
Cal3(gtsam::Vector v);
// Testable
void print(string s = "Cal3") const;
bool equals(const gtsam::Cal3& rhs, double tol) const;
// Standard Interface
double fx() const;
double fy() const;
double aspectRatio() const;
double skew() const;
double px() const;
double py() const;
gtsam::Point2 principalPoint() const;
gtsam::Vector vector() const;
gtsam::Matrix K() const;
gtsam::Matrix inverse() const;
// Manifold
static size_t Dim();
size_t dim() const;
};
#include <gtsam/geometry/Cal3_S2.h>
class Cal3_S2 {
virtual class Cal3_S2 : gtsam::Cal3 {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
@ -672,23 +700,12 @@ class Cal3_S2 {
Eigen::Ref<Eigen::MatrixXd> Dcal,
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double px() const;
double py() const;
gtsam::Point2 principalPoint() const;
gtsam::Vector vector() const;
gtsam::Matrix K() const;
gtsam::Matrix inverse() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base {
virtual class Cal3DS2_Base : gtsam::Cal3 {
// Standard Constructors
Cal3DS2_Base();
@ -696,16 +713,8 @@ virtual class Cal3DS2_Base {
void print(string s = "") const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double px() const;
double py() const;
double k1() const;
double k2() const;
gtsam::Matrix K() const;
gtsam::Vector k() const;
gtsam::Vector vector() const;
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
@ -785,7 +794,7 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
};
#include <gtsam/geometry/Cal3Fisheye.h>
class Cal3Fisheye {
virtual class Cal3Fisheye : gtsam::Cal3 {
// Standard Constructors
Cal3Fisheye();
Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1,
@ -797,8 +806,6 @@ class Cal3Fisheye {
bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Fisheye retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;
@ -813,35 +820,23 @@ class Cal3Fisheye {
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double k1() const;
double k2() const;
double k3() const;
double k4() const;
double px() const;
double py() const;
gtsam::Point2 principalPoint() const;
gtsam::Vector vector() const;
gtsam::Vector k() const;
gtsam::Matrix K() const;
gtsam::Matrix inverse() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
class Cal3_S2Stereo {
virtual class Cal3_S2Stereo : gtsam::Cal3{
// Standard Constructors
Cal3_S2Stereo();
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
Cal3_S2Stereo(gtsam::Vector v);
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3_S2Stereo retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
@ -850,35 +845,23 @@ class Cal3_S2Stereo {
bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double px() const;
double py() const;
gtsam::Matrix K() const;
gtsam::Point2 principalPoint() const;
double baseline() const;
gtsam::Vector6 vector() const;
gtsam::Matrix inverse() const;
};
#include <gtsam/geometry/Cal3Bundler.h>
class Cal3Bundler {
virtual class Cal3f : gtsam::Cal3 {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
Cal3Bundler(double fx, double k1, double k2, double u0, double v0,
double tol);
Cal3f();
Cal3f(double fx, double u0, double v0);
// Testable
void print(string s = "") const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
bool equals(const gtsam::Cal3f& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Bundler retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
gtsam::Cal3f retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::Cal3f& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
@ -891,15 +874,32 @@ class Cal3Bundler {
Eigen::Ref<Eigen::MatrixXd> Dp) const;
// Standard Interface
double fx() const;
double fy() const;
double f() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3Bundler.h>
virtual class Cal3Bundler : gtsam::Cal3f {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
Cal3Bundler(double fx, double k1, double k2, double u0, double v0,
double tol);
// Testable
void print(string s = "") const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold
gtsam::Cal3Bundler retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Standard Interface
double k1() const;
double k2() const;
double px() const;
double py() const;
gtsam::Vector vector() const;
gtsam::Vector k() const;
gtsam::Matrix K() const;
// enabling serialization functionality
void serialize() const;
@ -1071,6 +1071,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3f> PinholeCameraCal3f;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
#include <gtsam/geometry/PinholePose.h>

40
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -29,7 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2, 3);
/* ************************************************************************* */
TEST(Cal3Bundler, vector) {
TEST(Cal3Bundler, Vector) {
Cal3Bundler K;
Vector expected(3);
expected << 1, 0, 0;
@ -37,16 +37,19 @@ TEST(Cal3Bundler, vector) {
}
/* ************************************************************************* */
TEST(Cal3Bundler, uncalibrate) {
TEST(Cal3Bundler, Uncalibrate) {
Vector v = K.vector();
double r = p.x() * p.x() + p.y() * p.y();
double g = v[0] * (1 + v[1] * r + v[2] * r * r);
Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
double distortion = 1.0 + v[1] * r + v[2] * r * r;
double u = K.px() + v[0] * distortion * p.x();
double v_coord = K.py() + v[0] * distortion * p.y();
Point2 expected(u, v_coord);
Point2 actual = K.uncalibrate(p);
CHECK(assert_equal(expected, actual));
}
TEST(Cal3Bundler, calibrate) {
/* ************************************************************************* */
TEST(Cal3Bundler, Calibrate) {
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
@ -63,11 +66,11 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibrateDefault) {
TEST(Cal3Bundler, DUncalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
Point2 expected = p;
Point2 expected(p); // Since K is identity, uncalibrate should return p
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
@ -76,7 +79,7 @@ TEST(Cal3Bundler, DuncalibrateDefault) {
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibrateDefault) {
TEST(Cal3Bundler, DCalibrateDefault) {
Cal3Bundler trueK(1, 0, 0);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
@ -90,11 +93,11 @@ TEST(Cal3Bundler, DcalibrateDefault) {
}
/* ************************************************************************* */
TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
TEST(Cal3Bundler, DUncalibratePrincipalPoint) {
Cal3Bundler K(5, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(12, 17);
Point2 expected(2.0 + 5.0 * p.x(), 2.0 + 5.0 * p.y());
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
@ -103,7 +106,7 @@ TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
}
/* ************************************************************************* */
TEST(Cal3Bundler, DcalibratePrincipalPoint) {
TEST(Cal3Bundler, DCalibratePrincipalPoint) {
Cal3Bundler K(2, 0, 0, 2, 2);
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
@ -117,11 +120,18 @@ TEST(Cal3Bundler, DcalibratePrincipalPoint) {
}
/* ************************************************************************* */
TEST(Cal3Bundler, Duncalibrate) {
TEST(Cal3Bundler, DUncalibrate) {
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(2182, 3773);
// Compute expected value manually
Vector v = K.vector();
double r2 = p.x() * p.x() + p.y() * p.y();
double distortion = 1.0 + v[1] * r2 + v[2] * r2 * r2;
Point2 expected(
K.px() + v[0] * distortion * p.x(),
K.py() + v[0] * distortion * p.y());
CHECK(assert_equal(expected, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-7));
@ -129,7 +139,7 @@ TEST(Cal3Bundler, Duncalibrate) {
}
/* ************************************************************************* */
TEST(Cal3Bundler, Dcalibrate) {
TEST(Cal3Bundler, DCalibrate) {
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
@ -145,7 +155,7 @@ TEST(Cal3Bundler, Dcalibrate) {
TEST(Cal3Bundler, assert_equal) { CHECK(assert_equal(K, K, 1e-7)); }
/* ************************************************************************* */
TEST(Cal3Bundler, retract) {
TEST(Cal3Bundler, Retract) {
Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
EXPECT_LONGS_EQUAL(3, expected.dim());

132
gtsam/geometry/tests/testCal3f.cpp Normal file
View File

@ -0,0 +1,132 @@
/* ----------------------------------------------------------------------------
* 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 testCal3F.cpp
* @brief Unit tests for the Cal3f calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3f.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3f)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3f)
static Cal3f K(500.0, 1000.0, 2000.0); // f = 500, u0 = 1000, v0 = 2000
static Point2 p(2.0, 3.0);
/* ************************************************************************* */
TEST(Cal3f, Vector) {
Cal3f K(1.0, 0.0, 0.0);
Vector expected(1);
expected << 1.0;
CHECK(assert_equal(expected, K.vector()));
}
/* ************************************************************************* */
TEST(Cal3f, Uncalibrate) {
// Expected output: apply the intrinsic calibration matrix to point p
Matrix3 K_matrix = K.K();
Vector3 p_homogeneous(p.x(), p.y(), 1.0);
Vector3 expected_homogeneous = K_matrix * p_homogeneous;
Point2 expected(expected_homogeneous.x() / expected_homogeneous.z(),
expected_homogeneous.y() / expected_homogeneous.z());
Point2 actual = K.uncalibrate(p);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Cal3f, Calibrate) {
Point2 pi = K.uncalibrate(p);
Point2 pn = K.calibrate(pi);
CHECK(traits<Point2>::Equals(p, pn, 1e-9));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3f& k, const Point2& pt) {
return k.uncalibrate(pt);
}
Point2 calibrate_(const Cal3f& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST(Cal3f, DUncalibrate) {
Cal3f K(500.0, 1000.0, 2000.0);
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
// Expected value computed manually
Point2 expected = Point2(K.px() + K.fx() * p.x(), K.py() + K.fx() * p.y());
CHECK(assert_equal(expected, actual, 1e-9));
// Compute numerical derivatives
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-6));
CHECK(assert_equal(numerical2, Dp, 1e-6));
}
/* ************************************************************************* */
TEST(Cal3f, DCalibrate) {
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
Point2 actual = K.calibrate(pi, Dcal, Dp);
CHECK(assert_equal(p, actual, 1e-9));
// Compute numerical derivatives
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1, Dcal, 1e-6));
CHECK(assert_equal(numerical2, Dp, 1e-6));
}
/* ************************************************************************* */
TEST(Cal3f, Manifold) {
Cal3f K1(500.0, 1000.0, 2000.0);
Vector1 delta;
delta << 10.0; // Increment focal length by 10
Cal3f K2 = K1.retract(delta);
CHECK(assert_equal(510.0, K2.fx(), 1e-9));
CHECK(assert_equal(K1.px(), K2.px(), 1e-9));
CHECK(assert_equal(K1.py(), K2.py(), 1e-9));
Vector1 delta_computed = K1.localCoordinates(K2);
CHECK(assert_equal(delta, delta_computed, 1e-9));
}
/* ************************************************************************* */
TEST(Cal3f, Assert_equal) {
CHECK(assert_equal(K, K, 1e-9));
Cal3f K2(500.0, 1000.0, 2000.0);
CHECK(assert_equal(K, K2, 1e-9));
}
/* ************************************************************************* */
TEST(Cal3f, Print) {
Cal3f cal(500.0, 1000.0, 2000.0);
std::stringstream os;
os << "f: " << cal.fx() << ", px: " << cal.px() << ", py: " << cal.py();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

5
gtsam/inference/EdgeKey.h
View File

@ -46,7 +46,10 @@ class GTSAM_EXPORT EdgeKey {
/// @{
/// Cast to Key
operator Key() const { return ((std::uint64_t)i_ << 32) | j_; }
Key key() const { return ((std::uint64_t)i_ << 32) | j_; }
/// Cast to Key
operator Key() const { return key(); }
/// Retrieve high 32 bits
inline std::uint32_t i() const { return i_; }

13
gtsam/inference/inference.i
View File

@ -96,6 +96,19 @@ unsigned char mrsymbolChr(size_t key);
unsigned char mrsymbolLabel(size_t key);
size_t mrsymbolIndex(size_t key);
#include <gtsam/inference/EdgeKey.h>
class EdgeKey {
EdgeKey(std::uint32_t i, std::uint32_t j);
EdgeKey(size_t key);
EdgeKey(const gtsam::EdgeKey& key);
std::uint32_t i() const;
std::uint32_t j() const;
size_t key() const;
void print(string s = "") const;
};
#include <gtsam/inference/Ordering.h>
class Ordering {
/// Type of ordering to use

12
gtsam/nonlinear/nonlinear.i
View File

@ -75,13 +75,20 @@ class NonlinearFactorGraph {
gtsam::Pose2,
gtsam::Pose3,
gtsam::Cal3_S2,
gtsam::Cal3f,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::CalibratedCamera,
gtsam::EssentialMatrix,
gtsam::FundamentalMatrix,
gtsam::SimpleFundamentalMatrix,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3f>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::PinholeCamera<gtsam::CalibratedCamera>,
gtsam::imuBias::ConstantBias}>
void addPrior(size_t key, const T& prior,
const gtsam::noiseModel::Base* noiseModel);
@ -537,9 +544,8 @@ class ISAM2 {
gtsam::Values calculateEstimate() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::Cal3Bundler, gtsam::FundamentalMatrix,
gtsam::Cal3Bundler, gtsam::SimpleFundamentalMatrix,
gtsam::Cal3f, gtsam::Cal3Bundler,
gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,

91
gtsam/nonlinear/values.i
View File

@ -4,13 +4,14 @@
namespace gtsam {
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3f.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
@ -67,7 +68,7 @@ class Values {
// gtsam::Value at(size_t j) const;
// The order is important: gtsam::Vector has to precede Point2/Point3 so `atVector`
// can work for those fixed-size vectors.
// can work for those fixed-size vectors. Same apparently for Cal3Bundler and Cal3f.
void insert(size_t j, gtsam::Vector vector);
void insert(size_t j, gtsam::Matrix matrix);
void insert(size_t j, const gtsam::Point2& point2);
@ -80,18 +81,25 @@ class Values {
void insert(size_t j, const gtsam::Rot3& rot3);
void insert(size_t j, const gtsam::Pose3& pose3);
void insert(size_t j, const gtsam::Unit3& unit3);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::Cal3f& cal3f);
void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
void insert(size_t j, const gtsam::Cal3Unified& cal3unified);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::EssentialMatrix& E);
void insert(size_t j, const gtsam::FundamentalMatrix& F);
void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3DS2>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3f>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
@ -115,18 +123,25 @@ class Values {
void update(size_t j, const gtsam::Rot3& rot3);
void update(size_t j, const gtsam::Pose3& pose3);
void update(size_t j, const gtsam::Unit3& unit3);
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::Cal3f& cal3f);
void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
void update(size_t j, const gtsam::Cal3Unified& cal3unified);
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::EssentialMatrix& E);
void update(size_t j, const gtsam::FundamentalMatrix& F);
void update(size_t j, const gtsam::SimpleFundamentalMatrix& F);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3DS2>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3f>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
@ -147,31 +162,28 @@ class Values {
void insert_or_assign(size_t j, const gtsam::Rot3& rot3);
void insert_or_assign(size_t j, const gtsam::Pose3& pose3);
void insert_or_assign(size_t j, const gtsam::Unit3& unit3);
void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f);
void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2);
void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2);
void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified);
void insert_or_assign(size_t j,
const gtsam::EssentialMatrix& essential_matrix);
void insert_or_assign(size_t j,
const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void insert_or_assign(size_t j,
const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j,
const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j,
const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j,
const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert_or_assign(size_t j,
const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j,
const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j,
const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j,
const gtsam::imuBias::ConstantBias& constant_bias);
void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E);
void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F);
void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3DS2>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3f>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3DS2>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
void insert_or_assign(size_t j, double c);
@ -185,18 +197,25 @@ class Values {
gtsam::Rot3,
gtsam::Pose3,
gtsam::Unit3,
gtsam::Cal3Bundler,
gtsam::Cal3f,
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::EssentialMatrix,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::EssentialMatrix,
gtsam::FundamentalMatrix,
gtsam::SimpleFundamentalMatrix,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3f>,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3DS2>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::PinholePose<gtsam::Cal3Bundler>,
gtsam::PinholePose<gtsam::Cal3f>,
gtsam::PinholePose<gtsam::Cal3_S2>,
gtsam::PinholePose<gtsam::Cal3DS2>,
gtsam::PinholePose<gtsam::Cal3Fisheye>,
gtsam::PinholePose<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias,

308
gtsam/sfm/TransferFactor.h
View File

@ -16,12 +16,66 @@
#pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/inference/EdgeKey.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <cstdint>
namespace gtsam {
/**
* Base class that holds the EdgeKeys and provides the getMatrices method.
*/
template <typename F>
class TransferEdges {
protected:
EdgeKey edge1_, edge2_; ///< The two EdgeKeys.
uint32_t c_; ///< The transfer target
public:
TransferEdges(EdgeKey edge1, EdgeKey edge2)
: edge1_(edge1), edge2_(edge2), c_(ViewC(edge1, edge2)) {}
/// Returns the view A index based on the EdgeKeys
static size_t ViewA(const EdgeKey& edge1, const EdgeKey& edge2) {
size_t c = ViewC(edge1, edge2);
return (edge1.i() == c) ? edge1.j() : edge1.i();
}
/// Returns the view B index based on the EdgeKeys
static size_t ViewB(const EdgeKey& edge1, const EdgeKey& edge2) {
size_t c = ViewC(edge1, edge2);
return (edge2.i() == c) ? edge2.j() : edge2.i();
}
/// Returns the view C index based on the EdgeKeys
static size_t ViewC(const EdgeKey& edge1, const EdgeKey& edge2) {
if (edge1.i() == edge2.i() || edge1.i() == edge2.j())
return edge1.i();
else if (edge1.j() == edge2.i() || edge1.j() == edge2.j())
return edge1.j();
else
throw std::runtime_error(
"EssentialTransferFactorK: No common key in edge keys.");
}
/// Create Matrix3 objects based on EdgeKey configurations.
std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
// Determine whether to transpose F1
const Matrix3 Fca =
edge1_.i() == c_ ? F1.matrix() : F1.matrix().transpose();
// Determine whether to transpose F2
const Matrix3 Fcb =
edge2_.i() == c_ ? F2.matrix() : F2.matrix().transpose();
return {Fca, Fcb};
}
};
/**
* Binary factor in the context of Structure from Motion (SfM).
* It is used to transfer transfer corresponding points from two views to a
@ -30,89 +84,219 @@ namespace gtsam {
* the target view. Jacobians are done using numerical differentiation.
*/
template <typename F>
class TransferFactor : public NoiseModelFactorN<F, F> {
EdgeKey key1_, key2_; ///< the two EdgeKeys
std::vector<std::tuple<Point2, Point2, Point2>>
triplets_; ///< Point triplets
class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
public:
using Base = NoiseModelFactorN<F, F>;
using Triplet = std::tuple<Point2, Point2, Point2>;
protected:
std::vector<Triplet> triplets_; ///< Point triplets.
public:
/**
* @brief Constructor for a single triplet of points
*
* @note: batching all points for the same transfer will be much faster.
*
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param pa The point in the first view (a).
* @param pb The point in the second view (b).
* @param pc The point in the third (and transfer target) view (c).
* @param model An optional SharedNoiseModel that defines the noise model
* for this factor. Defaults to nullptr.
*/
TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb,
const Point2& pc, const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<F, F>(model, key1, key2),
key1_(key1),
key2_(key2),
triplets_({std::make_tuple(pa, pb, pc)}) {}
/**
* @brief Constructor that accepts a vector of point triplets.
*
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param edge1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param triplets A vector of triplets containing (pa, pb, pc).
* @param model An optional SharedNoiseModel that defines the noise model
* for this factor. Defaults to nullptr.
*/
TransferFactor(
EdgeKey key1, EdgeKey key2,
const std::vector<std::tuple<Point2, Point2, Point2>>& triplets,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<F, F>(model, key1, key2),
key1_(key1),
key2_(key2),
TransferFactor(EdgeKey edge1, EdgeKey edge2,
const std::vector<Triplet>& triplets,
const SharedNoiseModel& model = nullptr)
: Base(model, edge1, edge2),
TransferEdges<F>(edge1, edge2),
triplets_(triplets) {}
// Create Matrix3 objects based on EdgeKey configurations
std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
// Fill Fca and Fcb based on EdgeKey configurations
if (key1_.i() == key2_.i()) {
return {F1.matrix(), F2.matrix()};
} else if (key1_.i() == key2_.j()) {
return {F1.matrix(), F2.matrix().transpose()};
} else if (key1_.j() == key2_.i()) {
return {F1.matrix().transpose(), F2.matrix()};
} else if (key1_.j() == key2_.j()) {
return {F1.matrix().transpose(), F2.matrix().transpose()};
} else {
throw std::runtime_error(
"TransferFactor: invalid EdgeKey configuration.");
}
}
/// vector of errors returns 2*N vector
/// Vector of errors returns 2*N vector.
Vector evaluateError(const F& F1, const F& F2,
OptionalMatrixType H1 = nullptr,
OptionalMatrixType H2 = nullptr) const override {
std::function<Vector(const F&, const F&)> transfer = [&](const F& F1,
const F& F2) {
std::function<Vector(const F&, const F&)> errorFunction = [&](const F& f1,
const F& f2) {
Vector errors(2 * triplets_.size());
size_t idx = 0;
auto [Fca, Fcb] = getMatrices(F1, F2);
for (const auto& tuple : triplets_) {
const auto& [pa, pb, pc] = tuple;
Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb);
Vector2 error = transferredPoint - pc;
errors.segment<2>(idx) = error;
auto [Fca, Fcb] = this->getMatrices(f1, f2);
for (const auto& [pa, pb, pc] : triplets_) {
errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc;
idx += 2;
}
return errors;
};
if (H1) *H1 = numericalDerivative21<Vector, F, F>(transfer, F1, F2);
if (H2) *H2 = numericalDerivative22<Vector, F, F>(transfer, F1, F2);
return transfer(F1, F2);
if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2);
if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2);
return errorFunction(F1, F2);
}
};
/**
* @class EssentialTransferFactor
* @brief Transfers points between views using essential matrices with a shared
* calibration.
*
* This factor is templated on the calibration class K and extends
* the TransferFactor for EssentialMatrices. It involves two essential matrices
* and a shared calibration object (K). The evaluateError function calibrates
* the image points, calls the base class's transfer method, and computes the
* error using bulk numerical differentiation.
*/
template <typename K>
class EssentialTransferFactor : public TransferFactor<EssentialMatrix> {
using EM = EssentialMatrix;
using Triplet = std::tuple<Point2, Point2, Point2>;
std::shared_ptr<K> calibration_; ///< Shared pointer to calibration object
public:
using Base = TransferFactor<EM>;
using This = EssentialTransferFactor<K>;
using shared_ptr = std::shared_ptr<This>;
/**
* @brief Constructor that accepts a vector of point triplets and a shared
* calibration.
*
* @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
* @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
* @param triplets A vector of triplets containing (pa, pb, pc)
* @param calibration Shared pointer to calibration object
* @param model An optional SharedNoiseModel
*/
EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2,
const std::vector<Triplet>& triplets,
const std::shared_ptr<K>& calibration,
const SharedNoiseModel& model = nullptr)
: Base(edge1, edge2, triplets, model), calibration_(calibration) {}
/// Transfer points pa and pb to view c and evaluate error.
Vector2 TransferError(const Matrix3& Eca, const Point2& pa,
const Matrix3& Ecb, const Point2& pb,
const Point2& pc) const {
const Point2 pA = calibration_->calibrate(pa);
const Point2 pB = calibration_->calibrate(pb);
const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
return calibration_->uncalibrate(pC) - pc;
}
/// Evaluate error function
Vector evaluateError(const EM& E1, const EM& E2,
OptionalMatrixType H1 = nullptr,
OptionalMatrixType H2 = nullptr) const override {
std::function<Vector(const EM&, const EM&)> errorFunction =
[&](const EM& e1, const EM& e2) {
Vector errors(2 * this->triplets_.size());
size_t idx = 0;
auto [Eca, Ecb] = this->getMatrices(e1, e2);
for (const auto& [pa, pb, pc] : this->triplets_) {
errors.segment<2>(idx) = TransferError(Eca, pa, Ecb, pb, pc);
idx += 2;
}
return errors;
};
// Compute error
Vector errors = errorFunction(E1, E2);
// Compute Jacobians if requested
if (H1) *H1 = numericalDerivative21(errorFunction, E1, E2);
if (H2) *H2 = numericalDerivative22(errorFunction, E1, E2);
return errors;
}
};
/**
* @class EssentialTransferFactorK
* @brief Transfers points between views using essential matrices, optimizes for
* calibrations of the views, as well. Note that the EssentialMatrixFactor4 does
* something similar but without transfer.
*
* @note Derives calibration keys from edges, and uses symbol 'k'.
*
* This factor is templated on the calibration class K and extends
* the TransferFactor for EssentialMatrices. It involves two essential matrices
* and three calibration objects (Ka, Kb, Kc). The evaluateError
* function calibrates the image points, calls the base class's transfer method,
* and computes the error using bulk numerical differentiation.
*/
template <typename K>
class EssentialTransferFactorK
: public NoiseModelFactorN<EssentialMatrix, EssentialMatrix, K, K, K>,
TransferEdges<EssentialMatrix> {
using EM = EssentialMatrix;
using Triplet = std::tuple<Point2, Point2, Point2>;
std::vector<Triplet> triplets_; ///< Point triplets
public:
using Base = NoiseModelFactorN<EM, EM, K, K, K>;
using This = EssentialTransferFactorK<K>;
using shared_ptr = std::shared_ptr<This>;
/**
* @brief Constructor that accepts a vector of point triplets.
*
* @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
* @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
* @param triplets A vector of triplets containing (pa, pb, pc)
* @param model An optional SharedNoiseModel
*/
EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2,
const std::vector<Triplet>& triplets,
const SharedNoiseModel& model = nullptr)
: Base(model, edge1, edge2,
Symbol('k', ViewA(edge1, edge2)), // calibration key for view a
Symbol('k', ViewB(edge1, edge2)), // calibration key for view b
Symbol('k', ViewC(edge1, edge2))), // calibration key for target c
TransferEdges<EM>(edge1, edge2),
triplets_(triplets) {}
/// Transfer points pa and pb to view c and evaluate error.
Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa,
const Matrix3& Ecb, const K& Kb, const Point2& pb,
const K& Kc, const Point2& pc) const {
const Point2 pA = Ka.calibrate(pa);
const Point2 pB = Kb.calibrate(pb);
const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
return Kc.uncalibrate(pC) - pc;
}
/// Evaluate error function
Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb,
const K& Kc, OptionalMatrixType H1 = nullptr,
OptionalMatrixType H2 = nullptr,
OptionalMatrixType H3 = nullptr,
OptionalMatrixType H4 = nullptr,
OptionalMatrixType H5 = nullptr) const override {
std::function<Vector(const EM&, const EM&, const K&, const K&, const K&)>
errorFunction = [&](const EM& e1, const EM& e2, const K& kA,
const K& kB, const K& kC) {
Vector errors(2 * triplets_.size());
size_t idx = 0;
auto [Eca, Ecb] = this->getMatrices(e1, e2);
for (const auto& [pa, pb, pc] : triplets_) {
errors.segment<2>(idx) =
TransferError(Eca, kA, pa, Ecb, kB, pb, kC, pc);
idx += 2;
}
return errors;
};
// Compute error
Vector errors = errorFunction(E1, E2, Ka, Kb, Kc);
// Compute Jacobians if requested
if (H1) *H1 = numericalDerivative51(errorFunction, E1, E2, Ka, Kb, Kc);
if (H2) *H2 = numericalDerivative52(errorFunction, E1, E2, Ka, Kb, Kc);
if (H3) *H3 = numericalDerivative53(errorFunction, E1, E2, Ka, Kb, Kc);
if (H4) *H4 = numericalDerivative54(errorFunction, E1, E2, Ka, Kb, Kc);
if (H5) *H5 = numericalDerivative55(errorFunction, E1, E2, Ka, Kb, Kc);
return errors;
}
/// Return the dimension of the factor
size_t dim() const override { return 2 * triplets_.size(); }
};
} // namespace gtsam

27
gtsam/sfm/sfm.i
View File

@ -75,6 +75,33 @@ bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
#include <gtsam/sfm/TransferFactor.h>
#include <gtsam/geometry/FundamentalMatrix.h>
template <F = {gtsam::SimpleFundamentalMatrix, gtsam::FundamentalMatrix}>
virtual class TransferFactor : gtsam::NoiseModelFactor {
TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
const gtsam::noiseModel::Base* model = nullptr);
};
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3f.h>
#include <gtsam/geometry/Cal3Bundler.h>
template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
virtual class EssentialTransferFactor : gtsam::NoiseModelFactor {
EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
const K* calibration,
const gtsam::noiseModel::Base* model = nullptr);
};
template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor {
EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
const gtsam::noiseModel::Base* model = nullptr);
};
#include <gtsam/sfm/ShonanFactor.h>
virtual class ShonanFactor3 : gtsam::NoiseModelFactor {

133
gtsam/sfm/tests/testTransferFactor.cpp
View File

@ -1,19 +1,23 @@
/*
* @file testTransferFactor.cpp
* @brief Test TransferFactor class
* @author Your Name
* @date October 23, 2024
* @brief Test TransferFactor classes
* @author Frank Dellaert
* @date October 2024
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/sfm/TransferFactor.h>
#include <memory>
using namespace gtsam;
using symbol_shorthand::K;
//*************************************************************************
/// Generate three cameras on a circle, looking in
/// Generate three cameras on a circle, looking inwards
std::array<Pose3, 3> generateCameraPoses() {
std::array<Pose3, 3> cameraPoses;
const double radius = 1.0;
@ -48,8 +52,12 @@ std::array<Pose3, 3> cameraPoses = generateCameraPoses();
auto triplet = generateTripleF(cameraPoses);
double focalLength = 1000;
Point2 principalPoint(640 / 2, 480 / 2);
const Cal3_S2 K(focalLength, focalLength, 0.0, //
principalPoint.x(), principalPoint.y());
const Cal3_S2 cal(focalLength, focalLength, 0.0, //
principalPoint.x(), principalPoint.y());
// Create cameras
auto f = [](const Pose3& pose) { return PinholeCamera<Cal3_S2>(pose, cal); };
std::array<PinholeCamera<Cal3_S2>, 3> cameras = {
f(cameraPoses[0]), f(cameraPoses[1]), f(cameraPoses[2])};
} // namespace fixture
//*************************************************************************
@ -71,20 +79,17 @@ TEST(TransferFactor, Jacobians) {
// Now project a point into the three cameras
const Point3 P(0.1, 0.2, 0.3);
std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) {
// Project the point into each camera
PinholeCameraCal3_S2 camera(cameraPoses[i], K);
p[i] = camera.project(P);
p[i] = cameras[i].project(P);
}
// Create a TransferFactor
EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
TransferFactor<SimpleFundamentalMatrix> //
factor0{key01, key20, p[1], p[2], p[0]},
factor1{key12, key01, p[2], p[0], p[1]},
factor2{key20, key12, p[0], p[1], p[2]};
factor0{key01, key20, {{p[1], p[2], p[0]}}},
factor1{key12, key01, {{p[2], p[0], p[1]}}},
factor2{key20, key12, {{p[0], p[1], p[2]}}};
// Create Values with edge keys
Values values;
@ -107,19 +112,14 @@ TEST(TransferFactor, MultipleTuples) {
// Now project multiple points into the three cameras
const size_t numPoints = 5; // Number of points to test
std::vector<Point3> points3D;
std::vector<std::array<Point2, 3>> projectedPoints;
// Generate random 3D points and project them
for (size_t n = 0; n < numPoints; ++n) {
Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
points3D.push_back(P);
std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) {
// Project the point into each camera
PinholeCameraCal3_S2 camera(cameraPoses[i], K);
p[i] = camera.project(P);
p[i] = cameras[i].project(P);
}
projectedPoints.push_back(p);
}
@ -153,9 +153,102 @@ TEST(TransferFactor, MultipleTuples) {
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
}
//*************************************************************************
// Test for EssentialTransferFactor
TEST(EssentialTransferFactor, Jacobians) {
using namespace fixture;
// Create EssentialMatrices between cameras
EssentialMatrix E01 =
EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1]));
EssentialMatrix E02 =
EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2]));
// Project a point into the three cameras
const Point3 P(0.1, 0.2, 0.3);
std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) {
p[i] = cameras[i].project(P);
}
// Create EdgeKeys
EdgeKey key01(0, 1);
EdgeKey key02(0, 2);
// Create an EssentialTransferFactor
auto sharedCal = std::make_shared<Cal3_S2>(cal);
EssentialTransferFactor<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}},
sharedCal);
// Create Values and insert variables
Values values;
values.insert(key01, E01); // Essential matrix between views 0 and 1
values.insert(key02, E02); // Essential matrix between views 0 and 2
// Check error
Matrix H1, H2, H3, H4, H5;
Vector error = factor.evaluateError(E01, E02, //
&H1, &H2);
EXPECT(H1.rows() == 2 && H1.cols() == 5)
EXPECT(H2.rows() == 2 && H2.cols() == 5)
EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
// Check Jacobians
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
}
//*************************************************************************
// Test for EssentialTransferFactorK
TEST(EssentialTransferFactorK, Jacobians) {
using namespace fixture;
// Create EssentialMatrices between cameras
EssentialMatrix E01 =
EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1]));
EssentialMatrix E02 =
EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2]));
// Project a point into the three cameras
const Point3 P(0.1, 0.2, 0.3);
std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) {
p[i] = cameras[i].project(P);
}
// Create EdgeKeys
EdgeKey key01(0, 1);
EdgeKey key02(0, 2);
// Create an EssentialTransferFactorK
EssentialTransferFactorK<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}});
// Create Values and insert variables
Values values;
values.insert(key01, E01); // Essential matrix between views 0 and 1
values.insert(key02, E02); // Essential matrix between views 0 and 2
values.insert(K(1), cal); // Calibration for view A (view 1)
values.insert(K(2), cal); // Calibration for view B (view 2)
values.insert(K(0), cal); // Calibration for view C (view 0)
// Check error
Matrix H1, H2, H3, H4, H5;
Vector error = factor.evaluateError(E01, E02, //
cal, cal, cal, //
&H1, &H2, &H3, &H4, &H5);
EXPECT(H1.rows() == 2 && H1.cols() == 5)
EXPECT(H2.rows() == 2 && H2.cols() == 5)
EXPECT(H3.rows() == 2 && H3.cols() == 5)
EXPECT(H4.rows() == 2 && H4.cols() == 5)
EXPECT(H5.rows() == 2 && H5.cols() == 5)
EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
// Check Jacobians
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
}
//*************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//*************************************************************************
//*************************************************************************

5
python/CMakeLists.txt
View File

@ -156,6 +156,11 @@ foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
get_filename_component(test_file_name ${test_file} NAME)
configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/examples/*.py")
foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
get_filename_component(test_file_name ${test_file} NAME)
configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY)
endforeach()
file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)

2
python/README.md
View File

@ -13,7 +13,7 @@ For instructions on updating the version of the [wrap library](https://github.co
use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),
then the environment should be active while building GTSAM.
- This wrapper needs `pyparsing(>=2.4.2)`, and `numpy(>=1.11.0)`. These can be installed as follows:
- This wrapper needs [pyparsing(>=2.4.2)](https://github.com/pyparsing/pyparsing), [pybind-stubgen>=2.5.1](https://github.com/sizmailov/pybind11-stubgen) and [numpy(>=1.11.0)](https://numpy.org/). These can all be installed as follows:
```bash
pip install -r <gtsam_folder>/python/dev_requirements.txt

120
python/gtsam/examples/EssentialViewGraphExample.py Normal file
View File

@ -0,0 +1,120 @@
"""
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
"""
"""
Python version of EssentialViewGraphExample.cpp
View-graph calibration with essential matrices.
Author: Frank Dellaert
Date: October 2024
"""
import numpy as np
from gtsam.examples import SFMdata
import gtsam
from gtsam import Cal3f, EdgeKey, EssentialMatrix
from gtsam import EssentialTransferFactorCal3f as Factor
from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
NonlinearFactorGraph, PinholeCameraCal3f, Values)
# For symbol shorthand (e.g., X(0), L(1))
K = gtsam.symbol_shorthand.K
# Formatter function for printing keys
def formatter(key):
sym = gtsam.Symbol(key)
if sym.chr() == ord("k"):
return f"k{sym.index()}"
else:
edge = EdgeKey(key)
return f"({edge.i()},{edge.j()})"
def main():
# Define the camera calibration parameters
cal = Cal3f(50.0, 50.0, 50.0)
# Create the set of 8 ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of 4 ground-truth poses
poses = SFMdata.posesOnCircle(4, 30)
# Calculate ground truth essential matrices, 1 and 2 poses apart
E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
# Simulate measurements from each camera pose
p = [[None for _ in range(8)] for _ in range(4)]
for i in range(4):
camera = PinholeCameraCal3f(poses[i], cal)
for j in range(8):
p[i][j] = camera.project(points[j])
# Create the factor graph
graph = NonlinearFactorGraph()
for a in range(4):
b = (a + 1) % 4 # Next camera
c = (a + 2) % 4 # Camera after next
# Collect data for the three factors
tuples1 = []
tuples2 = []
tuples3 = []
for j in range(8):
tuples1.append((p[a][j], p[b][j], p[c][j]))
tuples2.append((p[a][j], p[c][j], p[b][j]))
tuples3.append((p[c][j], p[b][j], p[a][j]))
# Add transfer factors between views a, b, and c.
graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1))
graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2))
graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3))
graph.print("graph", formatter)
# Create a delta vector to perturb the ground truth (small perturbation)
delta = np.ones(5) * 1e-2
# Create the initial estimate for essential matrices
initialEstimate = Values()
for a in range(4):
b = (a + 1) % 4 # Next camera
c = (a + 2) % 4 # Camera after next
initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(delta))
initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(delta))
# Insert initial calibrations
for i in range(4):
initialEstimate.insert(K(i), cal)
# Optimize the graph and print results
params = LevenbergMarquardtParams()
params.setlambdaInitial(1000.0) # Initialize lambda to a high value
params.setVerbosityLM("SUMMARY")
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
result = optimizer.optimize()
print("Initial error = ", graph.error(initialEstimate))
print("Final error = ", graph.error(result))
# Print final results
print("Final Results:")
result.print("", formatter)
# Print ground truth essential matrices
print("Ground Truth E1:\n", E1)
print("Ground Truth E2:\n", E2)
if __name__ == "__main__":
main()

39
python/gtsam/examples/SFMExample.py
View File

@ -9,20 +9,22 @@ See LICENSE for the license information
A structure-from-motion problem on a simulated dataset
"""
import gtsam
import matplotlib.pyplot as plt
import numpy as np
import gtsam
from gtsam import symbol_shorthand
L = symbol_shorthand.L
X = symbol_shorthand.X
from gtsam.examples import SFMdata
from gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
from gtsam.utils import plot
from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2,
Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2,
PriorFactorPoint3, PriorFactorPose3, Values)
def main():
"""
@ -43,7 +45,7 @@ def main():
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
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system
@ -78,8 +80,7 @@ def main():
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, measurement_noise, X(i), L(j), K)
factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
@ -88,28 +89,29 @@ def main():
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print('Factor Graph:\n')
graph.print("Factor Graph:\n")
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
rng = np.random.default_rng()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
transformed_pose = pose.retract(0.1 * rng.standard_normal(6).reshape(6, 1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = point + 0.1*np.random.randn(3)
transformed_point = point + 0.1 * rng.standard_normal(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print('Initial Estimates:\n')
initial_estimate.print("Initial Estimates:\n")
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
params.setVerbosity("TERMINATION")
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
print("Optimizing:")
result = optimizer.optimize()
result.print('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
result.print("Final results:\n")
print("initial error = {}".format(graph.error(initial_estimate)))
print("final error = {}".format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
@ -117,5 +119,6 @@ def main():
plot.set_axes_equal(1)
plt.show()
if __name__ == '__main__':
if __name__ == "__main__":
main()

372
python/gtsam/examples/ViewGraphComparison.py Normal file
View File

@ -0,0 +1,372 @@
"""
Compare several methods for optimizing the view-graph.
We measure the distance from the ground truth in terms of the norm of
local coordinates (geodesic distance) on the F-manifold.
We also plot the final error of the optimization.
Author: Frank Dellaert (with heavy assist from ChatGPT)
Date: October 2024
"""
import argparse
import matplotlib.pyplot as plt
import numpy as np
from gtsam.examples import SFMdata
import gtsam
from gtsam import (
Cal3f,
EdgeKey,
EssentialMatrix,
FundamentalMatrix,
LevenbergMarquardtOptimizer,
LevenbergMarquardtParams,
NonlinearFactorGraph,
PinholeCameraCal3f,
SimpleFundamentalMatrix,
Values,
)
# For symbol shorthand (e.g., K(0), K(1))
K = gtsam.symbol_shorthand.K
# Methods to compare
methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"]
# Formatter function for printing keys
def formatter(key):
sym = gtsam.Symbol(key)
if sym.chr() == ord("k"):
return f"k{sym.index()}"
else:
edge = EdgeKey(key)
return f"({edge.i()},{edge.j()})"
def simulate_geometry(num_cameras, rng, num_random_points=12):
"""simulate geometry (points and poses)"""
# Define the camera calibration parameters
cal = Cal3f(50.0, 50.0, 50.0)
# Create the set of 8 ground-truth landmarks
points = SFMdata.createPoints()
# Create extra random points in the -10,10 cube around the origin
extra_points = rng.uniform(-10, 10, (num_random_points, 3))
points.extend([gtsam.Point3(p) for p in extra_points])
# Create the set of ground-truth poses
poses = SFMdata.posesOnCircle(num_cameras, 30)
return points, poses, cal
def simulate_data(points, poses, cal, rng, noise_std):
"""Simulate measurements from each camera pose"""
measurements = [[None for _ in points] for _ in poses]
for i, pose in enumerate(poses):
camera = PinholeCameraCal3f(pose, cal)
for j, point in enumerate(points):
projection = camera.project(point)
noise = rng.normal(0, noise_std, size=2)
measurements[i][j] = projection + noise
return measurements
# Function to compute ground truth matrices
def compute_ground_truth(method, poses, cal):
E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1]))
E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2]))
F1 = FundamentalMatrix(cal.K(), E1, cal.K())
F2 = FundamentalMatrix(cal.K(), E2, cal.K())
if method == "Fundamental":
return F1, F2
elif method == "SimpleF":
f = cal.fx()
c = cal.principalPoint()
SF1 = SimpleFundamentalMatrix(E1, f, f, c, c)
SF2 = SimpleFundamentalMatrix(E2, f, f, c, c)
return SF1, SF2
elif method == "Essential+Ks" or method == "Calibrated":
return E1, E2
else:
raise ValueError(f"Unknown method {method}")
def build_factor_graph(method, num_cameras, measurements, cal):
"""build the factor graph"""
graph = NonlinearFactorGraph()
if method == "Fundamental":
FactorClass = gtsam.TransferFactorFundamentalMatrix
elif method == "SimpleF":
FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix
elif method == "Essential+Ks":
FactorClass = gtsam.EssentialTransferFactorKCal3f
# add priors on all calibrations:
for i in range(num_cameras):
model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0)
graph.addPriorCal3f(K(i), cal, model)
elif method == "Calibrated":
FactorClass = gtsam.EssentialTransferFactorCal3f
# No priors on calibration needed
else:
raise ValueError(f"Unknown method {method}")
z = measurements # shorthand
for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera
c = (a + 2) % num_cameras # Camera after next
# Vectors to collect tuples for each factor
tuples1 = []
tuples2 = []
tuples3 = []
# Collect data for the three factors
for j in range(len(measurements[0])):
tuples1.append((z[a][j], z[b][j], z[c][j]))
tuples2.append((z[a][j], z[c][j], z[b][j]))
tuples3.append((z[c][j], z[b][j], z[a][j]))
# Add transfer factors between views a, b, and c.
if method in ["Calibrated"]:
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal))
else:
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1))
graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2))
graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3))
return graph
def get_initial_estimate(method, num_cameras, ground_truth, cal):
"""get initial estimate for method"""
initialEstimate = Values()
total_dimension = 0
if method in ["Fundamental", "SimpleF"]:
F1, F2 = ground_truth
for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera
c = (a + 2) % num_cameras # Camera after next
initialEstimate.insert(EdgeKey(a, b).key(), F1)
initialEstimate.insert(EdgeKey(a, c).key(), F2)
total_dimension += F1.dim() + F2.dim()
elif method in ["Essential+Ks", "Calibrated"]:
E1, E2 = ground_truth
for a in range(num_cameras):
b = (a + 1) % num_cameras # Next camera
c = (a + 2) % num_cameras # Camera after next
initialEstimate.insert(EdgeKey(a, b).key(), E1)
initialEstimate.insert(EdgeKey(a, c).key(), E2)
total_dimension += E1.dim() + E2.dim()
else:
raise ValueError(f"Unknown method {method}")
if method == "Essential+Ks":
# Insert initial calibrations
for i in range(num_cameras):
initialEstimate.insert(K(i), cal)
total_dimension += cal.dim()
print(f"Total dimension of the problem: {total_dimension}")
return initialEstimate
def optimize(graph, initialEstimate, method):
"""optimize the graph"""
params = LevenbergMarquardtParams()
params.setlambdaInitial(1e10) # Initialize lambda to a high value
params.setlambdaUpperBound(1e10)
# params.setAbsoluteErrorTol(0.1)
params.setVerbosityLM("SUMMARY")
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
result = optimizer.optimize()
iterations = optimizer.iterations()
return result, iterations
def compute_distances(method, result, ground_truth, num_cameras, cal):
"""Compute geodesic distances from ground truth"""
distances = []
F1, F2 = ground_truth["Fundamental"]
for a in range(num_cameras):
b = (a + 1) % num_cameras
c = (a + 2) % num_cameras
key_ab = EdgeKey(a, b).key()
key_ac = EdgeKey(a, c).key()
if method in ["Essential+Ks", "Calibrated"]:
E_est_ab = result.atEssentialMatrix(key_ab)
E_est_ac = result.atEssentialMatrix(key_ac)
# Compute estimated FundamentalMatrices
if method == "Fundamental":
F_est_ab = result.atFundamentalMatrix(key_ab)
F_est_ac = result.atFundamentalMatrix(key_ac)
elif method == "SimpleF":
SF_est_ab = result.atSimpleFundamentalMatrix(key_ab).matrix()
SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix()
F_est_ab = FundamentalMatrix(SF_est_ab)
F_est_ac = FundamentalMatrix(SF_est_ac)
elif method == "Essential+Ks":
# Retrieve calibrations from result:
cal_a = result.atCal3f(K(a))
cal_b = result.atCal3f(K(b))
cal_c = result.atCal3f(K(c))
# Convert estimated EssentialMatrices to FundamentalMatrices
F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K())
F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K())
elif method == "Calibrated":
# Use ground truth calibration
F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K())
F_est_ac = FundamentalMatrix(cal.K(), E_est_ac, cal.K())
else:
raise ValueError(f"Unknown method {method}")
# Compute local coordinates (geodesic distance on the F-manifold)
dist_ab = np.linalg.norm(F1.localCoordinates(F_est_ab))
dist_ac = np.linalg.norm(F2.localCoordinates(F_est_ac))
distances.append(dist_ab)
distances.append(dist_ac)
return distances
def plot_results(results):
"""plot results"""
methods = list(results.keys())
final_errors = [results[method]["final_error"] for method in methods]
distances = [results[method]["distances"] for method in methods]
iterations = [results[method]["iterations"] for method in methods]
fig, ax1 = plt.subplots()
color = "tab:red"
ax1.set_xlabel("Method")
ax1.set_ylabel("Median Error (log scale)", color=color)
ax1.set_yscale("log")
ax1.bar(methods, final_errors, color=color, alpha=0.6)
ax1.tick_params(axis="y", labelcolor=color)
ax2 = ax1.twinx()
color = "tab:blue"
ax2.set_ylabel("Median Geodesic Distance", color=color)
ax2.plot(methods, distances, color=color, marker="o", linestyle="-")
ax2.tick_params(axis="y", labelcolor=color)
# Annotate the blue data points with the average number of iterations
for i, method in enumerate(methods):
ax2.annotate(
f"{iterations[i]:.1f}",
(i, distances[i]),
textcoords="offset points",
xytext=(0, 10),
ha="center",
color=color,
)
plt.title("Comparison of Methods (Labels show avg iterations)")
fig.tight_layout()
plt.show()
# Main function
def main():
# Parse command line arguments
parser = argparse.ArgumentParser(description="Compare Fundamental and Essential Matrix Methods")
parser.add_argument("--num_cameras", type=int, default=4, help="Number of cameras (default: 4)")
parser.add_argument("--num_extra_points", type=int, default=12, help="Number of extra random points (default: 12)")
parser.add_argument("--num_trials", type=int, default=5, help="Number of trials (default: 5)")
parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)")
parser.add_argument("--noise_std", type=float, default=0.5, help="Standard deviation of noise (default: 0.5)")
args = parser.parse_args()
# Initialize the random number generator
rng = np.random.default_rng(seed=args.seed)
# Initialize results dictionary
results = {method: {"distances": [], "final_error": [], "iterations": []} for method in methods}
# Simulate geometry
points, poses, cal = simulate_geometry(args.num_cameras, rng, args.num_extra_points)
# Compute ground truth matrices
ground_truth = {method: compute_ground_truth(method, poses, cal) for method in methods}
# Get initial estimates
initial_estimate: dict[Values] = {
method: get_initial_estimate(method, args.num_cameras, ground_truth[method], cal) for method in methods
}
simple_f_result: Values = Values()
for trial in range(args.num_trials):
print(f"\nTrial {trial + 1}/{args.num_trials}")
# Simulate data
measurements = simulate_data(points, poses, cal, rng, args.noise_std)
for method in methods:
print(f"\nRunning method: {method}")
# Build the factor graph
graph = build_factor_graph(method, args.num_cameras, measurements, cal)
# For F, initialize from SimpleF:
if method == "Fundamental":
initial_estimate[method] = simple_f_result
# Optimize the graph
result, iterations = optimize(graph, initial_estimate[method], method)
# Store SimpleF result as a set of FundamentalMatrices
if method == "SimpleF":
simple_f_result = Values()
for a in range(args.num_cameras):
b = (a + 1) % args.num_cameras # Next camera
c = (a + 2) % args.num_cameras # Camera after next
key_ab = EdgeKey(a, b).key()
key_ac = EdgeKey(a, c).key()
F1 = result.atSimpleFundamentalMatrix(key_ab).matrix()
F2 = result.atSimpleFundamentalMatrix(key_ac).matrix()
simple_f_result.insert(key_ab, FundamentalMatrix(F1))
simple_f_result.insert(key_ac, FundamentalMatrix(F2))
# Compute distances from ground truth
distances = compute_distances(method, result, ground_truth, args.num_cameras, cal)
# Compute final error
final_error = graph.error(result)
# Store results
results[method]["distances"].extend(distances)
results[method]["final_error"].append(final_error)
results[method]["iterations"].append(iterations)
print(f"Method: {method}")
print(f"Final Error: {final_error:.3f}")
print(f"Mean Geodesic Distance: {np.mean(distances):.3f}")
print(f"Number of Iterations: {iterations}\n")
# Average results over trials
for method in methods:
results[method]["final_error"] = np.median(results[method]["final_error"])
results[method]["distances"] = np.median(results[method]["distances"])
results[method]["iterations"] = np.median(results[method]["iterations"])
# Plot results
plot_results(results)
if __name__ == "__main__":
main()

111
python/gtsam/examples/ViewGraphExample.py Normal file
View File

@ -0,0 +1,111 @@
"""
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
"""
"""
Python version of ViewGraphExample.cpp
View-graph calibration on a simulated dataset, a la Sweeney 2015
Author: Frank Dellaert
Date: October 2024
"""
import numpy as np
from gtsam.examples import SFMdata
from gtsam import (Cal3_S2, EdgeKey, FundamentalMatrix,
LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
NonlinearFactorGraph, PinholeCameraCal3_S2)
from gtsam import TransferFactorFundamentalMatrix as Factor
from gtsam import Values
# Formatter function for printing keys
def formatter(key):
edge = EdgeKey(key)
return f"({edge.i()},{edge.j()})"
def main():
# Define the camera calibration parameters
cal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Create the set of 8 ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of 4 ground-truth poses
poses = SFMdata.posesOnCircle(4, 30)
# Calculate ground truth fundamental matrices, 1 and 2 poses apart
F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K())
F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K())
# Simulate measurements from each camera pose
p = [[None for _ in range(8)] for _ in range(4)]
for i in range(4):
camera = PinholeCameraCal3_S2(poses[i], cal)
for j in range(8):
p[i][j] = camera.project(points[j])
# Create the factor graph
graph = NonlinearFactorGraph()
for a in range(4):
b = (a + 1) % 4 # Next camera
c = (a + 2) % 4 # Camera after next
# Vectors to collect tuples for each factor
tuples1 = []
tuples2 = []
tuples3 = []
# Collect data for the three factors
for j in range(8):
tuples1.append((p[a][j], p[b][j], p[c][j]))
tuples2.append((p[a][j], p[c][j], p[b][j]))
tuples3.append((p[c][j], p[b][j], p[a][j]))
# Add transfer factors between views a, b, and c.
graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1))
graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2))
graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3))
# Print the factor graph
graph.print("Factor Graph:\n", formatter)
# Create a delta vector to perturb the ground truth
delta = np.array([1, 2, 3, 4, 5, 6, 7]) * 1e-5
# Create the data structure to hold the initial estimate to the solution
initialEstimate = Values()
for a in range(4):
b = (a + 1) % 4 # Next camera
c = (a + 2) % 4 # Camera after next
initialEstimate.insert(EdgeKey(a, b).key(), F1.retract(delta))
initialEstimate.insert(EdgeKey(a, c).key(), F2.retract(delta))
initialEstimate.print("Initial Estimates:\n", formatter)
graph.printErrors(initialEstimate, "Initial Errors:\n", formatter)
# Optimize the graph and print results
params = LevenbergMarquardtParams()
params.setlambdaInitial(1000.0) # Initialize lambda to a high value
params.setVerbosityLM("SUMMARY")
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params)
result = optimizer.optimize()
print(f"Initial error = {graph.error(initialEstimate)}")
print(f"Final error = {graph.error(result)}")
result.print("Final Results:\n", formatter)
print("Ground Truth F1:\n", F1.matrix())
print("Ground Truth F2:\n", F2.matrix())
if __name__ == "__main__":
main()