1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2018 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26
27 #include <iostream>
28 #include <memory>
29 #include <Eigen/Dense>
30
31 #include <Core/Core.h>
32 #include <IO/IO.h>
33 #include <Visualization/Visualization.h>
34
35 #include <Core/Utility/Timer.h>
36
37 using namespace three;
38
39 std::tuple<std::shared_ptr<PointCloud>, std::shared_ptr<Feature>>
PreprocessPointCloud(const char * file_name)40 PreprocessPointCloud(const char* file_name)
41 {
42 auto pcd = three::CreatePointCloudFromFile(file_name);
43 auto pcd_down = VoxelDownSample(*pcd, 0.05);
44 EstimateNormals(*pcd_down, three::KDTreeSearchParamHybrid(0.1, 30));
45 auto pcd_fpfh = ComputeFPFHFeature(
46 *pcd_down, three::KDTreeSearchParamHybrid(0.25, 100));
47 return std::make_tuple(pcd_down, pcd_fpfh);
48 }
49
VisualizeRegistration(const three::PointCloud & source,const three::PointCloud & target,const Eigen::Matrix4d & Transformation)50 void VisualizeRegistration(const three::PointCloud &source,
51 const three::PointCloud &target, const Eigen::Matrix4d &Transformation)
52 {
53 std::shared_ptr<PointCloud> source_transformed_ptr(new PointCloud);
54 std::shared_ptr<PointCloud> target_ptr(new PointCloud);
55 *source_transformed_ptr = source;
56 *target_ptr = target;
57 source_transformed_ptr->Transform(Transformation);
58 DrawGeometries({ source_transformed_ptr, target_ptr }, "Registration result");
59 }
60
main(int argc,char * argv[])61 int main(int argc, char *argv[])
62 {
63 using namespace three;
64
65 SetVerbosityLevel(VerbosityLevel::VerboseAlways);
66
67 if (argc != 3) {
68 PrintDebug("Usage : %s [path_to_first_point_cloud] [path_to_second_point_cloud]\n",
69 argv[0]);
70 return 1;
71 }
72
73 bool visualization = false;
74
75 #ifdef _OPENMP
76 PrintDebug("OpenMP is supported. Using %d threads.", omp_get_num_threads());
77 #endif
78
79 for (int i = 0; i < 50000; i++) {
80 ScopeTimer t("one iteration");
81
82 std::shared_ptr<PointCloud> source, target;
83 std::shared_ptr<Feature> source_fpfh, target_fpfh;
84 std::tie(source, source_fpfh) =
85 PreprocessPointCloud(argv[1]);
86 std::tie(target, target_fpfh) =
87 PreprocessPointCloud(argv[2]);
88
89 std::vector<std::reference_wrapper<const CorrespondenceChecker>>
90 correspondence_checker;
91 auto correspondence_checker_edge_length =
92 CorrespondenceCheckerBasedOnEdgeLength(0.9);
93 auto correspondence_checker_distance =
94 CorrespondenceCheckerBasedOnDistance(0.075);
95 auto correspondence_checker_normal =
96 CorrespondenceCheckerBasedOnNormal(0.52359878);
97
98 correspondence_checker.push_back(correspondence_checker_edge_length);
99 correspondence_checker.push_back(correspondence_checker_distance);
100 correspondence_checker.push_back(correspondence_checker_normal);
101 auto registration_result = RegistrationRANSACBasedOnFeatureMatching(
102 *source, *target, *source_fpfh, *target_fpfh, 0.075,
103 TransformationEstimationPointToPoint(false), 4,
104 correspondence_checker, RANSACConvergenceCriteria(4000000, 1000));
105
106 if (visualization)
107 VisualizeRegistration(*source, *target,
108 registration_result.transformation_);
109 }
110
111 return 0;
112 }
113