MRMesh/MRICP.h

Go to the documentation of this file.

#pragma once
 
#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
{
    // required to declare explicitly to avoid warnings
    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;
    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]; }
    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, double* inaccuracy = nullptr );
 
[[nodiscard]] MRMESH_API NumSum getSumSqDistToPlane( const IPointPairs& pairs, double* inaccuracy = nullptr);
 
[[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 AffineXf3f 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; // [radians]
 
    float p2plScaleLimit = 2;
 
    float cosThreshold = 0.7f; // in [-1,1]
 
    float distThresholdSq = 1.f; // [distance^2]
 
    float farDistFactor = 3.f; // dimensionless
 
    ICPMode icpMode = ICPMode::AnyRigidXf;
 
    Vector3f fixedRotationAxis;
 
    int iterLimit = 10;
 
    int badIterStopCount = 3;
 
    float exitVal = 0; // [distance]
 
    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 sampleFltPoints( float samplingVoxelSize );
 
    MRMESH_API void sampleRefPoints( float samplingVoxelSize );
 
    void samplePoints( float samplingVoxelSize ) { sampleFltPoints( samplingVoxelSize ); sampleRefPoints( samplingVoxelSize ); }
 
    [[deprecated]] 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; // returns status info string
 
    [[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_();
};
 
} //namespace MR