From b60e2e4ff7d4fcaf9444678ec86d1156892e19d4 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 23 Apr 2024 10:21:01 +0200 Subject: [PATCH 01/14] sfmTriangulation class --- .../multiview/triangulation/Triangulation.cpp | 21 ++ .../multiview/triangulation/Triangulation.hpp | 58 +++++ .../TriangulationSphericalKernel.hpp | 225 +++++++++++++++++ .../robustEstimation/NACRansac.hpp | 221 +++++++++++++++++ src/aliceVision/sfm/CMakeLists.txt | 2 + .../pipeline/expanding/SfmTriangulation.cpp | 227 ++++++++++++++++++ .../pipeline/expanding/SfmTriangulation.hpp | 92 +++++++ 7 files changed, 846 insertions(+) create mode 100644 src/aliceVision/multiview/triangulation/TriangulationSphericalKernel.hpp create mode 100644 src/aliceVision/robustEstimation/NACRansac.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp diff --git a/src/aliceVision/multiview/triangulation/Triangulation.cpp b/src/aliceVision/multiview/triangulation/Triangulation.cpp index ac7b61aa3d..5a46bef73a 100644 --- a/src/aliceVision/multiview/triangulation/Triangulation.cpp +++ b/src/aliceVision/multiview/triangulation/Triangulation.cpp @@ -34,6 +34,27 @@ void TriangulateNView(const Mat2X& x, const std::vector& Ps, Vec4& X, con X = X_and_alphas.head(4); } +void TriangulateNViewAlgebraicSpherical(const std::vector &xs, + const std::vector & Ts, + Vec4 & X, + const std::vector *weights) + +{ + Mat2X::Index nviews = xs.size(); + assert(static_cast(nviews) == Ts.size()); + + Mat design(3 * nviews, 4); + for(Mat2X::Index i = 0; i < nviews; ++i) + { + design.block<3, 4>(3 * i, 0) = CrossProductMatrix(xs[i]) * Ts[i].block<3, 4>(0, 0); + if(weights != nullptr) + { + design.block<3, 4>(3 * i, 0) *= (*weights)[i]; + } + } + Nullspace(design, X); +} + void TriangulateNViewLORANSAC(const std::vector& pts, const std::vector& Ps, std::mt19937& generator, diff --git a/src/aliceVision/multiview/triangulation/Triangulation.hpp b/src/aliceVision/multiview/triangulation/Triangulation.hpp index 8054d84e7d..b76907cb59 100644 --- a/src/aliceVision/multiview/triangulation/Triangulation.hpp +++ b/src/aliceVision/multiview/triangulation/Triangulation.hpp @@ -32,6 +32,7 @@ namespace multiview { */ void TriangulateNView(const Mat2X& x, const std::vector& Ps, Vec4& X, const std::vector* weights = nullptr); + /** * @brief Compute a 3D position of a point from several images of it. In particular, * compute the projective point X in R^4 such that x ~ PX. @@ -63,6 +64,23 @@ void TriangulateNViewAlgebraic(const ContainerT& x, const std::vector& Ps Nullspace(design, X); } +/** + * @brief Compute a 3D position of a point from several images of it. In particular, + * compute the projective point X in R^4 such that x ~ PX. + * Algorithm is the standard DLT + * It also allows to specify some (optional) weight for each point (solving the + * weighted least squared problem) + * + * @param[in] xs are 2D coordinates (x,y) in each image + * @param[in] Ps is the list of Transformation matrices for each camera + * @param[out] X is the estimated 3D point + * @param[in] weights a (optional) list of weights for each point + */ +void TriangulateNViewAlgebraicSpherical(const std::vector &xs, + const std::vector & Ts, + Vec4 & X, + const std::vector *weights = nullptr); + /** * @brief Compute a 3D position of a point from several images of it. In particular, * compute the projective point X in R^4 such that x ~ PX. @@ -151,5 +169,45 @@ struct TriangulateNViewsSolver } }; +struct TriangulateNViewsSphericalSolver +{ + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const + { + return 2; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const + { + return 1; + } + + void solve(const std::vector & x, const std::vector& Ts, std::vector>& X) const + { + Vec4 pt3d; + TriangulateNViewAlgebraicSpherical(x, Ts, pt3d); + X.push_back(robustEstimation::MatrixModel(pt3d)); + assert(X.size() == 1); + } + + + + void solve(const std::vector & x, const std::vector& Ts, std::vector>& X, const std::vector& weights) const + { + Vec4 pt3d; + TriangulateNViewAlgebraicSpherical(x, Ts, pt3d, &weights); + X.push_back(robustEstimation::MatrixModel(pt3d)); + assert(X.size() == 1); + } + +}; + } // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/triangulation/TriangulationSphericalKernel.hpp b/src/aliceVision/multiview/triangulation/TriangulationSphericalKernel.hpp new file mode 100644 index 0000000000..153dfdd0ce --- /dev/null +++ b/src/aliceVision/multiview/triangulation/TriangulationSphericalKernel.hpp @@ -0,0 +1,225 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace multiview { + +/** + * Triangulation kernel which works for any camera model. + * The estimation is done on lifted points but error is computed in pixels +*/ +class TriangulationSphericalKernel : public robustEstimation::IRansacKernel> +{ +public: + using ModelT = robustEstimation::MatrixModel; +public: + /** + * @brief Constructor. + * @param[in] observations The observations 2d points in pixels. + * @param[in] poses the transformation for each observation. + * @param[in] intrinsics the camera intrinsic for each observation. + */ + TriangulationSphericalKernel( + const std::vector & observations, + const std::vector& poses, + std::vector> & intrinsics + ) + : _observations(observations) + , _poses(poses) + , _intrinsics(intrinsics) + { + for (int id = 0; id < observations.size(); id++) + { + //Lift all points onto the metric unit sphere + const Vec2 & obs = _observations[id]; + std::shared_ptr camera = _intrinsics[id]; + _lifted.push_back(camera->toUnitSphere(camera->removeDistortion(camera->ima2cam(obs)))); + } + } + + /** + * @brief Return the minimum number of required samples + * @return minimum number of required samples + */ + inline std::size_t getMinimumNbRequiredSamples() const override + { + return 2; + } + + /** + * @brief Return the maximum number of models + * @return maximum number of models + */ + inline std::size_t getMaximumNbModels() const override + { + return 1; + } + + inline std::size_t getMinimumNbRequiredSamplesLS() const override + { + return 2; + } + + /** + * @brief Triangulate 2 points. + * @param[in] samples The index of two points to triangulate. + * @param[out] models The estimated 3D points. + */ + void fit(const std::vector& samples, std::vector& models) const override + { + std::vector sampledPts; + std::vector sampledMats; + + for (int i = 0; i < samples.size(); i++) + { + std::size_t idx = samples[i]; + sampledMats.push_back(_poses[idx]); + sampledPts.push_back(_lifted[idx]); + } + + _solver.solve(sampledPts, sampledMats, models); + } + + /** + * @brief Triangulate N points using the least squared solver. + * @param[in] inliers The index of the N points to triangulate. + * @param[out] models The estimated 3D point. + * @param[in] weights The optional array of weight for each of the N points. + */ + void fitLS(const std::vector& inliers, + std::vector& models, + const std::vector *weights = nullptr) const override + { + std::vector sampledPts; + std::vector sampledMats; + + for (int i = 0; i < inliers.size(); i++) + { + std::size_t idx = inliers[i]; + sampledMats.push_back(_poses[idx]); + sampledPts.push_back(_lifted[idx]); + } + + _solver.solve(sampledPts, sampledMats, models, *weights); + } + + + /** + * @brief Compute the weights.. + * @param[in] model The 3D point for which the weights are computed. + * @param[in] inliers The array of the indices of the data to be used. + * @param[out] vec_weights The array of weight of the same size as \p inliers. + * @param[in] eps An optional threshold to max out the value of the threshold (typically + * to avoid division by zero or too small numbers). + */ + void computeWeights(const ModelT & model, + const std::vector &inliers, + std::vector& weights, + const double eps = 0.001) const override + { + const auto numInliers = inliers.size(); + weights.resize(numInliers); + + for(std::size_t sample = 0; sample < numInliers; ++sample) + { + const auto idx = inliers[sample]; + weights[sample] = 1.0 / std::pow(std::max(eps, error(idx, model)), 2); + } + } + + /** + * @brief Error for the i-th view + * @param[in] sample The index of the view for which the error is computed. + * @param[in] model The 3D point. + * @return The estimation error for the given view and 3D point. + */ + double error(std::size_t sample, const ModelT & model) const override + { + Vec4 X = model.getMatrix(); + if (std::abs(X(3)) > 1e-16) + { + X = X / X(3); + } + + Vec2 residual = _intrinsics[sample]->residual(_poses[sample], X, _observations[sample]); + + return residual.norm(); + } + + /** + * @brief Error for each view. + * @param[in] model The 3D point. + * @param[out] vec_errors The vector containing all the estimation errors for every view. + */ + void errors(const ModelT & model, std::vector& errors) const override + { + errors.resize(nbSamples()); + for(Mat::Index i = 0; i < _lifted.size(); ++i) + { + errors[i] = error(i, model); + } + } + + /** + * @brief Unnormalize the model. (not used) + * @param[in,out] model the 3D point. + */ + void unnormalize(robustEstimation::MatrixModel & model) const override + { + + } + + /** + * @brief Return the number of view. + * @return the number of view. + */ + std::size_t nbSamples() const override + { + return _lifted.size(); + } + + double logalpha0() const override + { + std::runtime_error("Method 'logalpha0()' is not defined for 'NViewsTriangulationLORansac'."); + return 0.0; + } + + double errorVectorDimension() const override + { + std::runtime_error("Method 'errorVectorDimension()' is not defined for 'NViewsTriangulationLORansac'."); + return 0.0; + } + + double unormalizeError(double val) const override + { + std::runtime_error("Method 'unormalizeError()' is not defined for 'NViewsTriangulationLORansac'."); + return 0.0; + } + + Mat3 normalizer1() const override + { + std::runtime_error("Method 'normalizer1()' is not defined for 'NViewsTriangulationLORansac'."); + return Mat3(); + } + + double thresholdNormalizer() const override + { + std::runtime_error("Method 'thresholdNormalizer()' is not defined for 'NViewsTriangulationLORansac'."); + return 0.0; + } + +private: + std::vector _lifted; + const std::vector _observations; + const std::vector _poses; + const std::vector> _intrinsics; + multiview::TriangulateNViewsSphericalSolver _solver; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/robustEstimation/NACRansac.hpp b/src/aliceVision/robustEstimation/NACRansac.hpp new file mode 100644 index 0000000000..3505b514b8 --- /dev/null +++ b/src/aliceVision/robustEstimation/NACRansac.hpp @@ -0,0 +1,221 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// Copyright (c) 2012 openMVG contributors. +// Copyright (c) 2012, 2013 Lionel MOISAN. +// Copyright (c) 2012, 2013 Pascal MONASSE. +// 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 "ACRansac.hpp" + +namespace aliceVision +{ +namespace robustEstimation +{ + +/** + * @brief Find best NFA and its index wrt square error threshold in e. + */ +inline ErrorIndex bestNFA2(int startIndex, //number of point required for estimation + double logalpha0, + const std::vector& e, + double loge0, + double maxThreshold, + const std::vector &logc_n, + const std::vector &logc_k, + double errorVectorDimension = 1.0) +{ + ErrorIndex bestIndex(std::numeric_limits::infinity(), startIndex); + const size_t n = e.size(); + + for(size_t k = startIndex + 1; k <= n && e[k - 1].first <= maxThreshold; ++k) + { + double residual = e[k - 1].first + std::numeric_limits::epsilon(); + + const double logalpha = logalpha0 + errorVectorDimension * log10(residual); + const double nfa = loge0 + logalpha * (double) (k - startIndex) + logc_n[k] + logc_k[k]; + + + if(nfa < bestIndex.first) + { + bestIndex = ErrorIndex(nfa, k); + } + } + + return bestIndex; +} + +/** + * @brief An implementation of the "Random Sample Consensus" algorithm based on a-contrario estimator + * to automatically estimate the error threshold. + * + * The A contrario parametrization have been first explained in [1] and + * later extended to generic model estimation in [2] (with a demonstration for + * the homography) and extended and use at large scale for Structure from Motion in [3]. + * + * @ref [1] Lionel Moisan, Berenger Stival. + * A probalistic criterion to detect rigid point matches between + * two images and estimate the fundamental matrix. + * IJCV 04. + * + * @ref [2] Lionel Moisan, Pierre Moulon, Pascal Monasse. + * Automatic Homographic Registration of a Pair of Images, + * with A Contrario Elimination of Outliers + * Image Processing On Line (IPOL), 2012. + * http://dx.doi.org/10.5201/ipol.2012.mmm-oh + * + * @ref [3] Pierre Moulon, Pascal Monasse and Renaud Marlet. + * Adaptive Structure from Motion with a contrario mode estimation. + * In 11th Asian Conference on Computer Vision (ACCV 2012) + * + * @param[in] kernel model and metric object + * @param[out] vec_inliers points that fit the estimated model + * @param[in] nIter maximum number of consecutive iterations + * @param[out] model returned model if found + * @param[in] precision upper bound of the precision + * + * @return (errorMax, minNFA) + */ +template +std::pair NACRANSAC(const Kernel& kernel, std::mt19937& randomNumberGenerator, + std::vector& vec_inliers, std::size_t nIter = 1024, + typename Kernel::ModelT* model = nullptr, + double precision = std::numeric_limits::infinity()) +{ + vec_inliers.clear(); + + const std::size_t sizeSample = kernel.getMinimumNbRequiredSamples(); + const std::size_t nData = kernel.nbSamples(); + + if(nData <= (std::size_t)sizeSample) + { + return std::make_pair(0.0, 0.0); + } + + const double maxThreshold = + (precision == std::numeric_limits::infinity()) ? std::numeric_limits::infinity() : precision; + + std::vector vec_residuals(nData); // [residual,index] + std::vector vec_residuals_(nData); + + // Possible sampling indices [0,..,nData] (will change in the optimization phase) + std::vector vec_index(nData); + std::iota(vec_index.begin(), vec_index.end(), 0); + + // Precompute log combi + const double loge0 = log10((double)kernel.getMaximumNbModels() * (nData - sizeSample)); + std::vector vec_logc_n, vec_logc_k; + makelogcombi(sizeSample, nData, vec_logc_k, vec_logc_n); + + // Output parameters + double minNFA = std::numeric_limits::infinity(); + double errorMax = std::numeric_limits::infinity(); + + // Reserve 10% of iterations for focused sampling + size_t nIterReserve = nIter / 10; + nIter -= nIterReserve; + + bool bACRansacMode = (precision == std::numeric_limits::infinity()); + + // Main estimation loop. + for(std::size_t iter = 0; iter < nIter; ++iter) + { + std::vector vec_sample(sizeSample); // Sample indices + if(bACRansacMode) + uniformSample(randomNumberGenerator, sizeSample, vec_index, vec_sample); // Get random sample + else + uniformSample(randomNumberGenerator, sizeSample, nData, vec_sample); // Get random sample + + std::vector vec_models; // Up to max_models solutions + kernel.fit(vec_sample, vec_models); + + // Evaluate models + bool better = false; + for(std::size_t k = 0; k < vec_models.size(); ++k) + { + // Residuals computation and ordering + kernel.errors(vec_models[k], vec_residuals_); + + if(!bACRansacMode) + { + unsigned int nInlier = 0; + for(std::size_t i = 0; i < nData; ++i) + { + if(vec_residuals_[i] <= maxThreshold) + ++nInlier; + } + if(nInlier > 2.5 * sizeSample) // does the model is meaningful + bACRansacMode = true; + } + if(bACRansacMode) + { + for(size_t i = 0; i < nData; ++i) + { + const double error = vec_residuals_[i]; + vec_residuals[i] = ErrorIndex(error, i); + } + std::sort(vec_residuals.begin(), vec_residuals.end()); + + // Most meaningful discrimination inliers/outliers + const ErrorIndex best = bestNFA2(sizeSample, kernel.logalpha0(), vec_residuals, loge0, maxThreshold, + vec_logc_n, vec_logc_k, kernel.errorVectorDimension()); + + if(best.first < minNFA /*&& vec_residuals[best.second-1].first < errorMax*/) + { + // A better model was found + better = true; + minNFA = best.first; + vec_inliers.resize(best.second); + for(size_t i = 0; i < best.second; ++i) + { + vec_inliers[i] = vec_residuals[i].second; + } + + errorMax = vec_residuals[best.second - 1].first; // Error threshold + if(model) + { + *model = vec_models[k]; + } + } + } // if(bACRansacMode) + } // for(size_t k... + + // Early exit test -> no meaningful model found after nIterReserve*2 iterations + if(!bACRansacMode && iter > nIterReserve * 2) + break; + + // ACRANSAC optimization: draw samples among best set of inliers so far + if(bACRansacMode && ((better && minNFA < 0) || (iter + 1 == nIter && nIterReserve))) + { + if(vec_inliers.empty()) + { + // No model found at all so far + ++nIter; // Continue to look for any model, even not meaningful + --nIterReserve; + } + else + { + // ACRANSAC optimization: draw samples among best set of inliers so far + vec_index = vec_inliers; + if(nIterReserve) + { + nIter = iter + 1 + nIterReserve; + nIterReserve = 0; + } + } + } + } + + if(minNFA >= 0) + { + vec_inliers.clear(); + } + + return std::make_pair(errorMax, minNFA); +} + +} // namespace robustEstimation +} // namespace aliceVision \ No newline at end of file diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index a0b41ba1ab..d6ede4bee5 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -26,6 +26,7 @@ set(sfm_files_headers pipeline/RelativePoseInfo.hpp pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.hpp pipeline/panorama/ReconstructionEngine_panorama.hpp + pipeline/expanding/SfmTriangulation.hpp pipeline/regionsIO.hpp utils/alignment.hpp utils/statistics.hpp @@ -53,6 +54,7 @@ set(sfm_files_sources pipeline/RelativePoseInfo.cpp pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp pipeline/panorama/ReconstructionEngine_panorama.cpp + pipeline/expanding/SfmTriangulation.cpp pipeline/regionsIO.cpp utils/alignment.cpp utils/statistics.cpp diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp new file mode 100644 index 0000000000..022498fd0e --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp @@ -0,0 +1,227 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 "SfmTriangulation.hpp" + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include + +namespace aliceVision { +namespace sfm { + + + + +bool SfmTriangulation::process( + const sfmData::SfMData & sfmData, + const track::TracksMap & tracks, + const track::TracksPerView & tracksPerView, + std::mt19937 &randomNumberGenerator, + const std::set &viewIds, + std::set & evaluatedTracks, + std::map & outputLandmarks + ) +{ + evaluatedTracks.clear(); + outputLandmarks.clear(); + + // Get all tracks id which are visible in views + std::set viewTracks; + track::getTracksInImagesFast(viewIds, tracksPerView, viewTracks); + + std::vector viewTracksVector; + std::copy(viewTracks.begin(), viewTracks.end(), std::back_inserter(viewTracksVector)); + + const std::set& validViews = sfmData.getValidViews(); + + // Get a set of all views to consider + std::set allInterestingViews; + allInterestingViews.insert(viewIds.begin(), viewIds.end()); + allInterestingViews.insert(validViews.begin(), validViews.end()); + + + #pragma omp parallel for + for(int pos = 0; pos < viewTracksVector.size(); pos++) + { + const std::size_t trackId = viewTracksVector[pos]; + const track::Track& track = tracks.at(trackId); + + // Get all views observing the current track (Keeping their Id) + std::set trackViews; + std::transform(track.featPerView.begin(), track.featPerView.end(), + std::inserter(trackViews, trackViews.begin()), stl::RetrieveKey()); + + // Intersect with list of interesting views + std::set trackViewsFiltered; + std::set_intersection(trackViews.begin(), trackViews.end(), + allInterestingViews.begin(), allInterestingViews.end(), + std::inserter(trackViewsFiltered, trackViewsFiltered.begin())); + + if(trackViewsFiltered.size() < _minObservations) + { + continue; + } + + #pragma omp critical + { + + evaluatedTracks.insert(trackId); + } + + sfmData::Landmark result; + if (!processTrack(sfmData, track, randomNumberGenerator, trackViewsFiltered, result)) + { + continue; + } + + #pragma omp critical + { + outputLandmarks[trackId] = result; + } + } + + return true; +} + +bool SfmTriangulation::processTrack( + const sfmData::SfMData & sfmData, + const track::Track & track, + std::mt19937 &randomNumberGenerator, + const std::set & viewIds, + sfmData::Landmark & result + ) +{ + feature::EImageDescriberType descType = track.descType; + + std::vector observations; + std::vector> intrinsics; + std::vector poses; + std::vector indexedViewIds; + + for (auto viewId : viewIds) + { + //Retrieve pose and feature coordinates for this observation + const sfmData::View & view = sfmData.getView(viewId); + const std::shared_ptr intrinsic = sfmData.getIntrinsicSharedPtr(view.getIntrinsicId()); + const Eigen::Matrix4d pose = sfmData.getPose(view).getTransform().getHomogeneous(); + + const auto & trackItem = track.featPerView.at(viewId); + + //Lift the coordinates to metric unit sphere + const Vec2 coords = trackItem.coords; + + observations.push_back(coords); + intrinsics.push_back(intrinsic); + poses.push_back(pose); + + indexedViewIds.push_back(viewId); + } + + + + robustEstimation::MatrixModel model; + std::vector inliers; + robustEstimation::ScoreEvaluator scorer(_maxError); + multiview::TriangulationSphericalKernel kernel(observations, poses, intrinsics); + + if (observations.size() <= 0) + { + } + else + { + model = robustEstimation::LO_RANSAC(kernel, scorer, randomNumberGenerator, &inliers); + } + + Vec4 X = model.getMatrix(); + + Vec3 X_euclidean; + homogeneousToEuclidean(X, X_euclidean); + + //Create landmark from result + result.X = X_euclidean; + result.descType = track.descType; + + for (const std::size_t & i : inliers) + { + //Inlier to view index + IndexT viewId = indexedViewIds[i]; + + sfmData::Observation & o = result.getObservations()[viewId]; + + //Retrieve observation data + const auto & trackItem = track.featPerView.at(viewId); + + o.setFeatureId(trackItem.featureId); + o.setScale(trackItem.scale); + o.setCoordinates(trackItem.coords); + } + + return true; +} + +bool SfmTriangulation::checkChierality(const sfmData::SfMData & sfmData, const sfmData::Landmark & landmark) +{ + for (const auto & pRefObs : landmark.getObservations()) + { + IndexT refViewId = pRefObs.first; + + const sfmData::View & refView = sfmData.getView(refViewId); + const sfmData::CameraPose & refCameraPose = sfmData.getPoses().at(refView.getPoseId()); + const geometry::Pose3 & refPose = refCameraPose.getTransform(); + + if (refPose.depth(landmark.X) < 0.0) + { + return false; + } + } + + return true; +} + +double SfmTriangulation::getMaximalAngle(const sfmData::SfMData & sfmData, const sfmData::Landmark & landmark) +{ + double max = 0.0; + + for (const auto & pRefObs : landmark.getObservations()) + { + IndexT refViewId = pRefObs.first; + + const sfmData::View & refView = sfmData.getView(refViewId); + const sfmData::CameraPose & refCameraPose = sfmData.getPoses().at(refView.getPoseId()); + const geometry::Pose3 & refPose = refCameraPose.getTransform(); + + for (const auto & pNextObs : landmark.getObservations()) + { + IndexT nextViewId = pNextObs.first; + if (refViewId > nextViewId) + { + continue; + } + + const sfmData::View & nextView = sfmData.getView(nextViewId); + const sfmData::CameraPose & nextCameraPose = sfmData.getPoses().at(nextView.getPoseId()); + const geometry::Pose3 & nextPose = nextCameraPose.getTransform(); + double angle_deg = camera::angleBetweenRays(refPose, nextPose, landmark.X); + + max = std::max(max, angle_deg); + } + } + + return max; +} + +} // namespace sfm +} // namespace aliceVision \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp new file mode 100644 index 0000000000..8a0df4d02d --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp @@ -0,0 +1,92 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 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 + +namespace aliceVision { +namespace sfm { + +/** + * Class to compute triangulation of a set of connected observations. +*/ +class SfmTriangulation +{ +public: + SfmTriangulation(size_t minObservations, double maxError) + : _minObservations(minObservations), + _maxError(maxError) + { + + } + + /** + * Process triangulation + * @param sfmData the actual state of the sfm + * @param tracks the list of tracks for this scene + * @param tracksPerView the list of tracks organized per views for the whole scene + * @param randomNumberGenerator random number generator object + * @param viewIds the set of view ids to process. Only tracks observed in these views will be considered + * @param evaluatedTracks output list of track ids which were evaluated (Not necessarily with success) + * @param outputLandmarks a set of generated landmarks indexed by their landmark id (the associated track id) + * @return false if a critical error occured + */ + bool process( + const sfmData::SfMData & sfmData, + const track::TracksMap & tracks, + const track::TracksPerView & tracksPerView, + std::mt19937 &randomNumberGenerator, + const std::set & viewIds, + std::set & evaluatedTracks, + std::map & outputLandmarks + ); + +public: + /** + * Check that all observation of a given landmark are physically possible + * @param sfmData the scene description + * @param landmark the landmark considered + * @return false if some observation is wrong + */ + static bool checkChierality(const sfmData::SfMData & sfmData, const sfmData::Landmark & landmark); + + /** + * Compute the maximal parallax between all observations of a given landmark + * @param sfmData the scene description + * @param landmark the landmark considered + * @return an angle in degree + */ + static double getMaximalAngle(const sfmData::SfMData & sfmData, const sfmData::Landmark & landmark); + +private: + /** + * Process triangulation of a track + * @param sfmData the actual state of the sfm + * @param track the track of interest + * @param randomNumberGenerator random number generator object + * @param viewIds the set of view ids to process. Only tracks observed in these views will be considered + * @param result the output landmark + * @return false if a critical error occured + */ + bool processTrack( + const sfmData::SfMData & sfmData, + const track::Track & track, + std::mt19937 &randomNumberGenerator, + const std::set & viewIds, + sfmData::Landmark & result + ); + +private: + const size_t _minObservations; + const double _maxError; +}; + +} // namespace sfm +} // namespace aliceVision \ No newline at end of file From 3ea09cc2f60ca93d2f7842eabbb0e3acce1d30f2 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 23 Apr 2024 10:21:17 +0200 Subject: [PATCH 02/14] Add resection class --- .../multiview/resection/P3PSolver.cpp | 24 +++ .../multiview/resection/P3PSolver.hpp | 9 + .../resection/ResectionSphericalKernel.hpp | 139 ++++++++++++++ src/aliceVision/sfm/CMakeLists.txt | 2 + .../sfm/pipeline/expanding/SfmResection.cpp | 179 ++++++++++++++++++ .../sfm/pipeline/expanding/SfmResection.hpp | 73 +++++++ 6 files changed, 426 insertions(+) create mode 100644 src/aliceVision/multiview/resection/ResectionSphericalKernel.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp diff --git a/src/aliceVision/multiview/resection/P3PSolver.cpp b/src/aliceVision/multiview/resection/P3PSolver.cpp index ab8f8b1084..5de2637e4a 100644 --- a/src/aliceVision/multiview/resection/P3PSolver.cpp +++ b/src/aliceVision/multiview/resection/P3PSolver.cpp @@ -264,6 +264,30 @@ void P3PSolver::solve(const Mat& x2d, const Mat& x3d, std::vector& models) const +{ + Mat3 R; + Vec3 t; + + Mat solutions = Mat(3, 16); + + models.clear(); + if(computeP3PPoses(lifted, structure, solutions)) + { + for(size_t i = 0; i < 4; ++i) + { + R = solutions.block<3, 3>(0, i * 4 + 1); + t = -R * solutions.col(i * 4); + + Eigen::Matrix4d model = Eigen::Matrix4d::Identity(); + model.block<3, 3>(0, 0) = R; + model.block<3, 1>(0, 3) = t; + + models.emplace_back(model); + } + } +} + } // namespace resection } // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/multiview/resection/P3PSolver.hpp b/src/aliceVision/multiview/resection/P3PSolver.hpp index 1f18ef3cac..17a735608c 100644 --- a/src/aliceVision/multiview/resection/P3PSolver.hpp +++ b/src/aliceVision/multiview/resection/P3PSolver.hpp @@ -40,6 +40,15 @@ class P3PSolver : public robustEstimation::ISolver */ void solve(const Mat& x2d, const Mat& x3d, std::vector& models) const override; + /** + * @brief Solve the problem of camera pose. + * + * @param[in] lifted 2d points lifted on the unit sphere. One per column. + * @param[in] structure Corresponding 3d points + * @param[out] models A list of at most 4 candidate solutions. + */ + void solve(const Mat3 & lifted, const Mat3 & structure, std::vector& models) const; + /** * @brief Solve the problem. * diff --git a/src/aliceVision/multiview/resection/ResectionSphericalKernel.hpp b/src/aliceVision/multiview/resection/ResectionSphericalKernel.hpp new file mode 100644 index 0000000000..15d422905b --- /dev/null +++ b/src/aliceVision/multiview/resection/ResectionSphericalKernel.hpp @@ -0,0 +1,139 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace multiview { +namespace resection { + +class ResectionSphericalKernel +{ +public: + using ModelT = Eigen::Matrix4d; + +public: + ResectionSphericalKernel(std::shared_ptr & camera, + const std::vector & structure, + const std::vector & observations) + : _camera(camera), + _structure(structure), + _observations(observations) + { + for (const auto & pt : _observations) + { + _liftedObservations.push_back(_camera->toUnitSphere(_camera->removeDistortion(_camera->ima2cam(pt)))); + } + + } + + /** + * @brief Return the minimum number of required samples for the solver + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamples() const + { + return 3; + } + + /** + * @brief Return the minimum number of required samples for the solver Ls + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamplesLS() const + { + return 3; + } + + /** + * @brief Return the maximum number of models for the solver + * @return maximum number of models + */ + std::size_t getMaximumNbModels() const + { + return 4; + } + + /** + * @brief The number of elements in the data. + * @return the number of elements in the data. + */ + std::size_t nbSamples() const + { + return _structure.size(); + } + + /** + * @brief Get logalpha0, Alpha0 is used to make the error adaptive to the image size + * @return logalpha0 + */ + double logalpha0() const + { + return log10(M_PI / (_camera->w() * _camera->h())); + } + + double errorVectorDimension() const + { + return 2.0; + } + + /** + * @brief This function is called to estimate the model from the minimum number + * of sample \p minSample (i.e. minimal problem solver). + * @param[in] samples A vector containing the indices of the data to be used for + * the minimal estimation. + * @param[out] models The model(s) estimated by the minimal solver. + */ + void fit(const std::vector& samples, std::vector& models) const + { + Mat3 X; + Mat3 x; + + for (int pos = 0; pos < 3; pos++) + { + size_t id = samples[pos]; + X.col(pos) = _structure[id]; + x.col(pos) = _liftedObservations[id]; + } + + _solver.solve(x, X, models); + } + + + /** + * @brief Function that computes the estimation error for a given model and all the elements. + * @param[in] model The model to consider. + * @param[out] vec_errors The vector containing all the estimation errors for every element. + */ + void errors(const Eigen::Matrix4d & model, std::vector& errors) const + { + for (int idx = 0; idx < _structure.size(); idx++) + { + const Vec4 X = _structure[idx].homogeneous(); + const Vec2 x = _observations[idx]; + + const Vec2 residual = _camera->residual(geometry::Pose3(model), X, x); + + errors[idx] = residual.norm(); + } + } + +private: + std::shared_ptr _camera; + std::vector _structure; + std::vector _observations; + std::vector _liftedObservations; + multiview::resection::P3PSolver _solver; +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index d6ede4bee5..c75a8bff68 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -27,6 +27,7 @@ set(sfm_files_headers pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.hpp pipeline/panorama/ReconstructionEngine_panorama.hpp pipeline/expanding/SfmTriangulation.hpp + pipeline/expanding/SfmResection.hpp pipeline/regionsIO.hpp utils/alignment.hpp utils/statistics.hpp @@ -55,6 +56,7 @@ set(sfm_files_sources pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp pipeline/panorama/ReconstructionEngine_panorama.cpp pipeline/expanding/SfmTriangulation.cpp + pipeline/expanding/SfmResection.cpp pipeline/regionsIO.cpp utils/alignment.cpp utils/statistics.cpp diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp new file mode 100644 index 0000000000..7a2f992ffa --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp @@ -0,0 +1,179 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 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 "SfmResection.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace aliceVision { +namespace sfm { + + +bool SfmResection::processView( + const sfmData::SfMData & sfmData, + const track::TracksMap & tracks, + const track::TracksPerView & tracksPerView, + std::mt19937 &randomNumberGenerator, + const IndexT viewId, + Eigen::Matrix4d & updatedPose, + double & updatedThreshold + ) +{ + // A. Compute 2D/3D matches + // A1. list tracks ids used by the view + const aliceVision::track::TrackIdSet & viewTracksIds = tracksPerView.at(viewId); + + // A2. Each landmark's id is equal to the associated track id + // Get list of landmarks = get list of reconstructed tracks + std::set reconstructedTrackId; + std::transform(sfmData.getLandmarks().begin(), sfmData.getLandmarks().end(), + std::inserter(reconstructedTrackId, reconstructedTrackId.begin()), + stl::RetrieveKey()); + + // Remove all reconstructed tracks which were not observed in the view to resect. + // --> Intersection of tracks observed in this view and tracks reconstructed. + std::set trackIds; + std::set_intersection(viewTracksIds.begin(), viewTracksIds.end(), + reconstructedTrackId.begin(), + reconstructedTrackId.end(), + std::inserter(trackIds, trackIds.begin())); + + if (trackIds.size() < 3) + { + // If less than 3 points, the resection is theorically impossible. + // Let ignore this view. + return false; + } + + + //Get information about this view + const std::shared_ptr view = sfmData.getViews().at(viewId); + std::shared_ptr intrinsic = sfmData.getIntrinsicSharedPtr(view->getIntrinsicId()); + + //Loop over tracks to build data needed by resection process + std::vector structure; + std::vector observations; + std::vector featureTypes; + for (const auto & trackId : trackIds) + { + const auto & track = tracks.at(trackId); + + const feature::EImageDescriberType descType = track.descType; + const Eigen::Vector3d X = sfmData.getLandmarks().at(trackId).X; + const Eigen::Vector2d x = track.featPerView.at(viewId).coords; + + structure.push_back(X); + observations.push_back(x); + featureTypes.push_back(descType); + } + + //Compute a first estimation of the pose + Eigen::Matrix4d pose; + std::vector inliers; + double errorMax = 0.0; + if (!internalResection(intrinsic, randomNumberGenerator, structure, observations, featureTypes, pose, inliers, errorMax)) + { + return false; + } + + //Refine the pose + if (!internalRefinement(structure, observations, inliers, pose, intrinsic)) + { + return false; + } + + updatedThreshold = errorMax; + updatedPose = pose; + + return true; +} + +bool SfmResection::internalResection( + std::shared_ptr & intrinsic, + std::mt19937 &randomNumberGenerator, + const std::vector & structure, + const std::vector & observations, + const std::vector & featureTypes, + Eigen::Matrix4d & pose, + std::vector & inliers, + double & errorMax + ) +{ + multiview::resection::ResectionSphericalKernel kernel(intrinsic, structure, observations); + + Eigen::Matrix4d model; + + inliers.clear(); + auto pairResult = robustEstimation::NACRANSAC(kernel, randomNumberGenerator, inliers, _maxIterations, &model, _precision); + + errorMax = pairResult.first; + const bool resection = matching::hasStrongSupport(inliers, featureTypes, 3); + + if (resection) + { + pose = model; + return true; + } + + return false; +} + +bool SfmResection::internalRefinement( + const std::vector & structure, + const std::vector & observations, + const std::vector & inliers, + Eigen::Matrix4d & pose, + std::shared_ptr & intrinsics) +{ + // Setup a tiny SfM scene with the corresponding 2D-3D data + sfmData::SfMData tinyScene; + + // view + std::shared_ptr view = std::make_shared("", 0, 0, 0); + tinyScene.getViews().insert(std::make_pair(0, view)); + + // pose + tinyScene.setPose(*view, sfmData::CameraPose(geometry::Pose3(pose))); + + // Intrinsics + tinyScene.getIntrinsics().emplace(0, intrinsics); + + const double unknownScale = 0.0; + + // structure data (2D-3D correspondences) + for(std::size_t i = 0; i < inliers.size(); ++i) + { + const std::size_t idx = inliers[i]; + + sfmData::Landmark landmark; + landmark.X = structure[idx]; + landmark.getObservations()[0] = sfmData::Observation(observations[idx], UndefinedIndexT, unknownScale); + tinyScene.getLandmarks()[i] = std::move(landmark); + } + + BundleAdjustmentCeres BA; + BundleAdjustment::ERefineOptions refineOptions = BundleAdjustment::REFINE_ROTATION | BundleAdjustment::REFINE_TRANSLATION; + + const bool success = BA.adjust(tinyScene, refineOptions); + if(!success) + { + return false; + } + + pose = tinyScene.getPose(*view).getTransform().getHomogeneous(); + + return true; +} + +} // namespace sfm +} // namespace aliceVision \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp new file mode 100644 index 0000000000..c7185694b9 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp @@ -0,0 +1,73 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class SfmResection +{ +public: + SfmResection(size_t maxIterations, double precision) + : _maxIterations(maxIterations), + _precision(precision) + { + + } + + /** + * Process resection + * @param sfmData the actual state of the sfm + * @param tracks the list of tracks for this scene + * @param tracksPerView the list of tracks organized per views for the whole scene + * @param randomNumberGenerator random number generator object + * @param viewId the view id to process + * @param updatedPose output estimated pose + * @param updatedThreshold estimated threshold + * @return false if a critical error occured + */ + bool processView( + const sfmData::SfMData & sfmData, + const track::TracksMap & tracks, + const track::TracksPerView & map_tracksPerView, + std::mt19937 &randomNumberGenerator, + const IndexT viewId, + Eigen::Matrix4d & updatedPose, + double & updatedThreshold + ); + +private: + bool internalResection( + std::shared_ptr & intrinsic, + std::mt19937 &randomNumberGenerator, + const std::vector & structure, + const std::vector & observations, + const std::vector & featureTypes, + Eigen::Matrix4d & pose, + std::vector & inliers, + double &errorMax + ); + + bool internalRefinement( + const std::vector & structure, + const std::vector & observations, + const std::vector & inliers, + Eigen::Matrix4d & pose, + std::shared_ptr & intrinsics + ); + +private: + double _precision; + size_t _maxIterations; +}; + +} // namespace sfm +} // namespace aliceVision \ No newline at end of file From 47880f4b4349de7e30aafbf7436958ebbb62805e Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 23 Apr 2024 11:04:34 +0200 Subject: [PATCH 03/14] Add a SfmBundle class --- src/aliceVision/sfm/CMakeLists.txt | 2 + .../sfm/pipeline/expanding/SfmBundle.cpp | 92 +++++++++++++++++++ .../sfm/pipeline/expanding/SfmBundle.hpp | 63 +++++++++++++ 3 files changed, 157 insertions(+) create mode 100644 src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index c75a8bff68..c728a10f80 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -28,6 +28,7 @@ set(sfm_files_headers pipeline/panorama/ReconstructionEngine_panorama.hpp pipeline/expanding/SfmTriangulation.hpp pipeline/expanding/SfmResection.hpp + pipeline/expanding/SfmBundle.hpp pipeline/regionsIO.hpp utils/alignment.hpp utils/statistics.hpp @@ -57,6 +58,7 @@ set(sfm_files_sources pipeline/panorama/ReconstructionEngine_panorama.cpp pipeline/expanding/SfmTriangulation.cpp pipeline/expanding/SfmResection.cpp + pipeline/expanding/SfmBundle.cpp pipeline/regionsIO.cpp utils/alignment.cpp utils/statistics.cpp diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp new file mode 100644 index 0000000000..4ed399d13c --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp @@ -0,0 +1,92 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 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 "SfmBundle.hpp" +#include +#include +#include + +namespace aliceVision { +namespace sfm { + +bool SfmBundle::process(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) +{ + BundleAdjustmentSymbolicCeres::CeresOptions options; + BundleAdjustment::ERefineOptions refineOptions; + + refineOptions |= BundleAdjustment::REFINE_ROTATION; + refineOptions |= BundleAdjustment::REFINE_TRANSLATION; + refineOptions |= BundleAdjustment::REFINE_STRUCTURE; + refineOptions |= BundleAdjustment::REFINE_INTRINSICS_ALL; + + if (!initialize(sfmData, tracksHandler, viewIds)) + { + return false; + } + + options.setSparseBA(); + BundleAdjustmentSymbolicCeres bundleObject(options, _minNbCamerasToRefinePrincipalPoint); + + //Repeat until nothing change + do + { + const bool success = bundleObject.adjust(sfmData, refineOptions); + if (!success) + { + return false; + } + } + while (cleanup(sfmData)); + + return true; +} + +bool SfmBundle::cleanup(sfmData::SfMData & sfmData) +{ + // Remove observations with too large residuals + const std::size_t nbOutliersResidualErr = removeOutliersWithPixelResidualError(sfmData, _featureConstraint, _maxReprojectionError, _minTrackLength); + + // Remove landmarks without enough observed parallax + const std::size_t nbOutliersAngleErr = removeOutliersWithAngleError(sfmData, _minAngleForLandmark); + + // Remove poses without enough observations in an interative fashion + const std::size_t nbOutliers = nbOutliersResidualErr + nbOutliersAngleErr; + std::set removedViewsIdIteration; + bool somethingErased = eraseUnstablePosesAndObservations(sfmData, _minPointsPerPose, _minTrackLength, &removedViewsIdIteration); + + bool somethingChanged = /*somethingErased || */(nbOutliers > _bundleAdjustmentMaxOutlier); + + return somethingChanged; +} + +bool SfmBundle::initialize(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) +{ + bool enableLocalStrategy = _useLBA; + + if (sfmData.getPoses().size() < _minNbCamerasLBA) + { + enableLocalStrategy = false; + } + + if (enableLocalStrategy) + { + /*if (_lbaPolicy) + { + _lbaPolicy->build(sfmData, tracksHandler, viewIds); + }*/ + + /*_lbaGraph = std::make_shared(sfmData); + _lbaGraph->setGraphDistanceLimit(_LBAGraphDistanceLimit); + _lbaGraph->updateGraphWithNewViews(sfmData, tracksHandler.getTracksPerView(), sfmData.getValidViews(), _LBAMinNbOfMatches); + _lbaGraph->computeGraphDistances(sfmData, viewIds); + _lbaGraph->convertDistancesToStates(sfmData);*/ + } + + return true; +} + +} // namespace sfm +} // namespace aliceVision \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp new file mode 100644 index 0000000000..8e36fba366 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp @@ -0,0 +1,63 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class SfmBundle +{ +public: + /** + * @brief Process bundle + * @param sfmData the scene description to optimize + * @param tracksHandler the tracks manager + * @param viewIds the set of views to bundle + * @return false if an error occured + */ + bool process(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); + +private: + /** + * Initialize bundle properties + */ + bool initialize(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); + + /** + * Cleanup sfmData + * @param sfmData the scene to clean + * @return true if enough change occured during the cleaning + */ + bool cleanup(sfmData::SfMData & sfmData); + +private: + //std::unique_ptr _lbaPolicy; + +private: + + EFeatureConstraint _featureConstraint = EFeatureConstraint::SCALE; + double _maxReprojectionError = 40.0; + double _minAngleForLandmark = 2.0; + size_t _minTrackLength = 2; + size_t _minPointsPerPose = 30; + size_t _bundleAdjustmentMaxOutlier = 50; + size_t _minNbCamerasToRefinePrincipalPoint = 3; + bool _useLBA = true; + size_t _minNbCamerasLBA = 100; + size_t _LBAGraphDistanceLimit = 1; + size_t _LBAMinNbOfMatches = 50; +}; + +} // namespace sfm +} // namespace aliceVision \ No newline at end of file From f4b1cfdfbea80df997b8bc10c3eab3fc1cd9bf2d Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Thu, 2 May 2024 11:40:18 +0200 Subject: [PATCH 04/14] relative pose update --- src/aliceVision/camera/Equidistant.cpp | 7 + src/aliceVision/camera/Equidistant.hpp | 7 + src/aliceVision/camera/IntrinsicBase.hpp | 6 + src/aliceVision/camera/Pinhole.cpp | 7 + src/aliceVision/camera/Pinhole.hpp | 6 + src/aliceVision/multiview/CMakeLists.txt | 2 + src/aliceVision/multiview/essential.cpp | 43 ++++ src/aliceVision/multiview/essential.hpp | 5 + .../relativePose/Essential5PSolver.cpp | 16 +- .../relativePose/RelativeSphericalKernel.hpp | 146 +++++++++++++ .../relativePose/RotationSphericalKernel.hpp | 147 +++++++++++++ .../triangulation/triangulationDLT.cpp | 18 +- .../triangulation/triangulationDLT.hpp | 12 +- src/aliceVision/track/CMakeLists.txt | 2 +- .../pipeline/main_relativePoseEstimating.cpp | 200 ++++++++---------- .../pipeline/main_sfmBootstraping.cpp | 122 +++++------ 16 files changed, 548 insertions(+), 198 deletions(-) create mode 100644 src/aliceVision/multiview/relativePose/RelativeSphericalKernel.hpp create mode 100644 src/aliceVision/multiview/relativePose/RotationSphericalKernel.hpp diff --git a/src/aliceVision/camera/Equidistant.cpp b/src/aliceVision/camera/Equidistant.cpp index bf0313b9e8..5aeb639bbb 100644 --- a/src/aliceVision/camera/Equidistant.cpp +++ b/src/aliceVision/camera/Equidistant.cpp @@ -481,5 +481,12 @@ double Equidistant::getHorizontalFov() const double Equidistant::getVerticalFov() const { return getHorizontalFov(); } +double Equidistant::pixelProbability() const +{ + return 1.0 / double(w()); +} + + + } // namespace camera } // namespace aliceVision diff --git a/src/aliceVision/camera/Equidistant.hpp b/src/aliceVision/camera/Equidistant.hpp index 1288515ea0..c3e7985027 100644 --- a/src/aliceVision/camera/Equidistant.hpp +++ b/src/aliceVision/camera/Equidistant.hpp @@ -143,6 +143,13 @@ class Equidistant : public IntrinsicScaleOffsetDisto */ double getVerticalFov() const override; + + /** + * @brief how a one pixel change relates to an angular change + * @return a value in radians + */ + virtual double pixelProbability() const override; + protected: double _circleRadius{0.0}; Vec2 _circleCenter{0.0, 0.0}; diff --git a/src/aliceVision/camera/IntrinsicBase.hpp b/src/aliceVision/camera/IntrinsicBase.hpp index c45e74b60c..a9137f1a49 100644 --- a/src/aliceVision/camera/IntrinsicBase.hpp +++ b/src/aliceVision/camera/IntrinsicBase.hpp @@ -363,6 +363,12 @@ class IntrinsicBase */ virtual double imagePlaneToCameraPlaneError(double value) const = 0; + /** + * @brief What is the probability of a pixel wrt the whole fov + * @return a value in radians + */ + virtual double pixelProbability() const = 0; + /** * @brief Return true if the intrinsic is valid * @return True if the intrinsic is valid diff --git a/src/aliceVision/camera/Pinhole.cpp b/src/aliceVision/camera/Pinhole.cpp index 3c9f1ecc03..81567b5daa 100644 --- a/src/aliceVision/camera/Pinhole.cpp +++ b/src/aliceVision/camera/Pinhole.cpp @@ -327,5 +327,12 @@ double Pinhole::getVerticalFov() const return 2.0 * atan2(sensorHeight() / 2.0, focalLengthMM); } +double Pinhole::pixelProbability() const +{ + const double focalLengthMM = sensorHeight() * getScale().y() / double(h()); + const double pixToMm = 1.0 / _scale(0); + return std::atan2(pixToMm, focalLengthMM) / getVerticalFov(); +} + } // namespace camera } // namespace aliceVision diff --git a/src/aliceVision/camera/Pinhole.hpp b/src/aliceVision/camera/Pinhole.hpp index b477462a5e..9b6b7da2d1 100644 --- a/src/aliceVision/camera/Pinhole.hpp +++ b/src/aliceVision/camera/Pinhole.hpp @@ -114,6 +114,12 @@ class Pinhole : public IntrinsicScaleOffsetDisto * @return Vertical FOV in radians */ double getVerticalFov() const override; + + /** + * @brief how a one pixel change relates to an angular change + * @return a value in radians + */ + virtual double pixelProbability() const override; }; } // namespace camera diff --git a/src/aliceVision/multiview/CMakeLists.txt b/src/aliceVision/multiview/CMakeLists.txt index 206b1ea354..b35d49504d 100644 --- a/src/aliceVision/multiview/CMakeLists.txt +++ b/src/aliceVision/multiview/CMakeLists.txt @@ -20,6 +20,8 @@ set(multiview_files_headers relativePose/HomographyKernel.hpp relativePose/Rotation3PSolver.hpp relativePose/ISolverErrorRelativePose.hpp + relativePose/RelativeSphericalKernel.hpp + relativePose/RotationSphericalKernel.hpp resection/EPnPSolver.hpp resection/EPnPKernel.hpp resection/P3PSolver.hpp diff --git a/src/aliceVision/multiview/essential.cpp b/src/aliceVision/multiview/essential.cpp index 472b06a11f..24389342c2 100644 --- a/src/aliceVision/multiview/essential.cpp +++ b/src/aliceVision/multiview/essential.cpp @@ -73,6 +73,49 @@ void motionFromEssential(const Mat3& E, std::vector* Rs, std::vector (*ts)[3] = -U.col(2); } +// HZ 9.7 page 259 (Result 9.19) +void motionFromEssential(const Mat3& E, std::vector & Ts) +{ + Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + Mat3 U = USV.matrixU(); + // Vec3 d = USV.singularValues(); + Mat3 Vt = USV.matrixV().transpose(); + + // Last column of U is undetermined since d = (a a 0). + if (U.determinant() < 0) + { + U.col(2) *= -1; + } + // Last row of Vt is undetermined since d = (a a 0). + if (Vt.determinant() < 0) + { + Vt.row(2) *= -1; + } + + Mat3 W; + W << 0, -1, 0, 1, 0, 0, 0, 0, 1; + + Mat3 U_W_Vt = U * W * Vt; + Mat3 U_Wt_Vt = U * W.transpose() * Vt; + + Ts.resize(4); + Ts[0].setIdentity(); + Ts[0].block<3, 3>(0, 0) = U_W_Vt; + Ts[0].block<3, 1>(0, 3) = U.col(2); + + Ts[1].setIdentity(); + Ts[1].block<3, 3>(0, 0) = U_W_Vt; + Ts[1].block<3, 1>(0, 3) = -U.col(2); + + Ts[2].setIdentity(); + Ts[2].block<3, 3>(0, 0) = U_Wt_Vt; + Ts[2].block<3, 1>(0, 3) = U.col(2); + + Ts[3].setIdentity(); + Ts[3].block<3, 3>(0, 0) = U_Wt_Vt; + Ts[3].block<3, 1>(0, 3) = -U.col(2); +} + // HZ 9.6 pag 259 (9.6.3 Geometrical interpretation of the 4 solutions) int motionFromEssentialChooseSolution(const std::vector& Rs, const std::vector& ts, diff --git a/src/aliceVision/multiview/essential.hpp b/src/aliceVision/multiview/essential.hpp index a207cbc731..464611de83 100644 --- a/src/aliceVision/multiview/essential.hpp +++ b/src/aliceVision/multiview/essential.hpp @@ -57,4 +57,9 @@ int motionFromEssentialChooseSolution(const std::vector& Rs, */ void motionFromEssential(const Mat3& E, std::vector* Rs, std::vector* ts); +/** + * @brief HZ 9.7 page 259 (Result 9.19) + */ +void motionFromEssential(const Mat3& E, std::vector & Ts); + } // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/Essential5PSolver.cpp b/src/aliceVision/multiview/relativePose/Essential5PSolver.cpp index 55787705ae..f94703fdef 100644 --- a/src/aliceVision/multiview/relativePose/Essential5PSolver.cpp +++ b/src/aliceVision/multiview/relativePose/Essential5PSolver.cpp @@ -71,6 +71,18 @@ Mat fivePointsNullspaceBasis(const Mat2X& x1, const Mat2X& x2) return svd.matrixV().topRightCorner<9, 4>(); } +/** + * @brief Compute the nullspace of the linear constraints given by the matches. + */ +Mat fivePointsNullspaceBasisSpherical(const Mat& x1, const Mat& x2) +{ + Eigen::Matrix A; + A.setZero(); // make A square until Eigen supports rectangular SVD. + encodeEpipolarSphericalEquation(x1, x2, &A); + Eigen::JacobiSVD> svd(A, Eigen::ComputeFullV); + return svd.matrixV().topRightCorner<9, 4>(); +} + /** * @brief Builds the polynomial constraint matrix M. */ @@ -139,13 +151,13 @@ Mat fivePointsPolynomialConstraints(const Mat& EBasis) void Essential5PSolver::solve(const Mat& x1, const Mat& x2, std::vector& models) const { - assert(2 == x1.rows()); + assert(2 == x1.rows() || 3 == x1.rows()); assert(5 <= x1.cols()); assert(x1.rows() == x2.rows()); assert(x1.cols() == x2.cols()); // step 1: Nullspace Extraction. - const Eigen::Matrix EBasis = fivePointsNullspaceBasis(x1, x2); + const Eigen::Matrix EBasis = (x1.rows()==2)?fivePointsNullspaceBasis(x1, x2):fivePointsNullspaceBasisSpherical(x1, x2); // step 2: Constraint Expansion. const Eigen::Matrix EConstraints = fivePointsPolynomialConstraints(EBasis); diff --git a/src/aliceVision/multiview/relativePose/RelativeSphericalKernel.hpp b/src/aliceVision/multiview/relativePose/RelativeSphericalKernel.hpp new file mode 100644 index 0000000000..926cc24bfa --- /dev/null +++ b/src/aliceVision/multiview/relativePose/RelativeSphericalKernel.hpp @@ -0,0 +1,146 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +class RelativeSphericalKernel +{ +public: + using ModelT = robustEstimation::Mat3Model; + +public: + RelativeSphericalKernel(const camera::IntrinsicBase & camera1, + const camera::IntrinsicBase & camera2, + const std::vector & observations1, + const std::vector & observations2) + : _camera1(camera1), + _camera2(camera2) + { + for (const auto & pt : observations1) + { + _liftedObservations1.push_back(camera1.toUnitSphere(camera1.removeDistortion(camera1.ima2cam(pt)))); + } + + for (const auto & pt : observations2) + { + _liftedObservations2.push_back(camera2.toUnitSphere(camera2.removeDistortion(camera2.ima2cam(pt)))); + } + } + + /** + * @brief Return the minimum number of required samples for the solver + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamples() const + { + return 5; + } + + /** + * @brief Return the minimum number of required samples for the solver Ls + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamplesLS() const + { + return 5; + } + + /** + * @brief Return the maximum number of models for the solver + * @return maximum number of models + */ + std::size_t getMaximumNbModels() const + { + return 10; + } + + /** + * @brief The number of elements in the data. + * @return the number of elements in the data. + */ + std::size_t nbSamples() const + { + return _liftedObservations1.size(); + } + + /** + * @brief Get logalpha0, Alpha0 is used to make the error adaptive to the image size + * @return logalpha0 + */ + double logalpha0() const + { + return log10(_camera2.pixelProbability() * 2.0); + } + + double errorVectorDimension() const + { + return 1.0; + } + + /** + * @brief This function is called to estimate the model from the minimum number + * of sample \p minSample (i.e. minimal problem solver). + * @param[in] samples A vector containing the indices of the data to be used for + * the minimal estimation. + * @param[out] models The model(s) estimated by the minimal solver. + */ + void fit(const std::vector& samples, std::vector& models) const + { + Mat x1(3, 5); + Mat x2(3, 5); + + for (int pos = 0; pos < 5; pos++) + { + size_t id = samples[pos]; + x1.col(pos) = _liftedObservations1[id]; + x2.col(pos) = _liftedObservations2[id]; + } + + _solver.solve(x1, x2, models); + } + + + /** + * @brief Function that computes the estimation error for a given model and all the elements. + * @param[in] model The model to consider. + * @param[out] vec_errors The vector containing all the estimation errors for every element. + */ + void errors(const robustEstimation::Mat3Model & model, std::vector& errors) const + { + Eigen::Matrix3d E = model.getMatrix(); + + for (int idx = 0; idx < _liftedObservations1.size(); idx++) + { + const Vec3 x1 = _liftedObservations1[idx]; + const Vec3 x2 = _liftedObservations2[idx]; + + const Vec3 x = (E * x1).normalized(); + + errors[idx] = std::abs(std::asin(x2.dot(x))); + } + } + +private: + const camera::IntrinsicBase & _camera1; + const camera::IntrinsicBase & _camera2; + + std::vector _liftedObservations1; + std::vector _liftedObservations2; + multiview::relativePose::Essential5PSolver _solver; +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/relativePose/RotationSphericalKernel.hpp b/src/aliceVision/multiview/relativePose/RotationSphericalKernel.hpp new file mode 100644 index 0000000000..c233f05262 --- /dev/null +++ b/src/aliceVision/multiview/relativePose/RotationSphericalKernel.hpp @@ -0,0 +1,147 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace multiview { +namespace relativePose { + +class RotationSphericalKernel +{ +public: + using ModelT = robustEstimation::Mat3Model; + +public: + RotationSphericalKernel(const camera::IntrinsicBase & camera1, + const camera::IntrinsicBase & camera2, + const std::vector & observations1, + const std::vector & observations2) + : _camera1(camera1), + _camera2(camera2) + { + for (const auto & pt : observations1) + { + _liftedObservations1.push_back(camera1.toUnitSphere(camera1.removeDistortion(camera1.ima2cam(pt)))); + } + + for (const auto & pt : observations2) + { + _liftedObservations2.push_back(camera2.toUnitSphere(camera2.removeDistortion(camera2.ima2cam(pt)))); + } + } + + /** + * @brief Return the minimum number of required samples for the solver + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamples() const + { + return 3; + } + + /** + * @brief Return the minimum number of required samples for the solver Ls + * @return minimum number of required samples + */ + std::size_t getMinimumNbRequiredSamplesLS() const + { + return 3; + } + + /** + * @brief Return the maximum number of models for the solver + * @return maximum number of models + */ + std::size_t getMaximumNbModels() const + { + return 1; + } + + /** + * @brief The number of elements in the data. + * @return the number of elements in the data. + */ + std::size_t nbSamples() const + { + return _liftedObservations1.size(); + } + + /** + * @brief Get logalpha0, Alpha0 is used to make the error adaptive to the image size + * @return logalpha0 + */ + double logalpha0() const + { + double aerr = _camera2.pixelProbability() * 2.0; + return log10(aerr); + } + + double errorVectorDimension() const + { + return 1.0; + } + + /** + * @brief This function is called to estimate the model from the minimum number + * of sample \p minSample (i.e. minimal problem solver). + * @param[in] samples A vector containing the indices of the data to be used for + * the minimal estimation. + * @param[out] models The model(s) estimated by the minimal solver. + */ + void fit(const std::vector& samples, std::vector& models) const + { + Mat x1(3, 3); + Mat x2(3, 3); + + for (int pos = 0; pos < 3; pos++) + { + size_t id = samples[pos]; + x1.col(pos) = _liftedObservations1[id]; + x2.col(pos) = _liftedObservations2[id]; + } + + _solver.solve(x1, x2, models); + } + + + /** + * @brief Function that computes the estimation error for a given model and all the elements. + * @param[in] model The model to consider. + * @param[out] vec_errors The vector containing all the estimation errors for every element. + */ + void errors(const robustEstimation::Mat3Model & model, std::vector& errors) const + { + Eigen::Matrix3d R = model.getMatrix(); + + for (int idx = 0; idx < _liftedObservations1.size(); idx++) + { + const Vec3 x1 = _liftedObservations1[idx]; + const Vec3 x2 = _liftedObservations2[idx]; + const Vec3 x = R * x1; + + errors[idx] = std::acos(x2.dot(x)); + } + } + +private: + const camera::IntrinsicBase & _camera1; + const camera::IntrinsicBase & _camera2; + + std::vector _liftedObservations1; + std::vector _liftedObservations2; + multiview::relativePose::Rotation3PSolver _solver; +}; + +} // namespace resection +} // namespace multiview +} // namespace aliceVision diff --git a/src/aliceVision/multiview/triangulation/triangulationDLT.cpp b/src/aliceVision/multiview/triangulation/triangulationDLT.cpp index 074bc553ca..02eb97dd98 100644 --- a/src/aliceVision/multiview/triangulation/triangulationDLT.cpp +++ b/src/aliceVision/multiview/triangulation/triangulationDLT.cpp @@ -37,26 +37,26 @@ void TriangulateDLT(const Mat34& P1, const Vec2& x1, const Mat34& P2, const Vec2 // Solve: // [cross(x0,P0) X = 0] // [cross(x1,P1) X = 0] -void TriangulateSphericalDLT(const Mat34& P1, const Vec3& x1, const Mat34& P2, const Vec3& x2, Vec4& X_homogeneous) +void TriangulateSphericalDLT(const Mat4& T1, const Vec3& x1, const Mat4& T2, const Vec3& x2, Vec4& X_homogeneous) { Mat design(6, 4); for (int i = 0; i < 4; ++i) { - design(0, i) = -x1[2] * P1(1, i) + x1[1] * P1(2, i); - design(1, i) = x1[2] * P1(0, i) - x1[0] * P1(2, i); - design(2, i) = -x1[1] * P1(0, i) + x1[0] * P1(1, i); + design(0, i) = -x1[2] * T1(1, i) + x1[1] * T1(2, i); + design(1, i) = x1[2] * T1(0, i) - x1[0] * T1(2, i); + design(2, i) = -x1[1] * T1(0, i) + x1[0] * T1(1, i); - design(3, i) = -x2[2] * P2(1, i) + x2[1] * P2(2, i); - design(4, i) = x2[2] * P2(0, i) - x2[0] * P2(2, i); - design(5, i) = -x2[1] * P2(0, i) + x2[0] * P2(1, i); + design(3, i) = -x2[2] * T2(1, i) + x2[1] * T2(2, i); + design(4, i) = x2[2] * T2(0, i) - x2[0] * T2(2, i); + design(5, i) = -x2[1] * T2(0, i) + x2[0] * T2(1, i); } Nullspace(design, X_homogeneous); } -void TriangulateSphericalDLT(const Mat34& P1, const Vec3& x1, const Mat34& P2, const Vec3& x2, Vec3& X_euclidean) +void TriangulateSphericalDLT(const Mat4 & T1, const Vec3& x1, const Mat4& T2, const Vec3& x2, Vec3& X_euclidean) { Vec4 X_homogeneous; - TriangulateSphericalDLT(P1, x1, P2, x2, X_homogeneous); + TriangulateSphericalDLT(T1, x1, T2, x2, X_homogeneous); homogeneousToEuclidean(X_homogeneous, X_euclidean); } diff --git a/src/aliceVision/multiview/triangulation/triangulationDLT.hpp b/src/aliceVision/multiview/triangulation/triangulationDLT.hpp index 0de2d9faf1..af232c1164 100644 --- a/src/aliceVision/multiview/triangulation/triangulationDLT.hpp +++ b/src/aliceVision/multiview/triangulation/triangulationDLT.hpp @@ -38,24 +38,24 @@ void TriangulateDLT(const Mat34& P1, const Vec2& x1, const Mat34& P2, const Vec2 /** * Triangulate a point given a set of bearing vectors * and associated projection matrices (in meters) - * @param P1 a projection matrix (R | t) + * @param T1 a transformation matrix (R | t) * @param x1 a unit bearing vector - * @param P2 a projection matrix K (R | t) + * @param T2 a transformation matrix (R | t) * @param x2 a unit bearing vector * @param X_homogeneous a homogeneous 3d point */ -void TriangulateSphericalDLT(const Mat34& P1, const Vec3& x1, const Mat34& P2, const Vec3& x2, Vec4& X_homogeneous); +void TriangulateSphericalDLT(const Mat4& T1, const Vec3& x1, const Mat4& T2, const Vec3& x2, Vec4& X_homogeneous); /** * Triangulate a point given a set of bearing vectors * and associated projection matrices (in meters) - * @param P1 a projection matrix (R | t) + * @param T1 a projection matrix (R | t) * @param x1 a unit bearing vector - * @param P2 a projection matrix K (R | t) + * @param T2 a projection matrix K (R | t) * @param x2 a unit bearing vector * @param X_homogeneous a 3d point */ -void TriangulateSphericalDLT(const Mat34& P1, const Vec3& x1, const Mat34& P2, const Vec3& x2, Vec3& X_euclidean); +void TriangulateSphericalDLT(const Mat4& T1, const Vec3& x1, const Mat4& T2, const Vec3& x2, Vec3& X_euclidean); } // namespace multiview } // namespace aliceVision diff --git a/src/aliceVision/track/CMakeLists.txt b/src/aliceVision/track/CMakeLists.txt index e83445eed5..e7c242fabd 100644 --- a/src/aliceVision/track/CMakeLists.txt +++ b/src/aliceVision/track/CMakeLists.txt @@ -10,7 +10,7 @@ set(tracks_files_headers # Sources set(tracks_files_sources TracksBuilder.cpp - TracksHandler.hpp + TracksHandler.cpp tracksUtils.cpp trackIO.cpp ) diff --git a/src/software/pipeline/main_relativePoseEstimating.cpp b/src/software/pipeline/main_relativePoseEstimating.cpp index 29bcc5a5ba..665d5e0c85 100644 --- a/src/software/pipeline/main_relativePoseEstimating.cpp +++ b/src/software/pipeline/main_relativePoseEstimating.cpp @@ -19,17 +19,16 @@ #include #include #include +#include #include -#include -#include -#include -#include +#include #include -#include -#include +#include +#include +#include #include #include @@ -49,35 +48,29 @@ using namespace aliceVision; namespace po = boost::program_options; -bool getPoseStructure(Mat3& R, - Vec3& t, + +bool getPoseStructure(Mat4 & T, std::vector& structure, std::vector& newVecInliers, const Mat3& E, const std::vector& vecInliers, - const Mat3& K1, - const Mat3& K2, - const Mat& x1, - const Mat& x2) + const camera::IntrinsicBase & cam1, + const camera::IntrinsicBase & cam2, + const std::vector & x1, + const std::vector & x2) { // Find set of analytical solutions - std::vector Rs; - std::vector ts; - motionFromEssential(E, &Rs, &ts); - - Mat34 P1, P2; - Mat3 R1 = Mat3::Identity(); - Vec3 t1 = Vec3::Zero(); - P_from_KRt(K1, R1, t1, P1); + std::vector Ts; + motionFromEssential(E, Ts); size_t bestCoundValid = 0; - for (int it = 0; it < Rs.size(); it++) + for (int it = 0; it < Ts.size(); it++) { - const Mat3& R2 = Rs[it]; - const Vec3& t2 = ts[it]; + const Mat4 T1 = Eigen::Matrix4d::Identity(); + const Mat4 & T2 = Ts[it]; - P_from_KRt(K2, R2, t2, P2); + std::cout << T2.inverse() << std::endl; std::vector points; std::vector updatedInliers; @@ -85,29 +78,42 @@ bool getPoseStructure(Mat3& R, size_t countValid = 0; for (size_t k = 0; k < vecInliers.size(); ++k) { - const Vec2& pt1 = x1.col(vecInliers[k]); - const Vec2& pt2 = x2.col(vecInliers[k]); + const Vec2& pt1 = x1[vecInliers[k]]; + const Vec2& pt2 = x2[vecInliers[k]]; + + const Vec3 pt3d1 = cam1.toUnitSphere(cam1.removeDistortion(cam1.ima2cam(pt1))); + const Vec3 pt3d2 = cam2.toUnitSphere(cam2.removeDistortion(cam2.ima2cam(pt2))); Vec3 X; - multiview::TriangulateDLT(P1, pt1, P2, pt2, X); + multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); - // Test if point is front to the two cameras. - if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) + Vec2 ptValid1 = cam1.project(T1, X.homogeneous(), true); + Vec2 ptValid2 = cam2.project(T2, X.homogeneous(), true); + + Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); + Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); + + std::cout << "*" << dirX1.dot(pt3d1) << " "; + std::cout << dirX2.dot(pt3d2) << std::endl; + + if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) { - countValid++; + continue; } updatedInliers.push_back(vecInliers[k]); points.push_back(X); + countValid++; } + std::cout << countValid << std::endl; + if (countValid > bestCoundValid) { bestCoundValid = countValid; structure = points; newVecInliers = updatedInliers; - R = Rs[it]; - t = ts[it]; + T = Ts[it]; } } @@ -119,34 +125,26 @@ bool getPoseStructure(Mat3& R, return true; } + + bool robustEssential(Mat3& E, std::vector& vecInliers, const camera::IntrinsicBase & cam1, const camera::IntrinsicBase & cam2, - const Mat& x1, - const Mat& x2, + const std::vector& x1, + const std::vector& x2, std::mt19937& randomNumberGenerator, const size_t maxIterationCount, const size_t minInliers) { - // use the 5 point solver to estimate E - using SolverT = multiview::relativePose::Essential5PSolver; - - // define the kernel - using KernelT = - multiview::RelativePoseKernel_K; - - const camera::Pinhole & pinhole1 = dynamic_cast(cam1); - const camera::Pinhole & pinhole2 = dynamic_cast(cam2); - - KernelT kernel(x1, cam1.w(), cam1.h(), x2, cam2.w(), cam2.h(), pinhole1.K(), pinhole2.K()); + multiview::relativePose::RelativeSphericalKernel kernel(cam1, cam2, x1, x2); robustEstimation::Mat3Model model; vecInliers.clear(); // robustly estimation of the Essential matrix and its precision const std::pair acRansacOut = - robustEstimation::ACRANSAC(kernel, randomNumberGenerator, vecInliers, maxIterationCount, &model); + robustEstimation::NACRANSAC(kernel, randomNumberGenerator, vecInliers, maxIterationCount, &model); if (vecInliers.size() < minInliers) { @@ -158,24 +156,25 @@ bool robustEssential(Mat3& E, return true; } + bool robustRotation(Mat3& R, std::vector& vecInliers, - const Mat& x1, - const Mat& x2, - std::mt19937& randomNumberGenerator, - const size_t maxIterationCount, - const size_t minInliers) + const camera::IntrinsicBase & cam1, + const camera::IntrinsicBase & cam2, + const std::vector& x1, + const std::vector& x2, + std::mt19937& randomNumberGenerator, + const size_t maxIterationCount, + const size_t minInliers) { - using KernelType = multiview:: - RelativePoseSphericalKernel; - - KernelType kernel(x1, x2); + multiview::relativePose::RotationSphericalKernel kernel(cam1, cam2, x1, x2); robustEstimation::Mat3Model model; vecInliers.clear(); // robustly estimation of the Essential matrix and its precision - robustEstimation::ACRANSAC(kernel, randomNumberGenerator, vecInliers, 1024, &model, std::numeric_limits::infinity()); + const std::pair acRansacOut = + robustEstimation::NACRANSAC(kernel, randomNumberGenerator, vecInliers, maxIterationCount, &model); if (vecInliers.size() < minInliers) { @@ -332,30 +331,16 @@ int aliceVision_main(int argc, char** argv) // Load tracks ALICEVISION_LOG_INFO("Load tracks"); - std::ifstream tracksFile(tracksFilename); - if (tracksFile.is_open() == false) + track::TracksHandler tracksHandler; + if (!tracksHandler.load(tracksFilename, sfmData.getViewsKeys())) { ALICEVISION_LOG_ERROR("The input tracks file '" + tracksFilename + "' cannot be read."); return EXIT_FAILURE; } - std::stringstream buffer; - buffer << tracksFile.rdbuf(); - boost::json::value jv = boost::json::parse(buffer.str()); - track::TracksMap mapTracks(track::flat_map_value_to(jv)); - - // Compute tracks per view - ALICEVISION_LOG_INFO("Estimate tracks per view"); - track::TracksPerView mapTracksPerView; - for (const auto& viewIt : sfmData.getViews()) - { - // create an entry in the map - mapTracksPerView[viewIt.first]; - } - track::computeTracksPerView(mapTracks, mapTracksPerView); - + ALICEVISION_LOG_INFO("Compute co-visibility"); std::map covisibility; - computeCovisibility(covisibility, mapTracks); + computeCovisibility(covisibility, tracksHandler.getAllTracks()); ALICEVISION_LOG_INFO("Process co-visibility"); std::stringstream ss; @@ -369,7 +354,7 @@ int aliceVision_main(int argc, char** argv) int chunkEnd = int(double(rangeStart + rangeSize) * ratioChunk); // For each covisible pair -#pragma omp parallel for +//#pragma omp parallel for for (int posPairs = chunkStart; posPairs < chunkEnd; posPairs++) { auto iterPairs = covisibility.begin(); @@ -384,25 +369,20 @@ int aliceVision_main(int argc, char** argv) std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); - std::shared_ptr refPinhole = std::dynamic_pointer_cast(refIntrinsics); - std::shared_ptr nextPinhole = std::dynamic_pointer_cast(nextIntrinsics); + aliceVision::track::TracksMap mapTracksCommon; - track::getCommonTracksInImagesFast({refImage, nextImage}, mapTracks, mapTracksPerView, mapTracksCommon); + track::getCommonTracksInImagesFast({refImage, nextImage}, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), mapTracksCommon); // Build features coordinates matrices const std::size_t n = mapTracksCommon.size(); - Mat refX(2, n); - Mat nextX(2, n); - IndexT pos = 0; + std::vector refpts, nextpts; + for (const auto& commonItem : mapTracksCommon) { - const track::Track& track = commonItem.second; - - refX.col(pos) = track.featPerView.at(refImage).coords; - nextX.col(pos) = track.featPerView.at(nextImage).coords; - - pos++; + const track::Track & track = commonItem.second; + refpts.push_back(track.featPerView.at(refImage).coords); + nextpts.push_back(track.featPerView.at(nextImage).coords); } std::vector vecInliers; @@ -410,19 +390,17 @@ int aliceVision_main(int argc, char** argv) if (enforcePureRotation) { - // Transform to vector - Mat refVecs(3, n); - Mat nextVecs(3, n); - for (int idx = 0; idx < n; idx++) - { - // Lift to unit sphere - refVecs.col(idx) = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refX.col(idx)))); - nextVecs.col(idx) = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextX.col(idx)))); - } - // Try to fit an essential matrix (we assume we are approx. calibrated) Mat3 R; - const bool relativeSuccess = robustRotation(R, vecInliers, refVecs, nextVecs, randomNumberGenerator, 1024, minInliers); + const bool relativeSuccess = robustRotation(R, + vecInliers, + *refIntrinsics, + *nextIntrinsics, + refpts, + nextpts, + randomNumberGenerator, + 1024, + minInliers); if (!relativeSuccess) { continue; @@ -442,8 +420,8 @@ int aliceVision_main(int argc, char** argv) inliers, *refIntrinsics, *nextIntrinsics, - refX, - nextX, + refpts, + nextpts, randomNumberGenerator, 1024, minInliers); @@ -452,28 +430,38 @@ int aliceVision_main(int argc, char** argv) continue; } + std::cout << inliers.size() << std::endl; + std::vector structure; reconstructed.reference = refImage; reconstructed.next = nextImage; - if (!getPoseStructure( - reconstructed.R, reconstructed.t, structure, vecInliers, E, inliers, refPinhole->K(), nextPinhole->K(), refX, nextX)) + Mat4 T; + if (!getPoseStructure(T, structure, vecInliers, E, inliers, *refIntrinsics, *nextIntrinsics, refpts, nextpts)) { continue; } + + std::cout << vecInliers.size() << std::endl; + + reconstructed.R = T.block<3, 3>(0, 0); + reconstructed.t = T.block<3, 1>(0, 3); } - std::vector refpts, nextpts; + std::cout << vecInliers.size() << std::endl; + + // Extract inliers + std::vector refPtsValid, nextPtsValid; for (auto id : vecInliers) { - refpts.push_back(refX.col(id)); - nextpts.push_back(nextX.col(id)); + refPtsValid.push_back(refpts[id]); + nextPtsValid.push_back(nextpts[id]); } // Compute matched points coverage of image double areaRef = refIntrinsics->w() * refIntrinsics->h(); double areaNext = nextIntrinsics->w() * nextIntrinsics->h(); - double areaScore = computeAreaScore(refpts, nextpts, areaRef, areaNext); + double areaScore = computeAreaScore(refPtsValid, nextPtsValid, areaRef, areaNext); // Compute ratio of matched points double iunion = n; diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp index dd71f11e3a..e2101e8786 100644 --- a/src/software/pipeline/main_sfmBootstraping.cpp +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -24,6 +24,7 @@ #include #include +#include #include @@ -52,28 +53,17 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::Reconstructe std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); - std::shared_ptr refPinhole = std::dynamic_pointer_cast(refIntrinsics); - std::shared_ptr nextPinhole = std::dynamic_pointer_cast(nextIntrinsics); - - if (refPinhole==nullptr || nextPinhole == nullptr) - { - return false; - } aliceVision::track::TracksMap mapTracksCommon; track::getCommonTracksInImagesFast({pair.reference, pair.next}, tracksMap, tracksPerView, mapTracksCommon); - const Eigen::Matrix3d Kref = refPinhole->K(); - const Eigen::Matrix3d Knext = nextPinhole->K(); - - Eigen::Matrix3d F = Knext.inverse().transpose() * CrossProductMatrix(pair.t) * pair.R * Kref.inverse(); - - Eigen::Vector3d c = - pair.R.transpose() * pair.t; - - Mat34 P1, P2; - P_from_KRt(Kref, Mat3::Identity(), Vec3::Zero(), P1); - P_from_KRt(Knext, pair.R, pair.t, P2); + const Mat4 T1 = Eigen::Matrix4d::Identity(); + Mat4 T2 = Eigen::Matrix4d::Identity(); + T2.block<3, 3>(0, 0) = pair.R; + T2.block<3, 1>(0, 3) = pair.t; + const Eigen::Vector3d c = - pair.R.transpose() * pair.t; + size_t count = 0; std::vector angles; for(const auto& commonItem : mapTracksCommon) @@ -82,26 +72,21 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::Reconstructe const IndexT refFeatureId = track.featPerView.at(pair.reference).featureId; const IndexT nextfeatureId = track.featPerView.at(pair.next).featureId; + const Vec2 refpt = track.featPerView.at(pair.reference).coords; const Vec2 nextpt = track.featPerView.at(pair.next).coords; + + const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); + const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); - const Vec2 refptu = refIntrinsics->getUndistortedPixel(refpt); - const Vec2 nextptu = nextIntrinsics->getUndistortedPixel(nextpt); - - Vec3 line = F * refptu.homogeneous(); - - //Make sure line normal is normalized - line = line * (1.0 / line.head<2>().norm()); - double distance = nextptu.homogeneous().dot(line); - if (distance > maxDistance) - { - continue; - } - + Vec3 X; - multiview::TriangulateDLT(P1, refptu, P2, nextptu, X); + multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); - if (X(2) < 0.0) + //Make sure + Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); + Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); + if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) { continue; } @@ -115,10 +100,17 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::Reconstructe usedTracks.push_back(commonItem.first); } + if (angles.size() == 0) + { + resultAngle = 0.0; + return false; + } + + std::cout << angles.size() << std::endl; + const unsigned medianIndex = angles.size() / 2; std::nth_element(angles.begin(), angles.begin() + medianIndex, angles.end()); resultAngle = angles[medianIndex]; - return true; } @@ -130,20 +122,7 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); - std::shared_ptr refPinhole = std::dynamic_pointer_cast(refIntrinsics); - std::shared_ptr nextPinhole = std::dynamic_pointer_cast(nextIntrinsics); - - if (refPinhole==nullptr || nextPinhole == nullptr) - { - return false; - } - const Eigen::Matrix3d Kref = refPinhole->K(); - const Eigen::Matrix3d Knext = nextPinhole->K(); - - Mat34 P1, P2; - P_from_KRt(Kref, Mat3::Identity(), Vec3::Zero(), P1); - P_from_KRt(Knext, pair.R, pair.t, P2); geometry::Pose3 poseNext; poseNext.setRotation(pair.R); @@ -151,6 +130,11 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai sfmData::CameraPose cposeNext(poseNext, false); sfmData.getPoses()[refView.getPoseId()] = sfmData::CameraPose(); sfmData.getPoses()[nextView.getPoseId()] = cposeNext; + + const Mat4 T1 = Eigen::Matrix4d::Identity(); + Mat4 T2 = Eigen::Matrix4d::Identity(); + T2.block<3, 3>(0, 0) = pair.R; + T2.block<3, 1>(0, 3) = pair.t; size_t count = 0; std::vector angles; @@ -161,16 +145,19 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai const track::TrackItem & refItem = track.featPerView.at(pair.reference); const track::TrackItem & nextItem = track.featPerView.at(pair.next); - const Vec2 refpt = refItem.coords; - const Vec2 nextpt = nextItem.coords; + const Vec2 refpt = track.featPerView.at(pair.reference).coords; + const Vec2 nextpt = track.featPerView.at(pair.next).coords; + + const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); + const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); - const Vec2 refptu = refIntrinsics->getUndistortedPixel(refpt); - const Vec2 nextptu = nextIntrinsics->getUndistortedPixel(nextpt); Vec3 X; - multiview::TriangulateDLT(P1, refptu, P2, nextptu, X); + multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); - if (X(2) < 0.0) + Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); + Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); + if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) { continue; } @@ -298,29 +285,14 @@ int aliceVision_main(int argc, char** argv) return EXIT_SUCCESS; } - // Load tracks ALICEVISION_LOG_INFO("Load tracks"); - std::ifstream tracksFile(tracksFilename); - if(tracksFile.is_open() == false) + track::TracksHandler tracksHandler; + if (!tracksHandler.load(tracksFilename, sfmData.getViewsKeys())) { ALICEVISION_LOG_ERROR("The input tracks file '" + tracksFilename + "' cannot be read."); return EXIT_FAILURE; } - std::stringstream buffer; - buffer << tracksFile.rdbuf(); - boost::json::value jv = boost::json::parse(buffer.str()); - track::TracksMap mapTracks(track::flat_map_value_to(jv)); - - // Compute tracks per view - ALICEVISION_LOG_INFO("Estimate tracks per view"); - track::TracksPerView mapTracksPerView; - for(const auto& viewIt : sfmData.getViews()) - { - // create an entry in the map - mapTracksPerView[viewIt.first]; - } - track::computeTracksPerView(mapTracks, mapTracksPerView); //Result of pair estimations are stored in multiple files @@ -357,7 +329,9 @@ int aliceVision_main(int argc, char** argv) { std::vector usedTracks; double angle = 0.0; - if (!estimatePairAngle(sfmData, pair, mapTracks, mapTracksPerView, maxEpipolarDistance, angle, usedTracks)) + + std::cout << "toto" << std::endl; + if (!estimatePairAngle(sfmData, pair, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), maxEpipolarDistance, angle, usedTracks)) { continue; } @@ -373,10 +347,10 @@ int aliceVision_main(int argc, char** argv) int maxref = std::max(vref.getImage().getWidth(), vref.getImage().getHeight()); int maxnext = std::max(vnext.getImage().getWidth(), vnext.getImage().getHeight()); - double refScore = computeScore(mapTracks, usedTracks, pair.reference, maxref, 5); - double nextScore = computeScore(mapTracks, usedTracks, pair.next, maxnext, 5); + double refScore = computeScore(tracksHandler.getAllTracks(), usedTracks, pair.reference, maxref, 5); + double nextScore = computeScore(tracksHandler.getAllTracks(), usedTracks, pair.next, maxnext, 5); - double score = std::min(refScore, nextScore) * radianToDegree(angle); + double score = std::min(refScore, nextScore) * std::min(10.0, radianToDegree(angle)); if (score > bestScore) { @@ -386,7 +360,7 @@ int aliceVision_main(int argc, char** argv) } } - if (!buildSfmData(sfmData, bestPair, mapTracks, bestUsedTracks)) + if (!buildSfmData(sfmData, bestPair, tracksHandler.getAllTracks(), bestUsedTracks)) { return EXIT_FAILURE; } From 511abbe47051fd775d3de6f3136f36296cbd4120 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Fri, 3 May 2024 17:21:00 +0200 Subject: [PATCH 05/14] New logic for sfm --- src/aliceVision/sfm/CMakeLists.txt | 11 + .../sfm/pipeline/expanding/ExpansionChunk.cpp | 144 +++++++++++++ .../sfm/pipeline/expanding/ExpansionChunk.hpp | 85 ++++++++ .../pipeline/expanding/ExpansionHistory.cpp | 139 ++++++++++++ .../pipeline/expanding/ExpansionHistory.hpp | 67 ++++++ .../pipeline/expanding/ExpansionIteration.cpp | 56 +++++ .../pipeline/expanding/ExpansionIteration.hpp | 79 +++++++ .../pipeline/expanding/ExpansionPolicy.hpp | 45 ++++ .../expanding/ExpansionPolicyLegacy.cpp | 199 ++++++++++++++++++ .../expanding/ExpansionPolicyLegacy.hpp | 81 +++++++ .../pipeline/expanding/ExpansionProcess.cpp | 138 ++++++++++++ .../pipeline/expanding/ExpansionProcess.hpp | 88 ++++++++ .../sfm/pipeline/expanding/SfmBundle.hpp | 3 + .../pipeline/expanding/SfmTriangulation.cpp | 10 +- src/aliceVision/sfmData/SfMData.hpp | 21 ++ src/software/pipeline/CMakeLists.txt | 15 ++ .../pipeline/main_relativePoseEstimating.cpp | 5 - src/software/pipeline/main_sfmExpanding.cpp | 112 ++++++++++ 18 files changed, 1290 insertions(+), 8 deletions(-) create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp create mode 100644 src/software/pipeline/main_sfmExpanding.cpp diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index c728a10f80..6c54022257 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -29,6 +29,12 @@ set(sfm_files_headers pipeline/expanding/SfmTriangulation.hpp pipeline/expanding/SfmResection.hpp pipeline/expanding/SfmBundle.hpp + pipeline/expanding/ExpansionHistory.hpp + pipeline/expanding/ExpansionChunk.hpp + pipeline/expanding/ExpansionIteration.hpp + pipeline/expanding/ExpansionPolicy.hpp + pipeline/expanding/ExpansionPolicyLegacy.hpp + pipeline/expanding/ExpansionProcess.hpp pipeline/regionsIO.hpp utils/alignment.hpp utils/statistics.hpp @@ -59,6 +65,11 @@ set(sfm_files_sources pipeline/expanding/SfmTriangulation.cpp pipeline/expanding/SfmResection.cpp pipeline/expanding/SfmBundle.cpp + pipeline/expanding/ExpansionHistory.cpp + pipeline/expanding/ExpansionChunk.cpp + pipeline/expanding/ExpansionIteration.cpp + pipeline/expanding/ExpansionPolicyLegacy.cpp + pipeline/expanding/ExpansionProcess.cpp pipeline/regionsIO.cpp utils/alignment.cpp utils/statistics.cpp diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp new file mode 100644 index 0000000000..07db683565 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp @@ -0,0 +1,144 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 "ExpansionChunk.hpp" + +#include +#include + +namespace aliceVision { +namespace sfm { + + +bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewsChunk) +{ + //For all views which have been required + //Compute the pose given the existing point cloud + if (!_bundleHandler) + { + return false; + } + + //#pragma omp parallel for + for (int i = 0; i < viewsChunk.size(); i++) + { + auto it = viewsChunk.begin(); + std::advance(it, i); + IndexT viewId = *it; + + if (!sfmData.isPoseAndIntrinsicDefined(viewId)) + { + SfmResection resection(_resectionIterations, std::numeric_limits::infinity()); + + Eigen::Matrix4d pose; + double threshold = 0.0; + std::mt19937 randomNumberGenerator; + if (!resection.processView(sfmData, + tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), + randomNumberGenerator, viewId, + pose, threshold)) + { + continue; + } + + #pragma omp critical + { + addPose(sfmData, viewId, pose); + } + } + } + + // Get a list of valid views + // We recompute this list because some wanted views + // may have been resectioned before and we still want to triangulate again + std::set validViewIds; + for (IndexT viewId : viewsChunk) + { + if (sfmData.isPoseAndIntrinsicDefined(viewId)) + { + validViewIds.insert(viewId); + } + } + + //Now that all views of the chunks + if (!triangulate(sfmData, tracksHandler, validViewIds)) + { + return false; + } + + if (!_bundleHandler->process(sfmData, tracksHandler, validViewIds)) + { + return false; + } + + if (_historyHandler) + { + _historyHandler->saveState(sfmData); + } + + return true; +} + +bool ExpansionChunk::triangulate(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) +{ + SfmTriangulation triangulation(_triangulationMinPoints, _maxTriangulationError); + + std::set evaluatedTracks; + std::map outputLandmarks; + + std::mt19937 randomNumberGenerator; + if (!triangulation.process(sfmData, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), + randomNumberGenerator, viewIds, + evaluatedTracks, outputLandmarks)) + { + return false; + } + + auto & landmarks = sfmData.getLandmarks(); + + for (const auto & pl : outputLandmarks) + { + const auto & landmark = pl.second; + + if (landmarks.find(pl.first) != landmarks.end()) + { + landmarks.erase(pl.first); + } + + if (landmark.getObservations().size() < _triangulationMinPoints) + { + continue; + } + + if (!SfmTriangulation::checkChierality(sfmData, landmark)) + { + continue; + } + + double maxAngle = SfmTriangulation::getMaximalAngle(sfmData, landmark); + if (maxAngle < _minTriangulationAngleDegrees) + { + continue; + } + + landmarks.insert(pl); + } + + return true; +} + +void ExpansionChunk::addPose(sfmData::SfMData & sfmData, IndexT viewId, const Eigen::Matrix4d & pose) +{ + const sfmData::View & v = sfmData.getView(viewId); + + sfmData::CameraPose cpose(geometry::Pose3(pose), false); + + sfmData.setPose(v, cpose); +} + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp new file mode 100644 index 0000000000..59adf931f5 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp @@ -0,0 +1,85 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class ExpansionChunk +{ +public: + using uptr = std::unique_ptr; + +public: + + /** + * @brief Compute a chunk of views assuming the sfmData already has an initial set of + * reconstructed cameras and 3D points to connect to. + * @param sfmData the sfmData which describes the current sfm state + * @param tracksHandler the scene tracks handler + * @param viewsChunks a list of view ids to process in this chunk + */ + bool process(sfmData::SfMData & sfmData, + const track::TracksHandler & tracksHandler, + const std::set & viewsChunk); + + /** + * brief setup the bundle handler + * @param bundleHandler a unique ptr. the Ownership will be taken + */ + void setBundleHandler(SfmBundle::uptr & bundleHandler) + { + _bundleHandler = std::move(bundleHandler); + } + + /** + * brief setup the expansion history handler + * @param expansionHistory a shared ptr + */ + void setExpansionHistoryHandler(ExpansionHistory::sptr & expansionHistory) + { + _historyHandler = expansionHistory; + } + +private: + + /** + * @Brief assign the computed pose to the view + * @param sfmData the sfmData to update + * @param viewId the viewId of interest + * @param pose the homogeneous matrix computed from the resection + */ + void addPose(sfmData::SfMData & sfmData, IndexT viewId, const Eigen::Matrix4d & pose); + + /** + * @brief Try to upgrade sfm with new landmarks + * @param sfmData the object to update + * @param tracks all tracks of the scene as a map {trackId, track} + * @param viewIds the set of views to triangulate + */ + bool triangulate(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); + +private: + SfmBundle::uptr _bundleHandler; + ExpansionHistory::sptr _historyHandler; + +private: + size_t _resectionIterations = 1024; + size_t _triangulationMinPoints = 2; + double _minTriangulationAngleDegrees = 3.0; + double _maxTriangulationError = 8.0; +}; + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp new file mode 100644 index 0000000000..fd02d9b3ca --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp @@ -0,0 +1,139 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 "ExpansionHistory.hpp" + + +#include + +namespace aliceVision { +namespace sfm { + +ExpansionHistory::ExpansionHistory() +{ +} + +bool ExpansionHistory::initialize(const sfmData::SfMData & sfmData) +{ + // Update epoch id + // We want to have a starting epoch larger than the one found in the existing views + _epoch = 0; + for (const auto & pv : sfmData.getViews()) + { + _epoch = std::max(_epoch, static_cast(pv.second->getResectionId())); + } + + _epoch++; + + return true; +} + +bool ExpansionHistory::beginEpoch(const sfmData::SfMData & sfmData) +{ + return true; +} + +void ExpansionHistory::endEpoch(sfmData::SfMData & sfmData, const std::set & selectedViews) +{ + // Get a list of valid views + std::set validViewIds; + for (IndexT viewId : selectedViews) + { + if (sfmData.isPoseAndIntrinsicDefined(viewId)) + { + validViewIds.insert(viewId); + } + } + + // Store epoch + for (IndexT viewId : validViewIds) + { + sfmData::View & v = sfmData.getView(viewId); + v.setResectionId(_epoch); + } + + // Make sure next call will use a different epoch + _epoch++; +} + + +void ExpansionHistory::saveState(const sfmData::SfMData & sfmData) +{ + for (const auto & pIntrinsic : sfmData.getIntrinsics()) + { + size_t usage = 0; + std::shared_ptr iso = std::dynamic_pointer_cast(pIntrinsic.second); + + for (const auto & pView : sfmData.getViews()) + { + IndexT viewIntrinsicId = pView.second->getIntrinsicId(); + if (!sfmData.isPoseAndIntrinsicDefined(pView.second.get())) + { + continue; + } + + if (pIntrinsic.first == viewIntrinsicId) + { + usage++; + } + } + + //Store usage counter + _focalHistory[pIntrinsic.first].push_back(std::make_pair(usage, iso->getScale().x())); + } + + for (auto & pfh : _focalHistory) + { + const auto & vec = pfh.second; + + size_t lastGood = std::numeric_limits::max(); + std::vector> filtered; + + for (int id = vec.size() - 1; id >= 0; id--) + { + //Make sure the usage decrease + if (vec[id].first < lastGood) + { + lastGood = vec[id].first; + filtered.push_back(vec[id]); + } + } + + std::vector cropped; + std::vector focals; + int largestCount = filtered.front().first; + bool nomore = false; + for (int id = 0; id < filtered.size(); id++) + { + if (!nomore) + { + cropped.push_back(filtered[id].second); + } + + if (largestCount - filtered[id].first > 25) + { + nomore = true; + } + + focals.push_back(filtered[id].second); + } + + + /*const double mean = std::accumulate(cropped.begin(), cropped.end(), 0.0) / cropped.size(); + std::vector diff(cropped.size()); + std::transform(cropped.begin(), cropped.end(), diff.begin(), [mean](double x) { return x - mean; }); + const double sqSum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); + double stddev = std::sqrt(sqSum / cropped.size()); + + double minVal = *std::min_element(focals.begin(), focals.end()); + double maxVal = *std::max_element(focals.begin(), focals.end()); + double normStdev = stddev / (maxVal - minVal);*/ + } +} + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp new file mode 100644 index 0000000000..56681c636f --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp @@ -0,0 +1,67 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class ExpansionHistory +{ +public: + using sptr = std::shared_ptr; + +public: + ExpansionHistory(); + + /** + * @brief initialize object + * @param sfmData scene object + * @return true if succeeded + */ + bool initialize(const sfmData::SfMData & sfmData); + + /** + * @brief Initialize an iteration + * @param sfmData the scene object + * @return true if succeeded + */ + bool beginEpoch(const sfmData::SfMData & sfmData); + + + /** + * @brief Terminate an iteration + * @param sfmData the scene object + * @param selectedViews the list of selected views for the current chunk + */ + void endEpoch(sfmData::SfMData & sfmData, const std::set & selectedViews); + + /** + * @brief get the iteration id + * @return iteration id + */ + const size_t getEpoch() const + { + return _epoch; + } + + void saveState(const sfmData::SfMData & sfmData); + +private: + // History of focals per intrinsics + std::map>> _focalHistory; + + // epoch ID + std::size_t _epoch = 0; +}; + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp new file mode 100644 index 0000000000..d0370fcace --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp @@ -0,0 +1,56 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 "ExpansionIteration.hpp" +#include + +namespace aliceVision { +namespace sfm { + + +bool ExpansionIteration::process(sfmData::SfMData & sfmData, track::TracksHandler & tracksHandler) +{ + if (!_chunkHandler) + { + return false; + } + + if (!_policy) + { + return false; + } + + if (!_policy->initialize(sfmData)) + { + return false; + } + + while (1) + { + if (!_historyHandler->beginEpoch(sfmData)) + { + break; + } + + if (!_policy->process(sfmData, tracksHandler)) + { + break; + } + + if (!_chunkHandler->process(sfmData, tracksHandler, _policy->getNextViews())) + { + continue; + } + + _historyHandler->endEpoch(sfmData, _policy->getNextViews()); + } + + return true; +} + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.hpp new file mode 100644 index 0000000000..e8d615e3c8 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.hpp @@ -0,0 +1,79 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class ExpansionIteration +{ +public: + using uptr = std::unique_ptr; + +public: + + /** + * @brief process an iteration of the sfm + * An iteration is a set of successive chunks selected dynamically. + * Multiple iteration may be necessary to complete. + * @param sfmData the scene to process + * @param tracksHandler the tracks for this scene + * @return true if the iteration succeeded + */ + bool process(sfmData::SfMData & sfmData, track::TracksHandler & tracksHandler); + + /** + * @return a pointer to the chunk handler + */ + ExpansionChunk * getChunkHandler() const + { + return _chunkHandler.get(); + } + + /** + * brief setup the expansion history handler + * @param expansionHistory a shared ptr + */ + void setExpansionHistoryHandler(ExpansionHistory::sptr & expansionHistory) + { + _historyHandler = expansionHistory; + } + + /** + * brief setup the expansion history handler + * @param expansionPolicy a unique ptr. Ownership will be taken + */ + void setExpansionPolicyHandler(ExpansionPolicy::uptr & expansionPolicy) + { + _policy = std::move(expansionPolicy); + } + + /** + * brief setup the expansion chunk handler + * @param expansionChunk a unique ptr. Ownership will be taken + */ + void setExpansionChunkHandler(ExpansionChunk::uptr & expansionChunk) + { + _chunkHandler = std::move(expansionChunk); + } + +private: + std::unique_ptr _chunkHandler; + std::shared_ptr _historyHandler; + std::unique_ptr _policy; +}; + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp new file mode 100644 index 0000000000..27f68bec08 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp @@ -0,0 +1,45 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class ExpansionPolicy +{ +public: + using uptr = std::unique_ptr; +public: + + /** + * @brief Initialize policy for an iteration + * @param sfmData the scene to process + * @return true if the init succeeded + */ + virtual bool initialize(const sfmData::SfMData & sfmData) = 0; + + /** + * @brief compute policy for an iteration + * @param sfmData the scene to process + * @param tracksHandler the tracks for this scene + * @return true if the policy succeeded + */ + virtual bool process(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler) = 0; + + /** + * @brief Retrieve the selected next views + * @return a set of views to process + */ + virtual std::set getNextViews() = 0; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp new file mode 100644 index 0000000000..32219750df --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp @@ -0,0 +1,199 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 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 + +namespace aliceVision { +namespace sfm { + +bool ExpansionPolicyLegacy::initialize(const sfmData::SfMData & sfmData) +{ + _availableViewsIds.clear(); + + std::set allViewsIds = sfmData.getViewsKeys(); + std::set allReconstructedViewsIds = sfmData.getValidViews(); + + //Initial list is the set of non localized views in the sfmData + std::set_difference(allViewsIds.begin(), allViewsIds.end(), + allReconstructedViewsIds.begin(), allReconstructedViewsIds.end(), + std::inserter(_availableViewsIds, _availableViewsIds.begin())); + + return true; +} + +bool ExpansionPolicyLegacy::process(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler) +{ + _selectedViews.clear(); + + if (_availableViewsIds.size() == 0) + { + return false; + } + + // Collect tracksIds + std::set reconstructedTracksIds; + std::transform(sfmData.getLandmarks().begin(), + sfmData.getLandmarks().end(), + std::inserter(reconstructedTracksIds, reconstructedTracksIds.begin()), + stl::RetrieveKey()); + + + struct ViewScoring + { + IndexT id; + size_t count; + double score; + }; + + std::vector vscoring; + + //Loop over possible views + for (IndexT cViewId : _availableViewsIds) + { + const sfmData::View & v = sfmData.getView(cViewId); + + //If this view has no intrinsic, + //Then we have now immediate way to do resection + //Actually, it should be an error given the pipeline + if (!sfmData.isIntrinsicDefined(v)) + { + continue; + } + + // Get all tracks which are visible in the view of interest + const aliceVision::track::TrackIdSet & tracksIds = tracksHandler.getTracksPerView().at(cViewId); + if (tracksIds.empty()) + { + continue; + } + + + // Intersection of tracks for the view and reconstructed tracks + // This will give the list of tracks for this view which are already reconstructed and have + // an associated landmark + std::vector viewReconstructedTracksIds; + std::set_intersection(tracksIds.begin(), + tracksIds.end(), + reconstructedTracksIds.begin(), + reconstructedTracksIds.end(), + std::back_inserter(viewReconstructedTracksIds)); + + + if (viewReconstructedTracksIds.empty()) + { + continue; + } + + int maxDim = std::max(v.getImage().getWidth(), v.getImage().getHeight()); + + //Compute a score for this view + ViewScoring scoring; + scoring.id = cViewId; + scoring.score = ExpansionPolicyLegacy::computeScore(tracksHandler.getAllTracks(), viewReconstructedTracksIds, cViewId, maxDim, _countPyramidLevels); + scoring.count = viewReconstructedTracksIds.size(); + vscoring.push_back(scoring); + } + + if (vscoring.size() == 0) + { + return false; + } + + //Sort views by decreasing scores + std::sort(vscoring.begin(), vscoring.end(), + [](const ViewScoring & v1, const ViewScoring & v2) + { + return v1.score > v2.score; + }); + + //Always add at least the best score, whatever it is + _selectedViews.insert(vscoring[0].id); + + int maxSetSize = _maxImagesPerGroup; + if (sfmData.getValidViews().size() < _nbFirstUnstableCameras) + { + maxSetSize = 1; + } + + //Add all views which have valid characteristics + for (int i = 1; i < vscoring.size() && _selectedViews.size() < maxSetSize; i++) + { + if (vscoring[i].count < _minPointsThreshold) + { + continue; + } + + _selectedViews.insert(vscoring[i].id); + } + + for (IndexT id : _selectedViews) + { + _availableViewsIds.erase(id); + } + + return true; +} + +std::set ExpansionPolicyLegacy::getNextViews() +{ + return _selectedViews; +} + +double ExpansionPolicyLegacy::computeScore(const track::TracksMap & tracksMap, + const std::vector & usedTracks, + const IndexT viewId, + const size_t maxSize, + const size_t countLevels) +{ + //Compute min and max level such that max(width, height) <= 2 on maxLevel + int maxLevel = std::ceil(std::log2(maxSize)); + int minLevel = std::max(1, maxLevel - int(countLevels)); + int realCountLevels = maxLevel - minLevel + 1; + + std::vector>> uniques(realCountLevels); + + + //Coordinates are considered as integers + //Power of two divide == right binary shift on integers + + for (auto trackId : usedTracks) + { + auto & track = tracksMap.at(trackId); + const Vec2 pt = track.featPerView.at(viewId).coords; + + unsigned int ptx = (unsigned int)(pt.x()); + unsigned int pty = (unsigned int)(pt.y()); + + for (unsigned int shiftLevel = 0; shiftLevel < realCountLevels; shiftLevel++) + { + unsigned int level = minLevel + shiftLevel; + unsigned int lptx = ptx >> level; + unsigned int lpty = pty >> level; + + uniques[shiftLevel].insert(std::make_pair(lptx, lpty)); + } + } + + double sum = 0.0; + for (unsigned int shiftLevel = 0; shiftLevel < realCountLevels; shiftLevel++) + { + int size = uniques[shiftLevel].size(); + if (size <= 1) + { + continue; + } + + //The higher the level, the higher the weight per cell + double w = pow(2.0, shiftLevel); + sum += w * double(size); + } + + return sum; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp new file mode 100644 index 0000000000..6cbb17aa43 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp @@ -0,0 +1,81 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 sfm { + +class ExpansionPolicyLegacy : public ExpansionPolicy +{ +public: + + /** + * @brief Initialize policy for an iteration + * @param sfmData the scene to process + * @return true if the init succeeded + */ + virtual bool initialize(const sfmData::SfMData & sfmData); + + /** + * @brief compute policy for an iteration + * @param sfmData the scene to process + * @param tracksHandler the tracks for this scene + * @return true if the policy succeeded + */ + virtual bool process(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler); + + /** + * @brief Retrieve the selected next views + * @return a set of views to process + */ + virtual std::set getNextViews(); + + /** + * @brief Compute score of a view + * @param tracksMap the scene set of tracks + * @param usedTracks the list of tracks to consider + * @param viewId the view of interest + * @param maxSize the largest dimension of the view's image + * @param countLevels the number of levels we want to use (starting from a single pixel level) + * @return a score + */ + static double computeScore(const track::TracksMap & tracksMap, + const std::vector & usedTracks, + const IndexT viewId, + const size_t maxSize, + const size_t countLevels); + +private: + + // vector of selected views for this iteration + std::set _selectedViews; + + // List of available view indices + std::set _availableViewsIds; + +private: + // Minimal number of points viewed in a view + // AND reconstructed in the sfm + std::size_t _minPointsThreshold = 30; + + // Number of levels in the pyramid starting + // from the level with a grid size of 2x2 + // Level 0 = 2x2, Level 1 = 4x4, etc. + std::size_t _countPyramidLevels = 5; + + // Number of cameras in scene under which the set is considered as unstable + size_t _nbFirstUnstableCameras = 30; + + // Maximal number of images in a chunk + size_t _maxImagesPerGroup = 30; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp new file mode 100644 index 0000000000..ef2f616b1d --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp @@ -0,0 +1,138 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 "ExpansionProcess.hpp" + + +namespace aliceVision { +namespace sfm { + +bool ExpansionProcess::process(sfmData::SfMData & sfmData, track::TracksHandler & tracksHandler) +{ + if (!_iterationHandler) + { + return false; + } + + if (!_historyHandler) + { + return false; + } + + if (!_historyHandler->initialize(sfmData)) + { + return false; + } + + //Prepare existing data + prepareExisting(sfmData, tracksHandler); + + int nbPoses = 0; + do + { + nbPoses = sfmData.getPoses().size(); + + if (!_iterationHandler->process(sfmData, tracksHandler)) + { + return false; + } + } + while (sfmData.getPoses().size() != nbPoses); + + return true; +} + +bool ExpansionProcess::prepareExisting(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler) +{ + //Prepare existing data + remapExistingLandmarks(sfmData, tracksHandler); + + // If there are some poses existing + // We want to make sure everything is on par with requirements + if (!sfmData.getPoses().empty()) + { + if (_iterationHandler->getChunkHandler() == nullptr) + { + return false; + } + + // Process everything in existing sfmData + if (!_iterationHandler->getChunkHandler()->process(sfmData, tracksHandler, sfmData.getValidViews())) + { + return false; + } + } + + return true; +} + +void ExpansionProcess::remapExistingLandmarks(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler) +{ + // get unmap landmarks + sfmData::Landmarks landmarks; + + // clear sfmData structure and store them locally + std::swap(landmarks, sfmData.getLandmarks()); + + // builds landmarks temporary comparison structure + // ObsKey + // ObsToLandmark + using ObsKey = std::tuple; + using ObsToLandmark = std::map; + + ObsToLandmark obsToLandmark; + for (const auto& landmarkPair : landmarks) + { + const IndexT landmarkId = landmarkPair.first; + if (landmarkPair.second.getObservations().size() == 0) + { + continue; + } + + const IndexT firstViewId = landmarkPair.second.getObservations().begin()->first; + const IndexT firstFeatureId = landmarkPair.second.getObservations().begin()->second.getFeatureId(); + const feature::EImageDescriberType descType = landmarkPair.second.descType; + + obsToLandmark.emplace(ObsKey(firstViewId, firstFeatureId, descType), landmarkId); + } + + + // For each track + for (const auto & trackPair : tracksHandler.getAllTracks()) + { + const IndexT trackId = trackPair.first; + const track::Track& track = trackPair.second; + + //For each feature in track + for (const auto& featView : track.featPerView) + { + ObsKey key(featView.first, featView.second.featureId, track.descType); + + //We assume one feature is associated to only one track + const ObsToLandmark::const_iterator it = obsToLandmark.find(key); + + if (it == obsToLandmark.end()) + { + continue; + } + + auto landmarkPair = landmarks.find(it->second); + landmarks.erase(landmarkPair->first); + + // re-insert the landmark with the new id + sfmData.getLandmarks().emplace(trackId, landmarkPair->second); + } + } + + if (landmarks.size() > 0) + { + ALICEVISION_LOG_INFO("Not all existing landmarks have been remapped"); + } +} + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp new file mode 100644 index 0000000000..2a8cf76c43 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp @@ -0,0 +1,88 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class ExpansionProcess{ +public: + using uptr = std::unique_ptr; + +public: + + /** + * @brief Process a scene, potentially handling multiple groups + * @param sfmData the in/out scene to process + * @param tracksHandler the tracks for this scene + */ + bool process(sfmData::SfMData & sfmData, track::TracksHandler & tracksHandler); + + /** + * @brief Return iteration handler pointer + * @return a pointer to the unique_ptr iterationhandler + */ + ExpansionIteration * getIterationHandler() const + { + return _iterationHandler.get(); + } + + /** + * brief setup the expansion history handler + * @param expansionHistory a shared ptr + */ + void setExpansionHistoryHandler(ExpansionHistory::sptr & expansionHistory) + { + _historyHandler = expansionHistory; + } + /** + * brief setup the expansion iteration handler + * @param expansionIteration a unique ptr. Ownership will be taken + */ + void setExpansionIterationHandler(ExpansionIteration::uptr & expansionIteration) + { + _iterationHandler = std::move(expansionIteration); + } + +private: + + /** + * @brief Process sfmData if something exists inside (previous sfm) + * @param[in] sfmData the object to update + * @param[in] tracks the tracks for this scene + */ + bool prepareExisting(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler); + + /** + * @brief Remap the sfmData landmarks to id compatible with trackmap + * @param[in] sfmData the object to update + * @param[in] tracks the tracks for this scene + */ + void remapExistingLandmarks(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler); + + + +private: + //Must be declared first for initialization + std::shared_ptr _historyHandler; + + /** + * Handle iteration prcess + */ + std::unique_ptr _iterationHandler; +}; + +} // namespace sfm +} // namespace aliceVision + diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp index 8e36fba366..e94d7fadee 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp @@ -18,6 +18,9 @@ namespace sfm { class SfmBundle { +public: + using uptr = std::unique_ptr; + public: /** * @brief Process bundle diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp index 022498fd0e..1c76d74750 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp @@ -177,12 +177,16 @@ bool SfmTriangulation::checkChierality(const sfmData::SfMData & sfmData, const s for (const auto & pRefObs : landmark.getObservations()) { IndexT refViewId = pRefObs.first; - const sfmData::View & refView = sfmData.getView(refViewId); - const sfmData::CameraPose & refCameraPose = sfmData.getPoses().at(refView.getPoseId()); + const sfmData::Observation & obs = pRefObs.second; + const camera::IntrinsicBase * intrinsic = sfmData.getIntrinsicPtr(refView.getIntrinsicId()); + const sfmData::CameraPose refCameraPose = sfmData.getPose(refView); const geometry::Pose3 & refPose = refCameraPose.getTransform(); - if (refPose.depth(landmark.X) < 0.0) + const Vec3 dir = intrinsic->toUnitSphere(intrinsic->removeDistortion(intrinsic->ima2cam(obs.getCoordinates()))); + const Vec3 ldir = refPose(landmark.X).normalized(); + + if (dir.dot(ldir) < 0.0) { return false; } diff --git a/src/aliceVision/sfmData/SfMData.hpp b/src/aliceVision/sfmData/SfMData.hpp index 8aa5e6c6b1..9f70b6a486 100644 --- a/src/aliceVision/sfmData/SfMData.hpp +++ b/src/aliceVision/sfmData/SfMData.hpp @@ -262,6 +262,27 @@ class SfMData /** * @brief Check if the given view have defined intrinsic and pose * @param[in] view The given view + * @return true if intrinsic defined + */ + bool isIntrinsicDefined(const View & view) const + { + return (view.getIntrinsicId() != UndefinedIndexT + && _intrinsics.find(view.getIntrinsicId()) != _intrinsics.end()); + } + + /** + * @brief Check if the given view have defined intrinsic and pose + * @param[in] viewID The given viewID + * @return true if intrinsic and pose defined + */ + bool isIntrinsicDefined(IndexT viewId) const + { + return isIntrinsicDefined(*_views.at(viewId)); + } + + /** + * @brief Check if the given view have defined intrinsi + * @param[in] view The given view * @return true if intrinsic and pose defined */ bool isPoseAndIntrinsicDefined(const View* view) const diff --git a/src/software/pipeline/CMakeLists.txt b/src/software/pipeline/CMakeLists.txt index d25cb98774..ea06fe9944 100644 --- a/src/software/pipeline/CMakeLists.txt +++ b/src/software/pipeline/CMakeLists.txt @@ -133,6 +133,21 @@ if(ALICEVISION_BUILD_SFM) Boost::json ) + # Expanding SfM + alicevision_add_software(aliceVision_sfmExpanding + SOURCE main_sfmExpanding.cpp + FOLDER ${FOLDER_SOFTWARE_PIPELINE} + LINKS aliceVision_system + aliceVision_cmdline + aliceVision_feature + aliceVision_sfm + aliceVision_sfmData + aliceVision_track + aliceVision_dataio + Boost::program_options + Boost::json + ) + # Incremental / Sequential SfM alicevision_add_software(aliceVision_incrementalSfM SOURCE main_incrementalSfM.cpp diff --git a/src/software/pipeline/main_relativePoseEstimating.cpp b/src/software/pipeline/main_relativePoseEstimating.cpp index 665d5e0c85..7b7ed2ef1b 100644 --- a/src/software/pipeline/main_relativePoseEstimating.cpp +++ b/src/software/pipeline/main_relativePoseEstimating.cpp @@ -93,9 +93,6 @@ bool getPoseStructure(Mat4 & T, Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); - std::cout << "*" << dirX1.dot(pt3d1) << " "; - std::cout << dirX2.dot(pt3d2) << std::endl; - if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) { continue; @@ -106,8 +103,6 @@ bool getPoseStructure(Mat4 & T, countValid++; } - std::cout << countValid << std::endl; - if (countValid > bestCoundValid) { bestCoundValid = countValid; diff --git a/src/software/pipeline/main_sfmExpanding.cpp b/src/software/pipeline/main_sfmExpanding.cpp new file mode 100644 index 0000000000..1b69db3aee --- /dev/null +++ b/src/software/pipeline/main_sfmExpanding.cpp @@ -0,0 +1,112 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +#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. +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 1 +#define ALICEVISION_SOFTWARE_VERSION_MINOR 0 + +using namespace aliceVision; + +namespace po = boost::program_options; + +int aliceVision_main(int argc, char** argv) +{ + // command-line parameters + std::string sfmDataFilename; + std::string sfmDataOutputFilename; + std::string tracksFilename; + + + int randomSeed = std::mt19937::default_seed; + + po::options_description requiredParams("Required parameters"); + requiredParams.add_options() + ("input,i", po::value(&sfmDataFilename)->required(), "SfMData file.") + ("output,o", po::value(&sfmDataOutputFilename)->required(), "SfMData output file.") + ("tracksFilename,t", po::value(&tracksFilename)->required(), "Tracks file."); + + CmdLine cmdline("AliceVision SfM Expanding"); + + cmdline.add(requiredParams); + if(!cmdline.execute(argc, argv)) + { + return EXIT_FAILURE; + } + + // set maxThreads + HardwareContext hwc = cmdline.getHardwareContext(); + omp_set_num_threads(hwc.getMaxThreads()); + + // load input SfMData scene + sfmData::SfMData sfmData; + if(!sfmDataIO::load(sfmData, sfmDataFilename, sfmDataIO::ESfMData::ALL)) + { + ALICEVISION_LOG_ERROR("The input SfMData file '" + sfmDataFilename + "' cannot be read."); + return EXIT_FAILURE; + } + + if (sfmData.getValidViews().size() < 2) + { + ALICEVISION_LOG_INFO("Expansion requires that some views are already defined."); + return EXIT_SUCCESS; + } + + // Load tracks + ALICEVISION_LOG_INFO("Load tracks"); + track::TracksHandler tracksHandler; + if (!tracksHandler.load(tracksFilename, sfmData.getViewsKeys())) + { + ALICEVISION_LOG_ERROR("The input tracks file '" + tracksFilename + "' cannot be read."); + return EXIT_FAILURE; + } + + + sfm::SfmBundle::uptr sfmBundle = std::make_unique(); + sfm::ExpansionHistory::sptr expansionHistory = std::make_shared(); + + sfm::ExpansionChunk::uptr expansionChunk = std::make_unique(); + expansionChunk->setBundleHandler(sfmBundle); + expansionChunk->setExpansionHistoryHandler(expansionHistory); + + sfm::ExpansionPolicy::uptr expansionPolicy = std::make_unique(); + sfm::ExpansionIteration::uptr expansionIteration = std::make_unique(); + expansionIteration->setExpansionHistoryHandler(expansionHistory); + expansionIteration->setExpansionPolicyHandler(expansionPolicy); + expansionIteration->setExpansionChunkHandler(expansionChunk); + + sfm::ExpansionProcess::uptr expansionProcess = std::make_unique(); + expansionProcess->setExpansionHistoryHandler(expansionHistory); + expansionProcess->setExpansionIterationHandler(expansionIteration); + + if (!expansionProcess->process(sfmData, tracksHandler)) + { + ALICEVISION_LOG_INFO("Error processing sfmData"); + return EXIT_FAILURE; + } + + sfmDataIO::save(sfmData, sfmDataOutputFilename, sfmDataIO::ESfMData::ALL); + + + + return EXIT_SUCCESS; +} \ No newline at end of file From 52c6770be1b271690e8a188ba9beb8165ff6e543 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 7 May 2024 15:06:23 +0200 Subject: [PATCH 06/14] sfmExpanding --- src/aliceVision/sfm/CMakeLists.txt | 5 + .../sfm/pipeline/expanding/ConnexityGraph.cpp | 228 ++++++++++++++++++ .../sfm/pipeline/expanding/ConnexityGraph.hpp | 43 ++++ .../pipeline/expanding/ExpansionHistory.cpp | 48 +--- .../pipeline/expanding/ExpansionHistory.hpp | 10 + .../sfm/pipeline/expanding/LbaPolicy.hpp | 47 ++++ .../pipeline/expanding/LbaPolicyConnexity.cpp | 148 ++++++++++++ .../pipeline/expanding/LbaPolicyConnexity.hpp | 38 +++ .../sfm/pipeline/expanding/SfmBundle.cpp | 12 +- .../sfm/pipeline/expanding/SfmBundle.hpp | 18 +- .../sfm/pipeline/expanding/SfmResection.cpp | 3 +- src/software/pipeline/main_sfmExpanding.cpp | 8 +- 12 files changed, 546 insertions(+), 62 deletions(-) create mode 100644 src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/LbaPolicy.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index 6c54022257..9619ddb9b3 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -35,6 +35,9 @@ set(sfm_files_headers pipeline/expanding/ExpansionPolicy.hpp pipeline/expanding/ExpansionPolicyLegacy.hpp pipeline/expanding/ExpansionProcess.hpp + pipeline/expanding/ConnexityGraph.hpp + pipeline/expanding/LbaPolicy.hpp + pipeline/expanding/LbaPolicyConnexity.hpp pipeline/regionsIO.hpp utils/alignment.hpp utils/statistics.hpp @@ -70,6 +73,8 @@ set(sfm_files_sources pipeline/expanding/ExpansionIteration.cpp pipeline/expanding/ExpansionPolicyLegacy.cpp pipeline/expanding/ExpansionProcess.cpp + pipeline/expanding/ConnexityGraph.cpp + pipeline/expanding/LbaPolicyConnexity.cpp pipeline/regionsIO.cpp utils/alignment.cpp utils/statistics.cpp diff --git a/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp b/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp new file mode 100644 index 0000000000..bbfaca57db --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp @@ -0,0 +1,228 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 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 "ConnexityGraph.hpp" +//#include +#include + +namespace aliceVision { +namespace sfm { + +struct viewIdScored +{ + viewIdScored() = default; + + viewIdScored(IndexT v, size_t c) : viewId(v), card(c) + { + } + + bool operator<(const viewIdScored & other) + { + return (card < other.card); + } + + bool operator>(const viewIdScored & other) + { + return (card > other.card); + } + + IndexT viewId = UndefinedIndexT; + size_t card = 0; +}; + +bool ConnexityGraph::build(const sfmData::SfMData & sfmData, const std::set & viewsOfInterest) +{ + lemon::ListGraph graph; + std::map _nodePerViewId; + std::map viewIdPerNode; + + //Create a list of reconstructed views + std::vector views; + for (const auto & pv : sfmData.getViews()) + { + if (sfmData.isPoseAndIntrinsicDefined(pv.first)) + { + views.push_back(pv.first); + + lemon::ListGraph::Node newNode = graph.addNode(); + _nodePerViewId[pv.first] = newNode; + viewIdPerNode[newNode] = pv.first; + } + } + + + //Build a list of landmarks index per view + std::map> landmarksPerView; + for (const auto & pl : sfmData.getLandmarks()) + { + for (const auto & po : pl.second.getObservations()) + { + landmarksPerView[po.first].insert(pl.first); + } + } + + //For all possible unique pairs + std::map> covisibility; + for (int idref = 0; idref < views.size(); idref++) + { + IndexT viewRef = views[idref]; + + const auto & ptref = landmarksPerView[viewRef]; + + for (int idcur = idref + 1; idcur < views.size(); idcur++) + { + IndexT viewCur = views[idcur]; + + const auto & ptcur = landmarksPerView[viewCur]; + + std::vector intersection; + std::set_intersection(ptref.begin(), ptref.end(), ptcur.begin(), ptcur.end(), + std::back_inserter(intersection)); + + size_t s = intersection.size(); + if (s == 0) + { + continue; + } + + covisibility[viewRef].push_back({viewCur, s}); + covisibility[viewCur].push_back({viewRef, s}); + } + } + + //Filter out connexions without enough information + for (auto & item : covisibility) + { + auto & vec = item.second; + if (vec.size() < _minLinksPerView) + { + continue; + } + + std::sort(vec.begin(), vec.end(), std::greater<>()); + + size_t pos = 0; + for (; pos < vec.size(); pos++) + { + if (vec[pos].card < _minCardinality) + { + break; + } + } + + pos = std::max(pos, _minLinksPerView); + vec.resize(pos); + } + + + /** + * For all covisible views, + * We establish a link in the graph + */ + for (const auto & item : covisibility) + { + IndexT viewId1 = item.first; + + for (const auto & part : item.second) + { + IndexT viewId2 = part.viewId; + + const lemon::ListGraph::Node & node1 = _nodePerViewId[viewId1]; + const lemon::ListGraph::Node & node2 = _nodePerViewId[viewId2]; + + graph.addEdge(node1, node2); + } + } + + /** + * For all views sharing a common intrinsic which is still refined, + * Make sure their distance is set to 1 + */ + for (int idref = 0; idref < views.size(); idref++) + { + IndexT viewRef = views[idref]; + IndexT intrinsicIdRef = sfmData.getView(viewRef).getIntrinsicId(); + + const auto * ptr = sfmData.getIntrinsicPtr(intrinsicIdRef); + if (ptr->getState() != EEstimatorParameterState::REFINED) + { + continue; + } + + for (int idcur = idref + 1; idcur < views.size(); idcur++) + { + IndexT viewCur = views[idcur]; + IndexT intrinsicIdCur = sfmData.getView(viewCur).getIntrinsicId(); + + if (intrinsicIdRef != intrinsicIdCur) continue; + + const lemon::ListGraph::Node & node1 = _nodePerViewId[viewRef]; + const lemon::ListGraph::Node & node2 = _nodePerViewId[viewCur]; + + graph.addEdge(node1, node2); + } + } + + /** + * Breath first search on the graph + */ + lemon::Bfs bfs(graph); + bfs.init(); + + for (auto id : viewsOfInterest) + { + auto it = _nodePerViewId.find(id); + if (it != _nodePerViewId.end()) + { + bfs.addSource(it->second); + } + } + + bfs.start(); + for (const auto & x : _nodePerViewId) + { + //Retrieve the poseId associated to this view + IndexT poseId = sfmData.getView(x.first).getPoseId(); + if (poseId == UndefinedIndexT) + { + continue; + } + + auto& node = x.second; + + if (bfs.reached(node)) + { + int d = bfs.dist(node); + + auto lookupIt = _distancesPerPoseId.find(poseId); + if (lookupIt == _distancesPerPoseId.end()) + { + _distancesPerPoseId[x.first] = d; + } + else + { + _distancesPerPoseId[x.first] = std::min(lookupIt->second, d); + } + + } + } + + return true; +} + +int ConnexityGraph::getDistance(IndexT poseId) const +{ + const auto it = _distancesPerPoseId.find(poseId); + if (it == _distancesPerPoseId.end()) + { + return std::numeric_limits::max(); + } + + return it->second; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.hpp b/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.hpp new file mode 100644 index 0000000000..bab421ff0e --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.hpp @@ -0,0 +1,43 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 { + +class ConnexityGraph +{ +public: + /** + * Compute distances of all the sfmData views to the viewsOfInterest views + * @param sfmData the sfmData containing all the views + * @param viewsOfInterest the list of views to compute the distance from. Those views must also be in the sfmData ! + * @return false if an error occured + */ + bool build(const sfmData::SfMData & sfmData, const std::set & viewsOfInterest); + + /** + * Get the distance for a particular poseId to one view of interest + * @param poseId the id of the pose to get the distance to + * @return a distance (minimal number of edges to join this view to a view of interest) + */ + int getDistance(IndexT poseId) const; + + +private: + std::map _distancesPerPoseId; + +private: + size_t _minLinksPerView = 10; + size_t _minCardinality = 50; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp index fd02d9b3ca..71dcbb81a4 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.cpp @@ -85,53 +85,7 @@ void ExpansionHistory::saveState(const sfmData::SfMData & sfmData) _focalHistory[pIntrinsic.first].push_back(std::make_pair(usage, iso->getScale().x())); } - for (auto & pfh : _focalHistory) - { - const auto & vec = pfh.second; - - size_t lastGood = std::numeric_limits::max(); - std::vector> filtered; - - for (int id = vec.size() - 1; id >= 0; id--) - { - //Make sure the usage decrease - if (vec[id].first < lastGood) - { - lastGood = vec[id].first; - filtered.push_back(vec[id]); - } - } - - std::vector cropped; - std::vector focals; - int largestCount = filtered.front().first; - bool nomore = false; - for (int id = 0; id < filtered.size(); id++) - { - if (!nomore) - { - cropped.push_back(filtered[id].second); - } - - if (largestCount - filtered[id].first > 25) - { - nomore = true; - } - - focals.push_back(filtered[id].second); - } - - - /*const double mean = std::accumulate(cropped.begin(), cropped.end(), 0.0) / cropped.size(); - std::vector diff(cropped.size()); - std::transform(cropped.begin(), cropped.end(), diff.begin(), [mean](double x) { return x - mean; }); - const double sqSum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); - double stddev = std::sqrt(sqSum / cropped.size()); - - double minVal = *std::min_element(focals.begin(), focals.end()); - double maxVal = *std::max_element(focals.begin(), focals.end()); - double normStdev = stddev / (maxVal - minVal);*/ - } + } } // namespace sfm diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp index 56681c636f..b207d83ae6 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp @@ -54,6 +54,16 @@ class ExpansionHistory void saveState(const sfmData::SfMData & sfmData); + /** + * Get the history of focals over time for a particular intrinsic id + * @param intrinsicId the id of the intrinsic + * @return a vector defining over time pairs of where count usage is the number of views using this intrinsic + */ + const std::vector> & getFocalHistory(IndexT intrinsicId) + { + return _focalHistory[intrinsicId]; + } + private: // History of focals per intrinsics std::map>> _focalHistory; diff --git a/src/aliceVision/sfm/pipeline/expanding/LbaPolicy.hpp b/src/aliceVision/sfm/pipeline/expanding/LbaPolicy.hpp new file mode 100644 index 0000000000..9024fb0daa --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/LbaPolicy.hpp @@ -0,0 +1,47 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class LbaPolicy +{ +public: + using uptr = std::unique_ptr; + +public: + /** + * @brief Build the policy using a scene + * @param sfmData the scene to process + * @param tracksHandler the tracks for this scene + * @param views the list of views of interest + */ + virtual bool build(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) = 0; + + /** + * brief setup the expansion history handler + * @param expansionHistory a shared ptr + */ + void setExpansionHistoryHandler(ExpansionHistory::sptr & expansionHistory) + { + _historyHandler = expansionHistory; + } + +protected: + std::shared_ptr _historyHandler; + +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.cpp b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.cpp new file mode 100644 index 0000000000..c7490bb92a --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.cpp @@ -0,0 +1,148 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 LbaPolicyConnexity::build(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) +{ + if (_historyHandler) + { + setupIntrinsics(sfmData); + } + + ConnexityGraph graph; + if (!graph.build(sfmData, viewIds)) + { + return false; + } + + upgradeSfmData(sfmData, graph); + + return true; +} + +void LbaPolicyConnexity::upgradeSfmData(sfmData::SfMData & sfmData, const ConnexityGraph & graph) +{ + /** + * Propagate states to views in sfmData + */ + for (auto & pp : sfmData.getPoses()) + { + int d = graph.getDistance(pp.first); + if (d <= _distanceLimit) + { + if (pp.second.isLocked()) + { + pp.second.setState(EEstimatorParameterState::CONSTANT); + } + else + { + pp.second.setState(EEstimatorParameterState::REFINED); + } + } + else if (d == (_distanceLimit + 1)) + { + pp.second.setState(EEstimatorParameterState::CONSTANT); + } + else + { + pp.second.setState(EEstimatorParameterState::IGNORED); + } + } + + /*Landmarks*/ + for (auto & pl : sfmData.getLandmarks()) + { + EEstimatorParameterState state = EEstimatorParameterState::REFINED; + + // By default, the landmark is refined, except if at least one observation comes from an ignored camera + + for (const auto & po : pl.second.getObservations()) + { + IndexT viewId = po.first; + IndexT poseId = sfmData.getView(viewId).getPoseId(); + + if (poseId == UndefinedIndexT) + { + continue; + } + + if (sfmData.getAbsolutePose(poseId).getState() == EEstimatorParameterState::IGNORED) + { + state = EEstimatorParameterState::IGNORED; + } + } + + pl.second.state = state; + } +} + +void LbaPolicyConnexity::setupIntrinsics(sfmData::SfMData & sfmData) +{ + for (auto & pi : sfmData.getIntrinsics()) + { + const auto & vec = _historyHandler->getFocalHistory(pi.first); + + size_t lastGood = std::numeric_limits::max(); + std::vector> filtered; + + for (int id = vec.size() - 1; id >= 0; id--) + { + //Make sure the usage decrease, + //Remove the steps where the usage increase (we are going to the past) + if (vec[id].first < lastGood) + { + lastGood = vec[id].first; + filtered.push_back(vec[id]); + } + } + + std::vector cropped; + std::vector focals; + int largestCount = filtered.front().first; + bool nomore = false; + for (int id = 0; id < filtered.size(); id++) + { + if (!nomore) + { + cropped.push_back(filtered[id].second); + } + + if (largestCount - filtered[id].first > 25) + { + nomore = true; + } + + focals.push_back(filtered[id].second); + } + + + const double mean = std::accumulate(cropped.begin(), cropped.end(), 0.0) / cropped.size(); + std::vector diff(cropped.size()); + std::transform(cropped.begin(), cropped.end(), diff.begin(), [mean](double x) { return x - mean; }); + const double sqSum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); + double stddev = std::sqrt(sqSum / cropped.size()); + + double minVal = *std::min_element(focals.begin(), focals.end()); + double maxVal = *std::max_element(focals.begin(), focals.end()); + double normStdev = stddev / (maxVal - minVal); + + if (normStdev < 0.01 || pi.second->isLocked()) + { + pi.second->setState(EEstimatorParameterState::CONSTANT); + } + else + { + pi.second->setState(EEstimatorParameterState::REFINED); + } + } +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp new file mode 100644 index 0000000000..f0938bd302 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp @@ -0,0 +1,38 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 { + +class LbaPolicyConnexity : public LbaPolicy +{ +public: + using uptr = std::unique_ptr; + +public: + /** + * @brief Build the policy using a scene + * @param sfmData the scene to process + * @param tracksHandler the tracks for this scene + * @param views the list of views of interest + */ + virtual bool build(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) override; + +private: + void upgradeSfmData(sfmData::SfMData & sfmData, const ConnexityGraph & graph); + void setupIntrinsics(sfmData::SfMData & sfmData); + +private: + int _distanceLimit = 1; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp index 4ed399d13c..6f602f3aa5 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp @@ -62,7 +62,7 @@ bool SfmBundle::cleanup(sfmData::SfMData & sfmData) return somethingChanged; } -bool SfmBundle::initialize(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) +bool SfmBundle::initialize(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) { bool enableLocalStrategy = _useLBA; @@ -73,16 +73,10 @@ bool SfmBundle::initialize(const sfmData::SfMData & sfmData, const track::Tracks if (enableLocalStrategy) { - /*if (_lbaPolicy) + if (_lbaPolicy) { _lbaPolicy->build(sfmData, tracksHandler, viewIds); - }*/ - - /*_lbaGraph = std::make_shared(sfmData); - _lbaGraph->setGraphDistanceLimit(_LBAGraphDistanceLimit); - _lbaGraph->updateGraphWithNewViews(sfmData, tracksHandler.getTracksPerView(), sfmData.getValidViews(), _LBAMinNbOfMatches); - _lbaGraph->computeGraphDistances(sfmData, viewIds); - _lbaGraph->convertDistancesToStates(sfmData);*/ + } } return true; diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp index e94d7fadee..d3fafab380 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp @@ -10,8 +10,8 @@ #include #include #include -//#include -//#include +#include +#include namespace aliceVision { namespace sfm { @@ -31,11 +31,21 @@ class SfmBundle */ bool process(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); + /** + * brief setup the expansion chunk handler + * @param expansionChunk a unique ptr. Ownership will be taken + */ + void setLbaPolicyHandler(LbaPolicy::uptr & lbaPolicy) + { + _lbaPolicy = std::move(lbaPolicy); + } + + private: /** * Initialize bundle properties */ - bool initialize(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); + bool initialize(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); /** * Cleanup sfmData @@ -45,7 +55,7 @@ class SfmBundle bool cleanup(sfmData::SfMData & sfmData); private: - //std::unique_ptr _lbaPolicy; + LbaPolicy::uptr _lbaPolicy; private: diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp index 7a2f992ffa..a804982f52 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace aliceVision { namespace sfm { @@ -161,7 +162,7 @@ bool SfmResection::internalRefinement( tinyScene.getLandmarks()[i] = std::move(landmark); } - BundleAdjustmentCeres BA; + BundleAdjustmentSymbolicCeres BA; BundleAdjustment::ERefineOptions refineOptions = BundleAdjustment::REFINE_ROTATION | BundleAdjustment::REFINE_TRANSLATION; const bool success = BA.adjust(tinyScene, refineOptions); diff --git a/src/software/pipeline/main_sfmExpanding.cpp b/src/software/pipeline/main_sfmExpanding.cpp index 1b69db3aee..dbdebbe5ec 100644 --- a/src/software/pipeline/main_sfmExpanding.cpp +++ b/src/software/pipeline/main_sfmExpanding.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include @@ -81,9 +82,14 @@ int aliceVision_main(int argc, char** argv) } - sfm::SfmBundle::uptr sfmBundle = std::make_unique(); sfm::ExpansionHistory::sptr expansionHistory = std::make_shared(); + sfm::LbaPolicy::uptr sfmPolicy = std::make_unique(); + sfmPolicy->setExpansionHistoryHandler(expansionHistory); + + sfm::SfmBundle::uptr sfmBundle = std::make_unique(); + sfmBundle->setLbaPolicyHandler(sfmPolicy); + sfm::ExpansionChunk::uptr expansionChunk = std::make_unique(); expansionChunk->setBundleHandler(sfmBundle); expansionChunk->setExpansionHistoryHandler(expansionHistory); From 3865416e48eba5fe81bdb4f998ffcf467dce93b0 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Mon, 27 May 2024 14:06:14 +0200 Subject: [PATCH 07/14] remove useless prints --- src/software/pipeline/main_nodalSfM.cpp | 3 --- src/software/pipeline/main_relativePoseEstimating.cpp | 10 +--------- src/software/pipeline/main_sfmBootstraping.cpp | 3 --- 3 files changed, 1 insertion(+), 15 deletions(-) diff --git a/src/software/pipeline/main_nodalSfM.cpp b/src/software/pipeline/main_nodalSfM.cpp index 9de612dc48..37dea6d8a4 100644 --- a/src/software/pipeline/main_nodalSfM.cpp +++ b/src/software/pipeline/main_nodalSfM.cpp @@ -226,8 +226,6 @@ bool localizeNext(sfmData::SfMData& sfmData, // Assign pose sfmData.setPose(newView, sfmData::CameraPose(geometry::Pose3(R, Vec3::Zero()))); - std::cout << vecInliers.size() << std::endl; - // Add observations for (size_t pos : vecInliers) { @@ -477,7 +475,6 @@ int aliceVision_main(int argc, char** argv) sfm::BundleAdjustmentSymbolicCeres BA(options, 3); const bool success = BA.adjust(sfmData, refineOptions); countRemoved = sfm::removeOutliersWithPixelResidualError(sfmData, sfm::EFeatureConstraint::SCALE, 2.0, 2); - std::cout << countRemoved << std::endl; } while (countRemoved > 0); sfmDataIO::save(sfmData, sfmDataOutputFilename, sfmDataIO::ESfMData::ALL); diff --git a/src/software/pipeline/main_relativePoseEstimating.cpp b/src/software/pipeline/main_relativePoseEstimating.cpp index 7b7ed2ef1b..de0e016a92 100644 --- a/src/software/pipeline/main_relativePoseEstimating.cpp +++ b/src/software/pipeline/main_relativePoseEstimating.cpp @@ -70,8 +70,6 @@ bool getPoseStructure(Mat4 & T, const Mat4 T1 = Eigen::Matrix4d::Identity(); const Mat4 & T2 = Ts[it]; - std::cout << T2.inverse() << std::endl; - std::vector points; std::vector updatedInliers; @@ -349,7 +347,7 @@ int aliceVision_main(int argc, char** argv) int chunkEnd = int(double(rangeStart + rangeSize) * ratioChunk); // For each covisible pair -//#pragma omp parallel for +#pragma omp parallel for for (int posPairs = chunkStart; posPairs < chunkEnd; posPairs++) { auto iterPairs = covisibility.begin(); @@ -425,8 +423,6 @@ int aliceVision_main(int argc, char** argv) continue; } - std::cout << inliers.size() << std::endl; - std::vector structure; reconstructed.reference = refImage; reconstructed.next = nextImage; @@ -437,13 +433,9 @@ int aliceVision_main(int argc, char** argv) continue; } - std::cout << vecInliers.size() << std::endl; - reconstructed.R = T.block<3, 3>(0, 0); reconstructed.t = T.block<3, 1>(0, 3); } - - std::cout << vecInliers.size() << std::endl; // Extract inliers std::vector refPtsValid, nextPtsValid; diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp index e2101e8786..20072be0a8 100644 --- a/src/software/pipeline/main_sfmBootstraping.cpp +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -106,8 +106,6 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::Reconstructe return false; } - std::cout << angles.size() << std::endl; - const unsigned medianIndex = angles.size() / 2; std::nth_element(angles.begin(), angles.begin() + medianIndex, angles.end()); resultAngle = angles[medianIndex]; @@ -330,7 +328,6 @@ int aliceVision_main(int argc, char** argv) std::vector usedTracks; double angle = 0.0; - std::cout << "toto" << std::endl; if (!estimatePairAngle(sfmData, pair, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), maxEpipolarDistance, angle, usedTracks)) { continue; From 527164db7f64ecf983d6d108e69abed7b1e28893 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Mon, 27 May 2024 16:25:01 +0200 Subject: [PATCH 08/14] add expansion parameterization --- .../sfm/pipeline/expanding/ExpansionChunk.cpp | 2 +- .../sfm/pipeline/expanding/ExpansionChunk.hpp | 38 ++++++++ .../expanding/ExpansionPolicyLegacy.cpp | 4 +- .../expanding/ExpansionPolicyLegacy.hpp | 25 ++++- .../pipeline/expanding/LbaPolicyConnexity.hpp | 11 ++- .../sfm/pipeline/expanding/SfmBundle.hpp | 34 ++++++- src/software/pipeline/main_sfmExpanding.cpp | 95 ++++++++++++++++++- 7 files changed, 199 insertions(+), 10 deletions(-) diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp index 07db683565..39c3c3d30e 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp @@ -31,7 +31,7 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand if (!sfmData.isPoseAndIntrinsicDefined(viewId)) { - SfmResection resection(_resectionIterations, std::numeric_limits::infinity()); + SfmResection resection(_resectionIterations, _resectionMaxError); Eigen::Matrix4d pose; double threshold = 0.0; diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp index 59adf931f5..d54454aa01 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp @@ -51,6 +51,43 @@ class ExpansionChunk _historyHandler = expansionHistory; } + void setResectionMaxIterations(size_t maxIterations) + { + _resectionIterations = maxIterations; + } + + /** + * @brief set the maximal error allowed for ransac resection module + * @param error the error value or <= 0 for automatic decision + * @param count the number of points + */ + void setResectionMaxError(double error) + { + _resectionMaxError = error; + if (_resectionMaxError <= 0.0) + { + _resectionMaxError = std::numeric_limits::infinity(); + } + } + + /** + * @brief set the minimal number of points to enable triangulation of a track + * @param count the number of points + */ + void setTriangulationMinPoints(size_t count) + { + _triangulationMinPoints = count; + } + + /** + * @brief set the minimal allowed parallax degree for triangulation + * @param angle the angle in DEGREES + */ + void setMinAngleTriangulation(double angle) + { + _minTriangulationAngleDegrees = angle; + } + private: /** @@ -78,6 +115,7 @@ class ExpansionChunk size_t _triangulationMinPoints = 2; double _minTriangulationAngleDegrees = 3.0; double _maxTriangulationError = 8.0; + double _resectionMaxError = std::numeric_limits::infinity(); }; } // namespace sfm diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp index 32219750df..36735831b4 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp @@ -113,8 +113,8 @@ bool ExpansionPolicyLegacy::process(const sfmData::SfMData & sfmData, const trac //Always add at least the best score, whatever it is _selectedViews.insert(vscoring[0].id); - int maxSetSize = _maxImagesPerGroup; - if (sfmData.getValidViews().size() < _nbFirstUnstableCameras) + int maxSetSize = _maxViewsPerGroup; + if (sfmData.getValidViews().size() < _nbFirstUnstableViews) { maxSetSize = 1; } diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp index 6cbb17aa43..25361f2f8d 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp @@ -14,6 +14,9 @@ namespace sfm { class ExpansionPolicyLegacy : public ExpansionPolicy { +public: + using uptr = std::unique_ptr; + public: /** @@ -52,6 +55,24 @@ class ExpansionPolicyLegacy : public ExpansionPolicy const size_t maxSize, const size_t countLevels); + /** + * @brief set the number of views for which we consider the sfm to be unstable + * @param count a number of views + */ + void setNbFirstUnstableViews(size_t count) + { + _nbFirstUnstableViews = count; + } + + /** + * @brief set the number of views we want for a given chunk + * @param count a number of views + */ + void setMaxViewsPerGroup(size_t count) + { + _maxViewsPerGroup = count; + } + private: // vector of selected views for this iteration @@ -71,10 +92,10 @@ class ExpansionPolicyLegacy : public ExpansionPolicy std::size_t _countPyramidLevels = 5; // Number of cameras in scene under which the set is considered as unstable - size_t _nbFirstUnstableCameras = 30; + size_t _nbFirstUnstableViews = 30; // Maximal number of images in a chunk - size_t _maxImagesPerGroup = 30; + size_t _maxViewsPerGroup = 30; }; } diff --git a/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp index f0938bd302..d994a0b50e 100644 --- a/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp @@ -15,7 +15,7 @@ namespace sfm { class LbaPolicyConnexity : public LbaPolicy { public: - using uptr = std::unique_ptr; + using uptr = std::unique_ptr; public: /** @@ -26,6 +26,15 @@ class LbaPolicyConnexity : public LbaPolicy */ virtual bool build(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) override; + /** + * @brief set the max distance in the graph to search for connexity + * @param distanceLimit is the max number of hops between two element in the graph + */ + void setDistanceLimit(int distanceLimit) + { + _distanceLimit = distanceLimit; + } + private: void upgradeSfmData(sfmData::SfMData & sfmData, const ConnexityGraph & graph); void setupIntrinsics(sfmData::SfMData & sfmData); diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp index d3fafab380..445617ff8f 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp @@ -40,6 +40,38 @@ class SfmBundle _lbaPolicy = std::move(lbaPolicy); } + void setBundleAdjustmentMaxOutlier(size_t bundleAdjustmentMaxOutlier) + { + _bundleAdjustmentMaxOutlier = bundleAdjustmentMaxOutlier; + } + + /** + * @brief set the minimal allowed parallax degree after bundle (Or the landmark will be removed) + * @param angle the angle in DEGREES + */ + void setMinAngleLandmark(double angle) + { + _minAngleForLandmark = angle; + } + + /** + * @brief set the maximal allowed error in pixels (Or the landmark will be removed) + * @param error the error in pixels + */ + void setMaxReprojectionError(double error) + { + _maxReprojectionError = error; + } + + /** + * @brief set the Minimal number of connected views to refine an intrinsic principal point + * @param count the number of views + */ + void setMinNbCamerasToRefinePrincipalPoint(size_t count) + { + _minNbCamerasToRefinePrincipalPoint = count; + } + private: /** @@ -60,7 +92,7 @@ class SfmBundle private: EFeatureConstraint _featureConstraint = EFeatureConstraint::SCALE; - double _maxReprojectionError = 40.0; + double _maxReprojectionError = 4.0; double _minAngleForLandmark = 2.0; size_t _minTrackLength = 2; size_t _minPointsPerPose = 30; diff --git a/src/software/pipeline/main_sfmExpanding.cpp b/src/software/pipeline/main_sfmExpanding.cpp index dbdebbe5ec..ec91cbcc64 100644 --- a/src/software/pipeline/main_sfmExpanding.cpp +++ b/src/software/pipeline/main_sfmExpanding.cpp @@ -37,18 +37,64 @@ int aliceVision_main(int argc, char** argv) std::string sfmDataOutputFilename; std::string tracksFilename; + std::size_t localizerEstimatorMaxIterations = 50000; + double localizerEstimatorError = 0.0; + bool lockScenePreviouslyReconstructed = false; + bool useLocalBA = true; + int lbaDistanceLimit = 1; + std::size_t nbFirstUnstableCameras = 30; + std::size_t maxImagesPerGroup = 30; + int bundleAdjustmentMaxOutliers = 50; + std::size_t minNbObservationsForTriangulation = 2; + double minAngleForTriangulation = 3.0; + double minAngleForLandmark = 2.0; + double maxReprojectionError = 4.0; + bool lockAllIntrinsics = false; + int minNbCamerasToRefinePrincipalPoint = 3; int randomSeed = std::mt19937::default_seed; + // clang-format off po::options_description requiredParams("Required parameters"); requiredParams.add_options() ("input,i", po::value(&sfmDataFilename)->required(), "SfMData file.") ("output,o", po::value(&sfmDataOutputFilename)->required(), "SfMData output file.") ("tracksFilename,t", po::value(&tracksFilename)->required(), "Tracks file."); + + po::options_description optionalParams("Optional parameters"); + optionalParams.add_options() + ("localizerEstimatorMaxIterations", po::value(&localizerEstimatorMaxIterations)->default_value(localizerEstimatorMaxIterations), "Maximum number of RANSAC iterations.") + ("localizerEstimatorError", po::value(&localizerEstimatorError)->default_value(0.0), "Reprojection error threshold (in pixels) for the localizer estimator (0 for default value according to the estimator).") + ("lockScenePreviouslyReconstructed", po::value(&lockScenePreviouslyReconstructed)->default_value(lockScenePreviouslyReconstructed),"Lock/Unlock scene previously reconstructed.") + ("useLocalBA,l", po::value(&useLocalBA)->default_value(useLocalBA), "Enable/Disable the Local bundle adjustment strategy.\n It reduces the reconstruction time, especially for big datasets (500+ images).") + ("localBAGraphDistance", po::value(&lbaDistanceLimit)->default_value(lbaDistanceLimit), "Graph-distance limit setting the Active region in the Local Bundle Adjustment strategy.") + ("nbFirstUnstableCameras", po::value(&nbFirstUnstableCameras)->default_value(nbFirstUnstableCameras), + "Number of cameras for which the bundle adjustment is performed every single time a camera is added, leading to more stable " + "results while the computations are not too expensive since there is not much data. Past this number, the bundle adjustment " + "will only be performed once for N added cameras.") + ("maxImagesPerGroup", po::value(&maxImagesPerGroup)->default_value(maxImagesPerGroup), + "Maximum number of cameras that can be added before the bundle adjustment is performed. " + "This prevents adding too much data at once without performing the bundle adjustment.") + ("bundleAdjustmentMaxOutliers", po::value(&bundleAdjustmentMaxOutliers)->default_value(bundleAdjustmentMaxOutliers), + "Threshold for the maximum number of outliers allowed at the end of a bundle adjustment iteration." + "Using a negative value for this threshold will disable BA iterations.") + ("minNumberOfObservationsForTriangulation", po::value(&minNbObservationsForTriangulation)->default_value(minNbObservationsForTriangulation),"Minimum number of observations to triangulate a point") + ("minAngleForTriangulation", po::value(&minAngleForTriangulation)->default_value(minAngleForTriangulation),"Minimum angle for triangulation.") + ("minAngleForLandmark", po::value(&minAngleForLandmark)->default_value(minAngleForLandmark), "Minimum angle for landmark.") + ("maxReprojectionError", po::value(&maxReprojectionError)->default_value(maxReprojectionError), "Maximum reprojection error.") + ("lockAllIntrinsics", po::value(&lockAllIntrinsics)->default_value(lockAllIntrinsics), "Force lock of all camera intrinsic parameters, so they will not be refined during Bundle Adjustment.") + ("minNbCamerasToRefinePrincipalPoint", po::value(&minNbCamerasToRefinePrincipalPoint)->default_value(minNbCamerasToRefinePrincipalPoint), + "Minimal number of cameras to refine the principal point of the cameras (one of the intrinsic parameters of the camera). " + "If we do not have enough cameras, the principal point in consider is considered in the center of the image. " + "If minNbCamerasToRefinePrincipalPoint<=0, the principal point is never refined. " + "If minNbCamerasToRefinePrincipalPoint==1, the principal point is always refined.") + ; + // clang-format on CmdLine cmdline("AliceVision SfM Expanding"); cmdline.add(requiredParams); + cmdline.add(optionalParams); if(!cmdline.execute(argc, argv)) { return EXIT_FAILURE; @@ -72,6 +118,27 @@ int aliceVision_main(int argc, char** argv) return EXIT_SUCCESS; } + // lock scene previously reconstructed + if (lockScenePreviouslyReconstructed) + { + // lock all reconstructed camera poses + for (auto& cameraPosePair : sfmData.getPoses()) + { + cameraPosePair.second.lock(); + } + + for (const auto& viewPair : sfmData.getViews()) + { + // lock all reconstructed views intrinsics + const sfmData::View& view = *(viewPair.second); + + if (sfmData.isPoseAndIntrinsicDefined(&view)) + { + sfmData.getIntrinsics().at(view.getIntrinsicId())->lock(); + } + } + } + // Load tracks ALICEVISION_LOG_INFO("Load tracks"); track::TracksHandler tracksHandler; @@ -84,17 +151,39 @@ int aliceVision_main(int argc, char** argv) sfm::ExpansionHistory::sptr expansionHistory = std::make_shared(); - sfm::LbaPolicy::uptr sfmPolicy = std::make_unique(); - sfmPolicy->setExpansionHistoryHandler(expansionHistory); + sfm::LbaPolicy::uptr sfmPolicy; + + if (useLocalBA) + { + sfm::LbaPolicyConnexity::uptr sfmPolicyTyped = std::make_unique(); + sfmPolicyTyped->setExpansionHistoryHandler(expansionHistory); + sfmPolicyTyped->setDistanceLimit(lbaDistanceLimit); + sfmPolicy = std::move(sfmPolicyTyped); + } sfm::SfmBundle::uptr sfmBundle = std::make_unique(); sfmBundle->setLbaPolicyHandler(sfmPolicy); + sfmBundle->setBundleAdjustmentMaxOutlier(bundleAdjustmentMaxOutliers); + sfmBundle->setMinAngleLandmark(minAngleForLandmark); + sfmBundle->setMaxReprojectionError(maxReprojectionError); + sfmBundle->setMinNbCamerasToRefinePrincipalPoint(minNbCamerasToRefinePrincipalPoint); sfm::ExpansionChunk::uptr expansionChunk = std::make_unique(); expansionChunk->setBundleHandler(sfmBundle); expansionChunk->setExpansionHistoryHandler(expansionHistory); + expansionChunk->setResectionMaxIterations(localizerEstimatorMaxIterations); + expansionChunk->setResectionMaxError(localizerEstimatorError); + expansionChunk->setTriangulationMinPoints(minNbObservationsForTriangulation); + expansionChunk->setMinAngleTriangulation(minAngleForTriangulation); + + sfm::ExpansionPolicy::uptr expansionPolicy; + { + sfm::ExpansionPolicyLegacy::uptr expansionPolicyTyped = std::make_unique(); + expansionPolicyTyped->setNbFirstUnstableViews(nbFirstUnstableCameras); + expansionPolicyTyped->setMaxViewsPerGroup(maxImagesPerGroup); + expansionPolicy = std::move(expansionPolicyTyped); + } - sfm::ExpansionPolicy::uptr expansionPolicy = std::make_unique(); sfm::ExpansionIteration::uptr expansionIteration = std::make_unique(); expansionIteration->setExpansionHistoryHandler(expansionHistory); expansionIteration->setExpansionPolicyHandler(expansionPolicy); From a218dc12d8a84f685c558bab9a311283ee1d4fdd Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 28 May 2024 10:27:30 +0200 Subject: [PATCH 09/14] Move export coordinates of track to library --- src/aliceVision/track/TracksBuilder.cpp | 24 ++++++++++++++++++- src/aliceVision/track/TracksBuilder.hpp | 7 +++++- src/software/pipeline/main_tracksBuilding.cpp | 21 +--------------- 3 files changed, 30 insertions(+), 22 deletions(-) diff --git a/src/aliceVision/track/TracksBuilder.cpp b/src/aliceVision/track/TracksBuilder.cpp index d5f24e6eb1..20bbe7be9b 100644 --- a/src/aliceVision/track/TracksBuilder.cpp +++ b/src/aliceVision/track/TracksBuilder.cpp @@ -169,7 +169,7 @@ bool TracksBuilder::exportToStream(std::ostream& os) return os.good(); } -void TracksBuilder::exportToSTL(TracksMap& allTracks) const +void TracksBuilder::exportToSTL(TracksMap& allTracks, const feature::FeaturesPerView * featuresPerView) const { allTracks.clear(); @@ -189,6 +189,28 @@ void TracksBuilder::exportToSTL(TracksMap& allTracks) const outTrack.featPerView[currentPair.first].featureId = currentPair.second.featIndex; } } + + //Fill additional data + if (featuresPerView != nullptr) + { + for (auto & ptrack : allTracks) + { + auto & track = ptrack.second; + + for (auto & pitem : track.featPerView) + { + IndexT viewId = pitem.first; + track::TrackItem & item = pitem.second; + const auto & feats = featuresPerView->getFeaturesPerDesc(viewId); + + const feature::PointFeatures & features = feats.at(track.descType); + const feature::PointFeature & feature = features.at(item.featureId); + + item.coords = feature.coords().cast(); + item.scale = feature.scale(); + } + } + } } std::size_t TracksBuilder::nbTracks() const diff --git a/src/aliceVision/track/TracksBuilder.hpp b/src/aliceVision/track/TracksBuilder.hpp index d9f60a8a1d..368aa91b92 100644 --- a/src/aliceVision/track/TracksBuilder.hpp +++ b/src/aliceVision/track/TracksBuilder.hpp @@ -7,6 +7,7 @@ #pragma once #include +#include #include @@ -50,6 +51,7 @@ class TracksBuilder /** * @brief Build tracks for a given series of pairWise matches * @param[in] pairwiseMatches PairWise matches + * @param[in] featuresPerView all features (used for matching) in pairwiseMatches */ void build(const PairwiseMatches& pairwiseMatches); @@ -71,8 +73,11 @@ class TracksBuilder /** * @brief Export tracks as a map (each entry is a sequence of imageId and keypointId): * {TrackIndex => {(imageIndex, keypointId), ... ,(imageIndex, keypointId)} + * @param allTracks output to tracks + * @param featuresPerView is the feature per view map for accessing features information. + * If nullptr, then no coordinates or scale will be saved (For legacy purpose) */ - void exportToSTL(TracksMap& allTracks) const; + void exportToSTL(TracksMap& allTracks, const feature::FeaturesPerView* featuresPerView = nullptr) const; /** * @brief Return the number of connected set in the UnionFind structure (tree forest) diff --git a/src/software/pipeline/main_tracksBuilding.cpp b/src/software/pipeline/main_tracksBuilding.cpp index 77df06871d..93b133b8d1 100644 --- a/src/software/pipeline/main_tracksBuilding.cpp +++ b/src/software/pipeline/main_tracksBuilding.cpp @@ -134,26 +134,7 @@ int aliceVision_main(int argc, char** argv) ALICEVISION_LOG_INFO("Track export to structure"); track::TracksMap mapTracks; - tracksBuilder.exportToSTL(mapTracks); - - //Fill additional data - for (auto & ptrack : mapTracks) - { - auto & track = ptrack.second; - - for (auto & pitem : track.featPerView) - { - IndexT viewId = pitem.first; - track::TrackItem & item = pitem.second; - const auto & feats = featuresPerView.getFeaturesPerDesc(viewId); - - const feature::PointFeatures & features = feats.at(track.descType); - const feature::PointFeature & feature = features.at(item.featureId); - - item.coords = feature.coords().cast(); - item.scale = feature.scale(); - } - } + tracksBuilder.exportToSTL(mapTracks, &featuresPerView); // write the json file with the tree ALICEVISION_LOG_INFO("Export to file"); From a2d77c1bbb851b315697414e191d480a36fbccf1 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 28 May 2024 13:41:12 +0200 Subject: [PATCH 10/14] remove functions from pose estimating app to lib --- src/aliceVision/multiview/CMakeLists.txt | 1 + src/aliceVision/multiview/essential.cpp | 72 +++++++ src/aliceVision/multiview/essential.hpp | 27 ++- src/aliceVision/sfm/utils/statistics.cpp | 34 +++ src/aliceVision/sfm/utils/statistics.hpp | 17 ++ src/aliceVision/track/tracksUtils.cpp | 28 +++ src/aliceVision/track/tracksUtils.hpp | 8 + .../pipeline/main_relativePoseEstimating.cpp | 195 ++++-------------- 8 files changed, 230 insertions(+), 152 deletions(-) diff --git a/src/aliceVision/multiview/CMakeLists.txt b/src/aliceVision/multiview/CMakeLists.txt index b35d49504d..be59051450 100644 --- a/src/aliceVision/multiview/CMakeLists.txt +++ b/src/aliceVision/multiview/CMakeLists.txt @@ -78,6 +78,7 @@ alicevision_add_library(aliceVision_multiview PUBLIC_LINKS aliceVision_numeric aliceVision_robustEstimation + aliceVision_camera ${CERES_LIBRARIES} PRIVATE_LINKS aliceVision_system diff --git a/src/aliceVision/multiview/essential.cpp b/src/aliceVision/multiview/essential.cpp index 24389342c2..55c4f81602 100644 --- a/src/aliceVision/multiview/essential.cpp +++ b/src/aliceVision/multiview/essential.cpp @@ -167,4 +167,76 @@ bool motionFromEssentialAndCorrespondence(const Mat3& E, const Mat3& K1, const V } } +bool estimateTransformStructureFromEssential(Mat4 & T, + std::vector& structure, + std::vector& newVecInliers, + const Mat3& E, + const std::vector& vecInliers, + const camera::IntrinsicBase & cam1, + const camera::IntrinsicBase & cam2, + const std::vector & x1, + const std::vector & x2) +{ + // Find set of analytical solutions + std::vector Ts; + motionFromEssential(E, Ts); + + // Check each possible solution and keep the best one + size_t bestCoundValid = 0; + for (int it = 0; it < Ts.size(); it++) + { + const Mat4 T1 = Eigen::Matrix4d::Identity(); + const Mat4 & T2 = Ts[it]; + + std::vector points; + std::vector updatedInliers; + + // Triangulate each point + size_t countValid = 0; + for (size_t k = 0; k < vecInliers.size(); ++k) + { + const Vec2& pt1 = x1[vecInliers[k]]; + const Vec2& pt2 = x2[vecInliers[k]]; + + const Vec3 pt3d1 = cam1.toUnitSphere(cam1.removeDistortion(cam1.ima2cam(pt1))); + const Vec3 pt3d2 = cam2.toUnitSphere(cam2.removeDistortion(cam2.ima2cam(pt2))); + + Vec3 X; + multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); + + Vec2 ptValid1 = cam1.project(T1, X.homogeneous(), true); + Vec2 ptValid2 = cam2.project(T2, X.homogeneous(), true); + + Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); + Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); + + // Check that the points are logically + if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) + { + continue; + } + + updatedInliers.push_back(vecInliers[k]); + points.push_back(X); + countValid++; + } + + if (countValid > bestCoundValid) + { + bestCoundValid = countValid; + structure = points; + newVecInliers = updatedInliers; + T = Ts[it]; + } + } + + // Check result validity + if (newVecInliers.size() < 10) + { + return false; + } + + return true; +} + } // namespace aliceVision diff --git a/src/aliceVision/multiview/essential.hpp b/src/aliceVision/multiview/essential.hpp index 464611de83..2a7f70484a 100644 --- a/src/aliceVision/multiview/essential.hpp +++ b/src/aliceVision/multiview/essential.hpp @@ -9,7 +9,7 @@ #pragma once #include - +#include #include namespace aliceVision { @@ -62,4 +62,29 @@ void motionFromEssential(const Mat3& E, std::vector* Rs, std::vector */ void motionFromEssential(const Mat3& E, std::vector & Ts); +/** + * @brief Estimate the relative transformation of the camera and + * the associated 3d structure of the observed points using an + * essential matrix as input prior + * @param T the output estimated pose + * @param structure the output estimated structure + * @param newVecInliers the updated inliers list (set of indices in the coordinates vectors) + * @param E the input Essential matrix prior + * @param vecInliers the input list of inliers (set of indices in the coordinates vectors) + * @param cam1 the first camera intrinsic object + * @param cam2 the second camera intrinsic object + * @param x1 the observed points coordinates in the first camera + * @param x2 the observed points coordinates in the second camera + * @return true if estimation succeeded +*/ +bool estimateTransformStructureFromEssential(Mat4 & T, + std::vector& structure, + std::vector& newVecInliers, + const Mat3& E, + const std::vector& vecInliers, + const camera::IntrinsicBase & cam1, + const camera::IntrinsicBase & cam2, + const std::vector & x1, + const std::vector & x2); + } // namespace aliceVision diff --git a/src/aliceVision/sfm/utils/statistics.cpp b/src/aliceVision/sfm/utils/statistics.cpp index 644abe8b74..581dc9cb44 100644 --- a/src/aliceVision/sfm/utils/statistics.cpp +++ b/src/aliceVision/sfm/utils/statistics.cpp @@ -6,7 +6,10 @@ // You can obtain one at https://mozilla.org/MPL/2.0/. #include "statistics.hpp" + #include +#include +#include namespace aliceVision { namespace sfm { @@ -35,5 +38,36 @@ double RMSE(const sfmData::SfMData& sfmData) return RMSE; } +double computeAreaScore(const std::vector& refPts, const std::vector& nextPts, size_t refWidth, size_t refHeight, size_t nextWidth, size_t nextHeight) +{ + namespace bg = boost::geometry; + + const double refArea = refWidth * refHeight; + const double nextArea = nextWidth * nextHeight; + + typedef boost::geometry::model::point point_t; + typedef boost::geometry::model::multi_point mpoint_t; + typedef boost::geometry::model::polygon polygon; + mpoint_t mpt1, mpt2; + + for (int idx = 0; idx < refPts.size(); idx++) + { + const auto& refPt = refPts[idx]; + const auto& nextPt = nextPts[idx]; + + boost::geometry::append(mpt1, point_t(refPt(0), refPt(1))); + boost::geometry::append(mpt2, point_t(nextPt(0), nextPt(1))); + } + + polygon hull1, hull2; + boost::geometry::convex_hull(mpt1, hull1); + boost::geometry::convex_hull(mpt2, hull2); + const double area1 = boost::geometry::area(hull1); + const double area2 = boost::geometry::area(hull1); + const double score = (area1 + area2) / (refArea + nextArea); + + return score; +} + } // namespace sfm } // namespace aliceVision diff --git a/src/aliceVision/sfm/utils/statistics.hpp b/src/aliceVision/sfm/utils/statistics.hpp index 9bb040cbcd..075448de1f 100644 --- a/src/aliceVision/sfm/utils/statistics.hpp +++ b/src/aliceVision/sfm/utils/statistics.hpp @@ -7,6 +7,9 @@ #pragma once +#include +#include + namespace aliceVision { namespace sfmData { @@ -22,5 +25,19 @@ namespace sfm { */ double RMSE(const sfmData::SfMData& sfmData); +/** + * @brief Compute area based score + * score is the ratio of the area of the convex hull of the points over the image area + * @param refPts the reference image points + * @param nextPts the next image points + * @param refWidth the refereince image width + * @param refHeight the reference image height + * @param nextWidth the next image width + * @param nextHeight the next image height + * @return score + * +*/ +double computeAreaScore(const std::vector& refPts, const std::vector& nextPts, size_t refWidth, size_t refHeight, size_t nextWidth, size_t nextHeight); + } // namespace sfm } // namespace aliceVision diff --git a/src/aliceVision/track/tracksUtils.cpp b/src/aliceVision/track/tracksUtils.cpp index d2bb72c07b..fddef0db4c 100644 --- a/src/aliceVision/track/tracksUtils.cpp +++ b/src/aliceVision/track/tracksUtils.cpp @@ -264,5 +264,33 @@ void imageIdInTracks(const TracksMap& tracks, std::set& imagesId) } } +void computeCovisibility(std::map& covisibility, const track::TracksMap& mapTracks) +{ + for (const auto& item : mapTracks) + { + const auto& track = item.second; + + for (auto it = track.featPerView.begin(); it != track.featPerView.end(); it++) + { + Pair p; + p.first = it->first; + + for (auto next = std::next(it); next != track.featPerView.end(); next++) + { + p.second = next->first; + + if (covisibility.find(p) == covisibility.end()) + { + covisibility[p] = 0; + } + else + { + covisibility[p]++; + } + } + } + } +} + } // namespace track } // namespace aliceVision diff --git a/src/aliceVision/track/tracksUtils.hpp b/src/aliceVision/track/tracksUtils.hpp index 9709bdb21f..595ff76d5b 100644 --- a/src/aliceVision/track/tracksUtils.hpp +++ b/src/aliceVision/track/tracksUtils.hpp @@ -141,5 +141,13 @@ void imageIdInTracks(const TracksPerView& tracksPerView, std::set& */ void imageIdInTracks(const TracksMap& tracks, std::set& imagesId); + +/** + * @brief compute the set of pairs of views which shares some observed features + * @param covisibility a map indexed by pair of views and whose values are the number of shared features + * @param mapTracks the input tracks map +*/ +void computeCovisibility(std::map& covisibility, const track::TracksMap& mapTracks); + } // namespace track } // namespace aliceVision diff --git a/src/software/pipeline/main_relativePoseEstimating.cpp b/src/software/pipeline/main_relativePoseEstimating.cpp index de0e016a92..47b8ba4ccc 100644 --- a/src/software/pipeline/main_relativePoseEstimating.cpp +++ b/src/software/pipeline/main_relativePoseEstimating.cpp @@ -30,10 +30,9 @@ #include #include +#include #include -#include -#include #include @@ -49,77 +48,19 @@ using namespace aliceVision; namespace po = boost::program_options; -bool getPoseStructure(Mat4 & T, - std::vector& structure, - std::vector& newVecInliers, - const Mat3& E, - const std::vector& vecInliers, - const camera::IntrinsicBase & cam1, - const camera::IntrinsicBase & cam2, - const std::vector & x1, - const std::vector & x2) -{ - // Find set of analytical solutions - std::vector Ts; - motionFromEssential(E, Ts); - - size_t bestCoundValid = 0; - - for (int it = 0; it < Ts.size(); it++) - { - const Mat4 T1 = Eigen::Matrix4d::Identity(); - const Mat4 & T2 = Ts[it]; - - std::vector points; - std::vector updatedInliers; - - size_t countValid = 0; - for (size_t k = 0; k < vecInliers.size(); ++k) - { - const Vec2& pt1 = x1[vecInliers[k]]; - const Vec2& pt2 = x2[vecInliers[k]]; - - const Vec3 pt3d1 = cam1.toUnitSphere(cam1.removeDistortion(cam1.ima2cam(pt1))); - const Vec3 pt3d2 = cam2.toUnitSphere(cam2.removeDistortion(cam2.ima2cam(pt2))); - - Vec3 X; - multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); - - Vec2 ptValid1 = cam1.project(T1, X.homogeneous(), true); - Vec2 ptValid2 = cam2.project(T2, X.homogeneous(), true); - - Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); - Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); - - if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) - { - continue; - } - - updatedInliers.push_back(vecInliers[k]); - points.push_back(X); - countValid++; - } - - if (countValid > bestCoundValid) - { - bestCoundValid = countValid; - structure = points; - newVecInliers = updatedInliers; - T = Ts[it]; - } - } - - if (newVecInliers.size() < 10) - { - return false; - } - - return true; -} - - - +/** + * @brief Estimate the relative essential matrix between two cameras + * @param E the output Essential matrix + * @param vecInliers the input list of inliers (set of indices in the coordinates vectors) + * @param cam1 the first camera intrinsic object + * @param cam2 the second camera intrinsic object + * @param x1 the observed points coordinates in the first camera + * @param x2 the observed points coordinates in the second camera + * @param randomNumberGenerator a random number generator object shared among objects + * @param maxIterationsCount how many iterations are allowed during ransac + * @param minInliers what is the minimal number of inliers required to consider the estimation successful + * @return true if estimation succeeded +*/ bool robustEssential(Mat3& E, std::vector& vecInliers, const camera::IntrinsicBase & cam1, @@ -149,7 +90,19 @@ bool robustEssential(Mat3& E, return true; } - +/** + * @brief Estimate the relative rortation matrix between two cameras + * @param R the output Rotation matrix + * @param vecInliers the input list of inliers (set of indices in the coordinates vectors) + * @param cam1 the first camera intrinsic object + * @param cam2 the second camera intrinsic object + * @param x1 the observed points coordinates in the first camera + * @param x2 the observed points coordinates in the second camera + * @param randomNumberGenerator a random number generator object shared among objects + * @param maxIterationsCount how many iterations are allowed during ransac + * @param minInliers what is the minimal number of inliers required to consider the estimation successful + * @return true if estimation succeeded +*/ bool robustRotation(Mat3& R, std::vector& vecInliers, const camera::IntrinsicBase & cam1, @@ -179,62 +132,6 @@ bool robustRotation(Mat3& R, return true; } -void computeCovisibility(std::map& covisibility, const track::TracksMap& mapTracks) -{ - for (const auto& item : mapTracks) - { - const auto& track = item.second; - - for (auto it = track.featPerView.begin(); it != track.featPerView.end(); it++) - { - Pair p; - p.first = it->first; - - for (auto next = std::next(it); next != track.featPerView.end(); next++) - { - p.second = next->first; - - if (covisibility.find(p) == covisibility.end()) - { - covisibility[p] = 0; - } - else - { - covisibility[p]++; - } - } - } - } -} - -double computeAreaScore(const std::vector& refPts, const std::vector& nextPts, double refArea, double nextArea) -{ - namespace bg = boost::geometry; - - typedef boost::geometry::model::point point_t; - typedef boost::geometry::model::multi_point mpoint_t; - typedef boost::geometry::model::polygon polygon; - mpoint_t mpt1, mpt2; - - for (int idx = 0; idx < refPts.size(); idx++) - { - const auto& refPt = refPts[idx]; - const auto& nextPt = nextPts[idx]; - - boost::geometry::append(mpt1, point_t(refPt(0), refPt(1))); - boost::geometry::append(mpt2, point_t(nextPt(0), nextPt(1))); - } - - polygon hull1, hull2; - boost::geometry::convex_hull(mpt1, hull1); - boost::geometry::convex_hull(mpt2, hull2); - double area1 = boost::geometry::area(hull1); - double area2 = boost::geometry::area(hull1); - double score = (area1 + area2) / (refArea + nextArea); - - return score; -} - int aliceVision_main(int argc, char** argv) { // command-line parameters @@ -254,21 +151,15 @@ int aliceVision_main(int argc, char** argv) // clang-format off po::options_description requiredParams("Required parameters"); requiredParams.add_options() - ("input,i", po::value(&sfmDataFilename)->required(), - "SfMData file.") - ("tracksFilename,t", po::value(&tracksFilename)->required(), - "Tracks file.") - ("output,o", po::value(&outputDirectory)->required(), - "Path to the output directory."); + ("input,i", po::value(&sfmDataFilename)->required(), "SfMData file.") + ("tracksFilename,t", po::value(&tracksFilename)->required(), "Tracks file.") + ("output,o", po::value(&outputDirectory)->required(),"Path to the output directory."); po::options_description optionalParams("Optional parameters"); optionalParams.add_options() - ("enforcePureRotation,e", po::value(&enforcePureRotation)->default_value(enforcePureRotation), - "Enforce pure rotation in estimation.") - ("rangeStart", po::value(&rangeStart)->default_value(rangeStart), - "Range image index start.") - ("rangeSize", po::value(&rangeSize)->default_value(rangeSize), - "Range size."); + ("enforcePureRotation,e", po::value(&enforcePureRotation)->default_value(enforcePureRotation), "Enforce pure rotation in estimation.") + ("rangeStart", po::value(&rangeStart)->default_value(rangeStart), "Range image index start.") + ("rangeSize", po::value(&rangeSize)->default_value(rangeSize), "Range size."); // clang-format on CmdLine cmdline("AliceVision relativePoseEstimating"); @@ -331,15 +222,18 @@ int aliceVision_main(int argc, char** argv) return EXIT_FAILURE; } + //Compute covisibility for tracks + //This will get the list of pair of views which observe common features ALICEVISION_LOG_INFO("Compute co-visibility"); std::map covisibility; - computeCovisibility(covisibility, tracksHandler.getAllTracks()); + track::computeCovisibility(covisibility, tracksHandler.getAllTracks()); ALICEVISION_LOG_INFO("Process co-visibility"); std::stringstream ss; ss << outputDirectory << "/pairs_" << rangeStart << ".json"; std::ofstream of(ss.str()); + //Output container std::vector reconstructedPairs; double ratioChunk = double(covisibility.size()) / double(sfmData.getViews().size()); @@ -370,7 +264,6 @@ int aliceVision_main(int argc, char** argv) // Build features coordinates matrices const std::size_t n = mapTracksCommon.size(); std::vector refpts, nextpts; - for (const auto& commonItem : mapTracksCommon) { const track::Track & track = commonItem.second; @@ -428,7 +321,9 @@ int aliceVision_main(int argc, char** argv) reconstructed.next = nextImage; Mat4 T; - if (!getPoseStructure(T, structure, vecInliers, E, inliers, *refIntrinsics, *nextIntrinsics, refpts, nextpts)) + if (!estimateTransformStructureFromEssential(T, structure, vecInliers, E, inliers, + *refIntrinsics, *nextIntrinsics, + refpts, nextpts)) { continue; } @@ -446,14 +341,12 @@ int aliceVision_main(int argc, char** argv) } // Compute matched points coverage of image - double areaRef = refIntrinsics->w() * refIntrinsics->h(); - double areaNext = nextIntrinsics->w() * nextIntrinsics->h(); - double areaScore = computeAreaScore(refPtsValid, nextPtsValid, areaRef, areaNext); + const double areaScore = sfm::computeAreaScore(refPtsValid, nextPtsValid, refIntrinsics->w(), refIntrinsics->h(), nextIntrinsics->w(), nextIntrinsics->h()); // Compute ratio of matched points - double iunion = n; - double iinter = vecInliers.size(); - double score = iinter / iunion; + const double iunion = n; + const double iinter = vecInliers.size(); + const double score = iinter / iunion; reconstructed.score = 0.5 * score + 0.5 * areaScore; // Buffered output to avoid lo From 599a4972d4e6c83ae2c8977937472dadb8d8c9c6 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 28 May 2024 14:26:44 +0200 Subject: [PATCH 11/14] Replace R and t with geometry::Pose3 --- .../sfm/pipeline/relativePoses.hpp | 11 +- src/software/pipeline/main_nodalSfM.cpp | 4 +- .../pipeline/main_relativePoseEstimating.cpp | 6 +- .../pipeline/main_sfmBootstraping.cpp | 147 ++++++++---------- 4 files changed, 70 insertions(+), 98 deletions(-) diff --git a/src/aliceVision/sfm/pipeline/relativePoses.hpp b/src/aliceVision/sfm/pipeline/relativePoses.hpp index 1d3c96c9f4..5eb230cf9d 100644 --- a/src/aliceVision/sfm/pipeline/relativePoses.hpp +++ b/src/aliceVision/sfm/pipeline/relativePoses.hpp @@ -16,8 +16,7 @@ struct ReconstructedPair { IndexT reference; IndexT next; - Mat3 R; - Vec3 t; + geometry::Pose3 pose; double score; }; @@ -25,8 +24,8 @@ void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, sfm: { jv = {{"reference", input.reference}, {"next", input.next}, - {"R", boost::json::value_from(SO3::logm(input.R))}, - {"t", boost::json::value_from(input.t)}, + {"R", boost::json::value_from(SO3::logm(input.pose.rotation()))}, + {"t", boost::json::value_from(input.pose.translation())}, {"score", boost::json::value_from(input.score)}}; } @@ -38,8 +37,8 @@ ReconstructedPair tag_invoke(boost::json::value_to_tag, boost ret.reference = boost::json::value_to(obj.at("reference")); ret.next = boost::json::value_to(obj.at("next")); - ret.R = SO3::expm(boost::json::value_to(obj.at("R"))); - ret.t = boost::json::value_to(obj.at("t")); + ret.pose.setRotation(SO3::expm(boost::json::value_to(obj.at("R")))); + ret.pose.setTranslation(boost::json::value_to(obj.at("t"))); ret.score = boost::json::value_to(obj.at("score")); return ret; diff --git a/src/software/pipeline/main_nodalSfM.cpp b/src/software/pipeline/main_nodalSfM.cpp index 37dea6d8a4..a811602c88 100644 --- a/src/software/pipeline/main_nodalSfM.cpp +++ b/src/software/pipeline/main_nodalSfM.cpp @@ -91,7 +91,7 @@ void buildInitialWorld(sfmData::SfMData& sfmData, // Make sure initial camera pose is identity sfmData.setPose(refView, sfmData::CameraPose()); - sfmData.setPose(nextView, sfmData::CameraPose(geometry::Pose3(pair.R, Vec3::Zero()))); + sfmData.setPose(nextView, sfmData::CameraPose(pair.pose)); std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); @@ -115,7 +115,7 @@ void buildInitialWorld(sfmData::SfMData& sfmData, Vec2 nextV = nextTrackItem.coords; Vec3 refP = refIntrinsics->toUnitSphere(refIntrinsics->ima2cam(refIntrinsics->getUndistortedPixel(refV))); - Vec3 tP = pair.R * refP; + Vec3 tP = pair.pose.rotation() * refP; Vec2 nextp = nextIntrinsics->getUndistortedPixel(nextV); Vec2 estp = nextIntrinsics->cam2ima((tP.head(2) / tP(2))); double dist = (nextp - estp).norm(); diff --git a/src/software/pipeline/main_relativePoseEstimating.cpp b/src/software/pipeline/main_relativePoseEstimating.cpp index 47b8ba4ccc..a5f0fdbf7a 100644 --- a/src/software/pipeline/main_relativePoseEstimating.cpp +++ b/src/software/pipeline/main_relativePoseEstimating.cpp @@ -294,8 +294,7 @@ int aliceVision_main(int argc, char** argv) reconstructed.reference = refImage; reconstructed.next = nextImage; - reconstructed.R = R; - reconstructed.t.fill(0); + reconstructed.pose.setRotation(R); } else { @@ -328,8 +327,7 @@ int aliceVision_main(int argc, char** argv) continue; } - reconstructed.R = T.block<3, 3>(0, 0); - reconstructed.t = T.block<3, 1>(0, 3); + reconstructed.pose = geometry::Pose3(T); } // Extract inliers diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp index 20072be0a8..469a207131 100644 --- a/src/software/pipeline/main_sfmBootstraping.cpp +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -44,25 +45,42 @@ using namespace aliceVision; namespace po = boost::program_options; namespace fs = boost::filesystem; -bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pair, const track::TracksMap& tracksMap, const track::TracksPerView & tracksPerView, const double maxDistance, double & resultAngle, std::vector & usedTracks) +/** + * @brief estimate a median angle (parallax) between a reference view and another view + * @param sfmData the input sfmData which contains camera information + * @param referenceViewId the reference view id + * @param otherViewId the other view id + * @param otherTreference the relative pose + * @param tracksMap the input map of tracks + * @param tracksPerView tracks grouped by views + * @param resultAngle the output median angle + * @param usedTracks the list of tracks which were succesfully reconstructed + * @return true +*/ +bool estimatePairAngle(const sfmData::SfMData & sfmData, + const IndexT referenceViewId, + const IndexT otherViewId, + const geometry::Pose3 & otherTreference, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView, + double & resultAngle, + std::vector & usedTracks) { usedTracks.clear(); - const sfmData::View& refView = sfmData.getView(pair.reference); - const sfmData::View& nextView = sfmData.getView(pair.next); + const sfmData::View& refView = sfmData.getView(referenceViewId); + const sfmData::View& nextView = sfmData.getView(otherViewId); std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); aliceVision::track::TracksMap mapTracksCommon; - track::getCommonTracksInImagesFast({pair.reference, pair.next}, tracksMap, tracksPerView, mapTracksCommon); + track::getCommonTracksInImagesFast({referenceViewId, otherViewId}, tracksMap, tracksPerView, mapTracksCommon); const Mat4 T1 = Eigen::Matrix4d::Identity(); - Mat4 T2 = Eigen::Matrix4d::Identity(); - T2.block<3, 3>(0, 0) = pair.R; - T2.block<3, 1>(0, 3) = pair.t; + const Mat4 T2 = otherTreference.getHomogeneous(); - const Eigen::Vector3d c = - pair.R.transpose() * pair.t; + const Eigen::Vector3d c = otherTreference.center(); size_t count = 0; std::vector angles; @@ -70,11 +88,11 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::Reconstructe { const track::Track& track = commonItem.second; - const IndexT refFeatureId = track.featPerView.at(pair.reference).featureId; - const IndexT nextfeatureId = track.featPerView.at(pair.next).featureId; + const IndexT refFeatureId = track.featPerView.at(referenceViewId).featureId; + const IndexT nextfeatureId = track.featPerView.at(otherViewId).featureId; - const Vec2 refpt = track.featPerView.at(pair.reference).coords; - const Vec2 nextpt = track.featPerView.at(pair.next).coords; + const Vec2 refpt = track.featPerView.at(referenceViewId).coords; + const Vec2 nextpt = track.featPerView.at(otherViewId).coords; const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); @@ -113,26 +131,35 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::Reconstructe } -bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pair, const track::TracksMap& tracksMap, const std::vector & usedTracks) +/** + * @brief build an initial sfmData from two views + * @param sfmData the input/output sfmData + * @param referenceViewId the reference view id + * @param otherViewId the other view id + * @param otherTreference the relative pose + * @param tracksMap the input map of tracks + * @param usedTracks the input list of valid tracks + * @return true +*/ +bool buildSfmData(sfmData::SfMData & sfmData, + const IndexT referenceViewId, + const IndexT otherViewId, + const geometry::Pose3 & otherTreference, + const track::TracksMap& tracksMap, + const std::vector & usedTracks) { - const sfmData::View& refView = sfmData.getView(pair.reference); - const sfmData::View& nextView = sfmData.getView(pair.next); + const sfmData::View& refView = sfmData.getView(referenceViewId); + const sfmData::View& nextView = sfmData.getView(otherViewId); std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); - - geometry::Pose3 poseNext; - poseNext.setRotation(pair.R); - poseNext.setTranslation(pair.t); - sfmData::CameraPose cposeNext(poseNext, false); + sfmData::CameraPose cposeNext(otherTreference, false); sfmData.getPoses()[refView.getPoseId()] = sfmData::CameraPose(); sfmData.getPoses()[nextView.getPoseId()] = cposeNext; const Mat4 T1 = Eigen::Matrix4d::Identity(); - Mat4 T2 = Eigen::Matrix4d::Identity(); - T2.block<3, 3>(0, 0) = pair.R; - T2.block<3, 1>(0, 3) = pair.t; + Mat4 T2 = otherTreference.getHomogeneous(); size_t count = 0; std::vector angles; @@ -140,11 +167,11 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai { const track::Track& track = tracksMap.at(trackId); - const track::TrackItem & refItem = track.featPerView.at(pair.reference); - const track::TrackItem & nextItem = track.featPerView.at(pair.next); + const track::TrackItem & refItem = track.featPerView.at(referenceViewId); + const track::TrackItem & nextItem = track.featPerView.at(otherViewId); - const Vec2 refpt = track.featPerView.at(pair.reference).coords; - const Vec2 nextpt = track.featPerView.at(pair.next).coords; + const Vec2 refpt = track.featPerView.at(referenceViewId).coords; + const Vec2 nextpt = track.featPerView.at(otherViewId).coords; const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); @@ -174,8 +201,8 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai nextObs.setScale(nextItem.scale); nextObs.setCoordinates(nextItem.coords); - landmark.getObservations()[pair.reference] = refObs; - landmark.getObservations()[pair.next] = nextObs; + landmark.getObservations()[referenceViewId] = refObs; + landmark.getObservations()[otherViewId] = nextObs; sfmData.getLandmarks()[trackId] = landmark; } @@ -183,58 +210,6 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai return true; } -double computeScore(const track::TracksMap & tracksMap, - const std::vector & usedTracks, - const IndexT viewId, - const size_t maxSize, - const size_t countLevels) -{ - //Compute min and max level such that max(width, height) <= 2 on maxLevel - int maxLevel = std::ceil(std::log2(maxSize)); - int minLevel = std::max(1, maxLevel - int(countLevels)); - int realCountLevels = maxLevel - minLevel + 1; - - std::vector>> uniques(realCountLevels); - - - //Coordinates are considered as integers - //Power of two divide == right binary shift on integers - - for (auto trackId : usedTracks) - { - auto & track = tracksMap.at(trackId); - const Vec2 pt = track.featPerView.at(viewId).coords; - - unsigned int ptx = (unsigned int)(pt.x()); - unsigned int pty = (unsigned int)(pt.y()); - - for (unsigned int shiftLevel = 0; shiftLevel < realCountLevels; shiftLevel++) - { - unsigned int level = minLevel + shiftLevel; - unsigned int lptx = ptx >> level; - unsigned int lpty = pty >> level; - - uniques[shiftLevel].insert(std::make_pair(lptx, lpty)); - } - } - - double sum = 0.0; - for (unsigned int shiftLevel = 0; shiftLevel < realCountLevels; shiftLevel++) - { - int size = uniques[shiftLevel].size(); - if (size <= 1) - { - continue; - } - - //The higher the level, the higher the weight per cell - double w = pow(2.0, shiftLevel); - sum += w * double(size); - } - - return sum; -} - int aliceVision_main(int argc, char** argv) { // command-line parameters @@ -328,7 +303,7 @@ int aliceVision_main(int argc, char** argv) std::vector usedTracks; double angle = 0.0; - if (!estimatePairAngle(sfmData, pair, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), maxEpipolarDistance, angle, usedTracks)) + if (!estimatePairAngle(sfmData, pair.reference, pair.next, pair.pose, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), angle, usedTracks)) { continue; } @@ -344,8 +319,8 @@ int aliceVision_main(int argc, char** argv) int maxref = std::max(vref.getImage().getWidth(), vref.getImage().getHeight()); int maxnext = std::max(vnext.getImage().getWidth(), vnext.getImage().getHeight()); - double refScore = computeScore(tracksHandler.getAllTracks(), usedTracks, pair.reference, maxref, 5); - double nextScore = computeScore(tracksHandler.getAllTracks(), usedTracks, pair.next, maxnext, 5); + double refScore = sfm::ExpansionPolicyLegacy::computeScore(tracksHandler.getAllTracks(), usedTracks, pair.reference, maxref, 5); + double nextScore = sfm::ExpansionPolicyLegacy::computeScore(tracksHandler.getAllTracks(), usedTracks, pair.next, maxnext, 5); double score = std::min(refScore, nextScore) * std::min(10.0, radianToDegree(angle)); @@ -356,8 +331,8 @@ int aliceVision_main(int argc, char** argv) bestUsedTracks = usedTracks; } } - - if (!buildSfmData(sfmData, bestPair, tracksHandler.getAllTracks(), bestUsedTracks)) + + if (!buildSfmData(sfmData, bestPair.reference, bestPair.next, bestPair.pose, tracksHandler.getAllTracks(), bestUsedTracks)) { return EXIT_FAILURE; } From 200dfe0266ee0e3192bf344a158272d13aba6c55 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Tue, 28 May 2024 17:12:45 +0200 Subject: [PATCH 12/14] Remove useless virtuals --- src/aliceVision/camera/Equidistant.hpp | 2 +- src/aliceVision/camera/Pinhole.hpp | 2 +- src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/aliceVision/camera/Equidistant.hpp b/src/aliceVision/camera/Equidistant.hpp index c3e7985027..464aa4443e 100644 --- a/src/aliceVision/camera/Equidistant.hpp +++ b/src/aliceVision/camera/Equidistant.hpp @@ -148,7 +148,7 @@ class Equidistant : public IntrinsicScaleOffsetDisto * @brief how a one pixel change relates to an angular change * @return a value in radians */ - virtual double pixelProbability() const override; + double pixelProbability() const override; protected: double _circleRadius{0.0}; diff --git a/src/aliceVision/camera/Pinhole.hpp b/src/aliceVision/camera/Pinhole.hpp index 9b6b7da2d1..9303cbfb7b 100644 --- a/src/aliceVision/camera/Pinhole.hpp +++ b/src/aliceVision/camera/Pinhole.hpp @@ -119,7 +119,7 @@ class Pinhole : public IntrinsicScaleOffsetDisto * @brief how a one pixel change relates to an angular change * @return a value in radians */ - virtual double pixelProbability() const override; + double pixelProbability() const override; }; } // namespace camera diff --git a/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp index d994a0b50e..40be509faf 100644 --- a/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/LbaPolicyConnexity.hpp @@ -24,7 +24,7 @@ class LbaPolicyConnexity : public LbaPolicy * @param tracksHandler the tracks for this scene * @param views the list of views of interest */ - virtual bool build(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) override; + bool build(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) override; /** * @brief set the max distance in the graph to search for connexity From 2800d71fd4a284f601d8ab21784d10bab2898d18 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Fri, 12 Jul 2024 10:09:09 +0200 Subject: [PATCH 13/14] Rig support --- src/aliceVision/sfm/CMakeLists.txt | 3 ++ .../expanding/ExpansionPostProcess.hpp | 45 ++++++++++++++++ .../expanding/ExpansionPostProcessRig.cpp | 51 +++++++++++++++++++ .../expanding/ExpansionPostProcessRig.hpp | 43 ++++++++++++++++ .../pipeline/expanding/ExpansionProcess.cpp | 19 +++++++ .../pipeline/expanding/ExpansionProcess.hpp | 16 ++++++ src/software/pipeline/main_sfmExpanding.cpp | 15 ++++++ 7 files changed, 192 insertions(+) create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcess.hpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.cpp create mode 100644 src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.hpp diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index 9619ddb9b3..d9889cfd7c 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -32,6 +32,8 @@ set(sfm_files_headers pipeline/expanding/ExpansionHistory.hpp pipeline/expanding/ExpansionChunk.hpp pipeline/expanding/ExpansionIteration.hpp + pipeline/expanding/ExpansionPostProcess.hpp + pipeline/expanding/ExpansionPostProcessRig.hpp pipeline/expanding/ExpansionPolicy.hpp pipeline/expanding/ExpansionPolicyLegacy.hpp pipeline/expanding/ExpansionProcess.hpp @@ -72,6 +74,7 @@ set(sfm_files_sources pipeline/expanding/ExpansionChunk.cpp pipeline/expanding/ExpansionIteration.cpp pipeline/expanding/ExpansionPolicyLegacy.cpp + pipeline/expanding/ExpansionPostProcessRig.cpp pipeline/expanding/ExpansionProcess.cpp pipeline/expanding/ConnexityGraph.cpp pipeline/expanding/LbaPolicyConnexity.cpp diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcess.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcess.hpp new file mode 100644 index 0000000000..45098471ac --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcess.hpp @@ -0,0 +1,45 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +class ExpansionPostProcess +{ +public: + using uptr = std::unique_ptr; + +public: + + /** + * @brief Perform post process for an iteration + * @param sfmData the scene to process + * @param tracksHandler the tracks for this scene + * @return true if the process succeeded + */ + virtual bool process(sfmData::SfMData & sfmData, track::TracksHandler & tracksHandler) = 0; + + /** + * @brief get updated views during process + * @return a set of updated views + */ + const std::set & getUpdatedViews() const + { + return _updatedViews; + } + +protected: + std::set _updatedViews; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.cpp new file mode 100644 index 0000000000..6483d3afc7 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.cpp @@ -0,0 +1,51 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 + +namespace aliceVision { +namespace sfm { + +bool ExpansionPostProcessRig::process(sfmData::SfMData & sfmData, track::TracksHandler & tracksHandler) +{ + RigParams params; + params.useRigConstraint = true; + params.minNbCamerasForCalibration = _minNumberCameras; + _updatedViews.clear(); + + if (sfmData.getRigs().empty()) + { + return false; + } + + /*Calibrate all rigs*/ + int countInit = 0; + for (const auto & [rigId, rigObject] : sfmData.getRigs()) + { + if (rigObject.isInitialized()) + { + continue; + } + + RigSequence sequence(sfmData, rigId, params); + sequence.init(tracksHandler.getTracksPerView()); + sequence.updateSfM(_updatedViews); + + countInit++; + } + + if (countInit == 0) + { + return false; + } + + + return true; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.hpp new file mode 100644 index 0000000000..e3453f26fb --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPostProcessRig.hpp @@ -0,0 +1,43 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 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 sfm { + +class ExpansionPostProcessRig : public ExpansionPostProcess +{ +public: + using uptr = std::unique_ptr; + +public: + + /** + * @brief Perform post process for an iteration + * @param sfmData the scene to process + * @param tracksHandler the tracks for this scene + * @return true if the process succeeded + */ + bool process(sfmData::SfMData & sfmData, track::TracksHandler & tracksHandler) override; + + /** + * @brief update the required number of cameras for rig resection + * param count the desired minimal value + */ + void setMinimalNumberCameras(std::size_t count) + { + _minNumberCameras = count; + } + +private: + std::size_t _minNumberCameras = 20; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp index ef2f616b1d..08edee269d 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.cpp @@ -39,6 +39,25 @@ bool ExpansionProcess::process(sfmData::SfMData & sfmData, track::TracksHandler { return false; } + + if (_postProcessHandler) + { + if (_postProcessHandler->process(sfmData, tracksHandler)) + { + return true; + + if (_iterationHandler->getChunkHandler() == nullptr) + { + return false; + } + + // Perform a new estimation step on modified views + if (!_iterationHandler->getChunkHandler()->process(sfmData, tracksHandler, _postProcessHandler->getUpdatedViews())) + { + return false; + } + } + } } while (sfmData.getPoses().size() != nbPoses); diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp index 2a8cf76c43..cd2f94500b 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionProcess.hpp @@ -12,6 +12,7 @@ #include #include #include +#include namespace aliceVision { namespace sfm { @@ -46,6 +47,7 @@ class ExpansionProcess{ { _historyHandler = expansionHistory; } + /** * brief setup the expansion iteration handler * @param expansionIteration a unique ptr. Ownership will be taken @@ -55,6 +57,15 @@ class ExpansionProcess{ _iterationHandler = std::move(expansionIteration); } + /** + * brief setup the expansion iteration post process handler + * @param expansionPostProcess a unique ptr. Ownership will be taken + */ + void setExpansionIterationPostProcessHandler(ExpansionPostProcess::uptr & expansionPostProcess) + { + _postProcessHandler = std::move(expansionPostProcess); + } + private: /** @@ -81,6 +92,11 @@ class ExpansionProcess{ * Handle iteration prcess */ std::unique_ptr _iterationHandler; + + /** + * Postprocess step + */ + ExpansionPostProcess::uptr _postProcessHandler; }; } // namespace sfm diff --git a/src/software/pipeline/main_sfmExpanding.cpp b/src/software/pipeline/main_sfmExpanding.cpp index ec91cbcc64..c43d46b129 100644 --- a/src/software/pipeline/main_sfmExpanding.cpp +++ b/src/software/pipeline/main_sfmExpanding.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include @@ -51,6 +52,8 @@ int aliceVision_main(int argc, char** argv) double maxReprojectionError = 4.0; bool lockAllIntrinsics = false; int minNbCamerasToRefinePrincipalPoint = 3; + bool useRigConstraint = true; + int minNbCamerasForRigCalibration = 20; int randomSeed = std::mt19937::default_seed; @@ -88,6 +91,8 @@ int aliceVision_main(int argc, char** argv) "If we do not have enough cameras, the principal point in consider is considered in the center of the image. " "If minNbCamerasToRefinePrincipalPoint<=0, the principal point is never refined. " "If minNbCamerasToRefinePrincipalPoint==1, the principal point is always refined.") + ("useRigConstraint", po::value(&useRigConstraint)->default_value(useRigConstraint), "Enable/Disable rig constraint.") + ("rigMinNbCamerasForCalibration", po::value(&minNbCamerasForRigCalibration)->default_value(minNbCamerasForRigCalibration),"Minimal number of cameras to start the calibration of the rig.") ; // clang-format on @@ -189,9 +194,19 @@ int aliceVision_main(int argc, char** argv) expansionIteration->setExpansionPolicyHandler(expansionPolicy); expansionIteration->setExpansionChunkHandler(expansionChunk); + /*sfm::ExpansionPostProcess::uptr expansionPostProcess; + if (useRigConstraint) + { + sfm::ExpansionPostProcessRig::uptr expansionPostProcessTyped = std::make_unique(); + expansionPostProcessTyped->setMinimalNumberCameras(minNbCamerasForRigCalibration); + expansionPostProcess = std::move(expansionPostProcessTyped); + }*/ + + sfm::ExpansionProcess::uptr expansionProcess = std::make_unique(); expansionProcess->setExpansionHistoryHandler(expansionHistory); expansionProcess->setExpansionIterationHandler(expansionIteration); + /*expansionProcess->setExpansionIterationPostProcessHandler(expansionPostProcess);*/ if (!expansionProcess->process(sfmData, tracksHandler)) { From dc3329ab70cd455bc829311ad958689026a65a97 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Mon, 15 Jul 2024 09:37:35 +0200 Subject: [PATCH 14/14] Copyright changes --- src/aliceVision/multiview/essential.cpp | 2 -- src/aliceVision/robustEstimation/NACRansac.hpp | 2 +- src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp | 2 +- .../sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp | 2 +- src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp | 2 +- src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp | 2 +- src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp | 2 +- src/software/pipeline/main_sfmBootstraping.cpp | 2 +- 8 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/aliceVision/multiview/essential.cpp b/src/aliceVision/multiview/essential.cpp index 55c4f81602..73340c6df5 100644 --- a/src/aliceVision/multiview/essential.cpp +++ b/src/aliceVision/multiview/essential.cpp @@ -41,7 +41,6 @@ void motionFromEssential(const Mat3& E, std::vector* Rs, std::vector { Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); Mat3 U = USV.matrixU(); - // Vec3 d = USV.singularValues(); Mat3 Vt = USV.matrixV().transpose(); // Last column of U is undetermined since d = (a a 0). @@ -78,7 +77,6 @@ void motionFromEssential(const Mat3& E, std::vector & Ts) { Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); Mat3 U = USV.matrixU(); - // Vec3 d = USV.singularValues(); Mat3 Vt = USV.matrixV().transpose(); // Last column of U is undetermined since d = (a a 0). diff --git a/src/aliceVision/robustEstimation/NACRansac.hpp b/src/aliceVision/robustEstimation/NACRansac.hpp index 3505b514b8..9fb779d709 100644 --- a/src/aliceVision/robustEstimation/NACRansac.hpp +++ b/src/aliceVision/robustEstimation/NACRansac.hpp @@ -1,5 +1,5 @@ // This file is part of the AliceVision project. -// Copyright (c) 2023 AliceVision contributors. +// Copyright (c) 2024 AliceVision contributors. // Copyright (c) 2012 openMVG contributors. // Copyright (c) 2012, 2013 Lionel MOISAN. // Copyright (c) 2012, 2013 Pascal MONASSE. diff --git a/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp b/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp index bbfaca57db..86394fb69d 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp @@ -1,5 +1,5 @@ // This file is part of the AliceVision project. -// Copyright (c) 2023 AliceVision contributors. +// Copyright (c) 2024 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/. diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp index 36735831b4..6a502f52c9 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp @@ -1,5 +1,5 @@ // This file is part of the AliceVision project. -// Copyright (c) 2023 AliceVision contributors. +// Copyright (c) 2024 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/. diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp index 6f602f3aa5..071e6da59a 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp @@ -1,5 +1,5 @@ // This file is part of the AliceVision project. -// Copyright (c) 2023 AliceVision contributors. +// Copyright (c) 2024 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/. diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp index a804982f52..f68d2ebf95 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp @@ -1,5 +1,5 @@ // This file is part of the AliceVision project. -// Copyright (c) 2023 AliceVision contributors. +// Copyright (c) 2024 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/. diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp index 8a0df4d02d..c31eb1c538 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp @@ -1,5 +1,5 @@ // This file is part of the AliceVision project. -// Copyright (c) 2023 AliceVision contributors. +// Copyright (c) 2024 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/. diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp index 469a207131..f5e429549e 100644 --- a/src/software/pipeline/main_sfmBootstraping.cpp +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -331,7 +331,7 @@ int aliceVision_main(int argc, char** argv) bestUsedTracks = usedTracks; } } - + if (!buildSfmData(sfmData, bestPair.reference, bestPair.next, bestPair.pose, tracksHandler.getAllTracks(), bestUsedTracks)) { return EXIT_FAILURE;