-
Notifications
You must be signed in to change notification settings - Fork 768
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #1884 from borglab/feature/F_matrix
Two fundamental matrix classes
- Loading branch information
Showing
4 changed files
with
568 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,150 @@ | ||
/* | ||
* @file FundamentalMatrix.cpp | ||
* @brief FundamentalMatrix classes | ||
* @author Frank Dellaert | ||
* @date Oct 23, 2024 | ||
*/ | ||
|
||
#include <gtsam/geometry/FundamentalMatrix.h> | ||
|
||
namespace gtsam { | ||
|
||
//************************************************************************* | ||
Point2 Transfer(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); | ||
|
||
// Cross the lines to find the intersection point | ||
Vector3 intersectionPoint = line_a.cross(line_b); | ||
|
||
// Normalize the intersection point | ||
intersectionPoint /= intersectionPoint(2); | ||
|
||
return intersectionPoint.head<2>(); // Return the 2D point | ||
} | ||
//************************************************************************* | ||
FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { | ||
// Perform SVD | ||
Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); | ||
|
||
// Extract U and V | ||
Matrix3 U = svd.matrixU(); | ||
Matrix3 V = svd.matrixV(); | ||
Vector3 singularValues = svd.singularValues(); | ||
|
||
// Scale the singular values | ||
double scale = singularValues(0); | ||
if (scale != 0) { | ||
singularValues /= scale; // Normalize the first singular value to 1.0 | ||
} | ||
|
||
// Check if the third singular value is close to zero (valid F condition) | ||
if (std::abs(singularValues(2)) > 1e-9) { | ||
throw std::invalid_argument( | ||
"The input matrix does not represent a valid fundamental matrix."); | ||
} | ||
|
||
// Ensure the second singular value is recorded as s | ||
s_ = singularValues(1); | ||
|
||
// Check if U is a reflection | ||
if (U.determinant() < 0) { | ||
U = -U; | ||
s_ = -s_; // Change sign of scalar if U is a reflection | ||
} | ||
|
||
// Check if V is a reflection | ||
if (V.determinant() < 0) { | ||
V = -V; | ||
s_ = -s_; // Change sign of scalar if U is a reflection | ||
} | ||
|
||
// Assign the rotations | ||
U_ = Rot3(U); | ||
V_ = Rot3(V); | ||
} | ||
|
||
Matrix3 FundamentalMatrix::matrix() const { | ||
return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); | ||
} | ||
|
||
void FundamentalMatrix::print(const std::string& s) const { | ||
std::cout << s << "U:\n" | ||
<< U_.matrix() << "\ns: " << s_ << "\nV:\n" | ||
<< V_.matrix() << std::endl; | ||
} | ||
|
||
bool FundamentalMatrix::equals(const FundamentalMatrix& other, | ||
double tol) const { | ||
return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && | ||
V_.equals(other.V_, tol); | ||
} | ||
|
||
Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { | ||
Vector result(7); | ||
result.head<3>() = U_.localCoordinates(F.U_); | ||
result(3) = F.s_ - s_; // Difference in scalar | ||
result.tail<3>() = V_.localCoordinates(F.V_); | ||
return result; | ||
} | ||
|
||
FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { | ||
Rot3 newU = U_.retract(delta.head<3>()); | ||
double newS = s_ + delta(3); // Update scalar | ||
Rot3 newV = V_.retract(delta.tail<3>()); | ||
return FundamentalMatrix(newU, newS, newV); | ||
} | ||
|
||
//************************************************************************* | ||
Matrix3 SimpleFundamentalMatrix::leftK() const { | ||
Matrix3 K; | ||
K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; | ||
return K; | ||
} | ||
|
||
Matrix3 SimpleFundamentalMatrix::rightK() 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(); | ||
} | ||
|
||
void SimpleFundamentalMatrix::print(const std::string& s) const { | ||
std::cout << s << " E:\n" | ||
<< E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ | ||
<< "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose() | ||
<< std::endl; | ||
} | ||
|
||
bool SimpleFundamentalMatrix::equals(const SimpleFundamentalMatrix& other, | ||
double tol) const { | ||
return E_.equals(other.E_, tol) && std::abs(fa_ - other.fa_) < tol && | ||
std::abs(fb_ - other.fb_) < tol && (ca_ - other.ca_).norm() < tol && | ||
(cb_ - other.cb_).norm() < tol; | ||
} | ||
|
||
Vector SimpleFundamentalMatrix::localCoordinates( | ||
const SimpleFundamentalMatrix& F) const { | ||
Vector result(7); | ||
result.head<5>() = E_.localCoordinates(F.E_); | ||
result(5) = F.fa_ - fa_; // Difference in fa | ||
result(6) = F.fb_ - fb_; // Difference in fb | ||
return result; | ||
} | ||
|
||
SimpleFundamentalMatrix SimpleFundamentalMatrix::retract( | ||
const Vector& delta) const { | ||
EssentialMatrix newE = E_.retract(delta.head<5>()); | ||
double newFa = fa_ + delta(5); // Update fa | ||
double newFb = fb_ + delta(6); // Update fb | ||
return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_); | ||
} | ||
|
||
//************************************************************************* | ||
|
||
} // namespace gtsam |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,183 @@ | ||
/* | ||
* @file FundamentalMatrix.h | ||
* @brief FundamentalMatrix classes | ||
* @author Frank Dellaert | ||
* @date Oct 23, 2024 | ||
*/ | ||
|
||
#pragma once | ||
|
||
#include <gtsam/geometry/EssentialMatrix.h> | ||
#include <gtsam/geometry/Rot3.h> | ||
#include <gtsam/geometry/Unit3.h> | ||
|
||
namespace gtsam { | ||
|
||
/** | ||
* @class FundamentalMatrix | ||
* @brief Represents a general fundamental matrix. | ||
* | ||
* This class represents a general fundamental matrix, which is a 3x3 matrix | ||
* that describes the relationship between two images. It is parameterized by a | ||
* left rotation U, a scalar s, and a right rotation V. | ||
*/ | ||
class GTSAM_EXPORT FundamentalMatrix { | ||
private: | ||
Rot3 U_; ///< Left rotation | ||
double s_; ///< Scalar parameter for S | ||
Rot3 V_; ///< Right rotation | ||
|
||
public: | ||
/// Default constructor | ||
FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} | ||
|
||
/** | ||
* @brief Construct from U, V, and scalar s | ||
* | ||
* Initializes the FundamentalMatrix with the given left rotation U, | ||
* scalar s, and right rotation V. | ||
* | ||
* @param U Left rotation matrix | ||
* @param s Scalar parameter for the fundamental matrix | ||
* @param V Right rotation matrix | ||
*/ | ||
FundamentalMatrix(const Rot3& U, double s, const Rot3& V) | ||
: U_(U), s_(s), V_(V) {} | ||
|
||
/** | ||
* @brief Construct from a 3x3 matrix using SVD | ||
* | ||
* Initializes the FundamentalMatrix by performing SVD on the given | ||
* matrix and ensuring U and V are not reflections. | ||
* | ||
* @param F A 3x3 matrix representing the fundamental matrix | ||
*/ | ||
FundamentalMatrix(const Matrix3& F); | ||
|
||
/// Return the fundamental matrix representation | ||
Matrix3 matrix() const; | ||
|
||
/// @name Testable | ||
/// @{ | ||
/// Print the FundamentalMatrix | ||
void print(const std::string& s = "") const; | ||
|
||
/// Check if the FundamentalMatrix is equal to another within a | ||
/// tolerance | ||
bool equals(const FundamentalMatrix& other, double tol = 1e-9) const; | ||
/// @} | ||
|
||
/// @name Manifold | ||
/// @{ | ||
enum { dimension = 7 }; // 3 for U, 1 for s, 3 for V | ||
inline static size_t Dim() { return dimension; } | ||
inline size_t dim() const { return dimension; } | ||
|
||
/// Return local coordinates with respect to another FundamentalMatrix | ||
Vector localCoordinates(const FundamentalMatrix& F) const; | ||
|
||
/// Retract the given vector to get a new FundamentalMatrix | ||
FundamentalMatrix retract(const Vector& delta) const; | ||
/// @} | ||
}; | ||
|
||
/** | ||
* @class SimpleFundamentalMatrix | ||
* @brief Class for representing a simple fundamental matrix. | ||
* | ||
* This class represents a simple fundamental matrix, which is a | ||
* parameterization of the essential matrix and focal lengths for left and right | ||
* cameras. Principal points are not part of the manifold but a convenience. | ||
*/ | ||
class GTSAM_EXPORT SimpleFundamentalMatrix { | ||
private: | ||
EssentialMatrix E_; ///< Essential matrix | ||
double fa_; ///< Focal length for left camera | ||
double fb_; ///< Focal length for right camera | ||
Point2 ca_; ///< Principal point for left camera | ||
Point2 cb_; ///< Principal point for right camera | ||
|
||
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 | ||
SimpleFundamentalMatrix(const EssentialMatrix& E, // | ||
double fa, double fb, | ||
const Point2& ca = Point2(0.0, 0.0), | ||
const Point2& cb = Point2(0.0, 0.0)) | ||
: 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 | ||
Matrix3 matrix() const; | ||
|
||
/// @name Testable | ||
/// @{ | ||
/// Print the SimpleFundamentalMatrix | ||
void print(const std::string& s = "") const; | ||
|
||
/// Check equality within a tolerance | ||
bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const; | ||
/// @} | ||
|
||
/// @name Manifold | ||
/// @{ | ||
enum { dimension = 7 }; // 5 for E, 1 for fa, 1 for fb | ||
inline static size_t Dim() { return dimension; } | ||
inline size_t dim() const { return dimension; } | ||
|
||
/// Return local coordinates with respect to another SimpleFundamentalMatrix | ||
Vector localCoordinates(const SimpleFundamentalMatrix& F) const; | ||
|
||
/// Retract the given vector to get a new SimpleFundamentalMatrix | ||
SimpleFundamentalMatrix retract(const Vector& delta) const; | ||
/// @} | ||
}; | ||
|
||
/** | ||
* @brief Transfer projections from cameras a and b to camera c | ||
* | ||
* 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); | ||
|
||
/// Represents a set of three fundamental matrices for transferring points | ||
/// between three cameras. | ||
template <typename F> | ||
struct TripleF { | ||
F Fab, Fbc, Fca; | ||
|
||
/// 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); | ||
} | ||
|
||
/// 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); | ||
} | ||
|
||
/// 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); | ||
} | ||
}; | ||
|
||
template <> | ||
struct traits<FundamentalMatrix> | ||
: public internal::Manifold<FundamentalMatrix> {}; | ||
|
||
template <> | ||
struct traits<SimpleFundamentalMatrix> | ||
: public internal::Manifold<SimpleFundamentalMatrix> {}; | ||
|
||
} // namespace gtsam |
Oops, something went wrong.