MeshLib
Loading...
Searching...
No Matches
MRMesh/MRICP.h
Go to the documentation of this file.
1#pragma once
2
3#include "MRICPEnums.h"
4#include "MRMeshOrPoints.h"
5#include "MRMatrix3.h"
6#include "MRId.h"
7#include "MRConstants.h"
8#include "MRAffineXf.h"
9#include "MRBitSet.h"
10#include <cfloat>
11
12namespace MR
13{
14
16{
19
22
25
28
30 float distSq = 0.f;
31
33 float weight = 1.f;
34
35 friend bool operator == ( const ICPPairData&, const ICPPairData& ) = default;
36};
37
39struct PointPair : public ICPPairData
40{
43
47
49 float normalsAngleCos = 1.f;
50
52 bool tgtOnBd = false;
53
54 friend bool operator == ( const PointPair&, const PointPair& ) = default;
55};
56
59{
60 // required to declare explicitly to avoid warnings
61 IPointPairs() = default;
62 IPointPairs( const IPointPairs& ) = default;
63 IPointPairs& operator=( const IPointPairs& ) = default;
64 IPointPairs( IPointPairs&& ) noexcept = default;
65 IPointPairs& operator=( IPointPairs&& ) noexcept = default;
66 virtual ~IPointPairs() = default;
67
68 virtual const ICPPairData& operator[]( size_t ) const = 0;
69 virtual ICPPairData& operator[]( size_t ) = 0;
71};
72
73struct PointPairs : public IPointPairs
74{
75 virtual const ICPPairData& operator[]( size_t idx ) const override { return vec[idx]; }
76 virtual ICPPairData& operator[]( size_t idx ) override { return vec[idx]; }
77 std::vector<PointPair> vec;
78};
79
81[[nodiscard]] inline size_t getNumSamples( const IPointPairs& pairs ) { return pairs.active.size(); }
82
84[[nodiscard]] MRMESH_API size_t getNumActivePairs( const IPointPairs& pairs );
85
86struct NumSum
87{
88 int num = 0;
89 double sum = 0;
90
91 friend NumSum operator + ( const NumSum & a, const NumSum & b ) { return { a.num + b.num, a.sum + b.sum }; }
92
93 [[nodiscard]] float rootMeanSqF() const { return ( num <= 0 ) ? FLT_MAX : (float)std::sqrt( sum / num ); }
94};
95
98[[nodiscard]] MRMESH_API NumSum getSumSqDistToPoint( const IPointPairs& pairs, double* inaccuracy = nullptr );
99
102[[nodiscard]] MRMESH_API NumSum getSumSqDistToPlane( const IPointPairs& pairs, double* inaccuracy = nullptr);
103
105[[nodiscard]] inline float getMeanSqDistToPoint( const IPointPairs& pairs ) { return getSumSqDistToPoint( pairs ).rootMeanSqF(); }
106
108[[nodiscard]] inline float getMeanSqDistToPlane( const IPointPairs& pairs ) { return getSumSqDistToPlane( pairs ).rootMeanSqF(); }
109
111[[nodiscard]] MRMESH_API std::string getICPStatusInfo( int iterations, ICPExitType exitType );
112
115 ICPMode mode, float angleLimit, float scaleLimit, const Vector3f & fixedRotationAxis );
116
117
119{
121 ICPMethod method = ICPMethod::PointToPlane;
122
124 float p2plAngleLimit = PI_F / 6.0f; // [radians]
125
127 float p2plScaleLimit = 2;
128
130 float cosThreshold = 0.7f; // in [-1,1]
131
133 float distThresholdSq = 1.f; // [distance^2]
134
137 float farDistFactor = 3.f; // dimensionless
138
140 ICPMode icpMode = ICPMode::AnyRigidXf;
141
144
146 int iterLimit = 10;
147
149 int badIterStopCount = 3;
150
152 float exitVal = 0; // [distance]
153
155 bool mutualClosest = false;
156};
157
159MRMESH_API size_t deactivateFarPairs( IPointPairs& pairs, float maxDistSq );
160
163 const MeshOrPointsXf& src, const MeshOrPointsXf& tgt,
164 float cosThreshold, float distThresholdSq, bool mutualClosest );
165
168class [[nodiscard]] ICP
169{
170public:
178 ICP( const MeshOrPoints& flt, const MeshOrPoints& ref, const AffineXf3f& fltXf, const AffineXf3f& refXf,
179 const VertBitSet& fltSamples = {}, const VertBitSet& refSamples = {} ) : ICP( { flt, fltXf }, { ref, refXf }, fltSamples, refSamples ) {}
180 MRMESH_API ICP( const MeshOrPointsXf& flt, const MeshOrPointsXf& ref, const VertBitSet& fltSamples = {}, const VertBitSet& refSamples = {} );
181
188 MRMESH_API ICP( const MeshOrPoints& flt, const MeshOrPoints& ref, const AffineXf3f& fltXf, const AffineXf3f& refXf,
189 float samplingVoxelSize ) : ICP( { flt, fltXf }, { ref, refXf }, samplingVoxelSize ) {}
190 MRMESH_API ICP( const MeshOrPointsXf& flt, const MeshOrPointsXf& ref, float samplingVoxelSize );
191
193 void setParams(const ICPProperties& prop) { prop_ = prop; }
194 MRMESH_API void setCosineLimit(const float cos);
195 MRMESH_API void setDistanceLimit( const float dist );
196 MRMESH_API void setBadIterCount( const int iter );
197 MRMESH_API void setFarDistFactor(const float factor);
198
200 MRMESH_API void setFltSamples( const VertBitSet& fltSamples );
201 MRMESH_API void sampleFltPoints( float samplingVoxelSize );
202
204 MRMESH_API void setRefSamples( const VertBitSet& refSamples );
205 MRMESH_API void sampleRefPoints( float samplingVoxelSize );
206
208 void samplePoints( float samplingVoxelSize ) { sampleFltPoints( samplingVoxelSize ); sampleRefPoints( samplingVoxelSize ); }
209
210 [[deprecated]] void recomputeBitSet( float fltSamplingVoxelSize ) { sampleFltPoints( fltSamplingVoxelSize ); }
211
213 MRMESH_API void setXfs( const AffineXf3f& fltXf, const AffineXf3f& refXf );
214
216 MRMESH_API void setFloatXf( const AffineXf3f& fltXf );
217
222
225
226 [[nodiscard]] const ICPProperties& getParams() const { return prop_; }
227
228 [[nodiscard]] MRMESH_API std::string getStatusInfo() const; // returns status info string
229
231 [[nodiscard]] size_t getNumSamples() const { return MR::getNumSamples( flt2refPairs_ ) + MR::getNumSamples( ref2fltPairs_ ); }
232
234 [[nodiscard]] size_t getNumActivePairs() const { return MR::getNumActivePairs( flt2refPairs_ ) + MR::getNumActivePairs( ref2fltPairs_ ); }
235
237 [[nodiscard]] MRMESH_API float getMeanSqDistToPoint() const;
238
240 [[nodiscard]] MRMESH_API float getMeanSqDistToPlane() const;
241
243 [[nodiscard]] const PointPairs & getFlt2RefPairs() const { return flt2refPairs_; }
244
246 [[nodiscard]] const PointPairs & getRef2FltPairs() const { return ref2fltPairs_; }
247
251
252private:
253 MeshOrPointsXf flt_;
254 MeshOrPointsXf ref_;
255
256 ICPProperties prop_;
257
258 PointPairs flt2refPairs_;
259 PointPairs ref2fltPairs_;
260
261 ICPExitType resultType_{ ICPExitType::NotStarted };
262
264 void deactivatefarDistPairs_();
265
266 int iter_ = 0;
267 bool p2ptIter_();
268 bool p2plIter_();
269};
270
271} //namespace MR
int VertId
Definition MRDotNet/MRMeshFwd.h:51
constexpr A operator+(A a)
Definition MRImGuiVectorOperators.h:97
#define MRMESH_API
Definition MRMesh/MRMeshFwd.h:46
Definition MRDotNet/MRBitSet.h:39
Definition MRDotNet/MRICP.h:97
container of bits
Definition MRMesh/MRBitSet.h:27
Definition MRMesh/MRICP.h:169
size_t getNumSamples() const
computes the number of samples able to form pairs
Definition MRMesh/MRICP.h:231
MRMESH_API void setCosineLimit(const float cos)
const ICPProperties & getParams() const
Definition MRMesh/MRICP.h:226
MRMESH_API AffineXf3f calculateTransformation()
void setParams(const ICPProperties &prop)
tune algorithm params before run calculateTransformation()
Definition MRMesh/MRICP.h:193
MRMESH_API void sampleRefPoints(float samplingVoxelSize)
MRMESH_API void setFltSamples(const VertBitSet &fltSamples)
select pairs with origin samples on floating object
void samplePoints(float samplingVoxelSize)
select pairs with origin samples on both objects
Definition MRMesh/MRICP.h:208
const PointPairs & getRef2FltPairs() const
returns current pairs formed from samples on reference object and projections on floating object
Definition MRMesh/MRICP.h:246
const PointPairs & getFlt2RefPairs() const
returns current pairs formed from samples on floating object and projections on reference object
Definition MRMesh/MRICP.h:243
MRMESH_API void setFloatXf(const AffineXf3f &fltXf)
sets to-world transformation for the floating object
size_t getNumActivePairs() const
computes the number of active point pairs
Definition MRMesh/MRICP.h:234
MRMESH_API void setRefSamples(const VertBitSet &refSamples)
select pairs with origin samples on reference object
MRMESH_API void sampleFltPoints(float samplingVoxelSize)
MRMESH_API void setBadIterCount(const int iter)
MRMESH_API void updatePointPairs()
recompute point pairs after manual change of transformations or parameters
MRMESH_API void setXfs(const AffineXf3f &fltXf, const AffineXf3f &refXf)
sets to-world transformations both for floating and reference objects
ICP(const MeshOrPoints &flt, const MeshOrPoints &ref, const AffineXf3f &fltXf, const AffineXf3f &refXf, const VertBitSet &fltSamples={}, const VertBitSet &refSamples={})
Definition MRMesh/MRICP.h:178
MRMESH_API ICP(const MeshOrPointsXf &flt, const MeshOrPointsXf &ref, const VertBitSet &fltSamples={}, const VertBitSet &refSamples={})
MRMESH_API float getMeanSqDistToPlane() const
computes root-mean-square deviation from points to target planes
MRMESH_API float getMeanSqDistToPoint() const
computes root-mean-square deviation between points
MRMESH_API ICP(const MeshOrPointsXf &flt, const MeshOrPointsXf &ref, float samplingVoxelSize)
MRMESH_API std::string getStatusInfo() const
MRMESH_API ICP(const MeshOrPoints &flt, const MeshOrPoints &ref, const AffineXf3f &fltXf, const AffineXf3f &refXf, float samplingVoxelSize)
Definition MRMesh/MRICP.h:188
MRMESH_API AffineXf3f autoSelectFloatXf()
MRMESH_API void setDistanceLimit(const float dist)
MRMESH_API void setFarDistFactor(const float factor)
void recomputeBitSet(float fltSamplingVoxelSize)
Definition MRMesh/MRICP.h:210
Definition MRMesh/MRMeshOrPoints.h:17
Definition MRPointToPlaneAligningTransform.h:20
represents a 3-dimentional float-typed vector
Definition MRDotNet/MRVector3.h:8
Definition MRCameraOrientationPlugin.h:8
float getMeanSqDistToPlane(const IPointPairs &pairs)
computes root-mean-square deviation from points to target planes
Definition MRMesh/MRICP.h:108
ICPMode
The group of transformations, each with its own degrees of freedom.
Definition MRICPEnums.h:16
MRMESH_API size_t getNumActivePairs(const IPointPairs &pairs)
computes the number of active pairs
size_t getNumSamples(const IPointPairs &pairs)
returns the number of samples able to form pairs
Definition MRMesh/MRICP.h:81
MRMESH_API std::string getICPStatusInfo(int iterations, ICPExitType exitType)
returns status info string
float getMeanSqDistToPoint(const IPointPairs &pairs)
computes root-mean-square deviation between points
Definition MRMesh/MRICP.h:105
ICPExitType
Definition MRICPEnums.h:25
MRMESH_API AffineXf3f getAligningXf(const PointToPlaneAligningTransform &p2pl, ICPMode mode, float angleLimit, float scaleLimit, const Vector3f &fixedRotationAxis)
given prepared (p2pl) object, finds the best transformation from it of given type with given limitati...
MRMESH_API NumSum getSumSqDistToPlane(const IPointPairs &pairs, double *inaccuracy=nullptr)
ICPMethod
The method how to update transformation from point pairs.
Definition MRICPEnums.h:6
MRMESH_API void updatePointPairs(PointPairs &pairs, const MeshOrPointsXf &src, const MeshOrPointsXf &tgt, float cosThreshold, float distThresholdSq, bool mutualClosest)
in each pair updates the target data and performs basic filtering (activation)
MRMESH_API size_t deactivateFarPairs(IPointPairs &pairs, float maxDistSq)
reset active bit if pair distance is further than maxDistSq
MRMESH_API NumSum getSumSqDistToPoint(const IPointPairs &pairs, double *inaccuracy=nullptr)
Definition MRMesh/MRICP.h:16
friend bool operator==(const ICPPairData &, const ICPPairData &)=default
Vector3f srcNorm
normal in source point after transforming in world space
Definition MRMesh/MRICP.h:21
Vector3f tgtPoint
coordinates of the closest point on target after transforming in world space
Definition MRMesh/MRICP.h:24
Vector3f tgtNorm
normal in the target point after transforming in world space
Definition MRMesh/MRICP.h:27
float weight
weight of the pair (to prioritize over other pairs)
Definition MRMesh/MRICP.h:33
Vector3f srcPoint
coordinates of the source point after transforming in world space
Definition MRMesh/MRICP.h:18
float distSq
squared distance between source and target points
Definition MRMesh/MRICP.h:30
Definition MRMesh/MRICP.h:119
Vector3f fixedRotationAxis
If this vector is not zero then rotation is allowed relative to this axis only.
Definition MRMesh/MRICP.h:143
Simple interface for pairs holder.
Definition MRMesh/MRICP.h:59
IPointPairs(const IPointPairs &)=default
IPointPairs(IPointPairs &&) noexcept=default
BitSet active
whether corresponding pair from vec must be considered during minimization
Definition MRMesh/MRICP.h:70
IPointPairs & operator=(const IPointPairs &)=default
IPointPairs()=default
an object and its transformation to global space with other objects
Definition MRMesh/MRMeshOrPoints.h:97
Definition MRMesh/MRICP.h:87
float rootMeanSqF() const
Definition MRMesh/MRICP.h:93
double sum
Definition MRMesh/MRICP.h:89
int num
Definition MRMesh/MRICP.h:88
Stores a pair of points: one samples on the source and the closest to it on the target.
Definition MRMesh/MRICP.h:40
float normalsAngleCos
cosine between normals in source and target points
Definition MRMesh/MRICP.h:49
VertId srcVertId
id of the source point
Definition MRMesh/MRICP.h:42
friend bool operator==(const PointPair &, const PointPair &)=default
VertId tgtCloseVert
Definition MRMesh/MRICP.h:46
bool tgtOnBd
true if if the closest point on target is located on the boundary (only for meshes)
Definition MRMesh/MRICP.h:52
Definition MRMesh/MRICP.h:74
std::vector< PointPair > vec
vector of all point pairs both active and not
Definition MRMesh/MRICP.h:77
virtual const ICPPairData & operator[](size_t idx) const override
Definition MRMesh/MRICP.h:75
virtual ICPPairData & operator[](size_t idx) override
Definition MRMesh/MRICP.h:76