From 33a581076009936e8d137c083d7f2cf1447714fa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Oct 2024 19:17:23 -0700 Subject: [PATCH] Batched version --- examples/ViewGraphExample.cpp | 35 +++++++++------ gtsam/sfm/TransferFactor.h | 60 +++++++++++++++++--------- gtsam/sfm/tests/testTransferFactor.cpp | 58 +++++++++++++++++++++++++ 3 files changed, 118 insertions(+), 35 deletions(-) diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index d1c7113bcc..c23ac084c9 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -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 @@ -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; + 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(EdgeKey(a, c), EdgeKey(b, c), p[a][j], - p[b][j], p[c][j]); - graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), p[a][j], - p[c][j], p[b][j]); - graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), p[c][j], - p[b][j], p[a][j]); + + // Vectors to collect tuples for each factor + std::vector> 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(EdgeKey(a, c), EdgeKey(b, c), tuples1); + graph.emplace_shared(EdgeKey(a, b), EdgeKey(b, c), tuples2); + graph.emplace_shared(EdgeKey(a, c), EdgeKey(a, b), tuples3); } auto formatter = [](Key key) { @@ -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; @@ -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; diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 8acde7b88f..35ef8c1ae4 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -32,22 +32,14 @@ namespace gtsam { template class TransferFactor : public NoiseModelFactorN { EdgeKey key1_, key2_; ///< the two EdgeKeys - Point2 pa, pb, pc; ///< The points in the three views + std::vector> + 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). @@ -62,9 +54,25 @@ class TransferFactor : public NoiseModelFactorN { : NoiseModelFactorN(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>& triplets, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactorN(model, key1, key2), + key1_(key1), + key2_(key2), + triplets_(triplets) {} // Create Matrix3 objects based on EdgeKey configurations std::pair getMatrices(const F& F1, const F& F2) const { @@ -83,17 +91,27 @@ class TransferFactor : public NoiseModelFactorN { } } - /// 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 transfer = [&](const F& F1, const F& F2) { + std::function 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(transfer, F1, F2); - if (H2) *H2 = numericalDerivative22(transfer, F1, F2); - return transfer(F1, F2) - pc; + if (H1) *H1 = numericalDerivative21(transfer, F1, F2); + if (H2) *H2 = numericalDerivative22(transfer, F1, F2); + return transfer(F1, F2); } }; diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 7bcd71b1fe..6092ec4562 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -89,6 +89,64 @@ TEST(TransferFactor, Jacobians) { } } +//************************************************************************* +// Test for TransferFactor with multiple tuples +TEST(TransferFactor, MultipleTuples) { + // Generate cameras on a circle + std::array 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 points3D; + std::vector> 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 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> 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 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::Zero(2 * numPoints), error, 1e-9)); + + // Check the Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + //************************************************************************* int main() { TestResult tr;