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
|
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>(5,1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the CloudIn data
for (auto& point : *cloud_in)
{
point.x = 1024 * rand() / (RAND_MAX + 1.0f);
point.y = 1024 * rand() / (RAND_MAX + 1.0f);
point.z = 1024 * rand() / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->size () << " data points to input:" << std::endl;
for (auto& point : *cloud_in)
std::cout << point << std::endl;
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->size() << std::endl;
for (auto& point : *cloud_out)
point.x += 0.7f;
std::cout << "Transformed " << cloud_in->size () << " data points:" << std::endl;
for (auto& point : *cloud_out)
std::cout << point << std::endl;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}
|
