Address review comments

release/4.3a0
Frank Dellaert 2024-10-25 20:48:02 -07:00
parent 00cb637db3
commit cbbe2d2e88
4 changed files with 52 additions and 39 deletions

View File

@ -97,20 +97,20 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const {
}
//*************************************************************************
Matrix3 SimpleFundamentalMatrix::leftK() const {
Matrix3 SimpleFundamentalMatrix::Ka() const {
Matrix3 K;
K << fa_, 0, ca_.x(), 0, fa_, ca_.y(), 0, 0, 1;
return K;
}
Matrix3 SimpleFundamentalMatrix::rightK() const {
Matrix3 SimpleFundamentalMatrix::Kb() 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();
return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
}
void SimpleFundamentalMatrix::print(const std::string& s) const {

View File

@ -68,8 +68,8 @@ class GTSAM_EXPORT FundamentalMatrix {
template <typename CAL>
FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb)
: FundamentalMatrix(Ka.K().transpose().inverse() *
EssentialMatrix::FromPose3(aPb).matrix() *
Kb.K().inverse()) {}
EssentialMatrix::FromPose3(aPb).matrix() *
Kb.K().inverse()) {}
/// Return the fundamental matrix representation
Matrix3 matrix() const;
@ -114,25 +114,32 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
Point2 ca_; ///< Principal point for left camera
Point2 cb_; ///< Principal point for right camera
/// Return the left calibration matrix
Matrix3 Ka() const;
/// Return the right calibration matrix
Matrix3 Kb() const;
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
/**
* @brief Construct from essential matrix and focal lengths
* @param E Essential matrix
* @param fa Focal length for left camera
* @param fb Focal length for right camera
* @param ca Principal point for left camera
* @param cb Principal point for right camera
*/
SimpleFundamentalMatrix(const EssentialMatrix& E, //
double fa, double fb,
const Point2& ca = Point2(0.0, 0.0),
const Point2& cb = Point2(0.0, 0.0))
double fa, double fb, const Point2& ca,
const Point2& cb)
: 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
/// F = Ka^(-T) * E * Kb^(-1)
Matrix3 matrix() const;
/// @name Testable

View File

@ -24,8 +24,8 @@ namespace gtsam {
/**
* Binary factor in the context of Structure from Motion (SfM).
* It is used to transfer points between different views based on the
* fundamental matrices between these views. The factor computes the error
* It is used to transfer transfer corresponding points from two views to a
* third based on two fundamental matrices. The factor computes the error
* between the transferred points `pa` and `pb`, and the actual point `pc` in
* the target view. Jacobians are done using numerical differentiation.
*/

View File

@ -12,9 +12,6 @@
using namespace gtsam;
double focalLength = 1000;
Point2 principalPoint(640 / 2, 480 / 2);
//*************************************************************************
/// Generate three cameras on a circle, looking in
std::array<Pose3, 3> generateCameraPoses() {
@ -44,17 +41,36 @@ TripleF<SimpleFundamentalMatrix> generateTripleF(
return {F[0], F[1], F[2]}; // Return a TripleF instance
}
//*************************************************************************
namespace fixture {
// Generate cameras on a circle
std::array<Pose3, 3> cameraPoses = generateCameraPoses();
auto triplet = generateTripleF(cameraPoses);
double focalLength = 1000;
Point2 principalPoint(640 / 2, 480 / 2);
const Cal3_S2 K(focalLength, focalLength, 0.0, //
principalPoint.x(), principalPoint.y());
} // namespace fixture
//*************************************************************************
// Test for getMatrices
TEST(TransferFactor, GetMatrices) {
using namespace fixture;
TransferFactor<SimpleFundamentalMatrix> factor{{2, 0}, {1, 2}, {}};
// Check that getMatrices is correct
auto [Fki, Fkj] = factor.getMatrices(triplet.Fca, triplet.Fbc);
EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));
}
//*************************************************************************
// Test for TransferFactor
TEST(TransferFactor, Jacobians) {
// Generate cameras on a circle
std::array<Pose3, 3> cameraPoses = generateCameraPoses();
auto triplet = generateTripleF(cameraPoses);
using namespace fixture;
// Now project a point into the three cameras
const Point3 P(0.1, 0.2, 0.3);
const Cal3_S2 K(focalLength, focalLength, 0.0, //
principalPoint.x(), principalPoint.y());
std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) {
@ -70,11 +86,6 @@ TEST(TransferFactor, Jacobians) {
factor1{key12, key01, p[2], p[0], p[1]},
factor2{key20, key12, p[0], p[1], p[2]};
// Check that getMatrices is correct
auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc);
EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));
// Create Values with edge keys
Values values;
values.insert(key01, triplet.Fab);
@ -92,18 +103,13 @@ TEST(TransferFactor, Jacobians) {
//*************************************************************************
// Test for TransferFactor with multiple tuples
TEST(TransferFactor, MultipleTuples) {
// Generate cameras on a circle
std::array<Pose3, 3> cameraPoses = generateCameraPoses();
auto triplet = generateTripleF(cameraPoses);
using namespace fixture;
// Now project multiple points into the three cameras
const size_t numPoints = 5; // Number of points to test
std::vector<Point3> points3D;
std::vector<std::array<Point2, 3>> projectedPoints;
const Cal3_S2 K(focalLength, focalLength, 0.0, //
principalPoint.x(), principalPoint.y());
// Generate random 3D points and project them
for (size_t n = 0; n < numPoints; ++n) {
Point3 P(0.1 * n, 0.2 * n, 0.3 + 0.1 * n);
@ -120,15 +126,15 @@ TEST(TransferFactor, MultipleTuples) {
// Create a vector of tuples for the TransferFactor
EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
std::vector<std::tuple<Point2, Point2, Point2>> tuples0, tuples1, tuples2;
std::vector<std::tuple<Point2, Point2, Point2>> tuples;
for (size_t n = 0; n < numPoints; ++n) {
const auto& p = projectedPoints[n];
tuples0.emplace_back(p[1], p[2], p[0]);
tuples.emplace_back(p[1], p[2], p[0]);
}
// Create TransferFactors using the new constructor
TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples0};
TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples};
// Create Values with edge keys
Values values;