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;
  }

• C

  #include <MRMeshC/MRBox.h>
  #include <MRMeshC/MRMultiwayICP.h>
  #include <MRMeshC/MRPointCloud.h>
  #include <MRMeshC/MRPointsLoad.h>
  #include <MRMeshC/MRPointsSave.h>
  #include <MRMeshC/MRString.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 MRMultiwayICP* icp )
  {
      size_t numSamples = mrMultiWayICPGetNumSamples( icp );
      size_t numActivePairs = mrMultiWayICPGetNumActivePairs( icp );
      printf( "Samples: %zu\n", numSamples );
      printf( "Active point pairs: %zu\n", numActivePairs );
      if ( numActivePairs > 0 )
      {
          double p2ptMetric = mrMultiWayICPGetMeanSqDistToPoint( icp, NULL );
          double p2ptInaccuracy = mrMultiWayICPGetMeanSqDistToPoint( icp, &p2ptMetric );
          printf( "RMS point-to-point distance: %f ± %f\n", p2ptMetric, p2ptInaccuracy );
   
          double p2plMetric = mrMultiWayICPGetMeanSqDistToPlane( icp, NULL );
          double p2plInaccuracy = mrMultiWayICPGetMeanSqDistToPlane( 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;
      }
   
      // error messages will be stored here
      MRString* errorString = NULL;
   
      // the global registration can be applied to meshes and point clouds
      // to simplify the sample app, we will work with point clouds only
      const int inputNum = argc - 2;
      MRPointCloud** inputs = malloc( sizeof( MRPointCloud* ) * inputNum );
      memset( inputs, 0, sizeof( MRPointCloud* ) * inputNum );
      // 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
      MRMeshOrPointsXf** inputXfs = malloc( sizeof( MRMeshOrPointsXf* ) * inputNum );
      memset( inputXfs, 0, sizeof( MRMeshOrPointsXf* ) * inputNum );
      MRBox3f maxBBox = mrBox3fNew();
      for ( int i = 0; i < inputNum; ++i )
      {
          MRPointsLoadSettings loadSettings = mrPointsLoadSettingsNew();
          inputs[i] = mrPointsLoadFromAnySupportedFormat( argv[1 + i], &loadSettings, &errorString );
          if ( errorString )
          {
              fprintf( stderr, "Failed to load point cloud: %s\n", mrStringData( errorString ) );
              mrStringFree( errorString );
              goto out_inputs;
          }
   
          // you may also set an affine transformation for each input as a second argument
          inputXfs[i] = mrMeshOrPointsXfFromPointCloud( inputs[i], NULL ); // or mrMeshOrPointsXfFromMesh for meshes
   
          MRBox3f bbox = mrPointCloudComputeBoundingBox( inputs[i], NULL );
          if ( !mrBox3fValid( &maxBBox ) || mrBox3fVolume( &bbox ) > mrBox3fVolume( &maxBBox ) )
              maxBBox = bbox;
      }
   
      // you can set various parameters for the global registration; see the documentation for more info
      MRMultiwayICPSamplingParameters samplingParams = mrMultiwayIcpSamplingParametersNew();
      // set sampling voxel size
      samplingParams.samplingVoxelSize = mrBox3fDiagonal( &maxBBox ) * 0.03f;
      // set progress callback
      samplingParams.cb = onProgress;
   
      MRMultiwayICP* icp = mrMultiwayICPNew( (const MRMeshOrPointsXf**)inputXfs, inputNum, &samplingParams );
   
      MRICPProperties params = mrICPPropertiesNew();
      mrMultiwayICPSetParams( icp, &params );
   
      // gather statistics
      mrMultiwayICPUpdateAllPointPairs( icp, NULL );
      printStats( icp );
   
      printf( "Calculating transformations...\n" );
      resetProgress();
      MRVectorAffineXf3f* xfs = mrMultiwayICPCalculateTransformations( icp, onProgress );
      printStats( icp );
   
      MRPointCloud* output = mrPointCloudNew();
      for ( int i = 0; i < inputNum; i++ )
      {
          const MRAffineXf3f* xf = xfs->data + i;
          for ( int j = 0; j < mrPointCloudPointsNum( inputs[i] ); j++ )
          {
              MRVector3f* point = mrPointCloudPointsRef( inputs[i] ) + j;
              mrAffineXf3fApply( xf, point );
              mrPointCloudAddPoint( output, point );
          }
      }
      
      MRSaveSettings saveSettings = mrSaveSettingsNew();
      mrPointsSaveToAnySupportedFormat( output, argv[argc - 1], &saveSettings, &errorString );
      if ( errorString )
      {
          fprintf( stderr, "Failed to save point cloud: %s\n", mrStringData( errorString ) );
          mrStringFree( errorString );
          goto out_output;
      }
   
      rc = EXIT_SUCCESS;
  out_output:
      mrPointCloudFree( output );
  out_xfs:
      mrVectorAffineXf3fFree( xfs );
  out_icp:
      mrMultiwayICPFree( icp );
  out_inputs:
      for ( int i = 0; i < inputNum; i++ )
      {
          mrMeshOrPointsXfFree( inputXfs[i] );
          mrPointCloudFree( inputs[i] );
      }
      free( inputXfs );
      free( 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);
          }
      }
  }

• 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)