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 };
    // this parameter indicates maximum number of objects that might be aligned simultaneously in multi-way mode
    // N<0 - means all of the objects
    // N=1 - means that all objects are aligned independently
    // N>1 - means that registration is applied cascade with N grouping step:
    //       1) separate all objects to groups with N members
    //       2) align objects inside these groups
    //       3) join groups and create groups of these joined objects (2nd layer groups)
    //       4) align this groups
    // N>number of objects - same as 0
    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 );
};
 
}