MRMultiwayICP.h

Go to the documentation of this file.

#pragma once
#include "MRICP.h"
#include "MRGridSampling.h"
 
namespace MR
{
 
 
using ICPObjects = Vector<MeshOrPointsXf, ObjId>;
 
using ICPLayer = int;
class ICPElemtTag;
using ICPElementId = Id<ICPElemtTag>;
using ICPElementBitSet = TaggedBitSet<ICPElemtTag>;
struct ICPGroupPair : public ICPPairData
{
    ObjVertId srcId;
    ObjVertId tgtClosestId;
};
struct ICPGroupPairs : public IPointPairs
{
    virtual const ICPPairData& operator[]( size_t idx ) const override { return vec[idx]; }
    virtual ICPPairData& operator[]( size_t idx ) override { return vec[idx]; }
    virtual size_t size() const override { return vec.size(); }
    std::vector<ICPGroupPair> vec;
};
 
using ICPGroupProjector = std::function<void( const Vector3f& p, MeshOrPoints::ProjectionResult& res, ObjId& resId )>;
MRMESH_API void updateGroupPairs( ICPGroupPairs& pairs, const ICPObjects& objs,
    ICPGroupProjector srcProjector, ICPGroupProjector tgtProjector,
    float cosThreshold, float distThresholdSq, bool mutualClosest );
 
using ICPPairsGrid = Vector<Vector<ICPGroupPairs, ICPElementId>, ICPElementId>;
 

class IICPTreeIndexer
{
public:
    virtual ~IICPTreeIndexer() = default;
    virtual bool fromSameNode( ICPLayer l, ICPElementId eI, ICPElementId eJ ) const = 0;

    virtual ObjBitSet getElementLeaves( ICPLayer l, ICPElementId eId ) const = 0;
    virtual ICPElementBitSet getElementNodes( ICPLayer l, ICPElementId eId ) const = 0;

    virtual size_t getNumElements( ICPLayer l ) const = 0;
    virtual size_t getNumLayers() const = 0;
};

struct MultiwayICPSamplingParameters
{
    float samplingVoxelSize = 0;

    int maxGroupSize = 64;
 
    enum class CascadeMode
    {
        Sequential, 
        AABBTreeBased 
    } cascadeMode{ CascadeMode::AABBTreeBased };

    ProgressCallback cb;
};

class MRMESH_CLASS MultiwayICP
{
public:
    MRMESH_API MultiwayICP( const ICPObjects& objects, const MultiwayICPSamplingParameters& samplingParams );

    [[nodiscard]] MRMESH_API Vector<AffineXf3f, ObjId> calculateTransformations( const ProgressCallback& cb = {} );

    [[nodiscard]] MRMESH_API Vector<AffineXf3f, ObjId> calculateTransformationsFixFirst( const ProgressCallback& cb = {} );

    MRMESH_API bool resamplePoints( const MultiwayICPSamplingParameters& samplingParams );

    MRMESH_API bool updateAllPointPairs( const ProgressCallback& cb = {} );

    void setParams( const ICPProperties& prop ) { prop_ = prop; }
    [[nodiscard]] const ICPProperties& getParams() const { return prop_; }

    [[nodiscard]] MRMESH_API float getMeanSqDistToPoint( std::optional<double> value = {} ) const;

    [[nodiscard]] MRMESH_API float getMeanSqDistToPlane( std::optional<double> value = {} ) const;

    [[nodiscard]] MRMESH_API size_t getNumSamples() const;

    [[nodiscard]] MRMESH_API size_t getNumActivePairs() const;

    void setPerIterationCallback( std::function<void( int inter )> callback ) { perIterationCb_ = std::move( callback ); }

    bool devIndependentEquationsModeEnabled() const { return maxGroupSize_ == 1; }
    void devEnableIndependentEquationsMode( bool on ) { maxGroupSize_ = on ? 1 : 0; }

    [[nodiscard]] MRMESH_API std::string getStatusInfo() const;
 
    using PairsPerLayer = Vector<ICPPairsGrid, ICPLayer>;
    const PairsPerLayer& getPairsPerLayer() const { return pairsGridPerLayer_; }

    const IICPTreeIndexer* getCascadeIndexer() const { return cascadeIndexer_.get(); }
private:
    ICPObjects objs_;
    PairsPerLayer pairsGridPerLayer_;
    ICPProperties prop_;
 
    ICPExitType resultType_{ ICPExitType::NotStarted };
 
    std::function<void( int )> perIterationCb_;
 
    std::unique_ptr<IICPTreeIndexer> cascadeIndexer_;

    void setupLayers_( MultiwayICPSamplingParameters::CascadeMode mode );

    bool reservePairsLayer0_( Vector<VertBitSet, ObjId>&& samples, const ProgressCallback& cb );
 
    using LayerSamples = Vector<Vector<MultiObjsSamples, ICPElementId>, ICPLayer>;
    std::optional<LayerSamples> resampleUpperLayers_( const ProgressCallback& cb );
    bool reserveUpperLayerPairs_( LayerSamples&& samples, const ProgressCallback& cb );

    bool updateLayerPairs_( ICPLayer l, const ProgressCallback& cb = {} );
    void deactivateFarDistPairs_( ICPLayer l );
 
    float samplingSize_{ 0.0f };
    int maxGroupSize_{ 64 };
    int iter_ = 0;
    bool doIteration_( bool p2pl );
    bool p2ptIter_();
    bool p2plIter_();
    bool multiwayIter_( bool p2pl = true );
    bool cascadeIter_( bool p2pl = true );
 
    void calcGen_( bool p2pl, const ProgressCallback& cb );
    void calcP2Pt_( const ProgressCallback& cb );
    void calcP2Pl_( const ProgressCallback& cb );
    void calcCombined_( const ProgressCallback& cb );
};
 
}