align_sequence.py
Go to the documentation of this file.
1 # Code example for ICP taking a sequence of point clouds relatively close
2 # and build a map with them.
3 # It assumes that: 3D point clouds are used, they were recorded in sequence
4 # and they are express in sensor frame.
5 
6 import numpy as np
7 
8 from pypointmatcher import pointmatcher as pm, pointmatchersupport as pms
9 
10 PM = pm.PointMatcher
11 PMIO = pm.PointMatcherIO
12 DP = PM.DataPoints
13 params = pms.Parametrizable.Parameters()
14 
15 # Path of output directory (default: tests/align_sequence/)
16 # The output directory must already exist
17 # Leave empty to save in the current directory
18 output_base_directory = "tests/align_sequence/"
19 
20 # Name of output files (default: align_sequence)
21 output_file_name = "align_sequence"
22 
23 # Rigid transformation
24 rigid_trans = PM.get().TransformationRegistrar.create("RigidTransformation")
25 
26 # Create filters manually to clean the global map
27 params["knn"] = "10"
28 params["epsilon"] = "5"
29 params["keepNormals"] = "0"
30 params["keepDensities"] = "1"
31 density_filter = PM.get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", params)
32 params.clear()
33 
34 params["maxDensity"] = "30"
35 max_density_subsample = PM.get().DataPointsFilterRegistrar.create("MaxDensityDataPointsFilter",
36  params)
37 params.clear()
38 
39 # Main algorithm definition
40 icp = PM.ICP()
41 
42 # load YAML config
43 config_file = "../data/default.yaml"
44 pms.validateFile(config_file)
45 icp.loadFromYaml(config_file)
46 
47 # Loading the list of files
48 # file_info_list = PMIO.FileInfoVector("../data/carCloudList.csv", "../data/")
49 # or
50 file_info_list = PMIO.FileInfoVector("../data/cloudList.csv", "../data/")
51 
52 map_point_cloud = DP()
53 new_cloud = DP()
54 
55 T_to_map_from_new = np.identity(4) # assumes 3D
56 
57 for i in range(len(file_info_list)):
58  print(f"---------------------\nLoading: {file_info_list[i].readingFileName}")
59 
60  # It is assume that the point cloud is express in sensor frame
61  new_cloud = DP.load(file_info_list[i].readingFileName)
62 
63  if map_point_cloud.getNbPoints() == 0:
64  map_point_cloud = new_cloud
65  continue
66 
67  # call ICP
68  try:
69  # We use the last transformation as a prior
70  # this assumes that the point clouds were recorded in
71  # sequence.
72  prior = T_to_map_from_new
73 
74  T_to_map_from_new = icp(new_cloud, map_point_cloud, prior)
75 
76  except PM.ConvergenceError as CE:
77  print(f"ERROR PM.ICP failed to converge: \n\t{CE}\n\n")
78  continue
79 
80  # This is not necessary in this example, but could be
81  # useful if the same matrix is composed in the loop.
82  T_to_map_from_new = rigid_trans.correctParameters(T_to_map_from_new)
83 
84  # Move the new point cloud in the map reference
85  new_cloud = rigid_trans.compute(new_cloud, T_to_map_from_new)
86 
87  # Merge point clouds to map
88  map_point_cloud.concatenate(new_cloud)
89 
90  # Clean the map
91  map_point_cloud = density_filter.filter(map_point_cloud)
92  map_point_cloud = max_density_subsample.filter(map_point_cloud)
93 
94  # Save the map at each iteration
95  output_file_name_iter = f"{output_file_name}_{i}.vtk"
96  print(f"outputFileName: {output_file_name_iter}")
97  map_point_cloud.save(f"{output_base_directory}{output_file_name_iter}")
Definition: icp.py:1
void validateFile(const std::string &fileName)
Throw a runtime_error exception if fileName cannot be opened.
A vector of file info, to be used in batch processing.
Definition: IO.h:245


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