-
Notifications
You must be signed in to change notification settings - Fork 21
/
main.cpp
102 lines (83 loc) · 4.76 KB
/
main.cpp
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
//
// Created by Zhang Zhimeng on 22-8-26.
//
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/icp.h>
#include <Eigen/Dense>
#include "optimized_ICP_GN.h"
using namespace std;
int main() {
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target_ptr(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source_ptr(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source_opti_transformed_ptr(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_source_svd_transformed_ptr(new pcl::PointCloud<pcl::PointXYZI>);
pcl::io::loadPCDFile("../data/room_scan2.pcd", *cloud_target_ptr);
pcl::io::loadPCDFile("../data/room_scan1.pcd", *cloud_source_ptr);
Eigen::Matrix4f T_predict, T_final;
T_predict.setIdentity();
T_predict << 0.765, 0.643, -0.027, -1.472,
-0.644, 0.765, -0.023, 1.366,
0.006, 0.035, 0.999, -0.125,
0.0, 0.0, 0.0, 1.0;
std::cout << "Wait, matching..." << std::endl;
// ======================= optimized icp =======================
OptimizedICPGN icp_opti;
icp_opti.SetTargetCloud(cloud_target_ptr);
icp_opti.SetTransformationEpsilon(1e-4);
icp_opti.SetMaxIterations(30);
icp_opti.SetMaxCorrespondDistance(0.5);
icp_opti.Match(cloud_source_ptr, T_predict, cloud_source_opti_transformed_ptr, T_final);
std::cout << "============== Optimized ICP =================" << std::endl;
std::cout << "T final:\n" << T_final << std::endl;
std::cout << "fitness score: " << icp_opti.GetFitnessScore() << std::endl;
// ======================= optimized icp =======================
// ======================= svd icp =======================
pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp_svd;
icp_svd.setInputTarget(cloud_target_ptr);
icp_svd.setInputSource(cloud_source_ptr);
icp_svd.setMaxCorrespondenceDistance(0.5);
icp_svd.setMaximumIterations(30);
icp_svd.setEuclideanFitnessEpsilon(1e-4);
icp_svd.setTransformationEpsilon(1e-4);
icp_svd.align(*cloud_source_svd_transformed_ptr, T_predict);
std::cout << "\n============== SVD ICP =================" << std::endl;
std::cout << "T final: \n" << icp_svd.getFinalTransformation() << std::endl;
std::cout << "fitness score: " << icp_svd.getFitnessScore() << std::endl;
// ======================= svd icp =======================
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer"));
viewer->initCameraParameters();
int v1(0);
int v2(1);
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer->setBackgroundColor(0, 0, 0, v1);
viewer->addText("Optimized ICP", 10, 10, "optimized icp", v1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> source_opti_color(
cloud_source_opti_transformed_ptr,
255, 0, 0);
viewer->addPointCloud<pcl::PointXYZI>(cloud_source_opti_transformed_ptr, source_opti_color, "source opti cloud",
v1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> target_color_0(cloud_target_ptr, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZI>(cloud_target_ptr, target_color_0, "target cloud1", v2);
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
viewer->setBackgroundColor(0.0, 0.0, 0.0, v2);
viewer->addText("SVD ICP", 10, 10, "svd icp", v2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> target_color_1(cloud_target_ptr, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZI>(cloud_target_ptr, target_color_1, "target cloud2", v2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> source_svd_color(cloud_source_svd_transformed_ptr,
0, 255, 0);
viewer->addPointCloud<pcl::PointXYZI>(cloud_source_svd_transformed_ptr, source_svd_color, "source svd cloud",
v2);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "source opti cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "source svd cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "target cloud1");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "target cloud2");
viewer->addCoordinateSystem(1.0);
viewer->setCameraPosition(0, 0, 20, 0, 10, 10, v1);
viewer->setCameraPosition(0, 0, 20, 0, 10, 10, v2);
viewer->spin();
return 0;
}