Skip to content

Commit

Permalink
Batched version
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert committed Oct 25, 2024
1 parent e87ce14 commit 33a5810
Show file tree
Hide file tree
Showing 3 changed files with 118 additions and 35 deletions.
35 changes: 21 additions & 14 deletions examples/ViewGraphExample.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
* @file ViewGraphExample.cpp
* @brief View-graph calibration on a simulated dataset, a la Sweeney 2015
* @author Frank Dellaert
* @author October 2024
* @date October 2024
*/

#include <gtsam/geometry/Cal3_S2.h>
Expand Down Expand Up @@ -68,21 +68,28 @@ int main(int argc, char* argv[]) {
// In this version, we only include triplets between 3 successive cameras.
NonlinearFactorGraph graph;
using Factor = TransferFactor<FundamentalMatrix>;

for (size_t a = 0; a < 4; ++a) {
size_t b = (a + 1) % 4; // Next camera
size_t c = (a + 2) % 4; // Camera after next
for (size_t j = 0; j < 4; ++j) {
// Add transfer factors between views a, b, and c. Note that the EdgeKeys
// are crucial in performing the transfer in the right direction. We use
// exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
// matrices we will optimize for.
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), p[a][j],
p[b][j], p[c][j]);
graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), p[a][j],
p[c][j], p[b][j]);
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), p[c][j],
p[b][j], p[a][j]);

// Vectors to collect tuples for each factor
std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;

// Collect data for the three factors
for (size_t j = 0; j < 8; ++j) {
tuples1.emplace_back(p[a][j], p[b][j], p[c][j]);
tuples2.emplace_back(p[a][j], p[c][j], p[b][j]);
tuples3.emplace_back(p[c][j], p[b][j], p[a][j]);
}

// Add transfer factors between views a, b, and c. Note that the EdgeKeys
// are crucial in performing the transfer in the right direction. We use
// exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
// matrices we will optimize for.
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
}

auto formatter = [](Key key) {
Expand All @@ -96,7 +103,7 @@ int main(int argc, char* argv[]) {
// We can't really go far before convergence becomes problematic :-(
Vector7 delta;
delta << 1, 2, 3, 4, 5, 6, 7;
delta *= 5e-5;
delta *= 1e-5;

// Create the data structure to hold the initial estimate to the solution
Values initialEstimate;
Expand All @@ -107,7 +114,7 @@ int main(int argc, char* argv[]) {
initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
}
initialEstimate.print("Initial Estimates:\n", formatter);
// graph.printErrors(initialEstimate, "errors: ", formatter);
graph.printErrors(initialEstimate, "errors: ", formatter);

/* Optimize the graph and print results */
LevenbergMarquardtParams params;
Expand Down
60 changes: 39 additions & 21 deletions gtsam/sfm/TransferFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,22 +32,14 @@ namespace gtsam {
template <typename F>
class TransferFactor : public NoiseModelFactorN<F, F> {
EdgeKey key1_, key2_; ///< the two EdgeKeys
Point2 pa, pb, pc; ///< The points in the three views
std::vector<std::tuple<Point2, Point2, Point2>>
triplets_; ///< Point triplets

public:
/**
* @brief Constructor for the TransferFactor class.
* @brief Constructor for a single triplet of points
*
* Uses EdgeKeys to determine how to use the two fundamental matrix unknowns
* F1 and F2, to transfer points pa and pb to the third view, and minimize the
* difference with pc.
*
* The edge keys must represent valid edges for the transfer operation,
* specifically one of the following configurations:
* - (a, c) and (b, c)
* - (a, c) and (c, b)
* - (c, a) and (b, c)
* - (c, a) and (c, b)
* @note: batching all points for the same transfer will be much faster.
*
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
Expand All @@ -62,9 +54,25 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
: NoiseModelFactorN<F, F>(model, key1, key2),
key1_(key1),
key2_(key2),
pa(pa),
pb(pb),
pc(pc) {}
triplets_({std::make_tuple(pa, pb, pc)}) {}

/**
* @brief Constructor that accepts a vector of point triplets.
*
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param triplets A vector of triplets containing (pa, pb, pc).
* @param model An optional SharedNoiseModel that defines the noise model
* for this factor. Defaults to nullptr.
*/
TransferFactor(
EdgeKey key1, EdgeKey key2,
const std::vector<std::tuple<Point2, Point2, Point2>>& triplets,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<F, F>(model, key1, key2),
key1_(key1),
key2_(key2),
triplets_(triplets) {}

// Create Matrix3 objects based on EdgeKey configurations
std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
Expand All @@ -83,17 +91,27 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
}
}

/// vector of errors returns 2D vector
/// vector of errors returns 2*N vector
Vector evaluateError(const F& F1, const F& F2,
OptionalMatrixType H1 = nullptr,
OptionalMatrixType H2 = nullptr) const override {
std::function<Point2(F, F)> transfer = [&](const F& F1, const F& F2) {
std::function<Vector(const F&, const F&)> transfer = [&](const F& F1,
const F& F2) {
Vector errors(2 * triplets_.size());
size_t idx = 0;
auto [Fca, Fcb] = getMatrices(F1, F2);
return Transfer(Fca, pa, Fcb, pb);
for (const auto& tuple : triplets_) {
const auto& [pa, pb, pc] = tuple;
Point2 transferredPoint = Transfer(Fca, pa, Fcb, pb);
Vector2 error = transferredPoint - pc;
errors.segment<2>(idx) = error;
idx += 2;
}
return errors;
};
if (H1) *H1 = numericalDerivative21<Point2, F, F>(transfer, F1, F2);
if (H2) *H2 = numericalDerivative22<Point2, F, F>(transfer, F1, F2);
return transfer(F1, F2) - pc;
if (H1) *H1 = numericalDerivative21<Vector, F, F>(transfer, F1, F2);
if (H2) *H2 = numericalDerivative22<Vector, F, F>(transfer, F1, F2);
return transfer(F1, F2);
}
};

Expand Down
58 changes: 58 additions & 0 deletions gtsam/sfm/tests/testTransferFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,64 @@ 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);

// 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);
points3D.push_back(P);

std::array<Point2, 3> p;
for (size_t i = 0; i < 3; ++i) {
// Project the point into each camera
PinholeCameraCal3_S2 camera(cameraPoses[i], K);
p[i] = camera.project(P);
}
projectedPoints.push_back(p);
}

// 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;

for (size_t n = 0; n < numPoints; ++n) {
const auto& p = projectedPoints[n];
tuples0.emplace_back(p[1], p[2], p[0]);
}

// Create TransferFactors using the new constructor
TransferFactor<SimpleFundamentalMatrix> factor{key01, key20, tuples0};

// Create Values with edge keys
Values values;
values.insert(key01, triplet.Fab);
values.insert(key12, triplet.Fbc);
values.insert(key20, triplet.Fca);

// Check error and Jacobians for multiple tuples
Vector error = factor.unwhitenedError(values);
// The error vector should be of size 2 * numPoints
EXPECT_LONGS_EQUAL(2 * numPoints, error.size());
// Since the points are perfectly matched, the error should be zero
EXPECT(assert_equal<Vector>(Vector::Zero(2 * numPoints), error, 1e-9));

// Check the Jacobians
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
}

//*************************************************************************
int main() {
TestResult tr;
Expand Down

0 comments on commit 33a5810

Please sign in to comment.