SfMData.hpp

// This file is part of the AliceVision project.
// Copyright (c) 2016 AliceVision contributors.
// Copyright (c) 2012 openMVG 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/sfmData/SharedPtrMap.hpp>
#include <aliceVision/sfmData/CameraPose.hpp>
#include <aliceVision/sfmData/Landmark.hpp>
#include <aliceVision/sfmData/Constraint2D.hpp>
#include <aliceVision/sfmData/ConstraintPoint.hpp>
#include <aliceVision/sfmData/RotationPrior.hpp>
#include <aliceVision/sfmData/SurveyPoint.hpp>
#include <aliceVision/sfmData/ImageGroup.hpp>
#include <aliceVision/sfmData/View.hpp>
#include <aliceVision/sfmData/Rig.hpp>
#include <aliceVision/camera/camera.hpp>
#include <aliceVision/types.hpp>
 
#include <stdexcept>
#include <cassert>
#include <random>
 
namespace aliceVision {
namespace sfmData {
 
using Views = SharedPtrMap<View>;
 
using ImageGroups = SharedPtrMap<ImageGroup>;
 
using ImageInfos = SharedPtrMap<ImageInfo>;
 
using Poses = SharedPtrMap<CameraPose>;
 
using Intrinsics = SharedPtrMap<camera::IntrinsicBase>;
 
using Landmarks = std::map<IndexT, Landmark>;
 
using Rigs = std::map<IndexT, Rig>;
 
using PosesUncertainty = std::map<IndexT, Vec6>;
 
using LandmarksUncertainty = std::map<IndexT, Vec3>;
 
using Constraints2D = std::vector<Constraint2D>;
 
using ConstraintsPoint = std::map<IndexT, ConstraintPoint>;
 
using SurveyPoints = std::map<IndexT, std::vector<SurveyPoint>>;
 
using RotationPriors = std::vector<RotationPrior>;
 
 
 
class SfMData
{
  public:
    PosesUncertainty _posesUncertainty;
    LandmarksUncertainty _landmarksUncertainty;
 
    SfMData() = default;
    
    SfMData(const SfMData & other, bool unused);
 
    SfMData(const SfMData & other, const Eigen::Vector3d & bbMin, const Eigen::Vector3d & bbMax);
 
    // Operators
    bool operator==(const SfMData& other) const;
 
    inline bool operator!=(const SfMData& other) const { return !(*this == other); }
 
    // Accessors
 
    const Views& getViews() const { return _views; }
    Views& getViews() { return _views; }
 
    const ImageInfos& getAncestors() const { return _ancestors; }
    ImageInfos& getAncestors() { return _ancestors; }
 
    const Poses& getPoses() const { return _poses; }
    Poses& getPoses() { return _poses; }
 
    const Rigs& getRigs() const { return _rigs; }
    Rigs& getRigs() { return _rigs; }
 
    const Intrinsics& getIntrinsics() const { return _intrinsics; }
    Intrinsics& getIntrinsics() { return _intrinsics; }
 
    const Landmarks& getLandmarks() const { return _landmarks; }
    Landmarks& getLandmarks() { return _landmarks; }
 
    const Constraints2D& getConstraints2D() const { return _constraints2d; }
    Constraints2D& getConstraints2D() { return _constraints2d; }
 
    const ConstraintsPoint& getConstraintsPoint() const { return _constraintsPoint; }
    ConstraintsPoint& getConstraintsPoint() { return _constraintsPoint; }
 
    const SurveyPoints & getSurveyPoints() const { return _surveyPoints; }
    SurveyPoints& getSurveyPoints() { return _surveyPoints; }
 
    const RotationPriors& getRotationPriors() const { return _rotationpriors; }
    RotationPriors& getRotationPriors() { return _rotationpriors; }
 
    const ImageGroups& getImageGroups() const { return _imageGroups; }
    ImageGroups& getImageGroups() { return _imageGroups; }
 
    const std::vector<std::string>& getRelativeFeaturesFolders() const { return _featuresFolders; }
 
    const std::vector<std::string>& getRelativeMatchesFolders() const { return _matchesFolders; }
 
    std::vector<std::string> getFeaturesFolders() const;
 
    std::vector<std::string> getMatchesFolders() const;
 
    std::set<IndexT> getValidViews() const;
 
    std::set<IndexT> getReconstructedIntrinsics() const;
 
    const camera::IntrinsicBase* getIntrinsicPtr(IndexT intrinsicId) const
    {
        if (_intrinsics.count(intrinsicId))
            return _intrinsics.at(intrinsicId).get();
        return nullptr;
    }
 
    camera::IntrinsicBase* getIntrinsicPtr(IndexT intrinsicId)
    {
        if (_intrinsics.count(intrinsicId))
            return _intrinsics.at(intrinsicId).get();
        return nullptr;
    }
 
    std::shared_ptr<camera::IntrinsicBase> getIntrinsicSharedPtr(IndexT intrinsicId)
    {
        if (_intrinsics.count(intrinsicId))
            return _intrinsics.at(intrinsicId);
        return nullptr;
    }
 
    std::shared_ptr<camera::IntrinsicBase> getIntrinsicSharedPtr(const View& v)
    {
        IndexT intrinsicId = v.getIntrinsicId();
 
        if (_intrinsics.count(intrinsicId))
            return _intrinsics.at(intrinsicId);
        return nullptr;
    }
 
    const std::shared_ptr<camera::IntrinsicBase> getIntrinsicSharedPtr(IndexT intrinsicId) const
    {
        if (_intrinsics.count(intrinsicId))
            return _intrinsics.at(intrinsicId);
        return nullptr;
    }
 
    const std::shared_ptr<camera::IntrinsicBase> getIntrinsicSharedPtr(const View& v) const
    {
        IndexT intrinsicId = v.getIntrinsicId();
 
        if (_intrinsics.count(intrinsicId))
            return _intrinsics.at(intrinsicId);
        return nullptr;
    }
 
    const camera::IntrinsicBase& getIntrinsic(IndexT intrinsicId) const { return *(_intrinsics.at(intrinsicId)); }
 
    camera::IntrinsicBase& getIntrinsic(IndexT intrinsicId) { return *(_intrinsics.at(intrinsicId)); }
 
    std::set<IndexT> getViewsKeys() const
    {
        std::set<IndexT> viewKeys;
        for (auto v : _views)
            viewKeys.insert(v.first);
        return viewKeys;
    }
 
    bool isIntrinsicDefined(const View & view) const
    {
        return (view.getIntrinsicId() != UndefinedIndexT  
                && _intrinsics.find(view.getIntrinsicId()) != _intrinsics.end());
    }
 
    bool isIntrinsicDefined(IndexT viewId) const 
    { 
        return isIntrinsicDefined(*_views.at(viewId)); 
    }
 
    bool isPoseDefined(const View & view) const
    {        
        const IndexT poseId = view.getPoseId();
        if (poseId == UndefinedIndexT)
        {
            return false;
        }
        
        auto it = _poses.find(poseId);
        if (it == _poses.end())
        {
            return false;
        }
 
        bool rigValid = ((!view.isPartOfRig() || view.isPoseIndependant() || getRigSubPose(view).status != ERigSubPoseStatus::UNINITIALIZED));
        if (!rigValid)
        {
            return false;
        }
 
        return true;
    }
 
    bool isPoseDefined(IndexT viewId) const 
    { 
        return isPoseDefined(*_views.at(viewId)); 
    }
 
    bool isPoseAndIntrinsicDefined(const View & view) const
    {        
        const IndexT poseId = view.getPoseId();
        if (poseId == UndefinedIndexT)
        {
            return false;
        }
 
        const IndexT intrinsicId = view.getIntrinsicId();
        if (intrinsicId == UndefinedIndexT)
        {
            return false;
        }
 
        if (_intrinsics.find(view.getIntrinsicId()) == _intrinsics.end())
        {
            return false;
        }
        
        auto it = _poses.find(poseId);
        if (it == _poses.end())
        {
            return false;
        }
 
        bool rigValid = ((!view.isPartOfRig() || view.isPoseIndependant() || getRigSubPose(view).status != ERigSubPoseStatus::UNINITIALIZED));
        if (!rigValid)
        {
            return false;
        }
 
        return true;
    }
 
    bool isPoseAndIntrinsicDefined(const View* view) const
    {
        if (view == nullptr)
        {
            return false;
        }
 
        return isPoseAndIntrinsicDefined(*view);
    }
 
    bool isPoseAndIntrinsicDefined(IndexT viewId) const 
    { 
        if (viewId == UndefinedIndexT)
        {
            return false;
        }
 
        if (_views.find(viewId) == _views.end())
        {
            return false;
        }
 
        return isPoseAndIntrinsicDefined(_views.at(viewId).get()); 
    }
 
    bool existsPose(const View& view) const { return (_poses.find(view.getPoseId()) != _poses.end()); }
 
    const View& getView(IndexT viewId) const { return *(_views.at(viewId)); }
 
    View& getView(IndexT viewId) { return *(_views.at(viewId)); }
 
    const View::ptr getViewPtr(IndexT viewId) const { return _views.at(viewId).get(); }
 
    View::ptr getViewPtr(IndexT viewId) { return _views.at(viewId).get(); }
 
    View::sptr getViewSharedPtr(IndexT viewId) { return _views.at(viewId); }
 
    const View::sptr getViewSharedPtr(IndexT viewId) const { return _views.at(viewId); }
 
    IndexT findView(const std::string & imageName) const;
 
    CameraPose getPose(const View& view) const
    {
        // check the view has valid pose / rig etc
        if (!view.isPartOfRig() || view.isPoseIndependant())
        {
            return *_poses.at(view.getPoseId());
        }
 
        // get the pose of the rig
        CameraPose pose = getRigPose(view);
 
        // multiply rig pose by camera subpose
        pose.setTransform(getRigSubPose(view).pose * pose.getTransform());
 
        return pose;
    }
 
    const CameraPose& getAbsolutePose(IndexT poseId) const { return *_poses.at(poseId); }
 
    CameraPose & getAbsolutePose(IndexT poseId) { return *_poses.at(poseId); }
 
    const Rig& getRig(const View& view) const
    {
        assert(view.isPartOfRig());
        return _rigs.at(view.getRigId());
    }
 
    std::set<feature::EImageDescriberType> getLandmarkDescTypes() const
    {
        std::set<feature::EImageDescriberType> output;
        for (auto s : getLandmarks())
        {
            output.insert(s.second.getDescType());
        }
        return output;
    }
 
    std::map<feature::EImageDescriberType, int> getLandmarkDescTypesUsages() const
    {
        std::map<feature::EImageDescriberType, int> output;
        for (auto s : getLandmarks())
        {
            if (output.find(s.second.getDescType()) == output.end())
            {
                output[s.second.getDescType()] = 1;
            }
            else
            {
                ++output[s.second.getDescType()];
            }
        }
        return output;
    }
 
    ExposureSetting getMedianCameraExposureSetting() const
    {
        std::vector<ExposureSetting> cameraExposureList;
        cameraExposureList.reserve(_views.size());
 
        for (const auto& view : _views)
        {
            const ExposureSetting ce = view.second->getImage().getCameraExposureSetting();
            if (ce.isPartiallyDefined())
            {
                auto find = std::find(std::begin(cameraExposureList), std::end(cameraExposureList), ce);
                if (find == std::end(cameraExposureList))
                    cameraExposureList.emplace_back(ce);
            }
        }
 
        std::nth_element(cameraExposureList.begin(), cameraExposureList.begin() + cameraExposureList.size() / 2, cameraExposureList.end());
        const ExposureSetting& ceMedian = cameraExposureList[cameraExposureList.size() / 2];
 
        return ceMedian;
    }
 
    inline void addFeaturesFolder(const std::string& folder) { addFeaturesFolders({folder}); }
 
    void addFeaturesFolders(const std::vector<std::string>& folders);
 
    inline void addMatchesFolder(const std::string& folder) { addMatchesFolders({folder}); }
 
    void addMatchesFolders(const std::vector<std::string>& folders);
 
    inline void setFeaturesFolders(const std::vector<std::string>& folders)
    {
        _featuresFolders.clear();
        addFeaturesFolders(folders);
    }
 
    inline void setMatchesFolders(const std::vector<std::string>& folders)
    {
        _matchesFolders.clear();
        addMatchesFolders(folders);
    }
 
    void setAbsolutePath(const std::string& path);
 
    void setPose(const View& view, const CameraPose& pose);
 
    void setAbsolutePose(IndexT poseId, const CameraPose& pose) 
    { 
        _poses.assign(poseId, pose); 
    }
 
    void erasePose(IndexT poseId, bool noThrow = false)
    {
        auto it = _poses.find(poseId);
        if (it != _poses.end())
        {
            _poses.erase(it);
        }
        else if (!noThrow)
        {
            throw std::out_of_range(std::string("Can't erase unfind pose ") + std::to_string(poseId));
        }
    }
 
    void resetRigs()
    {
        for (auto rigIt : _rigs)
        {
            rigIt.second.reset();
        }
    }
 
    void addAncestor(IndexT ancestorId, std::shared_ptr<ImageInfo> image) { _ancestors.emplace(ancestorId, image); }
 
    void combine(const SfMData& sfmData);
 
    void clear();
 
    void removeUnusedImageGroups();
 
    void removeUnusedIntrinsics();
 
    void removeUnusedCameraPoses();
 
    void removeInvalidObservations();
 
    void removeUnusedLandmarks();
 
    void repair();
 
    void resetParameterStates();
 
    void getBoundingBox(Eigen::Vector3d & bbMin, Eigen::Vector3d & bbMax);
 
    bool isFullyReconstructed() const;
 
  private:
    Landmarks _landmarks;
    Intrinsics _intrinsics;
    Views _views;
    ImageInfos _ancestors;
    std::string _absolutePath;
    std::vector<std::string> _featuresFolders;
    std::vector<std::string> _matchesFolders;
    Poses _poses;
    Rigs _rigs;
    Constraints2D _constraints2d;
    ConstraintsPoint _constraintsPoint;
    RotationPriors _rotationpriors;
    SurveyPoints _surveyPoints;
    ImageGroups _imageGroups;
 
    const CameraPose& getRigPose(const View& view) const 
    { 
        return *_poses.at(view.getPoseId()); 
    }
 
    const RigSubPose& getRigSubPose(const View& view) const
    {
        assert(view.isPartOfRig());
        const Rig& rig = _rigs.at(view.getRigId());
        return rig.getSubPose(view.getSubPoseId());
    }
 
    CameraPose& getRigPose(const View& view) 
    { 
        return *_poses.at(view.getPoseId()); 
    }
 
    RigSubPose& getRigSubPose(const View& view)
    {
        assert(view.isPartOfRig());
        Rig& rig = _rigs.at(view.getRigId());
        return rig.getSubPose(view.getSubPoseId());
    }
};
 
using LandmarkIdSet = std::vector<std::size_t>;
using LandmarksPerView = stl::flat_map<std::size_t, LandmarkIdSet>;
 
LandmarksPerView getLandmarksPerViews(const SfMData& sfmData);
 
}  // namespace sfmData
}  // namespace aliceVision