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

Merge branch 'feature/wrapF' into feature/essential_transfer

release/4.3a0
Frank Dellaert 2024-10-31 10:34:31 -07:00
parent d0808fcfc5 56943f7be8
commit dfb69f367d
2 changed files with 24 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -57,32 +57,20 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) {
initialize(U, singularValues(1), V);
}
void FundamentalMatrix::initialize(const Matrix3& U, double s,
const Matrix3& V) {
s_ = s;
sign_ = 1.0;
// Check if U is a reflection and flip U and sign_ if so
double detU = U.determinant(); // detU will be -1 or 1
if (detU < 0) {
U_ = Rot3(-U);
sign_ = -sign_;
} else {
U_ = Rot3(U);
}
void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) {
// Check if U is a reflection and flip third column if so
if (U.determinant() < 0) U.col(2) *= -1;
// Same check for V
double detV = V.determinant();
if (detV < 0) {
V_ = Rot3(-V);
sign_ = -sign_;
} else {
V_ = Rot3(V);
}
if (V.determinant() < 0) V.col(2) *= -1;
U_ = Rot3(U);
s_ = s;
V_ = Rot3(V);
}
Matrix3 FundamentalMatrix::matrix() const {
return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() *
V_.transpose().matrix();
}
@ -92,8 +80,8 @@ void FundamentalMatrix::print(const std::string& s) const {
bool FundamentalMatrix::equals(const FundamentalMatrix& other,
double tol) const {
return U_.equals(other.U_, tol) && sign_ == other.sign_ &&
std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol);
return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
V_.equals(other.V_, tol);
}
Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const {
@ -105,10 +93,10 @@ Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const {
}
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, sign_, newS, newV);
const Rot3 newU = U_.retract(delta.head<3>());
const double newS = s_ + delta(3);
const Rot3 newV = V_.retract(delta.tail<3>());
return FundamentalMatrix(newU, newS, newV);
}
//*************************************************************************

21
gtsam/geometry/FundamentalMatrix.h
View File

@ -20,23 +20,20 @@ namespace gtsam {
*
* The FundamentalMatrix class encapsulates the fundamental matrix, which
* relates corresponding points in stereo images. It is parameterized by two
* rotation matrices (U and V), a scalar parameter (s), and a sign.
* rotation matrices (U and V) and a scalar parameter (s).
* Using these values, the fundamental matrix is represented as
*
* F = sign * U * diag(1, s, 0) * V^T
*
* We need the `sign` because we use SO(3) for U and V, not O(3).
* F = U * diag(1, s, 0) * V^T
*/
class GTSAM_EXPORT FundamentalMatrix {
private:
Rot3 U_; ///< Left rotation
double sign_; ///< Whether to flip the sign or not
double s_; ///< Scalar parameter for S
Rot3 V_; ///< Right rotation
Rot3 U_; ///< Left rotation
double s_; ///< Scalar parameter for S
Rot3 V_; ///< Right rotation
public:
/// Default constructor
FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {}
FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {}
/**
* @brief Construct from U, V, and scalar s
@ -113,11 +110,11 @@ class GTSAM_EXPORT FundamentalMatrix {
/// @}
private:
/// Private constructor for internal use
FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V)
: U_(U), sign_(sign), s_(scaled_s), V_(V) {}
FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
: U_(U), s_(s), V_(V) {}
/// Initialize SO(3) matrices from general O(3) matrices
void initialize(const Matrix3& U, double s, const Matrix3& V);
void initialize(Matrix3 U, double s, Matrix3 V);
};
/**