From 762d86775be93190ef835c8de3def1dc1b31da4a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Nov 2025 22:17:23 -0500 Subject: [PATCH 01/10] feat: introduce BatchFactor for efficient linearization of multiple identical factors into a single JacobianFactor --- gtsam/nonlinear/BatchFactor.h | 306 ++++++++++++++++++++++ gtsam/nonlinear/tests/testBatchFactor.cpp | 84 ++++++ 2 files changed, 390 insertions(+) create mode 100644 gtsam/nonlinear/BatchFactor.h create mode 100644 gtsam/nonlinear/tests/testBatchFactor.cpp diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h new file mode 100644 index 0000000000..af8e12a3fd --- /dev/null +++ b/gtsam/nonlinear/BatchFactor.h @@ -0,0 +1,306 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BatchFactor.h + * @brief A batch of factors that linearizes to a single JacobianFactor + * @author Frank Dellaert + * @date Nov 2023 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +/** + * BatchFactor is a NonlinearFactor that wraps a collection of identical factors. + * It linearizes them all at once into a single JacobianFactor. + * + * This is useful for optimizing Structure-from-Motion (SfM) and SLAM graphs where + * we have many factors of the same type (e.g., projection factors) that can be + * grouped together to reduce overhead. + * + * @tparam FactorType The type of the individual factors (must derive from NoiseModelFactor) + * @tparam ErrorDim The dimension of the error vector for a single factor + */ +template +class BatchFactor : public NonlinearFactor { + public: + // Static assertion to ensure FactorType derives from NoiseModelFactor + static_assert(std::is_base_of::value, + "FactorType must derive from NoiseModelFactor"); + + using Base = NonlinearFactor; + using This = BatchFactor; + using shared_ptr = std::shared_ptr; + + private: + std::vector factors_; ///< Contiguous storage for factors + + public: + /// @name Constructors + /// @{ + + /** Default constructor */ + BatchFactor() {} + + /** Constructor from a vector of factors (moves the vector) */ + explicit BatchFactor(std::vector&& factors) + : factors_(std::move(factors)) { + updateKeys(); + } + + /** Constructor from a vector of factors (copies the vector) */ + explicit BatchFactor(const std::vector& factors) + : factors_(factors) { + updateKeys(); + } + + /** + * Simplified batch constructor for binary factors (N=2). + * Broadcasts keys if one of the key vectors has size 1. + * + * @param keys1 Keys for the first variable + * @param keys2 Keys for the second variable + * @param measurements Vector of measurements + * @param model Shared noise model + */ + template + BatchFactor(const std::vector& keys1, const std::vector& keys2, + const std::vector& measurements, + const SharedNoiseModel& model) { + static_assert(FactorType::N == 2, + "Helper constructor only available for binary factors (N=2)"); + + size_t n = measurements.size(); + bool broadcast1 = (keys1.size() == 1); + bool broadcast2 = (keys2.size() == 1); + + if (!broadcast1 && keys1.size() != n) { + throw std::invalid_argument("BatchFactor: keys1 size mismatch"); + } + if (!broadcast2 && keys2.size() != n) { + throw std::invalid_argument("BatchFactor: keys2 size mismatch"); + } + + factors_.reserve(n); + for (size_t i = 0; i < n; ++i) { + Key k1 = broadcast1 ? keys1[0] : keys1[i]; + Key k2 = broadcast2 ? keys2[0] : keys2[i]; + factors_.emplace_back(k1, k2, measurements[i], model); + } + updateKeys(); + } + + /// @} + /// @name Testable + /// @{ + + /** print */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "BatchFactor with " << factors_.size() << " factors:" << std::endl; + for (const auto& f : factors_) { + f.print("", keyFormatter); + } + } + + /** equals */ + bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { + const This* p = dynamic_cast(&f); + if (!p || factors_.size() != p->factors_.size()) return false; + for (size_t i = 0; i < factors_.size(); ++i) { + if (!factors_[i].equals(p->factors_[i], tol)) return false; + } + return true; + } + + /// @} + /// @name Standard Interface + /// @{ + + /** + * Calculate the error of the factor. + * This is the sum of the errors of all internal factors. + */ + double error(const Values& c) const override { + double total_error = 0.0; + for (const auto& f : factors_) { + total_error += f.error(c); + } + return total_error; + } + + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const override { + return factors_.size() * ErrorDim; + } + + /** + * Linearize to a single JacobianFactor. + * + * Optimization: + * - Pre-calculates the total size required for the JacobianFactor. + * - Collects all unique Keys involved across all sub-factors. + * - Iterates linearly over factors_ (cache-friendly) to compute Jacobians. + * - Fills the pre-allocated JacobianFactor directly. + */ + std::shared_ptr linearize(const Values& c) const override { + if (factors_.empty()) return std::make_shared(); + + // 1. Collect all unique keys and their dimensions + std::map key_dims; + for (const auto& factor : factors_) { + collectKeyDims<1>(factor, key_dims); + } + + // 2. Prepare keys and dimensions for JacobianFactor construction + std::vector keys; + std::vector dims; + keys.reserve(key_dims.size()); + dims.reserve(key_dims.size()); + for (const auto& [key, dim] : key_dims) { + keys.push_back(key); + dims.push_back(dim); + } + + // 3. Allocate JacobianFactor + // We create a VerticalBlockMatrix with the correct dimensions. + // The total number of rows is the sum of the error dimensions of all factors. + size_t total_rows = factors_.size() * ErrorDim; + VerticalBlockMatrix Ab(dims, total_rows, true); + Ab.matrix().setZero(); // Important: Initialize to zero as we will fill blocks + + // 4. Fill the JacobianFactor + // We reuse a vector of matrices for the Jacobians to avoid repeated allocations. + std::vector H(FactorType::N); + + // Optimization: Allocate Eigen::Matrix on the stack + // for the error vector computation to avoid heap fragmentation. + // Note: unwhitenedError returns a dynamic Vector, but we can use a fixed-size + // vector for intermediate storage if needed, or just rely on the fact that + // we are writing directly into the large matrix block. + // Here we use the return value of unwhitenedError directly to whiten. + + for (size_t i = 0; i < factors_.size(); ++i) { + const auto& factor = factors_[i]; + size_t row_start = i * ErrorDim; + + // Compute unwhitened error and Jacobians + // We use the factor's unwhitenedError method which fills H. + Vector raw_error = factor.unwhitenedError(c, H); + + // Apply noise model (whitening) + // This modifies H and raw_error in place. + if (factor.noiseModel()) { + factor.noiseModel()->WhitenSystem(H, raw_error); + } + + // Place Jacobians into the large matrix + for (size_t j = 0; j < FactorType::N; ++j) { + Key key = factor.keys()[j]; + // Find the block column index for this key. + // Since 'keys' vector is sorted (from map), we could use binary search or map lookup. + // But VerticalBlockMatrix access by Key is not O(1) unless we know the index. + // Ab(key) does a linear search or similar. + // To optimize, we could pre-calculate the block index for each key in the map. + // But for now, we use Ab(key) which is convenient. + + // Ab(key) returns a block corresponding to the key. + // We write into the segment corresponding to this factor. + Ab(key).block(row_start, 0, ErrorDim, H[j].cols()) = H[j]; + } + + // Place the negative error into the RHS (last column) + // JacobianFactor stores Ax - b, so b = -error. + // Ab(size) gives the last block which is the RHS vector b. + // We use block() to access the segment as a matrix block. + Ab(keys.size()).block(row_start, 0, ErrorDim, 1) = -raw_error; + } + + // 5. Create and return the JacobianFactor + // We pass a Unit noise model because we have already whitened the system. + return std::make_shared(keys, std::move(Ab), + noiseModel::Unit::Create(total_rows)); + } + + private: + /** Helper to collect keys from factors */ + void updateKeys() { + std::set unique_keys; + for (const auto& f : factors_) { + for (size_t i = 0; i < FactorType::N; ++i) { + // Access keys via the base NoiseModelFactor keys_ array if possible, + // or use the key() method. NoiseModelFactorN exposes key(). + // We can also access the public keys() method from NonlinearFactor. + unique_keys.insert(f.keys()[i]); + } + } + this->keys_.assign(unique_keys.begin(), unique_keys.end()); + } + + /** Helper to collect key dimensions recursively */ + template + void collectKeyDims(const FactorType& f, std::map& key_dims) const { + if constexpr (I <= FactorType::N) { + // Get key and dimension for the I-th variable + Key k = f.template key(); + // Use traits to get dimension of the ValueType + using V = typename FactorType::template ValueType; + key_dims[k] = traits::dimension; + + // Recurse + collectKeyDims(f, key_dims); + } + } +}; + +} // namespace gtsam + +/* + * Usage Example: + * + * // Assume we have GenericProjectionFactor + * using ProjectionFactor = GenericProjectionFactor; + * + * // Create a batch factor + * std::vector poses = {Symbol('x', 1)}; + * std::vector points; + * std::vector measurements; + * for (int i = 0; i < 100; ++i) { + * points.push_back(Symbol('l', i)); + * measurements.push_back(Point2(10, 10)); // Dummy measurement + * } + * + * auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + * + * // Construct using the helper (1 camera, 100 points) + * auto batch = std::make_shared>( + * poses, points, measurements, noise); + * + * // Add to graph + * NonlinearFactorGraph graph; + * graph.add(batch); + * + * // Optimize as usual + * LevenbergMarquardtOptimizer optimizer(graph, initial_values); + * Values result = optimizer.optimize(); + */ diff --git a/gtsam/nonlinear/tests/testBatchFactor.cpp b/gtsam/nonlinear/tests/testBatchFactor.cpp new file mode 100644 index 0000000000..2a227a0ca5 --- /dev/null +++ b/gtsam/nonlinear/tests/testBatchFactor.cpp @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testBatchFactor.cpp + * @brief Unit tests for BatchFactor class + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace gtsam; +using namespace std; + +// Define a specific projection factor type for testing +using MyProjectionFactor = GenericProjectionFactor; + +/* ************************************************************************* */ +TEST(BatchFactor, ConstructorAndLinearize) { + // 1. Setup data + std::vector poses = {Symbol('x', 0)}; + std::vector points; + std::vector measurements; + + // Create 10 points + for (int i = 0; i < 10; ++i) { + points.push_back(Symbol('l', i)); + measurements.push_back(Point2(double(i), double(i))); + } + + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + auto K = std::make_shared(); + + // 2. Create factors manually + std::vector factors; + for (size_t i = 0; i < points.size(); ++i) { + factors.emplace_back(measurements[i], noise, poses[0], points[i], K); + } + + // 3. Construct BatchFactor + auto batch = std::make_shared>(std::move(factors)); + + // 4. Linearize + Values values; + values.insert(Symbol('x', 0), Pose3()); + for (int i = 0; i < 10; ++i) { + values.insert(Symbol('l', i), Point3(0, 0, 10)); + } + + auto gaussian = batch->linearize(values); + auto jacobian = std::dynamic_pointer_cast(gaussian); + + // 5. Verify + CHECK(jacobian); + LONGS_EQUAL(20, (long)jacobian->rows()); + // Cols depends on the number of variables and their dimensions. + // 1 Pose3 (6) + 10 Point3 (3*10) = 36 columns. + // However, JacobianFactor might store them in a block structure. + // Let's check the variable count or just existence. + LONGS_EQUAL(11, (long)jacobian->size()); // 1 pose + 10 points +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From 5b59af08dbc9268430f6e34ceb498cfb0904d032 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Nov 2025 22:28:31 -0500 Subject: [PATCH 02/10] feat: Add `BatchFactor` convenience constructor test and supporting `TestProjectionFactor` wrapper. --- gtsam/nonlinear/tests/testBatchFactor.cpp | 68 +++++++++++++++++++---- 1 file changed, 58 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/tests/testBatchFactor.cpp b/gtsam/nonlinear/tests/testBatchFactor.cpp index 2a227a0ca5..ca4992fd56 100644 --- a/gtsam/nonlinear/tests/testBatchFactor.cpp +++ b/gtsam/nonlinear/tests/testBatchFactor.cpp @@ -32,7 +32,20 @@ using namespace gtsam; using namespace std; // Define a specific projection factor type for testing -using MyProjectionFactor = GenericProjectionFactor; +// Define a wrapper to match the constructor signature expected by BatchFactor helper +class TestProjectionFactor : public GenericProjectionFactor { +public: + using Base = GenericProjectionFactor; + static std::shared_ptr sharedK; + + TestProjectionFactor() : Base() {} + + // The constructor expected by BatchFactor helper: (Key, Key, Measurement, Model) + TestProjectionFactor(Key poseKey, Key pointKey, const Point2& measured, const SharedNoiseModel& model) + : Base(measured, model, poseKey, pointKey, sharedK) {} +}; + +std::shared_ptr TestProjectionFactor::sharedK = std::make_shared(); /* ************************************************************************* */ TEST(BatchFactor, ConstructorAndLinearize) { @@ -48,16 +61,15 @@ TEST(BatchFactor, ConstructorAndLinearize) { } auto noise = noiseModel::Isotropic::Sigma(2, 1.0); - auto K = std::make_shared(); - + // 2. Create factors manually - std::vector factors; + std::vector factors; for (size_t i = 0; i < points.size(); ++i) { - factors.emplace_back(measurements[i], noise, poses[0], points[i], K); + factors.emplace_back(poses[0], points[i], measurements[i], noise); } // 3. Construct BatchFactor - auto batch = std::make_shared>(std::move(factors)); + auto batch = std::make_shared>(std::move(factors)); // 4. Linearize Values values; @@ -72,13 +84,49 @@ TEST(BatchFactor, ConstructorAndLinearize) { // 5. Verify CHECK(jacobian); LONGS_EQUAL(20, (long)jacobian->rows()); - // Cols depends on the number of variables and their dimensions. - // 1 Pose3 (6) + 10 Point3 (3*10) = 36 columns. - // However, JacobianFactor might store them in a block structure. - // Let's check the variable count or just existence. LONGS_EQUAL(11, (long)jacobian->size()); // 1 pose + 10 points } +/* ************************************************************************* */ +TEST(BatchFactor, ConvenienceConstructor) { + // 1. Setup data + std::vector poses = {Symbol('x', 0)}; + std::vector points; + std::vector measurements; + + for (int i = 0; i < 10; ++i) { + points.push_back(Symbol('l', i)); + measurements.push_back(Point2(double(i), double(i))); + } + + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + + // 2. Construct using Convenience Constructor (1 camera, 10 points) + // The helper broadcasts the single pose key against the 10 point keys. + auto batchConvenience = std::make_shared>( + poses, points, measurements, noise); + + // 3. Construct Manually for comparison + std::vector factors; + for (size_t i = 0; i < points.size(); ++i) { + factors.emplace_back(poses[0], points[i], measurements[i], noise); + } + auto batchManual = std::make_shared>(std::move(factors)); + + // 4. Linearize both + Values values; + values.insert(Symbol('x', 0), Pose3()); + for (int i = 0; i < 10; ++i) { + values.insert(Symbol('l', i), Point3(0, 0, 10)); + } + + auto gaussianConvenience = batchConvenience->linearize(values); + auto gaussianManual = batchManual->linearize(values); + + // 5. Verify they are equal + CHECK(gaussianConvenience->equals(*gaussianManual, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From e618683d5a7c8d20dd30799240ee7272f54e808d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Nov 2025 23:06:15 -0500 Subject: [PATCH 03/10] feat: Add generic BatchFactor constructor taking a container and factory function. --- gtsam/nonlinear/BatchFactor.h | 130 ++++++++++++++-------- gtsam/nonlinear/tests/testBatchFactor.cpp | 97 +++++++++++----- 2 files changed, 154 insertions(+), 73 deletions(-) diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h index af8e12a3fd..cc9b031bcb 100644 --- a/gtsam/nonlinear/BatchFactor.h +++ b/gtsam/nonlinear/BatchFactor.h @@ -18,27 +18,28 @@ #pragma once -#include +#include #include #include -#include +#include -#include -#include #include +#include #include +#include namespace gtsam { /** - * BatchFactor is a NonlinearFactor that wraps a collection of identical factors. - * It linearizes them all at once into a single JacobianFactor. - * - * This is useful for optimizing Structure-from-Motion (SfM) and SLAM graphs where - * we have many factors of the same type (e.g., projection factors) that can be - * grouped together to reduce overhead. - * - * @tparam FactorType The type of the individual factors (must derive from NoiseModelFactor) + * BatchFactor is a NonlinearFactor that wraps a collection of identical + * factors. It linearizes them all at once into a single JacobianFactor. + * + * This is useful for optimizing Structure-from-Motion (SfM) and SLAM graphs + * where we have many factors of the same type (e.g., projection factors) that + * can be grouped together to reduce overhead. + * + * @tparam FactorType The type of the individual factors (must derive from + * NoiseModelFactor) * @tparam ErrorDim The dimension of the error vector for a single factor */ template @@ -74,10 +75,39 @@ class BatchFactor : public NonlinearFactor { updateKeys(); } + /** + * @brief Generic Constructor using a Factory function. + * Allows constructing a BatchFactor from any container and a lambda that + * creates the factors. This resolves issues with varying constructor + * signatures (argument order, extra parameters). + * + * Example: + * \code + * std::map measurements; + * auto batch = std::make_shared>( + * measurements, + * [&](const std::pair& item) { + * return ProjectionFactor(item.second, model, poseKey, item.first, K); + * }); + * \endcode + * + * @tparam Container Any iterable container (std::vector, std::map, etc.) + * @tparam Factory A callable that takes an element of Container and returns a + * FactorType + */ + template + BatchFactor(const Container& container, Factory factory) { + factors_.reserve(container.size()); + for (const auto& item : container) { + factors_.push_back(factory(item)); + } + updateKeys(); + } + /** * Simplified batch constructor for binary factors (N=2). * Broadcasts keys if one of the key vectors has size 1. - * + * * @param keys1 Keys for the first variable * @param keys2 Keys for the second variable * @param measurements Vector of measurements @@ -115,9 +145,11 @@ class BatchFactor : public NonlinearFactor { /// @{ /** print */ - void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "BatchFactor with " << factors_.size() << " factors:" << std::endl; + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "BatchFactor with " << factors_.size() + << " factors:" << std::endl; for (const auto& f : factors_) { f.print("", keyFormatter); } @@ -150,13 +182,11 @@ class BatchFactor : public NonlinearFactor { } /** get the dimension of the factor (number of rows on linearization) */ - size_t dim() const override { - return factors_.size() * ErrorDim; - } + size_t dim() const override { return factors_.size() * ErrorDim; } /** * Linearize to a single JacobianFactor. - * + * * Optimization: * - Pre-calculates the total size required for the JacobianFactor. * - Collects all unique Keys involved across all sub-factors. @@ -184,21 +214,24 @@ class BatchFactor : public NonlinearFactor { // 3. Allocate JacobianFactor // We create a VerticalBlockMatrix with the correct dimensions. - // The total number of rows is the sum of the error dimensions of all factors. + // The total number of rows is the sum of the error dimensions of all + // factors. size_t total_rows = factors_.size() * ErrorDim; VerticalBlockMatrix Ab(dims, total_rows, true); - Ab.matrix().setZero(); // Important: Initialize to zero as we will fill blocks + Ab.matrix() + .setZero(); // Important: Initialize to zero as we will fill blocks // 4. Fill the JacobianFactor - // We reuse a vector of matrices for the Jacobians to avoid repeated allocations. + // We reuse a vector of matrices for the Jacobians to avoid repeated + // allocations. std::vector H(FactorType::N); - - // Optimization: Allocate Eigen::Matrix on the stack + + // Optimization: Allocate Eigen::Matrix on the stack // for the error vector computation to avoid heap fragmentation. - // Note: unwhitenedError returns a dynamic Vector, but we can use a fixed-size - // vector for intermediate storage if needed, or just rely on the fact that - // we are writing directly into the large matrix block. - // Here we use the return value of unwhitenedError directly to whiten. + // Note: unwhitenedError returns a dynamic Vector, but we can use a + // fixed-size vector for intermediate storage if needed, or just rely on the + // fact that we are writing directly into the large matrix block. Here we + // use the return value of unwhitenedError directly to whiten. for (size_t i = 0; i < factors_.size(); ++i) { const auto& factor = factors_[i]; @@ -216,14 +249,14 @@ class BatchFactor : public NonlinearFactor { // Place Jacobians into the large matrix for (size_t j = 0; j < FactorType::N; ++j) { - Key key = factor.keys()[j]; - // Find the block column index for this key. - // Since 'keys' vector is sorted (from map), we could use binary search or map lookup. - // But VerticalBlockMatrix access by Key is not O(1) unless we know the index. - // Ab(key) does a linear search or similar. - // To optimize, we could pre-calculate the block index for each key in the map. - // But for now, we use Ab(key) which is convenient. - + Key key = factor.keys()[j]; + // Find the block column index for this key. + // Since 'keys' vector is sorted (from map), we could use binary search + // or map lookup. But VerticalBlockMatrix access by Key is not O(1) + // unless we know the index. Ab(key) does a linear search or similar. To + // optimize, we could pre-calculate the block index for each key in the + // map. But for now, we use Ab(key) which is convenient. + // Ab(key) returns a block corresponding to the key. // We write into the segment corresponding to this factor. Ab(key).block(row_start, 0, ErrorDim, H[j].cols()) = H[j]; @@ -238,8 +271,8 @@ class BatchFactor : public NonlinearFactor { // 5. Create and return the JacobianFactor // We pass a Unit noise model because we have already whitened the system. - return std::make_shared(keys, std::move(Ab), - noiseModel::Unit::Create(total_rows)); + return std::make_shared( + keys, std::move(Ab), noiseModel::Unit::Create(total_rows)); } private: @@ -259,28 +292,29 @@ class BatchFactor : public NonlinearFactor { /** Helper to collect key dimensions recursively */ template - void collectKeyDims(const FactorType& f, std::map& key_dims) const { + void collectKeyDims(const FactorType& f, + std::map& key_dims) const { if constexpr (I <= FactorType::N) { // Get key and dimension for the I-th variable Key k = f.template key(); // Use traits to get dimension of the ValueType using V = typename FactorType::template ValueType; key_dims[k] = traits::dimension; - + // Recurse collectKeyDims(f, key_dims); } } }; -} // namespace gtsam +} // namespace gtsam /* * Usage Example: - * + * * // Assume we have GenericProjectionFactor * using ProjectionFactor = GenericProjectionFactor; - * + * * // Create a batch factor * std::vector poses = {Symbol('x', 1)}; * std::vector points; @@ -289,17 +323,17 @@ class BatchFactor : public NonlinearFactor { * points.push_back(Symbol('l', i)); * measurements.push_back(Point2(10, 10)); // Dummy measurement * } - * + * * auto noise = noiseModel::Isotropic::Sigma(2, 1.0); - * + * * // Construct using the helper (1 camera, 100 points) * auto batch = std::make_shared>( * poses, points, measurements, noise); - * + * * // Add to graph * NonlinearFactorGraph graph; * graph.add(batch); - * + * * // Optimize as usual * LevenbergMarquardtOptimizer optimizer(graph, initial_values); * Values result = optimizer.optimize(); diff --git a/gtsam/nonlinear/tests/testBatchFactor.cpp b/gtsam/nonlinear/tests/testBatchFactor.cpp index ca4992fd56..87f31b2cc5 100644 --- a/gtsam/nonlinear/tests/testBatchFactor.cpp +++ b/gtsam/nonlinear/tests/testBatchFactor.cpp @@ -15,45 +15,48 @@ * @author Frank Dellaert */ -#include -#include -#include -#include +#include +#include #include -#include +#include +#include #include -#include - -#include +#include +#include +#include #include using namespace gtsam; using namespace std; +using ProjectionFactor = GenericProjectionFactor; + +static std::shared_ptr sharedK = std::make_shared(); + // Define a specific projection factor type for testing -// Define a wrapper to match the constructor signature expected by BatchFactor helper -class TestProjectionFactor : public GenericProjectionFactor { -public: +// Define a wrapper to match the constructor signature expected by BatchFactor +// helper +class TestProjectionFactor : public ProjectionFactor { + public: using Base = GenericProjectionFactor; - static std::shared_ptr sharedK; TestProjectionFactor() : Base() {} - // The constructor expected by BatchFactor helper: (Key, Key, Measurement, Model) - TestProjectionFactor(Key poseKey, Key pointKey, const Point2& measured, const SharedNoiseModel& model) + // The constructor expected by BatchFactor helper: (Key, Key, Measurement, + // Model) + TestProjectionFactor(Key poseKey, Key pointKey, const Point2& measured, + const SharedNoiseModel& model) : Base(measured, model, poseKey, pointKey, sharedK) {} }; -std::shared_ptr TestProjectionFactor::sharedK = std::make_shared(); - /* ************************************************************************* */ TEST(BatchFactor, ConstructorAndLinearize) { // 1. Setup data std::vector poses = {Symbol('x', 0)}; std::vector points; std::vector measurements; - + // Create 10 points for (int i = 0; i < 10; ++i) { points.push_back(Symbol('l', i)); @@ -61,7 +64,7 @@ TEST(BatchFactor, ConstructorAndLinearize) { } auto noise = noiseModel::Isotropic::Sigma(2, 1.0); - + // 2. Create factors manually std::vector factors; for (size_t i = 0; i < points.size(); ++i) { @@ -69,7 +72,8 @@ TEST(BatchFactor, ConstructorAndLinearize) { } // 3. Construct BatchFactor - auto batch = std::make_shared>(std::move(factors)); + auto batch = std::make_shared>( + std::move(factors)); // 4. Linearize Values values; @@ -84,7 +88,7 @@ TEST(BatchFactor, ConstructorAndLinearize) { // 5. Verify CHECK(jacobian); LONGS_EQUAL(20, (long)jacobian->rows()); - LONGS_EQUAL(11, (long)jacobian->size()); // 1 pose + 10 points + LONGS_EQUAL(11, (long)jacobian->size()); // 1 pose + 10 points } /* ************************************************************************* */ @@ -93,7 +97,7 @@ TEST(BatchFactor, ConvenienceConstructor) { std::vector poses = {Symbol('x', 0)}; std::vector points; std::vector measurements; - + for (int i = 0; i < 10; ++i) { points.push_back(Symbol('l', i)); measurements.push_back(Point2(double(i), double(i))); @@ -103,15 +107,17 @@ TEST(BatchFactor, ConvenienceConstructor) { // 2. Construct using Convenience Constructor (1 camera, 10 points) // The helper broadcasts the single pose key against the 10 point keys. - auto batchConvenience = std::make_shared>( - poses, points, measurements, noise); + auto batchConvenience = + std::make_shared>( + poses, points, measurements, noise); // 3. Construct Manually for comparison std::vector factors; for (size_t i = 0; i < points.size(); ++i) { factors.emplace_back(poses[0], points[i], measurements[i], noise); } - auto batchManual = std::make_shared>(std::move(factors)); + auto batchManual = std::make_shared>( + std::move(factors)); // 4. Linearize both Values values; @@ -128,5 +134,46 @@ TEST(BatchFactor, ConvenienceConstructor) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +TEST(BatchFactor, FactoryConstructor) { + // 1. Setup data + Key poseKey = Symbol('x', 0); + std::map measurements; + + for (int i = 0; i < 10; ++i) { + measurements[Symbol('l', i)] = Point2(double(i), double(i)); + } + + auto model = noiseModel::Isotropic::Sigma(2, 1.0); + + // 2. Construct using Factory Constructor + // We iterate over the map, and the lambda creates the factor. + // This handles the "Argument Order" issue (Measurement first vs Key first) + // and "Extra Parameters" issue (K) gracefully. + auto batch = std::make_shared>( + measurements, [&](const std::pair& item) { + // item.first is Key, item.second is Measurement + return ProjectionFactor(item.second, model, poseKey, item.first, + sharedK); + }); + + // 3. Verify + Values values; + values.insert(poseKey, Pose3()); + for (int i = 0; i < 10; ++i) { + values.insert(Symbol('l', i), Point3(0, 0, 10)); + } + + auto gaussian = batch->linearize(values); + auto jacobian = std::dynamic_pointer_cast(gaussian); + + CHECK(jacobian); + LONGS_EQUAL(20, (long)jacobian->rows()); + LONGS_EQUAL(11, (long)jacobian->size()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From b4a89e715f7157ea3ca7299ade5e899356337fa9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Nov 2025 23:14:10 -0500 Subject: [PATCH 04/10] feat: Implement new BatchFactor constructors that accept measurement maps and automatically deduce factor constructor signatures using metaprogramming. --- gtsam/nonlinear/BatchFactor.h | 74 +++++++++++++++++++++++ gtsam/nonlinear/tests/testBatchFactor.cpp | 69 +++++++++++++++++++++ 2 files changed, 143 insertions(+) diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h index cc9b031bcb..9398141719 100644 --- a/gtsam/nonlinear/BatchFactor.h +++ b/gtsam/nonlinear/BatchFactor.h @@ -75,6 +75,80 @@ class BatchFactor : public NonlinearFactor { updateKeys(); } + /** + * @brief Helper to construct a factor by trying common signature patterns. + * + * Tries the following constructor signatures for FactorType: + * 1. (Key1, Key2, Measurement, Model, Args...) [Standard] + * 2. (Measurement, Model, Key1, Key2, Args...) [Projection/SFM] + * 3. (Key1, Key2, Measurement, Args..., Model) [MagFactor style] + */ + template + static FactorType createFactor(K1 k1, K2 k2, const Meas& z, + const Model& model, Args&&... args) { + if constexpr (std::is_constructible_v) { + return FactorType(k1, k2, z, model, std::forward(args)...); + } else if constexpr (std::is_constructible_v) { + return FactorType(z, model, k1, k2, std::forward(args)...); + } else if constexpr (std::is_constructible_v) { + return FactorType(k1, k2, z, std::forward(args)..., model); + } else { + // This static_assert will trigger if none of the above match. + // We repeat the check to produce a readable error message. + static_assert( + std::is_constructible_v, + "BatchFactor: Could not find a matching constructor for FactorType. " + "Tried: (K1, K2, Z, Model, Args...), (Z, Model, K1, K2, Args...), " + "(K1, K2, Z, Args..., Model)"); + return FactorType(k1, k2, z, model, std::forward(args)...); + } + } + + public: + /** + * @brief Map-based Constructor (Varying Key2). + * Constructs factors from a map of measurements, where the map key is the + * second factor key. + * + * @param key1 The fixed first key (e.g., camera pose). + * @param measurements Map from Key (2nd key) to Measurement. + * @param model Noise model. + * @param args Extra arguments passed to the factor constructor. + */ + template + BatchFactor(Key key1, const std::map& measurements, + const SharedNoiseModel& model, Args&&... args) { + factors_.reserve(measurements.size()); + for (const auto& [key2, z] : measurements) { + factors_.push_back(createFactor(key1, key2, z, model, args...)); + } + updateKeys(); + } + + /** + * @brief Map-based Constructor (Varying Key1). + * Constructs factors from a map of measurements, where the map key is the + * first factor key. + * + * @param measurements Map from Key (1st key) to Measurement. + * @param key2 The fixed second key (e.g., landmark). + * @param model Noise model. + * @param args Extra arguments passed to the factor constructor. + */ + template + BatchFactor(const std::map& measurements, Key key2, + const SharedNoiseModel& model, Args&&... args) { + factors_.reserve(measurements.size()); + for (const auto& [key1, z] : measurements) { + factors_.push_back(createFactor(key1, key2, z, model, args...)); + } + updateKeys(); + } + /** * @brief Generic Constructor using a Factory function. * Allows constructing a BatchFactor from any container and a lambda that diff --git a/gtsam/nonlinear/tests/testBatchFactor.cpp b/gtsam/nonlinear/tests/testBatchFactor.cpp index 87f31b2cc5..8284cf03eb 100644 --- a/gtsam/nonlinear/tests/testBatchFactor.cpp +++ b/gtsam/nonlinear/tests/testBatchFactor.cpp @@ -171,6 +171,75 @@ TEST(BatchFactor, FactoryConstructor) { LONGS_EQUAL(11, (long)jacobian->size()); } +/* ************************************************************************* */ +TEST(BatchFactor, MetaProgrammingConstructor_Projection) { + // 1. Setup data + Key poseKey = Symbol('x', 0); + std::map measurements; + + for (int i = 0; i < 10; ++i) { + measurements[Symbol('l', i)] = Point2(double(i), double(i)); + } + + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + + // 2. Construct using Map Constructor (ProjectionFactor style) + // This should automatically detect the signature: (Measurement, Model, Key1, + // Key2, K) We pass 'sharedK' as the extra argument. + auto batch = std::make_shared>( + poseKey, measurements, noise, sharedK); + + // 3. Verify + Values values; + values.insert(poseKey, Pose3()); + for (int i = 0; i < 10; ++i) { + values.insert(Symbol('l', i), Point3(0, 0, 10)); + } + + auto gaussian = batch->linearize(values); + auto jacobian = std::dynamic_pointer_cast(gaussian); + + CHECK(jacobian); + LONGS_EQUAL(20, (long)jacobian->rows()); + LONGS_EQUAL(11, (long)jacobian->size()); +} + +#include +#include + +/* ************************************************************************* */ +TEST(BatchFactor, MetaProgrammingConstructor_Between) { + // 1. Setup data + Key key1 = Symbol('x', 0); + std::map measurements; + + for (int i = 1; i <= 10; ++i) { + measurements[Symbol('x', i)] = Pose2(1.0, 0.0, 0.0); + } + + auto noise = noiseModel::Isotropic::Sigma(3, 0.1); + + // 2. Construct using Map Constructor (Standard style) + // This should detect: (Key1, Key2, Measurement, Model) + // BetweenFactor takes (Key, Key, Measurement, Model) + using Between = BetweenFactor; + auto batch = + std::make_shared>(key1, measurements, noise); + + // 3. Verify + Values values; + for (int i = 0; i <= 10; ++i) { + values.insert(Symbol('x', i), Pose2(double(i), 0.0, 0.0)); + } + + auto gaussian = batch->linearize(values); + auto jacobian = std::dynamic_pointer_cast(gaussian); + + CHECK(jacobian); + LONGS_EQUAL(30, (long)jacobian->rows()); // 10 factors * 3 dim + LONGS_EQUAL(11, (long)jacobian->size()); // 11 poses +} + /* ************************************************************************* */ int main() { TestResult tr; From 4adc1a3769644cd4197c4d64eb08b7d76394b80c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Nov 2025 23:24:58 -0500 Subject: [PATCH 05/10] refactor: move `createFactor` helper to `detail` namespace and remove several `BatchFactor` constructors. --- gtsam/nonlinear/BatchFactor.h | 144 +++++++--------------- gtsam/nonlinear/tests/testBatchFactor.cpp | 109 ++-------------- 2 files changed, 49 insertions(+), 204 deletions(-) diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h index 9398141719..7bcb0acc33 100644 --- a/gtsam/nonlinear/BatchFactor.h +++ b/gtsam/nonlinear/BatchFactor.h @@ -30,6 +30,43 @@ namespace gtsam { +namespace detail { + +/** + * @brief Helper to construct a factor by trying common signature patterns. + * + * Tries the following constructor signatures for FactorType: + * 1. (Key1, Key2, Measurement, Model, Args...) [Standard] + * 2. (Measurement, Model, Key1, Key2, Args...) [Projection/SFM] + * 3. (Key1, Key2, Measurement, Args..., Model) [MagFactor style] + */ +template +static FactorType createFactor(K1 k1, K2 k2, const Meas& z, const Model& model, + Args&&... args) { + if constexpr (std::is_constructible_v) { + return FactorType(k1, k2, z, model, std::forward(args)...); + } else if constexpr (std::is_constructible_v) { + return FactorType(z, model, k1, k2, std::forward(args)...); + } else if constexpr (std::is_constructible_v) { + return FactorType(k1, k2, z, std::forward(args)..., model); + } else { + // This static_assert will trigger if none of the above match. + // We repeat the check to produce a readable error message. + static_assert( + std::is_constructible_v, + "BatchFactor: Could not find a matching constructor for FactorType. " + "Tried: (K1, K2, Z, Model, Args...), (Z, Model, K1, K2, Args...), (K1, " + "K2, Z, Args..., Model)"); + return FactorType(k1, k2, z, model, std::forward(args)...); + } +} + +} // namespace detail + /** * BatchFactor is a NonlinearFactor that wraps a collection of identical * factors. It linearizes them all at once into a single JacobianFactor. @@ -61,7 +98,7 @@ class BatchFactor : public NonlinearFactor { /// @{ /** Default constructor */ - BatchFactor() {} + BatchFactor() = default; /** Constructor from a vector of factors (moves the vector) */ explicit BatchFactor(std::vector&& factors) @@ -75,40 +112,6 @@ class BatchFactor : public NonlinearFactor { updateKeys(); } - /** - * @brief Helper to construct a factor by trying common signature patterns. - * - * Tries the following constructor signatures for FactorType: - * 1. (Key1, Key2, Measurement, Model, Args...) [Standard] - * 2. (Measurement, Model, Key1, Key2, Args...) [Projection/SFM] - * 3. (Key1, Key2, Measurement, Args..., Model) [MagFactor style] - */ - template - static FactorType createFactor(K1 k1, K2 k2, const Meas& z, - const Model& model, Args&&... args) { - if constexpr (std::is_constructible_v) { - return FactorType(k1, k2, z, model, std::forward(args)...); - } else if constexpr (std::is_constructible_v) { - return FactorType(z, model, k1, k2, std::forward(args)...); - } else if constexpr (std::is_constructible_v) { - return FactorType(k1, k2, z, std::forward(args)..., model); - } else { - // This static_assert will trigger if none of the above match. - // We repeat the check to produce a readable error message. - static_assert( - std::is_constructible_v, - "BatchFactor: Could not find a matching constructor for FactorType. " - "Tried: (K1, K2, Z, Model, Args...), (Z, Model, K1, K2, Args...), " - "(K1, K2, Z, Args..., Model)"); - return FactorType(k1, k2, z, model, std::forward(args)...); - } - } - - public: /** * @brief Map-based Constructor (Varying Key2). * Constructs factors from a map of measurements, where the map key is the @@ -124,7 +127,8 @@ class BatchFactor : public NonlinearFactor { const SharedNoiseModel& model, Args&&... args) { factors_.reserve(measurements.size()); for (const auto& [key2, z] : measurements) { - factors_.push_back(createFactor(key1, key2, z, model, args...)); + factors_.push_back(detail::createFactor( + key1, key2, z, model, std::forward(args)...)); } updateKeys(); } @@ -144,72 +148,8 @@ class BatchFactor : public NonlinearFactor { const SharedNoiseModel& model, Args&&... args) { factors_.reserve(measurements.size()); for (const auto& [key1, z] : measurements) { - factors_.push_back(createFactor(key1, key2, z, model, args...)); - } - updateKeys(); - } - - /** - * @brief Generic Constructor using a Factory function. - * Allows constructing a BatchFactor from any container and a lambda that - * creates the factors. This resolves issues with varying constructor - * signatures (argument order, extra parameters). - * - * Example: - * \code - * std::map measurements; - * auto batch = std::make_shared>( - * measurements, - * [&](const std::pair& item) { - * return ProjectionFactor(item.second, model, poseKey, item.first, K); - * }); - * \endcode - * - * @tparam Container Any iterable container (std::vector, std::map, etc.) - * @tparam Factory A callable that takes an element of Container and returns a - * FactorType - */ - template - BatchFactor(const Container& container, Factory factory) { - factors_.reserve(container.size()); - for (const auto& item : container) { - factors_.push_back(factory(item)); - } - updateKeys(); - } - - /** - * Simplified batch constructor for binary factors (N=2). - * Broadcasts keys if one of the key vectors has size 1. - * - * @param keys1 Keys for the first variable - * @param keys2 Keys for the second variable - * @param measurements Vector of measurements - * @param model Shared noise model - */ - template - BatchFactor(const std::vector& keys1, const std::vector& keys2, - const std::vector& measurements, - const SharedNoiseModel& model) { - static_assert(FactorType::N == 2, - "Helper constructor only available for binary factors (N=2)"); - - size_t n = measurements.size(); - bool broadcast1 = (keys1.size() == 1); - bool broadcast2 = (keys2.size() == 1); - - if (!broadcast1 && keys1.size() != n) { - throw std::invalid_argument("BatchFactor: keys1 size mismatch"); - } - if (!broadcast2 && keys2.size() != n) { - throw std::invalid_argument("BatchFactor: keys2 size mismatch"); - } - - factors_.reserve(n); - for (size_t i = 0; i < n; ++i) { - Key k1 = broadcast1 ? keys1[0] : keys1[i]; - Key k2 = broadcast2 ? keys2[0] : keys2[i]; - factors_.emplace_back(k1, k2, measurements[i], model); + factors_.push_back(detail::createFactor( + key1, key2, z, model, std::forward(args)...)); } updateKeys(); } diff --git a/gtsam/nonlinear/tests/testBatchFactor.cpp b/gtsam/nonlinear/tests/testBatchFactor.cpp index 8284cf03eb..abb1803988 100644 --- a/gtsam/nonlinear/tests/testBatchFactor.cpp +++ b/gtsam/nonlinear/tests/testBatchFactor.cpp @@ -30,26 +30,11 @@ using namespace gtsam; using namespace std; +// Define a specific projection factor type for testing using ProjectionFactor = GenericProjectionFactor; static std::shared_ptr sharedK = std::make_shared(); -// Define a specific projection factor type for testing -// Define a wrapper to match the constructor signature expected by BatchFactor -// helper -class TestProjectionFactor : public ProjectionFactor { - public: - using Base = GenericProjectionFactor; - - TestProjectionFactor() : Base() {} - - // The constructor expected by BatchFactor helper: (Key, Key, Measurement, - // Model) - TestProjectionFactor(Key poseKey, Key pointKey, const Point2& measured, - const SharedNoiseModel& model) - : Base(measured, model, poseKey, pointKey, sharedK) {} -}; - /* ************************************************************************* */ TEST(BatchFactor, ConstructorAndLinearize) { // 1. Setup data @@ -66,14 +51,14 @@ TEST(BatchFactor, ConstructorAndLinearize) { auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // 2. Create factors manually - std::vector factors; + std::vector factors; for (size_t i = 0; i < points.size(); ++i) { - factors.emplace_back(poses[0], points[i], measurements[i], noise); + factors.emplace_back(measurements[i], noise, poses[0], points[i], sharedK); } // 3. Construct BatchFactor - auto batch = std::make_shared>( - std::move(factors)); + auto batch = + std::make_shared>(std::move(factors)); // 4. Linearize Values values; @@ -92,87 +77,7 @@ TEST(BatchFactor, ConstructorAndLinearize) { } /* ************************************************************************* */ -TEST(BatchFactor, ConvenienceConstructor) { - // 1. Setup data - std::vector poses = {Symbol('x', 0)}; - std::vector points; - std::vector measurements; - - for (int i = 0; i < 10; ++i) { - points.push_back(Symbol('l', i)); - measurements.push_back(Point2(double(i), double(i))); - } - - auto noise = noiseModel::Isotropic::Sigma(2, 1.0); - - // 2. Construct using Convenience Constructor (1 camera, 10 points) - // The helper broadcasts the single pose key against the 10 point keys. - auto batchConvenience = - std::make_shared>( - poses, points, measurements, noise); - - // 3. Construct Manually for comparison - std::vector factors; - for (size_t i = 0; i < points.size(); ++i) { - factors.emplace_back(poses[0], points[i], measurements[i], noise); - } - auto batchManual = std::make_shared>( - std::move(factors)); - - // 4. Linearize both - Values values; - values.insert(Symbol('x', 0), Pose3()); - for (int i = 0; i < 10; ++i) { - values.insert(Symbol('l', i), Point3(0, 0, 10)); - } - - auto gaussianConvenience = batchConvenience->linearize(values); - auto gaussianManual = batchManual->linearize(values); - - // 5. Verify they are equal - CHECK(gaussianConvenience->equals(*gaussianManual, 1e-9)); -} - -/* ************************************************************************* */ -TEST(BatchFactor, FactoryConstructor) { - // 1. Setup data - Key poseKey = Symbol('x', 0); - std::map measurements; - - for (int i = 0; i < 10; ++i) { - measurements[Symbol('l', i)] = Point2(double(i), double(i)); - } - - auto model = noiseModel::Isotropic::Sigma(2, 1.0); - - // 2. Construct using Factory Constructor - // We iterate over the map, and the lambda creates the factor. - // This handles the "Argument Order" issue (Measurement first vs Key first) - // and "Extra Parameters" issue (K) gracefully. - auto batch = std::make_shared>( - measurements, [&](const std::pair& item) { - // item.first is Key, item.second is Measurement - return ProjectionFactor(item.second, model, poseKey, item.first, - sharedK); - }); - - // 3. Verify - Values values; - values.insert(poseKey, Pose3()); - for (int i = 0; i < 10; ++i) { - values.insert(Symbol('l', i), Point3(0, 0, 10)); - } - - auto gaussian = batch->linearize(values); - auto jacobian = std::dynamic_pointer_cast(gaussian); - - CHECK(jacobian); - LONGS_EQUAL(20, (long)jacobian->rows()); - LONGS_EQUAL(11, (long)jacobian->size()); -} - -/* ************************************************************************* */ -TEST(BatchFactor, MetaProgrammingConstructor_Projection) { +TEST(BatchFactor, Constructor_Projection) { // 1. Setup data Key poseKey = Symbol('x', 0); std::map measurements; @@ -208,7 +113,7 @@ TEST(BatchFactor, MetaProgrammingConstructor_Projection) { #include /* ************************************************************************* */ -TEST(BatchFactor, MetaProgrammingConstructor_Between) { +TEST(BatchFactor, Constructor_Between) { // 1. Setup data Key key1 = Symbol('x', 0); std::map measurements; From 64d4375f5a4aea44183b617963d33b7f8f6dcf26 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 19 Nov 2025 01:32:22 -0500 Subject: [PATCH 06/10] Fix Jacobian --- gtsam/nonlinear/BatchFactor.h | 43 ++++++++++++++++------------------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h index 7bcb0acc33..49962b1ae9 100644 --- a/gtsam/nonlinear/BatchFactor.h +++ b/gtsam/nonlinear/BatchFactor.h @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -91,7 +92,8 @@ class BatchFactor : public NonlinearFactor { using shared_ptr = std::shared_ptr; private: - std::vector factors_; ///< Contiguous storage for factors + using Allocator = Eigen::aligned_allocator; + std::vector factors_; ///< Contiguous storage public: /// @name Constructors @@ -101,13 +103,13 @@ class BatchFactor : public NonlinearFactor { BatchFactor() = default; /** Constructor from a vector of factors (moves the vector) */ - explicit BatchFactor(std::vector&& factors) + explicit BatchFactor(std::vector&& factors) : factors_(std::move(factors)) { updateKeys(); } /** Constructor from a vector of factors (copies the vector) */ - explicit BatchFactor(const std::vector& factors) + explicit BatchFactor(const std::vector& factors) : factors_(factors) { updateKeys(); } @@ -162,7 +164,8 @@ class BatchFactor : public NonlinearFactor { void print( const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "BatchFactor with " << factors_.size() + Base::print(s, keyFormatter); + std::cout << "BatchFactor with " << factors_.size() << " factors:" << std::endl; for (const auto& f : factors_) { f.print("", keyFormatter); @@ -217,13 +220,14 @@ class BatchFactor : public NonlinearFactor { } // 2. Prepare keys and dimensions for JacobianFactor construction - std::vector keys; + const size_t numKeys = key_dims.size(); std::vector dims; - keys.reserve(key_dims.size()); - dims.reserve(key_dims.size()); - for (const auto& [key, dim] : key_dims) { - keys.push_back(key); - dims.push_back(dim); + dims.reserve(numKeys); + std::map indices; + DenseIndex index = 0; + for (const auto& key : keys()) { + dims.push_back(key_dims.at(key)); + indices[key] = index++; } // 3. Allocate JacobianFactor @@ -262,31 +266,24 @@ class BatchFactor : public NonlinearFactor { } // Place Jacobians into the large matrix - for (size_t j = 0; j < FactorType::N; ++j) { - Key key = factor.keys()[j]; + for (size_t k = 0; k < FactorType::N; ++k) { + Key key = factor.keys()[k]; // Find the block column index for this key. - // Since 'keys' vector is sorted (from map), we could use binary search - // or map lookup. But VerticalBlockMatrix access by Key is not O(1) - // unless we know the index. Ab(key) does a linear search or similar. To - // optimize, we could pre-calculate the block index for each key in the - // map. But for now, we use Ab(key) which is convenient. - - // Ab(key) returns a block corresponding to the key. - // We write into the segment corresponding to this factor. - Ab(key).block(row_start, 0, ErrorDim, H[j].cols()) = H[j]; + DenseIndex index = indices.at(key); + Ab(index).block(row_start, 0, ErrorDim, H[k].cols()) = H[k]; } // Place the negative error into the RHS (last column) // JacobianFactor stores Ax - b, so b = -error. // Ab(size) gives the last block which is the RHS vector b. // We use block() to access the segment as a matrix block. - Ab(keys.size()).block(row_start, 0, ErrorDim, 1) = -raw_error; + Ab(indices.size()).block(row_start, 0, ErrorDim, 1) = -raw_error; } // 5. Create and return the JacobianFactor // We pass a Unit noise model because we have already whitened the system. return std::make_shared( - keys, std::move(Ab), noiseModel::Unit::Create(total_rows)); + keys(), std::move(Ab), noiseModel::Unit::Create(total_rows)); } private: From 80486fc7286ce65edcb902460f0c1b207824d943 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 19 Nov 2025 01:33:11 -0500 Subject: [PATCH 07/10] Compare with batch version --- timing/timeSFMBAL.cpp | 51 ++++++++++++++++++++++++++++++++++--------- 1 file changed, 41 insertions(+), 10 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index c1f36abd06..c1abbd401f 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -18,11 +18,11 @@ #include "timeSFMBAL.h" -#include #include #include #include - +#include +#include using namespace std; using namespace gtsam; @@ -34,22 +34,53 @@ int main(int argc, char* argv[]) { // parse options and read BAL file SfmData db = preamble(argc, argv); - // Build graph using conventional GeneralSFMFactor + // 1. Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.numberTracks(); j++) { - for (const SfmMeasurement& m: db.tracks[j].measurements) { + for (const SfmMeasurement& m : db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; graph.emplace_shared(z, gNoiseModel, C(i), P(j)); } } - Values initial; size_t i = 0, j = 0; - for (const SfmCamera& camera: db.cameras) - initial.insert(C(i++), camera); - for (const SfmTrack& track: db.tracks) - initial.insert(P(j++), track.p); + for (const SfmCamera& camera : db.cameras) initial.insert(C(i++), camera); + for (const SfmTrack& track : db.tracks) initial.insert(P(j++), track.p); + + cout << "Optimizing Regular Graph..." << endl; + optimize(db, graph, initial); + + // 2. Build graph using BatchFactor + // We batch by Point (Track). Each batch contains measurements from multiple + // cameras for one point. + NonlinearFactorGraph graphBatch; + // Limit to 10 tracks for debugging + size_t num_tracks = std::min((size_t)10, db.numberTracks()); + for (size_t j = 0; j < num_tracks; j++) { + std::map measurements; + for (const SfmMeasurement& m : db.tracks[j].measurements) { + measurements[C(m.first)] = m.second; + } + + // BatchFactor(Vector) + std::vector> factors; + for (const auto& [key, z] : measurements) { + // Correct order: (Camera, Point) + factors.emplace_back(z, gNoiseModel, key, P(j)); + } + auto batch = + std::make_shared>(std::move(factors)); + // GTSAM_PRINT(*batch); + // auto linearized = batch->linearize(initial); + // cout << "Linearized batch factor " << j << endl; + // GTSAM_PRINT(*linearized); + + graphBatch.add(batch); + } + + cout << "Optimizing Batch Graph..." << endl; + optimize(db, graphBatch, initial); - return optimize(db, graph, initial); + return 0; } From 7c026d642982ef1508b1741c99252bde398b32c2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 19 Nov 2025 01:53:37 -0500 Subject: [PATCH 08/10] Fixed bug in timing [skip ci] --- timing/timeSFMBAL.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index c1abbd401f..b73b07c96f 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -48,16 +48,17 @@ int main(int argc, char* argv[]) { for (const SfmCamera& camera : db.cameras) initial.insert(C(i++), camera); for (const SfmTrack& track : db.tracks) initial.insert(P(j++), track.p); - cout << "Optimizing Regular Graph..." << endl; - optimize(db, graph, initial); + { + gttic_(regular); + cout << "Optimizing Regular Graph..." << endl; + optimize(db, graph, initial); + } // 2. Build graph using BatchFactor // We batch by Point (Track). Each batch contains measurements from multiple // cameras for one point. NonlinearFactorGraph graphBatch; - // Limit to 10 tracks for debugging - size_t num_tracks = std::min((size_t)10, db.numberTracks()); - for (size_t j = 0; j < num_tracks; j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { std::map measurements; for (const SfmMeasurement& m : db.tracks[j].measurements) { measurements[C(m.first)] = m.second; @@ -79,8 +80,10 @@ int main(int argc, char* argv[]) { graphBatch.add(batch); } - cout << "Optimizing Batch Graph..." << endl; - optimize(db, graphBatch, initial); - + { + gttic_(batch); + cout << "Optimizing Batch Graph..." << endl; + optimize(db, graphBatch, initial); + } return 0; } From db9b65643963f96c509acdb4278a4252abe9dc10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 19 Nov 2025 16:03:47 -0500 Subject: [PATCH 09/10] Lots of experimentation, no clear win Compare with Metis Dramatically simplified Hessian path (and way faster!) Add linearize time Hessian! Kill "fast" path revert that More micro-optimization ScratchMatrix offsets_ added a Value cache feat: Optimize BatchFactor linearization by enabling zero-malloc Jacobian computation via OptionalJacobian stride support. refactor: optimize BatchFactor key and dimension indexing. refactor: cache factor-specific key indices in BatchFactor for improved linearization and enable LM sequential Cholesky solver with summary verbosity. refactor: Pre-calculate key dimensions and indices in BatchFactor, add vector constructors, and enable LM parameters in timeSFMBAL. Address comments --- gtsam/base/OptionalJacobian.h | 54 +++-- gtsam/nonlinear/BatchFactor.h | 225 ++++++++++-------- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 47 ++-- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 3 + gtsam/nonlinear/tests/testBatchFactor.cpp | 116 ++++++++- gtsam/slam/GeneralSFMFactor.h | 204 +++++++++------- timing/timeSFMBAL.cpp | 22 ++ timing/timeSFMBAL.h | 11 +- 8 files changed, 436 insertions(+), 246 deletions(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 22a17e131b..1e18b66b49 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -42,20 +42,23 @@ class OptionalJacobian { /// Jacobian size type /// TODO(frank): how to enforce RowMajor? Or better, make it work with any storage order? typedef Eigen::Matrix Jacobian; + typedef Eigen::Stride Stride; + typedef Eigen::Map Map; private: - Eigen::Map map_; /// View on constructor argument, if given + Map map_; /// View on constructor argument, if given // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html // uses "placement new" to make map_ usurp the memory of the fixed size matrix - void usurp(double* data) { - new (&map_) Eigen::Map(data); + void usurp(double* data, Eigen::Index outerStride) { + new (&map_) Map(data, Stride(outerStride, 1)); } // Private and very dangerous constructor straight from memory - OptionalJacobian(double* data) : map_(nullptr) { - if (data) usurp(data); + OptionalJacobian(double* data, Eigen::Index outerStride) + : map_(nullptr, Stride(1, 1)) { + if (data) usurp(data, outerStride); } template @@ -64,43 +67,42 @@ class OptionalJacobian { public: /// Default constructor - OptionalJacobian() : - map_(nullptr) { + OptionalJacobian() : map_(nullptr, Stride(1, 1)) { } /// Default constructor with nullptr_t /// To guide the compiler when nullptr /// is passed to args of the type OptionalJacobian OptionalJacobian(std::nullptr_t /*unused*/) : - map_(nullptr) { + map_(nullptr, Stride(1, 1)) { } /// Constructor that will usurp data of a fixed-size matrix OptionalJacobian(Jacobian& fixed) : - map_(nullptr) { - usurp(fixed.data()); + map_(nullptr, Stride(1, 1)) { + usurp(fixed.data(), fixed.rows()); } /// Constructor that will usurp data of a fixed-size matrix, pointer version OptionalJacobian(Jacobian* fixedPtr) : - map_(nullptr) { + map_(nullptr, Stride(1, 1)) { if (fixedPtr) - usurp(fixedPtr->data()); + usurp(fixedPtr->data(), fixedPtr->rows()); } /// Constructor that will resize a dynamic matrix (unless already correct) OptionalJacobian(Eigen::MatrixXd& dynamic) : - map_(nullptr) { + map_(nullptr, Stride(1, 1)) { dynamic.resize(Rows, Cols); // no malloc if correct size - usurp(dynamic.data()); + usurp(dynamic.data(), dynamic.rows()); } /// Constructor that will resize a dynamic matrix (unless already correct) OptionalJacobian(Eigen::MatrixXd* dynamic) : - map_(nullptr) { + map_(nullptr, Stride(1, 1)) { if (dynamic) { dynamic->resize(Rows, Cols); // no malloc if correct size - usurp(dynamic->data()); + usurp(dynamic->data(), dynamic->rows()); } } @@ -110,9 +112,10 @@ class OptionalJacobian { */ template OptionalJacobian(Eigen::Ref dynamic_ref) : - map_(nullptr) { - if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) { - usurp(dynamic_ref.data()); + map_(nullptr, Stride(1, 1)) { + if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && + !dynamic_ref.IsRowMajor && dynamic_ref.innerStride() == 1) { + usurp(dynamic_ref.data(), dynamic_ref.outerStride()); } else { throw std::invalid_argument( std::string("OptionalJacobian called with wrong dimensions or " @@ -124,15 +127,15 @@ class OptionalJacobian { /// Constructor with std::nullopt just makes empty OptionalJacobian(std::nullopt_t /*none*/) : - map_(nullptr) { + map_(nullptr, Stride(1, 1)) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const std::optional> optional) : - map_(nullptr) { + map_(nullptr, Stride(1, 1)) { if (optional) { optional->get().resize(Rows, Cols); - usurp(optional->get().data()); + usurp(optional->get().data(), optional->get().rows()); } } /// Constructor that will usurp data of a block expression @@ -146,12 +149,12 @@ class OptionalJacobian { } /// De-reference, like boost optional - Eigen::Map& operator*() { + Map& operator*() { return map_; } /// operator->() - Eigen::Map* operator->() { + Map* operator->() { return &map_; } @@ -171,7 +174,7 @@ class OptionalJacobian { template OptionalJacobian cols(int startCol) { if (*this) - return OptionalJacobian(&map_(0,startCol)); + return OptionalJacobian(&map_(0, startCol), map_.outerStride()); else return OptionalJacobian(); } @@ -261,4 +264,3 @@ struct MakeOptionalJacobian { }; } // namespace gtsam - diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h index 49962b1ae9..860ad98832 100644 --- a/gtsam/nonlinear/BatchFactor.h +++ b/gtsam/nonlinear/BatchFactor.h @@ -13,18 +13,21 @@ * @file BatchFactor.h * @brief A batch of factors that linearizes to a single JacobianFactor * @author Frank Dellaert - * @date Nov 2023 + * @author Fan Jiang + * @date Nov 2025 */ #pragma once #include +#include #include #include #include #include #include +#include #include #include #include @@ -76,12 +79,39 @@ static FactorType createFactor(K1 k1, K2 k2, const Meas& z, const Model& model, * where we have many factors of the same type (e.g., projection factors) that * can be grouped together to reduce overhead. * - * @tparam FactorType The type of the individual factors (must derive from - * NoiseModelFactor) + * Usage Example: + * + * // Assume we have GenericProjectionFactor + * using ProjectionFactor = GenericProjectionFactor; + * + * // Create a batch factor + * std::vector poses = {Symbol('x', 1)}; + * std::vector points; + * std::vector measurements; + * for (int i = 0; i < 100; ++i) { + * points.push_back(Symbol('l', i)); + * measurements.push_back(Point2(10, 10)); // Dummy measurement + * } + * + * auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + * + * // Construct using the helper (1 camera, 100 points) + * auto batch = std::make_shared>( + * poses, points, measurements, noise); + * + * // Add to graph + * NonlinearFactorGraph graph; + * graph.add(batch); + * + * // Optimize as usual + * LevenbergMarquardtOptimizer optimizer(graph, initial_values); + * Values result = optimizer.optimize(); + * + * @tparam FactorType The type of the individual factors * @tparam ErrorDim The dimension of the error vector for a single factor */ template -class BatchFactor : public NonlinearFactor { +class GTSAM_EXPORT BatchFactor : public NonlinearFactor { public: // Static assertion to ensure FactorType derives from NoiseModelFactor static_assert(std::is_base_of::value, @@ -94,6 +124,15 @@ class BatchFactor : public NonlinearFactor { private: using Allocator = Eigen::aligned_allocator; std::vector factors_; ///< Contiguous storage + struct KeyInfo { + Key key; + size_t dim; + size_t slot; + DenseIndex offset; + }; + std::vector keyInfo_; + std::vector> indices_; + bool useHessianFactor_{false}; public: /// @name Constructors @@ -114,6 +153,22 @@ class BatchFactor : public NonlinearFactor { updateKeys(); } + /** Constructor from a standard vector of factors (copies the vector) */ + explicit BatchFactor(const std::vector& factors) { + factors_.reserve(factors.size()); + factors_.assign(factors.begin(), factors.end()); + updateKeys(); + } + + /** Constructor from a standard vector of factors (moves elements) */ + explicit BatchFactor(std::vector&& factors) { + factors_.reserve(factors.size()); + for (auto&& f : factors) { + factors_.push_back(std::move(f)); + } + updateKeys(); + } + /** * @brief Map-based Constructor (Varying Key2). * Constructs factors from a map of measurements, where the map key is the @@ -160,7 +215,7 @@ class BatchFactor : public NonlinearFactor { /// @name Testable /// @{ - /** print */ + /// Print the BatchFactor void print( const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { @@ -172,7 +227,7 @@ class BatchFactor : public NonlinearFactor { } } - /** equals */ + /// Check equality with another factor. bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const This* p = dynamic_cast(&f); if (!p || factors_.size() != p->factors_.size()) return false; @@ -198,9 +253,11 @@ class BatchFactor : public NonlinearFactor { return total_error; } - /** get the dimension of the factor (number of rows on linearization) */ + /// Get the dimension of the factor (number of rows on linearization) size_t dim() const override { return factors_.size() * ErrorDim; } + void setUseHessianFactor(bool flag) { useHessianFactor_ = flag; } + /** * Linearize to a single JacobianFactor. * @@ -210,54 +267,34 @@ class BatchFactor : public NonlinearFactor { * - Iterates linearly over factors_ (cache-friendly) to compute Jacobians. * - Fills the pre-allocated JacobianFactor directly. */ - std::shared_ptr linearize(const Values& c) const override { + std::shared_ptr linearize( + const Values& values) const override { if (factors_.empty()) return std::make_shared(); - // 1. Collect all unique keys and their dimensions - std::map key_dims; - for (const auto& factor : factors_) { - collectKeyDims<1>(factor, key_dims); - } - - // 2. Prepare keys and dimensions for JacobianFactor construction - const size_t numKeys = key_dims.size(); - std::vector dims; - dims.reserve(numKeys); - std::map indices; - DenseIndex index = 0; - for (const auto& key : keys()) { - dims.push_back(key_dims.at(key)); - indices[key] = index++; - } - // 3. Allocate JacobianFactor // We create a VerticalBlockMatrix with the correct dimensions. // The total number of rows is the sum of the error dimensions of all // factors. size_t total_rows = factors_.size() * ErrorDim; + std::vector dims; + dims.reserve(keyInfo_.size()); + for (const auto& info : keyInfo_) dims.push_back(info.dim); VerticalBlockMatrix Ab(dims, total_rows, true); - Ab.matrix() - .setZero(); // Important: Initialize to zero as we will fill blocks + Ab.matrix().setZero(); // Important: Initialize to zero. // 4. Fill the JacobianFactor // We reuse a vector of matrices for the Jacobians to avoid repeated // allocations. std::vector H(FactorType::N); - - // Optimization: Allocate Eigen::Matrix on the stack - // for the error vector computation to avoid heap fragmentation. - // Note: unwhitenedError returns a dynamic Vector, but we can use a - // fixed-size vector for intermediate storage if needed, or just rely on the - // fact that we are writing directly into the large matrix block. Here we - // use the return value of unwhitenedError directly to whiten. + std::vector cache(keys().size(), nullptr); for (size_t i = 0; i < factors_.size(); ++i) { const auto& factor = factors_[i]; size_t row_start = i * ErrorDim; - // Compute unwhitened error and Jacobians + // Legacy path (mallocs) // We use the factor's unwhitenedError method which fills H. - Vector raw_error = factor.unwhitenedError(c, H); + Vector raw_error = factor.unwhitenedError(values, H); // Apply noise model (whitening) // This modifies H and raw_error in place. @@ -266,10 +303,9 @@ class BatchFactor : public NonlinearFactor { } // Place Jacobians into the large matrix + const auto& indices_i = indices_[i]; for (size_t k = 0; k < FactorType::N; ++k) { - Key key = factor.keys()[k]; - // Find the block column index for this key. - DenseIndex index = indices.at(key); + const DenseIndex index = indices_i[k]; Ab(index).block(row_start, 0, ErrorDim, H[k].cols()) = H[k]; } @@ -277,75 +313,74 @@ class BatchFactor : public NonlinearFactor { // JacobianFactor stores Ax - b, so b = -error. // Ab(size) gives the last block which is the RHS vector b. // We use block() to access the segment as a matrix block. - Ab(indices.size()).block(row_start, 0, ErrorDim, 1) = -raw_error; + Ab(keys().size()).block(row_start, 0, ErrorDim, 1) = -raw_error; } // 5. Create and return the JacobianFactor // We pass a Unit noise model because we have already whitened the system. - return std::make_shared( + auto jacobian = std::make_shared( keys(), std::move(Ab), noiseModel::Unit::Create(total_rows)); + if (useHessianFactor_) { + return std::make_shared(*jacobian); + } + return jacobian; } - private: - /** Helper to collect keys from factors */ + /// Helper to collect keys and dimensions using fold expression + template + void collectKeys(const FactorType& f, std::index_sequence) { + (keyInfo_.push_back(KeyInfo{ + f.keys()[Is], + traits>::dimension, Is, + 0}), + ...); + } + + /// Update keys_ by collecting unique keys from all factors void updateKeys() { - std::set unique_keys; + // 1. Collect all keys and their dimensions + keyInfo_.clear(); + keyInfo_.reserve(factors_.size() * FactorType::N); + for (const auto& f : factors_) { - for (size_t i = 0; i < FactorType::N; ++i) { - // Access keys via the base NoiseModelFactor keys_ array if possible, - // or use the key() method. NoiseModelFactorN exposes key(). - // We can also access the public keys() method from NonlinearFactor. - unique_keys.insert(f.keys()[i]); - } + collectKeys(f, std::make_index_sequence{}); } - this->keys_.assign(unique_keys.begin(), unique_keys.end()); - } - /** Helper to collect key dimensions recursively */ - template - void collectKeyDims(const FactorType& f, - std::map& key_dims) const { - if constexpr (I <= FactorType::N) { - // Get key and dimension for the I-th variable - Key k = f.template key(); - // Use traits to get dimension of the ValueType - using V = typename FactorType::template ValueType; - key_dims[k] = traits::dimension; - - // Recurse - collectKeyDims(f, key_dims); + // 2. Sort and remove duplicates to get unique keys + std::sort(keyInfo_.begin(), keyInfo_.end(), + [](const KeyInfo& a, const KeyInfo& b) { return a.key < b.key; }); + auto isDuplicate = [](const KeyInfo& a, const KeyInfo& b) { + if (a.key != b.key) return false; + assert(a.dim == b.dim); + assert(a.slot == b.slot); + return true; + }; + auto last = std::unique(keyInfo_.begin(), keyInfo_.end(), isDuplicate); + keyInfo_.erase(last, keyInfo_.end()); + + // 3. Fill keys_ and key information + keys_.clear(); + keys_.reserve(keyInfo_.size()); + DenseIndex offset = 0; + for (auto& info : keyInfo_) { + info.offset = offset; + keys_.push_back(info.key); + offset += static_cast(info.dim); + } + + // 4. Cache factor indices + // Since keys_ is sorted, we can use binary search + indices_.clear(); + indices_.reserve(factors_.size()); + for (const auto& f : factors_) { + std::array indices_i; + for (size_t k = 0; k < FactorType::N; ++k) { + auto it = std::lower_bound(keys_.begin(), keys_.end(), f.keys()[k]); + indices_i[k] = std::distance(keys_.begin(), it); + } + indices_.push_back(indices_i); } } }; } // namespace gtsam - -/* - * Usage Example: - * - * // Assume we have GenericProjectionFactor - * using ProjectionFactor = GenericProjectionFactor; - * - * // Create a batch factor - * std::vector poses = {Symbol('x', 1)}; - * std::vector points; - * std::vector measurements; - * for (int i = 0; i < 100; ++i) { - * points.push_back(Symbol('l', i)); - * measurements.push_back(Point2(10, 10)); // Dummy measurement - * } - * - * auto noise = noiseModel::Isotropic::Sigma(2, 1.0); - * - * // Construct using the helper (1 camera, 100 points) - * auto batch = std::make_shared>( - * poses, points, measurements, noise); - * - * // Add to graph - * NonlinearFactorGraph graph; - * graph.add(batch); - * - * // Optimize as usual - * LevenbergMarquardtOptimizer optimizer(graph, initial_values); - * Values result = optimizer.optimize(); - */ diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index f6711f0f08..3fcd267cfa 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -122,18 +122,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal) { auto currentState = static_cast(state_.get()); bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); - -#if GTSAM_USE_BOOST_FEATURES -#ifdef GTSAM_USING_NEW_BOOST_TIMERS - boost::timer::cpu_timer lamda_iteration_timer; - lamda_iteration_timer.start(); -#else - boost::timer lamda_iteration_timer; - lamda_iteration_timer.restart(); -#endif -#else - auto start = std::chrono::high_resolution_clock::now(); -#endif + auto solveStart = std::chrono::high_resolution_clock::now(); if (verbose) cout << "trying lambda = " << currentState->lambda << endl; @@ -221,24 +210,25 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, } } // if (systemSolvedSuccessfully) + lastSolveTime_ = + std::chrono::duration(std::chrono::high_resolution_clock::now() - + solveStart) + .count(); if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { -#if GTSAM_USE_BOOST_FEATURES -// do timing -#ifdef GTSAM_USING_NEW_BOOST_TIMERS - double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; -#else - double iterationTime = lamda_iteration_timer.elapsed(); -#endif -#else - auto end = std::chrono::high_resolution_clock::now(); - double iterationTime = std::chrono::duration_cast(end - start).count() / 1e6; -#endif + double iterationTime = + std::chrono::duration(std::chrono::high_resolution_clock::now() - + iterationStart_) + .count(); if (currentState->iterations == 0) { - cout << "iter cost cost_change lambda success iter_time" << endl; + cout << "iter cost cost_change lambda success " + "lin_time solve_time total_time" + << endl; } cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2) << costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6) - << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl; + << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << lastLinearizeTime_ << " " + << setw(10) << setprecision(2) << lastSolveTime_ << " " << setw(10) << setprecision(2) << iterationTime + << endl; } if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity @@ -275,10 +265,16 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::iterate() { gttic(LM_iterate); + iterationStart_ = std::chrono::high_resolution_clock::now(); + auto linStart = iterationStart_; // Linearize graph if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); + lastLinearizeTime_ = + std::chrono::duration(std::chrono::high_resolution_clock::now() - + linStart) + .count(); if(currentState->totalNumberInnerIterations==0) { // write initial error writeLogFile(currentState->error); @@ -308,4 +304,3 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::iterate() { } } /* namespace gtsam */ - diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 826eed5d69..62ce75e4d7 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -39,6 +39,9 @@ class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer { // startTime_ is a chrono time point std::chrono::time_point startTime_; ///< time when optimization started + std::chrono::time_point iterationStart_; + double lastLinearizeTime_{0.0}; + double lastSolveTime_{0.0}; void initTime(); diff --git a/gtsam/nonlinear/tests/testBatchFactor.cpp b/gtsam/nonlinear/tests/testBatchFactor.cpp index abb1803988..b542d8157f 100644 --- a/gtsam/nonlinear/tests/testBatchFactor.cpp +++ b/gtsam/nonlinear/tests/testBatchFactor.cpp @@ -19,10 +19,14 @@ #include #include #include +#include #include #include #include #include +#include +#include +#include #include #include @@ -109,9 +113,6 @@ TEST(BatchFactor, Constructor_Projection) { LONGS_EQUAL(11, (long)jacobian->size()); } -#include -#include - /* ************************************************************************* */ TEST(BatchFactor, Constructor_Between) { // 1. Setup data @@ -145,6 +146,115 @@ TEST(BatchFactor, Constructor_Between) { LONGS_EQUAL(11, (long)jacobian->size()); // 11 poses } +/* ************************************************************************* */ +TEST(BatchFactor, MultipleCamerasPerPoint) { + // Create a batch factor with 2 factors: + // Factor 1: cam1, point + // Factor 2: cam2, point + // This mimics the timeSFMBAL scenario where we batch by point. + + using Factor = GeneralSFMFactor, Point3>; + + std::vector factors; + Key c1 = Symbol('c', 1); + Key c2 = Symbol('c', 2); + Key p0 = Symbol('p', 1); + + Point2 measurement(0, 0); + auto noise = noiseModel::Unit::Create(2); + auto K = std::make_shared(); + + factors.emplace_back(measurement, noise, c1, p0); + factors.emplace_back(measurement, noise, c2, p0); + + BatchFactor batch(factors); + + // Create values + Values values; + values.insert(c1, PinholeCamera(Pose3(), *K)); + values.insert(c2, PinholeCamera(Pose3(Rot3(), Point3(1, 0, 0)), *K)); + values.insert(p0, Point3(0, 0, 10)); + + auto gaussian = batch.linearize(values); + auto jacobian = std::dynamic_pointer_cast(gaussian); + + // Verify sparsity + // keys_ should be {c1, c2, p1} (sorted) + // Indices: c1->0, c2->1, p1->2 + + // Jacobian has 4 rows (2 factors * 2 dim) + // Block 0 (c1): Should be non-zero in rows 0-1, ZERO in rows 2-3 + // Block 1 (c2): Should be ZERO in rows 0-1, non-zero in rows 2-3 + // Block 2 (p1): Should be non-zero in all rows + + Matrix A_c1 = jacobian->getA(jacobian->begin()); + Matrix A_c2 = jacobian->getA(jacobian->begin() + 1); + Matrix A_p1 = jacobian->getA(jacobian->begin() + 2); + + // Check dimensions + EXPECT_LONGS_EQUAL(4, A_c1.rows()); + EXPECT_LONGS_EQUAL(11, A_c1.cols()); + EXPECT_LONGS_EQUAL(4, A_c2.rows()); + EXPECT_LONGS_EQUAL(11, A_c2.cols()); + EXPECT_LONGS_EQUAL(4, A_p1.rows()); + EXPECT_LONGS_EQUAL(3, A_p1.cols()); + + // Check zeros + // A_c1 (c1) should be zero in rows 2-3 (Factor 2) + EXPECT(assert_equal(Matrix::Zero(2, 11), Matrix(A_c1.block(2, 0, 2, 11)))); + // A_c2 (c2) should be zero in rows 0-1 (Factor 1) + EXPECT(assert_equal(Matrix::Zero(2, 11), Matrix(A_c2.block(0, 0, 2, 11)))); +} + +/* ************************************************************************* */ +TEST(BatchFactor, JacobianVsHessian) { + std::vector factors; + Key poseKey = Symbol('x', 0); + Key l1 = Symbol('l', 1); + Key l2 = Symbol('l', 2); + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); + factors.emplace_back(Point2(0, 0), noise, poseKey, l1, sharedK); + factors.emplace_back(Point2(1, 1), noise, poseKey, l2, sharedK); + BatchFactor batch(factors); + + Values values; + values.insert(poseKey, Pose3()); + values.insert(l1, Point3(0, 0, 10)); + values.insert(l2, Point3(0, 0, 10)); + + auto jacobian = + std::dynamic_pointer_cast(batch.linearize(values)); + CHECK(jacobian); + + batch.setUseHessianFactor(true); + auto hessian = + std::dynamic_pointer_cast(batch.linearize(values)); + CHECK(hessian); + + const size_t n = jacobian->size(); + const Vector& b = jacobian->getb(); + double expectedF = b.squaredNorm(); + EXPECT_DOUBLES_EQUAL(expectedF, hessian->constantTerm(), 1e-9); + + for (size_t i = 0; i < n; ++i) { + auto itI = jacobian->begin() + i; + const Matrix& Ai = jacobian->getA(itI); + Matrix expectedDiag = Ai.transpose() * Ai; + EXPECT(assert_equal(expectedDiag, hessian->info().block(i, i), 1e-9)); + + Vector expectedGi = Ai.transpose() * b; + Matrix actualGiMat = hessian->linearTerm(hessian->begin() + i); + EXPECT(assert_equal(expectedGi, actualGiMat.col(0), 1e-9)); + + for (size_t j = i + 1; j < n; ++j) { + auto itJ = jacobian->begin() + j; + const Matrix& Aj = jacobian->getA(itJ); + Matrix expectedOff = Ai.transpose() * Aj; + EXPECT(assert_equal(expectedOff, hessian->info().block(i, j), 1e-9)); + } + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 06c832eb40..182d4cbe7e 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -16,37 +16,37 @@ * * @date Dec 15, 2010 * @author Kai Ni + * @author Frank Dellaert */ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include #include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include #if GTSAM_ENABLE_BOOST_SERIALIZATION #include -#endif -#include -#include - namespace boost { namespace serialization { class access; } /* namespace serialization */ } /* namespace boost */ +#endif +#include +#include namespace gtsam { @@ -55,9 +55,8 @@ namespace gtsam { * The calibration is unknown here compared to GenericProjectionFactor * @ingroup slam */ -template -class GeneralSFMFactor: public NoiseModelFactorN { - +template +class GeneralSFMFactor : public NoiseModelFactorN { GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) @@ -66,14 +65,13 @@ class GeneralSFMFactor: public NoiseModelFactorN { typedef Eigen::Matrix JacobianC; typedef Eigen::Matrix JacobianL; -protected: - - Point2 measured_; ///< the 2D measurement + protected: + Point2 measured_; ///< the 2D measurement -public: - - typedef GeneralSFMFactor This;///< typedef for this object - typedef NoiseModelFactorN Base;///< typedef for the base class + public: + typedef GeneralSFMFactor This; ///< typedef for this object + typedef NoiseModelFactorN + Base; ///< typedef for the base class // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -83,7 +81,7 @@ class GeneralSFMFactor: public NoiseModelFactorN { /** * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) + * @param measured is the 2 dimensional measurement of point in image * @param model is the standard deviation of the measurements * @param cameraKey is the index of the camera * @param landmarkKey is the index of the landmark @@ -98,19 +96,22 @@ class GeneralSFMFactor: public NoiseModelFactorN { ///< constructor that takes doubles x,y to make a Point2 GeneralSFMFactor(double x, double y) : measured_(x, y) {} - ~GeneralSFMFactor() override {} ///< destructor + ~GeneralSFMFactor() override {} ///< destructor /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this)));} + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter for printing out Symbols */ - void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + void print( + const std::string& s = "SFMFactor", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } @@ -118,27 +119,44 @@ class GeneralSFMFactor: public NoiseModelFactorN { /** * equals */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const override { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e && Base::equals(p, tol) && + traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, - OptionalMatrixType H1, OptionalMatrixType H2) const override { + OptionalMatrixType H1, + OptionalMatrixType H2) const override { try { - return camera.project2(point,H1,H2) - measured_; - } - catch( CheiralityException& e [[maybe_unused]]) { + return camera.project2(point, H1, H2) - measured_; + } catch (CheiralityException& e [[maybe_unused]]) { if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); - //TODO Print the exception via logging + // TODO Print the exception via logging + return Z_2x1; + } + } + + /** + * Error function with Eigen::Ref for zero-malloc linearization. + * Writes Jacobians directly into the provided matrix blocks. + */ + Vector2 evaluateError(const CAMERA& camera, const LANDMARK& point, + Eigen::Ref H1, Eigen::Ref H2) const { + try { + return camera.project2(point, H1, H2) - measured_; + } catch (CheiralityException& e [[maybe_unused]]) { + H1.setZero(); + H2.setZero(); return Z_2x1; } } /// Linearize using fixed-size matrices - std::shared_ptr linearize(const Values& values) const override { + std::shared_ptr linearize( + const Values& values) const override { // Only linearize if the factor is active if (!this->active(values)) return std::shared_ptr(); @@ -154,7 +172,7 @@ class GeneralSFMFactor: public NoiseModelFactorN { H1.setZero(); H2.setZero(); b.setZero(); - //TODO Print the exception via logging + // TODO Print the exception via logging } // Whiten the system if needed @@ -170,84 +188,86 @@ class GeneralSFMFactor: public NoiseModelFactorN { // Create new (unit) noiseModel, preserving constraints if applicable SharedDiagonal model; if (noiseModel && noiseModel->isConstrained()) { - model = std::static_pointer_cast(noiseModel)->unit(); + model = + std::static_pointer_cast(noiseModel)->unit(); } - return std::make_shared >(key1, H1, key2, H2, b, model); + return std::make_shared >( + key1, H1, key2, H2, b, model); } /** return the measured */ - inline const Point2 measured() const { - return measured_; - } + inline const Point2 measured() const { return measured_; } -private: + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { + template + void serialize(Archive& ar, const unsigned int /*version*/) { // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); + ar& boost::serialization::make_nvp( + "NoiseModelFactor2", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(measured_); } #endif }; -template -struct traits > : Testable< - GeneralSFMFactor > { -}; +template +struct traits > + : Testable > {}; /** * Non-linear factor for a constraint derived from a 2D measurement. - * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration + * is isolated from camera.. */ -template -class GeneralSFMFactor2: public NoiseModelFactorN { - +template +class GeneralSFMFactor2 : public NoiseModelFactorN { GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) static const int DimK = FixedDimension::value; -protected: - - Point2 measured_; ///< the 2D measurement - -public: + protected: + Point2 measured_; ///< the 2D measurement + public: typedef GeneralSFMFactor2 This; - typedef PinholeCamera Camera;///< typedef for camera type - typedef NoiseModelFactorN Base;///< typedef for the base class + typedef PinholeCamera Camera; ///< typedef for camera type + typedef NoiseModelFactorN + Base; ///< typedef for the base class // shorthand for a smart pointer to a factor typedef std::shared_ptr shared_ptr; /** * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) + * @param measured is the 2 dimensional measurement of point in image * @param model is the standard deviation of the measurements * @param poseKey is the index of the camera * @param landmarkKey is the index of the landmark * @param calibKey is the index of the calibration */ - GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : - Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} - GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor + GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, + Key poseKey, Key landmarkKey, Key calibKey) + : Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2() : measured_(0.0, 0.0) {} ///< default constructor - ~GeneralSFMFactor2() override {} ///< destructor + ~GeneralSFMFactor2() override {} ///< destructor /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this)));} + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + void print( + const std::string& s = "SFMFactor2", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); traits::Print(measured_, s + ".z"); } @@ -255,50 +275,50 @@ class GeneralSFMFactor2: public NoiseModelFactorN { /** * equals */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const override { + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e && Base::equals(p, tol) && + traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ - Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, - OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override { + Vector evaluateError(const Pose3& pose3, const Point3& point, + const CALIBRATION& calib, OptionalMatrixType H1, + OptionalMatrixType H2, + OptionalMatrixType H3) const override { try { - Camera camera(pose3,calib); + Camera camera(pose3, calib); return camera.project(point, H1, H2, H3) - measured_; - } - catch( CheiralityException& e) { + } catch (CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2, 6); if (H2) *H2 = Matrix::Zero(2, 3); if (H3) *H3 = Matrix::Zero(2, DimK); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->key2()) << " behind Camera " + << DefaultKeyFormatter(this->key1()) << std::endl; } return Z_2x1; } /** return the measured */ - inline const Point2 measured() const { - return measured_; - } + inline const Point2 measured() const { return measured_; } -private: + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { + template + void serialize(Archive& ar, const unsigned int /*version*/) { // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility - ar & boost::serialization::make_nvp("NoiseModelFactor3", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); + ar& boost::serialization::make_nvp( + "NoiseModelFactor3", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(measured_); } #endif }; -template -struct traits > : Testable< - GeneralSFMFactor2 > { -}; +template +struct traits > + : Testable > {}; -} //namespace +} // namespace gtsam diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index b73b07c96f..6c54da6fb3 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -49,15 +49,23 @@ int main(int argc, char* argv[]) { for (const SfmTrack& track : db.tracks) initial.insert(P(j++), track.p); { + tictoc_reset_(); gttic_(regular); cout << "Optimizing Regular Graph..." << endl; optimize(db, graph, initial); } + { + tictoc_reset_(); + gttic_(regular_metis); + cout << "Optimizing Regular Graph (METIS ordering)..." << endl; + optimize(db, graph, initial, false, true); + } // 2. Build graph using BatchFactor // We batch by Point (Track). Each batch contains measurements from multiple // cameras for one point. NonlinearFactorGraph graphBatch; + NonlinearFactorGraph graphBatchHessian; for (size_t j = 0; j < db.numberTracks(); j++) { std::map measurements; for (const SfmMeasurement& m : db.tracks[j].measurements) { @@ -66,24 +74,38 @@ int main(int argc, char* argv[]) { // BatchFactor(Vector) std::vector> factors; + std::vector> hessianFactors; for (const auto& [key, z] : measurements) { // Correct order: (Camera, Point) factors.emplace_back(z, gNoiseModel, key, P(j)); + hessianFactors.emplace_back(z, gNoiseModel, key, P(j)); } auto batch = std::make_shared>(std::move(factors)); + auto batchHessian = + std::make_shared>(std::move(hessianFactors)); + batchHessian->setUseHessianFactor(true); // GTSAM_PRINT(*batch); // auto linearized = batch->linearize(initial); // cout << "Linearized batch factor " << j << endl; // GTSAM_PRINT(*linearized); graphBatch.add(batch); + graphBatchHessian.add(batchHessian); } { + tictoc_reset_(); gttic_(batch); cout << "Optimizing Batch Graph..." << endl; optimize(db, graphBatch, initial); } + + { + tictoc_reset_(); + gttic_(batch_hessian); + cout << "Optimizing Batch (Hessian) Graph..." << endl; + optimize(db, graphBatchHessian, initial); + } return 0; } diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 7af7988875..7727f5277d 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -62,16 +62,19 @@ SfmData preamble(int argc, char* argv[]) { // Create ordering and optimize int optimize(const SfmData& db, const NonlinearFactorGraph& graph, - const Values& initial, bool separateCalibration = false) { + const Values& initial, bool separateCalibration = false, + bool useMetisOrdering = false) { using symbol_shorthand::P; // Set parameters to be similar to ceres LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); -// params.setLinearSolverType("SEQUENTIAL_CHOLESKY"); -// params.setVerbosityLM("SUMMARY"); + params.setLinearSolverType("SEQUENTIAL_CHOLESKY"); + params.setVerbosityLM("SUMMARY"); - if (gUseSchur) { + if (useMetisOrdering) { + params.setOrderingType("METIS"); + } else if (gUseSchur) { // Create Schur-complement ordering Ordering ordering; for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j)); From 637cba8a8553c2fc43af83b32db9f96bbd06944d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 19 Nov 2025 18:27:38 -0500 Subject: [PATCH 10/10] No EXPORT --- gtsam/nonlinear/BatchFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h index 860ad98832..6cd5e078ac 100644 --- a/gtsam/nonlinear/BatchFactor.h +++ b/gtsam/nonlinear/BatchFactor.h @@ -111,7 +111,7 @@ static FactorType createFactor(K1 k1, K2 k2, const Meas& z, const Model& model, * @tparam ErrorDim The dimension of the error vector for a single factor */ template -class GTSAM_EXPORT BatchFactor : public NonlinearFactor { +class BatchFactor : public NonlinearFactor { public: // Static assertion to ensure FactorType derives from NoiseModelFactor static_assert(std::is_base_of::value,