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
PrintPointCloud(const three::PointCloud & pointcloud)35 void PrintPointCloud(const three::PointCloud &pointcloud)
36 {
37 using namespace three;
38
39 bool pointcloud_has_normal = pointcloud.HasNormals();
40 PrintInfo("Pointcloud has %d points.\n",
41 (int)pointcloud.points_.size());
42
43 Eigen::Vector3d min_bound = pointcloud.GetMinBound();
44 Eigen::Vector3d max_bound = pointcloud.GetMaxBound();
45 PrintInfo("Bounding box is: (%.4f, %.4f, %.4f) - (%.4f, %.4f, %.4f)\n",
46 min_bound(0), min_bound(1), min_bound(2),
47 max_bound(0), max_bound(1), max_bound(2));
48
49 for (size_t i = 0; i < pointcloud.points_.size(); i++) {
50 if (pointcloud_has_normal) {
51 const Eigen::Vector3d &point = pointcloud.points_[i];
52 const Eigen::Vector3d &normal = pointcloud.normals_[i];
53 PrintDebug("%.6f %.6f %.6f %.6f %.6f %.6f\n",
54 point(0), point(1), point(2),
55 normal(0), normal(1), normal(2));
56 } else {
57 const Eigen::Vector3d &point = pointcloud.points_[i];
58 PrintDebug("%.6f %.6f %.6f\n", point(0), point(1), point(2));
59 }
60 }
61 PrintDebug("End of the list.\n\n");
62 }
63
main(int argc,char * argv[])64 int main(int argc, char *argv[])
65 {
66 using namespace three;
67
68 SetVerbosityLevel(VerbosityLevel::VerboseAlways);
69
70 auto pcd = CreatePointCloudFromFile(argv[1]);
71 {
72 ScopeTimer timer("FPFH estimation with Radius 0.25");
73 //for (int i = 0; i < 20; i++) {
74 ComputeFPFHFeature(*pcd,
75 three::KDTreeSearchParamRadius(0.25));
76 //}
77 }
78
79 {
80 ScopeTimer timer("Normal estimation with KNN20");
81 for (int i = 0; i < 20; i++) {
82 EstimateNormals(*pcd,
83 three::KDTreeSearchParamKNN(20));
84 }
85 }
86 std::cout << pcd->normals_[0] << std::endl;
87 std::cout << pcd->normals_[10] << std::endl;
88
89 {
90 ScopeTimer timer("Normal estimation with Radius 0.01666");
91 for (int i = 0; i < 20; i++) {
92 EstimateNormals(*pcd,
93 three::KDTreeSearchParamRadius(0.01666));
94 }
95 }
96 std::cout << pcd->normals_[0] << std::endl;
97 std::cout << pcd->normals_[10] << std::endl;
98
99 {
100 ScopeTimer timer("Normal estimation with Hybrid 0.01666, 60");
101 for (int i = 0; i < 20; i++) {
102 EstimateNormals(*pcd,
103 three::KDTreeSearchParamHybrid(0.01666, 60));
104 }
105 }
106 std::cout << pcd->normals_[0] << std::endl;
107 std::cout << pcd->normals_[10] << std::endl;
108
109 auto downpcd = VoxelDownSample(*pcd, 0.05);
110
111 // 1. test basic pointcloud functions.
112
113 PointCloud pointcloud;
114 PrintPointCloud(pointcloud);
115
116 pointcloud.points_.push_back(Eigen::Vector3d(0.0, 0.0, 0.0));
117 pointcloud.points_.push_back(Eigen::Vector3d(1.0, 0.0, 0.0));
118 pointcloud.points_.push_back(Eigen::Vector3d(0.0, 1.0, 0.0));
119 pointcloud.points_.push_back(Eigen::Vector3d(0.0, 0.0, 1.0));
120 PrintPointCloud(pointcloud);
121
122 // 2. test pointcloud IO.
123
124 const std::string filename_xyz("test.xyz");
125 const std::string filename_ply("test.ply");
126
127 if (ReadPointCloud(argv[1], pointcloud)) {
128 PrintWarning("Successfully read %s\n", argv[1]);
129
130 /*
131 PointCloud pointcloud_copy;
132 pointcloud_copy.CloneFrom(pointcloud);
133
134 if (WritePointCloud(filename_xyz, pointcloud)) {
135 PrintWarning("Successfully wrote %s\n\n", filename_xyz.c_str());
136 } else {
137 PrintError("Failed to write %s\n\n", filename_xyz.c_str());
138 }
139
140 if (WritePointCloud(filename_ply, pointcloud_copy)) {
141 PrintWarning("Successfully wrote %s\n\n", filename_ply.c_str());
142 } else {
143 PrintError("Failed to write %s\n\n", filename_ply.c_str());
144 }
145 */
146 } else {
147 PrintError("Failed to read %s\n\n", argv[1]);
148 }
149
150 // 3. test pointcloud visualization
151
152 Visualizer visualizer;
153 std::shared_ptr<PointCloud> pointcloud_ptr(new PointCloud);
154 *pointcloud_ptr = pointcloud;
155 pointcloud_ptr->NormalizeNormals();
156 BoundingBox bounding_box;
157 bounding_box.FitInGeometry(*pointcloud_ptr);
158
159 std::shared_ptr<PointCloud> pointcloud_transformed_ptr(new PointCloud);
160 *pointcloud_transformed_ptr = *pointcloud_ptr;
161 Eigen::Matrix4d trans_to_origin = Eigen::Matrix4d::Identity();
162 trans_to_origin.block<3, 1>(0, 3) = bounding_box.GetCenter() * -1.0;
163 Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
164 transformation.block<3, 3>(0, 0) = static_cast<Eigen::Matrix3d>(
165 Eigen::AngleAxisd(M_PI / 4.0, Eigen::Vector3d::UnitX()));
166 pointcloud_transformed_ptr->Transform(
167 trans_to_origin.inverse() * transformation * trans_to_origin);
168
169 visualizer.CreateWindow("Open3D", 1600, 900);
170 visualizer.AddGeometry(pointcloud_ptr);
171 visualizer.AddGeometry(pointcloud_transformed_ptr);
172 visualizer.Run();
173 visualizer.DestroyWindow();
174
175 // 4. test operations
176 *pointcloud_transformed_ptr += *pointcloud_ptr;
177 DrawGeometries({pointcloud_transformed_ptr}, "Combined Pointcloud");
178
179 // 5. test downsample
180 auto downsampled = VoxelDownSample(*pointcloud_ptr, 0.05);
181 DrawGeometries({downsampled}, "Down Sampled Pointcloud");
182
183 // 6. test normal estimation
184 DrawGeometriesWithKeyCallbacks({pointcloud_ptr},
185 {{GLFW_KEY_SPACE, [&](Visualizer *vis) {
186 //EstimateNormals(*pointcloud_ptr,
187 // three::KDTreeSearchParamKNN(20));
188 EstimateNormals(*pointcloud_ptr,
189 three::KDTreeSearchParamRadius(0.05));
190 PrintInfo("Done.\n");
191 return true;
192 }}},
193 "Press Space to Estimate Normal", 1600, 900);
194
195 // n. test end
196
197 PrintAlways("End of the test.\n");
198 }
199