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 <thread>
28
29 #include <Core/Core.h>
30 #include <IO/IO.h>
31 #include <Visualization/Visualization.h>
32
33 #include "VisualizerForAlignment.h"
34
PrintTransformation(const Eigen::Matrix4d & transformation)35 void PrintTransformation(const Eigen::Matrix4d &transformation)
36 {
37 using namespace three;
38 PrintInfo("Current transformation is:\n");
39 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
40 transformation(0, 0), transformation(0, 1),
41 transformation(0, 2), transformation(0, 3));
42 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
43 transformation(1, 0), transformation(1, 1),
44 transformation(1, 2), transformation(1, 3));
45 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
46 transformation(2, 0), transformation(2, 1),
47 transformation(2, 2), transformation(2, 3));
48 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
49 transformation(3, 0), transformation(3, 1),
50 transformation(3, 2), transformation(3, 3));
51 }
52
PrintHelp()53 void PrintHelp()
54 {
55 printf("Open3D %s\n", OPEN3D_VERSION);
56 printf("\n");
57 printf("Usage:\n");
58 printf(" > ManuallyAlignPointCloud source_file target_file [options]\n");
59 printf(" Manually align point clouds in source_file and target_file.\n");
60 printf("\n");
61 printf("Options:\n");
62 printf(" --help, -h : Print help information.\n");
63 printf(" --verbose n : Set verbose level (0-4).\n");
64 printf(" --voxel_size d : Set downsample voxel size.\n");
65 printf(" --max_corres_distance d : Set max correspondence distance.\n");
66 printf(" --without_scaling : Disable scaling in transformations.\n");
67 printf(" --without_dialog : Disable dialogs. Default files will be used.\n");
68 printf(" --without_gui_icp file : The program runs as a console command. No window\n");
69 printf(" will be created. The program reads an alignment\n");
70 printf(" from file. It does cropping, downsample, ICP,\n");
71 printf(" then saves the alignment session into file.\n");
72 printf(" --without_gui_eval file : The program runs as a console command. No window\n");
73 printf(" will be created. The program reads an alignment\n");
74 printf(" from file. It does cropping, downsample,\n");
75 printf(" evaluation, then saves everything.\n");
76 }
77
main(int argc,char ** argv)78 int main(int argc, char **argv)
79 {
80 using namespace three;
81
82 if (argc < 3 || ProgramOptionExists(argc, argv, "--help") ||
83 ProgramOptionExists(argc, argv, "-h")) {
84 PrintHelp();
85 return 0;
86 }
87
88 int verbose = GetProgramOptionAsInt(argc, argv, "--verbose", 2);
89 SetVerbosityLevel((VerbosityLevel)verbose);
90 double voxel_size = GetProgramOptionAsDouble(argc, argv, "--voxel_size",
91 -1.0);
92 double max_corres_distance = GetProgramOptionAsDouble(argc, argv,
93 "--max_corres_distance", -1.0);
94 bool with_scaling = !ProgramOptionExists(argc, argv, "--without_scaling");
95 bool with_dialog = !ProgramOptionExists(argc, argv, "--without_dialog");
96 std::string default_polygon_filename =
97 filesystem::GetFileNameWithoutExtension(argv[2]) + ".json";
98 std::string alignment_filename = GetProgramOptionAsString(argc, argv,
99 "--without_gui_icp", "");
100 std::string eval_filename = GetProgramOptionAsString(argc, argv,
101 "--without_gui_eval", "");
102 std::string default_directory = filesystem::GetFileParentDirectory(argv[1]);
103
104 auto source_ptr = CreatePointCloudFromFile(argv[1]);
105 auto target_ptr = CreatePointCloudFromFile(argv[2]);
106 if (source_ptr->IsEmpty() || target_ptr->IsEmpty()) {
107 PrintWarning("Failed to read one of the point clouds.\n");
108 return 0;
109 }
110
111 if (!alignment_filename.empty()) {
112 AlignmentSession session;
113 if (ReadIJsonConvertible(alignment_filename, session) == false) {
114 return 0;
115 }
116 session.voxel_size_ = voxel_size;
117 session.max_correspondence_distance_ = max_corres_distance;
118 source_ptr->Transform(session.transformation_);
119 auto polygon_volume = std::make_shared<SelectionPolygonVolume>();
120 if (ReadIJsonConvertible(default_polygon_filename, *polygon_volume)) {
121 PrintInfo("Crop point cloud.\n");
122 source_ptr = polygon_volume->CropPointCloud(*source_ptr);
123 }
124 if (voxel_size > 0.0) {
125 PrintInfo("Downsample point cloud with voxel size %.4f.\n",
126 voxel_size);
127 source_ptr = VoxelDownSample(*source_ptr, voxel_size);
128 }
129 if (max_corres_distance > 0.0) {
130 PrintInfo("ICP with max correspondence distance %.4f.\n",
131 max_corres_distance);
132 auto result = RegistrationICP(*source_ptr, *target_ptr,
133 max_corres_distance, Eigen::Matrix4d::Identity(),
134 TransformationEstimationPointToPoint(true),
135 ICPConvergenceCriteria(1e-6, 1e-6, 30));
136 PrintInfo("Registration finished with fitness %.4f and RMSE %.4f.\n",
137 result.fitness_, result.inlier_rmse_);
138 if (result.fitness_ > 0.0) {
139 session.transformation_ = result.transformation_ *
140 session.transformation_;
141 PrintTransformation(session.transformation_);
142 source_ptr->Transform(result.transformation_);
143 }
144 }
145 WriteIJsonConvertible(alignment_filename, session);
146 return 1;
147 }
148
149 if (!eval_filename.empty()) {
150 AlignmentSession session;
151 if (ReadIJsonConvertible(eval_filename, session) == false) {
152 return 0;
153 }
154 source_ptr->Transform(session.transformation_);
155 auto polygon_volume = std::make_shared<SelectionPolygonVolume>();
156 if (ReadIJsonConvertible(default_polygon_filename, *polygon_volume)) {
157 PrintInfo("Crop point cloud.\n");
158 source_ptr = polygon_volume->CropPointCloud(*source_ptr);
159 }
160 if (voxel_size > 0.0) {
161 PrintInfo("Downsample point cloud with voxel size %.4f.\n",
162 voxel_size);
163 source_ptr = VoxelDownSample(*source_ptr, voxel_size);
164 }
165 std::string source_filename = filesystem::GetFileNameWithoutExtension(
166 eval_filename) + ".source.ply";
167 std::string target_filename = filesystem::GetFileNameWithoutExtension(
168 eval_filename) + ".target.ply";
169 std::string source_binname = filesystem::GetFileNameWithoutExtension(
170 eval_filename) + ".source.bin";
171 std::string target_binname = filesystem::GetFileNameWithoutExtension(
172 eval_filename) + ".target.bin";
173 FILE * f;
174
175 WritePointCloud(source_filename, *source_ptr);
176 auto source_dis = ComputePointCloudToPointCloudDistance(
177 *source_ptr, *target_ptr);
178 f = fopen(source_binname.c_str(), "wb");
179 fwrite(source_dis.data(), sizeof(double), source_dis.size(), f);
180 fclose(f);
181 WritePointCloud(target_filename, *target_ptr);
182 auto target_dis = ComputePointCloudToPointCloudDistance(
183 *target_ptr, *source_ptr);
184 f = fopen(target_binname.c_str(), "wb");
185 fwrite(target_dis.data(), sizeof(double), target_dis.size(), f);
186 fclose(f);
187 return 1;
188 }
189
190 VisualizerWithEditing vis_source, vis_target;
191 VisualizerForAlignment vis_main(vis_source, vis_target, voxel_size,
192 max_corres_distance, with_scaling, with_dialog,
193 default_polygon_filename, default_directory);
194
195 vis_source.CreateWindow("Source Point Cloud", 1280, 720, 10, 100);
196 vis_source.AddGeometry(source_ptr);
197 if (source_ptr->points_.size() > 5000000) {
198 vis_source.GetRenderOption().point_size_ = 1.0;
199 }
200 vis_source.BuildUtilities();
201 vis_target.CreateWindow("Target Point Cloud", 1280, 720, 10, 880);
202 vis_target.AddGeometry(target_ptr);
203 if (target_ptr->points_.size() > 5000000) {
204 vis_target.GetRenderOption().point_size_ = 1.0;
205 }
206 vis_target.BuildUtilities();
207 vis_main.CreateWindow("Alignment", 1280, 1440, 1300, 100);
208 vis_main.AddSourceAndTarget(source_ptr, target_ptr);
209 vis_main.BuildUtilities();
210
211 while (vis_source.PollEvents() && vis_target.PollEvents() &&
212 vis_main.PollEvents()) {
213 }
214
215 vis_source.DestroyWindow();
216 vis_target.DestroyWindow();
217 vis_main.DestroyWindow();
218 return 1;
219 }
220