diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index fddca81694..0b4dc4036f 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -39,7 +39,7 @@ // Finally, once all of the factors have been added to our factor graph, we will want to // solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. // GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg +// trust-region method known as Powell's Dogleg #include // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the @@ -57,7 +57,7 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + auto K = std::make_shared(50.0, 50.0, 0.0, 50.0, 50.0); // Define the camera observation noise model auto measurementNoise = diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 3031828f19..f2561b4900 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -16,56 +16,89 @@ */ /** - * A structure-from-motion example with landmarks, default function arguments give + * A structure-from-motion example with landmarks, default arguments give: * - The landmarks form a 10 meter cube * - The robot rotates around the landmarks, always facing towards the cube - * Passing function argument allows to specificy an initial position, a pose increment and step count. + * Passing function argument allows to specify an initial position, a pose + * increment and step count. */ #pragma once -// As this is a full 3D problem, we will use Pose3 variables to represent the camera -// positions and Point3 variables (x, y, z) to represent the landmark coordinates. -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -// We will also need a camera object to hold calibration information and perform projections. -#include +// As this is a full 3D problem, we will use Pose3 variables to represent the +// camera positions and Point3 variables (x, y, z) to represent the landmark +// coordinates. Camera observations of landmarks (i.e. pixel coordinates) will +// be stored as Point2 (x, y). #include +#include -// We will also need a camera object to hold calibration information and perform projections. -#include +// We will also need a camera object to hold calibration information and perform +// projections. #include +#include -/* ************************************************************************* */ -std::vector createPoints() { +namespace gtsam { - // Create the set of ground-truth landmarks - std::vector points; - points.push_back(gtsam::Point3(10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); +/// Create a set of ground-truth landmarks +std::vector createPoints() { + std::vector points; + points.push_back(Point3(10.0, 10.0, 10.0)); + points.push_back(Point3(-10.0, 10.0, 10.0)); + points.push_back(Point3(-10.0, -10.0, 10.0)); + points.push_back(Point3(10.0, -10.0, 10.0)); + points.push_back(Point3(10.0, 10.0, -10.0)); + points.push_back(Point3(-10.0, 10.0, -10.0)); + points.push_back(Point3(-10.0, -10.0, -10.0)); + points.push_back(Point3(10.0, -10.0, -10.0)); return points; } -/* ************************************************************************* */ -std::vector createPoses( - const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), - const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), - int steps = 8) { +/** + * Create a set of ground-truth poses + * Default values give a circular trajectory, radius 30 at pi/4 intervals, + * always facing the circle center + */ +std::vector createPoses( + const Pose3& init = Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}), + const Pose3& delta = Pose3(Rot3::Ypr(0, -M_PI_4, 0), + {sin(M_PI_4) * 30, 0, 30 * (1 - sin(M_PI_4))}), + int steps = 8) { + std::vector poses; + poses.reserve(steps); - // Create the set of ground-truth poses - // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center - std::vector poses; - int i = 1; poses.push_back(init); - for(; i < steps; ++i) { - poses.push_back(poses[i-1].compose(delta)); + for (int i = 1; i < steps; ++i) { + poses.push_back(poses[i - 1].compose(delta)); } return poses; } + +/** + * Create regularly spaced poses with specified radius and number of cameras + */ +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 0000000000..c23ac084c9 --- /dev/null +++ b/examples/ViewGraphExample.cpp @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * 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 + * @date October 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "SFMdata.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 K(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); + + // 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); + + // Simulate measurements from each camera pose + std::array, 4> p; + for (size_t i = 0; i < 4; ++i) { + PinholeCamera camera(poses[i], K); + 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. + 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 + + // Vectors to collect tuples for each factor + std::vector> 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. Note that the EdgeKeys + // are crucial in performing the transfer in the right direction. We use + // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental + // matrices we will optimize for. + graph.emplace_shared(EdgeKey(a, c), EdgeKey(b, c), tuples1); + graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), tuples2); + graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), tuples3); + } + + auto formatter = [](Key key) { + 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 the data structure to hold the initial estimate to the solution + 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.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(); + + cout << "initial error = " << graph.error(initialEstimate) << endl; + cout << "final error = " << graph.error(result) << endl; + + result.print("Final results:\n", formatter); + + cout << "Ground Truth F1:\n" << F1.matrix() << endl; + cout << "Ground Truth F2:\n" << F2.matrix() << endl; + + return 0; +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 71ca47418b..d65053d196 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -10,8 +10,8 @@ namespace gtsam { //************************************************************************* -Point2 Transfer(const Matrix3& Fca, const Point2& pa, // - const Matrix3& Fcb, const Point2& pb) { +Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, // + const Matrix3& Fcb, const Point2& pb) { // Create lines in camera a from projections of the other two cameras Vector3 line_a = Fca * Vector3(pa.x(), pa.y(), 1); Vector3 line_b = Fcb * Vector3(pb.x(), pb.y(), 1); @@ -24,6 +24,7 @@ Point2 Transfer(const Matrix3& Fca, const Point2& pa, // return intersectionPoint.head<2>(); // Return the 2D point } + //************************************************************************* FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { // Perform SVD @@ -71,9 +72,7 @@ Matrix3 FundamentalMatrix::matrix() const { } void FundamentalMatrix::print(const std::string& s) const { - std::cout << s << "U:\n" - << U_.matrix() << "\ns: " << s_ << "\nV:\n" - << V_.matrix() << std::endl; + std::cout << s << matrix() << std::endl; } bool FundamentalMatrix::equals(const FundamentalMatrix& other, @@ -98,20 +97,20 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { } //************************************************************************* -Matrix3 SimpleFundamentalMatrix::leftK() const { +Matrix3 SimpleFundamentalMatrix::Ka() const { Matrix3 K; K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; return K; } -Matrix3 SimpleFundamentalMatrix::rightK() const { +Matrix3 SimpleFundamentalMatrix::Kb() const { Matrix3 K; K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1; return K; } Matrix3 SimpleFundamentalMatrix::matrix() const { - return leftK().transpose().inverse() * E_.matrix() * rightK().inverse(); + return Ka().transpose().inverse() * E_.matrix() * Kb().inverse(); } void SimpleFundamentalMatrix::print(const std::string& s) const { diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 718364ca0c..6f04f45e8e 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -54,6 +54,23 @@ class GTSAM_EXPORT FundamentalMatrix { */ FundamentalMatrix(const Matrix3& F); + /** + * @brief Construct from calibration matrices Ka, Kb, and pose aPb + * + * Initializes the FundamentalMatrix from the given calibration + * matrices Ka and Kb, and the pose aPb. + * + * @tparam CAL Calibration type, expected to have a matrix() method + * @param Ka Calibration matrix for the left camera + * @param aPb Pose from the left to the right camera + * @param Kb Calibration matrix for the right camera + */ + template + FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) + : FundamentalMatrix(Ka.K().transpose().inverse() * + EssentialMatrix::FromPose3(aPb).matrix() * + Kb.K().inverse()) {} + /// Return the fundamental matrix representation Matrix3 matrix() const; @@ -97,25 +114,32 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { Point2 ca_; ///< Principal point for left camera Point2 cb_; ///< Principal point for right camera + /// Return the left calibration matrix + Matrix3 Ka() const; + + /// Return the right calibration matrix + Matrix3 Kb() const; + public: /// Default constructor SimpleFundamentalMatrix() : E_(), fa_(1.0), fb_(1.0), ca_(0.0, 0.0), cb_(0.0, 0.0) {} - /// Construct from essential matrix and focal lengths + /** + * @brief Construct from essential matrix and focal lengths + * @param E Essential matrix + * @param fa Focal length for left camera + * @param fb Focal length for right camera + * @param ca Principal point for left camera + * @param cb Principal point for right camera + */ SimpleFundamentalMatrix(const EssentialMatrix& E, // - double fa, double fb, - const Point2& ca = Point2(0.0, 0.0), - const Point2& cb = Point2(0.0, 0.0)) + double fa, double fb, const Point2& ca, + const Point2& cb) : E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {} - /// Return the left calibration matrix - Matrix3 leftK() const; - - /// Return the right calibration matrix - Matrix3 rightK() const; - /// Return the fundamental matrix representation + /// F = Ka^(-T) * E * Kb^(-1) Matrix3 matrix() const; /// @name Testable @@ -147,8 +171,8 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { * Take two fundamental matrices Fca and Fcb, and two points pa and pb, and * returns the 2D point in view (c) where the epipolar lines intersect. */ -GTSAM_EXPORT Point2 Transfer(const Matrix3& Fca, const Point2& pa, - const Matrix3& Fcb, const Point2& pb); +GTSAM_EXPORT Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, + const Matrix3& Fcb, const Point2& pb); /// Represents a set of three fundamental matrices for transferring points /// between three cameras. @@ -158,17 +182,17 @@ struct TripleF { /// Transfers a point from cameras b,c to camera a. Point2 transferToA(const Point2& pb, const Point2& pc) { - return Transfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc); + return EpipolarTransfer(Fab.matrix(), pb, Fca.matrix().transpose(), pc); } /// Transfers a point from camera a,c to camera b. Point2 transferToB(const Point2& pa, const Point2& pc) { - return Transfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc); + return EpipolarTransfer(Fab.matrix().transpose(), pa, Fbc.matrix(), pc); } /// Transfers a point from cameras a,b to camera c. Point2 transferToC(const Point2& pa, const Point2& pb) { - return Transfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb); + return EpipolarTransfer(Fca.matrix(), pa, Fbc.matrix().transpose(), pb); } }; diff --git a/gtsam/inference/EdgeKey.cpp b/gtsam/inference/EdgeKey.cpp new file mode 100644 index 0000000000..f8caf8ce1b --- /dev/null +++ b/gtsam/inference/EdgeKey.cpp @@ -0,0 +1,36 @@ +/* ---------------------------------------------------------------------------- + + * 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 EdgeKey.cpp + * @date Oct 24, 2024 + * @author: Frank Dellaert + * @author: Akshay Krishnan + */ + +#include + +namespace gtsam { + +EdgeKey::operator std::string() const { + return "{" + std::to_string(i_) + ", " + std::to_string(j_) + "}"; +} + +GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const EdgeKey& key) { + os << (std::string)key; + return os; +} + +void EdgeKey::print(const std::string& s) const { + std::cout << s << *this << std::endl; +} + +} // namespace gtsam diff --git a/gtsam/inference/EdgeKey.h b/gtsam/inference/EdgeKey.h new file mode 100644 index 0000000000..bf1bf60300 --- /dev/null +++ b/gtsam/inference/EdgeKey.h @@ -0,0 +1,81 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file EdgeKey.h + * @date Oct 24, 2024 + * @author: Frank Dellaert + * @author: Akshay Krishnan + */ + +#include +#include + +namespace gtsam { +class GTSAM_EXPORT EdgeKey { + protected: + std::uint32_t i_; ///< Upper 32 bits + std::uint32_t j_; ///< Lower 32 bits + + public: + /// @name Constructors + /// @{ + + /// Default constructor + EdgeKey() : i_(0), j_(0) {} + + /// Constructor + EdgeKey(std::uint32_t i, std::uint32_t j) : i_(i), j_(j) {} + + EdgeKey(Key key) + : i_(static_cast(key >> 32)), + j_(static_cast(key)) {} + + /// @} + /// @name API + /// @{ + + /// Cast to Key + operator Key() const { return ((std::uint64_t)i_ << 32) | j_; } + + /// Retrieve high 32 bits + inline std::uint32_t i() const { return i_; } + + /// Retrieve low 32 bits + inline std::uint32_t j() const { return j_; } + + /** Create a string from the key */ + operator std::string() const; + + /// Output stream operator + friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&, const EdgeKey&); + + /// @} + /// @name Testable + /// @{ + + /// Prints the EdgeKey with an optional prefix string. + void print(const std::string& s = "") const; + + /// Checks if this EdgeKey is equal to another, tolerance is ignored. + bool equals(const EdgeKey& expected, double tol = 0.0) const { + return (*this) == expected; + } + /// @} +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 5a7a98c1d9..927fb76692 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -19,9 +19,11 @@ #pragma once -#include +#include #include +#include + namespace gtsam { /** @@ -33,113 +35,143 @@ namespace gtsam { * which allows expressing "Pose 7 from robot B" as "xB7". */ class GTSAM_EXPORT LabeledSymbol { -protected: + protected: unsigned char c_, label_; std::uint64_t j_; -public: - /** Default constructor */ + public: + /// @name Constructors + /// @{ + + /// Default constructor LabeledSymbol(); - /** Copy constructor */ + /// Copy constructor LabeledSymbol(const LabeledSymbol& key); - /** Constructor */ + /// Constructor fro characters c and label, and integer j LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j); - /** Constructor that decodes an integer gtsam::Key */ - LabeledSymbol(gtsam::Key key); + /// Constructor that decodes an integer Key + LabeledSymbol(Key key); - /** Cast to integer */ - operator gtsam::Key() const; + /// @} + /// @name Testable + /// @{ - // Testable Requirements + /// Prints the LabeledSymbol with an optional prefix string. void print(const std::string& s = "") const; + /// Checks if this LabeledSymbol is equal to another, tolerance is ignored. bool equals(const LabeledSymbol& expected, double tol = 0.0) const { return (*this) == expected; } - /** return the integer version */ - gtsam::Key key() const { return (gtsam::Key) *this; } + /// @} + /// @name Standard API + /// @{ - /** Retrieve label character */ + /// Cast to Key + operator Key() const; + + /// return the integer version + Key key() const { return (Key) * this; } + + /// Retrieve label character inline unsigned char label() const { return label_; } - /** Retrieve key character */ + /// Retrieve key character inline unsigned char chr() const { return c_; } - /** Retrieve key index */ + /// Retrieve key index inline size_t index() const { return j_; } - /** Create a string from the key */ + /// Create a string from the key operator std::string() const; - /** Comparison for use in maps */ + /// Output stream operator that can be used with key_formatter (see Key.h). + friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&, + const LabeledSymbol&); + + /// @} + /// @name Comparison + /// @{ + bool operator<(const LabeledSymbol& comp) const; bool operator==(const LabeledSymbol& comp) const; - bool operator==(gtsam::Key comp) const; + bool operator==(Key comp) const; bool operator!=(const LabeledSymbol& comp) const; - bool operator!=(gtsam::Key comp) const; + bool operator!=(Key comp) const; + + /// @} + /// @name Filtering + /// @{ + /// Return a filter function that returns true when evaluated on a Key whose + /// character (when converted to a LabeledSymbol) matches \c c. Use this with + /// the Values::filter() function to retrieve all key-value pairs with the + /// requested character. - /** Return a filter function that returns true when evaluated on a gtsam::Key whose - * character (when converted to a LabeledSymbol) matches \c c. Use this with the - * Values::filter() function to retrieve all key-value pairs with the - * requested character. - */ + /// Checks only the type + static std::function TypeTest(unsigned char c); - // Checks only the type - static std::function TypeTest(unsigned char c); + /// Checks only the robot ID (label_) + static std::function LabelTest(unsigned char label); - // Checks only the robot ID (label_) - static std::function LabelTest(unsigned char label); + /// Checks both type and the robot ID + static std::function TypeLabelTest(unsigned char c, + unsigned char label); - // Checks both type and the robot ID - static std::function TypeLabelTest(unsigned char c, unsigned char label); + /// @} + /// @name Advanced API + /// @{ - // Converts to upper/lower versions of labels + /// Converts to upper/lower versions of labels LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); } - // Create a new symbol with a different character. - LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); } - - // Create a new symbol with a different label. - LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); } + /// Create a new symbol with a different character. + LabeledSymbol newChr(unsigned char c) const { + return LabeledSymbol(c, label_, j_); + } - /// Output stream operator that can be used with key_formatter (see Key.h). - friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const LabeledSymbol &); + /// Create a new symbol with a different label. + LabeledSymbol newLabel(unsigned char label) const { + return LabeledSymbol(c_, label, j_); + } -private: + /// @} + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION - /** Serialization function */ + /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(c_); - ar & BOOST_SERIALIZATION_NVP(label_); - ar & BOOST_SERIALIZATION_NVP(j_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(c_); + ar& BOOST_SERIALIZATION_NVP(label_); + ar& BOOST_SERIALIZATION_NVP(j_); } #endif -}; // \class LabeledSymbol +}; // \class LabeledSymbol -/** Create a symbol key from a character, label and index, i.e. xA5. */ +/// Create a symbol key from a character, label and index, i.e. xA5. inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) { - return (Key)LabeledSymbol(c,label,j); + return (Key)LabeledSymbol(c, label, j); } -/** Return the character portion of a symbol key. */ +/// Return the character portion of a symbol key. inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); } -/** Return the label portion of a symbol key. */ -inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); } +/// Return the label portion of a symbol key. +inline unsigned char mrsymbolLabel(Key key) { + return LabeledSymbol(key).label(); +} -/** Return the index portion of a symbol key. */ +/// Return the index portion of a symbol key. inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); } /// traits -template<> struct traits : public Testable {}; - -} // \namespace gtsam +template <> +struct traits : public Testable {}; +} // namespace gtsam diff --git a/gtsam/inference/tests/testEdgeKey.cpp b/gtsam/inference/tests/testEdgeKey.cpp new file mode 100644 index 0000000000..88fafbdd1f --- /dev/null +++ b/gtsam/inference/tests/testEdgeKey.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEdgeKey.cpp + * @date Oct 24, 2024 + * @author: Frank Dellaert + * @author: Akshay Krishnan + */ + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(EdgeKey, Construction) { + EdgeKey edge(1, 2); + EXPECT(edge.i() == 1); + EXPECT(edge.j() == 2); +} + +/* ************************************************************************* */ +TEST(EdgeKey, Equality) { + EdgeKey edge1(1, 2); + EdgeKey edge2(1, 2); + EdgeKey edge3(2, 3); + + EXPECT(assert_equal(edge1, edge2)); + EXPECT(!edge1.equals(edge3)); +} + +/* ************************************************************************* */ +TEST(EdgeKey, StreamOutput) { + EdgeKey edge(1, 2); + std::ostringstream oss; + oss << edge; + EXPECT("{1, 2}" == oss.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h new file mode 100644 index 0000000000..d2085f57ea --- /dev/null +++ b/gtsam/sfm/TransferFactor.h @@ -0,0 +1,118 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010-2024, 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 TransferFactor.h + * @brief TransferFactor class + * @author Frank Dellaert + * @date October 24, 2024 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor in the context of Structure from Motion (SfM). + * It is used to transfer transfer corresponding points from two views to a + * third based on two fundamental matrices. The factor computes the error + * between the transferred points `pa` and `pb`, and the actual point `pc` in + * the target view. Jacobians are done using numerical differentiation. + */ +template +class TransferFactor : public NoiseModelFactorN { + EdgeKey key1_, key2_; ///< the two EdgeKeys + std::vector> + 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(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 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>& triplets, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactorN(model, key1, key2), + key1_(key1), + key2_(key2), + triplets_(triplets) {} + + // Create Matrix3 objects based on EdgeKey configurations + std::pair 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 evaluateError(const F& F1, const F& F2, + OptionalMatrixType H1 = nullptr, + OptionalMatrixType H2 = nullptr) const override { + std::function transfer = [&](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; + idx += 2; + } + return errors; + }; + if (H1) *H1 = numericalDerivative21(transfer, F1, F2); + if (H2) *H2 = numericalDerivative22(transfer, F1, F2); + return transfer(F1, F2); + } +}; + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 33bc82f5a9..cfb9cff20d 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -55,7 +55,7 @@ class TranslationFactor : public NoiseModelFactorN { : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} /** - * @brief Caclulate error: (norm(Tb - Ta) - measurement) + * @brief Calculate error: (norm(Tb - Ta) - measurement) * where Tb and Ta are Point3 translations and measurement is * the Unit3 translation direction from a to b. * @@ -120,7 +120,7 @@ class BilinearAngleTranslationFactor using NoiseModelFactor2::evaluateError; /** - * @brief Caclulate error: (scale * (Tb - Ta) - measurement) + * @brief Calculate error: (scale * (Tb - Ta) - measurement) * where Tb and Ta are Point3 translations and measurement is * the Unit3 translation direction from a to b. * diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h index a91ef01f94..f821da9755 100644 --- a/gtsam/sfm/TranslationRecovery.h +++ b/gtsam/sfm/TranslationRecovery.h @@ -119,7 +119,7 @@ class GTSAM_EXPORT TranslationRecovery { * @param betweenTranslations relative translations (with scale) between 2 * points in world coordinate frame known a priori. * @param rng random number generator - * @param intialValues (optional) initial values from a prior + * @param initialValues (optional) initial values from a prior * @return Values */ Values initializeRandomly( @@ -156,7 +156,7 @@ class GTSAM_EXPORT TranslationRecovery { * points in world coordinate frame known a priori. Unlike * relativeTranslations, zero-magnitude betweenTranslations are not treated as * hard constraints. - * @param initialValues intial values for optimization. Initializes randomly + * @param initialValues initial values for optimization. Initializes randomly * if not provided. * @return Values */ diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp new file mode 100644 index 0000000000..2dca12c2ac --- /dev/null +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -0,0 +1,161 @@ +/* + * @file testTransferFactor.cpp + * @brief Test TransferFactor class + * @author Your Name + * @date October 23, 2024 + */ + +#include +#include +#include +#include + +using namespace gtsam; + +//************************************************************************* +/// Generate three cameras on a circle, looking in +std::array generateCameraPoses() { + std::array cameraPoses; + const double radius = 1.0; + for (int i = 0; i < 3; ++i) { + double angle = i * 2.0 * M_PI / 3.0; + double c = cos(angle), s = sin(angle); + Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0}); + cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)}; + } + return cameraPoses; +} + +//************************************************************************* +// Function to generate a TripleF from camera poses +TripleF generateTripleF( + const std::array& cameraPoses) { + std::array F; + for (size_t i = 0; i < 3; ++i) { + size_t j = (i + 1) % 3; + const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]); + EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation())); + F[i] = {E, 1000.0, 1000.0, Point2(640 / 2, 480 / 2), + Point2(640 / 2, 480 / 2)}; + } + return {F[0], F[1], F[2]}; // Return a TripleF instance +} + +//************************************************************************* +namespace fixture { +// Generate cameras on a circle +std::array 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()); +} // namespace fixture + +//************************************************************************* +// Test for getMatrices +TEST(TransferFactor, GetMatrices) { + using namespace fixture; + TransferFactor factor{{2, 0}, {1, 2}, {}}; + + // Check that getMatrices is correct + auto [Fki, Fkj] = factor.getMatrices(triplet.Fca, triplet.Fbc); + EXPECT(assert_equal(triplet.Fca.matrix(), Fki)); + EXPECT(assert_equal(triplet.Fbc.matrix().transpose(), Fkj)); +} + +//************************************************************************* +// Test for TransferFactor +TEST(TransferFactor, Jacobians) { + using namespace fixture; + + // Now project a point into the three cameras + const Point3 P(0.1, 0.2, 0.3); + + std::array 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); + } + + // Create a TransferFactor + EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); + TransferFactor // + 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; + values.insert(key01, triplet.Fab); + values.insert(key12, triplet.Fbc); + values.insert(key20, triplet.Fca); + + // Check error and Jacobians + for (auto&& f : {factor0, factor1, factor2}) { + Vector error = f.unwhitenedError(values); + EXPECT(assert_equal(Z_2x1, error)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-7); + } +} + +//************************************************************************* +// Test for TransferFactor with multiple tuples +TEST(TransferFactor, MultipleTuples) { + using namespace fixture; + + // Now project multiple points into the three cameras + const size_t numPoints = 5; // Number of points to test + std::vector points3D; + std::vector> 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 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); + } + projectedPoints.push_back(p); + } + + // Create a vector of tuples for the TransferFactor + EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); + std::vector> tuples; + + for (size_t n = 0; n < numPoints; ++n) { + const auto& p = projectedPoints[n]; + tuples.emplace_back(p[1], p[2], p[0]); + } + + // Create TransferFactors using the new constructor + TransferFactor factor{key01, key20, tuples}; + + // Create Values with edge keys + Values values; + values.insert(key01, triplet.Fab); + values.insert(key12, triplet.Fbc); + values.insert(key20, triplet.Fca); + + // Check error and Jacobians for multiple tuples + Vector error = factor.unwhitenedError(values); + // The error vector should be of size 2 * numPoints + EXPECT_LONGS_EQUAL(2 * numPoints, error.size()); + // Since the points are perfectly matched, the error should be zero + EXPECT(assert_equal(Vector::Zero(2 * numPoints), error, 1e-9)); + + // Check the Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//*************************************************************************