MultiwayICP::calculateTransformationsFixFirst (#3383)

Co-authored-by: Grant Karapetyan <grant.karapetyan@meshinspector.com>
MeshInspector · Sep 18, 2024 · 81285b8 · 81285b8
1 parent b3e249b
commit 81285b8
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 0 deletions.
diff --git a/source/MRMesh/MRMultiwayICP.cpp b/source/MRMesh/MRMultiwayICP.cpp
@@ -286,6 +286,7 @@ MultiwayICP::MultiwayICP( const ICPObjects& objects, const MultiwayICPSamplingPa
 
 Vector<AffineXf3f, ObjId> MultiwayICP::calculateTransformations( ProgressCallback cb )
 {
+    MR_TIMER
     float minDist = std::numeric_limits<float>::max();
     int badIterCount = 0;
     resultType_ = ICPExitType::MaxIterations;
@@ -339,6 +340,29 @@ Vector<AffineXf3f, ObjId> MultiwayICP::calculateTransformations( ProgressCallbac
     return res;
 }
 
+Vector<AffineXf3f, ObjId> MultiwayICP::calculateTransformationsFixFirst( ProgressCallback cb )
+{
+    Vector<AffineXf3f, ObjId> res;
+    if ( objs_.empty() )
+        return res;
+
+    const auto xf0 = objs_[ObjId( 0 )].xf;
+    res = calculateTransformations( cb );
+
+    /// apply the same (updateXf) to all objects to restore transformation of first object,
+    /// and make relative position of others the same
+    assert( res[ObjId( 0 )] == objs_[ObjId( 0 )].xf );
+    const auto updateXf = xf0 * res[ObjId( 0 )].inverse();
+    res[ObjId( 0 )] = objs_[ObjId( 0 )].xf = xf0;
+    for ( int i = 1; i < objs_.size(); ++i )
+    {
+        assert( res[ObjId( i )] == objs_[ObjId( i )].xf );
+        res[ObjId( i )] = objs_[ObjId( i )].xf = updateXf * res[ObjId( i )];
+    }
+
+    return res;
+}
+
 bool MultiwayICP::resamplePoints( const MultiwayICPSamplingParameters& samplingParams )
 {
     MR_TIMER;

diff --git a/source/MRMesh/MRMultiwayICP.h b/source/MRMesh/MRMultiwayICP.h
@@ -85,8 +85,14 @@ class MRMESH_CLASS MultiwayICP
 
     /// runs ICP algorithm given input objects, transformations, and parameters;
     /// \return adjusted transformations of all objects to reach registered state
+    /// the transformation of the last object is fixed and does not change here
     [[nodiscard]] MRMESH_API Vector<AffineXf3f, ObjId> calculateTransformations( ProgressCallback cb = {} );
 
+    /// runs ICP algorithm given input objects, transformations, and parameters;
+    /// \return adjusted transformations of all objects to reach registered state
+    /// the transformation of the first object is fixed and does not change here
+    [[nodiscard]] MRMESH_API Vector<AffineXf3f, ObjId> calculateTransformationsFixFirst( ProgressCallback cb = {} );
+
     /// select pairs with origin samples on all objects
     MRMESH_API bool resamplePoints( const MultiwayICPSamplingParameters& samplingParams );