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 "VisualizerForAlignment.h"
28
29 #include <External/tinyfiledialogs/tinyfiledialogs.h>
30
31 namespace three {
32
PrintVisualizerHelp()33 void VisualizerForAlignment::PrintVisualizerHelp()
34 {
35 Visualizer::PrintVisualizerHelp();
36 PrintInfo(" -- Alignment control --\n");
37 PrintInfo(" Ctrl + R : Reset source and target to initial state.\n");
38 PrintInfo(" Ctrl + S : Save current alignment session into a JSON file.\n");
39 PrintInfo(" Ctrl + O : Load current alignment session from a JSON file.\n");
40 PrintInfo(" Ctrl + A : Align point clouds based on manually annotations.\n");
41 PrintInfo(" Ctrl + I : Run ICP refinement.\n");
42 PrintInfo(" Ctrl + D : Run voxel downsample for both source and target.\n");
43 PrintInfo(" Ctrl + K : Load a polygon from a JSON file and crop source.\n");
44 PrintInfo(" Ctrl + E : Evaluate error and save to files.\n");
45 }
46
AddSourceAndTarget(std::shared_ptr<PointCloud> source,std::shared_ptr<PointCloud> target)47 bool VisualizerForAlignment::AddSourceAndTarget(
48 std::shared_ptr<PointCloud> source, std::shared_ptr<PointCloud> target)
49 {
50 GetRenderOption().point_size_ = 1.0;
51 alignment_session_.source_ptr_ = source;
52 alignment_session_.target_ptr_ = target;
53 source_copy_ptr_ = std::make_shared<PointCloud>();
54 target_copy_ptr_ = std::make_shared<PointCloud>();
55 *source_copy_ptr_ = *source;
56 *target_copy_ptr_ = *target;
57 return AddGeometry(source_copy_ptr_) && AddGeometry(target_copy_ptr_);
58 }
59
KeyPressCallback(GLFWwindow * window,int key,int scancode,int action,int mods)60 void VisualizerForAlignment::KeyPressCallback(GLFWwindow *window, int key,
61 int scancode, int action, int mods)
62 {
63 if (action == GLFW_PRESS && (mods & GLFW_MOD_CONTROL)) {
64 const char *filename;
65 const char *pattern[1] = {"*.json"};
66 switch (key) {
67 case GLFW_KEY_R: {
68 *source_copy_ptr_ = *alignment_session_.source_ptr_;
69 *target_copy_ptr_ = *alignment_session_.target_ptr_;
70 ResetViewPoint(true);
71 UpdateGeometry();
72 return;
73 }
74 case GLFW_KEY_S: {
75 std::string default_alignment = default_directory_ +
76 "alignment.json";
77 if (use_dialog_) {
78 filename = tinyfd_saveFileDialog("Alignment session",
79 default_alignment.c_str(), 1, pattern,
80 "JSON file (*.json)");
81 } else {
82 filename = default_alignment.c_str();
83 }
84 if (filename != NULL) {
85 SaveSessionToFile(filename);
86 }
87 return;
88 }
89 case GLFW_KEY_O: {
90 std::string default_alignment = default_directory_ +
91 "alignment.json";
92 if (use_dialog_) {
93 filename = tinyfd_openFileDialog("Alignment session",
94 default_alignment.c_str(), 1, pattern,
95 "JSON file (*.json)", 0);
96 } else {
97 filename = default_alignment.c_str();
98 }
99 if (filename != NULL) {
100 LoadSessionFromFile(filename);
101 }
102 return;
103 }
104 case GLFW_KEY_A: {
105 if (AlignWithManualAnnotation()) {
106 ResetViewPoint(true);
107 UpdateGeometry();
108 }
109 return;
110 }
111 case GLFW_KEY_I: {
112 if (use_dialog_) {
113 char buff[DEFAULT_IO_BUFFER_SIZE];
114 sprintf(buff, "%.4f", max_correspondence_distance_);
115 const char *str = tinyfd_inputBox("Set voxel size",
116 "Set max correspondence distance for ICP (ignored if it is non-positive)",
117 buff);
118 if (str == NULL) {
119 PrintDebug("Dialog closed.\n");
120 return;
121 } else {
122 char *end;
123 errno = 0;
124 double l = std::strtod(str, &end);
125 if (errno == ERANGE && (l == HUGE_VAL || l == -HUGE_VAL)) {
126 PrintDebug("Illegal input, use default max correspondence distance.\n");
127 } else {
128 max_correspondence_distance_ = l;
129 }
130 }
131 }
132 if (max_correspondence_distance_ > 0.0) {
133 PrintInfo("ICP with max correspondence distance %.4f.\n",
134 max_correspondence_distance_);
135 auto result = RegistrationICP(*source_copy_ptr_,
136 *target_copy_ptr_, max_correspondence_distance_,
137 Eigen::Matrix4d::Identity(),
138 TransformationEstimationPointToPoint(true),
139 ICPConvergenceCriteria(1e-6, 1e-6, 30));
140 PrintInfo("Registration finished with fitness %.4f and RMSE %.4f.\n",
141 result.fitness_, result.inlier_rmse_);
142 if (result.fitness_ > 0.0) {
143 transformation_ = result.transformation_ * transformation_;
144 PrintTransformation();
145 source_copy_ptr_->Transform(result.transformation_);
146 UpdateGeometry();
147 }
148 } else {
149 PrintInfo("No ICP performed due to illegal max correspondence distance.\n");
150 }
151 return;
152 }
153 case GLFW_KEY_D: {
154 if (use_dialog_) {
155 char buff[DEFAULT_IO_BUFFER_SIZE];
156 sprintf(buff, "%.4f", voxel_size_);
157 const char *str = tinyfd_inputBox("Set voxel size",
158 "Set voxel size (ignored if it is non-positive)",
159 buff);
160 if (str == NULL) {
161 PrintDebug("Dialog closed.\n");
162 return;
163 } else {
164 char *end;
165 errno = 0;
166 double l = std::strtod(str, &end);
167 if (errno == ERANGE && (l == HUGE_VAL || l == -HUGE_VAL)) {
168 PrintDebug("Illegal input, use default voxel size.\n");
169 } else {
170 voxel_size_ = l;
171 }
172 }
173 }
174 if (voxel_size_ > 0.0) {
175 PrintInfo("Voxel downsample with voxel size %.4f.\n",
176 voxel_size_);
177 *source_copy_ptr_ = *VoxelDownSample(*source_copy_ptr_,
178 voxel_size_);
179 UpdateGeometry();
180 } else {
181 PrintInfo("No voxel downsample performed due to illegal voxel size.\n");
182 }
183 return;
184 }
185 case GLFW_KEY_K: {
186 if (!filesystem::FileExists(polygon_filename_)) {
187 if (use_dialog_) {
188 polygon_filename_ = tinyfd_openFileDialog(
189 "Bounding polygon", "polygon.json", 0, NULL, NULL,
190 0);
191 } else {
192 polygon_filename_ = "polygon.json";
193 }
194 }
195 auto polygon_volume = std::make_shared<SelectionPolygonVolume>();
196 if (ReadIJsonConvertible(polygon_filename_, *polygon_volume)) {
197 *source_copy_ptr_ = *polygon_volume->CropPointCloud(
198 *source_copy_ptr_);
199 ResetViewPoint(true);
200 UpdateGeometry();
201 }
202 return;
203 }
204 case GLFW_KEY_E: {
205 std::string default_alignment = default_directory_ +
206 "alignment.json";
207 if (use_dialog_) {
208 filename = tinyfd_saveFileDialog("Alignment session",
209 default_alignment.c_str(), 1, pattern,
210 "JSON file (*.json)");
211 } else {
212 filename = default_alignment.c_str();
213 }
214 if (filename != NULL) {
215 SaveSessionToFile(filename);
216 EvaluateAlignmentAndSave(filename);
217 }
218 return;
219 }
220 }
221 }
222 Visualizer::KeyPressCallback(window, key, scancode, action, mods);
223 }
224
SaveSessionToFile(const std::string & filename)225 bool VisualizerForAlignment::SaveSessionToFile(const std::string &filename)
226 {
227 alignment_session_.source_indices_ = source_visualizer_.GetPickedPoints();
228 alignment_session_.target_indices_ = target_visualizer_.GetPickedPoints();
229 alignment_session_.voxel_size_ = voxel_size_;
230 alignment_session_.max_correspondence_distance_ =
231 max_correspondence_distance_;
232 alignment_session_.with_scaling_ = with_scaling_;
233 alignment_session_.transformation_ = transformation_;
234 return WriteIJsonConvertible(filename, alignment_session_);
235 }
236
LoadSessionFromFile(const std::string & filename)237 bool VisualizerForAlignment::LoadSessionFromFile(const std::string &filename)
238 {
239 if (ReadIJsonConvertible(filename, alignment_session_) == false) {
240 return false;
241 }
242 source_visualizer_.GetPickedPoints() = alignment_session_.source_indices_;
243 target_visualizer_.GetPickedPoints() = alignment_session_.target_indices_;
244 voxel_size_ = alignment_session_.voxel_size_;
245 max_correspondence_distance_ =
246 alignment_session_.max_correspondence_distance_;
247 with_scaling_ = alignment_session_.with_scaling_;
248 transformation_ = alignment_session_.transformation_;
249 *source_copy_ptr_ = *alignment_session_.source_ptr_;
250 source_copy_ptr_->Transform(transformation_);
251 source_visualizer_.UpdateRender();
252 target_visualizer_.UpdateRender();
253 ResetViewPoint(true);
254 return UpdateGeometry();
255 }
256
AlignWithManualAnnotation()257 bool VisualizerForAlignment::AlignWithManualAnnotation()
258 {
259 const auto &source_idx = source_visualizer_.GetPickedPoints();
260 const auto &target_idx = target_visualizer_.GetPickedPoints();
261 if (source_idx.empty() || target_idx.empty() ||
262 source_idx.size() != target_idx.size()) {
263 PrintWarning("# of picked points mismatch: %d in source, %d in target.\n",
264 (int)source_idx.size(), (int)target_idx.size());
265 return false;
266 }
267 TransformationEstimationPointToPoint p2p(with_scaling_);
268 CorrespondenceSet corres;
269 for (size_t i = 0; i < source_idx.size(); i++) {
270 corres.push_back(Eigen::Vector2i(source_idx[i], target_idx[i]));
271 }
272 PrintInfo("Error is %.4f before alignment.\n",
273 p2p.ComputeRMSE(*alignment_session_.source_ptr_,
274 *alignment_session_.target_ptr_, corres));
275 transformation_ = p2p.ComputeTransformation(
276 *alignment_session_.source_ptr_,
277 *alignment_session_.target_ptr_, corres);
278 PrintTransformation();
279 *source_copy_ptr_ = *alignment_session_.source_ptr_;
280 source_copy_ptr_->Transform(transformation_);
281 PrintInfo("Error is %.4f before alignment.\n",
282 p2p.ComputeRMSE(*source_copy_ptr_,
283 *alignment_session_.target_ptr_, corres));
284 return true;
285 }
286
PrintTransformation()287 void VisualizerForAlignment::PrintTransformation()
288 {
289 PrintInfo("Current transformation is:\n");
290 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
291 transformation_(0, 0), transformation_(0, 1),
292 transformation_(0, 2), transformation_(0, 3));
293 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
294 transformation_(1, 0), transformation_(1, 1),
295 transformation_(1, 2), transformation_(1, 3));
296 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
297 transformation_(2, 0), transformation_(2, 1),
298 transformation_(2, 2), transformation_(2, 3));
299 PrintInfo("\t%.6f %.6f %.6f %.6f\n",
300 transformation_(3, 0), transformation_(3, 1),
301 transformation_(3, 2), transformation_(3, 3));
302 }
303
EvaluateAlignmentAndSave(const std::string & filename)304 void VisualizerForAlignment::EvaluateAlignmentAndSave(
305 const std::string &filename)
306 {
307 // Evaluate source_copy_ptr_ and target_copy_ptr_
308 std::string source_filename = filesystem::GetFileNameWithoutExtension(
309 filename) + ".source.ply";
310 std::string target_filename = filesystem::GetFileNameWithoutExtension(
311 filename) + ".target.ply";
312 std::string source_binname = filesystem::GetFileNameWithoutExtension(
313 filename) + ".source.bin";
314 std::string target_binname = filesystem::GetFileNameWithoutExtension(
315 filename) + ".target.bin";
316 FILE * f;
317
318 WritePointCloud(source_filename, *source_copy_ptr_);
319 auto source_dis = ComputePointCloudToPointCloudDistance(
320 *source_copy_ptr_, *target_copy_ptr_);
321 f = fopen(source_binname.c_str(), "wb");
322 fwrite(source_dis.data(), sizeof(double), source_dis.size(), f);
323 fclose(f);
324 WritePointCloud(target_filename, *target_copy_ptr_);
325 auto target_dis = ComputePointCloudToPointCloudDistance(
326 *target_copy_ptr_, *source_copy_ptr_);
327 f = fopen(target_binname.c_str(), "wb");
328 fwrite(target_dis.data(), sizeof(double), target_dis.size(), f);
329 fclose(f);
330 }
331
332 } // namespace three
333