New logic for sfm

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

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 <aliceVision/sfm/pipeline/expanding/SfmTriangulation.hpp>
+#include <aliceVision/sfm/pipeline/expanding/SfmResection.hpp>
+
+namespace aliceVision {
+namespace sfm {
+
+
+bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set<IndexT> & 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<double>::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<IndexT> 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<IndexT> & viewIds)
+{
+    SfmTriangulation triangulation(_triangulationMinPoints, _maxTriangulationError);
+
+    std::set<IndexT> evaluatedTracks;
+    std::map<IndexT, sfmData::Landmark> 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
+

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 <aliceVision/types.hpp>
+#include <aliceVision/track/TracksHandler.hpp>
+#include <aliceVision/sfmData/SfMData.hpp>
+#include <aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp>
+#include <aliceVision/sfm/pipeline/expanding/SfmBundle.hpp>
+
+namespace aliceVision {
+namespace sfm {
+
+class ExpansionChunk
+{
+public:
+    using uptr = std::unique_ptr<ExpansionChunk>;
+
+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<IndexT> & 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<IndexT> & 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
+

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 <aliceVision/stl/mapUtils.hpp>
+
+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<size_t>(pv.second->getResectionId()));
+    }
+
+    _epoch++;
+
+    return true;
+}
+
+bool ExpansionHistory::beginEpoch(const sfmData::SfMData & sfmData)
+{
+    return true;
+}
+
+void ExpansionHistory::endEpoch(sfmData::SfMData & sfmData, const std::set<IndexT> & selectedViews)
+{
+    // Get a list of valid views
+    std::set<IndexT> 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<camera::IntrinsicScaleOffset> iso = std::dynamic_pointer_cast<camera::IntrinsicScaleOffset>(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<size_t>::max();
+        std::vector<std::pair<size_t, double>> 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<double> cropped;
+        std::vector<double> 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<double> 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
+