From 293c122723a867927adbec345998f256ada4bb1c Mon Sep 17 00:00:00 2001 From: leolrg <13660505036@163.com> Date: Mon, 6 Jul 2026 06:42:52 +0000 Subject: [PATCH 1/3] added more timing artifacts --- .gitignore | 1 + gtsam/base/cuda/CudaDeviceArray.h | 66 +++ gtsam/nonlinear/BatchFactor.h | 12 +- .../cuda/DeviceSparseNormalEquations.h | 12 +- gtsam/nonlinear/cuda/DeviceValues.h | 20 +- gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu | 385 ++++++++++++--- gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h | 78 +++ gtsam/slam/cuda/CudaSfmProjectionBatch.h | 66 ++- gtsam/slam/cuda/CudaSfmValues.h | 87 +++- gtsam/slam/cuda/cuda_sfm.i | 19 + gtsam/slam/tests/testCudaSfm.cpp | 248 ++++++++++ timing/sfm_ba/timeCudaSFMBAL.cpp | 450 ++++++++++++++++-- timing/timeSFMBAL.cpp | 2 - 13 files changed, 1298 insertions(+), 148 deletions(-) diff --git a/.gitignore b/.gitignore index 93b105f095..bc32d983ba 100644 --- a/.gitignore +++ b/.gitignore @@ -35,6 +35,7 @@ conductor/ .ccache/ gtsam_unstable/timing/data/ timing/results/bayes_tree_covariance/ +timing/**/benchmark_logs/ # Local development docs/artifacts docs/* diff --git a/gtsam/base/cuda/CudaDeviceArray.h b/gtsam/base/cuda/CudaDeviceArray.h index 2e178245f5..c4c13c4e83 100644 --- a/gtsam/base/cuda/CudaDeviceArray.h +++ b/gtsam/base/cuda/CudaDeviceArray.h @@ -4,6 +4,7 @@ #include +#include #include #include #include @@ -11,6 +12,35 @@ 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(std::chrono::steady_clock::now() - + start) + .count(); +} + +} // namespace internal + template class CudaDeviceArray { static_assert(std::is_trivially_copyable_v, @@ -61,6 +91,24 @@ class CudaDeviceArray { cudaMemcpyHostToDevice, stream)); } + CudaDeviceTransferTiming uploadProfiled( + const std::vector& 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* 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_)); diff --git a/gtsam/nonlinear/BatchFactor.h b/gtsam/nonlinear/BatchFactor.h index ad26698237..d0271931be 100644 --- a/gtsam/nonlinear/BatchFactor.h +++ b/gtsam/nonlinear/BatchFactor.h @@ -121,10 +121,12 @@ class BatchFactor : public NonlinearFactor { using Base = NonlinearFactor; using This = BatchFactor; using shared_ptr = std::shared_ptr; + using FactorVector = + std::vector>; private: - using Allocator = Eigen::aligned_allocator; - std::vector 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&& factors) : factors_(std::move(factors)) { diff --git a/gtsam/nonlinear/cuda/DeviceSparseNormalEquations.h b/gtsam/nonlinear/cuda/DeviceSparseNormalEquations.h index 5de418adff..d7d8fed632 100644 --- a/gtsam/nonlinear/cuda/DeviceSparseNormalEquations.h +++ b/gtsam/nonlinear/cuda/DeviceSparseNormalEquations.h @@ -14,7 +14,8 @@ class DeviceSparseNormalEquations { public: void uploadPattern(int rows, const std::vector& rowPointers, const std::vector& 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 newValues; CudaDeviceArray 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); diff --git a/gtsam/nonlinear/cuda/DeviceValues.h b/gtsam/nonlinear/cuda/DeviceValues.h index 9ef38acee4..71b6a3f4d4 100644 --- a/gtsam/nonlinear/cuda/DeviceValues.h +++ b/gtsam/nonlinear/cuda/DeviceValues.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -31,7 +32,10 @@ class DeviceValues { DeviceValueBlock& addBlock(uint32_t typeId, int tangentDim, const std::vector& keys, const std::vector& 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>(); 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(tangentDim)); + if (deltaResizeElapsed) { + *deltaResizeElapsed = + std::chrono::duration(std::chrono::steady_clock::now() - + deltaResizeStart) + .count(); + } DeviceValueBlock* result = &storage->block; std::vector committedKeys; diff --git a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu index a037072570..21819a63ad 100644 --- a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu +++ b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,7 @@ namespace { constexpr int kApplyDeltaBlockSize = 256; using Clock = std::chrono::steady_clock; using BundlerProjectionFactor = GeneralSFMFactor; +using BundlerProjectionBatchFactor = BatchFactor; struct CudaSfmLmExecutionOptions { bool downloadOptimizedValues = true; @@ -46,6 +48,23 @@ double ElapsedSinceAfterSync(Clock::time_point start, cudaStream_t stream) { return ElapsedSince(start); } +void AddH2dTransfer(const CudaDeviceTransferSummary& transfer, + CudaSfmLevenbergMarquardtResult* result) { + result->totalH2dBytes += transfer.bytes; + result->totalH2dCopyElapsed += transfer.copyElapsed; +} + +void SetDownloadTransferProfile( + const CudaSfmValuesDownloadProfile& profile, + CudaSfmLevenbergMarquardtResult* result) { + result->downloadHostAllocElapsed = profile.hostAllocElapsed; + result->downloadD2hCopyElapsed = profile.d2h.copyElapsed; + result->downloadValuesBuildElapsed = profile.hostBuildElapsed; + result->downloadD2hBytes = profile.d2h.bytes; + result->totalD2hCopyElapsed = profile.d2h.copyElapsed; + result->totalD2hBytes = profile.d2h.bytes; +} + __global__ void ApplyDeltaKernel( const DevicePinholeCameraCal3Bundler* cameras, const DevicePoint3* points, int numCameras, int numPoints, @@ -240,6 +259,125 @@ CudaSfmRobustModel ExtractProjectionRobustModel( "noise models"); } +void EnsureRobustModelStorage(CudaSfmFactorGraphData* converted) { + if (converted->hasRobustNoise) { + return; + } + converted->hasRobustNoise = true; + converted->robustModelsByTrack.resize(converted->data.numberTracks()); + for (size_t trackIndex = 0; trackIndex < converted->data.numberTracks(); + ++trackIndex) { + converted->robustModelsByTrack[trackIndex].assign( + converted->data.track(trackIndex).numberMeasurements(), + CudaSfmProjectionBatch::NoRobustModel()); + } +} + +void AppendProjectionFactorWithSlotsToCudaSfmData( + const BundlerProjectionFactor& sfmFactor, size_t cameraSlot, + size_t pointSlot, CudaSfmFactorGraphData* converted) { + SharedNoiseModel model = sfmFactor.noiseModel(); + CudaSfmRobustModel robustModel = + CudaSfmProjectionBatch::NoRobustModel(); + if (model && !model->isUnit()) { + if (const auto robust = + std::dynamic_pointer_cast(model)) { + EnsureRobustModelStorage(converted); + robustModel = ExtractProjectionRobustModel(robust->robust()); + model = robust->noise(); + } + } + bool factorHasNonUnitNoise = false; + const CudaSfmSqrtInfo2 sqrtInfo = + ExtractProjectionSqrtInfo(model, &factorHasNonUnitNoise); + converted->hasNonUnitNoise = + converted->hasNonUnitNoise || factorHasNonUnitNoise; + + converted->data.tracks[pointSlot].measurements.emplace_back( + cameraSlot, sfmFactor.measured()); + converted->sqrtInfoByTrack[pointSlot].push_back(sqrtInfo); + if (converted->hasRobustNoise) { + converted->robustModelsByTrack[pointSlot].push_back(robustModel); + } +} + +size_t FindRequiredSlot(const std::unordered_map& slots, Key key, + const char* description) { + const auto slot = slots.find(key); + if (slot == slots.end()) { + throw std::invalid_argument(description); + } + return slot->second; +} + +void AppendProjectionFactorToCudaSfmData( + const BundlerProjectionFactor& sfmFactor, + const std::unordered_map& cameraSlots, + const std::unordered_map& pointSlots, + CudaSfmFactorGraphData* converted) { + const size_t cameraSlot = FindRequiredSlot( + cameraSlots, sfmFactor.key1(), + "CUDA SFM conversion found a factor camera key missing from Values"); + const size_t pointSlot = FindRequiredSlot( + pointSlots, sfmFactor.key2(), + "CUDA SFM conversion found a factor point key missing from Values"); + AppendProjectionFactorWithSlotsToCudaSfmData(sfmFactor, cameraSlot, pointSlot, + converted); +} + +void AppendProjectionBatchFactorToCudaSfmData( + const BundlerProjectionBatchFactor& batchFactor, + const std::unordered_map& cameraSlots, + const std::unordered_map& pointSlots, + CudaSfmFactorGraphData* converted) { + const auto& factors = batchFactor.factors(); + if (factors.empty()) { + return; + } + + const Key firstCameraKey = factors.front().key1(); + const Key firstPointKey = factors.front().key2(); + bool fixedCamera = true; + bool fixedPoint = true; + for (const BundlerProjectionFactor& sfmFactor : factors) { + fixedCamera = fixedCamera && sfmFactor.key1() == firstCameraKey; + fixedPoint = fixedPoint && sfmFactor.key2() == firstPointKey; + } + + if (fixedPoint) { + const size_t pointSlot = FindRequiredSlot( + pointSlots, firstPointKey, + "CUDA SFM conversion found a batch factor point key missing from Values"); + for (const BundlerProjectionFactor& sfmFactor : factors) { + const size_t cameraSlot = FindRequiredSlot( + cameraSlots, sfmFactor.key1(), + "CUDA SFM conversion found a factor camera key missing from Values"); + AppendProjectionFactorWithSlotsToCudaSfmData( + sfmFactor, cameraSlot, pointSlot, converted); + } + return; + } + + if (fixedCamera) { + const size_t cameraSlot = FindRequiredSlot( + cameraSlots, firstCameraKey, + "CUDA SFM conversion found a batch factor camera key missing from Values"); + for (const BundlerProjectionFactor& sfmFactor : factors) { + const size_t pointSlot = FindRequiredSlot( + pointSlots, sfmFactor.key2(), + "CUDA SFM conversion found a factor point key missing from Values"); + AppendProjectionFactorWithSlotsToCudaSfmData( + sfmFactor, cameraSlot, pointSlot, converted); + } + return; + } + + for (const BundlerProjectionFactor& sfmFactor : factors) { + AppendProjectionFactorToCudaSfmData(sfmFactor, cameraSlots, pointSlots, + converted); + } +} + void IncreaseLambda(const CudaSfmLevenbergMarquardtParams& params, double* lambda, double* currentFactor) { *lambda *= *currentFactor; @@ -394,58 +532,23 @@ CudaSfmFactorGraphData ConvertGeneralSfmGraphToCudaSfmData( continue; } - const auto sfmFactor = - std::dynamic_pointer_cast(factor); - if (!sfmFactor) { - throw std::invalid_argument( - "CUDA SFM conversion only supports GeneralSFMFactor"); - } - - SharedNoiseModel model = sfmFactor->noiseModel(); - CudaSfmRobustModel robustModel = - CudaSfmProjectionBatch::NoRobustModel(); - if (model && !model->isUnit()) { - if (const auto robust = - std::dynamic_pointer_cast(model)) { - if (!converted.hasRobustNoise) { - converted.hasRobustNoise = true; - converted.robustModelsByTrack.resize(converted.data.numberTracks()); - for (size_t trackIndex = 0; trackIndex < converted.data.numberTracks(); - ++trackIndex) { - converted.robustModelsByTrack[trackIndex].assign( - converted.data.track(trackIndex).numberMeasurements(), - CudaSfmProjectionBatch::NoRobustModel()); - } - } - robustModel = ExtractProjectionRobustModel(robust->robust()); - model = robust->noise(); - } - } - bool factorHasNonUnitNoise = false; - const CudaSfmSqrtInfo2 sqrtInfo = - ExtractProjectionSqrtInfo(model, &factorHasNonUnitNoise); - converted.hasNonUnitNoise = - converted.hasNonUnitNoise || factorHasNonUnitNoise; - - const auto cameraSlot = cameraSlots.find(sfmFactor->key1()); - if (cameraSlot == cameraSlots.end()) { - throw std::invalid_argument( - "CUDA SFM conversion found a factor camera key missing from Values"); + if (const auto sfmFactor = + std::dynamic_pointer_cast(factor)) { + AppendProjectionFactorToCudaSfmData(*sfmFactor, cameraSlots, pointSlots, + &converted); + continue; } - const auto pointSlot = pointSlots.find(sfmFactor->key2()); - if (pointSlot == pointSlots.end()) { - throw std::invalid_argument( - "CUDA SFM conversion found a factor point key missing from Values"); + if (const auto batchFactor = + std::dynamic_pointer_cast(factor)) { + AppendProjectionBatchFactorToCudaSfmData(*batchFactor, cameraSlots, + pointSlots, &converted); + continue; } - converted.data.tracks[pointSlot->second].measurements.emplace_back( - cameraSlot->second, sfmFactor->measured()); - converted.sqrtInfoByTrack[pointSlot->second].push_back(sqrtInfo); - if (converted.hasRobustNoise) { - converted.robustModelsByTrack[pointSlot->second].push_back(robustModel); - } + throw std::invalid_argument( + "CUDA SFM conversion only supports GeneralSFMFactor or BatchFactor, 2>"); } return converted; @@ -471,23 +574,38 @@ CudaSfmLevenbergMarquardtOptimizer::CudaSfmLevenbergMarquardtOptimizer( baseParams_(BaseParamsAdapter(params)) {} const Values& CudaSfmLevenbergMarquardtOptimizer::optimize() { - const CudaSfmFactorGraphData converted = - ConvertGeneralSfmGraphToCudaSfmData(graph(), values()); - result_ = OptimizeCudaSfmImpl( - converted.data, converted.cameraKeys, converted.pointKeys, params_, - (converted.hasNonUnitNoise || converted.hasRobustNoise) - ? &converted.sqrtInfoByTrack - : nullptr, - converted.hasRobustNoise ? &converted.robustModelsByTrack : nullptr, - CudaSfmLmExecutionOptions{true}); - - Values merged(values()); - for (Key key : result_.optimizedValues.keys()) { - merged.update(key, result_.optimizedValues.at(key)); - } - state_ = std::unique_ptr( - new gtsam::internal::NonlinearOptimizerState( - std::move(merged), result_.finalError, result_.iterations)); + auto stageStart = Clock::now(); + auto convertedDestructionStart = Clock::now(); + { + const CudaSfmFactorGraphData converted = + ConvertGeneralSfmGraphToCudaSfmData(graph(), values()); + const double graphConversionElapsed = ElapsedSince(stageStart); + + stageStart = Clock::now(); + result_ = OptimizeCudaSfmImpl( + converted.data, converted.cameraKeys, converted.pointKeys, params_, + (converted.hasNonUnitNoise || converted.hasRobustNoise) + ? &converted.sqrtInfoByTrack + : nullptr, + converted.hasRobustNoise ? &converted.robustModelsByTrack : nullptr, + CudaSfmLmExecutionOptions{true}); + result_.graphBackendCallElapsed = ElapsedSince(stageStart); + result_.graphConversionElapsed = graphConversionElapsed; + + stageStart = Clock::now(); + Values merged(values()); + for (Key key : result_.optimizedValues.keys()) { + merged.update(key, result_.optimizedValues.at(key)); + } + state_ = std::unique_ptr( + new gtsam::internal::NonlinearOptimizerState( + std::move(merged), result_.finalError, result_.iterations)); + result_.graphValueMergeElapsed = ElapsedSince(stageStart); + + convertedDestructionStart = Clock::now(); + } + result_.graphConvertedDataDestructionElapsed = + ElapsedSince(convertedDestructionStart); return values(); } @@ -563,10 +681,18 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( // SFM data again. Consider constructing the projection batch from existing // device buffers or sharing the packed representation. stageStart = Clock::now(); + CudaSfmValuesPackProfile packValuesProfile; DeviceValues current = - PackSfmValues(data, cameraKeys, pointKeys, context.stream()); + PackSfmValues(data, cameraKeys, pointKeys, context.stream(), + &packValuesProfile); result.packValuesElapsed = ElapsedSinceAfterSync(stageStart, context.stream()); + result.packValuesHostBuildElapsed = packValuesProfile.hostBuildElapsed; + result.packValuesDeviceAllocElapsed = + packValuesProfile.deviceAllocElapsed; + result.packValuesH2dCopyElapsed = packValuesProfile.h2d.copyElapsed; + result.packValuesH2dBytes = packValuesProfile.h2d.bytes; + AddH2dTransfer(packValuesProfile.h2d, &result); stageStart = Clock::now(); DeviceValues trial = AllocateSfmValuesLike(current); @@ -577,17 +703,29 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( throw std::invalid_argument( "OptimizeCudaSfm robust noise requires projection sqrt-info"); } + CudaSfmProjectionBatchTransferProfile projectionBatchProfile; const CudaSfmProjectionBatch batch = robustModelsByTrack ? CudaSfmProjectionBatch::FromSfmData(data, *sqrtInfoByTrack, *robustModelsByTrack, - context.stream()) + context.stream(), + &projectionBatchProfile) : sqrtInfoByTrack ? CudaSfmProjectionBatch::FromSfmData(data, *sqrtInfoByTrack, - context.stream()) - : CudaSfmProjectionBatch::FromSfmData(data, context.stream()); + context.stream(), + &projectionBatchProfile) + : CudaSfmProjectionBatch::FromSfmData( + data, context.stream(), &projectionBatchProfile); result.projectionBatchElapsed = ElapsedSinceAfterSync(stageStart, context.stream()); + result.projectionBatchHostBuildElapsed = + projectionBatchProfile.hostBuildElapsed; + result.projectionBatchDeviceAllocElapsed = + projectionBatchProfile.deviceAllocElapsed; + result.projectionBatchH2dCopyElapsed = + projectionBatchProfile.h2d.copyElapsed; + result.projectionBatchH2dBytes = projectionBatchProfile.h2d.bytes; + AddH2dTransfer(projectionBatchProfile.h2d, &result); const int numCameras = static_cast(data.numberCameras()); const int numPoints = static_cast(data.numberTracks()); @@ -605,8 +743,11 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( result.finalLambda = params.lambdaInitial; if (executionOptions.downloadOptimizedValues) { stageStart = Clock::now(); - result.optimizedValues = DownloadSfmValues(current, context.stream()); + CudaSfmValuesDownloadProfile downloadProfile; + result.optimizedValues = + DownloadSfmValues(current, context.stream(), &downloadProfile); result.downloadElapsed = ElapsedSince(stageStart); + SetDownloadTransferProfile(downloadProfile, &result); } result.totalMeasuredElapsed = ElapsedSince(totalStart); return result; @@ -627,10 +768,17 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( result.csrStructureElapsed = ElapsedSince(stageStart); stageStart = Clock::now(); + CudaDeviceTransferSummary uploadPatternProfile; system.uploadPattern(structure.dimension(), structure.rowPointers(), - structure.colIndices(), context.stream()); + structure.colIndices(), context.stream(), + &uploadPatternProfile); result.uploadPatternElapsed = ElapsedSinceAfterSync(stageStart, context.stream()); + result.uploadPatternDeviceAllocElapsed = + uploadPatternProfile.resizeElapsed; + result.uploadPatternH2dCopyElapsed = uploadPatternProfile.copyElapsed; + result.uploadPatternH2dBytes = uploadPatternProfile.bytes; + AddH2dTransfer(uploadPatternProfile, &result); } double lambda = params.lambdaInitial; @@ -642,15 +790,35 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( bool terminate = false; while (result.iterations < params.maxIterations && std::isfinite(currentError) && !terminate) { + const auto iterationStart = Clock::now(); + CudaSfmLmIterationProfile iterationProfile; + iterationProfile.iteration = + static_cast(result.iterationProfiles.size()); + iterationProfile.startError = currentError; + iterationProfile.startLambda = lambda; + if (params.diagonalDamping) { + stageStart = Clock::now(); ComputeCudaSfmHessianDiagonal(current, batch, numCameras, params.minDiagonal, params.maxDiagonal, &dampingDiagonal, context.stream()); + iterationProfile.dampingDiagonalElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.dampingDiagonalElapsed += + iterationProfile.dampingDiagonalElapsed; } bool acceptedOrDone = false; + int attemptIndex = 0; while (!acceptedOrDone) { + const auto attemptStart = Clock::now(); + CudaSfmLmAttemptProfile attemptProfile; + attemptProfile.iteration = iterationProfile.iteration; + attemptProfile.attempt = attemptIndex++; + attemptProfile.lambda = lambda; + if (params.linearSolver == CudaSfmLinearSolverType::DenseSchur) { + stageStart = Clock::now(); if (params.diagonalDamping) { denseSchurSolver.solve(current, batch, numCameras, lambda, dampingDiagonal, &delta, context.stream()); @@ -658,31 +826,61 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( denseSchurSolver.solve(current, batch, numCameras, lambda, &delta, context.stream()); } + attemptProfile.denseSchurSolveElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.denseSchurSolveElapsed += + attemptProfile.denseSchurSolveElapsed; } else { + stageStart = Clock::now(); AccumulateCudaSfmNormalEquations(current, batch, numCameras, &system, context.stream()); + attemptProfile.normalEquationsElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.normalEquationsElapsed += + attemptProfile.normalEquationsElapsed; + + stageStart = Clock::now(); if (params.diagonalDamping) { system.addDiagonalDamping(lambda, dampingDiagonal, context.stream()); } else { system.addDiagonalDamping(lambda, context.stream()); } + attemptProfile.addDampingElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.addDampingElapsed += attemptProfile.addDampingElapsed; + if (!solverAnalyzed) { stageStart = Clock::now(); solver.analyze(system, &delta, context.stream()); - result.firstCudssAnalyzeElapsed = + attemptProfile.cudssAnalyzeElapsed = ElapsedSinceAfterSync(stageStart, context.stream()); + result.firstCudssAnalyzeElapsed = + attemptProfile.cudssAnalyzeElapsed; + result.cudssAnalyzeElapsed += attemptProfile.cudssAnalyzeElapsed; solverAnalyzed = true; } + stageStart = Clock::now(); solver.solve(system, &delta, context.stream()); + attemptProfile.cudssSolveElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.cudssSolveElapsed += attemptProfile.cudssSolveElapsed; } ++result.innerIterations; double oldLinearizedError = 0.0; double newLinearizedError = 0.0; + stageStart = Clock::now(); const double linearizedCostChange = ComputeCudaSfmLinearizedErrorChange( current, batch, numCameras, delta, &oldLinearizedError, &newLinearizedError, context.stream()); + attemptProfile.linearizedErrorElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.linearizedErrorElapsed += + attemptProfile.linearizedErrorElapsed; + attemptProfile.oldLinearizedError = oldLinearizedError; + attemptProfile.newLinearizedError = newLinearizedError; + attemptProfile.linearizedCostChange = linearizedCostChange; double trialError = std::numeric_limits::infinity(); double costChange = 0.0; @@ -691,10 +889,20 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( bool stopSearchingLambda = false; if (linearizedCostChange >= 0.0) { + attemptProfile.attemptedTrial = true; + stageStart = Clock::now(); ApplyDelta(current, delta, &trial, numCameras, numPoints, context.stream()); + attemptProfile.applyDeltaElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.applyDeltaElapsed += attemptProfile.applyDeltaElapsed; + + stageStart = Clock::now(); trialError = ComputeCudaSfmProjectionError(trial, batch, context.stream()); + attemptProfile.trialErrorElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.trialErrorElapsed += attemptProfile.trialErrorElapsed; costChange = currentError - trialError; if (linearizedCostChange > @@ -709,28 +917,56 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( stopSearchingLambda = true; } } + attemptProfile.trialError = trialError; + attemptProfile.costChange = costChange; + attemptProfile.modelFidelity = modelFidelity; + attemptProfile.stepSuccessful = stepSuccessful; + attemptProfile.stopSearchingLambda = stopSearchingLambda; if (stepSuccessful) { const double previousError = currentError; + stageStart = Clock::now(); AcceptTrial(trial, ¤t, context.stream()); + iterationProfile.acceptTrialElapsed = + ElapsedSinceAfterSync(stageStart, context.stream()); + result.acceptTrialElapsed += iterationProfile.acceptTrialElapsed; currentError = trialError; ++result.iterations; ++result.acceptedSteps; + stageStart = Clock::now(); DecreaseLambda(params, modelFidelity, &lambda, ¤tFactor); acceptedOrDone = true; terminate = CheckCudaLmConvergence(params, previousError, currentError); + attemptProfile.lambdaUpdateElapsed = ElapsedSince(stageStart); + result.lambdaUpdateElapsed += attemptProfile.lambdaUpdateElapsed; + iterationProfile.acceptedStep = true; + attemptProfile.accepted = true; + attemptProfile.terminated = terminate; } else if (!stopSearchingLambda) { + stageStart = Clock::now(); IncreaseLambda(params, &lambda, ¤tFactor); if (lambda >= params.lambdaUpperBound) { acceptedOrDone = true; terminate = true; + attemptProfile.lambdaUpperBoundReached = true; } + attemptProfile.lambdaUpdateElapsed = ElapsedSince(stageStart); + result.lambdaUpdateElapsed += attemptProfile.lambdaUpdateElapsed; + attemptProfile.terminated = terminate; } else { acceptedOrDone = true; terminate = true; + attemptProfile.terminated = true; } + attemptProfile.totalElapsed = ElapsedSince(attemptStart); + iterationProfile.attemptProfiles.push_back(attemptProfile); } + iterationProfile.endError = currentError; + iterationProfile.endLambda = lambda; + iterationProfile.terminated = terminate; + iterationProfile.totalElapsed = ElapsedSince(iterationStart); + result.iterationProfiles.push_back(iterationProfile); } GTSAM_CUDA_CHECK(cudaStreamSynchronize(context.stream())); const auto solveLoopEnd = std::chrono::steady_clock::now(); @@ -741,8 +977,11 @@ CudaSfmLevenbergMarquardtResult OptimizeCudaSfmImpl( result.finalLambda = lambda; if (executionOptions.downloadOptimizedValues) { stageStart = Clock::now(); - result.optimizedValues = DownloadSfmValues(current, context.stream()); + CudaSfmValuesDownloadProfile downloadProfile; + result.optimizedValues = + DownloadSfmValues(current, context.stream(), &downloadProfile); result.downloadElapsed = ElapsedSince(stageStart); + SetDownloadTransferProfile(downloadProfile, &result); } result.totalMeasuredElapsed = ElapsedSince(totalStart); return result; diff --git a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h index 8018a22472..eb3c9650a2 100644 --- a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h +++ b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -48,27 +49,104 @@ class GTSAM_EXPORT CudaSfmLevenbergMarquardtParams { void print(const std::string& str = "") const; }; +struct CudaSfmLmAttemptProfile { + int iteration = 0; + int attempt = 0; + double lambda = 0.0; + double totalElapsed = 0.0; + double denseSchurSolveElapsed = 0.0; + double normalEquationsElapsed = 0.0; + double addDampingElapsed = 0.0; + double cudssAnalyzeElapsed = 0.0; + double cudssSolveElapsed = 0.0; + double linearizedErrorElapsed = 0.0; + double applyDeltaElapsed = 0.0; + double trialErrorElapsed = 0.0; + double lambdaUpdateElapsed = 0.0; + double oldLinearizedError = 0.0; + double newLinearizedError = 0.0; + double linearizedCostChange = 0.0; + double trialError = 0.0; + double costChange = 0.0; + double modelFidelity = 0.0; + bool attemptedTrial = false; + bool stepSuccessful = false; + bool accepted = false; + bool stopSearchingLambda = false; + bool lambdaUpperBoundReached = false; + bool terminated = false; +}; + +struct CudaSfmLmIterationProfile { + int iteration = 0; + double startError = 0.0; + double endError = 0.0; + double startLambda = 0.0; + double endLambda = 0.0; + double totalElapsed = 0.0; + double dampingDiagonalElapsed = 0.0; + double acceptTrialElapsed = 0.0; + bool acceptedStep = false; + bool terminated = false; + std::vector attemptProfiles; +}; + struct CudaSfmLevenbergMarquardtResult { double initialError = 0.0; double finalError = 0.0; double totalMeasuredElapsed = 0.0; + double graphConversionElapsed = 0.0; + double graphBackendCallElapsed = 0.0; + double graphConvertedDataDestructionElapsed = 0.0; + double graphValueMergeElapsed = 0.0; double setupElapsed = 0.0; double solveLoopElapsed = 0.0; double contextElapsed = 0.0; double packValuesElapsed = 0.0; + double packValuesHostBuildElapsed = 0.0; + double packValuesDeviceAllocElapsed = 0.0; + double packValuesH2dCopyElapsed = 0.0; + size_t packValuesH2dBytes = 0; double allocateTrialElapsed = 0.0; double projectionBatchElapsed = 0.0; + double projectionBatchHostBuildElapsed = 0.0; + double projectionBatchDeviceAllocElapsed = 0.0; + double projectionBatchH2dCopyElapsed = 0.0; + size_t projectionBatchH2dBytes = 0; double initialErrorElapsed = 0.0; double cudssSolverConstructionElapsed = 0.0; double denseSchurSolverConstructionElapsed = 0.0; double csrStructureElapsed = 0.0; double uploadPatternElapsed = 0.0; + double uploadPatternDeviceAllocElapsed = 0.0; + double uploadPatternH2dCopyElapsed = 0.0; + size_t uploadPatternH2dBytes = 0; double firstCudssAnalyzeElapsed = 0.0; double downloadElapsed = 0.0; + double downloadHostAllocElapsed = 0.0; + double downloadD2hCopyElapsed = 0.0; + double downloadValuesBuildElapsed = 0.0; + size_t downloadD2hBytes = 0; + double totalH2dCopyElapsed = 0.0; + size_t totalH2dBytes = 0; + double totalD2hCopyElapsed = 0.0; + size_t totalD2hBytes = 0; + double dampingDiagonalElapsed = 0.0; + double denseSchurSolveElapsed = 0.0; + double normalEquationsElapsed = 0.0; + double addDampingElapsed = 0.0; + double cudssAnalyzeElapsed = 0.0; + double cudssSolveElapsed = 0.0; + double linearizedErrorElapsed = 0.0; + double applyDeltaElapsed = 0.0; + double trialErrorElapsed = 0.0; + double acceptTrialElapsed = 0.0; + double lambdaUpdateElapsed = 0.0; int iterations = 0; int innerIterations = 0; int acceptedSteps = 0; double finalLambda = 0.0; + std::vector iterationProfiles; Values optimizedValues; }; diff --git a/gtsam/slam/cuda/CudaSfmProjectionBatch.h b/gtsam/slam/cuda/CudaSfmProjectionBatch.h index b917344ab0..1ef36c404c 100644 --- a/gtsam/slam/cuda/CudaSfmProjectionBatch.h +++ b/gtsam/slam/cuda/CudaSfmProjectionBatch.h @@ -6,6 +6,7 @@ #include +#include #include #include #include @@ -20,29 +21,38 @@ enum class CudaSfmProjectionNoiseMode { Robust, }; +struct CudaSfmProjectionBatchTransferProfile { + double hostBuildElapsed = 0.0; + double deviceAllocElapsed = 0.0; + CudaDeviceTransferSummary h2d; +}; + class CudaSfmProjectionBatch { public: static constexpr int kLongTrackMeasurementThreshold = 4; - static CudaSfmProjectionBatch FromSfmData(const SfmData& data, - cudaStream_t stream = nullptr) { - return FromSfmDataImpl(data, nullptr, nullptr, stream); + static CudaSfmProjectionBatch FromSfmData( + const SfmData& data, cudaStream_t stream = nullptr, + CudaSfmProjectionBatchTransferProfile* profile = nullptr) { + return FromSfmDataImpl(data, nullptr, nullptr, stream, profile); } static CudaSfmProjectionBatch FromSfmData( const SfmData& data, const std::vector>& sqrtInfoByTrack, - cudaStream_t stream = nullptr) { - return FromSfmDataImpl(data, &sqrtInfoByTrack, nullptr, stream); + cudaStream_t stream = nullptr, + CudaSfmProjectionBatchTransferProfile* profile = nullptr) { + return FromSfmDataImpl(data, &sqrtInfoByTrack, nullptr, stream, profile); } static CudaSfmProjectionBatch FromSfmData( const SfmData& data, const std::vector>& sqrtInfoByTrack, const std::vector>& robustModelsByTrack, - cudaStream_t stream = nullptr) { + cudaStream_t stream = nullptr, + CudaSfmProjectionBatchTransferProfile* profile = nullptr) { return FromSfmDataImpl(data, &sqrtInfoByTrack, &robustModelsByTrack, - stream); + stream, profile); } static CudaSfmSqrtInfo2 UnitSqrtInfo() { @@ -84,7 +94,12 @@ class CudaSfmProjectionBatch { const SfmData& data, const std::vector>* sqrtInfoByTrack, const std::vector>* robustModelsByTrack, - cudaStream_t stream) { + cudaStream_t stream, + CudaSfmProjectionBatchTransferProfile* profile) { + if (profile) { + *profile = CudaSfmProjectionBatchTransferProfile{}; + } + const auto hostBuildStart = std::chrono::steady_clock::now(); CudaSfmProjectionBatch batch; batch.numCameras_ = data.numberCameras(); batch.numPoints_ = data.numberTracks(); @@ -179,22 +194,47 @@ class CudaSfmProjectionBatch { pointObservationOffsets.push_back(static_cast(observations.size())); } - batch.observations_.upload(observations, stream); - batch.pointObservationOffsets_.upload(pointObservationOffsets, stream); - batch.longTrackPointSlots_.upload(longTrackPointSlots, stream); + if (profile) { + profile->hostBuildElapsed = + std::chrono::duration(std::chrono::steady_clock::now() - + hostBuildStart) + .count(); + profile->h2d.add(batch.observations_.uploadProfiled(observations, + stream)); + profile->h2d.add(batch.pointObservationOffsets_.uploadProfiled( + pointObservationOffsets, stream)); + profile->h2d.add(batch.longTrackPointSlots_.uploadProfiled( + longTrackPointSlots, stream)); + } else { + batch.observations_.upload(observations, stream); + batch.pointObservationOffsets_.upload(pointObservationOffsets, stream); + batch.longTrackPointSlots_.upload(longTrackPointSlots, stream); + } if (sqrtInfoByTrack) { if (sqrtInfos.size() != observations.size()) { throw std::invalid_argument( "CudaSfmProjectionBatch sqrt-info/observation count mismatch"); } - batch.sqrtInfos_.upload(sqrtInfos, stream); + if (profile) { + profile->h2d.add(batch.sqrtInfos_.uploadProfiled(sqrtInfos, stream)); + } else { + batch.sqrtInfos_.upload(sqrtInfos, stream); + } } if (robustModelsByTrack) { if (robustModels.size() != observations.size()) { throw std::invalid_argument( "CudaSfmProjectionBatch robust model/observation count mismatch"); } - batch.robustModels_.upload(robustModels, stream); + if (profile) { + profile->h2d.add( + batch.robustModels_.uploadProfiled(robustModels, stream)); + } else { + batch.robustModels_.upload(robustModels, stream); + } + } + if (profile) { + profile->deviceAllocElapsed = profile->h2d.resizeElapsed; } return batch; } diff --git a/gtsam/slam/cuda/CudaSfmValues.h b/gtsam/slam/cuda/CudaSfmValues.h index 6a751bc1b2..17c6c28af9 100644 --- a/gtsam/slam/cuda/CudaSfmValues.h +++ b/gtsam/slam/cuda/CudaSfmValues.h @@ -12,6 +12,7 @@ #include +#include #include #include #include @@ -43,10 +44,38 @@ inline DevicePoint3 PackDevicePoint3(const Point3& point) { return {point.x(), point.y(), point.z()}; } +struct CudaSfmValuesPackProfile { + double hostBuildElapsed = 0.0; + double deviceAllocElapsed = 0.0; + CudaDeviceTransferSummary h2d; +}; + +struct CudaSfmValuesDownloadProfile { + double hostAllocElapsed = 0.0; + double hostBuildElapsed = 0.0; + CudaDeviceTransferSummary d2h; +}; + +namespace internal { + +inline double CudaSfmValuesElapsedSince( + std::chrono::steady_clock::time_point start) { + return std::chrono::duration(std::chrono::steady_clock::now() - + start) + .count(); +} + +} // namespace internal + inline DeviceValues PackSfmValues(const SfmData& data, const std::vector& cameraKeys, const std::vector& pointKeys, - cudaStream_t stream = nullptr) { + cudaStream_t stream = nullptr, + CudaSfmValuesPackProfile* profile = + nullptr) { + if (profile) { + *profile = CudaSfmValuesPackProfile{}; + } if (cameraKeys.size() != data.numberCameras()) { throw std::invalid_argument( "PackSfmValues camera key count does not match SfmData"); @@ -56,6 +85,7 @@ inline DeviceValues PackSfmValues(const SfmData& data, "PackSfmValues point key count does not match SfmData"); } + const auto hostBuildStart = std::chrono::steady_clock::now(); std::vector cameras; cameras.reserve(data.numberCameras()); for (size_t i = 0; i < data.numberCameras(); ++i) { @@ -67,18 +97,39 @@ inline DeviceValues PackSfmValues(const SfmData& data, for (size_t i = 0; i < data.numberTracks(); ++i) { points.push_back(PackDevicePoint3(data.track(i).point3())); } + if (profile) { + profile->hostBuildElapsed = + internal::CudaSfmValuesElapsedSince(hostBuildStart); + } DeviceValues values; + CudaDeviceTransferTiming cameraUpload; + CudaDeviceTransferTiming pointUpload; + double cameraDeltaAllocElapsed = 0.0; + double pointDeltaAllocElapsed = 0.0; values.addBlock( kDevicePinholeCameraCal3BundlerType, - kDevicePinholeCameraCal3BundlerTangentDim, cameraKeys, cameras, stream); + kDevicePinholeCameraCal3BundlerTangentDim, cameraKeys, cameras, stream, + profile ? &cameraUpload : nullptr, + profile ? &cameraDeltaAllocElapsed : nullptr); values.addBlock(kDevicePoint3Type, kDevicePoint3TangentDim, - pointKeys, points, stream); + pointKeys, points, stream, + profile ? &pointUpload : nullptr, + profile ? &pointDeltaAllocElapsed : nullptr); + if (profile) { + profile->h2d.add(cameraUpload); + profile->h2d.add(pointUpload); + profile->deviceAllocElapsed = + profile->h2d.resizeElapsed + cameraDeltaAllocElapsed + + pointDeltaAllocElapsed; + } return values; } inline DeviceValues PackSfmValues(const SfmData& data, - cudaStream_t stream = nullptr) { + cudaStream_t stream = nullptr, + CudaSfmValuesPackProfile* profile = + nullptr) { std::vector cameraKeys; cameraKeys.reserve(data.numberCameras()); for (size_t i = 0; i < data.numberCameras(); ++i) { @@ -91,7 +142,7 @@ inline DeviceValues PackSfmValues(const SfmData& data, pointKeys.push_back(symbol_shorthand::P(i)); } - return PackSfmValues(data, cameraKeys, pointKeys, stream); + return PackSfmValues(data, cameraKeys, pointKeys, stream, profile); } inline DeviceValues AllocateSfmValuesLike(const DeviceValues& reference) { @@ -110,17 +161,33 @@ inline DeviceValues AllocateSfmValuesLike(const DeviceValues& reference) { } inline Values DownloadSfmValues(const DeviceValues& deviceValues, - cudaStream_t stream = nullptr) { + cudaStream_t stream = nullptr, + CudaSfmValuesDownloadProfile* profile = + nullptr) { + if (profile) { + *profile = CudaSfmValuesDownloadProfile{}; + } std::vector cameras; std::vector points; const auto& cameraBlock = deviceValues.block( kDevicePinholeCameraCal3BundlerType); const auto& pointBlock = deviceValues.block(kDevicePoint3Type); - cameraBlock.values.download(&cameras, stream); - pointBlock.values.download(&points, stream); + if (profile) { + const CudaDeviceTransferTiming cameraDownload = + cameraBlock.values.downloadProfiled(&cameras, stream); + const CudaDeviceTransferTiming pointDownload = + pointBlock.values.downloadProfiled(&points, stream); + profile->d2h.add(cameraDownload); + profile->d2h.add(pointDownload); + profile->hostAllocElapsed = profile->d2h.resizeElapsed; + } else { + cameraBlock.values.download(&cameras, stream); + pointBlock.values.download(&points, stream); + } GTSAM_CUDA_CHECK(cudaStreamSynchronize(stream)); + const auto hostBuildStart = std::chrono::steady_clock::now(); Values result; for (size_t i = 0; i < cameras.size(); ++i) { Matrix3 R; @@ -139,6 +206,10 @@ inline Values DownloadSfmValues(const DeviceValues& deviceValues, result.insert(pointBlock.keys[i], Point3(points[i].x, points[i].y, points[i].z)); } + if (profile) { + profile->hostBuildElapsed = + internal::CudaSfmValuesElapsedSince(hostBuildStart); + } return result; } diff --git a/gtsam/slam/cuda/cuda_sfm.i b/gtsam/slam/cuda/cuda_sfm.i index f836fdf1cd..ff5a0d4c51 100644 --- a/gtsam/slam/cuda/cuda_sfm.i +++ b/gtsam/slam/cuda/cuda_sfm.i @@ -53,15 +53,34 @@ class CudaSfmLevenbergMarquardtResult { double solveLoopElapsed; double contextElapsed; double packValuesElapsed; + double packValuesHostBuildElapsed; + double packValuesDeviceAllocElapsed; + double packValuesH2dCopyElapsed; + size_t packValuesH2dBytes; double allocateTrialElapsed; double projectionBatchElapsed; + double projectionBatchHostBuildElapsed; + double projectionBatchDeviceAllocElapsed; + double projectionBatchH2dCopyElapsed; + size_t projectionBatchH2dBytes; double initialErrorElapsed; double cudssSolverConstructionElapsed; double denseSchurSolverConstructionElapsed; double csrStructureElapsed; double uploadPatternElapsed; + double uploadPatternDeviceAllocElapsed; + double uploadPatternH2dCopyElapsed; + size_t uploadPatternH2dBytes; double firstCudssAnalyzeElapsed; double downloadElapsed; + double downloadHostAllocElapsed; + double downloadD2hCopyElapsed; + double downloadValuesBuildElapsed; + size_t downloadD2hBytes; + double totalH2dCopyElapsed; + size_t totalH2dBytes; + double totalD2hCopyElapsed; + size_t totalD2hBytes; int iterations; int innerIterations; int acceptedSteps; diff --git a/gtsam/slam/tests/testCudaSfm.cpp b/gtsam/slam/tests/testCudaSfm.cpp index eca17c71c8..c7030fa873 100644 --- a/gtsam/slam/tests/testCudaSfm.cpp +++ b/gtsam/slam/tests/testCudaSfm.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -19,6 +20,7 @@ #include #include #include +#include #include #include @@ -301,6 +303,84 @@ CudaSfmRobustModel MakeRobustModel( return CudaSfmRobustModel{kind, reweightScheme, parameter}; } +bool ConvertedSfmDataEquals(const CudaSfmFactorGraphData& expected, + const CudaSfmFactorGraphData& actual) { + if (expected.cameraKeys.size() != actual.cameraKeys.size() || + expected.pointKeys.size() != actual.pointKeys.size()) { + return false; + } + for (size_t i = 0; i < expected.cameraKeys.size(); ++i) { + if (expected.cameraKeys[i] != actual.cameraKeys[i] || + !CameraEquals(expected.data.camera(i), actual.data.camera(i))) { + return false; + } + } + for (size_t i = 0; i < expected.pointKeys.size(); ++i) { + if (expected.pointKeys[i] != actual.pointKeys[i] || + !Point3Equals(expected.data.track(i).point3(), + actual.data.track(i).point3()) || + expected.data.track(i).numberMeasurements() != + actual.data.track(i).numberMeasurements()) { + return false; + } + for (size_t j = 0; j < expected.data.track(i).numberMeasurements(); ++j) { + const SfmMeasurement& expectedMeasurement = + expected.data.track(i).measurement(j); + const SfmMeasurement& actualMeasurement = + actual.data.track(i).measurement(j); + if (expectedMeasurement.first != actualMeasurement.first || + std::abs(expectedMeasurement.second(0) - + actualMeasurement.second(0)) > 1e-12 || + std::abs(expectedMeasurement.second(1) - + actualMeasurement.second(1)) > 1e-12) { + return false; + } + } + } + if (expected.hasNonUnitNoise != actual.hasNonUnitNoise || + expected.hasRobustNoise != actual.hasRobustNoise || + expected.sqrtInfoByTrack.size() != actual.sqrtInfoByTrack.size()) { + return false; + } + for (size_t i = 0; i < expected.sqrtInfoByTrack.size(); ++i) { + if (expected.sqrtInfoByTrack[i].size() != + actual.sqrtInfoByTrack[i].size()) { + return false; + } + for (size_t j = 0; j < expected.sqrtInfoByTrack[i].size(); ++j) { + if (std::abs(expected.sqrtInfoByTrack[i][j].r00 - + actual.sqrtInfoByTrack[i][j].r00) > 1e-12 || + std::abs(expected.sqrtInfoByTrack[i][j].r01 - + actual.sqrtInfoByTrack[i][j].r01) > 1e-12 || + std::abs(expected.sqrtInfoByTrack[i][j].r11 - + actual.sqrtInfoByTrack[i][j].r11) > 1e-12) { + return false; + } + } + } + if (expected.robustModelsByTrack.size() != + actual.robustModelsByTrack.size()) { + return false; + } + for (size_t i = 0; i < expected.robustModelsByTrack.size(); ++i) { + if (expected.robustModelsByTrack[i].size() != + actual.robustModelsByTrack[i].size()) { + return false; + } + for (size_t j = 0; j < expected.robustModelsByTrack[i].size(); ++j) { + if (expected.robustModelsByTrack[i][j].kind != + actual.robustModelsByTrack[i][j].kind || + expected.robustModelsByTrack[i][j].reweightScheme != + actual.robustModelsByTrack[i][j].reweightScheme || + std::abs(expected.robustModelsByTrack[i][j].parameter - + actual.robustModelsByTrack[i][j].parameter) > 1e-12) { + return false; + } + } + } + return true; +} + Vector2 WhitenResidual(const CudaSfmSqrtInfo2& sqrtInfo, const Vector2& residual) { return Vector2(sqrtInfo.r00 * residual(0) + @@ -1257,6 +1337,92 @@ TEST(CudaSfmFactorGraphConversion, converted.data.track(3).point3())); } +TEST(CudaSfmFactorGraphConversion, + ConvertsPointBatchedGeneralSfmFactors) { + const SfmData measuredData = makeTrueBalLikeData(); + const SfmData initialData = makePerturbedBalLikeData(measuredData); + const std::vector cameraKeys = {Symbol('x', 10), Symbol('x', 20)}; + const std::vector pointKeys = {Symbol('l', 100), Symbol('l', 200), + Symbol('l', 300), Symbol('l', 400)}; + + Values initial; + for (size_t i = 0; i < cameraKeys.size(); ++i) { + initial.insert(cameraKeys[i], initialData.camera(i)); + } + for (size_t i = 0; i < pointKeys.size(); ++i) { + initial.insert(pointKeys[i], initialData.track(i).point3()); + } + + NonlinearFactorGraph rawGraph; + NonlinearFactorGraph batchGraph; + const auto model = noiseModel::Unit::Create(2); + for (size_t pointSlot = 0; pointSlot < measuredData.numberTracks(); + ++pointSlot) { + std::map measurements; + const SfmTrack& track = measuredData.track(pointSlot); + for (const SfmMeasurement& measurement : track.measurements) { + rawGraph.emplace_shared( + measurement.second, model, cameraKeys[measurement.first], + pointKeys[pointSlot]); + measurements[cameraKeys[measurement.first]] = measurement.second; + } + batchGraph.add( + std::make_shared>( + measurements, pointKeys[pointSlot], model)); + } + + const CudaSfmFactorGraphData raw = + ConvertGeneralSfmGraphToCudaSfmData(rawGraph, initial); + const CudaSfmFactorGraphData batched = + ConvertGeneralSfmGraphToCudaSfmData(batchGraph, initial); + CHECK(ConvertedSfmDataEquals(raw, batched)); +} + +TEST(CudaSfmFactorGraphConversion, + ConvertsCameraBatchedGeneralSfmFactors) { + const SfmData measuredData = makeTrueBalLikeData(); + const SfmData initialData = makePerturbedBalLikeData(measuredData); + const std::vector cameraKeys = {Symbol('x', 10), Symbol('x', 20)}; + const std::vector pointKeys = {Symbol('l', 100), Symbol('l', 200), + Symbol('l', 300), Symbol('l', 400)}; + + Values initial; + for (size_t i = 0; i < cameraKeys.size(); ++i) { + initial.insert(cameraKeys[i], initialData.camera(i)); + } + for (size_t i = 0; i < pointKeys.size(); ++i) { + initial.insert(pointKeys[i], initialData.track(i).point3()); + } + + NonlinearFactorGraph rawGraph; + NonlinearFactorGraph batchGraph; + std::vector> measurementsByCamera(cameraKeys.size()); + const auto model = noiseModel::Unit::Create(2); + for (size_t pointSlot = 0; pointSlot < measuredData.numberTracks(); + ++pointSlot) { + const SfmTrack& track = measuredData.track(pointSlot); + for (const SfmMeasurement& measurement : track.measurements) { + rawGraph.emplace_shared( + measurement.second, model, cameraKeys[measurement.first], + pointKeys[pointSlot]); + measurementsByCamera[measurement.first][pointKeys[pointSlot]] = + measurement.second; + } + } + for (size_t cameraSlot = 0; cameraSlot < measurementsByCamera.size(); + ++cameraSlot) { + batchGraph.add( + std::make_shared>( + cameraKeys[cameraSlot], measurementsByCamera[cameraSlot], model)); + } + + const CudaSfmFactorGraphData raw = + ConvertGeneralSfmGraphToCudaSfmData(rawGraph, initial); + const CudaSfmFactorGraphData batched = + ConvertGeneralSfmGraphToCudaSfmData(batchGraph, initial); + CHECK(ConvertedSfmDataEquals(raw, batched)); +} + TEST(CudaSfmFactorGraphConversion, AcceptsFixedGaussianNoiseAndPreservesFlattenedWhiteningOrder) { const SfmData measuredData = makeTrueBalLikeData(); @@ -1538,6 +1704,11 @@ TEST(CudaSfmLevenbergMarquardtOptimizer, CHECK(graph.error(result) < graph.error(initial)); CHECK(optimizer.result().innerIterations >= static_cast(optimizer.iterations())); + CHECK(optimizer.result().graphConversionElapsed > 0.0); + CHECK(optimizer.result().graphBackendCallElapsed >= + optimizer.result().totalMeasuredElapsed); + CHECK(optimizer.result().graphConvertedDataDestructionElapsed >= 0.0); + CHECK(optimizer.result().graphValueMergeElapsed > 0.0); CHECK(result.exists(cameraKeys[0])); CHECK(result.exists(pointKeys[3])); CHECK(result.exists(Symbol('u', 1))); @@ -1725,6 +1896,83 @@ TEST(CudaSfmLevenbergMarquardt, CanSkipOptimizedValueDownload) { CHECK(result.optimizedValues.empty()); } +TEST(CudaSfmLevenbergMarquardt, RecordsDetailedTimingBreakdown) { + const SfmData measuredData = makeTrueBalLikeData(); + const SfmData data = makePerturbedBalLikeData(measuredData); + + CudaSfmLevenbergMarquardtParams params = + CudaSfmLevenbergMarquardtParams::CeresDefaults(); + params.maxIterations = 5; + params.relativeErrorTol = 1e-12; + params.lambdaInitial = 1e-3; + + const CudaSfmLevenbergMarquardtResult result = + OptimizeCudaSfmWithoutValueDownload(data, params); + + CHECK(result.solveLoopElapsed > 0.0); + CHECK(result.dampingDiagonalElapsed > 0.0); + CHECK(result.denseSchurSolveElapsed > 0.0); + CHECK(result.linearizedErrorElapsed > 0.0); + CHECK(result.applyDeltaElapsed > 0.0); + CHECK(result.trialErrorElapsed > 0.0); + CHECK(result.lambdaUpdateElapsed > 0.0); + CHECK(!result.iterationProfiles.empty()); + + size_t attempts = 0; + for (const auto& iteration : result.iterationProfiles) { + CHECK(iteration.totalElapsed > 0.0); + CHECK(iteration.dampingDiagonalElapsed >= 0.0); + CHECK(!iteration.attemptProfiles.empty()); + attempts += iteration.attemptProfiles.size(); + for (const auto& attempt : iteration.attemptProfiles) { + CHECK(attempt.totalElapsed > 0.0); + CHECK(attempt.lambda > 0.0); + CHECK(attempt.linearizedErrorElapsed > 0.0); + CHECK(attempt.linearizedCostChange != 0.0); + CHECK(attempt.denseSchurSolveElapsed > 0.0); + } + } + EXPECT_LONGS_EQUAL(result.innerIterations, attempts); +} + +TEST(CudaSfmLevenbergMarquardt, RecordsPureTransferTimingBreakdown) { + const SfmData measuredData = makeTrueBalLikeData(); + const SfmData data = makePerturbedBalLikeData(measuredData); + + CudaSfmLevenbergMarquardtParams params = + CudaSfmLevenbergMarquardtParams::CeresDefaults(); + params.maxIterations = 1; + params.relativeErrorTol = 1e-12; + params.lambdaInitial = 1e-3; + + const CudaSfmLevenbergMarquardtResult result = + OptimizeCudaSfm(data, params); + + const size_t expectedValueBytes = + data.numberCameras() * sizeof(DevicePinholeCameraCal3Bundler) + + data.numberTracks() * sizeof(DevicePoint3); + const size_t expectedProjectionBytes = + 8 * sizeof(CudaSfmObservation) + + (data.numberTracks() + 1) * sizeof(int); + + EXPECT_LONGS_EQUAL(expectedValueBytes, result.packValuesH2dBytes); + EXPECT_LONGS_EQUAL(expectedProjectionBytes, result.projectionBatchH2dBytes); + EXPECT_LONGS_EQUAL(expectedValueBytes + expectedProjectionBytes, + result.totalH2dBytes); + EXPECT_LONGS_EQUAL(expectedValueBytes, result.downloadD2hBytes); + EXPECT_LONGS_EQUAL(expectedValueBytes, result.totalD2hBytes); + + CHECK(result.packValuesHostBuildElapsed >= 0.0); + CHECK(result.packValuesDeviceAllocElapsed >= 0.0); + CHECK(result.packValuesH2dCopyElapsed >= 0.0); + CHECK(result.projectionBatchHostBuildElapsed >= 0.0); + CHECK(result.projectionBatchDeviceAllocElapsed >= 0.0); + CHECK(result.projectionBatchH2dCopyElapsed >= 0.0); + CHECK(result.downloadHostAllocElapsed >= 0.0); + CHECK(result.downloadD2hCopyElapsed >= 0.0); + CHECK(result.downloadValuesBuildElapsed >= 0.0); +} + TEST(CudaSfmDenseSchurSolver, MatchesFullNormalEquationDeltaOnTinyBal) { const SfmData measuredData = makeTrueBalLikeData(); const SfmData data = makePerturbedBalLikeData(measuredData); diff --git a/timing/sfm_ba/timeCudaSFMBAL.cpp b/timing/sfm_ba/timeCudaSFMBAL.cpp index 59e1c9debb..d1a39f731a 100644 --- a/timing/sfm_ba/timeCudaSFMBAL.cpp +++ b/timing/sfm_ba/timeCudaSFMBAL.cpp @@ -18,6 +18,8 @@ #include "../timeSFMBAL.h" +#include + #if GTSAM_ENABLE_CUDA #include #include @@ -32,15 +34,21 @@ #include #include #include +#include namespace { constexpr const char* kDefaultBenchmarkDataset = "dubrovnik-16-22106-pre"; constexpr const char* kProfileDataset = "dubrovnik-135-90642-pre"; +using Camera = PinholeCamera; +using SfmFactor = GeneralSFMFactor; + std::string usage() { return "Usage: timeCudaSFMBAL [--colamd] [--profile] [--cuda-structure-only] " "[--cuda-lm] [--cuda-lm-graph] " "[--cuda-linear-solver dense-schur|cudss-full-normal] " + "[--cuda-lm-graph-kind raw|point-batch|camera-batch] " + "[--batch-chunk-size N] " "[--cuda-warmup-file FILE] " "[--projection-noise unit|huber|tukey] " "[--benchmark-action-json FILE] [BALfile ...]"; @@ -57,6 +65,12 @@ enum class CudaLinearSolverOption { CudssFullNormal, }; +enum class CudaGraphKind { + Raw, + PointBatch, + CameraBatch, +}; + struct RunOptions { bool profile = false; bool cudaStructureOnly = false; @@ -64,6 +78,10 @@ struct RunOptions { bool cudaLmGraph = false; bool cudaLinearSolverSpecified = false; CudaLinearSolverOption cudaLinearSolver = CudaLinearSolverOption::DenseSchur; + bool cudaGraphKindSpecified = false; + CudaGraphKind cudaGraphKind = CudaGraphKind::Raw; + bool batchChunkSizeSpecified = false; + size_t batchChunkSize = 0; bool cudaWarmupFileSpecified = false; std::string cudaWarmupFile; bool benchmarkActionJson = false; @@ -81,6 +99,18 @@ const char* cudaLinearSolverName(CudaLinearSolverOption solver) { return "unknown"; } +const char* cudaGraphKindName(CudaGraphKind kind) { + switch (kind) { + case CudaGraphKind::Raw: + return "raw"; + case CudaGraphKind::PointBatch: + return "point-batch"; + case CudaGraphKind::CameraBatch: + return "camera-batch"; + } + return "unknown"; +} + void setProjectionNoiseModel(const char* noiseModel) { if (strcmp(noiseModel, "unit") == 0) { gNoiseModel = noiseModel::Unit::Create(2); @@ -157,18 +187,38 @@ CudaBackendLmRun runCudaBackendLm( return run; } -void printCudaBackendLmRun(const CudaBackendLmRun& run, - CudaLinearSolverOption solverOption) { - const auto& result = run.result; - std::cout << " CUDA LM: " << run.elapsed << " s\n"; - std::cout << " CUDA LM linear solver: " - << cudaLinearSolverName(solverOption) << "\n"; - std::cout << " CUDA LM solve loop: " << result.solveLoopElapsed << " s\n"; - std::cout << " CUDA LM measured total: " << result.totalMeasuredElapsed - << " s\n"; - std::cout << " CUDA LM setup before solve loop: " << result.setupElapsed - << " s\n"; - std::cout << " CUDA LM setup breakdown:\n"; +const char* yesNo(bool value) { return value ? "yes" : "no"; } + +void printProfileRow(const std::string& indent, const std::string& label, + double elapsed, double total) { + std::cout << indent << std::left << std::setw(30) << label << std::right + << elapsed << " s"; + if (total > 0.0) { + std::cout << " (" << (100.0 * elapsed / total) << "%)"; + } + std::cout << "\n"; +} + +void printTransferRow(const std::string& indent, const std::string& label, + double elapsed, size_t bytes, double total) { + std::cout << indent << std::left << std::setw(30) << label << std::right + << elapsed << " s"; + if (total > 0.0) { + std::cout << " (" << (100.0 * elapsed / total) << "%)"; + } + std::cout << " [" << bytes << " B"; + if (elapsed > 0.0 && bytes > 0) { + const double gib = + static_cast(bytes) / (1024.0 * 1024.0 * 1024.0); + std::cout << ", " << (gib / elapsed) << " GiB/s"; + } + std::cout << "]\n"; +} + +void printCudaLmSetupBreakdown( + const gtsam::cuda::CudaSfmLevenbergMarquardtResult& result, + const std::string& prefix) { + std::cout << prefix << "stage breakdown:\n"; std::cout << " context: " << result.contextElapsed << " s\n"; std::cout << " pack values: " << result.packValuesElapsed << " s\n"; std::cout << " allocate trial values: " << result.allocateTrialElapsed @@ -182,9 +232,160 @@ void printCudaBackendLmRun(const CudaBackendLmRun& run, << result.denseSchurSolverConstructionElapsed << " s\n"; std::cout << " CSR structure: " << result.csrStructureElapsed << " s\n"; std::cout << " upload pattern: " << result.uploadPatternElapsed << " s\n"; - std::cout << " first cuDSS analyze: " + std::cout << " first cuDSS analyze (solve loop): " << result.firstCudssAnalyzeElapsed << " s\n"; - std::cout << " download values: " << result.downloadElapsed << " s\n"; + std::cout << " download values (post solve): " << result.downloadElapsed + << " s\n"; +} + +void printCudaLmTransferBreakdown( + const gtsam::cuda::CudaSfmLevenbergMarquardtResult& result, + const std::string& prefix) { + const double total = result.totalMeasuredElapsed; + std::cout << prefix << "pure transfer breakdown:\n"; + printProfileRow(" ", "pack values host build", + result.packValuesHostBuildElapsed, total); + printProfileRow(" ", "pack values device alloc", + result.packValuesDeviceAllocElapsed, total); + printTransferRow(" ", "pack values H2D memcpy", + result.packValuesH2dCopyElapsed, result.packValuesH2dBytes, + total); + printProfileRow(" ", "projection host build", + result.projectionBatchHostBuildElapsed, total); + printProfileRow(" ", "projection device alloc", + result.projectionBatchDeviceAllocElapsed, total); + printTransferRow(" ", "projection H2D memcpy", + result.projectionBatchH2dCopyElapsed, + result.projectionBatchH2dBytes, total); + printProfileRow(" ", "CSR pattern device alloc", + result.uploadPatternDeviceAllocElapsed, total); + printTransferRow(" ", "CSR pattern H2D memcpy", + result.uploadPatternH2dCopyElapsed, + result.uploadPatternH2dBytes, total); + printTransferRow(" ", "total H2D memcpy", result.totalH2dCopyElapsed, + result.totalH2dBytes, total); + printProfileRow(" ", "download host alloc", + result.downloadHostAllocElapsed, total); + printTransferRow(" ", "download D2H memcpy", + result.downloadD2hCopyElapsed, result.downloadD2hBytes, + total); + printProfileRow(" ", "download Values rebuild", + result.downloadValuesBuildElapsed, total); + printTransferRow(" ", "total D2H memcpy", result.totalD2hCopyElapsed, + result.totalD2hBytes, total); +} + +void printCudaLmDetailedBreakdown( + const gtsam::cuda::CudaSfmLevenbergMarquardtResult& result, + const std::string& prefix) { + const double solveLoop = result.solveLoopElapsed; + const double accounted = + result.dampingDiagonalElapsed + result.denseSchurSolveElapsed + + result.normalEquationsElapsed + result.addDampingElapsed + + result.cudssAnalyzeElapsed + result.cudssSolveElapsed + + result.linearizedErrorElapsed + result.applyDeltaElapsed + + result.trialErrorElapsed + result.acceptTrialElapsed + + result.lambdaUpdateElapsed; + + std::cout << prefix << "solve-loop aggregate breakdown:\n"; + printProfileRow(" ", "damping diagonal", result.dampingDiagonalElapsed, + solveLoop); + printProfileRow(" ", "dense Schur solve", result.denseSchurSolveElapsed, + solveLoop); + printProfileRow(" ", "normal equations", result.normalEquationsElapsed, + solveLoop); + printProfileRow(" ", "add diagonal damping", result.addDampingElapsed, + solveLoop); + printProfileRow(" ", "cuDSS analyze", result.cudssAnalyzeElapsed, + solveLoop); + printProfileRow(" ", "cuDSS solve", result.cudssSolveElapsed, solveLoop); + printProfileRow(" ", "linearized error change", + result.linearizedErrorElapsed, solveLoop); + printProfileRow(" ", "apply delta", result.applyDeltaElapsed, solveLoop); + printProfileRow(" ", "trial projection error", result.trialErrorElapsed, + solveLoop); + printProfileRow(" ", "accept trial copy", result.acceptTrialElapsed, + solveLoop); + printProfileRow(" ", "lambda/convergence update", + result.lambdaUpdateElapsed, solveLoop); + printProfileRow(" ", "profiled subtotal", accounted, solveLoop); + printProfileRow(" ", "unprofiled loop time", solveLoop - accounted, + solveLoop); + + std::cout << prefix << "per-iteration breakdown:\n"; + if (result.iterationProfiles.empty()) { + std::cout << " no LM iterations recorded\n"; + return; + } + for (const auto& iteration : result.iterationProfiles) { + std::cout << " iteration " << iteration.iteration << ": total " + << iteration.totalElapsed << " s, attempts " + << iteration.attemptProfiles.size() + << ", accepted: " << yesNo(iteration.acceptedStep) + << ", terminated: " << yesNo(iteration.terminated) << "\n"; + std::cout << " error: " << std::setprecision(15) + << iteration.startError << " -> " << iteration.endError + << std::setprecision(6) << "\n"; + std::cout << " lambda: " << iteration.startLambda << " -> " + << iteration.endLambda << "\n"; + printProfileRow(" ", "damping diagonal", + iteration.dampingDiagonalElapsed, iteration.totalElapsed); + printProfileRow(" ", "accept trial copy", + iteration.acceptTrialElapsed, iteration.totalElapsed); + + for (const auto& attempt : iteration.attemptProfiles) { + std::cout << " attempt " << attempt.attempt << ": total " + << attempt.totalElapsed << " s, lambda " << attempt.lambda + << ", accepted: " << yesNo(attempt.accepted) + << ", step successful: " << yesNo(attempt.stepSuccessful) + << ", attempted trial: " << yesNo(attempt.attemptedTrial) + << ", terminated: " << yesNo(attempt.terminated) << "\n"; + std::cout << " costs: linearized change " + << attempt.linearizedCostChange << ", actual change " + << attempt.costChange << ", fidelity " + << attempt.modelFidelity << ", trial error " + << std::setprecision(15) << attempt.trialError + << std::setprecision(6) << "\n"; + std::cout << " flags: stop lambda search " + << yesNo(attempt.stopSearchingLambda) + << ", lambda upper bound " + << yesNo(attempt.lambdaUpperBoundReached) << "\n"; + printProfileRow(" ", "dense Schur solve", + attempt.denseSchurSolveElapsed, attempt.totalElapsed); + printProfileRow(" ", "normal equations", + attempt.normalEquationsElapsed, attempt.totalElapsed); + printProfileRow(" ", "add diagonal damping", + attempt.addDampingElapsed, attempt.totalElapsed); + printProfileRow(" ", "cuDSS analyze", + attempt.cudssAnalyzeElapsed, attempt.totalElapsed); + printProfileRow(" ", "cuDSS solve", attempt.cudssSolveElapsed, + attempt.totalElapsed); + printProfileRow(" ", "linearized error change", + attempt.linearizedErrorElapsed, attempt.totalElapsed); + printProfileRow(" ", "apply delta", attempt.applyDeltaElapsed, + attempt.totalElapsed); + printProfileRow(" ", "trial projection error", + attempt.trialErrorElapsed, attempt.totalElapsed); + printProfileRow(" ", "lambda/convergence update", + attempt.lambdaUpdateElapsed, attempt.totalElapsed); + } + } +} + +void printCudaBackendLmRun(const CudaBackendLmRun& run, + CudaLinearSolverOption solverOption) { + const auto& result = run.result; + std::cout << " CUDA LM: " << run.elapsed << " s\n"; + std::cout << " CUDA LM linear solver: " + << cudaLinearSolverName(solverOption) << "\n"; + std::cout << " CUDA LM solve loop: " << result.solveLoopElapsed << " s\n"; + std::cout << " CUDA LM measured total: " << result.totalMeasuredElapsed + << " s\n"; + std::cout << " CUDA LM setup before solve loop: " << result.setupElapsed + << " s\n"; + printCudaLmSetupBreakdown(result, " CUDA LM "); + printCudaLmTransferBreakdown(result, " CUDA LM "); + printCudaLmDetailedBreakdown(result, " CUDA LM "); std::cout << "Initial error: " << std::setprecision(15) << result.initialError << "\n"; std::cout << "Final error: " << result.finalError @@ -195,6 +396,9 @@ void printCudaBackendLmRun(const CudaBackendLmRun& run, struct CudaGraphLmRun { double elapsed = 0.0; + double optimizerConstructionElapsed = 0.0; + double optimizeElapsed = 0.0; + double resultQueryElapsed = 0.0; double initialError = 0.0; double finalError = 0.0; size_t iterations = 0; @@ -206,53 +410,92 @@ CudaGraphLmRun runCudaGraphLm(const NonlinearFactorGraph& graph, const gtsam::cuda::CudaSfmLevenbergMarquardtParams& params) { const auto start = std::chrono::high_resolution_clock::now(); + + const auto constructionStart = std::chrono::high_resolution_clock::now(); gtsam::cuda::CudaSfmLevenbergMarquardtOptimizer lm(graph, initial, params); - lm.optimize(); + const auto constructionEnd = std::chrono::high_resolution_clock::now(); + + const auto optimizeStart = std::chrono::high_resolution_clock::now(); + const Values& optimized = lm.optimize(); + (void)optimized; + const auto optimizeEnd = std::chrono::high_resolution_clock::now(); + + const auto resultQueryStart = std::chrono::high_resolution_clock::now(); + const double initialError = lm.result().initialError; + const double finalError = lm.error(); + const size_t iterations = lm.iterations(); + const gtsam::cuda::CudaSfmLevenbergMarquardtResult backend = lm.result(); + const auto resultQueryEnd = std::chrono::high_resolution_clock::now(); + const auto end = std::chrono::high_resolution_clock::now(); CudaGraphLmRun run; run.elapsed = std::chrono::duration(end - start).count(); - run.initialError = lm.result().initialError; - run.finalError = lm.error(); - run.iterations = lm.iterations(); - run.backend = lm.result(); + run.optimizerConstructionElapsed = + std::chrono::duration(constructionEnd - constructionStart) + .count(); + run.optimizeElapsed = + std::chrono::duration(optimizeEnd - optimizeStart).count(); + run.resultQueryElapsed = + std::chrono::duration(resultQueryEnd - resultQueryStart).count(); + run.initialError = initialError; + run.finalError = finalError; + run.iterations = iterations; + run.backend = backend; return run; } void printCudaGraphLmRun(const CudaGraphLmRun& run, - CudaLinearSolverOption solverOption) { + CudaLinearSolverOption solverOption, + CudaGraphKind graphKind) { + const double graphBackendCallOverhead = + run.backend.graphBackendCallElapsed - run.backend.totalMeasuredElapsed; + const double graphApiOtherOverhead = + run.optimizeElapsed - run.backend.totalMeasuredElapsed - + run.backend.graphConversionElapsed - run.backend.graphValueMergeElapsed; + const double graphApiRemainingOptimizeOverhead = + run.optimizeElapsed - run.backend.graphConversionElapsed - + run.backend.graphBackendCallElapsed - + run.backend.graphValueMergeElapsed - + run.backend.graphConvertedDataDestructionElapsed; + std::cout << " CUDA LM graph API: " << run.elapsed << " s\n"; std::cout << " CUDA LM graph linear solver: " << cudaLinearSolverName(solverOption) << "\n"; + std::cout << " CUDA LM graph kind: " << cudaGraphKindName(graphKind) + << "\n"; std::cout << " CUDA LM graph API overhead over backend: " << run.elapsed - run.backend.totalMeasuredElapsed << " s\n"; + std::cout << " CUDA LM graph API breakdown:\n"; + printProfileRow(" ", "optimizer construction", + run.optimizerConstructionElapsed, run.elapsed); + printProfileRow(" ", "optimize call", run.optimizeElapsed, run.elapsed); + printProfileRow(" ", "result/error queries", run.resultQueryElapsed, + run.elapsed); + printProfileRow(" ", "graph conversion", + run.backend.graphConversionElapsed, run.elapsed); + printProfileRow(" ", "backend measured total", + run.backend.totalMeasuredElapsed, run.elapsed); + printProfileRow(" ", "backend return/assignment", + graphBackendCallOverhead, run.elapsed); + printProfileRow(" ", "value merge/state update", + run.backend.graphValueMergeElapsed, run.elapsed); + printProfileRow(" ", "converted data destruction", + run.backend.graphConvertedDataDestructionElapsed, + run.elapsed); + printProfileRow(" ", "other graph optimize overhead", + graphApiOtherOverhead, run.elapsed); + printProfileRow(" ", "remaining optimize overhead", + graphApiRemainingOptimizeOverhead, run.elapsed); std::cout << " CUDA LM backend solve loop: " << run.backend.solveLoopElapsed << " s\n"; std::cout << " CUDA LM backend measured total: " << run.backend.totalMeasuredElapsed << " s\n"; std::cout << " CUDA LM backend setup before solve loop: " << run.backend.setupElapsed << " s\n"; - std::cout << " CUDA LM backend setup breakdown:\n"; - std::cout << " context: " << run.backend.contextElapsed << " s\n"; - std::cout << " pack values: " << run.backend.packValuesElapsed << " s\n"; - std::cout << " allocate trial values: " - << run.backend.allocateTrialElapsed << " s\n"; - std::cout << " projection batch: " << run.backend.projectionBatchElapsed - << " s\n"; - std::cout << " initial error: " << run.backend.initialErrorElapsed - << " s\n"; - std::cout << " cuDSS solver construction: " - << run.backend.cudssSolverConstructionElapsed << " s\n"; - std::cout << " dense Schur solver construction: " - << run.backend.denseSchurSolverConstructionElapsed << " s\n"; - std::cout << " CSR structure: " << run.backend.csrStructureElapsed - << " s\n"; - std::cout << " upload pattern: " << run.backend.uploadPatternElapsed - << " s\n"; - std::cout << " first cuDSS analyze: " - << run.backend.firstCudssAnalyzeElapsed << " s\n"; - std::cout << " download values: " << run.backend.downloadElapsed - << " s\n"; + printCudaLmSetupBreakdown(run.backend, " CUDA LM backend "); + printCudaLmTransferBreakdown(run.backend, " CUDA LM backend "); + printCudaLmDetailedBreakdown(run.backend, " CUDA LM backend "); std::cout << "Initial error: " << std::setprecision(15) << run.initialError << "\n"; std::cout << "Final error: " << run.finalError @@ -328,6 +571,10 @@ RunOptions parseBalFiles(int argc, char* argv[]) { bool cudaLmGraph = false; bool cudaLinearSolverSpecified = false; CudaLinearSolverOption cudaLinearSolver = CudaLinearSolverOption::DenseSchur; + bool cudaGraphKindSpecified = false; + CudaGraphKind cudaGraphKind = CudaGraphKind::Raw; + bool batchChunkSizeSpecified = false; + size_t batchChunkSize = 0; bool cudaWarmupFileSpecified = false; std::string cudaWarmupFile; bool benchmarkActionJson = false; @@ -368,6 +615,30 @@ RunOptions parseBalFiles(int argc, char* argv[]) { } continue; } + if (strcmp(argv[i], "--cuda-lm-graph-kind") == 0) { + if (++i >= argc || argv[i][0] == '-') { + throw runtime_error(usage()); + } + cudaGraphKindSpecified = true; + if (strcmp(argv[i], "raw") == 0) { + cudaGraphKind = CudaGraphKind::Raw; + } else if (strcmp(argv[i], "point-batch") == 0) { + cudaGraphKind = CudaGraphKind::PointBatch; + } else if (strcmp(argv[i], "camera-batch") == 0) { + cudaGraphKind = CudaGraphKind::CameraBatch; + } else { + throw runtime_error(usage()); + } + continue; + } + if (strcmp(argv[i], "--batch-chunk-size") == 0) { + if (++i >= argc || argv[i][0] == '-') { + throw runtime_error(usage()); + } + batchChunkSizeSpecified = true; + batchChunkSize = std::stoul(argv[i]); + continue; + } if (strcmp(argv[i], "--cuda-warmup-file") == 0) { if (++i >= argc || argv[i][0] == '-') { throw runtime_error(usage()); @@ -433,6 +704,14 @@ RunOptions parseBalFiles(int argc, char* argv[]) { if (cudaWarmupFileSpecified && !cudaLm && !cudaLmGraph) { throw runtime_error(usage()); } + if (cudaGraphKindSpecified && !cudaLmGraph) { + throw runtime_error("--cuda-lm-graph-kind requires --cuda-lm-graph"); + } + if (batchChunkSizeSpecified && + (!cudaLmGraph || cudaGraphKind != CudaGraphKind::PointBatch)) { + throw runtime_error( + "--batch-chunk-size only applies to --cuda-lm-graph-kind point-batch"); + } if (!filenames.empty()) { return {profile, @@ -441,6 +720,10 @@ RunOptions parseBalFiles(int argc, char* argv[]) { cudaLmGraph, cudaLinearSolverSpecified, cudaLinearSolver, + cudaGraphKindSpecified, + cudaGraphKind, + batchChunkSizeSpecified, + batchChunkSize, cudaWarmupFileSpecified, cudaWarmupFile, benchmarkActionJson, @@ -455,6 +738,10 @@ RunOptions parseBalFiles(int argc, char* argv[]) { cudaLmGraph, cudaLinearSolverSpecified, cudaLinearSolver, + cudaGraphKindSpecified, + cudaGraphKind, + batchChunkSizeSpecified, + batchChunkSize, cudaWarmupFileSpecified, cudaWarmupFile, benchmarkActionJson, @@ -469,6 +756,10 @@ RunOptions parseBalFiles(int argc, char* argv[]) { cudaLmGraph, cudaLinearSolverSpecified, cudaLinearSolver, + cudaGraphKindSpecified, + cudaGraphKind, + batchChunkSizeSpecified, + batchChunkSize, cudaWarmupFileSpecified, cudaWarmupFile, benchmarkActionJson, @@ -482,6 +773,10 @@ RunOptions parseBalFiles(int argc, char* argv[]) { cudaLmGraph, cudaLinearSolverSpecified, cudaLinearSolver, + cudaGraphKindSpecified, + cudaGraphKind, + batchChunkSizeSpecified, + batchChunkSize, cudaWarmupFileSpecified, cudaWarmupFile, benchmarkActionJson, @@ -521,6 +816,69 @@ double runSolver(const NonlinearFactorGraph& graph, const Values& initial, << "\n"; return elapsed.count(); } + +NonlinearFactorGraph buildPointBatchSfmGraph(const SfmData& db, + size_t chunkSize) { + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.numberTracks(); ++j) { + const auto& measurementsForTrack = db.tracks[j].measurements; + if (measurementsForTrack.size() < 2) continue; + + const size_t nMeasurements = measurementsForTrack.size(); + const size_t effectiveChunkSize = + (chunkSize == 0) ? nMeasurements : std::min(chunkSize, nMeasurements); + if (effectiveChunkSize == 0) continue; + + for (size_t start = 0; start < nMeasurements; start += effectiveChunkSize) { + const size_t end = std::min(start + effectiveChunkSize, nMeasurements); + std::map measurements; + for (size_t i = start; i < end; ++i) { + const SfmMeasurement& measurement = measurementsForTrack[i]; + measurements[C(measurement.first)] = measurement.second; + } + graph.add(std::make_shared>( + measurements, P(j), gNoiseModel)); + } + } + return graph; +} + +NonlinearFactorGraph buildCameraBatchSfmGraph(const SfmData& db) { + NonlinearFactorGraph graph; + std::vector> measurementsByCamera(db.numberCameras()); + + for (size_t j = 0; j < db.numberTracks(); ++j) { + const auto& measurementsForTrack = db.tracks[j].measurements; + if (measurementsForTrack.size() < 2) continue; + + for (const SfmMeasurement& measurement : measurementsForTrack) { + measurementsByCamera[measurement.first][P(j)] = measurement.second; + } + } + + for (size_t i = 0; i < measurementsByCamera.size(); ++i) { + const auto& measurements = measurementsByCamera[i]; + if (measurements.empty()) continue; + + graph.add(std::make_shared>( + C(i), measurements, gNoiseModel)); + } + return graph; +} + +NonlinearFactorGraph buildCudaGraphSfmGraph(const SfmData& db, + CudaGraphKind graphKind, + size_t batchChunkSize) { + switch (graphKind) { + case CudaGraphKind::Raw: + return buildGeneralSfmGraph(db); + case CudaGraphKind::PointBatch: + return buildPointBatchSfmGraph(db, batchChunkSize); + case CudaGraphKind::CameraBatch: + return buildCameraBatchSfmGraph(db); + } + return buildGeneralSfmGraph(db); +} } // namespace int main(int argc, char* argv[]) { @@ -595,7 +953,8 @@ int main(int argc, char* argv[]) { } #endif - NonlinearFactorGraph graph = buildGeneralSfmGraph(db); + NonlinearFactorGraph graph = buildCudaGraphSfmGraph( + db, options.cudaGraphKind, options.batchChunkSize); Values initial = buildGeneralSfmInitial(db); Ordering ordering; @@ -612,8 +971,8 @@ int main(int argc, char* argv[]) { std::cout << " CUDA graph warmup file: " << options.cudaWarmupFile << " (timing ignored)\n"; const SfmData warmupDb = SfmData::FromBalFile(options.cudaWarmupFile); - const NonlinearFactorGraph warmupGraph = - buildGeneralSfmGraph(warmupDb); + const NonlinearFactorGraph warmupGraph = buildCudaGraphSfmGraph( + warmupDb, options.cudaGraphKind, options.batchChunkSize); const Values warmupInitial = buildGeneralSfmInitial(warmupDb); const CudaGraphLmRun warmupRun = runCudaGraphLm(warmupGraph, warmupInitial, cudaParams); @@ -626,7 +985,8 @@ int main(int argc, char* argv[]) { } const CudaGraphLmRun run = runCudaGraphLm(graph, initial, cudaParams); - printCudaGraphLmRun(run, options.cudaLinearSolver); + printCudaGraphLmRun(run, options.cudaLinearSolver, + options.cudaGraphKind); continue; } #endif diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 8d3672590b..84e19706bb 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -440,8 +440,6 @@ NonlinearFactorGraph buildBatchSfmGraph(const SfmData& db, const SfmMeasurement& measurement = measurementsForTrack[i]; measurements[C(measurement.first)] = measurement.second; } - if (measurements.size() < 2) continue; - auto batch = std::make_shared>(measurements, P(j), gNoiseModel); batch->setUseHessianFactor(useHessianFactor); From 55d40643143bce45600ef14cc358df31aa3ca255 Mon Sep 17 00:00:00 2001 From: leolrg <13660505036@163.com> Date: Tue, 7 Jul 2026 00:24:35 +0000 Subject: [PATCH 2/3] Enable GncOptimizer with CUDA SFM LM as inner solver GncOptimizer> now works as a drop-in: add the equals() method GncParams expects from its base params type, and accept zero-diagonal sqrt-information in the CUDA SFM conversion, since GNC weighted graphs scale factor information by weights that TLS drives to exactly zero. Add parity tests against CPU GNC (TLS and GM classification on a synthetic BAL problem with injected outliers) and a zero-information weighted-graph regression test. Co-Authored-By: Claude Fable 5 --- gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu | 18 ++ gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h | 2 + gtsam/slam/cuda/CudaSfmProjectionBatch.h | 6 +- gtsam/slam/tests/testCudaSfm.cpp | 213 ++++++++++++++++++- 4 files changed, 233 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu index 21819a63ad..5ecd1fd739 100644 --- a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu +++ b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.cu @@ -499,6 +499,24 @@ void CudaSfmLevenbergMarquardtParams::print(const std::string& str) const { std::cout << " linearSolver: " << getLinearSolver() << "\n"; } +bool CudaSfmLevenbergMarquardtParams::equals( + const CudaSfmLevenbergMarquardtParams& other, double tol) const { + return maxIterations == other.maxIterations && + std::abs(lambdaInitial - other.lambdaInitial) <= tol && + std::abs(lambdaFactor - other.lambdaFactor) <= tol && + std::abs(lambdaUpperBound - other.lambdaUpperBound) <= tol && + std::abs(lambdaLowerBound - other.lambdaLowerBound) <= tol && + std::abs(relativeErrorTol - other.relativeErrorTol) <= tol && + std::abs(absoluteErrorTol - other.absoluteErrorTol) <= tol && + std::abs(errorTol - other.errorTol) <= tol && + std::abs(minModelFidelity - other.minModelFidelity) <= tol && + useFixedLambdaFactor == other.useFixedLambdaFactor && + diagonalDamping == other.diagonalDamping && + std::abs(minDiagonal - other.minDiagonal) <= tol && + std::abs(maxDiagonal - other.maxDiagonal) <= tol && + linearSolver == other.linearSolver; +} + CudaSfmFactorGraphData ConvertGeneralSfmGraphToCudaSfmData( const NonlinearFactorGraph& graph, const Values& initialValues) { CudaSfmFactorGraphData converted; diff --git a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h index eb3c9650a2..be160e7f33 100644 --- a/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h +++ b/gtsam/slam/cuda/CudaSfmLevenbergMarquardt.h @@ -47,6 +47,8 @@ class GTSAM_EXPORT CudaSfmLevenbergMarquardtParams { std::string getLinearSolver() const; void setLinearSolver(const std::string& solver); void print(const std::string& str = "") const; + bool equals(const CudaSfmLevenbergMarquardtParams& other, + double tol = 1e-9) const; }; struct CudaSfmLmAttemptProfile { diff --git a/gtsam/slam/cuda/CudaSfmProjectionBatch.h b/gtsam/slam/cuda/CudaSfmProjectionBatch.h index 1ef36c404c..2246651225 100644 --- a/gtsam/slam/cuda/CudaSfmProjectionBatch.h +++ b/gtsam/slam/cuda/CudaSfmProjectionBatch.h @@ -69,10 +69,12 @@ class CudaSfmProjectionBatch { sqrtInfo.r11 == 1.0; } + // Zero diagonals are allowed: they encode a zero-information (fully + // down-weighted) measurement, as produced by GNC weighted graphs. static bool IsUsableSqrtInfo(const CudaSfmSqrtInfo2& sqrtInfo) { return std::isfinite(sqrtInfo.r00) && std::isfinite(sqrtInfo.r01) && - std::isfinite(sqrtInfo.r11) && sqrtInfo.r00 > 0.0 && - sqrtInfo.r11 > 0.0; + std::isfinite(sqrtInfo.r11) && sqrtInfo.r00 >= 0.0 && + sqrtInfo.r11 >= 0.0; } static bool IsUsableRobustModel(const CudaSfmRobustModel& robustModel) { diff --git a/gtsam/slam/tests/testCudaSfm.cpp b/gtsam/slam/tests/testCudaSfm.cpp index c7030fa873..a5ea0eda7e 100644 --- a/gtsam/slam/tests/testCudaSfm.cpp +++ b/gtsam/slam/tests/testCudaSfm.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -17,11 +18,13 @@ #include +#include #include #include #include #include #include +#include #include using namespace gtsam; @@ -1656,10 +1659,6 @@ TEST(CudaSfmFactorGraphConversion, RejectsUnsupportedNoiseModels) { nonzeroLowerLeftR << 1.0, 0.0, 1e-3, 1.0; checkRejectedSqrtInformation(nonzeroLowerLeftR); - Matrix2 zeroDiagonalR; - zeroDiagonalR << 0.0, 0.0, 0.0, 1.0; - checkRejectedSqrtInformation(zeroDiagonalR); - Matrix2 negativeDiagonalR; negativeDiagonalR << 1.0, 0.0, 0.0, -1.0; checkRejectedSqrtInformation(negativeDiagonalR); @@ -2297,6 +2296,212 @@ TEST(CudaBalCsrStructure, BuildsUpperTrianglePatternForMeasuredTrack) { } } +namespace { + +// Synthetic BAL-like problem for GNC: every point is observed by every +// camera, so a track stays well constrained even after GNC down-weights its +// corrupted measurements to zero. +struct GncTestProblem { + NonlinearFactorGraph graph; + NonlinearFactorGraph inlierGraph; + Values initial; + std::vector outlierFactorSlots; + + bool isOutlierSlot(size_t slot) const { + return std::find(outlierFactorSlots.begin(), outlierFactorSlots.end(), + slot) != outlierFactorSlots.end(); + } +}; + +GncTestProblem makeGncBalLikeProblem() { + // The geometry must be rigid (diverse viewpoints, many points) and the + // corruptions moderate: with a weakly constrained problem or extreme + // outliers, plain LM can absorb the corrupted measurements into a + // consistent (wrong) solution with near-zero residuals, and GNC has no + // signal left to reject them. + constexpr size_t kNumCameras = 5; + std::vector points; + for (size_t j = 0; j < 20; ++j) { + const double a = 2.399963 * static_cast(j); // golden angle + const double r = 0.3 + 0.08 * static_cast(j % 7); + points.emplace_back(r * std::cos(a), r * std::sin(a), + 4.0 + 0.37 * static_cast((j * 5) % 9)); + } + + std::vector cameras; + const std::vector centers = { + Point3(-1.5, 0.3, -0.4), Point3(-0.7, -0.9, 0.3), Point3(0.1, 0.8, -0.2), + Point3(0.9, -0.4, 0.5), Point3(1.6, 0.6, -0.3)}; + for (size_t i = 0; i < kNumCameras; ++i) { + const double s = static_cast(i); + cameras.emplace_back( + Pose3(Rot3::RzRyRx(0.15 - 0.08 * s, 0.25 - 0.12 * s, 0.05 * s), + centers[i]), + Cal3Bundler(160.0 + 4.0 * s, 1e-4, -1e-6)); + } + + // Corrupted (pointSlot, cameraSlot) pairs; each corrupted track keeps + // four clean views. + const std::vector> corrupted = { + {1, 0}, {7, 2}, {14, 4}}; + const std::vector corruptions = { + Point2(14.0, -10.0), Point2(-12.0, 9.0), Point2(10.0, 13.0)}; + + GncTestProblem problem; + const auto model = noiseModel::Unit::Create(2); + for (size_t pointSlot = 0; pointSlot < points.size(); ++pointSlot) { + for (size_t cameraSlot = 0; cameraSlot < kNumCameras; ++cameraSlot) { + Point2 measured = cameras[cameraSlot].project2(points[pointSlot]); + const bool isOutlier = + std::find(corrupted.begin(), corrupted.end(), + std::make_pair(pointSlot, cameraSlot)) != corrupted.end(); + if (isOutlier) { + measured += corruptions[problem.outlierFactorSlots.size()]; + problem.outlierFactorSlots.push_back(problem.graph.size()); + } + auto factor = std::make_shared( + measured, model, C(cameraSlot), P(pointSlot)); + problem.graph.push_back(factor); + if (!isOutlier) { + problem.inlierGraph.push_back(factor); + } + } + } + + for (size_t i = 0; i < kNumCameras; ++i) { + const double sign = (i % 2 == 0) ? 1.0 : -1.0; + Vector delta(9); + delta << 0.002 * sign, -0.0015, 0.001 * sign, 0.03, -0.02 * sign, 0.025, + 0.8 * sign, 1e-5, -1e-7; + problem.initial.insert(C(i), cameras[i].retract(delta)); + } + for (size_t j = 0; j < points.size(); ++j) { + const double sign = (j % 2 == 0) ? 1.0 : -1.0; + problem.initial.insert( + P(j), Point3(points[j].x() + 0.02 * sign, points[j].y() - 0.015, + points[j].z() + 0.03 * sign)); + } + return problem; +} + +} // namespace + +TEST(CudaSfmLevenbergMarquardtParams, EqualsComparesFields) { + const CudaSfmLevenbergMarquardtParams a = + CudaSfmLevenbergMarquardtParams::LegacyDefaults(); + CudaSfmLevenbergMarquardtParams b = a; + CHECK(a.equals(b)); + + b.lambdaInitial = 2.0 * a.lambdaInitial + 1.0; + CHECK(!a.equals(b)); + + b = a; + b.linearSolver = CudaSfmLinearSolverType::CudssFullNormal; + CHECK(!a.equals(b)); +} + +TEST(GncCudaSfmOptimizer, TlsClassificationMatchesCpuGnc) { + const GncTestProblem problem = makeGncBalLikeProblem(); + + GncParams cpuGncParams{LevenbergMarquardtParams()}; + cpuGncParams.setLossType(GncLossType::TLS); + GncOptimizer> cpuGnc( + problem.graph, problem.initial, cpuGncParams); + const Values cpuResult = cpuGnc.optimize(); + + GncParams cudaGncParams{ + CudaSfmLevenbergMarquardtParams::LegacyDefaults()}; + cudaGncParams.setLossType(GncLossType::TLS); + GncOptimizer> cudaGnc( + problem.graph, problem.initial, cudaGncParams); + const Values cudaResult = cudaGnc.optimize(); + + const Vector& cpuWeights = cpuGnc.getWeights(); + const Vector& cudaWeights = cudaGnc.getWeights(); + EXPECT_LONGS_EQUAL(problem.graph.size(), cudaWeights.size()); + for (size_t slot = 0; slot < problem.graph.size(); ++slot) { + if (problem.isOutlierSlot(slot)) { + CHECK(cpuWeights[slot] < 0.05); + CHECK(cudaWeights[slot] < 0.05); + } else { + CHECK(cpuWeights[slot] > 0.95); + CHECK(cudaWeights[slot] > 0.95); + } + } + + const double initialInlierError = problem.inlierGraph.error(problem.initial); + const double cpuInlierError = problem.inlierGraph.error(cpuResult); + const double cudaInlierError = problem.inlierGraph.error(cudaResult); + CHECK(cpuInlierError < 1e-3); + CHECK(cudaInlierError < 1e-3); + CHECK(cudaInlierError < initialInlierError); +} + +TEST(GncCudaSfmOptimizer, GmClassificationMatchesCpuGnc) { + const GncTestProblem problem = makeGncBalLikeProblem(); + + GncParams cpuGncParams{LevenbergMarquardtParams()}; + cpuGncParams.setLossType(GncLossType::GM); + GncOptimizer> cpuGnc( + problem.graph, problem.initial, cpuGncParams); + const Values cpuResult = cpuGnc.optimize(); + + GncParams cudaGncParams{ + CudaSfmLevenbergMarquardtParams::LegacyDefaults()}; + cudaGncParams.setLossType(GncLossType::GM); + GncOptimizer> cudaGnc( + problem.graph, problem.initial, cudaGncParams); + const Values cudaResult = cudaGnc.optimize(); + + // GM weights do not converge to exactly {0, 1}; check the separation. + const Vector& cpuWeights = cpuGnc.getWeights(); + const Vector& cudaWeights = cudaGnc.getWeights(); + for (size_t slot = 0; slot < problem.graph.size(); ++slot) { + if (problem.isOutlierSlot(slot)) { + CHECK(cpuWeights[slot] < 0.5); + CHECK(cudaWeights[slot] < 0.5); + } else { + CHECK(cpuWeights[slot] > 0.9); + CHECK(cudaWeights[slot] > 0.9); + } + } + + CHECK(problem.inlierGraph.error(cpuResult) < 1e-2); + CHECK(problem.inlierGraph.error(cudaResult) < 1e-2); +} + +TEST(GncCudaSfmOptimizer, KnownOutliersProduceZeroInformationGraph) { + // GNC weights a known outlier by zero, which reaches the CUDA backend as a + // zero-information Gaussian noise model. The corrupted measurement must be + // ignored by the optimization. + const GncTestProblem problem = makeGncBalLikeProblem(); + + NonlinearFactorGraph weightedGraph; + for (size_t slot = 0; slot < problem.graph.size(); ++slot) { + const auto factor = + std::static_pointer_cast(problem.graph[slot]); + if (problem.isOutlierSlot(slot)) { + // Same construction as GncOptimizer::makeWeightedGraph with weight 0. + const auto zeroInformation = + noiseModel::Gaussian::Information(Matrix2::Zero()); + weightedGraph.push_back(factor->cloneWithNewNoiseModel(zeroInformation)); + } else { + weightedGraph.push_back(factor); + } + } + + CudaSfmLevenbergMarquardtParams params = + CudaSfmLevenbergMarquardtParams::LegacyDefaults(); + CudaSfmLevenbergMarquardtOptimizer optimizer(weightedGraph, problem.initial, + params); + const Values& result = optimizer.optimize(); + + CHECK(problem.inlierGraph.error(result) < 1e-3); + // The zero-information factors contribute exactly zero error. + DOUBLES_EQUAL(problem.inlierGraph.error(result), + weightedGraph.error(result), 1e-9); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 930e58706ad27f5704bab5fc5cce89575e9fcd83 Mon Sep 17 00:00:00 2001 From: leolrg <13660505036@163.com> Date: Tue, 7 Jul 2026 00:24:47 +0000 Subject: [PATCH 3/3] Add GNC benchmark mode to timeCudaSFMBAL Instrument GncOptimizer::optimize() with per-outer-iteration stage timings (weight update, weighted graph rebuild, base optimize, cost evaluation), exposed via getTiming(). Add --gnc cpu|cuda to timeCudaSFMBAL with deterministic outlier injection (--gnc-outlier-fraction/-pixels/-seed, corrupting only tracks that keep >= 2 clean views), loss selection, per-iteration timing output, and outlier-classification precision/recall plus inlier-only RMS reprojection error against the injected ground truth. On dubrovnik 16/88/135 with 10% outliers at 12 px (TLS), CUDA GNC is 18-22x faster than CPU GNC at matched classification quality; >95% of remaining CUDA GNC time is host-side graph rebuild and per-iteration re-upload, motivating a device-resident GNC loop. Co-Authored-By: Claude Fable 5 --- gtsam/nonlinear/GncOptimizer.h | 66 ++++ timing/sfm_ba/timeCudaSFMBAL.cpp | 600 +++++++++++++++++++++++-------- 2 files changed, 514 insertions(+), 152 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 18ce5a67e8..953a025fad 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -27,6 +27,7 @@ #pragma once #include +#include #include #include @@ -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 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 GncOptimizer { @@ -86,6 +124,9 @@ class GncOptimizer { /// Cached factor types for GNC. std::vector 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(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; } diff --git a/timing/sfm_ba/timeCudaSFMBAL.cpp b/timing/sfm_ba/timeCudaSFMBAL.cpp index d1a39f731a..c59f69bf0f 100644 --- a/timing/sfm_ba/timeCudaSFMBAL.cpp +++ b/timing/sfm_ba/timeCudaSFMBAL.cpp @@ -28,13 +28,18 @@ #include #endif +#include + +#include #include +#include #include #include #include #include #include #include +#include namespace { constexpr const char* kDefaultBenchmarkDataset = "dubrovnik-16-22106-pre"; @@ -51,6 +56,9 @@ std::string usage() { "[--batch-chunk-size N] " "[--cuda-warmup-file FILE] " "[--projection-noise unit|huber|tukey] " + "[--gnc cpu|cuda] [--gnc-loss tls|gm] " + "[--gnc-outlier-fraction F] [--gnc-outlier-pixels P] " + "[--gnc-seed N] [--gnc-max-outer N] " "[--benchmark-action-json FILE] [BALfile ...]"; } @@ -71,6 +79,21 @@ enum class CudaGraphKind { CameraBatch, }; +enum class GncBackend { + None, + Cpu, + Cuda, +}; + +struct GncRunOptions { + GncBackend backend = GncBackend::None; + GncLossType lossType = GncLossType::TLS; + double outlierFraction = 0.1; + double outlierPixels = 12.0; + unsigned int seed = 42; + size_t maxOuterIterations = 100; +}; + struct RunOptions { bool profile = false; bool cudaStructureOnly = false; @@ -86,6 +109,7 @@ struct RunOptions { std::string cudaWarmupFile; bool benchmarkActionJson = false; std::string benchmarkActionJsonPath; + GncRunOptions gnc; std::vector filenames; }; @@ -140,6 +164,16 @@ LevenbergMarquardtParams makeBalLevenbergMarquardtParams() { return params; } +void printProfileRow(const std::string& indent, const std::string& label, + double elapsed, double total) { + std::cout << indent << std::left << std::setw(30) << label << std::right + << elapsed << " s"; + if (total > 0.0) { + std::cout << " (" << (100.0 * elapsed / total) << "%)"; + } + std::cout << "\n"; +} + #if GTSAM_ENABLE_CUDA && GTSAM_ENABLE_CUDSS enum class CudaLmDefaults { Backend, @@ -189,16 +223,6 @@ CudaBackendLmRun runCudaBackendLm( const char* yesNo(bool value) { return value ? "yes" : "no"; } -void printProfileRow(const std::string& indent, const std::string& label, - double elapsed, double total) { - std::cout << indent << std::left << std::setw(30) << label << std::right - << elapsed << " s"; - if (total > 0.0) { - std::cout << " (" << (100.0 * elapsed / total) << "%)"; - } - std::cout << "\n"; -} - void printTransferRow(const std::string& indent, const std::string& label, double elapsed, size_t bytes, double total) { std::cout << indent << std::left << std::setw(30) << label << std::right @@ -564,228 +588,211 @@ void writeBenchmarkActionJson(const std::vector& rows, } RunOptions parseBalFiles(int argc, char* argv[]) { - std::vector filenames; - bool profile = false; - bool cudaStructureOnly = false; - bool cudaLm = false; - bool cudaLmGraph = false; - bool cudaLinearSolverSpecified = false; - CudaLinearSolverOption cudaLinearSolver = CudaLinearSolverOption::DenseSchur; - bool cudaGraphKindSpecified = false; - CudaGraphKind cudaGraphKind = CudaGraphKind::Raw; - bool batchChunkSizeSpecified = false; - size_t batchChunkSize = 0; - bool cudaWarmupFileSpecified = false; - std::string cudaWarmupFile; - bool benchmarkActionJson = false; - std::string benchmarkActionJsonPath; + RunOptions options; bool projectionNoiseSpecified = false; + bool gncOptionSpecified = false; + const auto nextArg = [&](int& i) -> const char* { + if (++i >= argc || argv[i][0] == '-') { + throw runtime_error(usage()); + } + return argv[i]; + }; for (int i = 1; i < argc; ++i) { if (strcmp(argv[i], "--colamd") == 0) { gUseSchur = false; continue; } if (strcmp(argv[i], "--profile") == 0) { - profile = true; + options.profile = true; continue; } if (strcmp(argv[i], "--cuda-structure-only") == 0) { - cudaStructureOnly = true; + options.cudaStructureOnly = true; continue; } if (strcmp(argv[i], "--cuda-lm") == 0) { - cudaLm = true; + options.cudaLm = true; continue; } if (strcmp(argv[i], "--cuda-lm-graph") == 0) { - cudaLmGraph = true; + options.cudaLmGraph = true; continue; } if (strcmp(argv[i], "--cuda-linear-solver") == 0) { - if (++i >= argc || argv[i][0] == '-') { - throw runtime_error(usage()); - } - cudaLinearSolverSpecified = true; - if (strcmp(argv[i], "dense-schur") == 0) { - cudaLinearSolver = CudaLinearSolverOption::DenseSchur; - } else if (strcmp(argv[i], "cudss-full-normal") == 0) { - cudaLinearSolver = CudaLinearSolverOption::CudssFullNormal; + const char* value = nextArg(i); + options.cudaLinearSolverSpecified = true; + if (strcmp(value, "dense-schur") == 0) { + options.cudaLinearSolver = CudaLinearSolverOption::DenseSchur; + } else if (strcmp(value, "cudss-full-normal") == 0) { + options.cudaLinearSolver = CudaLinearSolverOption::CudssFullNormal; } else { throw runtime_error(usage()); } continue; } if (strcmp(argv[i], "--cuda-lm-graph-kind") == 0) { - if (++i >= argc || argv[i][0] == '-') { - throw runtime_error(usage()); - } - cudaGraphKindSpecified = true; - if (strcmp(argv[i], "raw") == 0) { - cudaGraphKind = CudaGraphKind::Raw; - } else if (strcmp(argv[i], "point-batch") == 0) { - cudaGraphKind = CudaGraphKind::PointBatch; - } else if (strcmp(argv[i], "camera-batch") == 0) { - cudaGraphKind = CudaGraphKind::CameraBatch; + const char* value = nextArg(i); + options.cudaGraphKindSpecified = true; + if (strcmp(value, "raw") == 0) { + options.cudaGraphKind = CudaGraphKind::Raw; + } else if (strcmp(value, "point-batch") == 0) { + options.cudaGraphKind = CudaGraphKind::PointBatch; + } else if (strcmp(value, "camera-batch") == 0) { + options.cudaGraphKind = CudaGraphKind::CameraBatch; } else { throw runtime_error(usage()); } continue; } if (strcmp(argv[i], "--batch-chunk-size") == 0) { - if (++i >= argc || argv[i][0] == '-') { - throw runtime_error(usage()); - } - batchChunkSizeSpecified = true; - batchChunkSize = std::stoul(argv[i]); + options.batchChunkSizeSpecified = true; + options.batchChunkSize = std::stoul(nextArg(i)); continue; } if (strcmp(argv[i], "--cuda-warmup-file") == 0) { - if (++i >= argc || argv[i][0] == '-') { - throw runtime_error(usage()); - } - cudaWarmupFileSpecified = true; - cudaWarmupFile = argv[i]; + options.cudaWarmupFileSpecified = true; + options.cudaWarmupFile = nextArg(i); continue; } if (strcmp(argv[i], "--projection-noise") == 0) { - if (++i >= argc || argv[i][0] == '-') { + projectionNoiseSpecified = true; + setProjectionNoiseModel(nextArg(i)); + continue; + } + if (strcmp(argv[i], "--gnc") == 0) { + const char* value = nextArg(i); + if (strcmp(value, "cpu") == 0) { + options.gnc.backend = GncBackend::Cpu; + } else if (strcmp(value, "cuda") == 0) { + options.gnc.backend = GncBackend::Cuda; + } else { throw runtime_error(usage()); } - projectionNoiseSpecified = true; - setProjectionNoiseModel(argv[i]); continue; } - if (strcmp(argv[i], "--benchmark-action-json") == 0) { - if (++i >= argc || argv[i][0] == '-') { + if (strcmp(argv[i], "--gnc-loss") == 0) { + const char* value = nextArg(i); + gncOptionSpecified = true; + if (strcmp(value, "tls") == 0) { + options.gnc.lossType = GncLossType::TLS; + } else if (strcmp(value, "gm") == 0) { + options.gnc.lossType = GncLossType::GM; + } else { throw runtime_error(usage()); } - benchmarkActionJson = true; - benchmarkActionJsonPath = argv[i]; + continue; + } + if (strcmp(argv[i], "--gnc-outlier-fraction") == 0) { + gncOptionSpecified = true; + options.gnc.outlierFraction = std::stod(nextArg(i)); + if (options.gnc.outlierFraction < 0.0 || + options.gnc.outlierFraction >= 1.0) { + throw runtime_error("--gnc-outlier-fraction must be in [0, 1)"); + } + continue; + } + if (strcmp(argv[i], "--gnc-outlier-pixels") == 0) { + gncOptionSpecified = true; + options.gnc.outlierPixels = std::stod(nextArg(i)); + continue; + } + if (strcmp(argv[i], "--gnc-seed") == 0) { + gncOptionSpecified = true; + options.gnc.seed = + static_cast(std::stoul(nextArg(i))); + continue; + } + if (strcmp(argv[i], "--gnc-max-outer") == 0) { + gncOptionSpecified = true; + options.gnc.maxOuterIterations = std::stoul(nextArg(i)); + continue; + } + if (strcmp(argv[i], "--benchmark-action-json") == 0) { + options.benchmarkActionJson = true; + options.benchmarkActionJsonPath = nextArg(i); continue; } if (argv[i][0] == '-') { throw runtime_error(usage()); } - filenames.emplace_back(argv[i]); + options.filenames.emplace_back(argv[i]); } - if (profile && !filenames.empty()) { + const bool gnc = options.gnc.backend != GncBackend::None; + if (options.profile && !options.filenames.empty()) { throw runtime_error(usage()); } - if (profile && benchmarkActionJson) { + if (options.profile && options.benchmarkActionJson) { throw runtime_error(usage()); } - if (cudaStructureOnly && benchmarkActionJson) { + if (options.cudaStructureOnly && options.benchmarkActionJson) { throw runtime_error(usage()); } - if (cudaLm && benchmarkActionJson) { + if (options.cudaLm && options.benchmarkActionJson) { throw runtime_error(usage()); } - if (cudaLmGraph && benchmarkActionJson) { + if (options.cudaLmGraph && options.benchmarkActionJson) { throw runtime_error(usage()); } - if (cudaLm && cudaStructureOnly) { + if (options.cudaLm && options.cudaStructureOnly) { throw runtime_error(usage()); } - if (cudaLmGraph && cudaStructureOnly) { + if (options.cudaLmGraph && options.cudaStructureOnly) { throw runtime_error(usage()); } - if (cudaLm && cudaLmGraph) { + if (options.cudaLm && options.cudaLmGraph) { throw runtime_error(usage()); } - if (projectionNoiseSpecified && cudaLm) { + if (gnc && (options.cudaLm || options.cudaLmGraph || + options.cudaStructureOnly || options.profile || + options.benchmarkActionJson)) { + throw runtime_error("--gnc cannot be combined with other run modes"); + } + if (gncOptionSpecified && !gnc) { + throw runtime_error("--gnc-* options require --gnc cpu|cuda"); + } + if (gnc && projectionNoiseSpecified) { + throw runtime_error( + "--projection-noise cannot be combined with --gnc: GNC replaces " + "robust noise with its own weighting"); + } + if (projectionNoiseSpecified && options.cudaLm) { throw runtime_error( "--projection-noise only applies to factor-graph runs; use " "--cuda-lm-graph for CUDA robust-noise benchmarking"); } - if (cudaLinearSolverSpecified && !cudaLm && !cudaLmGraph) { + if (options.cudaLinearSolverSpecified && !options.cudaLm && + !options.cudaLmGraph && options.gnc.backend != GncBackend::Cuda) { throw runtime_error(usage()); } - if (cudaWarmupFileSpecified && !cudaLm && !cudaLmGraph) { + if (options.cudaWarmupFileSpecified && !options.cudaLm && + !options.cudaLmGraph && !gnc) { throw runtime_error(usage()); } - if (cudaGraphKindSpecified && !cudaLmGraph) { + if (options.cudaGraphKindSpecified && !options.cudaLmGraph) { throw runtime_error("--cuda-lm-graph-kind requires --cuda-lm-graph"); } - if (batchChunkSizeSpecified && - (!cudaLmGraph || cudaGraphKind != CudaGraphKind::PointBatch)) { + if (options.batchChunkSizeSpecified && + (!options.cudaLmGraph || + options.cudaGraphKind != CudaGraphKind::PointBatch)) { throw runtime_error( "--batch-chunk-size only applies to --cuda-lm-graph-kind point-batch"); } - if (!filenames.empty()) { - return {profile, - cudaStructureOnly, - cudaLm, - cudaLmGraph, - cudaLinearSolverSpecified, - cudaLinearSolver, - cudaGraphKindSpecified, - cudaGraphKind, - batchChunkSizeSpecified, - batchChunkSize, - cudaWarmupFileSpecified, - cudaWarmupFile, - benchmarkActionJson, - benchmarkActionJsonPath, - filenames}; - } - - if (profile) { - return {profile, - cudaStructureOnly, - cudaLm, - cudaLmGraph, - cudaLinearSolverSpecified, - cudaLinearSolver, - cudaGraphKindSpecified, - cudaGraphKind, - batchChunkSizeSpecified, - batchChunkSize, - cudaWarmupFileSpecified, - cudaWarmupFile, - benchmarkActionJson, - benchmarkActionJsonPath, - {findExampleDataFile(kProfileDataset)}}; - } - - if (benchmarkActionJson) { - return {profile, - cudaStructureOnly, - cudaLm, - cudaLmGraph, - cudaLinearSolverSpecified, - cudaLinearSolver, - cudaGraphKindSpecified, - cudaGraphKind, - batchChunkSizeSpecified, - batchChunkSize, - cudaWarmupFileSpecified, - cudaWarmupFile, - benchmarkActionJson, - benchmarkActionJsonPath, - {findExampleDataFile(kDefaultBenchmarkDataset)}}; + if (options.filenames.empty()) { + if (options.profile) { + options.filenames = {findExampleDataFile(kProfileDataset)}; + } else if (options.benchmarkActionJson || gnc) { + options.filenames = {findExampleDataFile(kDefaultBenchmarkDataset)}; + } else { + options.filenames = { + findExampleDataFile("dubrovnik-16-22106-pre"), + findExampleDataFile("dubrovnik-88-64298-pre"), + findExampleDataFile("dubrovnik-135-90642-pre"), + }; + } } - - return {profile, - cudaStructureOnly, - cudaLm, - cudaLmGraph, - cudaLinearSolverSpecified, - cudaLinearSolver, - cudaGraphKindSpecified, - cudaGraphKind, - batchChunkSizeSpecified, - batchChunkSize, - cudaWarmupFileSpecified, - cudaWarmupFile, - benchmarkActionJson, - benchmarkActionJsonPath, - { - findExampleDataFile("dubrovnik-16-22106-pre"), - findExampleDataFile("dubrovnik-88-64298-pre"), - findExampleDataFile("dubrovnik-135-90642-pre"), - }}; + return options; } double runSolver(const NonlinearFactorGraph& graph, const Values& initial, @@ -879,6 +886,241 @@ NonlinearFactorGraph buildCudaGraphSfmGraph(const SfmData& db, } return buildGeneralSfmGraph(db); } + +/* ************************************************************************* */ +// GNC benchmarking + +const char* gncBackendName(GncBackend backend) { + switch (backend) { + case GncBackend::Cpu: + return "cpu"; + case GncBackend::Cuda: + return "cuda"; + case GncBackend::None: + break; + } + return "none"; +} + +const char* gncLossName(GncLossType lossType) { + return lossType == GncLossType::TLS ? "tls" : "gm"; +} + +// A BAL problem with a fraction of measurements corrupted by a fixed-magnitude +// offset in a random direction. Factor slot i of the graph corresponds to +// isOutlier[i]. +struct CorruptedBalProblem { + SfmData data; // corrupted copy + std::vector isOutlier; + size_t outlierCount = 0; +}; + +// Corrupt a deterministic random subset of measurements. Only tracks with +// > 2 measurements are eligible so that every corrupted track keeps at least +// two clean views and remains constrained when GNC removes the outlier. +CorruptedBalProblem corruptBalMeasurements(const SfmData& db, + double outlierFraction, + double outlierPixels, + unsigned int seed) { + CorruptedBalProblem problem; + problem.data = db; + + // Global factor slots follow buildGeneralSfmGraph order: tracks with < 2 + // measurements are skipped entirely. + std::vector> eligible; // (trackIndex, measIndex) + size_t numFactors = 0; + for (size_t j = 0; j < db.numberTracks(); ++j) { + const size_t n = db.tracks[j].measurements.size(); + if (n < 2) continue; + numFactors += n; + if (n <= 2) continue; + // Leave at least two clean measurements per track. + const size_t maxCorruptible = n - 2; + for (size_t m = 0; m < maxCorruptible; ++m) { + eligible.emplace_back(j, m); + } + } + problem.isOutlier.assign(numFactors, false); + + const size_t requested = static_cast( + std::llround(outlierFraction * static_cast(numFactors))); + const size_t numOutliers = std::min(requested, eligible.size()); + + std::mt19937 rng(seed); + std::shuffle(eligible.begin(), eligible.end(), rng); + std::uniform_real_distribution angle(0.0, 2.0 * M_PI); + + // Corrupt the chosen measurements in the SfmData copy. + std::vector> corruptByTrack(db.numberTracks()); + for (size_t k = 0; k < numOutliers; ++k) { + const auto [trackIndex, measIndex] = eligible[k]; + SfmMeasurement& measurement = + problem.data.tracks[trackIndex].measurements[measIndex]; + const double a = angle(rng); + measurement.second += + Point2(outlierPixels * std::cos(a), outlierPixels * std::sin(a)); + if (corruptByTrack[trackIndex].empty()) { + corruptByTrack[trackIndex].assign( + db.tracks[trackIndex].measurements.size(), false); + } + corruptByTrack[trackIndex][measIndex] = true; + } + problem.outlierCount = numOutliers; + + // Map (track, measurement) corruption flags to global factor slots. + size_t slot = 0; + for (size_t j = 0; j < db.numberTracks(); ++j) { + const size_t n = db.tracks[j].measurements.size(); + if (n < 2) continue; + for (size_t m = 0; m < n; ++m, ++slot) { + if (!corruptByTrack[j].empty() && corruptByTrack[j][m]) { + problem.isOutlier[slot] = true; + } + } + } + return problem; +} + +struct GncClassificationMetrics { + size_t truePositives = 0; // outlier classified as outlier + size_t falsePositives = 0; // inlier classified as outlier + size_t falseNegatives = 0; // outlier classified as inlier + double minInlierWeight = 1.0; + double maxOutlierWeight = 0.0; + + double precision() const { + const size_t denominator = truePositives + falsePositives; + return denominator == 0 + ? 1.0 + : static_cast(truePositives) / denominator; + } + double recall() const { + const size_t denominator = truePositives + falseNegatives; + return denominator == 0 + ? 1.0 + : static_cast(truePositives) / denominator; + } +}; + +GncClassificationMetrics classifyGncWeights( + const Vector& weights, const std::vector& isOutlier) { + GncClassificationMetrics metrics; + for (size_t i = 0; i < isOutlier.size(); ++i) { + const double w = weights[i]; + const bool classifiedOutlier = w < 0.5; + if (isOutlier[i]) { + metrics.maxOutlierWeight = std::max(metrics.maxOutlierWeight, w); + if (classifiedOutlier) { + ++metrics.truePositives; + } else { + ++metrics.falseNegatives; + } + } else { + metrics.minInlierWeight = std::min(metrics.minInlierWeight, w); + if (classifiedOutlier) { + ++metrics.falsePositives; + } + } + } + return metrics; +} + +// Inlier-only RMS reprojection error (pixels) of the solution. +double inlierRmsReprojectionError(const NonlinearFactorGraph& graph, + const std::vector& isOutlier, + const Values& solution) { + double sumSquared = 0.0; + size_t count = 0; + for (size_t i = 0; i < graph.size(); ++i) { + if (isOutlier[i]) continue; + // error() is 0.5 * ||r||^2 for the unit-noise BAL factors. + sumSquared += 2.0 * graph.at(i)->error(solution); + ++count; + } + return count == 0 ? 0.0 : std::sqrt(sumSquared / static_cast(count)); +} + +struct GncBenchmarkRun { + double elapsed = 0.0; + GncTiming timing; + Vector weights; + Values solution; + size_t outerIterations = 0; +}; + +template +GncBenchmarkRun runGncBenchmark(const NonlinearFactorGraph& graph, + const Values& initial, + const BaseParams& baseParams, + const GncRunOptions& gncOptions) { + GncParams gncParams(baseParams); + gncParams.setLossType(gncOptions.lossType); + if (gncOptions.maxOuterIterations != gncParams.maxIterations) { + gncParams.maxIterations = gncOptions.maxOuterIterations; + } + + const auto start = std::chrono::high_resolution_clock::now(); + GncOptimizer> gnc(graph, initial, gncParams); + GncBenchmarkRun run; + run.solution = gnc.optimize(); + const auto end = std::chrono::high_resolution_clock::now(); + + run.elapsed = std::chrono::duration(end - start).count(); + run.timing = gnc.getTiming(); + run.weights = gnc.getWeights(); + run.outerIterations = run.timing.iterations.size(); + return run; +} + +void printGncRun(const GncBenchmarkRun& run, + const CorruptedBalProblem& problem, + const NonlinearFactorGraph& graph, + const GncRunOptions& gncOptions) { + const GncTiming& timing = run.timing; + std::cout << " GNC (" << gncBackendName(gncOptions.backend) + << ", " << gncLossName(gncOptions.lossType) + << "): " << run.elapsed << " s\n"; + std::cout << " GNC outer iterations: " << run.outerIterations + << " (+1 initial optimize)\n"; + std::cout << " GNC timing breakdown:\n"; + printProfileRow(" ", "initial optimize", timing.initialOptimizeElapsed, + timing.totalElapsed); + printProfileRow(" ", "weights update (factor errors)", + timing.sumWeightsUpdate(), timing.totalElapsed); + printProfileRow(" ", "weighted graph rebuild", timing.sumMakeGraph(), + timing.totalElapsed); + printProfileRow(" ", "base optimize calls", timing.sumBaseOptimize(), + timing.totalElapsed); + printProfileRow(" ", "cost evaluation", timing.sumCostEvaluation(), + timing.totalElapsed); + std::cout << " GNC per-outer-iteration timings:\n"; + for (size_t i = 0; i < timing.iterations.size(); ++i) { + const GncIterationTiming& it = timing.iterations[i]; + std::cout << " outer " << i << ": total " << it.totalElapsed + << " s (weights " << it.weightsUpdateElapsed << ", graph " + << it.makeGraphElapsed << ", optimize " + << it.baseOptimizeElapsed << ", cost " + << it.costEvaluationElapsed << ")\n"; + } + + const GncClassificationMetrics metrics = + classifyGncWeights(run.weights, problem.isOutlier); + std::cout << " GNC injected outliers: " << problem.outlierCount << " / " + << problem.isOutlier.size() << " measurements (" + << gncOptions.outlierPixels << " px, seed " << gncOptions.seed + << ")\n"; + std::cout << " GNC classification: precision " << metrics.precision() + << ", recall " << metrics.recall() << " (TP " + << metrics.truePositives << ", FP " << metrics.falsePositives + << ", FN " << metrics.falseNegatives << ")\n"; + std::cout << " GNC weight separation: min inlier weight " + << metrics.minInlierWeight << ", max outlier weight " + << metrics.maxOutlierWeight << "\n"; + std::cout << "Inlier RMS reprojection error: " << std::setprecision(15) + << inlierRmsReprojectionError(graph, problem.isOutlier, + run.solution) + << " px" << std::setprecision(6) << "\n"; +} } // namespace int main(int argc, char* argv[]) { @@ -892,6 +1134,60 @@ int main(int argc, char* argv[]) { std::cout << "\nProcessing BAL file: " << filename << std::endl; const SfmData db = SfmData::FromBalFile(filename); + if (options.gnc.backend != GncBackend::None) { + const CorruptedBalProblem problem = corruptBalMeasurements( + db, options.gnc.outlierFraction, options.gnc.outlierPixels, + options.gnc.seed); + const NonlinearFactorGraph graph = buildGeneralSfmGraph(problem.data); + const Values initial = buildGeneralSfmInitial(problem.data); + std::cout << " GNC problem: " << graph.size() << " factors, " + << problem.outlierCount << " corrupted\n"; + + if (options.gnc.backend == GncBackend::Cpu) { + const GncBenchmarkRun run = runGncBenchmark( + graph, initial, makeBalLevenbergMarquardtParams(), options.gnc); + printGncRun(run, problem, graph, options.gnc); + continue; + } + +#if GTSAM_ENABLE_CUDA && GTSAM_ENABLE_CUDSS + const auto cudaParams = + makeBalCudaLmParams(options.cudaLinearSolver, CudaLmDefaults::Graph); + + if (options.cudaWarmupFileSpecified && !cudaWarmupDone) { + std::cout << " CUDA GNC warmup file: " << options.cudaWarmupFile + << " (timing ignored)\n"; + const SfmData warmupDb = SfmData::FromBalFile(options.cudaWarmupFile); + const CudaBackendLmRun warmupRun = + runCudaBackendLm(warmupDb, cudaParams); + std::cout << " CUDA GNC warmup: " << warmupRun.elapsed + << " s ignored\n"; + cudaWarmupDone = true; + } + + // Reference single CUDA LM solve on the corrupted problem: the gap + // between (outer iterations x this) and the GNC total is the per-outer + // re-conversion/re-upload overhead that a device-resident GNC removes. + const CudaGraphLmRun referenceLm = + runCudaGraphLm(graph, initial, cudaParams); + std::cout << " CUDA LM single solve (reference): " + << referenceLm.elapsed << " s (backend measured " + << referenceLm.backend.totalMeasuredElapsed + << " s, setup " << referenceLm.backend.setupElapsed + << " s, solve loop " + << referenceLm.backend.solveLoopElapsed << " s)\n"; + + const GncBenchmarkRun run = + runGncBenchmark(graph, initial, cudaParams, options.gnc); + printGncRun(run, problem, graph, options.gnc); + continue; +#else + throw std::runtime_error( + "--gnc cuda requires configuring with GTSAM_ENABLE_CUDA=ON and " + "GTSAM_ENABLE_CUDSS=ON"); +#endif + } + #if GTSAM_ENABLE_CUDA && GTSAM_ENABLE_CUDSS if (options.cudaLm) { const auto cudaParams = @@ -1006,7 +1302,7 @@ int main(int argc, char* argv[]) { } } if (!options.profile && !options.cudaStructureOnly && !options.cudaLm && - !options.cudaLmGraph) { + !options.cudaLmGraph && options.gnc.backend == GncBackend::None) { std::cout << "\n| Dataset | Legacy (Cholesky) s | New (Solver) s | Speedup |\n"; std::cout << "| --- | --- | --- | --- |\n";