Global Registration

Example of Global Registration

• C++

  #include <MRMesh/MRBox.h>
  #include <MRMesh/MRMultiwayICP.h>
  #include <MRMesh/MRPointCloud.h>
  #include <MRMesh/MRPointsLoad.h>
  #include <MRMesh/MRPointsSave.h>
   
  #include <iostream>
   
  void printStats( const MR::MultiwayICP& icp )
  {
      std::cout << "Samples: " << icp.getNumSamples() << std::endl
                << "Active point pairs: " << icp.getNumActivePairs() << std::endl;
      if ( icp.getNumActivePairs() > 0 )
      {
          const auto p2ptMetric = icp.getMeanSqDistToPoint();
          const auto p2plMetric = icp.getMeanSqDistToPlane();
          std::cout << "RMS point-to-point distance: " << p2ptMetric << " ± " << icp.getMeanSqDistToPoint( p2ptMetric ) << std::endl
                    << "RMS point-to-plane distance: " << p2plMetric << " ± " << icp.getMeanSqDistToPlane( p2plMetric ) << std::endl;
      }
  }
   
  int main( int argc, char* argv[] )
  {
      if ( argc < 4 )
      {
          std::cerr << "Usage: " << argv[0] << " INPUT1 INPUT2 [INPUTS...] OUTPUT" << std::endl;
          return EXIT_FAILURE;
      }
   
      // the global registration can be applied to meshes and point clouds
      // to simplify the sample app, we will work with point clouds only
      std::vector<MR::PointCloud> inputs;
      // as ICP and MultiwayICP classes accept both meshes and point clouds,
      // the input data must be converted to special wrapper objects
      // NB: the wrapper objects hold *references* to the source data, NOT their copies
      MR::ICPObjects objects;
      MR::Box3f maxBBox;
      for ( auto i = 1; i < argc - 1; ++i )
      {
          auto pointCloud = MR::PointsLoad::fromAnySupportedFormat( argv[i] );
          if ( !pointCloud )
          {
              std::cerr << "Failed to load point cloud: " << pointCloud.error() << std::endl;
              return EXIT_FAILURE;
          }
   
          auto bbox = pointCloud->computeBoundingBox();
          if ( !maxBBox.valid() || bbox.volume() > maxBBox.volume() )
              maxBBox = bbox;
   
          inputs.emplace_back( std::move( *pointCloud ) );
          // you may also set an affine transformation for each input as a second argument
          objects.push_back( { inputs.back(), {} } );
      }
   
      // you can set various parameters for the global registration; see the documentation for more info
      MR::MultiwayICP icp( objects, {
          // set sampling voxel size
          .samplingVoxelSize = maxBBox.diagonal() * 0.03f,
      } );
   
      icp.setParams( {} );
   
      // gather statistics
      icp.updateAllPointPairs();
      printStats( icp );
   
      std::cout << "Calculating transformations..." << std::endl;
      auto xfs = icp.calculateTransformations();
      printStats( icp );
   
      MR::PointCloud output;
      for ( auto i = MR::ObjId( 0 ); i < inputs.size(); ++i )
      {
          const auto& input = inputs[i];
          const auto& xf = xfs[i];
          for ( const auto& point : input.points )
              output.points.emplace_back( xf( point ) );
      }
   
      auto res = MR::PointsSave::toAnySupportedFormat( output, argv[argc - 1] );
      if ( !res )
      {
          std::cerr << "Failed to save point cloud: " << res.error() << std::endl;
          return EXIT_FAILURE;
      }
   
      return EXIT_SUCCESS;
  }

• Python

  import os
   
  import meshlib.mrmeshpy as mrmeshpy
  import sys
   
   
  def print_stats(icp):
      num_active_pairs = icp.getNumActivePairs()
      print(f"Number of samples: {icp.getNumSamples()}")
      print(f"Number of active pairs: {num_active_pairs}")
   
      if num_active_pairs > 0:
          p2pt_metric = icp.getMeanSqDistToPoint()
          p2pt_inaccuracy = icp.getMeanSqDistToPoint(value=p2pt_metric)
          print(f"RMS point-to-point distance: {p2pt_metric} ± {p2pt_inaccuracy}")
   
          p2pl_metric = icp.getMeanSqDistToPlane()
          p2pl_inaccuracy = icp.getMeanSqDistToPlane(p2pt_metric)
          print(f"RMS point-to-plane distance: {p2pl_metric} ± {p2pl_inaccuracy}")
   
  def main(argv):
      if len(argv) < 4:
          print(f"Usage for script: {argv[0]} INPUT1 INPUT2 [INPUTS...] OUTPUT")
          return
      file_args = argv[1:]
   
      # Loading inputs and finding max bounding box
      input_clouds_num = len(file_args) - 1
      inputs = []
      max_bbox = None
      for i in range(input_clouds_num):
          points = mrmeshpy.loadPoints(file_args[i])
          transform = mrmeshpy.AffineXf3f()
          points_with_transform = mrmeshpy.MeshOrPointsXf(points, transform)
          inputs.append(points_with_transform)
   
          bbox = points.getBoundingBox()
          if not max_bbox or bbox.volume() > max_bbox.volume():
              max_bbox = bbox
   
      # ICP initialization
      sampling_params = mrmeshpy.MultiwayICPSamplingParameters()
      sampling_params.samplingVoxelSize = max_bbox.diagonal() * 0.03
   
      inputs_obj = mrmeshpy.Vector_MeshOrPointsXf_ObjId(inputs)
   
      icp = mrmeshpy.MultiwayICP(inputs_obj, sampling_params)
      icp_properties = mrmeshpy.ICPProperties()
      icp.setParams(icp_properties)
   
      icp.updateAllPointPairs()
   
      print("Calculating transformations...")
      xfs = icp.calculateTransformations()
      print_stats(icp)
   
      # Saving result
      output = mrmeshpy.PointCloud()
      for i in range(input_clouds_num):
          transform = xfs.vec_[i]
          for point in inputs[i].obj.points():
              transformed_point = transform(point)
              output.addPoint(transformed_point)
   
      mrmeshpy.PointsSave.toAnySupportedFormat(output, file_args[-1])
   
  main(sys.argv)

• C

  #include <MRCMesh/MRAffineXf.h>
  #include <MRCMesh/MRBox.h>
  #include <MRCMesh/MRICP.h>
  #include <MRCMesh/MRMeshOrPoints.h>
  #include <MRCMesh/MRMultiwayICP.h>
  #include <MRCMesh/MRPointCloud.h>
  #include <MRCMesh/MRPointCloudPart.h>
  #include <MRCMesh/MRPointsLoad.h>
  #include <MRCMesh/MRPointsSave.h>
  #include <MRCMesh/MRString.h>
  #include <MRCMesh/MRVector.h>
  #include <MRCMisc/expected_MR_PointCloud_std_string.h>
  #include <MRCMisc/expected_void_std_string.h>
  #include <MRCMisc/std_function_bool_from_float.h>
  #include <MRCMisc/std_string.h>
   
  #include <stdio.h>
  #include <stdlib.h>
  #include <string.h>
   
  // print progress every 10%
  int gProgress = -1;
  bool onProgress( float v )
  {
      int progress = (int)( 10.f * v );
      if ( progress != gProgress )
      {
          gProgress = progress;
          printf( "%d%%...\n", progress * 10 );
      }
      return true;
  }
   
  void resetProgress( void )
  {
      gProgress = -1;
  }
   
  void printStats( const MR_MultiwayICP* icp )
  {
      size_t numSamples = MR_MultiwayICP_getNumSamples( icp );
      size_t numActivePairs = MR_MultiwayICP_getNumActivePairs( icp );
      printf( "Samples: %zu\n", numSamples );
      printf( "Active point pairs: %zu\n", numActivePairs );
      if ( numActivePairs > 0 )
      {
          double p2ptMetric = MR_MultiwayICP_getMeanSqDistToPoint( icp, NULL );
          double p2ptInaccuracy = MR_MultiwayICP_getMeanSqDistToPoint( icp, &p2ptMetric );
          printf( "RMS point-to-point distance: %f ± %f\n", p2ptMetric, p2ptInaccuracy );
   
          double p2plMetric = MR_MultiwayICP_getMeanSqDistToPlane( icp, NULL );
          double p2plInaccuracy = MR_MultiwayICP_getMeanSqDistToPlane( icp, &p2plMetric );
          printf( "RMS point-to-plane distance: %f ± %f\n", p2plMetric, p2plInaccuracy );
      }
  }
   
  int main( int argc, char* argv[] )
  {
      int rc = EXIT_FAILURE;
      if ( argc < 4 )
      {
          fprintf( stderr, "Usage: %s INPUT1 INPUT2 [INPUTS...] OUTPUT\n", argv[0] );
          goto out;
      }
   
      // the global registration can be applied to meshes and point clouds
      // to simplify the sample app, we will work with point clouds only
   
      // Note that the default ICP method (point-to-plane) relies on the point cloud having the normals information, and will not work otherwise.
      // If your point cloud doesn't have normals, switch to the point-to-point method by calling `MR_ICPProperties_Set_method( params, MR_ICPMethod_PointToPoint );`.
   
      const int inputNum = argc - 2;
   
      // as ICP and MultiwayICP classes accept both meshes and point clouds,
      // the input data must be converted to special wrapper objects
      // NB: The wrapper is non-owning, it holds a reference to an existing mesh or point cloud and doesn't automatically destroy it when the wrapper is destroyed.
      //   So we manually destroy the wrapped contents before destroying the wrappers.
      MR_Vector_MR_MeshOrPointsXf_MR_ObjId* inputs = MR_Vector_MR_MeshOrPointsXf_MR_ObjId_DefaultConstruct();
   
      MR_Box3f maxBBox = MR_Box3f_DefaultConstruct();
   
      for ( int i = 0; i < inputNum; ++i )
      {
          MR_expected_MR_PointCloud_std_string* pointCloudEx = MR_PointsLoad_fromAnySupportedFormat_2( argv[1 + i], NULL, NULL );
          MR_PointCloud* pointCloud = MR_expected_MR_PointCloud_std_string_GetMutableValue( pointCloudEx );
   
          if ( !pointCloud )
          {
              // Failed to load.
              fprintf( stderr, "Failed to load point cloud: %s\n", MR_std_string_Data( MR_expected_MR_PointCloud_std_string_GetError( pointCloudEx ) ) );
              MR_expected_MR_PointCloud_std_string_Destroy( pointCloudEx );
              goto out_inputs;
          }
   
          // Construct the wrapper.
          MR_MeshOrPoints* mop = MR_MeshOrPoints_Construct_MR_PointCloud( pointCloud ); // Or `MR_MeshOrPoints_Construct_MR_Mesh()` for meshes.
          MR_MeshOrPointsXf* mopXf = MR_MeshOrPointsXf_ConstructFrom( mop, MR_AffineXf3f_DefaultConstruct() ); // Here you can optionally specify an affine transformation.
          MR_MeshOrPoints_Destroy( mop );
   
          // Insert the wrapper into the vector.
          MR_Vector_MR_MeshOrPointsXf_MR_ObjId_push_back_MR_MeshOrPointsXf_rvalue_ref( inputs, mopXf );
          MR_MeshOrPointsXf_Destroy( mopXf );
   
          MR_Box3f bbox = MR_PointCloud_computeBoundingBox_1( pointCloud, NULL );
          if ( !MR_Box3f_valid( &maxBBox ) || MR_Box3f_volume( &bbox ) > MR_Box3f_volume( &maxBBox ) )
              maxBBox = bbox;
      }
   
      // you can set various parameters for the global registration; see the documentation for more info
      MR_MultiwayICPSamplingParameters* samplingParams = MR_MultiwayICPSamplingParameters_DefaultConstruct();
      // set sampling voxel size
      MR_MultiwayICPSamplingParameters_Set_samplingVoxelSize( samplingParams, MR_Box3f_diagonal( &maxBBox ) * 0.03f );
      // set progress callback
      MR_std_function_bool_from_float* cb = MR_std_function_bool_from_float_DefaultConstruct();
      MR_std_function_bool_from_float_Assign( cb, onProgress );
      MR_MultiwayICPSamplingParameters_Set_cb( samplingParams, MR_PassBy_Copy, cb );
   
      MR_MultiwayICP* icp = MR_MultiwayICP_Construct( inputs, samplingParams );
      MR_MultiwayICPSamplingParameters_Destroy( samplingParams );
   
      MR_ICPProperties* params = MR_ICPProperties_DefaultConstruct();
      MR_MultiwayICP_setParams( icp, params );
      MR_ICPProperties_Destroy( params );
   
      // gather statistics
      MR_MultiwayICP_updateAllPointPairs( icp, MR_PassBy_DefaultArgument, NULL );
      printStats( icp );
   
      printf( "Calculating transformations...\n" );
      resetProgress();
      MR_Vector_MR_AffineXf3f_MR_ObjId* xfs = MR_MultiwayICP_calculateTransformations( icp, MR_PassBy_Copy, cb );
      printStats( icp );
   
      MR_PointCloud* output = MR_PointCloud_DefaultConstruct();
      for ( int i = 0; i < inputNum; i++ )
      {
          const MR_MeshOrPointsXf* input = MR_Vector_MR_MeshOrPointsXf_MR_ObjId_index_const( inputs, (MR_ObjId){i} );
          const MR_AffineXf3f* xf = MR_Vector_MR_AffineXf3f_MR_ObjId_index_const( xfs, (MR_ObjId){i} );
          const MR_PointCloud* cloud = MR_PointCloudPart_Get_cloud( MR_MeshOrPoints_asPointCloudPart( MR_MeshOrPointsXf_Get_obj( input ) ) );
          const MR_VertCoords* points = MR_PointCloud_Get_points( cloud );
          size_t numPoints = MR_VertCoords_size( points );
          printf("Resulting transform for part %d:\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n\n", i, 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);
          for ( size_t j = 0; j < numPoints; j++ )
          {
              MR_Vector3f point = *MR_VertCoords_index_const( points, (MR_VertId){j} );
              point = MR_AffineXf3f_call( xf, &point );
              MR_PointCloud_addPoint_1( output, &point );
          }
      }
   
      MR_expected_void_std_string* saveEx = MR_PointsSave_toAnySupportedFormat_3( output, argv[argc - 1], NULL, NULL );
      const MR_std_string* saveError = MR_expected_void_std_string_GetError( saveEx );
      if ( !saveError )
      {
          rc = EXIT_SUCCESS;
      }
      else
      {
          fprintf( stderr, "Failed to save point cloud: %s\n", MR_std_string_Data( saveError ) );
      }
      MR_std_string_Destroy( saveError );
   
      MR_PointCloud_Destroy( output );
      MR_Vector_MR_AffineXf3f_MR_ObjId_Destroy( xfs );
      MR_std_function_bool_from_float_Destroy( cb );
      MR_MultiwayICP_Destroy( icp );
  out_inputs: ;
      // As explained earlier, since those wrappers are non-owning, we have to manually destroy the contents.
      size_t numLoadedInputs = MR_Vector_MR_MeshOrPointsXf_MR_ObjId_size( inputs );
      for ( size_t i = 0; i < numLoadedInputs; i++ )
      {
          MR_PointCloud_Destroy( MR_PointCloudPart_Get_cloud( MR_MeshOrPoints_asPointCloudPart( MR_MeshOrPointsXf_Get_obj( MR_Vector_MR_MeshOrPointsXf_MR_ObjId_index( inputs, (MR_ObjId){i} ) ) ) ) );
      }
      MR_Vector_MR_MeshOrPointsXf_MR_ObjId_Destroy( inputs );
  out:
      return rc;
  }

• C#

  using System.Reflection;
  using static MR.DotNet;
   
  public class GlobalRegistrationExample
  {
      static void PrintStats(MultiwayICP icp)
      {
          int numActivePairs = icp.GetNumActivePairs();
          Console.WriteLine($"Number of samples: {icp.GetNumSamples()}");
          Console.WriteLine($"Number of active pairs: {numActivePairs}");
   
          if (numActivePairs > 0)
          {
              double p2ptMetric = icp.GetMeanSqDistToPoint();
              double p2ptInaccuracy = icp.GetMeanSqDistToPoint(p2ptMetric);
              Console.WriteLine($"RMS point-to-point distance: {p2ptMetric} ± {p2ptInaccuracy}");
   
              double p2plMetric = icp.GetMeanSqDistToPlane();
              double p2plInaccuracy = icp.GetMeanSqDistToPlane(p2ptMetric);
              Console.WriteLine($"RMS point-to-plane distance: {p2plMetric} ± {p2plInaccuracy}");
          }
      }
      public static void Run(string[] args)
      {
          if (args.Length < 4)
          {
              Console.WriteLine("Usage: {0} GlobalRegistrationExample INPUT1 INPUT2 [INPUTS...] OUTPUT", Assembly.GetExecutingAssembly().GetName().Name);
              return;
          }
   
          try
          {
              int inputNum = args.Length - 2;
              List<MeshOrPointsXf> inputs = new List<MeshOrPointsXf>(inputNum);
              Box3f maxBBox = new Box3f();
              for (int i = 0; i < inputNum; ++i)
              {
                  MeshOrPointsXf obj = new MeshOrPointsXf(PointsLoad.FromAnySupportedFormat(args[i + 1]), new AffineXf3f());
                  inputs.Add(obj);
                  Box3f bbox = obj.obj.BoundingBox;
                  if (!maxBBox.Valid() || bbox.Volume() > maxBBox.Volume())
                      maxBBox = bbox;
              }
   
              MultiwayICPSamplingParameters samplingParams = new MultiwayICPSamplingParameters();
              samplingParams.samplingVoxelSize = maxBBox.Diagonal() * 0.03f;
   
              MultiwayICP icp = new MultiwayICP(inputs, samplingParams);
              ICPProperties iCPProperties = new ICPProperties();
              icp.SetParams(iCPProperties);
              icp.UpdateAllPointPairs();
              PrintStats(icp);
   
              Console.WriteLine("Calculating transformations...");
              var xfs = icp.CalculateTransformations();
              PrintStats(icp);
              PointCloud output = new PointCloud();
              for (int i = 0; i < inputNum; ++i)
              {
                  var xf = xfs[i];
                  for (int j = 0; j < inputs[i].obj.Points.Count; j++)
                      output.AddPoint(xf.Apply(inputs[i].obj.Points[j]));
              }
   
              PointsSave.ToAnySupportedFormat(output, args[args.Length - 1]);
          }
          catch (Exception e)
          {
              Console.WriteLine("Error: {0}", e.Message);
          }
      }
  }