MRICP.h

Go to the documentation of this file.

#pragma once
 
#include "MRPch/MRBindingMacros.h"
#include "MRICPEnums.h"
#include "MRMeshOrPoints.h"
#include "MRMatrix3.h"
#include "MRId.h"
#include "MRConstants.h"
#include "MRAffineXf.h"
#include "MRBitSet.h"
#include <cfloat>
 
namespace MR
{
 
 
struct ICPPairData
{
    Vector3f srcPoint;

    Vector3f srcNorm;

    Vector3f tgtPoint;

    Vector3f tgtNorm;

    float distSq = 0.f;

    float weight = 1.f;
 
    friend bool operator == ( const ICPPairData&, const ICPPairData& ) = default;
};

struct PointPair : public ICPPairData
{
    VertId srcVertId;

    VertId tgtCloseVert;

    float normalsAngleCos = 1.f;

    bool tgtOnBd = false;
 
    friend bool operator == ( const PointPair&, const PointPair& ) = default;
};

struct IPointPairs
{
    IPointPairs() = default;
    IPointPairs( const IPointPairs& ) = default;
    IPointPairs& operator=( const IPointPairs& ) = default;
    IPointPairs( IPointPairs&& ) noexcept = default;
    IPointPairs& operator=( IPointPairs&& ) noexcept = default;
    virtual ~IPointPairs() = default;
 
    virtual const ICPPairData& operator[]( size_t ) const = 0;
    virtual ICPPairData& operator[]( size_t ) = 0;
    virtual size_t size() const = 0;
    BitSet active; 
};
 
struct PointPairs : 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<PointPair> vec; 
};

[[nodiscard]] inline size_t getNumSamples( const IPointPairs& pairs ) { return pairs.active.size(); }

[[nodiscard]] MRMESH_API size_t getNumActivePairs( const IPointPairs& pairs );
 
struct NumSum
{
    int num = 0;
    double sum = 0;
 
    friend NumSum operator + ( const NumSum & a, const NumSum & b ) { return { a.num + b.num, a.sum + b.sum }; }
 
    [[nodiscard]] float rootMeanSqF() const { return ( num <= 0 ) ? FLT_MAX : (float)std::sqrt( sum / num ); }
};

[[nodiscard]] MRMESH_API NumSum getSumSqDistToPoint( const IPointPairs& pairs, std::optional<double> inaccuracy = {} );

[[nodiscard]] MRMESH_API NumSum getSumSqDistToPlane( const IPointPairs& pairs, std::optional<double> inaccuracy = {});

[[nodiscard]] inline float getMeanSqDistToPoint( const IPointPairs& pairs ) { return getSumSqDistToPoint( pairs ).rootMeanSqF(); }

[[nodiscard]] inline float getMeanSqDistToPlane( const IPointPairs& pairs ) { return getSumSqDistToPlane( pairs ).rootMeanSqF(); }

[[nodiscard]] MRMESH_API std::string getICPStatusInfo( int iterations, ICPExitType exitType );

[[nodiscard]] MRMESH_API AffineXf3d getAligningXf( const PointToPlaneAligningTransform & p2pl,
    ICPMode mode, float angleLimit, float scaleLimit, const Vector3f & fixedRotationAxis );

struct ICPProperties
{
    ICPMethod method = ICPMethod::PointToPlane;

    float p2plAngleLimit = PI_F / 6.0f; 

    float p2plScaleLimit = 2;

    float cosThreshold = 0.7f; 

    float distThresholdSq = 1.f; 

    float farDistFactor = 3.f; 

    ICPMode icpMode = ICPMode::AnyRigidXf;

    Vector3f fixedRotationAxis;

    int iterLimit = 10;

    int badIterStopCount = 3;

    float exitVal = 0; 

    bool mutualClosest = false;
};

MRMESH_API size_t deactivateFarPairs( IPointPairs& pairs, float maxDistSq );

MRMESH_API void updatePointPairs( PointPairs& pairs,
    const MeshOrPointsXf& src, const MeshOrPointsXf& tgt,
    float cosThreshold, float distThresholdSq, bool mutualClosest );

class [[nodiscard]] ICP
{
public:
    ICP( const MeshOrPoints& flt, const MeshOrPoints& ref, const AffineXf3f& fltXf, const AffineXf3f& refXf,
        const VertBitSet& fltSamples = {}, const VertBitSet& refSamples = {} ) : ICP( { flt, fltXf }, { ref, refXf }, fltSamples, refSamples ) {}
    MRMESH_API ICP( const MeshOrPointsXf& flt, const MeshOrPointsXf& ref, const VertBitSet& fltSamples = {}, const VertBitSet& refSamples = {} );

    MRMESH_API ICP( const MeshOrPoints& flt, const MeshOrPoints& ref, const AffineXf3f& fltXf, const AffineXf3f& refXf,
        float samplingVoxelSize ) : ICP( { flt, fltXf }, { ref, refXf }, samplingVoxelSize ) {}
    MRMESH_API ICP( const MeshOrPointsXf& flt, const MeshOrPointsXf& ref, float samplingVoxelSize );

    void setParams(const ICPProperties& prop) { prop_ = prop; }
    MRMESH_API void setCosineLimit(const float cos);
    MRMESH_API void setDistanceLimit( const float dist );
    MRMESH_API void setBadIterCount( const int iter );
    MRMESH_API void setFarDistFactor(const float factor);

    MRMESH_API void setFltSamples( const VertBitSet& fltSamples );
    MRMESH_API void sampleFltPoints( float samplingVoxelSize );

    MRMESH_API void setRefSamples( const VertBitSet& refSamples );
    MRMESH_API void sampleRefPoints( float samplingVoxelSize );

    void samplePoints( float samplingVoxelSize ) { sampleFltPoints( samplingVoxelSize ); sampleRefPoints( samplingVoxelSize ); }
 
    [[deprecated]] MR_BIND_IGNORE void recomputeBitSet( float fltSamplingVoxelSize ) { sampleFltPoints( fltSamplingVoxelSize ); }

    MRMESH_API void setXfs( const AffineXf3f& fltXf, const AffineXf3f& refXf );

    MRMESH_API void setFloatXf( const AffineXf3f& fltXf );

    MRMESH_API AffineXf3f autoSelectFloatXf();

    MRMESH_API void updatePointPairs();
 
    [[nodiscard]] const ICPProperties& getParams() const { return prop_; }
 
    [[nodiscard]] MRMESH_API std::string getStatusInfo() const; 

    [[nodiscard]] size_t getNumSamples() const { return MR::getNumSamples( flt2refPairs_ ) + MR::getNumSamples( ref2fltPairs_ ); }

    [[nodiscard]] size_t getNumActivePairs() const { return MR::getNumActivePairs( flt2refPairs_ ) + MR::getNumActivePairs( ref2fltPairs_ ); }

    [[nodiscard]] MRMESH_API float getMeanSqDistToPoint() const;

    [[nodiscard]] MRMESH_API float getMeanSqDistToPlane() const;

    [[nodiscard]] const PointPairs & getFlt2RefPairs() const { return flt2refPairs_; }

    [[nodiscard]] const PointPairs & getRef2FltPairs() const { return ref2fltPairs_; }

    [[nodiscard]] MRMESH_API AffineXf3f calculateTransformation();
 
private:
    MeshOrPointsXf flt_;
    MeshOrPointsXf ref_;
 
    ICPProperties prop_;
 
    PointPairs flt2refPairs_;
    PointPairs ref2fltPairs_;
 
    ICPExitType resultType_{ ICPExitType::NotStarted };

    void deactivatefarDistPairs_();
 
    int iter_ = 0;
    bool p2ptIter_();
    bool p2plIter_();
 
    void calcGen_( float (ICP::*dist)() const, bool (ICP::*iter)() );
    void calcP2Pt_();
    void calcP2Pl_();
    void calcCombined_();
};
 
}