CUDA GNC: run GncOptimizer with the CUDA SFM LM inner solver, ~20x over CPU GNC

.gitignore

@@ -35,6 +35,7 @@ conductor/
 .ccache/
 gtsam_unstable/timing/data/
 timing/results/bayes_tree_covariance/
+timing/**/benchmark_logs/
 
 # Local development docs/artifacts
 docs/*

gtsam/base/cuda/CudaDeviceArray.h

@@ -4,13 +4,43 @@
 
 #include <cuda_runtime_api.h>
 
+#include <chrono>
 #include <cstddef>
 #include <type_traits>
 #include <utility>
 #include <vector>
 
 namespace gtsam::cuda {
 
+struct CudaDeviceTransferTiming {
+  size_t bytes = 0;
+  double resizeElapsed = 0.0;
+  double copyElapsed = 0.0;
+};
+
+struct CudaDeviceTransferSummary {
+  size_t bytes = 0;
+  double resizeElapsed = 0.0;
+  double copyElapsed = 0.0;
+
+  void add(const CudaDeviceTransferTiming& timing) {
+    bytes += timing.bytes;
+    resizeElapsed += timing.resizeElapsed;
+    copyElapsed += timing.copyElapsed;
+  }
+};
+
+namespace internal {
+
+inline double CudaTransferElapsedSince(
+    std::chrono::steady_clock::time_point start) {
+  return std::chrono::duration<double>(std::chrono::steady_clock::now() -
+                                       start)
+      .count();
+}
+
+}  // namespace internal
+
 template <typename T>
 class CudaDeviceArray {
   static_assert(std::is_trivially_copyable_v<T>,
@@ -61,6 +91,24 @@ class CudaDeviceArray {
                                      cudaMemcpyHostToDevice, stream));
   }
 
+  CudaDeviceTransferTiming uploadProfiled(
+      const std::vector<T>& host, cudaStream_t stream = nullptr) {
+    CudaDeviceTransferTiming timing;
+    timing.bytes = sizeof(T) * host.size();
+
+    const auto resizeStart = std::chrono::steady_clock::now();
+    resize(host.size());
+    timing.resizeElapsed = internal::CudaTransferElapsedSince(resizeStart);
+
+    if (host.empty()) return timing;
+    const auto copyStart = std::chrono::steady_clock::now();
+    GTSAM_CUDA_CHECK(cudaMemcpyAsync(data_, host.data(), sizeof(T) * size_,
+                                     cudaMemcpyHostToDevice, stream));
+    GTSAM_CUDA_CHECK(cudaStreamSynchronize(stream));
+    timing.copyElapsed = internal::CudaTransferElapsedSince(copyStart);
+    return timing;
+  }
+
   // Fills the allocation with bitwise zero.
   void zero(cudaStream_t stream = nullptr) {
     if (size_ == 0) return;
@@ -83,6 +131,24 @@ class CudaDeviceArray {
                                      cudaMemcpyDeviceToHost, stream));
   }
 
+  CudaDeviceTransferTiming downloadProfiled(
+      std::vector<T>* host, cudaStream_t stream = nullptr) const {
+    CudaDeviceTransferTiming timing;
+    timing.bytes = sizeof(T) * size_;
+
+    const auto resizeStart = std::chrono::steady_clock::now();
+    host->resize(size_);
+    timing.resizeElapsed = internal::CudaTransferElapsedSince(resizeStart);
+
+    if (size_ == 0) return timing;
+    const auto copyStart = std::chrono::steady_clock::now();
+    GTSAM_CUDA_CHECK(cudaMemcpyAsync(host->data(), data_, sizeof(T) * size_,
+                                     cudaMemcpyDeviceToHost, stream));
+    GTSAM_CUDA_CHECK(cudaStreamSynchronize(stream));
+    timing.copyElapsed = internal::CudaTransferElapsedSince(copyStart);
+    return timing;
+  }
+
   void reset() {
     if (data_) {
       GTSAM_CUDA_CHECK(cudaFree(data_));

gtsam/nonlinear/BatchFactor.h

@@ -121,10 +121,12 @@ class BatchFactor : public NonlinearFactor {
   using Base = NonlinearFactor;
   using This = BatchFactor<FactorType, ErrorDim>;
   using shared_ptr = std::shared_ptr<This>;
+  using FactorVector =
+      std::vector<FactorType, Eigen::aligned_allocator<FactorType>>;
 
  private:
-  using Allocator = Eigen::aligned_allocator<FactorType>;
-  std::vector<FactorType, Allocator> factors_;  ///< Contiguous storage
+  using Allocator = typename FactorVector::allocator_type;
+  FactorVector factors_;  ///< Contiguous storage
   struct KeyInfo {
     Key key;
     int dim;
@@ -283,6 +285,12 @@ class BatchFactor : public NonlinearFactor {
   /** Default constructor */
   BatchFactor() = default;
 
+  /// Return the child factors represented by this batch.
+  const FactorVector& factors() const { return factors_; }
+
+  /// Return the number of child factors represented by this batch.
+  size_t numFactors() const { return factors_.size(); }
+
   /** Constructor from a vector of factors (moves the vector) */
   explicit BatchFactor(std::vector<FactorType, Allocator>&& factors)
       : factors_(std::move(factors)) {

gtsam/nonlinear/GncOptimizer.h

@@ -27,6 +27,7 @@
 #pragma once
 
 #include <algorithm>
+#include <chrono>
 
 #include <gtsam/linear/LossFunctions.h>
 #include <gtsam/nonlinear/GncParams.h>
@@ -59,6 +60,43 @@ GTSAM_EXPORT bool needsWeightUpdate(GncFactorType type);
 
 GTSAM_EXPORT bool hasNoise(GncFactorType type);
 
+/// Timing of one GNC outer iteration (all in seconds).
+struct GncIterationTiming {
+  double weightsUpdateElapsed = 0.0;   ///< calculateWeights (per-factor errors)
+  double makeGraphElapsed = 0.0;       ///< makeWeightedGraph (factor cloning)
+  double baseOptimizeElapsed = 0.0;    ///< inner optimizer construction + optimize()
+  double costEvaluationElapsed = 0.0;  ///< weighted graph error for convergence check
+  double totalElapsed = 0.0;
+};
+
+/// Timing of a full GncOptimizer::optimize() call.
+struct GncTiming {
+  double initialOptimizeElapsed = 0.0;  ///< optimize before the GNC loop
+  double totalElapsed = 0.0;
+  std::vector<GncIterationTiming> iterations;
+
+  double sumWeightsUpdate() const {
+    double sum = 0.0;
+    for (const auto& it : iterations) sum += it.weightsUpdateElapsed;
+    return sum;
+  }
+  double sumMakeGraph() const {
+    double sum = 0.0;
+    for (const auto& it : iterations) sum += it.makeGraphElapsed;
+    return sum;
+  }
+  double sumBaseOptimize() const {
+    double sum = 0.0;
+    for (const auto& it : iterations) sum += it.baseOptimizeElapsed;
+    return sum;
+  }
+  double sumCostEvaluation() const {
+    double sum = 0.0;
+    for (const auto& it : iterations) sum += it.costEvaluationElapsed;
+    return sum;
+  }
+};
+
 /* ************************************************************************* */
 template<class GncParameters>
 class GncOptimizer {
@@ -86,6 +124,9 @@ class GncOptimizer {
   /// Cached factor types for GNC.
   std::vector<GncFactorType> factorTypes_;
 
+  /// Timing of the last optimize() call.
+  GncTiming timing_;
+
  public:
   /// Constructor.
   GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
@@ -208,6 +249,9 @@ class GncOptimizer {
   /// Get the inlier threshold.
   const Vector& getInlierCostThresholds() const {return barcSq_;}
 
+  /// Get the timing of the last optimize() call.
+  const GncTiming& getTiming() const { return timing_; }
+
   /// Equals.
   bool equals(const GncOptimizer& other, double tol = 1e-9) const {
     return nfg_.equals(other.getFactors())
@@ -227,11 +271,19 @@ class GncOptimizer {
 
   /// Compute optimal solution using graduated non-convexity.
   Values optimize() {
+    using Clock = std::chrono::steady_clock;
+    const auto elapsedSince = [](Clock::time_point start) {
+      return std::chrono::duration<double>(Clock::now() - start).count();
+    };
+    timing_ = GncTiming();
+    const auto totalStart = Clock::now();
+
     validateLossSchedulerCombination();
     NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
     BaseOptimizer baseOptimizer(
         graph_initial, state_, params_.baseOptimizerParams);
     Values result = baseOptimizer.optimize();
+    timing_.initialOptimizeElapsed = elapsedSince(totalStart);
     double mu = initializeMu();
     double prev_cost = graph_initial.error(result);
     double cost = 0.0;  // this will be updated in the main loop
@@ -263,11 +315,14 @@ class GncOptimizer {
       if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
         result.print("result\n");
       }
+      timing_.totalElapsed = elapsedSince(totalStart);
       return result;
     }
 
     size_t iter;
     for (iter = 0; iter < params_.maxIterations; iter++) {
+      const auto iterationStart = Clock::now();
+      GncIterationTiming iterationTiming;
 
       // display info
       if (params_.verbosity >= GncParameters::Verbosity::MU) {
@@ -281,16 +336,26 @@ class GncOptimizer {
         result.print("result\n");
       }
       // weights update
+      auto stageStart = Clock::now();
       weights_ = calculateWeights(result, mu);
+      iterationTiming.weightsUpdateElapsed = elapsedSince(stageStart);
 
       // variable/values update
+      stageStart = Clock::now();
       NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
+      iterationTiming.makeGraphElapsed = elapsedSince(stageStart);
+      stageStart = Clock::now();
       BaseOptimizer baseOptimizer_iter(
           graph_iter, state_, params_.baseOptimizerParams);
       result = baseOptimizer_iter.optimize();
+      iterationTiming.baseOptimizeElapsed = elapsedSince(stageStart);
 
       // stopping condition
+      stageStart = Clock::now();
       cost = graph_iter.error(result);
+      iterationTiming.costEvaluationElapsed = elapsedSince(stageStart);
+      iterationTiming.totalElapsed = elapsedSince(iterationStart);
+      timing_.iterations.push_back(iterationTiming);
       if (checkConvergence(mu, weights_, cost, prev_cost)) {
         break;
       }
@@ -317,6 +382,7 @@ class GncOptimizer {
     if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
       std::cout << "final weights: " << weights_ << std::endl;
     }
+    timing_.totalElapsed = elapsedSince(totalStart);
     return result;
   }
 

gtsam/nonlinear/cuda/DeviceSparseNormalEquations.h

@@ -14,7 +14,8 @@ class DeviceSparseNormalEquations {
  public:
   void uploadPattern(int rows, const std::vector<int>& rowPointers,
                      const std::vector<int>& colIndices,
-                     cudaStream_t stream = nullptr) {
+                     cudaStream_t stream = nullptr,
+                     CudaDeviceTransferSummary* transferProfile = nullptr) {
     if (rows < 0) {
       throw std::invalid_argument("DeviceSparseNormalEquations rows < 0");
     }
@@ -55,8 +56,13 @@ class DeviceSparseNormalEquations {
     CudaDeviceArray<double> newValues;
     CudaDeviceArray<double> newRhs;
 
-    newRowPointers.upload(rowPointers, stream);
-    newColIndices.upload(colIndices, stream);
+    if (transferProfile) {
+      transferProfile->add(newRowPointers.uploadProfiled(rowPointers, stream));
+      transferProfile->add(newColIndices.uploadProfiled(colIndices, stream));
+    } else {
+      newRowPointers.upload(rowPointers, stream);
+      newColIndices.upload(colIndices, stream);
+    }
     newValues.resize(colIndices.size());
     newRhs.resize(rows);
 

gtsam/nonlinear/cuda/DeviceValues.h

@@ -3,6 +3,7 @@
 #include <gtsam/base/cuda/CudaDeviceArray.h>
 #include <gtsam/nonlinear/cuda/DeviceVariableIndex.h>
 
+#include <chrono>
 #include <cstddef>
 #include <cstdint>
 #include <limits>
@@ -31,7 +32,10 @@ class DeviceValues {
   DeviceValueBlock<T>& addBlock(uint32_t typeId, int tangentDim,
                                 const std::vector<Key>& keys,
                                 const std::vector<T>& hostValues,
-                                cudaStream_t stream = nullptr) {
+                                cudaStream_t stream = nullptr,
+                                CudaDeviceTransferTiming* valuesUploadTiming =
+                                    nullptr,
+                                double* deltaResizeElapsed = nullptr) {
     if (keys.size() != hostValues.size()) {
       throw std::invalid_argument("DeviceValues keys and values size mismatch");
     }
@@ -59,9 +63,21 @@ class DeviceValues {
     auto storage = std::make_unique<TypedBlock<T>>();
     storage->block.tangentDim = tangentDim;
     storage->block.keys = keys;
-    storage->block.values.upload(hostValues, stream);
+    if (valuesUploadTiming) {
+      *valuesUploadTiming =
+          storage->block.values.uploadProfiled(hostValues, stream);
+    } else {
+      storage->block.values.upload(hostValues, stream);
+    }
+    const auto deltaResizeStart = std::chrono::steady_clock::now();
     storage->block.delta.resize(hostValues.size() *
                                 static_cast<size_t>(tangentDim));
+    if (deltaResizeElapsed) {
+      *deltaResizeElapsed =
+          std::chrono::duration<double>(std::chrono::steady_clock::now() -
+                                        deltaResizeStart)
+              .count();
+    }
 
     DeviceValueBlock<T>* result = &storage->block;
     std::vector<Key> committedKeys;