Mesh ICP

Example of mesh ICP (finding transformation to match objects)

• C++

  #include <MRMesh/MRBox.h>
  #include <MRMesh/MRICP.h>
  #include <MRMesh/MRMesh.h>
  #include <MRMesh/MRMeshLoad.h>
  #include <MRMesh/MRMeshSave.h>
   
  #include <iostream>
   
  int main()
  {
      // Load meshes
      auto meshFloatingRes = MR::MeshLoad::fromAnySupportedFormat( "meshA.stl" );
      if ( !meshFloatingRes.has_value() )
      {
          std::cerr << meshFloatingRes.error() << std::endl;
          return 1;
      }
      MR::Mesh& meshFloating = *meshFloatingRes;
   
      auto meshFixedRes = MR::MeshLoad::fromAnySupportedFormat( "meshB.stl" );
      if ( !meshFixedRes.has_value() )
      {
          std::cerr << meshFixedRes.error() << std::endl;
          return 1;
      }
      MR::Mesh& meshFixed = *meshFixedRes;
   
      // Prepare ICP parameters
      float diagonal = meshFixed.getBoundingBox().diagonal();
      float icpSamplingVoxelSize = diagonal * 0.01f; // To sample points from object
      MR::ICPProperties icpParams;
      icpParams.distThresholdSq = MR::sqr( diagonal * 0.1f ); // Use points pairs with maximum distance specified
      icpParams.exitVal = diagonal * 0.003f; // Stop when distance reached
   
      // Calculate transformation
      MR::ICP icp(
          MR::MeshOrPoints{ MR::MeshPart{ meshFloating } },
          MR::MeshOrPoints{ MR::MeshPart{ meshFixed } },
          MR::AffineXf3f(), MR::AffineXf3f(),
          icpSamplingVoxelSize );
      icp.setParams( icpParams );
      MR::AffineXf3f xf = icp.calculateTransformation();
   
      // Transform floating mesh
      meshFloating.transform( xf );
   
      // Output information string
      std::string info = icp.getStatusInfo();
      std::cerr << info << std::endl;
   
      // Save result
      if ( auto saveRes = MR::MeshSave::toAnySupportedFormat( meshFloating, "meshA_icp.stl" ); !saveRes )
      {
          std::cerr << saveRes.error() << std::endl;
          return 1;
      }
  }

• Python

  import meshlib.mrmeshpy as mrmeshpy
   
  # Load meshes
  meshFloating = mrmeshpy.loadMesh("meshA.stl")
  meshFixed = mrmeshpy.loadMesh("meshB.stl")
   
  # Prepare ICP parameters
  diagonal = meshFixed.getBoundingBox().diagonal()
  icp_sampling_voxel_size = diagonal * 0.01  # To sample points from object
  icp_params = mrmeshpy.ICPProperties()
  icp_params.distThresholdSq = (diagonal * 0.1) ** 2  # Use points pairs with maximum distance specified
  icp_params.exitVal = diagonal * 0.003  # Stop when this distance reached
   
  # Calculate transformation
  icp = mrmeshpy.ICP(meshFloating, meshFixed,
                     mrmeshpy.AffineXf3f(), mrmeshpy.AffineXf3f(),
                     icp_sampling_voxel_size)
  icp.setParams(icp_params)
  xf = icp.calculateTransformation()
   
  # Transform floating mesh
  meshFloating.transform(xf)
   
  # Output information string
  print(icp.getLastICPInfo())
   
  # Save result
  mrmeshpy.saveMesh(meshFloating, "meshA_icp.stl")

• C

  #include <MRCMesh/MRICP.h>
  #include <MRCMesh/MRMesh.h>
  #include <MRCMesh/MRMeshLoad.h>
  #include <MRCMesh/MRMeshOrPoints.h>
  #include <MRCMesh/MRMeshSave.h>
  #include <MRCMesh/MRString.h>
  #include <MRCMisc/expected_MR_Mesh_std_string.h>
  #include <MRCMisc/expected_void_std_string.h>
  #include <MRCMisc/std_string.h>
   
  #include <stdio.h>
  #include <stdlib.h>
   
  int main( void )
  {
      int rc = EXIT_FAILURE;
   
      // Load meshes:
   
      // First mesh, which will be moved.
      MR_expected_MR_Mesh_std_string* meshFloatingEx = MR_MeshLoad_fromAnySupportedFormat_2( "meshA.stl", NULL, NULL );
      MR_Mesh* meshFloating = MR_expected_MR_Mesh_std_string_value_mut( meshFloatingEx );
      if ( !meshFloating )
      {
          fprintf( stderr, "Failed to load mesh A: %s\n", MR_std_string_data( MR_expected_MR_Mesh_std_string_error( meshFloatingEx ) ) );
          goto fail_mesh_loading_a;
      }
   
      // Second mesh, static.
      MR_expected_MR_Mesh_std_string* meshReferenceEx = MR_MeshLoad_fromAnySupportedFormat_2( "meshB.stl", NULL, NULL );
      MR_Mesh* meshReference = MR_expected_MR_Mesh_std_string_value_mut( meshReferenceEx );
      if ( !meshReference )
      {
          fprintf( stderr, "Failed to load mesh B: %s\n", MR_std_string_data( MR_expected_MR_Mesh_std_string_error( meshReferenceEx ) ) );
          goto fail_mesh_loading_b;
      }
   
      // Prepare ICP parameters
      MR_Box3f bbox = MR_Mesh_computeBoundingBox_1( meshReference, NULL );
      float diagonal = MR_Box3f_diagonal( &bbox );
      float icpSamplingVoxelSize = diagonal * 0.01f; // To sample points from object
      MR_ICPProperties* icpParams = MR_ICPProperties_DefaultConstruct();
      MR_ICPProperties_Set_distThresholdSq( icpParams, diagonal * diagonal * 0.01f ); // Use points pairs with maximum distance specified
      MR_ICPProperties_Set_exitVal( icpParams, diagonal * 0.003f ); // Stop when distance reached
   
      // Calculate transformation
      MR_MeshOrPoints* fltMop = MR_MeshOrPoints_Construct_MR_Mesh( meshFloating );
      MR_MeshOrPoints* refMop = MR_MeshOrPoints_Construct_MR_Mesh( meshFloating );
      MR_MeshOrPointsXf* flt = MR_MeshOrPointsXf_ConstructFrom( fltMop, MR_AffineXf3f_DefaultConstruct() );
      MR_MeshOrPointsXf* ref = MR_MeshOrPointsXf_ConstructFrom( refMop, MR_AffineXf3f_DefaultConstruct() );
      MR_MeshOrPoints_Destroy( fltMop );
      MR_MeshOrPoints_Destroy( refMop );
   
      MR_ICP* icp = MR_ICP_Construct_3( flt, ref, icpSamplingVoxelSize );
      MR_ICP_setParams( icp, icpParams );
      MR_ICPProperties_Destroy( icpParams );
      MR_AffineXf3f xf = MR_ICP_calculateTransformation( icp );
   
      // Transform floating mesh
      MR_Mesh_transform( meshFloating, &xf, NULL );
   
      // Output information string
      MR_std_string* info = MR_ICP_getStatusInfo( icp );
      printf( "%s\n", MR_std_string_data( info ) );
      MR_std_string_Destroy( info );
   
      printf("Final transform:\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n", xf.A.x.x, xf.A.x.y, xf.A.x.z, xf.b.x, xf.A.y.x, xf.A.y.y, xf.A.y.z, xf.b.y, xf.A.z.x, xf.A.z.y, xf.A.z.z, xf.b.z);
   
      MR_ICP_Destroy( icp );
      MR_MeshOrPointsXf_Destroy( flt );
      MR_MeshOrPointsXf_Destroy( ref );
   
      // Save result
      MR_expected_void_std_string* saveEx = MR_MeshSave_toAnySupportedFormat_3( meshFloating, "meshA_icp.stl", NULL, NULL);
      if ( MR_expected_void_std_string_error( saveEx ) )
      {
          fprintf( stderr, "Failed to save mesh: %s\n", MR_std_string_data( MR_expected_void_std_string_error( saveEx ) ) );
          goto fail_save;
      }
   
      rc = EXIT_SUCCESS;
   
  fail_save:
      MR_expected_void_std_string_Destroy( saveEx );
  fail_mesh_loading_b:
      MR_expected_MR_Mesh_std_string_Destroy( meshReferenceEx );
  fail_mesh_loading_a:
      MR_expected_MR_Mesh_std_string_Destroy( meshFloatingEx );
      return rc;
  }

• C#

  public class MeshICPExample
  {
      public static void Run()
      {
          try
          {
              // Load meshes
   
              var mesh_floating = MR.MeshLoad.fromAnySupportedFormat("meshA.stl");
              var mesh_fixed = MR.MeshLoad.fromAnySupportedFormat("meshB.stl");
   
              MR.MeshOrPointsXf mesh_xf_floating = new(mesh_floating, new MR.AffineXf3f());
              MR.MeshOrPointsXf mesh_xf_fixed = new(mesh_fixed, new MR.AffineXf3f());
   
              // Prepare ICP parameters
              float diagonal = mesh_xf_fixed.obj.computeBoundingBox().diagonal();
              float icpSamplingVoxelSize = diagonal * 0.01f; // To sample points from object
              MR.ICPProperties icpParams = new();
              icpParams.distThresholdSq = diagonal * diagonal * 0.01f; // Use points pairs with maximum distance specified
              icpParams.exitVal = diagonal * 0.003f; // Stop when distance reached
   
              // Calculate transformation
              MR.ICP icp = new(mesh_xf_floating, mesh_xf_fixed, icpSamplingVoxelSize);
              icp.setParams(icpParams);
              MR.AffineXf3f xf = icp.calculateTransformation();
   
              // Transform floating mesh
              mesh_floating.transform(xf);
   
              // Output information string
              Console.WriteLine("info {0}", icp.getStatusInfo());
   
              // Save result
              MR.MeshSave.toAnySupportedFormat(mesh_floating, "meshA_icp.stl");
          }
          catch (Exception e)
          {
              Console.WriteLine("Error: {0}", e.Message);
          }
      }
  }