icp_advance_api.py
Go to the documentation of this file.
1 # Code example for ICP taking 2 points clouds (2D or 3D) relatively close
2 # and computing the transformation between them.
3 #
4 # This code is more complete than icp_simple. It can load parameter files and has more options.
5 
6 import numpy as np
7 
8 from math import sqrt
9 from pypointmatcher import pointmatcher as pm, pointmatchersupport as pms
10 from utils import parse_translation, parse_rotation
11 
12 PM = pm.PointMatcher
13 DP = PM.DataPoints
14 Parameters = pms.Parametrizable.Parameters
15 
16 # Save transformation matrix in three different files:
17 # - BASEFILENAME_inti_transfo.txt
18 # - BASEFILENAME_icp_transfo.txt
19 # - BASEFILENAME_complete_transfo.txt
20 # (default: false)
21 is_transfo_saved = False
22 
23 # Be more verbose (info logging to the console)
24 is_verbose = True
25 
26 # Load the config from a YAML file (default: default.yaml)
27 config_file = "../data/default.yaml"
28 
29 # Path of output directory (default: tests/icp_advance_api/)
30 # The output directory must already exist
31 # Leave empty to save in the current directory
32 output_base_directory = "tests/icp_advance_api/"
33 
34 # Name of output files (default: test)
35 output_base_file = "test"
36 
37 # Toggle to switch between 2D and 3D clouds
38 is_3D = True
39 
40 # Add an initial 3D translation before applying ICP (default: 0,0,0)
41 # Add an initial 2D translation before applying ICP (default: 0,0)
42 init_translation = "0,0,0" if is_3D else "0,0"
43 # Add an initial 3D rotation before applying ICP (default: 1,0,0;0,1,0;0,0,1)
44 # Add an initial 2D rotation before applying ICP (default: 1,0;0,1)
45 init_rotation = "1,0,0;0,1,0;0,0,1" if is_3D else "1,0;0,1"
46 
47 if is_3D:
48  # 3D point clouds
49  ref = DP.load('../data/car_cloud400.csv')
50  data = DP.load('../data/car_cloud401.csv')
51  test_base = "3D"
52 else:
53  # 2D point clouds
54  ref = DP.load('../data/2D_twoBoxes.csv')
55  data = DP.load('../data/2D_oneBox.csv')
56  test_base = "2D"
57 
58 # Create the default ICP algorithm
59 icp = PM.ICP()
60 
61 if len(config_file) == 0:
62  # See the implementation of setDefault() to create a custom ICP algorithm
63  icp.setDefault()
64 else:
65  # load YAML config
66  icp.loadFromYaml(config_file)
67 
68 cloud_dimension = ref.getEuclideanDim()
69 
70 assert cloud_dimension == 2 or cloud_dimension == 3, "Invalid input point clouds dimension"
71 
72 # Parse the translation and rotation to be used to compute the initial transformation
73 translation = parse_translation(init_translation, cloud_dimension)
74 rotation = parse_rotation(init_rotation, cloud_dimension)
75 
76 init_transfo = np.matmul(translation, rotation)
77 
78 rigid_trans = PM.get().TransformationRegistrar.create("RigidTransformation")
79 
80 if not rigid_trans.checkParameters(init_transfo):
81  print("Initial transformations is not rigid, identiy will be used")
82  init_transfo = np.identity(cloud_dimension + 1)
83 
84 initialized_data = rigid_trans.compute(data, init_transfo)
85 
86 # Compute the transformation to express data in ref
87 T = icp(initialized_data, ref)
88 match_ratio = icp.errorMinimizer.getWeightedPointUsedRatio()
89 print(f"match ratio: {match_ratio:.6}")
90 
91 # Transform data to express it in ref
92 data_out = DP(initialized_data)
93 icp.transformations.apply(data_out, T)
94 
95 print("\n------------------")
96 
97 # START demo 1
98 # Test for retrieving Haussdorff distance (with outliers). We generate new matching module
99 # specifically for this purpose.
100 #
101 # INPUTS:
102 # ref: point cloud used as reference
103 # data_out: aligned point cloud (using the transformation outputted by icp)
104 # icp: icp object used to aligned the point clouds
105 
106 params = Parameters()
107 
108 params["knn"] = "1" # for Hausdorff distance, we only need the first closest point
109 params["epsilon"] = "0"
110 matcher_Hausdorff = PM.get().MatcherRegistrar.create("KDTreeMatcher", params)
111 
112 # max. distance from reading to reference
113 matcher_Hausdorff.init(ref)
114 matches = matcher_Hausdorff.findClosests(data_out)
115 max_dist1 = matches.getDistsQuantile(1.0)
116 max_dist_robust1 = matches.getDistsQuantile(0.85)
117 
118 # max. distance from reference to reading
119 matcher_Hausdorff.init(data_out)
120 matches = matcher_Hausdorff.findClosests(ref)
121 max_dist2 = matches.getDistsQuantile(1.0)
122 max_dist_robust2 = matches.getDistsQuantile(0.85)
123 
124 haussdorff_dist = max(max_dist1, max_dist2)
125 haussdorff_quantile_dist = max(max_dist_robust1, max_dist_robust2)
126 
127 print(f"Haussdorff distance: {sqrt(haussdorff_dist):.6} m")
128 print(f"Haussdorff quantile distance: {sqrt(haussdorff_quantile_dist):.6} m")
129 
130 # START demo 2
131 # Test for retrieving paired point mean distance without outliers.
132 # We reuse the same module used for the icp object.
133 #
134 # INPUTS:
135 # ref: point cloud used as reference
136 # data_out: aligned point cloud (using the transformation outputted by icp)
137 # icp: icp object used to aligned the point clouds
138 
139 # initiate the matching with unfiltered point cloud
140 icp.matcher.init(ref)
141 
142 # extract closest points
143 matches = icp.matcher.findClosests(data_out)
144 
145 # weight paired points
146 outlier_weights = icp.outlierFilters.compute(data_out, ref, matches)
147 
148 # generate tuples of matched points and remove pairs with zero weight
149 matched_points = PM.ErrorMinimizer.ErrorElements(data_out, ref, outlier_weights, matches)
150 
151 # extract relevant information for convenience
152 dim = matched_points.reading.getEuclideanDim()
153 nb_matched_points = matched_points.reading.getNbPoints()
154 matched_read = matched_points.reading.features[:dim]
155 matched_ref = matched_points.reference.features[:dim]
156 
157 # compute mean distance
158 dist = np.linalg.norm(matched_read - matched_ref, axis=0)
159 mean_dist = dist.sum() / nb_matched_points
160 print(f"Robust mean distance: {mean_dist:.6} m")
161 
162 # End demo
163 
164 print("------------------\n")
165 
166 # Save files to see the results
167 ref.save(f"{output_base_directory + test_base}_{output_base_file}_ref.vtk")
168 data.save(f"{output_base_directory + test_base}_{output_base_file}_data_in.vtk")
169 data_out.save(f"{output_base_directory + test_base}_{output_base_file}_data_out.vtk")
170 
171 if is_transfo_saved:
172  init_file_name = f"{output_base_directory + test_base}_{output_base_file}_init_transfo.txt"
173  icp_file_name = f"{output_base_directory + test_base}_{output_base_file}_icp.transfo.txt"
174  complete_file_name = f"{output_base_directory + test_base}_{output_base_file}_complete_transfo.txt"
175 
176  with open(init_file_name, "w") as f:
177  f.write(f"{init_transfo}".replace("[", " ").replace("]", " "))
178 
179  with open(icp_file_name, "w") as f:
180  f.write(f"{T}".replace("[", " ").replace("]", " "))
181 
182  with open(complete_file_name, "w") as f:
183  f.write(f"{np.matmul(T, init_transfo)}".replace("[", " ").replace("]", " "))
184 
185 else:
186  if is_verbose:
187  print(f"{test_base} ICP transformation:\n{T}".replace("[", " ").replace("]", " "))
Definition: icp.py:1
def parse_rotation
Definition: utils.py:22
def parse_translation
Definition: utils.py:9


libpointmatcher
Author(s):
autogenerated on Sat May 27 2023 02:38:02