#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_string.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
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: %g ± %g\n", p2ptMetric, p2ptInaccuracy );
double p2plMetric = MR_MultiwayICP_getMeanSqDistToPlane( icp, NULL );
double p2plInaccuracy = MR_MultiwayICP_getMeanSqDistToPlane( icp, &p2plMetric );
printf( "RMS point-to-plane distance: %g ± %g\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;
}
const int inputNum = argc - 2;
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( argv[1 + i], NULL, NULL );
MR_PointCloud*
pointCloud = MR_expected_MR_PointCloud_std_string_value_mut( pointCloudEx );
{
fprintf( stderr, "Failed to load point cloud: %s\n", MR_std_string_data( MR_expected_MR_PointCloud_std_string_error( pointCloudEx ) ) );
MR_expected_MR_PointCloud_std_string_Destroy( pointCloudEx );
goto out_inputs;
}
MR_MeshOrPoints* mop = MR_MeshOrPoints_Construct_MR_PointCloud(
pointCloud );
MR_MeshOrPointsXf* mopXf = MR_MeshOrPointsXf_ConstructFrom( mop, MR_AffineXf3f_DefaultConstruct() );
MR_MeshOrPoints_Destroy( mop );
MR_Vector_MR_MeshOrPointsXf_MR_ObjId_push_back_MR_MeshOrPointsXf_rvalue_ref( inputs, mopXf );
MR_MeshOrPointsXf_Destroy( mopXf );
MR_Box3f bbox = MR_PointCloud_computeBoundingBox(
pointCloud, NULL );
if ( !MR_Box3f_valid( &maxBBox ) || MR_Box3f_volume( &bbox ) > MR_Box3f_volume( &maxBBox ) )
maxBBox = bbox;
}
MR_MultiwayICPSamplingParameters* samplingParams = MR_MultiwayICPSamplingParameters_DefaultConstruct();
MR_MultiwayICPSamplingParameters_Set_samplingVoxelSize( samplingParams, MR_Box3f_diagonal( &maxBBox ) * 0.03f );
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 );
MR_MultiwayICP_updateAllPointPairs( icp, NULL );
printStats( icp );
printf( "Calculating transformations...\n" );
MR_Vector_MR_AffineXf3f_MR_ObjId* xfs = MR_MultiwayICP_calculateTransformations( icp, NULL );
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( inputs, (MR_ObjId){i} );
const MR_AffineXf3f* xf = MR_Vector_MR_AffineXf3f_MR_ObjId_index( 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 );
for ( size_t j = 0; j < numPoints; j++ )
{
MR_Vector3f
point = *MR_VertCoords_index(
points, (MR_VertId){(int)j} );
point = MR_AffineXf3f_call( xf, &point );
MR_PointCloud_addPoint( output, &point );
}
}
MR_expected_void_std_string* saveEx = MR_PointsSave_toAnySupportedFormat( output, argv[argc - 1], NULL, NULL );
const MR_std_string* saveError = MR_expected_void_std_string_error( saveEx );
if ( !saveError )
rc = EXIT_SUCCESS;
else
fprintf( stderr, "Failed to save point cloud: %s\n", MR_std_string_data( saveError ) );
MR_expected_void_std_string_Destroy( saveEx );
MR_PointCloud_Destroy( output );
MR_Vector_MR_AffineXf3f_MR_ObjId_Destroy( xfs );
MR_MultiwayICP_Destroy( icp );
out_inputs: ;
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){(int)i} ) ) ) ) );
}
MR_Vector_MR_MeshOrPointsXf_MR_ObjId_Destroy( inputs );
out:
return rc;
}
const UiRenderParams * params
VertCoords & points() const