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); initialize(U, singularValues(1), V);
} }
void FundamentalMatrix::initialize(const Matrix3& U, double s, void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) {
const Matrix3& V) { // Check if U is a reflection and flip third column if so
s_ = s; if (U.determinant() < 0) U.col(2) *= -1;
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);
}
// Same check for V // Same check for V
double detV = V.determinant(); if (V.determinant() < 0) V.col(2) *= -1;
if (detV < 0) {
V_ = Rot3(-V); U_ = Rot3(U);
sign_ = -sign_; s_ = s;
} else { V_ = Rot3(V);
V_ = Rot3(V);
}
} }
Matrix3 FundamentalMatrix::matrix() const { 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(); V_.transpose().matrix();
} }
@ -92,8 +80,8 @@ void FundamentalMatrix::print(const std::string& s) const {
bool FundamentalMatrix::equals(const FundamentalMatrix& other, bool FundamentalMatrix::equals(const FundamentalMatrix& other,
double tol) const { double tol) const {
return U_.equals(other.U_, tol) && sign_ == other.sign_ && return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol &&
std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); V_.equals(other.V_, tol);
} }
Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { 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 { FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const {
Rot3 newU = U_.retract(delta.head<3>()); const Rot3 newU = U_.retract(delta.head<3>());
double newS = s_ + delta(3); // Update scalar const double newS = s_ + delta(3);
Rot3 newV = V_.retract(delta.tail<3>()); const Rot3 newV = V_.retract(delta.tail<3>());
return FundamentalMatrix(newU, sign_, newS, newV); 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 * The FundamentalMatrix class encapsulates the fundamental matrix, which
* relates corresponding points in stereo images. It is parameterized by two * 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 * Using these values, the fundamental matrix is represented as
* *
* F = sign * U * diag(1, s, 0) * V^T * F = U * diag(1, s, 0) * V^T
*
* We need the `sign` because we use SO(3) for U and V, not O(3).
*/ */
class GTSAM_EXPORT FundamentalMatrix { class GTSAM_EXPORT FundamentalMatrix {
private: private:
Rot3 U_; ///< Left rotation Rot3 U_; ///< Left rotation
double sign_; ///< Whether to flip the sign or not double s_; ///< Scalar parameter for S
double s_; ///< Scalar parameter for S Rot3 V_; ///< Right rotation
Rot3 V_; ///< Right rotation
public: public:
/// Default constructor /// 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 * @brief Construct from U, V, and scalar s
@ -113,11 +110,11 @@ class GTSAM_EXPORT FundamentalMatrix {
/// @} /// @}
private: private:
/// Private constructor for internal use /// Private constructor for internal use
FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V) FundamentalMatrix(const Rot3& U, double s, const Rot3& V)
: U_(U), sign_(sign), s_(scaled_s), V_(V) {} : U_(U), s_(s), V_(V) {}
/// Initialize SO(3) matrices from general O(3) matrices /// 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);
}; };
/** /**