1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
# -*- coding: utf-8 -*-
# How to use Normal Distributions Transform
# http://pointclouds.org/documentation/tutorials/normal_distributions_transform.php#normal-distributions-transform
import pcl
import pcl_visualization
# int main (int argc, char** argv)
def main():
# // Loading first scan of room.
# pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
# if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
# {
# PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
# return (-1);
# }
# std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
target_cloud = pcl.load('room_scan1.pcd')
print('Loaded ' + str(target_cloud.size) + ' data points from room_scan1.pcd')
# // Loading second scan of room from new perspective.
# pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
# if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
# {
# PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
# return (-1);
# }
# std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
input_cloud = pcl.load('room_scan2.pcd')
print('Loaded ' + str(input_cloud.size) + ' data points from room_scan2.pcd')
# // Filtering input scan to roughly 10% of original size to increase speed of registration.
# pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
# pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
# approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
# approximate_voxel_filter.setInputCloud (input_cloud);
# approximate_voxel_filter.filter (*filtered_cloud);
# std::cout << "Filtered cloud contains " << filtered_cloud->size () << " data points from room_scan2.pcd" << std::endl;
##
approximate_voxel_filter = input_cloud.make_ApproximateVoxelGrid()
approximate_voxel_filter.set_leaf_size (0.2, 0.2, 0.2)
filtered_cloud = approximate_voxel_filter.filter ()
print('Filtered cloud contains ' + str(filtered_cloud.size) + ' data points from room_scan2.pcd')
# pcl version 1.7.2
# // Initializing Normal Distributions Transform (NDT).
# pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
#
# // Setting scale dependent NDT parameters
# // Setting minimum transformation difference for termination condition.
# ndt.setTransformationEpsilon (0.01);
# // Setting maximum step size for More-Thuente line search.
# ndt.setStepSize (0.1);
# //Setting Resolution of NDT grid structure (VoxelGridCovariance).
# ndt.setResolution (1.0);
#
# // Setting max number of registration iterations.
# ndt.setMaximumIterations (35);
#
# // Setting point cloud to be aligned.
# ndt.setInputSource (filtered_cloud);
# Setting point cloud to be aligned to.
# ndt.setInputTarget (target_cloud);
##
# Set initial alignment estimate found using robot odometry.
# Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
# Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
# Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
##
# Calculating required rigid transform to align the input cloud to the target cloud.
# pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
# ndt.align (*output_cloud, init_guess);
##
# std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged () << " score: " << ndt.getFitnessScore () << std::endl;
##
# // Transforming unfiltered, input cloud using found transform.
# pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
##
# // Saving transformed input cloud.
# pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
##
# // Initializing point cloud visualizer
# boost::shared_ptr<pcl::visualization::PCLVisualizer>
# viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
# viewer_final->setBackgroundColor (0, 0, 0);
##
# // Coloring and visualizing target cloud (red).
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
# target_color (target_cloud, 255, 0, 0);
# viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
# viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
##
# // Coloring and visualizing transformed input cloud (green).
# pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
# output_color (output_cloud, 0, 255, 0);
# viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
# viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");
##
# // Starting visualizer
# viewer_final->addCoordinateSystem (1.0, "global");
# viewer_final->initCameraParameters ();
##
# // Wait until visualizer window is closed.
# while (!viewer_final->wasStopped ())
# {
# viewer_final->spinOnce (100);
# boost::this_thread::sleep (boost::posix_time::microseconds (100000));
# }
|
