cpp file
Frank Dellaert
parent
984232defb
commit
82224a611b
|
|
@ -0,0 +1,133 @@
|
|||
/*
|
||||
* @file FundamentalMatrix.cpp
|
||||
* @brief FundamentalMatrix classes
|
||||
* @author Frank Dellaert
|
||||
* @date Oct 23, 2024
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/FundamentalMatrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
GeneralFundamentalMatrix::GeneralFundamentalMatrix(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 GeneralFundamentalMatrix::matrix() const {
|
||||
return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix();
|
||||
}
|
||||
|
||||
void GeneralFundamentalMatrix::print(const std::string& s) const {
|
||||
std::cout << s << "U:\n"
|
||||
<< U_.matrix() << "\ns: " << s_ << "\nV:\n"
|
||||
<< V_.matrix() << std::endl;
|
||||
}
|
||||
|
||||
bool GeneralFundamentalMatrix::equals(const GeneralFundamentalMatrix& other,
|
||||
double tol) const {
|
||||
return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
|
||||
V_.equals(other.V_, tol);
|
||||
}
|
||||
|
||||
Vector GeneralFundamentalMatrix::localCoordinates(
|
||||
const GeneralFundamentalMatrix& 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;
|
||||
}
|
||||
|
||||
GeneralFundamentalMatrix GeneralFundamentalMatrix::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 GeneralFundamentalMatrix(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
|
||||
|
|
@ -121,70 +121,19 @@ class GeneralFundamentalMatrix : public FundamentalMatrix {
|
|||
*
|
||||
* @param F A 3x3 matrix representing the fundamental matrix
|
||||
*/
|
||||
GeneralFundamentalMatrix(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);
|
||||
}
|
||||
GeneralFundamentalMatrix(const Matrix3& F);
|
||||
|
||||
/// Return the fundamental matrix representation
|
||||
Matrix3 matrix() const override {
|
||||
return U_.matrix() * Vector3(1, s_, 0).asDiagonal() *
|
||||
V_.transpose().matrix();
|
||||
}
|
||||
Matrix3 matrix() const override;
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Print the GeneralFundamentalMatrix
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << s << "U:\n"
|
||||
<< U_.matrix() << "\ns: " << s_ << "\nV:\n"
|
||||
<< V_.matrix() << std::endl;
|
||||
}
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// Check if the GeneralFundamentalMatrix is equal to another within a
|
||||
/// tolerance
|
||||
bool equals(const GeneralFundamentalMatrix& other, double tol = 1e-9) const {
|
||||
return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
|
||||
V_.equals(other.V_, tol);
|
||||
}
|
||||
|
||||
bool equals(const GeneralFundamentalMatrix& other, double tol = 1e-9) const;
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
|
|
@ -194,22 +143,10 @@ class GeneralFundamentalMatrix : public FundamentalMatrix {
|
|||
inline size_t dim() const { return dimension; }
|
||||
|
||||
/// Return local coordinates with respect to another GeneralFundamentalMatrix
|
||||
Vector localCoordinates(const GeneralFundamentalMatrix& 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;
|
||||
}
|
||||
Vector localCoordinates(const GeneralFundamentalMatrix& F) const;
|
||||
|
||||
/// Retract the given vector to get a new GeneralFundamentalMatrix
|
||||
GeneralFundamentalMatrix 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 GeneralFundamentalMatrix(newU, newS, newV);
|
||||
}
|
||||
|
||||
GeneralFundamentalMatrix retract(const Vector& delta) const;
|
||||
/// @}
|
||||
};
|
||||
|
||||
|
|
@ -221,7 +158,7 @@ class GeneralFundamentalMatrix : public FundamentalMatrix {
|
|||
* 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 SimpleFundamentalMatrix : FundamentalMatrix {
|
||||
class SimpleFundamentalMatrix : public FundamentalMatrix {
|
||||
private:
|
||||
EssentialMatrix E_; ///< Essential matrix
|
||||
double fa_; ///< Focal length for left camera
|
||||
|
|
@ -242,40 +179,21 @@ class SimpleFundamentalMatrix : FundamentalMatrix {
|
|||
: E_(E), fa_(fa), fb_(fb), ca_(ca), cb_(cb) {}
|
||||
|
||||
/// Return the left calibration matrix
|
||||
Matrix3 leftK() const {
|
||||
Matrix3 K;
|
||||
K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
|
||||
return K;
|
||||
}
|
||||
Matrix3 leftK() const;
|
||||
|
||||
/// Return the right calibration matrix
|
||||
Matrix3 rightK() const {
|
||||
Matrix3 K;
|
||||
K << fb_, 0, cb_.x(), 0, fb_, cb_.y(), 0, 0, 1;
|
||||
return K;
|
||||
}
|
||||
Matrix3 rightK() const;
|
||||
|
||||
/// Return the fundamental matrix representation
|
||||
Matrix3 matrix() const override {
|
||||
return leftK().transpose().inverse() * E_.matrix() * rightK().inverse();
|
||||
}
|
||||
Matrix3 matrix() const override;
|
||||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Print the SimpleFundamentalMatrix
|
||||
void 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;
|
||||
}
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// Check equality within a tolerance
|
||||
bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) 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;
|
||||
}
|
||||
bool equals(const SimpleFundamentalMatrix& other, double tol = 1e-9) const;
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
|
|
@ -284,23 +202,11 @@ class SimpleFundamentalMatrix : FundamentalMatrix {
|
|||
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 {
|
||||
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;
|
||||
}
|
||||
/// 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 {
|
||||
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_);
|
||||
}
|
||||
SimpleFundamentalMatrix retract(const Vector& delta) const;
|
||||
/// @}
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue