Adding libpointmatcher's Python bindings

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.11)
+cmake_minimum_required(VERSION 2.8.12)
 
 include(CheckSymbolExists)
 
@@ -446,6 +446,13 @@ if (BUILD_TESTS)
 	add_subdirectory(utest)
 endif()
 
+# ========================= Python module =============================
+option(BUILD_PYTHON_MODULE "Build the python module for libpointmatcher" OFF)
+
+if (BUILD_PYTHON_MODULE)
+	add_subdirectory(python)
+endif()
+
 #=================== allow find_package() =========================
 #
 # the following case be used in an external project requiring libpointmatcher:

examples/python/icp.py

@@ -0,0 +1,60 @@
+import numpy as np
+
+from pypointmatcher.pointmatcher import PointMatcher
+from examples.python.utils import parse_translation, parse_rotation
+
+PM = PointMatcher
+DP = PM.DataPoints
+
+is_3D = True
+config_file = "../data/default.yaml"
+output_base_file = "tests_output/icp/"
+init_translation = "1,1,1" if is_3D else "1,1"
+init_rotation = "1,0,0;0,1,0;0,0,1" if is_3D else "1,0;0,1"
+
+
+if is_3D:
+    # 3D point clouds
+    ref = DP.load('../data/car_cloud400.csv')
+    data = DP.load('../data/car_cloud401.csv')
+else:
+    # 2D point clouds
+    ref = DP.load('../data/2D_twoBoxes.csv')
+    data = DP.load('../data/2D_oneBox.csv')
+
+icp = PM.ICP()
+
+icp.loadFromYaml(config_file)
+# icp.setDefault()
+
+cloud_dimension = ref.getEuclideanDim()
+
+assert cloud_dimension == 2 or cloud_dimension == 3, "Invalid input point clouds dimension"
+
+translation = parse_translation(init_translation, cloud_dimension)
+rotation = parse_rotation(init_rotation, cloud_dimension)
+init_transfo = np.matmul(translation, rotation)
+
+rigid_trans = PM.get().TransformationRegistrar.create("RigidTransformation")
+
+if not rigid_trans.checkParameters(init_transfo):
+    print("Initial transformations is not rigid, identiy will be used")
+    init_transfo = np.identity(cloud_dimension + 1)
+
+initialized_data = rigid_trans.compute(data, init_transfo)
+
+T = icp(initialized_data, ref)
+
+data_out = DP(initialized_data)
+icp.transformations.apply(data_out, T)
+
+print(f"ICP transformation:\n{T}")
+
+if cloud_dimension == 2:
+    ref.save(output_base_file + "2D_tests_ref.vtk")
+    data.save(output_base_file + "2D_tests_data_in.vtk")
+    data_out.save(output_base_file + "2D_tests_data_out.vtk")
+elif cloud_dimension == 3:
+    ref.save(output_base_file + "3D_tests_ref.vtk")
+    data.save(output_base_file + "3D_tests_data_in.vtk")
+    data_out.save(output_base_file + "3D_tests_data_out.vtk")

examples/python/icp_advance_api.py

@@ -0,0 +1,138 @@
+import numpy as np
+import pypointmatcher
+
+from math import sqrt
+from pypointmatcher import pointmatcher
+from examples.python.utils import parse_translation, parse_rotation
+
+PM = pointmatcher.PointMatcher
+DP = PM.DataPoints
+Parameters = pypointmatcher.map_string_Parameter
+
+#  Code example for ICP taking 2 points clouds (2D or 3D) relatively close
+#  and computing the transformation between them.
+#
+#  This code is more complete than icp_simple. It can load parameter files and has more options.
+is_3D = True
+config_file = "../data/default.yaml"
+output_base_file = "tests_output/icp_advance_api/"
+init_translation = "1,1,1" if is_3D else "1,1"
+init_rotation = "1,0,0;0,1,0;0,0,1" if is_3D else "1,0;0,1"
+
+if is_3D:
+    # 3D point clouds
+    ref = DP.load('../data/car_cloud400.csv')
+    data = DP.load('../data/car_cloud401.csv')
+else:
+    # 2D point clouds
+    ref = DP.load('../data/2D_twoBoxes.csv')
+    data = DP.load('../data/2D_oneBox.csv')
+
+icp = PM.ICP()
+
+# icp.setDefault()
+icp.loadFromYaml(config_file)
+
+cloud_dimension = ref.getEuclideanDim()
+
+assert cloud_dimension == 2 or cloud_dimension == 3, "Invalid input point clouds dimension"
+
+translation = parse_translation(init_translation, cloud_dimension)
+rotation = parse_rotation(init_rotation, cloud_dimension)
+init_transfo = np.matmul(translation, rotation)
+
+rigid_trans = PM.get().TransformationRegistrar.create("RigidTransformation")
+
+if not rigid_trans.checkParameters(init_transfo):
+    print("Initial transformations is not rigid, identiy will be used")
+    init_transfo = np.identity(cloud_dimension + 1)
+
+initialized_data = rigid_trans.compute(data, init_transfo)
+
+T = icp(initialized_data, ref)
+match_ratio = icp.errorMinimizer.getWeightedPointUsedRatio()
+print(f"match ratio: {match_ratio}")
+print("\n------------------")
+
+data_out = DP(initialized_data)
+icp.transformations.apply(data_out, T)
+
+#  START demo 1
+#  Test for retrieving Haussdorff distance (with outliers). We generate new matching module
+#  specifically for this purpose.
+#
+#  INPUTS:
+#  ref: point cloud used as reference
+#  data_out: aligned point cloud (using the transformation outputted by icp)
+#  icp: icp object used to aligned the point clouds
+
+params = Parameters()
+params["knn"] = "1"
+params["epsilon"] = "0"
+
+matcher_Hausdorff = PM.get().MatcherRegistrar.create("KDTreeMatcher", params)
+
+# max. distance from reading to reference
+matcher_Hausdorff.init(ref)
+matches = matcher_Hausdorff.findClosests(data_out)
+max_dist1 = matches.getDistsQuantile(1.0)
+max_dist_robust1 = matches.getDistsQuantile(0.85)
+
+# max. distance from reference to reading
+matcher_Hausdorff.init(data_out)
+matches = matcher_Hausdorff.findClosests(ref)
+max_dist2 = matches.getDistsQuantile(1.0)
+max_dist_robust2 = matches.getDistsQuantile(0.85)
+
+haussdorff_dist = max(max_dist1, max_dist2)
+haussdorff_quantile_dist = max(max_dist_robust1, max_dist_robust2)
+
+print(f"Haussdorff distance: {sqrt(haussdorff_dist)} m")
+print(f"Haussdorff quantile distance: {sqrt(haussdorff_quantile_dist)} m")
+
+#  START demo 2
+#  Test for retrieving paired point mean distance without outliers.
+#  We reuse the same module used for the icp object.
+#
+#  INPUTS:
+#  ref: point cloud used as reference
+#  data_out: aligned point cloud (using the transformation outputted by icp)
+#  icp: icp object used to aligned the point clouds
+
+# initiate the matching with unfiltered point cloud
+icp.matcher.init(ref)
+
+# extract closest points
+matches = icp.matcher.findClosests(data_out)
+
+# weight paired points
+outlier_weights = icp.outlierFilters.compute(data_out, ref, matches)
+
+# generate tuples of matched points and remove pairs with zero weight
+matched_points = PM.ErrorMinimizer.ErrorElements(data_out, ref, outlier_weights, matches)
+
+# extract relevant information for convenience
+dim = matched_points.reading.getEuclideanDim()
+nb_matched_points = matched_points.reading.getNbPoints()
+matched_read = matched_points.reading.features[:dim]
+matched_ref = matched_points.reference.features[:dim]
+
+# compute mean distance
+dist = np.linalg.norm(matched_read - matched_ref, axis=0)
+mean_dist = dist.sum() / nb_matched_points
+
+print(f"Robust mean distance: {mean_dist} m")
+print("------------------\n")
+
+# End demo
+
+print(f"ICP transformation:\n{T}")
+
+if cloud_dimension == 2:
+    ref.save(output_base_file + "2D_tests_ref.vtk")
+    data.save(output_base_file + "2D_tests_data_in.vtk")
+    data_out.save(output_base_file + "2D_tests_data_out.vtk")
+elif cloud_dimension == 3:
+    ref.save(output_base_file + "3D_tests_ref.vtk")
+    data.save(output_base_file + "3D_tests_data_in.vtk")
+    data_out.save(output_base_file + "3D_tests_data_out.vtk")

examples/python/icp_customized.py

@@ -0,0 +1,122 @@
+from pypointmatcher import pointmatchersupport, map_string_Parameter
+from pypointmatcher.pointmatcher import PointMatcher
+
+PM = PointMatcher
+DP = PM.DataPoints
+Parameters = map_string_Parameter
+output_base_file = "tests_output/icp_customized/"
+
+# Load point clouds
+# ref = DP.load('../data/2D_twoBoxes.csv')
+# data = DP.load('../data/2D_oneBox.csv')
+
+ref = DP.load('../data/car_cloud400.csv')
+data = DP.load('../data/car_cloud401.csv')
+
+# Create the default ICP algrotithm
+icp = PointMatcher.ICP()
+params = Parameters()
+
+pointmatchersupport.setLogger(PM.get().LoggerRegistrar.create("FileLogger"))
+
+# Prepare reading filters
+name = "MinDistDataPointsFilter"
+params["minDist"] = "1.0"
+minDist_read = PM.get().DataPointsFilterRegistrar.create(name, params)
+params = Parameters()
+
+name = "RandomSamplingDataPointsFilter"
+params["prob"] = "0.05"
+rand_read = PM.get().DataPointsFilterRegistrar.create(name, params)
+params = Parameters()
+
+# Prepare reference filters
+name = "MinDistDataPointsFilter"
+params["minDist"] = "1.0"
+minDist_ref = PM.get().DataPointsFilterRegistrar.create(name, params)
+params = Parameters()
+
+name = "RandomSamplingDataPointsFilter"
+params["prob"] = "0.05"
+rand_ref = PM.get().DataPointsFilterRegistrar.create(name, params)
+params = Parameters()
+
+# Prepare matching function
+name = "KDTreeMatcher"
+params["knn"] = "1"
+params["epsilon"] = "3.16"
+kdtree = PM.get().MatcherRegistrar.create(name, params)
+params = Parameters()
+
+# Prepare outlier filters
+name = "TrimmedDistOutlierFilter"
+params["ratio"] = "0.75"
+trim = PM.get().OutlierFilterRegistrar.create(name, params)
+params = Parameters()
+
+# Prepare error minimization
+name = "PointToPointErrorMinimizer"
+pointToPoint = PM.get().ErrorMinimizerRegistrar.create(name)
+
+# Prepare transformation checker filters
+name = "CounterTransformationChecker"
+params["maxIterationCount"] = "150"
+maxIter = PM.get().TransformationCheckerRegistrar.create(name, params)
+params = Parameters()
+
+name = "DifferentialTransformationChecker"
+params["minDiffRotErr"] = "0.001"
+params["minDiffTransErr"] = "0.01"
+params["smoothLength"] = "4"
+diff = PM.get().TransformationCheckerRegistrar.create(name, params)
+params = Parameters()
+
+# Prepare inspector
+# toggle to write vtk files per iteration
+name = "NullInspector"
+nullInspect = PM.get().InspectorRegistrar.create(name)
+
+# uncomment to write vtk files per iteration
+# name = "VTKFileInspector"
+# params["dumpDataLinks"] = "1"
+# params["dumpReading"] = "1"
+# params["dumpReference"] = "1"
+# vtkInspect = PM.get().InspectorRegistrar.create(name, params)
+# params = Parametrizable.Parameters()
+
+# Prepare transformation
+name = "RigidTransformation"
+rigid_trans = PM.get().TransformationRegistrar.create(name)
+
+# Build ICP solution
+icp.readingDataPointsFilters.append(minDist_read)
+icp.readingDataPointsFilters.append(rand_read)
+
+icp.referenceDataPointsFilters.append(minDist_ref)
+icp.referenceDataPointsFilters.append(rand_ref)
+
+icp.matcher = kdtree
+
+icp.outlierFilters.append(trim)
+
+icp.errorMinimizer = pointToPoint
+
+icp.transformationCheckers.append(maxIter)
+icp.transformationCheckers.append(diff)
+
+# toggle to write vtk files per iteration
+icp.inspector = nullInspect
+# icp.inspector = vtkInspect
+
+icp.transformations.append(rigid_trans)
+
+T = icp(data, ref)
+
+data_out = DP(data)
+icp.transformations.apply(data_out, T)
+
+ref.save(output_base_file + "test_ref.vtk")
+data.save(output_base_file + "test_data_in.vtk")
+data_out.save(output_base_file + "test_data_out.vtk")
+
+print(T)

examples/python/icp_simple.py

@@ -0,0 +1,44 @@
+from pypointmatcher.pointmatcher import PointMatcher
+
+PM = PointMatcher
+DP = PM.DataPoints
+
+# 2D example
+ref = DP.load('../data/2D_twoBoxes.csv')
+data = DP.load('../data/2D_oneBox.csv')
+
+icp = PM.ICP()
+
+icp.setDefault()
+
+TP = icp(ref, data)
+
+data_out = DP(data)
+
+icp.transformations.apply(data_out, TP)
+
+ref.save("tests_output/icp_simple/2D_tests_ref.vtk")
+data.save("tests_output/icp_simple/2D_tests_data_in.vtk")
+data_out.save("tests_output/icp_simple/2D_tests_data_out.vtk")
+
+print(f"Final 2D transformations:\n{TP}\n")
+
+# 3D example
+ref = DP.load('../data/car_cloud400.csv')
+data = DP.load('../data/car_cloud401.csv')
+
+icp = PM.ICP()
+
+icp.setDefault()
+
+TP = icp(ref, data)
+
+data_out = DP(data)
+
+icp.transformations.apply(data_out, TP)
+
+ref.save("tests_output/icp_simple/3D_tests_ref.vtk")
+data.save("tests_output/icp_simple/3D_tests_data_in.vtk")
+data_out.save("tests_output/icp_simple/3D_tests_data_out.vtk")
+
+print(f"Final 3D transformations:\n{TP}")