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

Move transfer to cpp

release/4.3a0
Frank Dellaert 2024-10-23 23:59:32 -07:00
parent 82224a611b
commit 0431299df6
2 changed files with 21 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/geometry/FundamentalMatrix.cpp
View File

@ -9,6 +9,23 @@
namespace gtsam { namespace gtsam {
//*************************************************************************
Point2 FundamentalMatrix::transfer(const Matrix3& F01, const Point2& p1,
const Matrix3& F02, const Point2& p2) {
// Create lines in camera 0 from projections of the other two cameras
Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1);
Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1);
// Cross the lines to find the intersection point
Vector3 intersectionPoint = line1.cross(line2);
// Normalize the intersection point
intersectionPoint /= intersectionPoint(2);
return intersectionPoint.head<2>(); // Return the 2D point
}
//*************************************************************************
GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) { GeneralFundamentalMatrix::GeneralFundamentalMatrix(const Matrix3& F) {
// Perform SVD // Perform SVD
Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::JacobiSVD<Matrix3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
@ -83,6 +100,7 @@ GeneralFundamentalMatrix GeneralFundamentalMatrix::retract(
return GeneralFundamentalMatrix(newU, newS, newV); return GeneralFundamentalMatrix(newU, newS, newV);
} }
//*************************************************************************
Matrix3 SimpleFundamentalMatrix::leftK() const { Matrix3 SimpleFundamentalMatrix::leftK() const {
Matrix3 K; Matrix3 K;
K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1; K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
@ -130,4 +148,6 @@ SimpleFundamentalMatrix SimpleFundamentalMatrix::retract(
return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_); return SimpleFundamentalMatrix(newE, newFa, newFb, ca_, cb_);
} }
//*************************************************************************
} // namespace gtsam } // namespace gtsam

14
gtsam/geometry/FundamentalMatrix.h
View File

@ -42,19 +42,7 @@ class FundamentalMatrix {
* returns the 2D point in camera 0 where the epipolar lines intersect. * returns the 2D point in camera 0 where the epipolar lines intersect.
*/ */
static Point2 transfer(const Matrix3& F01, const Point2& p1, static Point2 transfer(const Matrix3& F01, const Point2& p1,
const Matrix3& F02, const Point2& p2) { const Matrix3& F02, const Point2& p2);
// Create lines in camera 0 from projections of the other two cameras
Vector3 line1 = F01 * Vector3(p1.x(), p1.y(), 1);
Vector3 line2 = F02 * Vector3(p2.x(), p2.y(), 1);
// Cross the lines to find the intersection point
Vector3 intersectionPoint = line1.cross(line2);
// Normalize the intersection point
intersectionPoint /= intersectionPoint(2);
return intersectionPoint.head<2>(); // Return the 2D point
}
}; };
/// Represents a set of three fundamental matrices for transferring points /// Represents a set of three fundamental matrices for transferring points