1 ////////////////////////////////////////////////////////////////////////////
2 // File: util.h
3 // Author: Changchang Wu (ccwu@cs.washington.edu)
4 // Description : some utility functions for reading/writing SfM data
5 //
6 // Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu)
7 // and the University of Washington at Seattle
8 //
9 // This library is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU General Public
11 // License as published by the Free Software Foundation; either
12 // Version 3 of the License, or (at your option) any later version.
13 //
14 // This library is distributed in the hope that it will be useful,
15 // but WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 // General Public License for more details.
18 //
19 ////////////////////////////////////////////////////////////////////////////////
20
21 #include <iostream>
22 #include <fstream>
23 #include <vector>
24 #include <string>
25 #include <math.h>
26 #include <time.h>
27 #include <iomanip>
28 #include <algorithm>
29 using namespace std;
30 #include "DataInterface.h"
31
32 namespace pba {
33
34 // File loader supports .nvm format and bundler format
35 bool LoadModelFile(const char* name, vector<CameraT>& camera_data,
36 vector<Point3D>& point_data, vector<Point2D>& measurements,
37 vector<int>& ptidx, vector<int>& camidx,
38 vector<string>& names, vector<int>& ptc);
39 void SaveNVM(const char* filename, vector<CameraT>& camera_data,
40 vector<Point3D>& point_data, vector<Point2D>& measurements,
41 vector<int>& ptidx, vector<int>& camidx, vector<string>& names,
42 vector<int>& ptc);
43 void SaveBundlerModel(const char* filename, vector<CameraT>& camera_data,
44 vector<Point3D>& point_data,
45 vector<Point2D>& measurements, vector<int>& ptidx,
46 vector<int>& camidx);
47
48 //////////////////////////////////////////////////////////////////
49 void AddNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
50 float percent);
51 void AddStableNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
52 const vector<int>& ptidx, const vector<int>& camidx,
53 float percent);
54 bool RemoveInvisiblePoints(vector<CameraT>& camera_data,
55 vector<Point3D>& point_data, vector<int>& ptidx,
56 vector<int>& camidx, vector<Point2D>& measurements,
57 vector<string>& names, vector<int>& ptc);
58
59 /////////////////////////////////////////////////////////////////////////////
LoadNVM(ifstream & in,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx,vector<string> & names,vector<int> & ptc)60 bool LoadNVM(ifstream& in, vector<CameraT>& camera_data,
61 vector<Point3D>& point_data, vector<Point2D>& measurements,
62 vector<int>& ptidx, vector<int>& camidx, vector<string>& names,
63 vector<int>& ptc) {
64 int rotation_parameter_num = 4;
65 bool format_r9t = false;
66 string token;
67 if (in.peek() == 'N') {
68 in >> token; // file header
69 if (strstr(token.c_str(), "R9T")) {
70 rotation_parameter_num = 9; // rotation as 3x3 matrix
71 format_r9t = true;
72 }
73 }
74
75 int ncam = 0, npoint = 0, nproj = 0;
76 // read # of cameras
77 in >> ncam;
78 if (ncam <= 1) return false;
79
80 // read the camera parameters
81 camera_data.resize(ncam); // allocate the camera data
82 names.resize(ncam);
83 for (int i = 0; i < ncam; ++i) {
84 double f, q[9], c[3], d[2];
85 in >> token >> f;
86 for (int j = 0; j < rotation_parameter_num; ++j) in >> q[j];
87 in >> c[0] >> c[1] >> c[2] >> d[0] >> d[1];
88
89 camera_data[i].SetFocalLength(f);
90 if (format_r9t) {
91 camera_data[i].SetMatrixRotation(q);
92 camera_data[i].SetTranslation(c);
93 } else {
94 // older format for compability
95 camera_data[i].SetQuaternionRotation(q); // quaternion from the file
96 camera_data[i].SetCameraCenterAfterRotation(
97 c); // camera center from the file
98 }
99 camera_data[i].SetNormalizedMeasurementDistortion(d[0]);
100 names[i] = token;
101 }
102
103 //////////////////////////////////////
104 in >> npoint;
105 if (npoint <= 0) return false;
106
107 // read image projections and 3D points.
108 point_data.resize(npoint);
109 for (int i = 0; i < npoint; ++i) {
110 float pt[3];
111 int cc[3], npj;
112 in >> pt[0] >> pt[1] >> pt[2] >> cc[0] >> cc[1] >> cc[2] >> npj;
113 for (int j = 0; j < npj; ++j) {
114 int cidx, fidx;
115 float imx, imy;
116 in >> cidx >> fidx >> imx >> imy;
117
118 camidx.push_back(cidx); // camera index
119 ptidx.push_back(i); // point index
120
121 // add a measurment to the vector
122 measurements.push_back(Point2D(imx, imy));
123 nproj++;
124 }
125 point_data[i].SetPoint(pt);
126 ptc.insert(ptc.end(), cc, cc + 3);
127 }
128 ///////////////////////////////////////////////////////////////////////////////
129 std::cout << ncam << " cameras; " << npoint << " 3D points; " << nproj
130 << " projections\n";
131
132 return true;
133 }
134
SaveNVM(const char * filename,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx,vector<string> & names,vector<int> & ptc)135 void SaveNVM(const char* filename, vector<CameraT>& camera_data,
136 vector<Point3D>& point_data, vector<Point2D>& measurements,
137 vector<int>& ptidx, vector<int>& camidx, vector<string>& names,
138 vector<int>& ptc) {
139 std::cout << "Saving model to " << filename << "...\n";
140 ofstream out(filename);
141
142 out << "NVM_V3_R9T\n" << camera_data.size() << '\n' << std::setprecision(12);
143 if (names.size() < camera_data.size())
144 names.resize(camera_data.size(), string("unknown"));
145 if (ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0);
146
147 ////////////////////////////////////
148 for (size_t i = 0; i < camera_data.size(); ++i) {
149 CameraT& cam = camera_data[i];
150 out << names[i] << ' ' << cam.GetFocalLength() << ' ';
151 for (int j = 0; j < 9; ++j) out << cam.m[0][j] << ' ';
152 out << cam.t[0] << ' ' << cam.t[1] << ' ' << cam.t[2] << ' '
153 << cam.GetNormalizedMeasurementDistortion() << " 0\n";
154 }
155
156 out << point_data.size() << '\n';
157
158 for (size_t i = 0, j = 0; i < point_data.size(); ++i) {
159 Point3D& pt = point_data[i];
160 int* pc = &ptc[i * 3];
161 out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << ' ' << pc[0]
162 << ' ' << pc[1] << ' ' << pc[2] << ' ';
163
164 size_t je = j;
165 while (je < ptidx.size() && ptidx[je] == (int)i) je++;
166
167 out << (je - j) << ' ';
168
169 for (; j < je; ++j)
170 out << camidx[j] << ' ' << " 0 " << measurements[j].x << ' '
171 << measurements[j].y << ' ';
172
173 out << '\n';
174 }
175 }
176
LoadBundlerOut(const char * name,ifstream & in,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx,vector<string> & names,vector<int> & ptc)177 bool LoadBundlerOut(const char* name, ifstream& in,
178 vector<CameraT>& camera_data, vector<Point3D>& point_data,
179 vector<Point2D>& measurements, vector<int>& ptidx,
180 vector<int>& camidx, vector<string>& names,
181 vector<int>& ptc) {
182 int rotation_parameter_num = 9;
183 string token;
184 while (in.peek() == '#') std::getline(in, token);
185
186 char listpath[1024], filepath[1024];
187 strcpy(listpath, name);
188 char* ext = strstr(listpath, ".out");
189 strcpy(ext, "-list.txt\0");
190
191 ///////////////////////////////////
192 ifstream listin(listpath);
193 if (!listin.is_open()) {
194 listin.close();
195 listin.clear();
196 char* slash = strrchr(listpath, '/');
197 if (slash == NULL) slash = strrchr(listpath, '\\');
198 slash = slash ? slash + 1 : listpath;
199 strcpy(slash, "image_list.txt");
200 listin.open(listpath);
201 }
202 if (listin) std::cout << "Using image list: " << listpath << '\n';
203
204 // read # of cameras
205 int ncam = 0, npoint = 0, nproj = 0;
206 in >> ncam >> npoint;
207 if (ncam <= 1 || npoint <= 1) return false;
208 std::cout << ncam << " cameras; " << npoint << " 3D points;\n";
209
210 // read the camera parameters
211 camera_data.resize(ncam); // allocate the camera data
212 names.resize(ncam);
213
214 bool det_checked = false;
215 for (int i = 0; i < ncam; ++i) {
216 float f, q[9], c[3], d[2];
217 in >> f >> d[0] >> d[1];
218 for (int j = 0; j < rotation_parameter_num; ++j) in >> q[j];
219 in >> c[0] >> c[1] >> c[2];
220
221 camera_data[i].SetFocalLength(f);
222 camera_data[i].SetInvertedR9T(q, c);
223 camera_data[i].SetProjectionDistortion(d[0]);
224
225 if (listin >> filepath && f != 0) {
226 char* slash = strrchr(filepath, '/');
227 if (slash == NULL) slash = strchr(filepath, '\\');
228 names[i] = (slash ? (slash + 1) : filepath);
229 std::getline(listin, token);
230
231 if (!det_checked) {
232 float det = camera_data[i].GetRotationMatrixDeterminant();
233 std::cout << "Check rotation matrix: " << det << '\n';
234 det_checked = true;
235 }
236 } else {
237 names[i] = "unknown";
238 }
239 }
240
241 // read image projections and 3D points.
242 point_data.resize(npoint);
243 for (int i = 0; i < npoint; ++i) {
244 float pt[3];
245 int cc[3], npj;
246 in >> pt[0] >> pt[1] >> pt[2] >> cc[0] >> cc[1] >> cc[2] >> npj;
247 for (int j = 0; j < npj; ++j) {
248 int cidx, fidx;
249 float imx, imy;
250 in >> cidx >> fidx >> imx >> imy;
251
252 camidx.push_back(cidx); // camera index
253 ptidx.push_back(i); // point index
254
255 // add a measurment to the vector
256 measurements.push_back(Point2D(imx, -imy));
257 nproj++;
258 }
259 point_data[i].SetPoint(pt[0], pt[1], pt[2]);
260 ptc.insert(ptc.end(), cc, cc + 3);
261 }
262 ///////////////////////////////////////////////////////////////////////////////
263 std::cout << ncam << " cameras; " << npoint << " 3D points; " << nproj
264 << " projections\n";
265 return true;
266 }
267
SaveBundlerOut(const char * filename,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx,vector<string> & names,vector<int> & ptc)268 void SaveBundlerOut(const char* filename, vector<CameraT>& camera_data,
269 vector<Point3D>& point_data, vector<Point2D>& measurements,
270 vector<int>& ptidx, vector<int>& camidx,
271 vector<string>& names, vector<int>& ptc) {
272 char listpath[1024];
273 strcpy(listpath, filename);
274 char* ext = strstr(listpath, ".out");
275 if (ext == NULL) return;
276 strcpy(ext, "-list.txt\0");
277
278 ofstream out(filename);
279 out << "# Bundle file v0.3\n";
280 out << std::setprecision(12); // need enough precision
281 out << camera_data.size() << " " << point_data.size() << '\n';
282
283 // save camera data
284 for (size_t i = 0; i < camera_data.size(); ++i) {
285 float q[9], c[3];
286 CameraT& ci = camera_data[i];
287 out << ci.GetFocalLength() << ' ' << ci.GetProjectionDistortion() << " 0\n";
288 ci.GetInvertedR9T(q, c);
289 for (int j = 0; j < 9; ++j) out << q[j] << (((j % 3) == 2) ? '\n' : ' ');
290 out << c[0] << ' ' << c[1] << ' ' << c[2] << '\n';
291 }
292 ///
293 for (size_t i = 0, j = 0; i < point_data.size(); ++i) {
294 int npj = 0, *ci = &ptc[i * 3];
295 Point3D& pt = point_data[i];
296 while (j + npj < point_data.size() && ptidx[j + npj] == ptidx[j]) npj++;
297 ///////////////////////////
298 out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n';
299 out << ci[0] << ' ' << ci[1] << ' ' << ci[2] << '\n';
300 out << npj << ' ';
301 for (int k = 0; k < npj; ++k)
302 out << camidx[j + k] << " 0 " << measurements[j + k].x << ' '
303 << -measurements[j + k].y << '\n';
304 out << '\n';
305 j += npj;
306 }
307
308 ofstream listout(listpath);
309 for (size_t i = 0; i < names.size(); ++i) listout << names[i] << '\n';
310 }
311
312 template <class CameraT, class Point3D>
LoadBundlerModel(ifstream & in,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx)313 bool LoadBundlerModel(ifstream& in, vector<CameraT>& camera_data,
314 vector<Point3D>& point_data,
315 vector<Point2D>& measurements, vector<int>& ptidx,
316 vector<int>& camidx) {
317 // read bundle data from a file
318 size_t ncam = 0, npt = 0, nproj = 0;
319 if (!(in >> ncam >> npt >> nproj)) return false;
320 ///////////////////////////////////////////////////////////////////////////////
321 std::cout << ncam << " cameras; " << npt << " 3D points; " << nproj
322 << " projections\n";
323
324 camera_data.resize(ncam);
325 point_data.resize(npt);
326 measurements.resize(nproj);
327 camidx.resize(nproj);
328 ptidx.resize(nproj);
329
330 for (size_t i = 0; i < nproj; ++i) {
331 double x, y;
332 int cidx, pidx;
333 in >> cidx >> pidx >> x >> y;
334 if (((size_t)pidx) == npt && camidx.size() > i) {
335 camidx.resize(i);
336 ptidx.resize(i);
337 measurements.resize(i);
338 std::cout << "Truncate measurements to " << i << '\n';
339 } else if (((size_t)pidx) >= npt) {
340 continue;
341 } else {
342 camidx[i] = cidx;
343 ptidx[i] = pidx;
344 measurements[i].SetPoint2D(x, -y);
345 }
346 }
347
348 for (size_t i = 0; i < ncam; ++i) {
349 double p[9];
350 for (int j = 0; j < 9; ++j) in >> p[j];
351 CameraT& cam = camera_data[i];
352 cam.SetFocalLength(p[6]);
353 cam.SetInvertedRT(p, p + 3);
354 cam.SetProjectionDistortion(p[7]);
355 }
356
357 for (size_t i = 0; i < npt; ++i) {
358 double pt[3];
359 in >> pt[0] >> pt[1] >> pt[2];
360 point_data[i].SetPoint(pt);
361 }
362 return true;
363 }
364
SaveBundlerModel(const char * filename,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx)365 void SaveBundlerModel(const char* filename, vector<CameraT>& camera_data,
366 vector<Point3D>& point_data,
367 vector<Point2D>& measurements, vector<int>& ptidx,
368 vector<int>& camidx) {
369 std::cout << "Saving model to " << filename << "...\n";
370 ofstream out(filename);
371 out << std::setprecision(12); // need enough precision
372 out << camera_data.size() << ' ' << point_data.size() << ' '
373 << measurements.size() << '\n';
374 for (size_t i = 0; i < measurements.size(); ++i) {
375 out << camidx[i] << ' ' << ptidx[i] << ' ' << measurements[i].x << ' '
376 << -measurements[i].y << '\n';
377 }
378
379 for (size_t i = 0; i < camera_data.size(); ++i) {
380 CameraT& cam = camera_data[i];
381 double r[3], t[3];
382 cam.GetInvertedRT(r, t);
383 out << r[0] << ' ' << r[1] << ' ' << r[2] << ' ' << t[0] << ' ' << t[1]
384 << ' ' << t[2] << ' ' << cam.f << ' ' << cam.GetProjectionDistortion()
385 << " 0\n";
386 }
387
388 for (size_t i = 0; i < point_data.size(); ++i) {
389 Point3D& pt = point_data[i];
390 out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n';
391 }
392 }
393
LoadModelFile(const char * name,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx,vector<string> & names,vector<int> & ptc)394 bool LoadModelFile(const char* name, vector<CameraT>& camera_data,
395 vector<Point3D>& point_data, vector<Point2D>& measurements,
396 vector<int>& ptidx, vector<int>& camidx,
397 vector<string>& names, vector<int>& ptc) {
398 if (name == NULL) return false;
399 ifstream in(name);
400
401 std::cout << "Loading cameras/points: " << name << "\n";
402 if (!in.is_open()) return false;
403
404 if (strstr(name, ".nvm"))
405 return LoadNVM(in, camera_data, point_data, measurements, ptidx, camidx,
406 names, ptc);
407 else if (strstr(name, ".out"))
408 return LoadBundlerOut(name, in, camera_data, point_data, measurements,
409 ptidx, camidx, names, ptc);
410 else
411 return LoadBundlerModel(in, camera_data, point_data, measurements, ptidx,
412 camidx);
413 }
414
random_ratio(float percent)415 float random_ratio(float percent) {
416 return (rand() % 101 - 50) * 0.02f * percent + 1.0f;
417 }
418
AddNoise(vector<CameraT> & camera_data,vector<Point3D> & point_data,float percent)419 void AddNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
420 float percent) {
421 std::srand((unsigned int)time(NULL));
422 for (size_t i = 0; i < camera_data.size(); ++i) {
423 camera_data[i].f *= random_ratio(percent);
424 camera_data[i].t[0] *= random_ratio(percent);
425 camera_data[i].t[1] *= random_ratio(percent);
426 camera_data[i].t[2] *= random_ratio(percent);
427 double e[3];
428 camera_data[i].GetRodriguesRotation(e);
429 e[0] *= random_ratio(percent);
430 e[1] *= random_ratio(percent);
431 e[2] *= random_ratio(percent);
432 camera_data[i].SetRodriguesRotation(e);
433 }
434
435 for (size_t i = 0; i < point_data.size(); ++i) {
436 point_data[i].xyz[0] *= random_ratio(percent);
437 point_data[i].xyz[1] *= random_ratio(percent);
438 point_data[i].xyz[2] *= random_ratio(percent);
439 }
440 }
441
AddStableNoise(vector<CameraT> & camera_data,vector<Point3D> & point_data,const vector<int> & ptidx,const vector<int> & camidx,float percent)442 void AddStableNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
443 const vector<int>& ptidx, const vector<int>& camidx,
444 float percent) {
445 ///
446 std::srand((unsigned int)time(NULL));
447 // do not modify the visibility status..
448 vector<float> zz0(ptidx.size());
449 vector<CameraT> backup = camera_data;
450 vector<float> vx(point_data.size()), vy(point_data.size()),
451 vz(point_data.size());
452 for (size_t i = 0; i < point_data.size(); ++i) {
453 Point3D& pt = point_data[i];
454 vx[i] = pt.xyz[0];
455 vy[i] = pt.xyz[1];
456 vz[i] = pt.xyz[2];
457 }
458
459 // find out the median location of all the 3D points.
460 size_t median_idx = point_data.size() / 2;
461
462 std::nth_element(vx.begin(), vx.begin() + median_idx, vx.end());
463 std::nth_element(vy.begin(), vy.begin() + median_idx, vy.end());
464 std::nth_element(vz.begin(), vz.begin() + median_idx, vz.end());
465 float cx = vx[median_idx], cy = vy[median_idx], cz = vz[median_idx];
466
467 for (size_t i = 0; i < ptidx.size(); ++i) {
468 CameraT& cam = camera_data[camidx[i]];
469 Point3D& pt = point_data[ptidx[i]];
470 zz0[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
471 cam.m[2][2] * pt.xyz[2] + cam.t[2];
472 }
473
474 vector<float> z2 = zz0;
475 median_idx = ptidx.size() / 2;
476 std::nth_element(z2.begin(), z2.begin() + median_idx, z2.end());
477 float mz = z2[median_idx]; // median depth
478 float dist_noise_base = mz * 0.2f;
479
480 /////////////////////////////////////////////////
481 // modify points first..
482 for (size_t i = 0; i < point_data.size(); ++i) {
483 Point3D& pt = point_data[i];
484 pt.xyz[0] = pt.xyz[0] - cx + dist_noise_base * random_ratio(percent);
485 pt.xyz[1] = pt.xyz[1] - cy + dist_noise_base * random_ratio(percent);
486 pt.xyz[2] = pt.xyz[2] - cz + dist_noise_base * random_ratio(percent);
487 }
488
489 vector<bool> need_modification(camera_data.size(), true);
490 int invalid_count = 0, modify_iteration = 1;
491
492 do {
493 if (invalid_count)
494 std::cout << "NOTE" << std::setw(2) << modify_iteration << ": modify "
495 << invalid_count << " camera to fix visibility\n";
496
497 //////////////////////////////////////////////////////
498 for (size_t i = 0; i < camera_data.size(); ++i) {
499 if (!need_modification[i]) continue;
500 CameraT& cam = camera_data[i];
501 double e[3], c[3];
502 cam = backup[i];
503 cam.f *= random_ratio(percent);
504
505 ///////////////////////////////////////////////////////////
506 cam.GetCameraCenter(c);
507 c[0] = c[0] - cx + dist_noise_base * random_ratio(percent);
508 c[1] = c[1] - cy + dist_noise_base * random_ratio(percent);
509 c[2] = c[2] - cz + dist_noise_base * random_ratio(percent);
510
511 ///////////////////////////////////////////////////////////
512 cam.GetRodriguesRotation(e);
513 e[0] *= random_ratio(percent);
514 e[1] *= random_ratio(percent);
515 e[2] *= random_ratio(percent);
516
517 ///////////////////////////////////////////////////////////
518 cam.SetRodriguesRotation(e);
519 cam.SetCameraCenterAfterRotation(c);
520 }
521 vector<bool> invalidc(camera_data.size(), false);
522
523 invalid_count = 0;
524 for (size_t i = 0; i < ptidx.size(); ++i) {
525 int cid = camidx[i];
526 if (need_modification[cid] == false) continue;
527 if (invalidc[cid]) continue;
528 CameraT& cam = camera_data[cid];
529 Point3D& pt = point_data[ptidx[i]];
530 float z = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
531 cam.m[2][2] * pt.xyz[2] + cam.t[2];
532 if (z * zz0[i] > 0) continue;
533 if (zz0[i] == 0 && z > 0) continue;
534 invalid_count++;
535 invalidc[cid] = true;
536 }
537
538 need_modification = invalidc;
539 modify_iteration++;
540
541 } while (invalid_count && modify_iteration < 20);
542 }
543
ExamineVisiblity(const char * input_filename)544 void ExamineVisiblity(const char* input_filename) {
545 //////////////
546 vector<CameraD> camera_data;
547 vector<Point3B> point_data;
548 vector<int> ptidx, camidx;
549 vector<Point2D> measurements;
550 ifstream in(input_filename);
551 LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, camidx);
552
553 ////////////////
554 int count = 0;
555 double d1 = 100, d2 = 100;
556 std::cout << "checking visibility...\n";
557 vector<double> zz(ptidx.size());
558 for (size_t i = 0; i < ptidx.size(); ++i) {
559 CameraD& cam = camera_data[camidx[i]];
560 Point3B& pt = point_data[ptidx[i]];
561 double dz = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
562 cam.m[2][2] * pt.xyz[2] + cam.t[2];
563 // double dx = cam.m[0][0] * pt.xyz[0] + cam.m[0][1] * pt.xyz[1] +
564 // cam.m[0][2] * pt.xyz[2] + cam.t[0];
565 // double dy = cam.m[1][0] * pt.xyz[0] + cam.m[1][1] * pt.xyz[1] +
566 // cam.m[1][2] * pt.xyz[2] + cam.t[1];
567
568 ////////////////////////////////////////
569 float c[3];
570 cam.GetCameraCenter(c);
571
572 CameraT camt;
573 camt.SetCameraT(cam);
574 Point3D ptt;
575 ptt.SetPoint(pt.xyz);
576 double fz = camt.m[2][0] * ptt.xyz[0] + camt.m[2][1] * ptt.xyz[1] +
577 camt.m[2][2] * ptt.xyz[2] + camt.t[2];
578 double fz2 = camt.m[2][0] * (ptt.xyz[0] - c[0]) +
579 camt.m[2][1] * (ptt.xyz[1] - c[1]) +
580 camt.m[2][2] * (ptt.xyz[2] - c[2]);
581
582 // if(dz == 0 && fz == 0) continue;
583
584 if (dz * fz <= 0 || fz == 0) {
585 std::cout << "cam "
586 << camidx[i] //<<// "; dx = " << dx << "; dy = " << dy
587 << "; double: " << dz << "; float " << fz << "; float2 " << fz2
588 << "\n";
589 // std::cout << cam.m[2][0] << " "<<cam.m[2][1]<< " " << cam.m[2][2] << "
590 // "<<cam.t[2] << "\n";
591 // std::cout << camt.m[2][0] << " "<<camt.m[2][1]<< " " << camt.m[2][2]
592 // << " "<<camt.t[2] << "\n";
593 // std::cout << cam.m[2][0] - camt.m[2][0] << " " <<cam.m[2][1] -
594 // camt.m[2][1]<< " "
595 // << cam.m[2][2] - camt.m[2][2] << " " <<cam.t[2] - camt.t[2]<<
596 // "\n";
597 }
598
599 zz[i] = dz;
600 d1 = std::min(fabs(dz), d1);
601 d2 = std::min(fabs(fz), d2);
602 }
603
604 std::cout << count << " points moved to wrong side " << d1 << ", " << d2
605 << "\n";
606 }
607
RemoveInvisiblePoints(vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<int> & ptidx,vector<int> & camidx,vector<Point2D> & measurements,vector<string> & names,vector<int> & ptc)608 bool RemoveInvisiblePoints(vector<CameraT>& camera_data,
609 vector<Point3D>& point_data, vector<int>& ptidx,
610 vector<int>& camidx, vector<Point2D>& measurements,
611 vector<string>& names, vector<int>& ptc) {
612 vector<float> zz(ptidx.size());
613 for (size_t i = 0; i < ptidx.size(); ++i) {
614 CameraT& cam = camera_data[camidx[i]];
615 Point3D& pt = point_data[ptidx[i]];
616 zz[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
617 cam.m[2][2] * pt.xyz[2] + cam.t[2];
618 }
619 size_t median_idx = ptidx.size() / 2;
620 std::nth_element(zz.begin(), zz.begin() + median_idx, zz.end());
621 float dist_threshold = zz[median_idx] * 0.001f;
622
623 // keep removing 3D points. until all of them are infront of the cameras..
624 vector<bool> pmask(point_data.size(), true);
625 int points_removed = 0;
626 for (size_t i = 0; i < ptidx.size(); ++i) {
627 int cid = camidx[i], pid = ptidx[i];
628 if (!pmask[pid]) continue;
629 CameraT& cam = camera_data[cid];
630 Point3D& pt = point_data[pid];
631 bool visible = (cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
632 cam.m[2][2] * pt.xyz[2] + cam.t[2] >
633 dist_threshold);
634 pmask[pid] = visible; // this point should be removed
635 if (!visible) points_removed++;
636 }
637 if (points_removed == 0) return false;
638 vector<int> cv(camera_data.size(), 0);
639 // should any cameras be removed ?
640 int min_observation = 20; // cameras should see at leat 20 points
641
642 do {
643 // count visible points for each camera
644 std::fill(cv.begin(), cv.end(), 0);
645 for (size_t i = 0; i < ptidx.size(); ++i) {
646 int cid = camidx[i], pid = ptidx[i];
647 if (pmask[pid]) cv[cid]++;
648 }
649
650 // check if any more points should be removed
651 vector<int> pv(point_data.size(), 0);
652 for (size_t i = 0; i < ptidx.size(); ++i) {
653 int cid = camidx[i], pid = ptidx[i];
654 if (!pmask[pid]) continue; // point already removed
655 if (cv[cid] < min_observation) // this camera shall be removed.
656 {
657 ///
658 } else {
659 pv[pid]++;
660 }
661 }
662
663 points_removed = 0;
664 for (size_t i = 0; i < point_data.size(); ++i) {
665 if (pmask[i] == false) continue;
666 if (pv[i] >= 2) continue;
667 pmask[i] = false;
668 points_removed++;
669 }
670 } while (points_removed > 0);
671
672 ////////////////////////////////////
673 vector<bool> cmask(camera_data.size(), true);
674 for (size_t i = 0; i < camera_data.size(); ++i)
675 cmask[i] = cv[i] >= min_observation;
676 ////////////////////////////////////////////////////////
677
678 vector<int> cidx(camera_data.size());
679 vector<int> pidx(point_data.size());
680
681 /// modified model.
682 vector<CameraT> camera_data2;
683 vector<Point3D> point_data2;
684 vector<int> ptidx2;
685 vector<int> camidx2;
686 vector<Point2D> measurements2;
687 vector<string> names2;
688 vector<int> ptc2;
689
690 //
691 if (names.size() < camera_data.size())
692 names.resize(camera_data.size(), string("unknown"));
693 if (ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0);
694
695 //////////////////////////////
696 int new_camera_count = 0, new_point_count = 0;
697 for (size_t i = 0; i < camera_data.size(); ++i) {
698 if (!cmask[i]) continue;
699 camera_data2.push_back(camera_data[i]);
700 names2.push_back(names[i]);
701 cidx[i] = new_camera_count++;
702 }
703
704 for (size_t i = 0; i < point_data.size(); ++i) {
705 if (!pmask[i]) continue;
706 point_data2.push_back(point_data[i]);
707 ptc.push_back(ptc[i]);
708 pidx[i] = new_point_count++;
709 }
710
711 int new_observation_count = 0;
712 for (size_t i = 0; i < ptidx.size(); ++i) {
713 int pid = ptidx[i], cid = camidx[i];
714 if (!pmask[pid] || !cmask[cid]) continue;
715 ptidx2.push_back(pidx[pid]);
716 camidx2.push_back(cidx[cid]);
717 measurements2.push_back(measurements[i]);
718 new_observation_count++;
719 }
720
721 std::cout << "NOTE: removing " << (camera_data.size() - new_camera_count)
722 << " cameras; " << (point_data.size() - new_point_count)
723 << " 3D Points; " << (measurements.size() - new_observation_count)
724 << " Observations;\n";
725
726 camera_data2.swap(camera_data);
727 names2.swap(names);
728 point_data2.swap(point_data);
729 ptc2.swap(ptc);
730 ptidx2.swap(ptidx);
731 camidx2.swap(camidx);
732 measurements2.swap(measurements);
733
734 return true;
735 }
736
SaveModelFile(const char * outpath,vector<CameraT> & camera_data,vector<Point3D> & point_data,vector<Point2D> & measurements,vector<int> & ptidx,vector<int> & camidx,vector<string> & names,vector<int> & ptc)737 void SaveModelFile(const char* outpath, vector<CameraT>& camera_data,
738 vector<Point3D>& point_data, vector<Point2D>& measurements,
739 vector<int>& ptidx, vector<int>& camidx,
740 vector<string>& names, vector<int>& ptc) {
741 if (outpath == NULL) return;
742 if (strstr(outpath, ".nvm"))
743 SaveNVM(outpath, camera_data, point_data, measurements, ptidx, camidx,
744 names, ptc);
745 else if (strstr(outpath, ".out"))
746 SaveBundlerOut(outpath, camera_data, point_data, measurements, ptidx,
747 camidx, names, ptc);
748 else
749 SaveBundlerModel(outpath, camera_data, point_data, measurements, ptidx,
750 camidx);
751 }
752
753 } // namespace pba
754