diff --git a/INSTALL.md b/INSTALL.md index 6590522fa0..838b5057a6 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -42,13 +42,12 @@ Other optional libraries can enable specific features (check "CMake Options" for * Alembic (data I/O) * CCTag (feature extraction/matching and localization on CPU or GPU) * Cuda >= 11.0 (feature extraction and depth map computation) -* Magma (required for UncertaintyTE) +* Magma * Mosek >= 6 (linear programming) * OpenCV >= 3.4.11 (feature extraction, calibration module, video IO), >= 4.5 for colorchecker (mcc) * OpenMP (enable multi-threading) * PCL (Point Cloud Library) >= 1.12.1 for the registration module * PopSift (feature extraction on GPU) -* UncertaintyTE (Uncertainty computation) * Lemon >= 1.3 * libe57format (support reading .e57 files) * SWIG, Python 3 and NumPy 1.26 (Python binding for AliceVision modules) @@ -222,11 +221,6 @@ At the end of the cmake process, a report shows for each library which version ( Enable GPU SIFT implementation. `-DPopSift_DIR:PATH=/path/to/popsift/install/lib/cmake/PopSift` (where PopSiftConfig.cmake can be found) -* `ALICEVISION_USE_UNCERTAINTYTE` (default: `AUTO`) - Enable Uncertainty computation. - `-DUNCERTAINTYTE_DIR:PATH=/path/to/uncertaintyTE/install/` (where the `include` and `lib` folders can be found) - `-DMAGMA_ROOT:PATH=/path/to/magma/install/` (where the `include` and `lib` folders can be found) - * `ALICEVISION_USE_OPENCV` (default: `OFF`) Build with OpenCV. `-DOpenCV_DIR:PATH=/path/to/opencv/install/share/OpenCV/` (where OpenCVConfig.cmake can be found) diff --git a/meshroom/aliceVision/ComputeUncertainty.py b/meshroom/aliceVision/ComputeUncertainty.py new file mode 100644 index 0000000000..26dd3aab5a --- /dev/null +++ b/meshroom/aliceVision/ComputeUncertainty.py @@ -0,0 +1,38 @@ +__version__ = "1.0" + +from meshroom.core import desc +from meshroom.core.utils import VERBOSE_LEVEL + + +class ComputeUncertainty(desc.AVCommandLineNode): + + commandLine = "aliceVision_computeUncertainty {allParams}" + size = desc.DynamicNodeSize("input") + + category = "Utils" + documentation = """ Compute solution covariance """ + + inputs = [ + desc.File( + name="input", + label="SfMData", + description="Input SfMData file.", + value="", + ), + desc.ChoiceParam( + name="verboseLevel", + label="Verbose Level", + description="Verbosity level (fatal, error, warning, info, debug, trace).", + values=VERBOSE_LEVEL, + value="info", + ), + ] + + outputs = [ + desc.File( + name="output", + label="SfMData", + description="Path to the output SfM file.", + value="{nodeCacheFolder}/sfmData.abc", + ) + ] diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 98b26615e9..c434b24f7e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -68,7 +68,6 @@ trilean_option(ALICEVISION_USE_CCTAG "Enable CCTAG markers" AUTO) trilean_option(ALICEVISION_USE_APRILTAG "Enable AprilTag markers" AUTO) trilean_option(ALICEVISION_USE_POPSIFT "Enable GPU SIFT implementation" AUTO) trilean_option(ALICEVISION_USE_ALEMBIC "Enable Alembic I/O" AUTO) -trilean_option(ALICEVISION_USE_UNCERTAINTYTE "Enable Uncertainty computation" AUTO) trilean_option(ALICEVISION_USE_ONNX "Enable ONNX Runtime" AUTO) option(ALICEVISION_USE_ONNX_GPU "Use CUDA with ONNX Runtime" ON) if(APPLE) @@ -217,6 +216,7 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) include(OFA/OptimizeForArchitecture) OptimizeForArchitecture() + set(ALICEVISION_HAVE_SSE 0 CACHE BOOL "SSE2 is available" FORCE) if (USE_SSE2 OR TARGET_ARCHITECTURE STREQUAL "native") if (MSVC AND NOT ${CMAKE_CL_64}) @@ -504,6 +504,7 @@ if (ALICEVISION_BUILD_SFM) endif() endif() + # ============================================================================== # CoinUtils, Clp, Osi # ============================================================================== @@ -634,31 +635,6 @@ if (NOT ALICEVISION_USE_USD STREQUAL "OFF") endif() endif() -# ============================================================================== -# UncertaintyTE -# ============================================================================== -# - optional, only external and enabled only if ALICEVISION_USE_UNCERTAINTYTE is ON -# ============================================================================== -set(ALICEVISION_HAVE_UNCERTAINTYTE 0) - -if (ALICEVISION_BUILD_SFM) - if (NOT ALICEVISION_USE_UNCERTAINTYTE STREQUAL "OFF") - find_package(UncertaintyTE) - - if (UNCERTAINTYTE_FOUND) - set(ALICEVISION_HAVE_UNCERTAINTYTE 1) - message(STATUS "UncertaintyTE found.") - elseif (ALICEVISION_USE_UNCERTAINTYTE STREQUAL "ON") - message(SEND_ERROR "Failed to find UncertaintyTE.") - endif() - endif() - - if (ALICEVISION_HAVE_UNCERTAINTYTE) - include_directories(${UNCERTAINTYTE_INCLUDE_DIR}) - link_directories(${UNCERTAINTYTE_LIBRARY_DIR}) - endif() -endif() - # ============================================================================== # ZLIB # ============================================================================== @@ -864,6 +840,20 @@ if (ALICEVISION_BUILD_SFM) endif() endif() +# ============================================================================== +# Magma +# ============================================================================== +if (ALICEVISION_BUILD_SFM) + if (ALICEVISION_HAVE_CUDA) + find_package(magma CONFIG REQUIRED) + + if (TARGET magma::magma) + set(MAGMA_LIBRARIES magma::magma) + else() + message(FATAL_ERROR "MAGMA can not be found") + endif() + endif() +endif() # ============================================================================== # Include directories @@ -883,7 +873,6 @@ set(ALICEVISION_INCLUDE_DIRS ${LEMON_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} - ${UNCERTAINTYTE_INCLUDE_DIRS} ${MAGMA_INCLUDE_DIRS} ${FLANN_INCLUDE_DIRS} ${LP_INCLUDE_DIRS} @@ -985,7 +974,6 @@ message("** Build LIDAR part: " ${ALICEVISION_BUILD_LIDAR}) message("** Build AliceVision tests: " ${ALICEVISION_BUILD_TESTS}) message("** Build AliceVision documentation: " ${ALICEVISION_HAVE_DOC}) message("** Build AliceVision+OpenCV samples programs: " ${ALICEVISION_HAVE_OPENCV}) -message("** Build UncertaintyTE: " ${ALICEVISION_HAVE_UNCERTAINTYTE}) message("** Build MeshSDFilter: " ${ALICEVISION_HAVE_MESHSDFILTER}) message("** Build Alembic exporter: " ${ALICEVISION_HAVE_ALEMBIC}) message("** Build SWIG Python binding: " ${ALICEVISION_BUILD_SWIG_BINDING}) diff --git a/src/aliceVision/CMakeLists.txt b/src/aliceVision/CMakeLists.txt index 25735af6a3..b4983fdc5c 100644 --- a/src/aliceVision/CMakeLists.txt +++ b/src/aliceVision/CMakeLists.txt @@ -33,6 +33,9 @@ if (ALICEVISION_BUILD_SFM) add_subdirectory(track) add_subdirectory(voctree) add_subdirectory(calibration) + if (ALICEVISION_HAVE_CUDA) + add_subdirectory(uncertainty) + endif() if (ALICEVISION_HAVE_OPENCV) add_subdirectory(imageMasking) add_subdirectory(keyframe) diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index eacc34f143..7a8369862e 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -52,6 +52,7 @@ set(sfm_files_headers utils/poseNoise.hpp utils/preprocess.hpp utils/syntheticScene.hpp + utils/gauge.hpp LocalBundleAdjustmentGraph.hpp FrustumFilter.hpp filters.hpp @@ -96,6 +97,7 @@ set(sfm_files_sources utils/poseNoise.cpp utils/preprocess.cpp utils/syntheticScene.cpp + utils/gauge.cpp LocalBundleAdjustmentGraph.cpp FrustumFilter.cpp generateReport.cpp diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index e6b6d0c1cd..2c252680f6 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -245,9 +245,6 @@ void BundleAdjustmentCeres::addExtrinsicsToProblem(const sfmData::SfMData& sfmDa double* poseBlockPtr = poseBlock.data(); problem.AddParameterBlock(poseBlockPtr, 6); - // add pose parameter to the all parameters blocks pointers list - _allParametersBlocks.push_back(poseBlockPtr); - // keep the camera extrinsics constants if (cameraPose.isLocked() || isConstant || (!refineTranslation && !refineRotation)) { @@ -392,9 +389,6 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa double* intrinsicBlockPtr = intrinsicBlock.data(); problem.AddParameterBlock(intrinsicBlockPtr, intrinsicBlock.size()); - // add intrinsic parameter to the all parameters blocks pointers list - _allParametersBlocks.push_back(intrinsicBlockPtr); - // keep the camera intrinsic constant if (intrinsicPtr->isLocked() || !refineIntrinsics || intrinsicPtr->getState() == EEstimatorParameterState::CONSTANT) { @@ -580,9 +574,6 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat double* fakeDistortionBlockPtr = &_fakeDistortionBlock; - // add landmark parameter to the all parameters blocks pointers list - _allParametersBlocks.push_back(landmarkBlockPtr); - // iterate over 2D observation associated to the 3D landmark for (const auto& [viewId, observation] : landmark.getObservations()) { @@ -1083,7 +1074,6 @@ void BundleAdjustmentCeres::resetProblem() { _statistics = Statistics(); - _allParametersBlocks.clear(); _posesBlocks.clear(); _intrinsicsBlocks.clear(); _landmarksBlocks.clear(); @@ -1220,21 +1210,49 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin } } -void BundleAdjustmentCeres::createJacobian(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::CRSMatrix& jacobian) +void BundleAdjustmentCeres::createJacobian(const sfmData::SfMData& sfmData, ceres::CRSMatrix& jacobian, std::map & poseToPosition, std::map & landmarkToPosition) { std::vector landmarksBlockIds; std::vector temporalConstraintBlockIds; + BundleAdjustment::ERefineOptions refineOptions = BundleAdjustment::REFINE_ROTATION | BundleAdjustment::REFINE_TRANSLATION | BundleAdjustment::REFINE_STRUCTURE; + // create problem ceres::Problem::Options problemOptions; problemOptions.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; ceres::Problem problem(problemOptions); createProblem(sfmData, refineOptions, problem, landmarksBlockIds, temporalConstraintBlockIds); + + std::vector allParametersBlocks; + for (auto & [id, ptr] : _posesBlocks) + { + if (problem.IsParameterBlockConstant(ptr.data())) + { + continue; + } + + poseToPosition[id] = allParametersBlocks.size(); + allParametersBlocks.push_back(ptr.data()); + } + + for (auto & [id, ptr] : _landmarksBlocks) + { + if (problem.IsParameterBlockConstant(ptr.data())) + { + continue; + } + + landmarkToPosition[id] = allParametersBlocks.size(); + allParametersBlocks.push_back(ptr.data()); + } + + + // configure Jacobian engine double cost = 0.0; ceres::Problem::EvaluateOptions evalOpt; - evalOpt.parameter_blocks = _allParametersBlocks; + evalOpt.parameter_blocks = allParametersBlocks; evalOpt.num_threads = 8; evalOpt.apply_loss_function = true; diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp index b54a9c561d..348abd9180 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp @@ -123,10 +123,11 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback /** * @brief Create a jacobian CRSMatrix * @param[in] sfmData The input SfMData contains all the information about the reconstruction - * @param[in] refineOptions The chosen refine flag * @param[out] jacobian The jacobian CSRMatrix + * @param[out] poseToPosition a map which gives the parameter block index of a pose Id in the jacobian + * @param[out] landmarkToPosition a map which gives the parameter block index of a landmark Id in the jacobian */ - void createJacobian(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::CRSMatrix& jacobian); + void createJacobian(const sfmData::SfMData& sfmData, ceres::CRSMatrix& jacobian, std::map & poseToPosition, std::map & landmarkToPosition); /** * @brief Perform a Bundle Adjustment on the SfM scene with refinement of the requested parameters @@ -258,13 +259,9 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback // data wrappers for refinement - /// all parameters blocks pointers - std::vector _allParametersBlocks; /// poses blocks wrapper /// block: ceres angleAxis(3) + translation(3) - std::map> _posesBlocks; // TODO : maybe we can use boost::flat_map instead of std::map ? - /// intrinsics blocks wrapper - /// block: intrinsics params + std::map> _posesBlocks; std::map> _intrinsicsBlocks; std::map> _distortionsBlocks; std::map> _intrinsicObjects; diff --git a/src/aliceVision/sfm/sfmStatistics.cpp b/src/aliceVision/sfm/sfmStatistics.cpp index 39109e6f62..d6a78c657e 100644 --- a/src/aliceVision/sfm/sfmStatistics.cpp +++ b/src/aliceVision/sfm/sfmStatistics.cpp @@ -428,5 +428,48 @@ void computeObservationsLengthsPerView(const sfmData::SfMData& sfmData, } } +void computeResidualsMedianPerView(const sfmData::SfMData& sfmData, const std::set& specificViews, std::map & medianPerView) +{ + std::set validViews = sfmData.getValidViews(); + + // Update valid views by intersecting with specificViews + if (!specificViews.empty()) + { + std::set filteredValidViews; + std::set_intersection(validViews.begin(), validViews.end(), specificViews.begin(), specificViews.end(), std::inserter(filteredValidViews, filteredValidViews.begin())); + validViews = std::move(filteredValidViews); + } + + + std::unordered_map> residualsPerView; + for (const auto & [_, landmark] : sfmData.getLandmarks()) + { + const aliceVision::sfmData::Observations& observations = landmark.getObservations(); + for (const auto & [idView, observation] : observations) + { + if (validViews.count(idView) == 0) + { + continue; + } + + const sfmData::View& view = sfmData.getView(idView); + const aliceVision::geometry::Pose3 pose = sfmData.getPose(view).getTransform(); + const aliceVision::camera::IntrinsicBase & intrinsic = sfmData.getIntrinsic(view.getIntrinsicId()); + const Vec2 residual = intrinsic.residual(pose, landmark.getX().homogeneous(), observation.getCoordinates()); + + residualsPerView[idView].push_back(residual.norm()); + } + } + + for (auto & [idView, vec]: residualsPerView) + { + const auto medianIt2 = vec.begin() + vec.size() / 2; + std::nth_element(vec.begin(), medianIt2, vec.end()); + const double med2 = *medianIt2; + + ALICEVISION_LOG_ERROR(med2); + } +} + } // namespace sfm } // namespace aliceVision diff --git a/src/aliceVision/sfm/sfmStatistics.hpp b/src/aliceVision/sfm/sfmStatistics.hpp index 9fd7b178da..b3e4126c94 100644 --- a/src/aliceVision/sfm/sfmStatistics.hpp +++ b/src/aliceVision/sfm/sfmStatistics.hpp @@ -119,5 +119,9 @@ void computeObservationsLengthsPerView(const sfmData::SfMData& sfmData, std::vector& nbResidualsPerViewFirstQuartile, std::vector& nbResidualsPerViewThirdQuartile); +void computeResidualsMedianPerView(const sfmData::SfMData& sfmData, + const std::set& specificViews, + std::map & medianPerView); + } // namespace sfm } // namespace aliceVision diff --git a/src/aliceVision/sfm/utils/gauge.cpp b/src/aliceVision/sfm/utils/gauge.cpp new file mode 100644 index 0000000000..86bc70c89a --- /dev/null +++ b/src/aliceVision/sfm/utils/gauge.cpp @@ -0,0 +1,77 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2027 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include + +namespace aliceVision +{ +namespace sfm +{ + +bool selectTripletForGaugeRemoval(const sfmData::SfMData& sfmData, std::array& selectedSet, int nbSamples) +{ + // Check landmarks + const auto & landmarks = sfmData.getLandmarks(); + if (landmarks.size() < 3) + { + return false; + } + + // Get a consecutive vector of landmarks indices + std::vector landmarksIndices; + landmarksIndices.reserve(landmarks.size()); + for (const auto & [key, _] : landmarks) + { + landmarksIndices.push_back(key); + } + + std::mt19937 gen; + std::uniform_int_distribution randIdx(0, landmarks.size() - 1); + + double maxScore = -1.0; + for (int i = 0; i < nbSamples; i++) + { + // Select 3 unique indices + size_t i0 = randIdx(gen); + size_t i1, i2; + do { i1 = randIdx(gen); } while (i1 == i0); + do { i2 = randIdx(gen); } while (i2 == i0 || i2 == i1); + + IndexT idx1 = landmarksIndices[i0]; + IndexT idx2 = landmarksIndices[i1]; + IndexT idx3 = landmarksIndices[i2]; + + const Vec3 & p1 = landmarks.at(idx1).getX(); + const Vec3 & p2 = landmarks.at(idx2).getX(); + const Vec3 & p3 = landmarks.at(idx3).getX(); + + const Vec3 d1 = p2 - p1; + const Vec3 d2 = p3 - p1; + const Vec3 d3 = p3 - p2; + + // Compute area of the triangle + const Vec3 crossProduct = d1.cross(d2); + const double areaDouble = crossProduct.norm(); + + // Privilege large area while penalizing elongated triangles + // Which may lead to colinearity + const double longestSize = std::max({d1.norm(), d2.norm(), d3.norm()}); + const double score = areaDouble / longestSize; + + if (score > maxScore) + { + maxScore = score; + selectedSet[0] = idx1; + selectedSet[1] = idx2; + selectedSet[2] = idx3; + } + } + + return true; +} + +} +} diff --git a/src/aliceVision/sfm/utils/gauge.hpp b/src/aliceVision/sfm/utils/gauge.hpp new file mode 100644 index 0000000000..6f55ca7548 --- /dev/null +++ b/src/aliceVision/sfm/utils/gauge.hpp @@ -0,0 +1,53 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2027 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include + +namespace aliceVision +{ +namespace sfm +{ + +/** + * @brief Select a well-conditioned triplet of landmarks to remove the gauge freedom + * before computing the BA Jacobian / covariance. + * + * Bundle adjustment has a 7-DOF gauge freedom (3 translation + 3 rotation + 1 scale). + * Fixing the 3D position of three non-collinear landmarks over-constrains these 7 DOF + * (9 fixed values) and makes the Hessian / information matrix full-rank. + * + * The selection maximises the minimum altitude of the candidate triangle: + * + * @f[ + * \text{score} = \frac{\|(p_2 - p_1) \times (p_3 - p_1)\|} + * {\max(\|p_2{-}p_1\|,\, \|p_3{-}p_1\|,\, \|p_3{-}p_2\|)} + * \;\propto\; h_{\min} + * @f] + * + * This metric simultaneously rewards large spread (large triangle area in the numerator) + * and penalises near-collinear configurations (large longest side in the denominator + * produces a small score when the triangle is a sliver). A score of 0 corresponds to + * exactly collinear points, which would leave at least one rotation DOF unresolved. + * + * The search is performed by random sampling: @p nbSamples candidate triplets are drawn + * uniformly at random from the landmark set and the highest-scoring one is returned. + * More samples reduce the probability of missing a well-conditioned triplet at the cost + * of a proportionally longer search. + * + * @param[in] sfmData The reconstruction whose landmarks are searched. + * @param[out] selectedSet The three landmark IDs forming the best triplet found. + * @param[in] nbSamples Number of random triplets to evaluate (default: 100,000). + * + * @return true if a valid triplet was found (i.e. the scene has at least 3 landmarks); + * false otherwise. + */ +bool selectTripletForGaugeRemoval(const sfmData::SfMData& sfmData, std::array& selectedSet, int nbSamples = 100000); + +} +} diff --git a/src/aliceVision/sfmData/SfMData.hpp b/src/aliceVision/sfmData/SfMData.hpp index b24df3ffda..50c4f4a945 100644 --- a/src/aliceVision/sfmData/SfMData.hpp +++ b/src/aliceVision/sfmData/SfMData.hpp @@ -48,8 +48,10 @@ using Landmarks = std::map; /// Define a collection of Rig using Rigs = std::map; +using PoseUncertainty = Eigen::Matrix; + /// Define uncertainty per pose -using PosesUncertainty = std::map; +using PosesUncertainty = std::map; /// Define uncertainty per landmark using LandmarksUncertainty = std::map; @@ -75,8 +77,6 @@ using RotationPriors = std::vector; class SfMData { public: - /// Uncertainty per pose - PosesUncertainty _posesUncertainty; /// Uncertainty per landmark LandmarksUncertainty _landmarksUncertainty; @@ -537,6 +537,41 @@ class SfMData */ CameraPose & getAbsolutePose(IndexT poseId) { return *_poses.at(poseId); } + /** + * @brief Get uncertainties associated with poses. + * @return mutable map of pose uncertainties indexed by pose id + */ + PosesUncertainty & getPosesUncertainty() + { + return _posesUncertainty; + } + + /** + * @brief Get uncertainties associated with poses. + * @return map of pose uncertainties indexed by pose id + */ + const PosesUncertainty & getPosesUncertainty() const + { + return _posesUncertainty; + } + + /** + * @brief Check if the given view has an existing pose uncertainty + * @param[in] view The given view + * @return true if the uncertainty exists + */ + bool existsPoseUncertainty(const View& view) const { return (_posesUncertainty.find(view.getPoseId()) != _posesUncertainty.end()); } + + /** + * @brief Set the uncertainty associated with a pose. + * @param[in] poseId pose identifier + * @param[in] mat uncertainty matrix indexed by the pose id + */ + void setPoseUncertainty(IndexT poseId, const PoseUncertainty & mat) + { + _posesUncertainty[poseId] = mat; + } + /** * @brief Get the rig of the given view * @param[in] view The given view @@ -809,6 +844,8 @@ class SfMData SurveyPoints _surveyPoints; /// Image groups ImageGroups _imageGroups; + /// Pose uncertainty + PosesUncertainty _posesUncertainty; /** * @brief Get Rig pose of a given camera view diff --git a/src/aliceVision/sfmDataIO/AlembicExporter.cpp b/src/aliceVision/sfmDataIO/AlembicExporter.cpp index 1fdffe9810..4a91a0ec8b 100644 --- a/src/aliceVision/sfmDataIO/AlembicExporter.cpp +++ b/src/aliceVision/sfmDataIO/AlembicExporter.cpp @@ -16,6 +16,7 @@ #include #include +#include namespace fs = std::filesystem; @@ -66,7 +67,7 @@ struct AlembicExporter::DataImpl const sfmData::View& view, const sfmData::CameraPose* pose = nullptr, std::shared_ptr intrinsic = nullptr, - const Vec6* uncertainty = nullptr, + const sfmData::PoseUncertainty* uncertainty = nullptr, const sfmData::ImageGroup * imageGroup = nullptr, Alembic::Abc::OObject* parent = nullptr); @@ -95,7 +96,7 @@ void AlembicExporter::DataImpl::addCamera(const std::string& name, const sfmData::View& view, const sfmData::CameraPose* pose, std::shared_ptr intrinsic, - const Vec6* uncertainty, + const sfmData::PoseUncertainty* uncertainty, const sfmData::ImageGroup * imageGroup, Alembic::Abc::OObject* parent) { @@ -301,8 +302,8 @@ void AlembicExporter::DataImpl::addCamera(const std::string& name, if (uncertainty) { - std::vector uncertaintyParams(uncertainty->data(), uncertainty->data() + 6); - ODoubleArrayProperty mvg_uncertaintyParams(userProps, "mvg_uncertaintyEigenValues"); + std::vector uncertaintyParams(uncertainty->data(), uncertainty->data() + 36); + ODoubleArrayProperty mvg_uncertaintyParams(userProps, "mvg_uncertaintyMatrix"); mvg_uncertaintyParams.set(uncertaintyParams); } @@ -420,6 +421,8 @@ void AlembicExporter::addSfMSingleCamera(const sfmData::SfMData& sfmData, const const std::string name = fs::path(view.getImage().getImagePath()).stem().string(); const sfmData::CameraPose* pose = ((flagsPart & ESfMData::EXTRINSICS) && sfmData.existsPose(view)) ? sfmData.getPoses().at(view.getPoseId()).get() : nullptr; + const sfmData::PoseUncertainty* uncertainty = + ((flagsPart & ESfMData::EXTRINSICS) && sfmData.existsPoseUncertainty(view)) ? &sfmData.getPosesUncertainty().at(view.getPoseId()) : nullptr; const std::shared_ptr intrinsic = (flagsPart & ESfMData::INTRINSICS) ? sfmData.getIntrinsicSharedPtr(view.getIntrinsicId()) : nullptr; @@ -435,9 +438,9 @@ void AlembicExporter::addSfMSingleCamera(const sfmData::SfMData& sfmData, const } if (sfmData.isPoseAndIntrinsicDefined(view) && (flagsPart & ESfMData::EXTRINSICS)) - _dataImpl->addCamera(name, view, pose, intrinsic, nullptr, group.get(), &_dataImpl->_mvgCameras); + _dataImpl->addCamera(name, view, pose, intrinsic, uncertainty, group.get(), &_dataImpl->_mvgCameras); else - _dataImpl->addCamera(name, view, pose, intrinsic, nullptr, group.get(), &_dataImpl->_mvgCamerasUndefined); + _dataImpl->addCamera(name, view, pose, intrinsic, uncertainty, group.get(), &_dataImpl->_mvgCamerasUndefined); } void AlembicExporter::addSfMCameraRig(const sfmData::SfMData& sfmData, IndexT rigId, const std::vector& viewIds, ESfMData flagsPart) @@ -714,7 +717,7 @@ void AlembicExporter::addCamera(const std::string& name, const sfmData::View& view, const sfmData::CameraPose* pose, std::shared_ptr intrinsic, - const Vec6* uncertainty, + const sfmData::PoseUncertainty* uncertainty, const sfmData::ImageGroup * imageGroup) { _dataImpl->addCamera(name, view, pose, intrinsic, uncertainty, imageGroup); @@ -762,7 +765,8 @@ void AlembicExporter::addCameraKeyframe(const geometry::Pose3& pose, const camera::Pinhole* cam, const std::string& imagePath, IndexT viewId, - IndexT intrinsicId) + IndexT intrinsicId, + const sfmData::PoseUncertainty* uncertainty) { Eigen::Matrix4d M = Eigen::Matrix4d::Identity(); M(1, 1) = -1; diff --git a/src/aliceVision/sfmDataIO/AlembicExporter.hpp b/src/aliceVision/sfmDataIO/AlembicExporter.hpp index 8c8b556178..2893847b2c 100644 --- a/src/aliceVision/sfmDataIO/AlembicExporter.hpp +++ b/src/aliceVision/sfmDataIO/AlembicExporter.hpp @@ -83,7 +83,7 @@ class AlembicExporter const sfmData::View& view, const sfmData::CameraPose* pose = nullptr, std::shared_ptr intrinsic = nullptr, - const Vec6* uncertainty = nullptr, + const sfmData::PoseUncertainty* uncertainty = nullptr, const sfmData::ImageGroup * imageGroup = nullptr); /** @@ -93,12 +93,14 @@ class AlembicExporter * @param[in] imagePath The localized image path * @param[in] viewId View id * @param[in] intrinsicId Intrinsic id + * @param[in] uncertainty The camera uncertainty values (nullptr if undefined) */ void addCameraKeyframe(const geometry::Pose3& pose, const camera::Pinhole* cam, const std::string& imagePath, IndexT viewId, - IndexT intrinsicId); + IndexT intrinsicId, + const sfmData::PoseUncertainty* uncertainty = nullptr); /** * @brief Initiate an animated camera diff --git a/src/aliceVision/sfmDataIO/AlembicImporter.cpp b/src/aliceVision/sfmDataIO/AlembicImporter.cpp index f4c5150f1c..57abac3145 100644 --- a/src/aliceVision/sfmDataIO/AlembicImporter.cpp +++ b/src/aliceVision/sfmDataIO/AlembicImporter.cpp @@ -558,6 +558,8 @@ bool readCamera(const Version& abcVersion, bool undistortionDesqueezed = false; bool undistortionLocked = false; std::string serialNumber = ""; + bool hasUncertainty = false; + std::vector uncertaintyParams; if (userProps) { @@ -775,6 +777,13 @@ bool readCamera(const Version& abcVersion, undistortionLocked = getAbcProp(userProps, *propHeader, "mvg_undistortionLocked", sampleFrame); } } + if (userProps.getPropertyHeader("mvg_uncertaintyMatrix")) + { + Alembic::Abc::IDoubleArrayProperty prop(userProps, "mvg_uncertaintyMatrix"); + Alembic::Abc::IDoubleArrayProperty::sample_ptr_type sample; + prop.get(sample, ISampleSelector(sampleFrame)); + uncertaintyParams.assign(sample->get(), sample->get() + sample->size()); + } } } @@ -970,6 +979,15 @@ bool readCamera(const Version& abcVersion, } } + if ((flagsPart & ESfMData::EXTRINSICS)) + { + if (uncertaintyParams.size() == 36) + { + Eigen::Map> pUncertainty(uncertaintyParams.data()); + sfmData.setPoseUncertainty(poseId, pUncertainty); + } + } + return true; } diff --git a/src/aliceVision/sfmDataIO/jsonIO.cpp b/src/aliceVision/sfmDataIO/jsonIO.cpp index 07831dcb87..5ce1c88e84 100644 --- a/src/aliceVision/sfmDataIO/jsonIO.cpp +++ b/src/aliceVision/sfmDataIO/jsonIO.cpp @@ -773,6 +773,23 @@ bool saveJSON(const sfmData::SfMData& sfmData, const std::string& filename, ESfM fileTree.add_child("poses", posesTree); } + // poses uncertainties + if (!sfmData.getPosesUncertainty().empty()) + { + bpt::ptree posesUncertaintyTree; + + for (const auto& [poseId, poseUncertainty] : sfmData.getPosesUncertainty()) + { + bpt::ptree poseUncertaintyTree; + + poseUncertaintyTree.put("poseId", poseId); + saveMatrix("poseUncertainty", poseUncertainty, poseUncertaintyTree); + posesUncertaintyTree.push_back(std::make_pair("", poseUncertaintyTree)); + } + + fileTree.add_child("posesUncertainty", posesUncertaintyTree); + } + // rigs if (!sfmData.getRigs().empty()) { @@ -995,6 +1012,23 @@ bool loadJSON(sfmData::SfMData& sfmData, rigs.emplace(rigId, rig); } } + + if (fileTree.count("posesUncertainty")) + { + sfmData::PosesUncertainty & posesUncertainties = sfmData.getPosesUncertainty(); + + for (bpt::ptree::value_type& poseUncertaintyNode : fileTree.get_child("posesUncertainty")) + { + bpt::ptree& poseUncertaintyTree = poseUncertaintyNode.second; + + sfmData::PoseUncertainty poseUncertainty; + + loadMatrix("poseUncertainty", poseUncertainty, poseUncertaintyTree); + IndexT poseId = poseUncertaintyTree.get("poseId"); + + posesUncertainties[poseId] = poseUncertainty; + } + } } // structure diff --git a/src/aliceVision/sfmDataIO/sceneSample.cpp b/src/aliceVision/sfmDataIO/sceneSample.cpp index 2a3eb2a29f..9080ea19e0 100644 --- a/src/aliceVision/sfmDataIO/sceneSample.cpp +++ b/src/aliceVision/sfmDataIO/sceneSample.cpp @@ -25,6 +25,38 @@ ESceneType ESceneType_stringToEnum(const std::string& SScene) { return ESceneType::SCENE_SPHERE; } + if (scene == "planar") + { + return ESceneType::SCENE_PLANAR; + } + if (scene == "collinear") + { + return ESceneType::SCENE_COLLINEAR; + } + if (scene == "forwardmotion") + { + return ESceneType::SCENE_FORWARD_MOTION; + } + if (scene == "narrowbaseline") + { + return ESceneType::SCENE_NARROW_BASELINE; + } + if (scene == "unevencoverage") + { + return ESceneType::SCENE_UNEVEN_COVERAGE; + } + if (scene == "purerotation") + { + return ESceneType::SCENE_PURE_ROTATION; + } + if (scene == "sparseobservations") + { + return ESceneType::SCENE_SPARSE_OBSERVATIONS; + } + if (scene == "clusteredobservations") + { + return ESceneType::SCENE_CLUSTERED_OBSERVATIONS; + } throw std::invalid_argument("Invalid scene type: " + scene); } @@ -38,6 +70,38 @@ std::string ESceneType_enumToString(const ESceneType EScene) { return "sphere"; } + if (EScene == ESceneType::SCENE_PLANAR) + { + return "planar"; + } + if (EScene == ESceneType::SCENE_COLLINEAR) + { + return "collinear"; + } + if (EScene == ESceneType::SCENE_FORWARD_MOTION) + { + return "forwardmotion"; + } + if (EScene == ESceneType::SCENE_NARROW_BASELINE) + { + return "narrowbaseline"; + } + if (EScene == ESceneType::SCENE_UNEVEN_COVERAGE) + { + return "unevencoverage"; + } + if (EScene == ESceneType::SCENE_PURE_ROTATION) + { + return "purerotation"; + } + if (EScene == ESceneType::SCENE_SPARSE_OBSERVATIONS) + { + return "sparseobservations"; + } + if (EScene == ESceneType::SCENE_CLUSTERED_OBSERVATIONS) + { + return "clusteredobservations"; + } throw std::invalid_argument("Unrecognized ESceneType: " + std::to_string(int(EScene))); } @@ -64,6 +128,38 @@ void generateSampleScene(sfmData::SfMData& output, ESceneType scene) { generateSphereScene(output, 1000, 240); } + else if (scene == ESceneType::SCENE_PLANAR) + { + generatePlanarScene(output, 30, 60); + } + else if (scene == ESceneType::SCENE_COLLINEAR) + { + generateCollinearCamerasScene(output, 500, 20); + } + else if (scene == ESceneType::SCENE_FORWARD_MOTION) + { + generateForwardMotionScene(output, 500, 30); + } + else if (scene == ESceneType::SCENE_NARROW_BASELINE) + { + generateNarrowBaselineScene(output, 1000, 240); + } + else if (scene == ESceneType::SCENE_UNEVEN_COVERAGE) + { + generateUnevenCoverageScene(output, 200, 800, 240); + } + else if (scene == ESceneType::SCENE_PURE_ROTATION) + { + generatePureRotationScene(output, 1000, 240); + } + else if (scene == ESceneType::SCENE_SPARSE_OBSERVATIONS) + { + generateSparseCameraObservationsScene(output, 100, 60, 5); + } + else if (scene == ESceneType::SCENE_CLUSTERED_OBSERVATIONS) + { + generateClusteredObservationsScene(output, 200, 240); + } else { throw std::out_of_range("Invalid ESceneType"); @@ -181,5 +277,503 @@ void generateSphereScene(sfmData::SfMData& output, int pointsNb, int posesNb) } } +// --------------------------------------------------------------------------- +// Private helpers +// --------------------------------------------------------------------------- + +namespace +{ + +/** + * @brief Build a world-to-camera rotation matrix from an eye position, a look-at + * target, and a world-up vector. + * + * The resulting matrix R satisfies: X_cam = R * (X_world - eye). + * + * Axes: + * zAxis (optical axis, into scene) = (target - eye).normalized() + * xAxis (right) = worldUp × zAxis, normalized + * yAxis (down in camera frame) = zAxis × xAxis + * + * When worldUp is nearly parallel to zAxis (degenerate case), a fallback + * direction (0,0,1) is used for the cross product. + * + * @param eye Camera centre in world coordinates. + * @param target World point that the camera looks towards. + * @param worldUp World up direction (does not need to be perpendicular to the axis). + * @return World-to-camera rotation matrix (3×3, rows = camera basis vectors). + */ +Eigen::Matrix3d computeLookAtRotation(const Eigen::Vector3d& eye, + const Eigen::Vector3d& target, + const Eigen::Vector3d& worldUp) +{ + Eigen::Vector3d zAxis = (target - eye).normalized(); + + Eigen::Vector3d xAxis = worldUp.cross(zAxis); + if (xAxis.norm() < 1e-6) + { + // worldUp is (nearly) parallel to zAxis – use a fallback + const Eigen::Vector3d fallback = (std::abs(zAxis.z()) < 0.9) + ? Eigen::Vector3d::UnitZ() + : Eigen::Vector3d::UnitX(); + xAxis = fallback.cross(zAxis); + } + xAxis.normalize(); + + const Eigen::Vector3d yAxis = zAxis.cross(xAxis); + + Eigen::Matrix3d R; + R.row(0) = xAxis; + R.row(1) = yAxis; + R.row(2) = zAxis; + return R; +} + +} // anonymous namespace + +// --------------------------------------------------------------------------- +// New scene generators +// --------------------------------------------------------------------------- + +void generatePlanarScene(sfmData::SfMData& output, int gridSize, int posesNb) +{ + const int w = 4092; + const int h = 2048; + const double focal = 10000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focal, focal, 0, 0)); + + // Landmarks on a regular grid in the z=0 plane, x/y in [-1, 1]. + IndexT landmarkId = 0; + for (int ix = 0; ix < gridSize; ++ix) + { + for (int iy = 0; iy < gridSize; ++iy) + { + const double x = (gridSize > 1) ? -1.0 + 2.0 * ix / (gridSize - 1) : 0.0; + const double y = (gridSize > 1) ? -1.0 + 2.0 * iy / (gridSize - 1) : 0.0; + output.getLandmarks().emplace(landmarkId, + sfmData::Landmark(Eigen::Vector3d(x, y, 0.0), feature::EImageDescriberType::UNKNOWN)); + ++landmarkId; + } + } + + // Cameras on a circle of radius 3 at height z = 5, looking at the origin. + // worldUp = +Z because the plane lies in XY and cameras are above it. + const double cameraRadius = 3.0; + const double cameraHeight = 5.0; + const Eigen::Vector3d sceneCenter(0.0, 0.0, 0.0); + const Eigen::Vector3d worldUp(0.0, 0.0, 1.0); + + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + const double angle = (double(idPV) * 2.0 * M_PI) / posesNb; + Eigen::Vector3d eye(cameraRadius * std::cos(angle), cameraRadius * std::sin(angle), cameraHeight); + Eigen::Matrix3d R = computeLookAtRotation(eye, sceneCenter, worldUp); + output.getPoses().assign(idPV, sfmData::CameraPose(geometry::Pose3(R, eye))); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + // Build observations by projecting every landmark into every view. + const double arbitraryScale = 1.0; + for (auto& [lmId, landmark] : output.getLandmarks()) + { + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, lmId, arbitraryScale)); + } + } +} + +void generateCollinearCamerasScene(sfmData::SfMData& output, int pointsNb, int posesNb) +{ + const int w = 4092; + const int h = 2048; + const double focal = 10000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focal, focal, 0, 0)); + + // Random landmarks in a box ahead of the cameras (+Z direction). + std::srand(12345); + for (int id = 0; id < pointsNb; ++id) + { + Eigen::Vector3d pt = Eigen::Vector3d::Random(); // components in [-1, 1] + pt.x() *= 3.0; + pt.y() *= 3.0; + pt.z() = 5.0 + (pt.z() + 1.0) * 2.5; // remap to [5, 10] + output.getLandmarks().emplace(id, + sfmData::Landmark(pt, feature::EImageDescriberType::UNKNOWN)); + } + + // Cameras uniformly spaced along the X axis, all looking in the +Z direction. + // The spacing between cameras is 0.3; the array is centred at the origin. + const double spacing = 0.3; + const Eigen::Vector3d lookDir(0.0, 0.0, 1.0); + const Eigen::Vector3d worldUp(0.0, 1.0, 0.0); + + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + const double xPos = spacing * (double(idPV) - double(posesNb - 1) / 2.0); + Eigen::Vector3d eye(xPos, 0.0, 0.0); + Eigen::Matrix3d R = computeLookAtRotation(eye, eye + lookDir, worldUp); + output.getPoses().assign(idPV, sfmData::CameraPose(geometry::Pose3(R, eye))); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + // Build observations. + const double arbitraryScale = 1.0; + for (auto& [lmId, landmark] : output.getLandmarks()) + { + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, lmId, arbitraryScale)); + } + } +} + +void generateForwardMotionScene(sfmData::SfMData& output, int pointsNb, int posesNb) +{ + const int w = 4092; + const int h = 2048; + const double focal = 10000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focal, focal, 0, 0)); + + // Camera step along Z. + const double step = 0.4; + const double cameraTrainLength = step * (posesNb - 1); + + // Random landmarks in a box well ahead of the entire camera train. + std::srand(23456); + for (int id = 0; id < pointsNb; ++id) + { + Eigen::Vector3d pt = Eigen::Vector3d::Random(); + pt.x() *= 1.0; // x in [-1, 1] + pt.y() *= 1.0; // y in [-1, 1] + // z well ahead of the last camera: [cameraTrainLength + 4, cameraTrainLength + 8] + pt.z() = cameraTrainLength + 4.0 + (pt.z() + 1.0) * 2.0; + output.getLandmarks().emplace(id, + sfmData::Landmark(pt, feature::EImageDescriberType::UNKNOWN)); + } + + // Cameras at (0, 0, i*step), all looking in +Z. + const Eigen::Vector3d lookDir(0.0, 0.0, 1.0); + const Eigen::Vector3d worldUp(0.0, 1.0, 0.0); + + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + Eigen::Vector3d eye(0.0, 0.0, double(idPV) * step); + Eigen::Matrix3d R = computeLookAtRotation(eye, eye + lookDir, worldUp); + output.getPoses().assign(idPV, sfmData::CameraPose(geometry::Pose3(R, eye))); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + // Build observations. + const double arbitraryScale = 1.0; + for (auto& [lmId, landmark] : output.getLandmarks()) + { + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, lmId, arbitraryScale)); + } + } +} + +void generateNarrowBaselineScene(sfmData::SfMData& output, int pointsNb, int posesNb) +{ + const int w = 4092; + const int h = 2048; + const double focalLengthPixX = 10000.0; + const double focalLengthPixY = 20000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, 0, 0)); + + // Very small camera circle radius (0.05) vs scene depth (~1.0) → narrow baseline. + const double cameraRadius = 0.05; + + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + const double angle2d = (double(idPV) * 2.0 * M_PI) / posesNb; + Eigen::AngleAxis aa(angle2d + 0.5 * M_PI, Eigen::Vector3d::UnitY()); + Eigen::Matrix3d rotation = aa.toRotationMatrix(); + Eigen::Vector3d position(std::cos(angle2d), 0.0, std::sin(angle2d)); + geometry::Pose3 pose(rotation, cameraRadius * position); + output.getPoses().assign(idPV, sfmData::CameraPose(pose)); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + // Landmarks uniformly distributed on the unit sphere. + std::srand(34567); + const double arbitraryScale = 1.0; + for (int landmarkId = 0; landmarkId < pointsNb; ++landmarkId) + { + Eigen::Vector3d point3D = Eigen::Vector3d::Random().normalized(); + + sfmData::Landmark landmark(point3D, feature::EImageDescriberType::UNKNOWN); + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + // Skip landmarks behind the camera (negative depth in camera space). + // For cameras at radius 0.05 looking inward, roughly half of unit-sphere + // landmarks lie behind each camera and must not generate observations. + // Also skip near-grazing observations (z_c < 0.1): these have tiny depth + // values that produce enormous Jacobian entries (f/z_c >> 1) which inflate + // column norms and create artificially small covariance estimates. + const double minDepth = 0.1; + const Vec3 transformed = pose(landmark.getX()); + if (transformed.z() <= minDepth) + { + continue; + } + + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, landmarkId, arbitraryScale)); + } + output.getLandmarks().emplace(landmarkId, landmark); + } +} + +void generateUnevenCoverageScene(sfmData::SfMData& output, int wellObservedNb, int poorlyObservedNb, int posesNb) +{ + const int w = 4092; + const int h = 2048; + const double focalLengthPixX = 10000.0; + const double focalLengthPixY = 20000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, 0, 0)); + + // Cameras arranged on a circle of radius 10, identical to the sphere scene. + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + const double angle2d = (double(idPV) * 2.0 * M_PI) / posesNb; + Eigen::AngleAxis aa(angle2d + 0.5 * M_PI, Eigen::Vector3d::UnitY()); + Eigen::Matrix3d rotation = aa.toRotationMatrix(); + Eigen::Vector3d position(std::cos(angle2d), 0.0, std::sin(angle2d)); + geometry::Pose3 pose(rotation, 10.0 * position); + output.getPoses().assign(idPV, sfmData::CameraPose(pose)); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + const double arbitraryScale = 1.0; + IndexT landmarkId = 0; + + // Well-observed landmarks: random points near the origin (|X| <= 0.3), + // observed by every camera. + std::srand(45678); + for (int i = 0; i < wellObservedNb; ++i) + { + Eigen::Vector3d point3D = Eigen::Vector3d::Random() * 0.3; + sfmData::Landmark landmark(point3D, feature::EImageDescriberType::UNKNOWN); + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, landmarkId, arbitraryScale)); + } + output.getLandmarks().emplace(landmarkId, landmark); + ++landmarkId; + } + + // Poorly-observed landmarks: random points on the unit sphere, each seen + // by only 2 adjacent cameras (k and k+1 mod posesNb). + for (int i = 0; i < poorlyObservedNb; ++i) + { + Eigen::Vector3d point3D = Eigen::Vector3d::Random().normalized(); + sfmData::Landmark landmark(point3D, feature::EImageDescriberType::UNKNOWN); + + const IndexT k = static_cast(i % posesNb); + const IndexT views[2] = {k, (k + 1) % static_cast(posesNb)}; + for (IndexT viewId : views) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, landmarkId, arbitraryScale)); + } + output.getLandmarks().emplace(landmarkId, landmark); + ++landmarkId; + } +} + +void generatePureRotationScene(sfmData::SfMData& output, int pointsNb, int posesNb) +{ + const int w = 4092; + const int h = 2048; + const double focalLengthPixX = 10000.0; + const double focalLengthPixY = 20000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, 0, 0)); + + // All cameras share the origin but have distinct orientations (sphere scene angles). + // The Jacobian d(proj)/d(depth) is identically zero for any point under perspective + // projection when the camera centre coincides with the world origin and the point + // lies on the unit sphere, so radial depth is completely unobservable. + const Eigen::Vector3d sharedCenter(0.0, 0.0, 0.0); + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + const double angle2d = (double(idPV) * 2.0 * M_PI) / posesNb; + Eigen::AngleAxis aa(angle2d + 0.5 * M_PI, Eigen::Vector3d::UnitY()); + Eigen::Matrix3d rotation = aa.toRotationMatrix(); + // All cameras are at the origin; only the rotation changes. + geometry::Pose3 pose(rotation, sharedCenter); + output.getPoses().assign(idPV, sfmData::CameraPose(pose)); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + // Landmarks on the unit sphere, projected into all views. + std::srand(56789); + const double arbitraryScale = 1.0; + for (int landmarkId = 0; landmarkId < pointsNb; ++landmarkId) + { + Eigen::Vector3d point3D = Eigen::Vector3d::Random().normalized(); + + sfmData::Landmark landmark(point3D, feature::EImageDescriberType::UNKNOWN); + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, landmarkId, arbitraryScale)); + } + output.getLandmarks().emplace(landmarkId, landmark); + } +} + +void generateSparseCameraObservationsScene(sfmData::SfMData& output, int pointsNb, int posesNb, int obsPerCamera) +{ + const int w = 4092; + const int h = 2048; + const double focalLengthPixX = 10000.0; + const double focalLengthPixY = 20000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, 0, 0)); + + // Cameras arranged on a circle of radius 10, all looking at the origin. + // worldUp = +Y (camera centres lie in the XZ plane). + const Eigen::Vector3d sceneCenter(0.0, 0.0, 0.0); + const Eigen::Vector3d worldUp(0.0, 1.0, 0.0); + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + const double angle2d = (double(idPV) * 2.0 * M_PI) / posesNb; + const Eigen::Vector3d eye(10.0 * std::cos(angle2d), 0.0, 10.0 * std::sin(angle2d)); + const Eigen::Matrix3d R = computeLookAtRotation(eye, sceneCenter, worldUp); + output.getPoses().assign(idPV, sfmData::CameraPose(geometry::Pose3(R, eye))); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + // Landmarks randomly distributed in the cube [-0.5, 0.5]^3 near the origin. + // All landmarks are visible from every camera (cameras are at radius 10, looking inward). + std::srand(78901); + for (int id = 0; id < pointsNb; ++id) + { + const Eigen::Vector3d pt = Eigen::Vector3d::Random() * 0.5; + output.getLandmarks().emplace(id, sfmData::Landmark(pt, feature::EImageDescriberType::UNKNOWN)); + } + + // Each camera observes exactly obsPerCamera landmarks using a round-robin + // assignment over the landmark list. With obsPerCamera << pointsNb the + // bundle-adjustment Jacobian block for each camera has very few rows, so + // the camera translation is weakly determined and carries high positional + // uncertainty. Each landmark is seen by approximately + // posesNb * obsPerCamera / pointsNb + // cameras and is therefore reasonably well constrained. + const double arbitraryScale = 1.0; + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + + for (int k = 0; k < obsPerCamera; ++k) + { + const IndexT lmId = (static_cast(viewId) * obsPerCamera + k) % pointsNb; + auto& landmark = output.getLandmarks().at(lmId); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, lmId, arbitraryScale)); + } + } +} + +void generateClusteredObservationsScene(sfmData::SfMData& output, int pointsNb, int posesNb) +{ + const int w = 4092; + const int h = 2048; + const double focalLengthPixX = 10000.0; + const double focalLengthPixY = 20000.0; + output.getIntrinsics().emplace( + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, 0, 0)); + + // Cameras arranged on a circle of radius 10, all looking at the origin. + // worldUp = +Y (camera centres lie in the XZ plane). + const Eigen::Vector3d sceneCenter(0.0, 0.0, 0.0); + const Eigen::Vector3d worldUp(0.0, 1.0, 0.0); + for (IndexT idPV = 0; idPV < static_cast(posesNb); ++idPV) + { + const double angle2d = (double(idPV) * 2.0 * M_PI) / posesNb; + const Eigen::Vector3d eye(10.0 * std::cos(angle2d), 0.0, 10.0 * std::sin(angle2d)); + const Eigen::Matrix3d R = computeLookAtRotation(eye, sceneCenter, worldUp); + output.getPoses().assign(idPV, sfmData::CameraPose(geometry::Pose3(R, eye))); + output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); + } + + // Landmarks placed in a narrow cone pointing towards +Z direction from the origin. + // The cone has angular width ~±7.5° (≈15% of image width/height for a typical camera). + // All landmarks, when projected from any camera, fall into a small clustered region. + // This creates severe directional ambiguity: cameras see all features in nearly the + // same direction, leading to high depth and lateral position uncertainty. + std::srand(89012); + const double maxConeAngle = 7.5 * M_PI / 180.0; // ±7.5 degrees in radians (~13% field of view) + const double coneDepthMin = 0.5; + const double coneDepthMax = 2.0; + + for (int id = 0; id < pointsNb; ++id) + { + // Random direction within a narrow cone (small angles around +Z axis) + double theta = (std::rand() / double(RAND_MAX)) * 2.0 * maxConeAngle - maxConeAngle; // azimuth angle within cone + double phi = (std::rand() / double(RAND_MAX)) * 2.0 * maxConeAngle - maxConeAngle; // elevation angle within cone + double depth = coneDepthMin + (std::rand() / double(RAND_MAX)) * (coneDepthMax - coneDepthMin); + + // Convert spherical coordinates to Cartesian (with small angles, cos(angle) ≈ 1) + Eigen::Vector3d point3D; + point3D.x() = depth * std::sin(theta); + point3D.y() = depth * std::sin(phi); + point3D.z() = depth * std::cos(theta) * std::cos(phi); // ≈ depth for small angles + + output.getLandmarks().emplace(id, sfmData::Landmark(point3D, feature::EImageDescriberType::UNKNOWN)); + } + + // Build observations: all landmarks projected into all views. + // The narrow clustering ensures that all observations fall within ~15% × 15% of the image. + const double arbitraryScale = 1.0; + for (auto& [lmId, landmark] : output.getLandmarks()) + { + for (IndexT viewId = 0; viewId < static_cast(posesNb); ++viewId) + { + const sfmData::View& view = output.getView(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intr = output.getIntrinsic(0); + const Eigen::Vector2d pt = intr.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, lmId, arbitraryScale)); + } + } +} + } // namespace sfmDataIO } // namespace aliceVision diff --git a/src/aliceVision/sfmDataIO/sceneSample.hpp b/src/aliceVision/sfmDataIO/sceneSample.hpp index c35abb1d8d..62970721fb 100644 --- a/src/aliceVision/sfmDataIO/sceneSample.hpp +++ b/src/aliceVision/sfmDataIO/sceneSample.hpp @@ -11,26 +11,161 @@ namespace aliceVision { namespace sfmDataIO { +/// Type of synthetic scene geometry to generate. enum ESceneType { - SCENE_CUBE, - SCENE_SPHERE, + SCENE_CUBE, ///< Scene whose landmarks are placed on the faces of a cube. + SCENE_SPHERE, ///< Scene whose landmarks are distributed over a sphere. + SCENE_PLANAR, ///< Planar / bas-relief scene: all landmarks lie on the z=0 plane, making depth poorly constrained. + SCENE_COLLINEAR, ///< Collinear cameras: all camera centres lie on a single line, producing strongly anisotropic covariance. + SCENE_FORWARD_MOTION, ///< Forward-motion scene: cameras move along their optical axis, making depth and lateral translation hard to separate. + SCENE_NARROW_BASELINE, ///< Narrow-baseline scene: very small camera circle radius relative to scene depth → high depth uncertainty. + SCENE_UNEVEN_COVERAGE, ///< Uneven-coverage scene: some landmarks seen by all cameras, others by only two adjacent cameras. + SCENE_PURE_ROTATION, ///< Pure-rotation scene: all cameras share the same centre; radial depth of every landmark is unobservable. + SCENE_SPARSE_OBSERVATIONS, ///< Sparse-observations scene: each camera observes only a very small number of landmarks, so camera positions are weakly constrained and carry high positional uncertainty. + SCENE_CLUSTERED_OBSERVATIONS, ///< Clustered-observations scene: all landmark observations fall within a tiny 15% x 15% region of the image, creating severe directional ambiguity and high uncertainty. }; +/// @brief Convert a string representation to the corresponding ESceneType enum value. +/// @param SScene String name of the scene type (e.g. "cube" or "sphere"). +/// @return The matching ESceneType value. ESceneType ESceneType_stringToEnum(const std::string& SScene); + +/// @brief Convert an ESceneType enum value to its string representation. +/// @param EScene The scene type enum value to convert. +/// @return A string name for the given scene type. std::string ESceneType_enumToString(const ESceneType EScene); + +/// @brief Stream insertion operator for ESceneType (writes the string representation). std::ostream& operator<<(std::ostream& os, ESceneType p); + +/// @brief Stream extraction operator for ESceneType (reads from string representation). std::istream& operator>>(std::istream& in, ESceneType& p); /** * @brief Create an SfmData with some arbitrary content. * This is used in unit tests to validate read/write sfmData files. + * @param[out] output The SfMData structure to populate. + * @param scene The type of synthetic scene to generate (default: SCENE_CUBE). */ void generateSampleScene(sfmData::SfMData& output, ESceneType scene=ESceneType::SCENE_CUBE); +/** + * @brief Populate an SfMData with a synthetic cube scene. + * Landmarks are placed at the corners and face centres of a unit cube, + * with cameras positioned around it. + * @param[out] output The SfMData structure to populate. + */ void generateCubeScene(sfmData::SfMData& output); +/** + * @brief Populate an SfMData with a synthetic sphere scene. + * Landmarks are distributed over the surface of a sphere, with cameras + * arranged uniformly around it. + * @param[out] output The SfMData structure to populate. + * @param pointsNb Number of 3D landmarks to generate. + * @param posesNb Number of camera poses to generate. + */ void generateSphereScene(sfmData::SfMData& output, int pointsNb, int posesNb); +/** + * @brief Populate an SfMData with a planar (bas-relief) scene. + * All landmarks lie on the z=0 plane. Cameras look down at the plane from a circle + * above it. The depth direction (Z) of each landmark is poorly constrained, mimicking + * the bas-relief ambiguity seen in real photogrammetric reconstructions. + * @param[out] output The SfMData structure to populate. + * @param gridSize Side length of the square grid of landmarks (total = gridSize²). + * @param posesNb Number of camera poses arranged in a circle above the plane. + */ +void generatePlanarScene(sfmData::SfMData& output, int gridSize, int posesNb); + +/** + * @brief Populate an SfMData with a collinear-cameras scene. + * All camera centres lie on the X axis. The scene has no Y or Z baseline, so + * the covariance of landmark positions is strongly anisotropic: uncertainty + * is much larger in the direction orthogonal to the baseline. + * @param[out] output The SfMData structure to populate. + * @param pointsNb Number of 3D landmarks to generate. + * @param posesNb Number of camera poses uniformly spaced along the X axis. + */ +void generateCollinearCamerasScene(sfmData::SfMData& output, int pointsNb, int posesNb); + +/** + * @brief Populate an SfMData with a forward-motion (corridor) scene. + * All cameras move along their common optical axis (+Z). Because the baseline + * is parallel to the viewing direction, depth and the translational component + * along Z are poorly decorrelated, producing high depth uncertainty. + * @param[out] output The SfMData structure to populate. + * @param pointsNb Number of 3D landmarks to generate. + * @param posesNb Number of camera poses uniformly spaced along the Z axis. + */ +void generateForwardMotionScene(sfmData::SfMData& output, int pointsNb, int posesNb); + +/** + * @brief Populate an SfMData with a narrow-baseline scene. + * Identical geometry to the sphere scene, but the camera circle radius is + * very small (0.05) relative to the scene depth (unit sphere). The large + * depth-to-baseline ratio causes high depth uncertainty for all landmarks. + * @param[out] output The SfMData structure to populate. + * @param pointsNb Number of 3D landmarks to generate. + * @param posesNb Number of camera poses uniformly spaced on the narrow circle. + */ +void generateNarrowBaselineScene(sfmData::SfMData& output, int pointsNb, int posesNb); + +/** + * @brief Populate an SfMData with an uneven-coverage scene. + * @c wellObservedNb landmarks (near the origin) are seen by every camera. + * @c poorlyObservedNb landmarks (on the unit sphere surface) are each seen by + * only two adjacent cameras, making their covariances much larger than those of + * the well-observed set. This highlights non-uniform uncertainty in a single scene. + * @param[out] output The SfMData structure to populate. + * @param wellObservedNb Number of landmarks observed by all cameras. + * @param poorlyObservedNb Number of landmarks each observed by exactly 2 cameras. + * @param posesNb Number of camera poses arranged in a circle. + */ +void generateUnevenCoverageScene(sfmData::SfMData& output, int wellObservedNb, int poorlyObservedNb, int posesNb); + +/** + * @brief Populate an SfMData with a pure-rotation scene. + * All camera poses share the same centre (the origin) and differ only in + * orientation. The Jacobian derivative with respect to the radial depth of + * any landmark is identically zero under perspective projection, so depths + * are completely unobservable. The resulting covariance is dominated by + * the regularisation term used to handle the gauge freedom. + * @param[out] output The SfMData structure to populate. + * @param pointsNb Number of 3D landmarks to generate. + * @param posesNb Number of camera poses with distinct rotations. + */ +void generatePureRotationScene(sfmData::SfMData& output, int pointsNb, int posesNb); + +/** + * @brief Populate an SfMData with a sparse-observations scene. + * Cameras are arranged on a circle of radius 10, but each camera observes + * only @p obsPerCamera landmarks (round-robin assignment). With so few + * geometric constraints per camera, the camera translation parameters are + * weakly determined by the bundle-adjustment Jacobian and carry high + * positional uncertainty, while landmarks (each seen by multiple cameras) + * remain reasonably well constrained. + * @param[out] output The SfMData structure to populate. + * @param pointsNb Number of 3D landmarks distributed near the origin. + * @param posesNb Number of camera poses arranged in a circle. + * @param obsPerCamera Number of landmarks each camera observes (must be > 0 + * and <= pointsNb). + */ +void generateSparseCameraObservationsScene(sfmData::SfMData& output, int pointsNb, int posesNb, int obsPerCamera); + +/** + * @brief Populate an SfMData with a clustered-observations scene. + * Landmarks are positioned such that all their image-space observations fall + * within a tiny region (approximately 15% × 15% of image dimensions). + * This creates severe directional ambiguity: all cameras observe features in + * nearly the same image direction, leading to high uncertainty in depth and + * the lateral position of landmarks in the plane perpendicular to the viewing direction. + * @param[out] output The SfMData structure to populate. + * @param pointsNb Number of 3D landmarks to generate. + * @param posesNb Number of camera poses arranged in a circle. + */ +void generateClusteredObservationsScene(sfmData::SfMData& output, int pointsNb, int posesNb); + } // namespace sfmDataIO } // namespace aliceVision diff --git a/src/aliceVision/uncertainty/CMakeLists.txt b/src/aliceVision/uncertainty/CMakeLists.txt new file mode 100644 index 0000000000..016eb4da27 --- /dev/null +++ b/src/aliceVision/uncertainty/CMakeLists.txt @@ -0,0 +1,38 @@ +# Headers +set(uncertainty_files_headers + uncertainty_kernels.hpp +) + +# Sources +set(uncertainty_files_sources + uncertainty.cpp + uncertainty_kernels.cu +) + +alicevision_add_library(aliceVision_uncertainty + USE_CUDA + SOURCES ${uncertainty_files_headers} ${uncertainty_files_sources} + PUBLIC_LINKS + aliceVision_sfmData + ${MAGMA_LIBRARIES} + CUDA::cudart + PRIVATE_LINKS + aliceVision_system +) + +# Unit tests +alicevision_add_test(uncertainty_kernels_test.cpp + NAME "uncertainty_kernels" + LINKS aliceVision_uncertainty CUDA::cudart +) + +alicevision_add_test(uncertainty_test.cpp + NAME "uncertainty" + LINKS aliceVision_uncertainty + aliceVision_sfm + aliceVision_sfmDataIO + aliceVision_sfmData + aliceVision_feature + aliceVision_system + CUDA::cudart +) \ No newline at end of file diff --git a/src/aliceVision/uncertainty/magma.hpp b/src/aliceVision/uncertainty/magma.hpp new file mode 100644 index 0000000000..6390c7b932 --- /dev/null +++ b/src/aliceVision/uncertainty/magma.hpp @@ -0,0 +1,56 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include "magma_v2.h" +#include +#include + +namespace aliceVision +{ +namespace uncertainty +{ + +struct MagmaBuffer +{ + double* ptr = nullptr; + + MagmaBuffer() = default; + + explicit MagmaBuffer(magma_int_t count) + { + if (magma_dmalloc(&ptr, count) != MAGMA_SUCCESS) + { + ptr = nullptr; + } + } + + ~MagmaBuffer() { magma_free(ptr); } + + MagmaBuffer(const MagmaBuffer &) = delete; + MagmaBuffer & operator=(const MagmaBuffer &) = delete; + + MagmaBuffer(MagmaBuffer && o) noexcept : ptr(o.ptr) { o.ptr = nullptr; } + + MagmaBuffer & operator=(MagmaBuffer && o) noexcept + { + if (this != &o) + { + magma_free(ptr); + ptr = o.ptr; + o.ptr = nullptr; + } + return *this; + } + + bool valid() const { return ptr != nullptr; } + operator double*() const { return ptr; } +}; + + +} +} \ No newline at end of file diff --git a/src/aliceVision/uncertainty/uncertainty.cpp b/src/aliceVision/uncertainty/uncertainty.cpp new file mode 100644 index 0000000000..021a247e8a --- /dev/null +++ b/src/aliceVision/uncertainty/uncertainty.cpp @@ -0,0 +1,187 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include + +#include +#include + +namespace +{ + double factorial(int n) + { + if (n > 1) + return n * factorial(n - 1); + else + return 1; + } +} + +namespace aliceVision +{ +namespace uncertainty +{ + +/** + * Computes an approximate inverse of the camera Schur-complement Hessian Hcc. + * + * REGULARIZATION + * Hcc may be singular or near-singular, so it is first regularized: + * + * Z = Hcc + lambda * I, lambda = 10^(-1.2653 * log10(n) - 2.9415) + * + * where n = Hcc.rows(). The formula empirically scales lambda with problem + * size so that Z is strictly positive definite while minimally perturbing Hcc. + * + * CHOLESKY INVERSION OF Z + * Since Z is symmetric positive definite, its inverse is computed via: + * + * Z = L * L^T (dpotrf, Cholesky factorization) + * Z^{-1} (dpotri, inversion from the Cholesky factor) + * + * This is ~2x cheaper in flops than a general LU factorization. + * + * NEUMANN-SERIES CORRECTION + * The exact Hcc^{-1} is recovered from Z^{-1} via the identity: + * + * Hcc^{-1} = (Z - lambda*I)^{-1} + * + * Writing Hcc = Z - lambda*I and factoring out Z^{-1}: + * + * Hcc^{-1} = Z^{-1} * (I - lambda * Z^{-1})^{-1} + * + * Expanding the geometric series (converges when ||lambda * Z^{-1}|| < 1): + * + * (I - lambda * Z^{-1})^{-1} = sum_{k=0}^{inf} (lambda * Z^{-1})^k + * + * Substituting back and collecting powers of Z^{-1}: + * + * Hcc^{-1} = Z^{-1} + sum_{i=1}^{inf} [ lambda^i / (i-1)! ] * Z^{-(i+1)} + * + * The series is truncated at i = 19. Each term contributes + * k_i * Z^{-(i+1)}, k_i = lambda^i / (i-1)! + * + * Since lambda << 1 (typically ~1e-7 for n=2400), k_i decays super-exponentially + * and the series converges in very few iterations in practice. + * + * CONVERGENCE GUARD + * To protect against numerical divergence, each term is only accumulated into + * the result if lambda * max|Z^{-(i+1)}| has not increased relative to the + * previous iteration. This check is performed entirely on the GPU with no + * CPU synchronization between iterations. + */ +bool computeHessianInverse(Eigen::MatrixXd & HccInverse, const Eigen::MatrixXd & Hcc, magma_queue_t queue) +{ + double lambda = pow(10, -1.2653*log10(Hcc.rows()) - 2.9415); + Eigen::MatrixXd Z = Hcc + lambda * Eigen::MatrixXd::Identity(Hcc.rows(), Hcc.cols()); + + const magma_int_t n = static_cast(Z.rows()); + magma_int_t magmaError; + + // Allocate 4 GPU buffers: dIZorig (Z^{-1}), dHcc (series accumulator), + // dIZadd (current power of Z^{-1}), dTemp (scratch for multiply) + MagmaBuffer dIZorig(n * n); + MagmaBuffer dHcc(n * n); + MagmaBuffer dIZadd(n * n); + MagmaBuffer dTemp(n * n); + + if (!dIZorig.valid() || !dHcc.valid() || !dIZadd.valid() || !dTemp.valid()) + { + ALICEVISION_LOG_ERROR("computeHessianInverse: GPU memory allocation failed (n=" << n << ")"); + return false; + } + + // Uploading Z to GPU (-> DIZorig) + magma_dsetmatrix(n, n, Z.data(), n, dIZorig, n, queue); + + // Cholesky factorization: dIZorig = L * L^T (lower triangle stored in-place). + // Requires Z to be symmetric positive definite; info > 0 means the leading + // minor of order info is not positive definite (Z is not SPD). + // Z being not SPD would mean that the selected lambda is not large enough + magma_dpotrf_gpu(MagmaLower, n, dIZorig, n, &magmaError); + if (magmaError != 0) + { + ALICEVISION_LOG_ERROR("computeHessianInverse: magma_dpotrf_gpu failed."); + ALICEVISION_LOG_ERROR("Reason : " << magma_strerror(magmaError)); + return false; + } + + // Inversion from the Cholesky factor: dIZorig <- Z^{-1} (lower triangle only). + // Uses the factored form produced by dpotrf; ~2x cheaper than a general LU inversion. + // Only the lower triangle of dIZorig is valid after this call. + magma_dpotri_gpu(MagmaLower, n, dIZorig, n, &magmaError); + if (magmaError != 0) + { + ALICEVISION_LOG_ERROR("computeHessianInverse: magma_dpotrf_gpu failed."); + ALICEVISION_LOG_ERROR("Reason : " << magma_strerror(magmaError)); + return false; + } + + // potri fills only the lower triangle; download, symmetrize on CPU, re-upload + HccInverse.resize(n, n); + magma_dgetmatrix(n, n, dIZorig, n, HccInverse.data(), n, queue); + HccInverse = HccInverse.selfadjointView(); + magma_dsetmatrix(n, n, HccInverse.data(), n, dIZorig, n, queue); + + // dHcc = Z^{-1} (first term of the series: i=0) + magma_dcopymatrix(n, n, dIZorig, n, dHcc, n, queue); + + // dIZadd = Z^{-2} = Z^{-1} * Z^{-1} + // dIZorig is fully symmetrized (both triangles valid), so dgemm is used + // throughout — it is significantly faster than dsymm for large n on GPU. + magma_dgemm(MagmaNoTrans, MagmaNoTrans, n, n, n, 1.0, dIZorig, n, dIZorig, n, 0.0, dIZadd, n, queue); + + // Two device scalars for the GPU-side convergence check: + // d_cur_maxabs — max|dIZadd| for the current iteration (written by reduction kernel) + // d_prev_norm — lambda * max|dIZadd| from the previous iteration + // Initialized to +inf so the first iteration always passes the check. + MagmaBuffer d_cur_maxabs(1); + MagmaBuffer d_prev_norm(1); + if (!d_cur_maxabs.valid() || !d_prev_norm.valid()) + { + ALICEVISION_LOG_ERROR("computeHessianInverse: GPU scalar allocation failed"); + return false; + } + + cudaStream_t stream = magma_queue_get_cuda_stream(queue); + + // d_prev_norm = infinity; + const double inf = std::numeric_limits::infinity(); + cudaMemcpyAsync(d_prev_norm, &inf, sizeof(double), cudaMemcpyHostToDevice, stream); + + for (int i = 1; i < 20; i++) + { + double k = pow(lambda, i) / factorial(i - 1); + + // Compute max|dIZadd| fully on GPU (no CPU sync; result in d_cur_maxabs) + launchMaxAbsReduction(dIZadd, d_cur_maxabs, n * n, stream); + + // dHcc += k * dIZadd, but only when lambda * max|dIZadd| has not increased. + // The kernel skips the axpy and leaves dHcc unchanged when diverging. + // d_prev_norm is updated in-place by the kernel when the check passes. + // Note that even if the value increase, the computations are still done but ignored. + // This is better than having to deal with costly cpu<-->gpu synchronisation + launchConditionalAxpy(k, dIZadd, dHcc, d_cur_maxabs, d_prev_norm, lambda, n * n, stream); + + // Advance the power: dTemp = dIZadd * dIZorig + magma_dgemm(MagmaNoTrans, MagmaNoTrans, n, n, n, 1.0, dIZadd, n, dIZorig, n, 0.0, dTemp, n, queue); + + std::swap(dIZadd, dTemp); + } + + // Download result from GPU to host + // The synchronisation with the GPU is done here. + magma_dgetmatrix(n, n, dHcc, n, HccInverse.data(), n, queue); + + // Although theorical symmetry is preserved, + // It is better to enforce numerically this symmetry here. + HccInverse = HccInverse.selfadjointView(); + + return true; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/uncertainty/uncertainty.hpp b/src/aliceVision/uncertainty/uncertainty.hpp new file mode 100644 index 0000000000..aafb6ca43f --- /dev/null +++ b/src/aliceVision/uncertainty/uncertainty.hpp @@ -0,0 +1,428 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "uncertainty_kernels.hpp" + +namespace aliceVision +{ +namespace uncertainty +{ + + + +/** + * @brief Compute the pseudoinverse of J^T J via CPU-side SVD (dense, exact). + * + * Intended for small-to-medium systems where an exact pseudoinverse is needed, + * e.g. to handle the gauge freedom of the reconstruction without regularization. + * + * Algorithm: + * 1. Form the normal matrix A = J^T J (numParams x numParams, dense). + * 2. Compute the full SVD A = U S V^T via LAPACK dgesvd. + * 3. Threshold singular values: s_i^+ = 1/s_i if s_i > 1e-12, else 0. + * This suppresses the 7-dimensional gauge null space (3 translation + + * 3 rotation + 1 scale) and any other numerical near-zero directions. + * 4. Assemble A^+ = V S^+ U^T via BLAS dgemm. + * + * The result stored in @p inverse is the Moore-Penrose pseudoinverse of J^T J, + * which equals the covariance matrix of the parameters in the observable + * (gauge-fixed) subspace. + * + * @tparam Derived Eigen dense matrix expression type. + * @param[out] inverse Pseudoinverse of J^T J (numParams x numParams). + * Must be pre-allocated to the correct size before calling. + * @param[in] jacobian The residual Jacobian J (residuals x numParams, dense). + * @return true on success; false if LAPACK reports an error (logged internally). + */ +template +bool computeDenseHessianInverse(Eigen::MatrixXd & inverse, const Eigen::MatrixBase & jacobian) +{ + using DRMatrix = Eigen::Matrix; + using DRVector = Eigen::Vector; + + DRMatrix JtJ = jacobian.transpose() * jacobian; + DRMatrix copy = JtJ; + + + int numParameters = JtJ.rows(); + int lwork = numParameters*numParameters + 3*numParameters + 2*numParameters * 32;; + std::vector hwork(lwork); + + DRVector sv(numParameters); + DRMatrix U(numParameters, numParameters); + DRMatrix Vt(numParameters, numParameters); + + magma_int_t err; + lapackf77_dgesvd( + lapack_vec_const(MagmaAllVec), + lapack_vec_const(MagmaAllVec), + &numParameters, + &numParameters, + JtJ.data(), + &numParameters, + sv.data(), + U.data(), + &numParameters, + Vt.data(), + &numParameters, + hwork.data(), + &lwork, + &err); + + if (err) + { + ALICEVISION_LOG_ERROR("lapackf77_dgesvd"); + ALICEVISION_LOG_ERROR(magma_strerror(err)); + return false; + } + + + for (int i = 0; i < numParameters; ++i) + { + for (int j = 0; j < numParameters; ++j) + { + U(i, j) *= (sv(j) < 1e-12) ? 0.0 : 1.0 / sv(j); + } + } + + + double alpha = 1, beta = 0; + blasf77_dgemm( + lapack_trans_const(MagmaNoTrans), + lapack_trans_const(MagmaNoTrans), + &numParameters, + &numParameters, + &numParameters, + &alpha, + U.data(), + &numParameters, + Vt.data(), + &numParameters, + &beta, + inverse.data(), + &numParameters); + + return true; +} + +/** + * @brief Compute an approximate inverse of the camera Schur-complement Hessian. + * + * See the implementation in uncertainty.cpp for a full mathematical description. + * In brief: + * 1. Regularize: Z = Hcc + lambda * I (lambda chosen to ensure Z is SPD). + * 2. Invert Z via GPU Cholesky (dpotrf + dpotri). + * 3. Recover Hcc^{-1} via a Neumann series correction truncated at 19 terms: + * Hcc^{-1} \approx Z^{-1} + \sum_{i=1}^{19} [\lambda^i / (i-1)!] * Z^{-(i+1)} + * The series is evaluated on GPU using MAGMA dgemm; each term is conditionally + * accumulated based on a GPU-side convergence check with no CPU synchronization. + * + * @param[out] HccInverse Approximate inverse of Hcc (n x n, symmetric). + * @param[in] Hcc Camera Schur-complement Hessian (n x n, symmetric SPD after regularization). + * @param[in] queue Active MAGMA queue (provides the CUDA stream used for all GPU operations). + * @return true on success; false if GPU allocation fails or Cholesky detects a non-SPD matrix. + */ +bool computeHessianInverse(Eigen::MatrixXd & HccInverse, const Eigen::MatrixXd & Hcc, magma_queue_t queue); + +/** + * @brief Build the camera Schur-complement Hessian Hcc from the BA Jacobian. + * + * Given the full bundle-adjustment Jacobian J partitioned as: + * + * J = [ Jc | Jp ] (Jc: camera columns, Jp: point/landmark columns) + * + * the full Hessian is: + * + * JtJ = [ U W ] U = Jc^T Jc, W = Jc^T Jp, V = Jp^T Jp + * [ W^T V ] + * + * The Schur complement (marginalizing out landmarks) is: + * + * Hcc = U - W V^{-1} W^T + * + * V is block-diagonal (3x3 per landmark), so V^{-1} is computed per-landmark + * via Cholesky (LLT). Each 3x3 block is inverted independently. + * + * The Jacobian is expected to be column-scaled by @p vectorScale before + * accumulation, i.e. the effective Jacobian used is J * diag(vectorScale). + * This scaling is applied element-wise during the sparse pass and must be + * undone on the result by the caller. + * + * @tparam Derived Row-major Eigen sparse matrix type. + * @param[out] Hcc Camera Schur-complement Hessian (camParams x camParams, symmetric). + * @param[in] vectorScale Per-column scale vector of length J.cols(). + * @param[in] jacobian Row-major sparse Jacobian (residuals x (camParams + landmarkParams)). + * @param[in] countCameras Number of camera blocks (each contributes 6 parameters). + * @param[in] countLandmarks Number of landmark blocks (each contributes 3 parameters). + * @return true on success. + */ +template +bool computeHessianCameras(Eigen::MatrixXd & Hcc, const Eigen::VectorXd & vectorScale, const Derived & jacobian, size_t countCameras, size_t countLandmarks) +{ + int camBlocks = countCameras; + int camParams = camBlocks * 6; + int landmarkBlocks = countLandmarks; + int landmarkParams = landmarkBlocks * 3; + + // Here we decompose JtJ = + // [U W] + // [Wt V] + + // U: dense cam×cam Schur complement accumulator (symmetric) + Eigen::MatrixXd U = Eigen::MatrixXd::Zero(camParams, camParams); + + struct ResidualContrib { + // Camera indexed jacobian + std::vector>> gradientsCameras; + Eigen::Vector3d gradientPoint = Eigen::Vector3d::Zero(); + }; + + struct LandmarkAccum { + Eigen::Matrix3d V = Eigen::Matrix3d::Zero(); + std::vector residuals; + }; + + // One LandmarkAccum per landmark + std::vector landmarkAccumulators(landmarkBlocks); + + // Single O(nnz) pass over J rows + for (int row = 0; row < jacobian.outerSize(); ++row) + { + std::map> gradientsCameras; + Eigen::Vector3d gradientPoint = Eigen::Vector3d::Zero(); + int lmIdx = -1; + + // Distribute the jacobian per block (camera / landmark) + for (typename Derived::InnerIterator it(jacobian, row); it; ++it) + { + const int column = it.col(); + const double cellValue = it.value() * vectorScale(column); + + if (column < camParams) + { + // Compute initial column for camera + // base is a multiple of 6 : It is the start column of the camera + const int base = (column / 6) * 6; + + // Store the value for this camera + gradientsCameras[base](column - base) = cellValue; + } + else + { + // Compute initial column for landmark + const int localId = column - camParams; + const int idx = localId / 3; + const int base = idx * 3; + + // There is only one landmark per residual + if (lmIdx < 0) + { + lmIdx = idx; + } + + // Store the jacobian for the landmark + gradientPoint(localId - base) = cellValue; + } + } + + if (gradientsCameras.empty() && lmIdx < 0) + { + continue; + } + + // Landmark-only residual (e.g. position prior) — accumulate V only + // Rank-1 updates to U: all pairs of camera blocks in this row + for (auto & [base_i, cam_i] : gradientsCameras) + { + for (auto & [base_j, cam_j] : gradientsCameras) + { + U.block<6,6>(base_i, base_j).noalias() += cam_i * cam_j.transpose(); + } + } + + if (lmIdx < 0) + { + continue; + } + + auto & landmarkAccumulator = landmarkAccumulators[lmIdx]; + + // Rank-1 update to V + landmarkAccumulator.V.noalias() += gradientPoint * gradientPoint.transpose(); + + // Store residual for Schur complement only if cameras are present + // (landmark-only priors contribute to V but not to the cross-term W) + if (!gradientsCameras.empty()) + { + ResidualContrib rc; + rc.gradientPoint = gradientPoint; + for (auto & [base, cam] : gradientsCameras) + { + rc.gradientsCameras.emplace_back(base, cam); + } + + landmarkAccumulator.residuals.push_back(std::move(rc)); + } + } + + // U is symmetric — it was built by accumulating all (cam_i, cam_j) pairs + // including cross-camera terms, so just symmetrize the full matrix + U = U.selfadjointView(); + + // Schur complement: S = U - sum_b W_b * V_b^{-1} * W_b^T + Hcc = U; + for (int b = 0; b < landmarkBlocks; ++b) + { + auto & landmarkAccumulator = landmarkAccumulators[b]; + if (landmarkAccumulator.residuals.empty()) + { + continue; + } + + Eigen::LLT llt(landmarkAccumulator.V); + + for (const auto & ri : landmarkAccumulator.residuals) + { + const Eigen::Vector3d z_i = llt.solve(ri.gradientPoint); + + for (const auto & rj : landmarkAccumulator.residuals) + { + const double scale = rj.gradientPoint.dot(z_i); + + for (const auto & [base_i, cam_i] : ri.gradientsCameras) + { + for (const auto & [base_j, cam_j] : rj.gradientsCameras) + { + Hcc.block<6,6>(base_i, base_j).noalias() -= scale * (cam_i * cam_j.transpose()); + } + } + } + } + } + + return true; +} + +/** + * @brief Compute the covariance matrix of all camera parameters from the BA Jacobian. + * + * This is the top-level entry point. The full pipeline is: + * + * 1. **Column scaling**: normalize each Jacobian column by 1/||col|| so that + * all parameters live on a comparable scale. Degenerate columns (norm < 1e-12) + * are zeroed. + * + * 2. **Schur complement** (computeHessianCameras): build the scaled camera + * Hessian Hcc' = Dc * Hcc * Dc, where Dc = diag(invNorms[0..camParams-1]), + * by marginalizing out the landmark parameters. + * + * 3. **Inversion** (computeHessianInverse): compute Hcc'^{-1} on GPU via + * Cholesky + Neumann series correction. + * + * 4. **Unscaling**: recover the covariance in the original parameter units: + * Sigma_cc = Dc * Hcc'^{-1} * Dc + * (equivalent to Dc^{-1} * (Hcc'^{-1}) * Dc^{-1} composed with the + * scaling identity Dc^2 = Dc * Dc). + * + * The result @p covarianceCameras is a (6*countCameras) x (6*countCameras) + * symmetric positive-semidefinite matrix. Rows/columns are ordered as + * [cam_0 (6 params), cam_1 (6 params), ..., cam_{N-1} (6 params)]. + * + * @note The gauge freedom (7 DOF: 3 translation + 3 rotation + 1 scale) is + * handled implicitly by the lambda regularization in computeHessianInverse. + * For gauge-invariant per-camera covariances, consider fixing the gauge + * explicitly before calling this function. + * + * @tparam Derived Row-major Eigen sparse matrix type. + * @param[out] covarianceCameras Output covariance matrix (6*countCameras x 6*countCameras). + * @param[in] jacobian Row-major sparse BA Jacobian. + * @param[in] countCameras Number of cameras. + * @param[in] countLandmarks Number of landmarks. + * @return true on success; false on any GPU or structural error. + */ +template +bool computeUncertainty(Eigen::MatrixXd & covarianceCameras, const Eigen::SparseMatrixBase & jacobian, size_t countCameras, size_t countLandmarks) +{ + static_assert(Derived::IsRowMajor, "jacobian must be a row-major sparse matrix (RowMajor storage order)"); + + magma_int_t err = magma_init(); + if (err) + { + ALICEVISION_LOG_ERROR("Error on magma_init: " << magma_strerror(err)); + return false; + } + + magma_queue_t queue; + magma_queue_create(0, &queue); + + utils::ScopeGuard guard([&queue]() { + magma_queue_destroy(queue); + magma_finalize(); + }); + + const Derived & J = jacobian.derived(); + const int n = J.cols(); + + // Build the the squared column norms of J + // We iterate per row as it is the storage of the matrix + Eigen::VectorXd colSqNorms = Eigen::VectorXd::Zero(n); + for (int row = 0; row < J.outerSize(); ++row) + { + for (typename Derived::InnerIterator it(J, row); it; ++it) + { + colSqNorms(it.col()) += it.value() * it.value(); + } + } + + // Compute the scale, which is the inverse of the square root of colSqNorm + Eigen::VectorXd invNorms = (colSqNorms.array() < 1e-24).select(Eigen::ArrayXd::Zero(n), colSqNorms.array().sqrt().inverse()); + + int camBlocks = countCameras; + int camParams = camBlocks * 6; + int landmarkBlocks = countLandmarks; + int landmarkParams = landmarkBlocks * 3; + + if (camParams + landmarkParams != n) + { + ALICEVISION_LOG_ERROR("Incorrect structure: camParams + landmarkParams != J.cols()"); + return false; + } + + Eigen::MatrixXd Hcc; + if (!computeHessianCameras(Hcc, invNorms, J, camBlocks, landmarkBlocks)) + { + ALICEVISION_LOG_ERROR("Failed to build Hcc"); + return false; + } + + Eigen::MatrixXd Hcc_inv; + if (!computeHessianInverse(Hcc_inv, Hcc, queue)) + { + ALICEVISION_LOG_ERROR("Failed to build inverse"); + return false; + } + + // Cancel the scaling + const Eigen::VectorXd invNorms_c = invNorms.head(camParams); + covarianceCameras = invNorms_c.asDiagonal() * Hcc_inv * invNorms_c.asDiagonal(); + + return true; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/uncertainty/uncertainty_kernels.cu b/src/aliceVision/uncertainty/uncertainty_kernels.cu new file mode 100644 index 0000000000..8198a180a5 --- /dev/null +++ b/src/aliceVision/uncertainty/uncertainty_kernels.cu @@ -0,0 +1,147 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "uncertainty_kernels.hpp" + +#include + +namespace aliceVision +{ +namespace uncertainty +{ + +// --------------------------------------------------------------------------- +// Max-abs reduction kernel +// --------------------------------------------------------------------------- +// One block per tile of BLOCK_SIZE elements; each block reduces to a single +// value via shared memory, then atomically updates the global result. +// The caller must zero d_result before launching. + +static constexpr int BLOCK_SIZE = 256; + +// atomicMax for doubles via CAS loop (not natively supported before Volta) +__device__ __forceinline__ void atomicMaxDouble(double* addr, double val) +{ + unsigned long long* addr_ull = reinterpret_cast(addr); + unsigned long long old_ull = *addr_ull; + unsigned long long new_ull; + do + { + double old_val = __longlong_as_double(static_cast(old_ull)); + if (old_val >= val) return; + new_ull = static_cast(__double_as_longlong(val)); + old_ull = atomicCAS(addr_ull, old_ull, new_ull); + } while (__longlong_as_double(static_cast(old_ull)) < val); +} + +__global__ void maxAbsReductionKernel(const double* __restrict__ data, double* result, int n) +{ + __shared__ double sdata[BLOCK_SIZE]; + + const int tid = threadIdx.x; + const int gid = blockIdx.x * blockDim.x + tid; + + sdata[tid] = (gid < n) ? fabs(data[gid]) : 0.0; + __syncthreads(); + + // Tree reduction within block + for (int s = blockDim.x / 2; s > 0; s >>= 1) + { + if (tid < s) + sdata[tid] = fmax(sdata[tid], sdata[tid + s]); + __syncthreads(); + } + + if (tid == 0) + atomicMaxDouble(result, sdata[0]); +} + +// --------------------------------------------------------------------------- +// Conditional axpy kernel +// --------------------------------------------------------------------------- +// Thread 0 in block 0 decides whether to execute the axpy. +// The decision is broadcast via shared memory so all threads in the grid +// read it from L1 cache rather than global memory. +// +// Layout: one kernel launch with enough blocks to cover n elements. +// Block 0 additionally runs the comparison and update logic in thread 0. + +__global__ void conditionalAxpyKernel(double k, + const double* __restrict__ src, + double* __restrict__ dst, + const double* d_cur_maxabs, + double* d_prev_norm, + double lambda, + int n) +{ + // Block 0, thread 0: compare and update d_prev_norm + __shared__ int do_axpy; + if (blockIdx.x == 0 && threadIdx.x == 0) + { + double cur = lambda * (*d_cur_maxabs); + do_axpy = (cur <= *d_prev_norm) ? 1 : 0; + if (do_axpy) + { + *d_prev_norm = cur; + } + } + + // All threads in block 0 wait for the decision; other blocks read it + // from global memory via the shared flag after block 0 has written it. + // Since blocks can execute in any order we use a device-side broadcast: + // block 0 writes do_axpy to d_cur_maxabs[1] (we repurpose a spare slot) + // — but that requires an extra device buffer. + // + // Simpler correct approach: re-read d_prev_norm comparison in every block. + // (One extra global read per block, negligible vs the axpy bandwidth.) + __syncthreads(); // sync within block 0 + + // Non-block-0 threads recompute the flag from global memory + int local_do_axpy; + if (blockIdx.x == 0) + { + local_do_axpy = do_axpy; + } + else + { + // d_prev_norm was updated by block 0 only if do_axpy; check the + // pre-update value by comparing directly (re-read is safe: block 0 + // has already written d_prev_norm before this block can read it + // because grids execute sequentially per stream). + local_do_axpy = (lambda * (*d_cur_maxabs) <= *d_prev_norm) ? 1 : 0; + } + + if (!local_do_axpy) return; + + const int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx < n) + dst[idx] += k * src[idx]; +} + +// --------------------------------------------------------------------------- +// Host-callable wrappers +// --------------------------------------------------------------------------- + +void launchMaxAbsReduction(const double* d_data, double* d_result, int n, cudaStream_t stream) +{ + // Zero the result first (done via a small cudaMemsetAsync) + cudaMemsetAsync(d_result, 0, sizeof(double), stream); + + const int blocks = (n + BLOCK_SIZE - 1) / BLOCK_SIZE; + maxAbsReductionKernel<<>>(d_data, d_result, n); +} + +void launchConditionalAxpy(double k, const double* d_src, double* d_dst, + const double* d_cur_maxabs, double* d_prev_norm, + double lambda, int n, cudaStream_t stream) +{ + const int blocks = (n + BLOCK_SIZE - 1) / BLOCK_SIZE; + conditionalAxpyKernel<<>>( + k, d_src, d_dst, d_cur_maxabs, d_prev_norm, lambda, n); +} + +} // namespace uncertainty +} // namespace aliceVision diff --git a/src/aliceVision/uncertainty/uncertainty_kernels.hpp b/src/aliceVision/uncertainty/uncertainty_kernels.hpp new file mode 100644 index 0000000000..cfc8e4d002 --- /dev/null +++ b/src/aliceVision/uncertainty/uncertainty_kernels.hpp @@ -0,0 +1,41 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include + +namespace aliceVision +{ +namespace uncertainty +{ + +/** + * @brief Compute the maximum absolute value of a device array and write it to a device scalar. + * + * Launches a parallel reduction over @p n elements of @p d_data. + * The result is written to @p d_result (device pointer, 1 element). + * Must be called on @p stream so it is sequenced with other MAGMA operations. + */ +void launchMaxAbsReduction(const double* d_data, double* d_result, int n, cudaStream_t stream); + +/** + * @brief Conditionally accumulate k * d_src into d_dst on GPU, skipping when the series diverges. + * + * Thread 0 reads d_cur_maxabs and computes cur = lambda * (*d_cur_maxabs). + * If cur <= *d_prev_norm the axpy is performed (series still converging) and *d_prev_norm is + * updated to cur. Otherwise the axpy is skipped and *d_prev_norm is left unchanged so that + * subsequent iterations also fail the check and are also skipped. + * + * Both d_cur_maxabs and d_prev_norm are device pointers (1 double each). + * Must be called on @p stream so it is sequenced with the preceding maxAbsReduction kernel. + */ +void launchConditionalAxpy(double k, const double* d_src, double* d_dst, + const double* d_cur_maxabs, double* d_prev_norm, + double lambda, int n, cudaStream_t stream); + +} // namespace uncertainty +} // namespace aliceVision diff --git a/src/aliceVision/uncertainty/uncertainty_kernels_test.cpp b/src/aliceVision/uncertainty/uncertainty_kernels_test.cpp new file mode 100644 index 0000000000..83206c35b3 --- /dev/null +++ b/src/aliceVision/uncertainty/uncertainty_kernels_test.cpp @@ -0,0 +1,288 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#define BOOST_TEST_MODULE uncertainty_kernels + +#include +#include + +#include "uncertainty_kernels.hpp" + +#include + +#include +#include +#include + +using namespace aliceVision::uncertainty; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +namespace +{ + +/// Allocate device memory, copy host data in, return the device pointer. +double* toDevice(const std::vector& host) +{ + double* d = nullptr; + cudaMalloc(&d, host.size() * sizeof(double)); + cudaMemcpy(d, host.data(), host.size() * sizeof(double), cudaMemcpyHostToDevice); + return d; +} + +/// Copy a single device double back to the host. +double fromDevice(const double* d) +{ + double h = 0.0; + cudaMemcpy(&h, d, sizeof(double), cudaMemcpyDeviceToHost); + return h; +} + +/// Allocate a device scalar and initialise it to val. +double* deviceScalar(double val) +{ + double* d = nullptr; + cudaMalloc(&d, sizeof(double)); + cudaMemcpy(d, &val, sizeof(double), cudaMemcpyHostToDevice); + return d; +} + +} // namespace + +// --------------------------------------------------------------------------- +// launchMaxAbsReduction tests +// --------------------------------------------------------------------------- + +BOOST_AUTO_TEST_CASE(MaxAbsReduction_AllPositive) +{ + // max(|v|) = 5.0 + const std::vector data = {1.0, 2.0, 3.0, 4.0, 5.0}; + double* d_data = toDevice(data); + double* d_result = deviceScalar(0.0); + + launchMaxAbsReduction(d_data, d_result, static_cast(data.size()), nullptr); + cudaDeviceSynchronize(); + + BOOST_CHECK_CLOSE(fromDevice(d_result), 5.0, 1e-10); + + cudaFree(d_data); + cudaFree(d_result); +} + +BOOST_AUTO_TEST_CASE(MaxAbsReduction_AllNegative) +{ + // max(|v|) = 7.0 + const std::vector data = {-7.0, -3.0, -1.0}; + double* d_data = toDevice(data); + double* d_result = deviceScalar(0.0); + + launchMaxAbsReduction(d_data, d_result, static_cast(data.size()), nullptr); + cudaDeviceSynchronize(); + + BOOST_CHECK_CLOSE(fromDevice(d_result), 7.0, 1e-10); + + cudaFree(d_data); + cudaFree(d_result); +} + +BOOST_AUTO_TEST_CASE(MaxAbsReduction_MixedSigns) +{ + // max(|v|) = 6.0 (from -6.0) + const std::vector data = {1.0, -6.0, 4.0, -2.0, 5.0}; + double* d_data = toDevice(data); + double* d_result = deviceScalar(0.0); + + launchMaxAbsReduction(d_data, d_result, static_cast(data.size()), nullptr); + cudaDeviceSynchronize(); + + BOOST_CHECK_CLOSE(fromDevice(d_result), 6.0, 1e-10); + + cudaFree(d_data); + cudaFree(d_result); +} + +BOOST_AUTO_TEST_CASE(MaxAbsReduction_SingleElement) +{ + const std::vector data = {-42.5}; + double* d_data = toDevice(data); + double* d_result = deviceScalar(0.0); + + launchMaxAbsReduction(d_data, d_result, 1, nullptr); + cudaDeviceSynchronize(); + + BOOST_CHECK_CLOSE(fromDevice(d_result), 42.5, 1e-10); + + cudaFree(d_data); + cudaFree(d_result); +} + +BOOST_AUTO_TEST_CASE(MaxAbsReduction_LargeArray) +{ + // Fill with values 1..N; max = N + const int N = 100'000; + std::vector data(N); + for (int i = 0; i < N; ++i) + data[i] = static_cast(i + 1); + + double* d_data = toDevice(data); + double* d_result = deviceScalar(0.0); + + launchMaxAbsReduction(d_data, d_result, N, nullptr); + cudaDeviceSynchronize(); + + BOOST_CHECK_CLOSE(fromDevice(d_result), static_cast(N), 1e-10); + + cudaFree(d_data); + cudaFree(d_result); +} + +BOOST_AUTO_TEST_CASE(MaxAbsReduction_AllZeros) +{ + const std::vector data(64, 0.0); + double* d_data = toDevice(data); + double* d_result = deviceScalar(0.0); + + launchMaxAbsReduction(d_data, d_result, static_cast(data.size()), nullptr); + cudaDeviceSynchronize(); + + BOOST_CHECK_SMALL(fromDevice(d_result), 1e-15); + + cudaFree(d_data); + cudaFree(d_result); +} + +// --------------------------------------------------------------------------- +// launchConditionalAxpy tests +// --------------------------------------------------------------------------- + +BOOST_AUTO_TEST_CASE(ConditionalAxpy_ConvergingAccumulates) +{ + // lambda * cur_maxabs (0.5) <= prev_norm (1.0) => axpy should execute. + // dst[i] = 1.0, src[i] = 2.0, k = 0.5 => dst[i] += 0.5 * 2.0 = 2.0 + const int n = 4; + const std::vector src(n, 2.0); + const std::vector dst_init(n, 1.0); + + double* d_src = toDevice(src); + double* d_dst = toDevice(dst_init); + double* d_cur_maxabs = deviceScalar(0.5); // lambda * 0.5 = 0.25 <= prev_norm 1.0 + double* d_prev_norm = deviceScalar(1.0); + + const double lambda = 0.5; + const double k = 0.5; + + launchConditionalAxpy(k, d_src, d_dst, d_cur_maxabs, d_prev_norm, lambda, n, nullptr); + cudaDeviceSynchronize(); + + // axpy executed: dst[i] = 1.0 + 0.5 * 2.0 = 2.0 + std::vector result(n); + cudaMemcpy(result.data(), d_dst, n * sizeof(double), cudaMemcpyDeviceToHost); + for (int i = 0; i < n; ++i) + BOOST_CHECK_CLOSE(result[i], 2.0, 1e-10); + + // d_prev_norm updated to lambda * cur_maxabs = 0.5 * 0.5 = 0.25 + BOOST_CHECK_CLOSE(fromDevice(d_prev_norm), 0.25, 1e-10); + + cudaFree(d_src); + cudaFree(d_dst); + cudaFree(d_cur_maxabs); + cudaFree(d_prev_norm); +} + +BOOST_AUTO_TEST_CASE(ConditionalAxpy_DivergingSkips) +{ + // lambda * cur_maxabs (10.0) = 5.0 > prev_norm (1.0) => axpy must be skipped. + const int n = 4; + const std::vector src(n, 2.0); + const std::vector dst_init(n, 1.0); + + double* d_src = toDevice(src); + double* d_dst = toDevice(dst_init); + double* d_cur_maxabs = deviceScalar(10.0); // lambda * 10.0 = 5.0 > prev_norm 1.0 + double* d_prev_norm = deviceScalar(1.0); + + const double lambda = 0.5; + const double k = 0.5; + + launchConditionalAxpy(k, d_src, d_dst, d_cur_maxabs, d_prev_norm, lambda, n, nullptr); + cudaDeviceSynchronize(); + + // axpy skipped: dst[i] remains 1.0 + std::vector result(n); + cudaMemcpy(result.data(), d_dst, n * sizeof(double), cudaMemcpyDeviceToHost); + for (int i = 0; i < n; ++i) + BOOST_CHECK_CLOSE(result[i], 1.0, 1e-10); + + // d_prev_norm must NOT have been updated (still 1.0) + BOOST_CHECK_CLOSE(fromDevice(d_prev_norm), 1.0, 1e-10); + + cudaFree(d_src); + cudaFree(d_dst); + cudaFree(d_cur_maxabs); + cudaFree(d_prev_norm); +} + +BOOST_AUTO_TEST_CASE(ConditionalAxpy_InitialInfAlwaysPasses) +{ + // d_prev_norm = +inf => any lambda * cur_maxabs passes on the first call. + const int n = 4; + const std::vector src(n, 3.0); + const std::vector dst_init(n, 0.0); + + double* d_src = toDevice(src); + double* d_dst = toDevice(dst_init); + double* d_cur_maxabs = deviceScalar(100.0); + double* d_prev_norm = deviceScalar(std::numeric_limits::infinity()); + + const double lambda = 1e-3; + const double k = 2.0; + + launchConditionalAxpy(k, d_src, d_dst, d_cur_maxabs, d_prev_norm, lambda, n, nullptr); + cudaDeviceSynchronize(); + + // axpy executed: dst[i] = 0.0 + 2.0 * 3.0 = 6.0 + std::vector result(n); + cudaMemcpy(result.data(), d_dst, n * sizeof(double), cudaMemcpyDeviceToHost); + for (int i = 0; i < n; ++i) + BOOST_CHECK_CLOSE(result[i], 6.0, 1e-10); + + // d_prev_norm updated to lambda * cur_maxabs = 1e-3 * 100.0 = 0.1 + BOOST_CHECK_CLOSE(fromDevice(d_prev_norm), 0.1, 1e-10); + + cudaFree(d_src); + cudaFree(d_dst); + cudaFree(d_cur_maxabs); + cudaFree(d_prev_norm); +} + +BOOST_AUTO_TEST_CASE(ConditionalAxpy_ExactEqualityPasses) +{ + // lambda * cur_maxabs == prev_norm (exactly equal) must also accumulate. + const int n = 2; + const std::vector src(n, 1.0); + const std::vector dst_init(n, 0.0); + + // lambda=0.5, cur=2.0 => lambda*cur = 1.0 == prev_norm=1.0 => pass + double* d_src = toDevice(src); + double* d_dst = toDevice(dst_init); + double* d_cur_maxabs = deviceScalar(2.0); + double* d_prev_norm = deviceScalar(1.0); + + launchConditionalAxpy(1.0, d_src, d_dst, d_cur_maxabs, d_prev_norm, 0.5, n, nullptr); + cudaDeviceSynchronize(); + + std::vector result(n); + cudaMemcpy(result.data(), d_dst, n * sizeof(double), cudaMemcpyDeviceToHost); + for (int i = 0; i < n; ++i) + BOOST_CHECK_CLOSE(result[i], 1.0, 1e-10); + + cudaFree(d_src); + cudaFree(d_dst); + cudaFree(d_cur_maxabs); + cudaFree(d_prev_norm); +} diff --git a/src/aliceVision/uncertainty/uncertainty_test.cpp b/src/aliceVision/uncertainty/uncertainty_test.cpp new file mode 100644 index 0000000000..b26c351cde --- /dev/null +++ b/src/aliceVision/uncertainty/uncertainty_test.cpp @@ -0,0 +1,141 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#define BOOST_TEST_MODULE uncertainty + +#include + +#include +#include +#include +#include +#include + +#include + +using namespace aliceVision; +using namespace aliceVision::sfmDataIO; + +namespace +{ + +/// Run the full uncertainty pipeline on a pre-populated SfMData. +/// Returns true if computeUncertainty succeeded. +bool runUncertainty(sfmData::SfMData& sfmData) +{ + std::array sample; + if (!sfm::selectTripletForGaugeRemoval(sfmData, sample)) + { + return false; + } + + std::array previouslyLocked; + for (int i = 0; i < 3; i++) + { + previouslyLocked[i] = sfmData.getLandmarks().at(sample[i]).isLocked(); + sfmData.getLandmarks().at(sample[i]).setLocked(true); + } + + std::map poseToPosition; + std::map landmarkToPosition; + ceres::CRSMatrix jacobian; + sfm::BundleAdjustmentCeres bundleAdjustmentObj; + bundleAdjustmentObj.createJacobian(sfmData, jacobian, poseToPosition, landmarkToPosition); + + Eigen::Map> J_eigen( + jacobian.num_rows, + jacobian.num_cols, + static_cast(jacobian.values.size()), + jacobian.rows.data(), + jacobian.cols.data(), + jacobian.values.data() + ); + + Eigen::MatrixXd covarianceCameras; + const bool res = uncertainty::computeUncertainty(covarianceCameras, J_eigen, poseToPosition.size(), landmarkToPosition.size()); + + for (int i = 0; i < 3; i++) + { + sfmData.getLandmarks().at(sample[i]).setLocked(previouslyLocked[i]); + } + + return res; +} + +} // anonymous namespace + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_Sphere) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_Sphere"); + sfmData::SfMData sfmData; + generateSphereScene(sfmData, 1000, 20); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_Planar) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_Planar"); + sfmData::SfMData sfmData; + generatePlanarScene(sfmData, 30, 60); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_CollinearCameras) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_CollinearCameras"); + sfmData::SfMData sfmData; + generateCollinearCamerasScene(sfmData, 500, 20); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_ForwardMotion) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_ForwardMotion"); + sfmData::SfMData sfmData; + generateForwardMotionScene(sfmData, 500, 30); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_NarrowBaseline) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_NarrowBaseline"); + sfmData::SfMData sfmData; + generateNarrowBaselineScene(sfmData, 1000, 240); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_UnevenCoverage) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_UnevenCoverage"); + sfmData::SfMData sfmData; + generateUnevenCoverageScene(sfmData, 200, 800, 240); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_PureRotation) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_PureRotation"); + sfmData::SfMData sfmData; + generatePureRotationScene(sfmData, 1000, 240); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_Sparse) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_Sparse"); + sfmData::SfMData sfmData; + generateSparseCameraObservationsScene(sfmData, 100, 60, 5); + BOOST_CHECK(runUncertainty(sfmData)); +} + +BOOST_AUTO_TEST_CASE(Uncertainty_SceneSample_ClusteredObservations) +{ + ALICEVISION_LOG_INFO("Uncertainty_SceneSample_ClusteredObservations"); + sfmData::SfMData sfmData; + generateClusteredObservationsScene(sfmData, 200, 2); + BOOST_CHECK(runUncertainty(sfmData)); +} + diff --git a/src/aliceVision/utils/scope.hpp b/src/aliceVision/utils/scope.hpp new file mode 100644 index 0000000000..96a5b48115 --- /dev/null +++ b/src/aliceVision/utils/scope.hpp @@ -0,0 +1,29 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2026 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include + +namespace aliceVision { +namespace utils { + +template +class ScopeGuard { +public: + explicit ScopeGuard(Fn&& fn) : fn_(std::forward(fn)) {} + ~ScopeGuard() { fn_(); } + + ScopeGuard(const ScopeGuard&) = delete; + ScopeGuard& operator=(const ScopeGuard&) = delete; + +private: + Fn fn_; +}; + +} +} diff --git a/src/cmake/ConfigureOptionsMerger.cmake b/src/cmake/ConfigureOptionsMerger.cmake index 706452e80b..291eecdf21 100644 --- a/src/cmake/ConfigureOptionsMerger.cmake +++ b/src/cmake/ConfigureOptionsMerger.cmake @@ -62,9 +62,6 @@ macro(av_merge_configure_options) if(DEFINED ALICEVISION_USE_ONNX) list(APPEND AV_TOPLEVEL_FLAGS -DALICEVISION_USE_ONNX=${ALICEVISION_USE_ONNX}) endif() - if(DEFINED ALICEVISION_USE_UNCERTAINTYTE) - list(APPEND AV_TOPLEVEL_FLAGS -DALICEVISION_USE_UNCERTAINTYTE=${ALICEVISION_USE_UNCERTAINTYTE}) - endif() if(DEFINED ALICEVISION_USE_ALEMBIC) list(APPEND AV_TOPLEVEL_FLAGS -DALICEVISION_USE_ALEMBIC=${ALICEVISION_USE_ALEMBIC}) endif() diff --git a/src/cmake/FindUncertaintyTE.cmake b/src/cmake/FindUncertaintyTE.cmake deleted file mode 100644 index bcd4d6bfe6..0000000000 --- a/src/cmake/FindUncertaintyTE.cmake +++ /dev/null @@ -1,76 +0,0 @@ -# Locate the UncertaintyTE libraries. -# -# Defines the following variables: -# -# UNCERTAINTYTE_FOUND - TRUE if the UncertaintyTE headers and libs are found -# UNCERTAINTYTE_INCLUDE_DIRS - The path to UncertaintyTE headers -# -# UNCERTAINTYTE_LIBRARY - The opengv library -# UNCERTAINTYTE_LIBRARY_DIR - The directory where the libraries are located -# -# Accepts the following variables as input: -# -# UNCERTAINTYTE_DIR - (as a CMake or environment variable) -# The root directory of the UncertaintyTE install prefix - -MESSAGE(STATUS "Looking for UncertaintyTE.") - -FIND_PATH(UNCERTAINTYTE_INCLUDE_DIR uncertaintyTE/uncertainty.h - HINTS - $ENV{UNCERTAINTYTE_DIR} - ${UNCERTAINTYTE_DIR} - PATH_SUFFIXES - include -) - -FIND_LIBRARY(UNCERTAINTYTE_LIBRARY NAMES uncertaintyTE - HINTS - $ENV{UNCERTAINTYTE_DIR} - ${UNCERTAINTYTE_DIR} - PATH_SUFFIXES - lib -) - -IF(UNCERTAINTYTE_INCLUDE_DIR) - MESSAGE(STATUS "UncertaintyTE headers found in ${UNCERTAINTYTE_INCLUDE_DIR}") - IF(NOT MAGMA_FOUND) - MESSAGE(STATUS "Looking for MAGMA dependency...") - FIND_PACKAGE(MAGMA) - IF(MAGMA_FOUND) - set(UNCERTAINTYTE_INCLUDE_DIR ${UNCERTAINTYTE_INCLUDE_DIR} ${MAGMA_INCLUDE_DIRS}) - set(UNCERTAINTYTE_LIBRARY ${UNCERTAINTYTE_LIBRARY} ${MAGMA_LIBRARIES}) - ELSE() - MESSAGE(WARNING "Couldn't find Magma, this is needed for compiling with UncertaintyTE") - # this is to make the find_package_handle_standard_args fail - SET(UNCERTAINTYTE_INCLUDE_DIR "UNCERTAINTYTE_INCLUDE_DIR-NOTFOUND") - ENDIF(MAGMA_FOUND) - ELSE(NOT MAGMA_FOUND) - MESSAGE(STATUS "Magma already found") - ENDIF(NOT MAGMA_FOUND) -ELSE(UNCERTAINTYTE_INCLUDE_DIR) - MESSAGE(STATUS "UncertaintyTE headers not found!") -ENDIF(UNCERTAINTYTE_INCLUDE_DIR) - - -GET_FILENAME_COMPONENT(UNCERTAINTYTE_LIBRARY_DIR "${UNCERTAINTYTE_LIBRARY}" PATH) - -if(UNCERTAINTYTE_INCLUDE_DIR) - message(STATUS "UncertaintyTE include directory: ${UNCERTAINTYTE_INCLUDE_DIR}") -else() - message(STATUS "UncertaintyTE library include not found") -endif() - -if(UNCERTAINTYTE_LIBRARY) - message(STATUS "UncertaintyTE libraries found: ${UNCERTAINTYTE_LIBRARY}") - message(STATUS "UncertaintyTE libraries directories: ${UNCERTAINTYTE_LIBRARY_DIR}") -else() - message(STATUS "UncertaintyTE library not found") -endif() - -include(FindPackageHandleStandardArgs) -# handle the QUIETLY and REQUIRED arguments and set UNCERTAINTYTE_FOUND to TRUE -# if all listed variables are TRUE -find_package_handle_standard_args(UncertaintyTE DEFAULT_MSG - UNCERTAINTYTE_LIBRARY UNCERTAINTYTE_INCLUDE_DIR) - -# MARK_AS_ADVANCED(UNCERTAINTYTE_INCLUDE_DIR UNCERTAINTYTE_LIBRARY) diff --git a/src/nonFree/sift/CMakeLists.txt b/src/nonFree/sift/CMakeLists.txt index 776978fd59..98ec582ec0 100644 --- a/src/nonFree/sift/CMakeLists.txt +++ b/src/nonFree/sift/CMakeLists.txt @@ -45,9 +45,7 @@ set_source_files_properties(${FEATS} ${FEATS_H} PROPERTIES LANGUAGE C) set_source_files_properties(${FEATS_H} PROPERTIES HEADER_FILE_ONLY TRUE) set(SIMD_DEFINITIONS) -if (NOT ALICEVISION_HAVE_AVX) - list(APPEND SIMD_DEFINITIONS "-DVL_DISABLE_AVX") -endif() +list(APPEND SIMD_DEFINITIONS "-DVL_DISABLE_AVX") if (NOT ALICEVISION_HAVE_SSE) list(APPEND SIMD_DEFINITIONS "-DVL_DISABLE_SSE2") endif() diff --git a/src/software/export/main_exportAlembic.cpp b/src/software/export/main_exportAlembic.cpp index aaf42ab437..9502586fdd 100644 --- a/src/software/export/main_exportAlembic.cpp +++ b/src/software/export/main_exportAlembic.cpp @@ -150,12 +150,20 @@ int aliceVision_main(int argc, char** argv) const IndexT viewId = findFrameIt->second; const sfmData::View & view = sfmData.getView(viewId); + + + sfmData::PoseUncertainty * pu = nullptr; + if (sfmData.existsPoseUncertainty(view)) + { + pu = &sfmData.getPosesUncertainty().at(view.getPoseId()); + } + const IndexT intrinsicId = view.getIntrinsicId(); const camera::Pinhole * cam = dynamic_cast(sfmData.getIntrinsicPtr(intrinsicId)); const sfmData::CameraPose pose = sfmData.getPose(view); const std::string& imagePath = view.getImage().getImagePath(); - exporter.addCameraKeyframe(pose.getTransform(), cam, imagePath, viewId, intrinsicId); + exporter.addCameraKeyframe(pose.getTransform(), cam, imagePath, viewId, intrinsicId, pu); } else { @@ -175,11 +183,18 @@ int aliceVision_main(int argc, char** argv) for (const auto& [key, id] : views) { const sfmData::View& view = sfmData.getView(id); + + sfmData::PoseUncertainty * pu = nullptr; + if (sfmData.existsPoseUncertainty(view)) + { + pu = &sfmData.getPosesUncertainty().at(view.getPoseId()); + } + const camera::Pinhole* cam = dynamic_cast(sfmData.getIntrinsicPtr(view.getIntrinsicId())); const sfmData::CameraPose pose = sfmData.getPose(view); const std::string& imagePath = view.getImage().getImagePath(); - exporter.addCameraKeyframe(pose.getTransform(), cam, imagePath, view.getViewId(), view.getIntrinsicId()); + exporter.addCameraKeyframe(pose.getTransform(), cam, imagePath, view.getViewId(), view.getIntrinsicId(), pu); } } diff --git a/src/software/utils/CMakeLists.txt b/src/software/utils/CMakeLists.txt index 111b6cebdb..8ee97b8fda 100644 --- a/src/software/utils/CMakeLists.txt +++ b/src/software/utils/CMakeLists.txt @@ -17,23 +17,17 @@ if (ALICEVISION_HAVE_CUDA) endif() if (ALICEVISION_BUILD_SFM) - # Uncertainty - if (ALICEVISION_HAVE_UNCERTAINTYTE) - if (ALICEVISION_HAVE_CUDA) - alicevision_add_software(aliceVision_computeUncertainty - SOURCE main_computeUncertainty.cpp - FOLDER ${FOLDER_SOFTWARE_UTILS} - LINKS aliceVision_sfm - aliceVision_system - aliceVision_cmdline - CUDA::cudart - CUDA::cublas - CUDA::cusparse - ${UNCERTAINTYTE_LIBRARY} - Boost::program_options - INCLUDE_DIRS ${UNCERTAINTYTE_INCLUDE_DIR} - ) - endif() + + if (ALICEVISION_HAVE_CUDA) + alicevision_add_software(aliceVision_computeUncertainty + SOURCE main_computeUncertainty.cpp + FOLDER ${FOLDER_SOFTWARE_UTILS} + LINKS aliceVision_sfm + aliceVision_system + aliceVision_cmdline + aliceVision_uncertainty + Boost::program_options + ) endif() alicevision_add_software(aliceVision_imageProcessing diff --git a/src/software/utils/main_computeUncertainty.cpp b/src/software/utils/main_computeUncertainty.cpp index c4cc49a02f..7a9acd0b0d 100644 --- a/src/software/utils/main_computeUncertainty.cpp +++ b/src/software/utils/main_computeUncertainty.cpp @@ -3,22 +3,23 @@ // v. 2.0. If a copy of the MPL was not distributed with this file, // You can obtain one at https://mozilla.org/MPL/2.0/. -#include +#include #include #include #include #include #include #include - -#include -#include +#include +#include #include +#include #include #include #include +#include // These constants define the current software version. // They must be updated when the command line is changed. @@ -31,126 +32,97 @@ using namespace aliceVision::sfmData; using namespace aliceVision::sfmDataIO; namespace po = boost::program_options; + int aliceVision_main(int argc, char** argv) { // command-line parameters + std::string verboseLevel = system::EVerboseLevel_enumToString(system::Logger::getDefaultVerboseLevel()); std::string sfmDataFilename; std::string outSfMDataFilename; - std::string outputStats; - std::string algorithm = cov::EAlgorithm_enumToString(cov::eAlgorithmSvdTaylorExpansion); - bool debug = false; // clang-format off - params.add_options() + po::options_description requiredParams("Required parameters"); + requiredParams.add_options() ("input,i", po::value(&sfmDataFilename)->required(), "SfMData file to align.") ("output,o", po::value(&outSfMDataFilename)->required(), - "Output SfMData scene.") - ("outputCov,c", po::value(&outputStats), - "Output covariances file.") - ("algorithm,a", po::value(&algorithm)->default_value(algorithm), - "Algorithm.") - ("debug,d", po::value(&debug)->default_value(debug), - "Enable creation of debug files in the current folder.") - ("verboseLevel,v", po::value(&verboseLevel)->default_value(verboseLevel), - "Verbosity level (fatal, error, warning, info, debug, trace)."); + "Output SfMData scene."); // clang-format on CmdLine cmdline("AliceVision computeUncertainty"); - cmdline.add(params); + cmdline.add(requiredParams); if (!cmdline.execute(argc, argv)) { return EXIT_FAILURE; } - if (sfmDataFilename.empty() || outSfMDataFilename.empty()) + + // Load input scene + SfMData sfmData; + if (!sfmDataIO::load(sfmData, sfmDataFilename, ESfMData(ALL))) { - std::cerr << "Invalid input or output filename." << std::endl; + ALICEVISION_LOG_ERROR("The input SfMData file " << sfmDataFilename << " cannot be read."); return EXIT_FAILURE; } - // Load input scene - SfMData sfmData; - if (!Load(sfmData, sfmDataFilename, ESfMData(ALL))) + ALICEVISION_LOG_INFO("Finding points to lock"); + std::array sample; + if (!selectTripletForGaugeRemoval(sfmData, sample)) { - std::cerr << std::endl << "The input SfMData file \"" << sfmDataFilename << "\" cannot be read." << std::endl; + ALICEVISION_LOG_ERROR("Failed to find initial set of point"); return EXIT_FAILURE; } - ceres::CRSMatrix jacobian; + std::array previouslyLocked; + for (int i = 0; i < 3; i++) { - BundleAdjustmentCeres bundleAdjustmentObj; - BundleAdjustment::ERefineOptions refineOptions = - BundleAdjustment::REFINE_ROTATION | BundleAdjustment::REFINE_TRANSLATION | BundleAdjustment::REFINE_STRUCTURE; - bundleAdjustmentObj.createJacobian(sfmData, refineOptions, jacobian); + previouslyLocked[i] = sfmData.getLandmarks().at(sample[i]).isLocked(); + sfmData.getLandmarks().at(sample[i]).setLocked(true); } + + ALICEVISION_LOG_INFO("Computing jacobian"); + std::map poseToPosition; + std::map landmarkToPosition; + ceres::CRSMatrix jacobian; + BundleAdjustmentCeres bundleAdjustmentObj; + bundleAdjustmentObj.createJacobian(sfmData, jacobian, poseToPosition, landmarkToPosition); + + std::map medianPerView; + computeResidualsMedianPerView(sfmData, std::set(), medianPerView); + + ALICEVISION_LOG_INFO("Building map to Ceres"); + Eigen::Map> J_eigen( + jacobian.num_rows, + jacobian.num_cols, + static_cast(jacobian.values.size()), + jacobian.rows.data(), // outer index ptr (row offsets), size = num_rows + 1 + jacobian.cols.data(), // inner indices (col ids), size = nnz + jacobian.values.data() // values, size = nnz + ); + + Eigen::MatrixXd covarianceCameras; + bool res = uncertainty::computeUncertainty(covarianceCameras, J_eigen, poseToPosition.size(), landmarkToPosition.size()); + if (!res) { - cov::Options options; - // Configure covariance engine (find the indexes of the most distatnt points etc.) - // setPts2Fix(opt, mutable_points.size() / 3, mutable_points.data()); - options._numCams = sfmData.getValidViews().size(); - options._camParams = 6; - options._numPoints = sfmData.getLandmarks().size(); - options._numObs = jacobian.num_rows / 2; - options._algorithm = cov::EAlgorithm_stringToEnum(algorithm); - options._epsilon = 1e-10; - options._lambda = -1; - options._svdRemoveN = -1; - options._maxIterTE = -1; - options._debug = debug; - - cov::Statistic statistic; - std::vector points3D; - points3D.reserve(sfmData.getLandmarks().size() * 3); - for (auto& landmarkIt : sfmData.getLandmarks()) - { - double* p = landmarkIt.second.getX().data(); - points3D.push_back(p[0]); - points3D.push_back(p[1]); - points3D.push_back(p[2]); - } - - cov::Uncertainty uncertainty; - - getCovariances(options, statistic, jacobian, &points3D[0], uncertainty); - - if (!outputStats.empty()) - saveResults(outputStats, options, statistic, uncertainty); - - { - const std::vector posesUncertainty = uncertainty.getCamerasUncEigenValues(); - - std::size_t indexPose = 0; - for (Poses::const_iterator itPose = sfmData.getPoses().begin(); itPose != sfmData.getPoses().end(); ++itPose, ++indexPose) - { - const IndexT idPose = itPose->first; - Vec6& u = sfmData._posesUncertainty[idPose]; // create uncertainty entry - const double* uIn = &posesUncertainty[indexPose * 6]; - u << uIn[0], uIn[1], uIn[2], uIn[3], uIn[4], uIn[5]; - } - } - { - const std::vector landmarksUncertainty = uncertainty.getPointsUncEigenValues(); - - std::size_t indexLandmark = 0; - for (Landmarks::const_iterator itLandmark = sfmData.getLandmarks().begin(); itLandmark != sfmData.getLandmarks().end(); - ++itLandmark, ++indexLandmark) - { - const IndexT idLandmark = itLandmark->first; - Vec3& u = sfmData._landmarksUncertainty[idLandmark]; // create uncertainty entry - const double* uIn = &landmarksUncertainty[indexLandmark * 3]; - u << uIn[0], uIn[1], uIn[2]; - } - } + return EXIT_FAILURE; } - std::cout << "Save into \"" << outSfMDataFilename << "\"" << std::endl; + for (int i = 0; i < 3; i++) + { + sfmData.getLandmarks().at(sample[i]).setLocked(previouslyLocked[i]); + } + + // Store covariance + for (const auto & [index, position] : poseToPosition) + { + sfmData.setPoseUncertainty(index, covarianceCameras.block<6, 6>(position * 6, position * 6)); + } - // Export the SfMData scene in the expected format - if (!Save(sfmData, outSfMDataFilename, ESfMData(ALL))) + ALICEVISION_LOG_INFO("Export SfM: " << outSfMDataFilename); + if (!sfmDataIO::save(sfmData, outSfMDataFilename, sfmDataIO::ESfMData::ALL)) { - std::cerr << std::endl << "An error occurred while trying to save \"" << outSfMDataFilename << "\"." << std::endl; + ALICEVISION_LOG_ERROR("The output SfMData file '" << outSfMDataFilename << "' cannot be written."); return EXIT_FAILURE; }