build_map.py

Go to the documentation of this file.

# Code example for DataFilter taking a sequence of point clouds with
# their global coordinates and build a map with a fix (manageable) number of points.
# The example shows how to generate filters in the source code.
# For an example generating filters using yaml configuration, see demo_cmake/convert.cpp
# For an example with a registration solution, see icp.cpp
 
import numpy as np
 
from pypointmatcher import pointmatcher as pm, pointmatchersupport as pms
 
PM = pm.PointMatcher
PMIO = pm.PointMatcherIO
DP = PM.DataPoints
params = pms.Parametrizable.Parameters()
 
# Loading the list of files
file_info_list = PMIO.FileInfoVector("../data/carCloudList.csv", "../data/")
total_point_count = 30000
 
# Path of output directory (default: tests/build_map/)
# The output directory must already exist
# Leave empty to save in the current directory
output_base_directory = "tests/build_map/"
 
# Name of output file: file_name.{vtk,csv,ply} (default: test.vtk)
output_file_name = "test.vtk"
 
pms.setLogger(PM.get().LoggerRegistrar.create("FileLogger"))
 
map_cloud = DP()
last_cloud = DP()
new_cloud = DP()
 
T = np.identity(4)
 
# Define transformation chain
transformation = PM.get().TransformationRegistrar.create("RigidTransformation")
 
# This filter will remove a sphere of 1 m radius. Easy way to remove the sensor self-scanning.
params["minDist"] = "1.0"
remove_scanner = PM.get().DataPointsFilterRegistrar.create("MinDistDataPointsFilter", params)
params.clear()
 
# This filter will randomly remove 35% of the points.
params["prob"] = "0.65"
rand_subsample = PM.get().DataPointsFilterRegistrar.create("RandomSamplingDataPointsFilter", params)
params.clear()
 
# For a complete description of filter, see
# https://github.com/norlab-ulaval/libpointmatcher/blob/master/doc/Datafilters.md
params["knn"] = "10"
params["epsilon"] = "5"
params["keepNormals"] = "1"
params["keepDensities"] = "0"
normal_filter = PM.get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", params)
params.clear()
 
params["knn"] = "10"
params["epsilon"] = "5"
params["keepDensities"] = "1"
params["keepNormals"] = "0"
density_filter = PM.get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", params)
params.clear()
 
observation_direction_filter = PM.get().DataPointsFilterRegistrar.create("ObservationDirectionDataPointsFilter")
 
params["towardCenter"] = "1"
orien_normal_filter = PM.get().DataPointsFilterRegistrar.create("OrientNormalsDataPointsFilter", params)
params.clear()
 
params["maxDensity"] = "30"
uniform_subsample = PM.get().DataPointsFilterRegistrar.create("MaxDensityDataPointsFilter", params)
params.clear()
 
shadow_filter = PM.get().DataPointsFilterRegistrar.create("ShadowDataPointsFilter")
 
for i in range(len(file_info_list)):
    print("\n-----------------------------")
    print(f"Loading {file_info_list[i].readingFileName} ", end="")
 
    new_cloud = DP.load(file_info_list[i].readingFileName)
 
    print(f"found {new_cloud.getNbPoints()} points.")
 
    if file_info_list[i].groundTruthTransformation.shape[0] != 0:
        T = file_info_list[i].groundTruthTransformation
    else:
        print("ERROR: the field gTXX (ground truth) is required")
        exit()
 
    # Remove the scanner
    new_cloud = remove_scanner.filter(new_cloud)
 
    # Accelerate the process and dissolve lines
    new_cloud = rand_subsample.filter(new_cloud)
 
    # Build filter to remove shadow points and down-sample
    new_cloud = normal_filter.filter(new_cloud)
    new_cloud = observation_direction_filter.filter(new_cloud)
    new_cloud = orien_normal_filter.filter(new_cloud)
    new_cloud = shadow_filter.filter(new_cloud)
 
    # Transforme pointCloud
    print(f"Transformation matrix:\n{T}\n".replace("[", " ").replace("]", " "), end="")
    new_cloud = transformation.compute(new_cloud, T)
 
    if i == 0:
        map_cloud = new_cloud
    else:
        map_cloud.concatenate(new_cloud)
 
        # Control point cloud size
        prob_to_keep = total_point_count / map_cloud.features.shape[1]
 
        if prob_to_keep < 1:
            map_cloud = density_filter.filter(map_cloud)
            map_cloud = uniform_subsample.filter(map_cloud)
 
            prob_to_keep = total_point_count / map_cloud.features.shape[1]
 
            if prob_to_keep < 1:
                print(f"Randomly keep {prob_to_keep * 100}% points")
 
                rand_subsample = PM.get().DataPointsFilterRegistrar.create(
                    "RandomSamplingDataPointsFilter",
                    {"prob": f"{prob_to_keep}"})
 
                map_cloud = rand_subsample.filter(map_cloud)
 
    map_cloud.save(f"{output_base_directory + output_file_name[:-4]}_{i}.vtk")
 
map_cloud = density_filter.filter(map_cloud)
map_cloud = uniform_subsample.filter(map_cloud)
map_cloud = density_filter.filter(map_cloud)
 
print("\n-----------------------------"*2)
print(f"Final number of points in the map: {map_cloud.getNbPoints()}")
 
map_cloud.save(f"{output_base_directory + output_file_name}")