1 #include <iostream>
2 #include "testlib/testlib_test.h"
3 #if 0
4 # include "testlib/testlib_root_dir.h"
5 #endif
6 #ifdef _MSC_VER
7 # include "vcl_msvc_warnings.h"
8 #endif
9 #include "vnl/vnl_fwd.h"
10 #include "vnl/vnl_vector_fixed.h"
11 #include "vnl/vnl_matrix_fixed.h"
12 #include "vnl/vnl_random.h"
13 #include "vgl/vgl_homg_point_3d.h"
14 #include "vgl/vgl_point_3d.h"
15 #include "vgl/vgl_point_2d.h"
16 #include "vgl/vgl_distance.h"
17 #include <vgl/algo/vgl_h_matrix_3d.h>
18 #include <vgl/algo/vgl_rotation_3d.h>
19 #include <vpgl/algo/vpgl_camera_compute.h>
20 #include <vpgl/algo/vpgl_calibration_matrix_compute.h>
21 #include <vpgl/algo/vpgl_ray.h>
22 #include "vpgl/vpgl_proj_camera.h"
23 #include "vpgl/vpgl_affine_camera.h"
24 #include "vpgl/vpgl_perspective_camera.h"
25 #include "vpgl/vpgl_calibration_matrix.h"
26
27 static void
test_camera_compute_setup()28 test_camera_compute_setup()
29 {
30 // PART 1: Test the affine camera computation
31
32 vnl_vector_fixed<double, 4> r1(1, 2, 3, 4);
33 vnl_vector_fixed<double, 4> r2(-1, 4, -2, 0);
34 vpgl_affine_camera<double> C1(r1, r2);
35 std::vector<vgl_point_3d<double>> world_pts;
36 world_pts.emplace_back(1, 0, -1);
37 world_pts.emplace_back(6, 1, 2);
38 world_pts.emplace_back(-1, -3, -2);
39 world_pts.emplace_back(0, 0, 2);
40 world_pts.emplace_back(2, -1, -5);
41 world_pts.emplace_back(8, 1, -2);
42 world_pts.emplace_back(-4, -4, 5);
43 world_pts.emplace_back(-1, 3, 4);
44 world_pts.emplace_back(1, 2, -7);
45 std::vector<vgl_point_2d<double>> image_pts;
46 image_pts.reserve(world_pts.size());
47 for (const auto & world_pt : world_pts)
48 image_pts.emplace_back(C1.project(vgl_homg_point_3d<double>(world_pt)));
49
50 vpgl_affine_camera<double> C1e;
51 vpgl_affine_camera_compute::compute(image_pts, world_pts, C1e);
52
53 std::cerr << "\nTrue camera matrix:\n"
54 << C1.get_matrix() << '\n'
55 << "\nEstimated camera matrix:\n"
56 << C1e.get_matrix() << '\n';
57 TEST_NEAR("vpgl_affine_camera_compute:", (C1.get_matrix() - C1e.get_matrix()).frobenius_norm(), 0, 1);
58 }
59
60 void
test_perspective_compute()61 test_perspective_compute()
62 {
63 std::cout << "Test Perspective Compute\n";
64 vnl_vector_fixed<double, 3> rv, trans;
65 for (unsigned i = 0; i < 3; ++i)
66 rv[i] = 0.9068996774314604; // axis along diagonal, rotation of 90 degrees
67 vgl_rotation_3d<double> rr(rv);
68
69 trans[0] = 10.0;
70 trans[1] = 20.0;
71 trans[2] = 30;
72
73 vnl_matrix<double> Y(3, 5);
74 Y[0][0] = 1.1;
75 Y[1][0] = -0.05;
76 Y[2][0] = 0.01;
77 Y[0][1] = 0.02;
78 Y[1][1] = 0.995;
79 Y[2][1] = -0.1;
80 Y[0][2] = -0.01;
81 Y[1][2] = 0.04;
82 Y[2][2] = 1.04;
83 Y[0][3] = 1.15;
84 Y[1][3] = 0.97;
85 Y[2][3] = -0.1;
86 Y[0][4] = 1.01;
87 Y[1][4] = 1.03;
88 Y[2][4] = 0.96;
89
90 vnl_matrix<double> J(4, 6);
91 for (unsigned c = 0; c < 5; ++c)
92 {
93 for (unsigned r = 0; r < 3; ++r)
94 J[r][c] = Y[r][c];
95 J[3][c] = 1.0;
96 }
97 J[0][5] = 0.5;
98 J[1][5] = 1.0;
99 J[2][5] = -0.5;
100 J[3][5] = 1.0;
101
102 vnl_matrix_fixed<double, 3, 3> pr = rr.as_matrix();
103 vnl_matrix_fixed<double, 3, 4> P;
104 for (unsigned r = 0; r < 3; ++r)
105 {
106 for (unsigned c = 0; c < 3; ++c)
107 P[r][c] = pr[r][c];
108 P[r][3] = trans[r];
109 }
110 // Project the 3-d points
111 vnl_matrix<double> Z(2, 6);
112 for (unsigned c = 0; c < 6; ++c)
113 {
114 vnl_vector_fixed<double, 4> vpr;
115 for (unsigned r = 0; r < 4; ++r)
116 vpr[r] = J[r][c];
117 vnl_vector_fixed<double, 3> pvpr = P * vpr;
118 for (unsigned r = 0; r < 2; ++r)
119 Z[r][c] = pvpr[r] / pvpr[2];
120 }
121 std::cout << "Projected points\n " << Z << '\n';
122 std::vector<vgl_point_2d<double>> image_pts;
123 std::vector<vgl_point_3d<double>> world_pts;
124 for (unsigned i = 0; i < 6; ++i)
125 {
126 vgl_point_2d<double> ip(Z[0][i], Z[1][i]);
127 vgl_point_3d<double> wp(J[0][i], J[1][i], J[2][i]);
128 image_pts.push_back(ip);
129 world_pts.push_back(wp);
130 }
131 vpgl_calibration_matrix<double> K;
132 vpgl_perspective_camera<double> pc;
133
134 vpgl_perspective_camera_compute::compute(image_pts, world_pts, K, pc);
135 std::cout << pc << '\n';
136 vgl_point_3d<double> cc = pc.get_camera_center();
137 TEST_NEAR("perspective camera from 6 points exact", cc.z(), -14.2265, 0.001);
138 }
139 // Tests the compute(world_pts, image_pts, camera) method in
140 // the vpgl_camera_compute class
141 static void
test_perspective_compute_direct_linear_transform()142 test_perspective_compute_direct_linear_transform()
143 {
144 // Create the world points
145 std::vector<vgl_point_3d<double>> world_pts;
146 world_pts.emplace_back(1, 0, -1);
147 world_pts.emplace_back(6, 1, 2);
148 world_pts.emplace_back(-1, -3, -2);
149 world_pts.emplace_back(0, 0, 2);
150 world_pts.emplace_back(2, -1, -5);
151 world_pts.emplace_back(8, 1, -2);
152
153 // Come up with the projection matrix.
154 vnl_matrix_fixed<double, 3, 4> proj;
155 proj.set(0, 0, 0);
156 proj.set(0, 1, 1);
157 proj.set(0, 2, 0);
158 proj.set(0, 3, 3);
159 proj.set(1, 0, -1);
160 proj.set(1, 1, 0);
161 proj.set(1, 2, 0);
162 proj.set(1, 3, 2);
163 proj.set(2, 0, 0);
164 proj.set(2, 1, 0);
165 proj.set(2, 2, 1);
166 proj.set(2, 3, 0);
167
168 // Do the projection for each of the points
169 std::vector<vgl_point_2d<double>> image_pts;
170 for (auto & i : world_pts)
171 {
172 vnl_vector_fixed<double, 4> world_pt;
173 world_pt[0] = i.x();
174 world_pt[1] = i.y();
175 world_pt[2] = i.z();
176 world_pt[3] = 1.0;
177
178 vnl_vector_fixed<double, 3> projed_pt = proj * world_pt;
179
180 image_pts.emplace_back(projed_pt[0] / projed_pt[2], projed_pt[1] / projed_pt[2]);
181 }
182
183 // Calculate the projected points
184 vpgl_perspective_camera<double> camera;
185 double err;
186 vpgl_perspective_camera_compute::compute_dlt(image_pts, world_pts, camera, err);
187
188 TEST_NEAR("Small error.", err, 0, .1);
189
190 // Check that it is close.
191 for (unsigned int i = 0; i < world_pts.size(); i++)
192 {
193 double x, y;
194 camera.project(world_pts[i].x(), world_pts[i].y(), world_pts[i].z(), x, y);
195
196 TEST_NEAR("Testing that x coord is close", x, image_pts[i].x(), .001);
197 TEST_NEAR("Testing that y coord is close", y, image_pts[i].y(), .001);
198 }
199 }
200
201 static void
test_perspective_compute_ground()202 test_perspective_compute_ground()
203 {
204 vpgl_calibration_matrix<double> trueK(1680, vgl_point_2d<double>(959.5, 539.5));
205 vgl_rotation_3d<double> trueR(vnl_vector_fixed<double, 3>(1.87379, 0.0215981, -0.0331475));
206 vgl_point_3d<double> trueC(14.5467, -6.71791, 4.79478);
207 vpgl_perspective_camera<double> trueP(trueK, trueC, trueR);
208
209 // generate some points on the ground
210 std::vector<vgl_point_2d<double>> ground_pts;
211 ground_pts.emplace_back(14.3256, 5.7912);
212 ground_pts.emplace_back(14.3256, 9.4488);
213 ground_pts.emplace_back(14.3256, 15.24);
214 ground_pts.emplace_back(5.7404, 9.398);
215 ground_pts.emplace_back(22.8092, 9.398);
216
217 // project them to the image
218 std::vector<vgl_point_2d<double>> image_pts;
219 for (auto & ground_pt : ground_pts)
220 {
221 vgl_homg_point_3d<double> world_pt(ground_pt.x(), ground_pt.y(), 0, 1);
222
223 vgl_point_2d<double> img_pt = trueP.project(world_pt);
224 assert(img_pt.x() >= 0);
225 assert(img_pt.x() <= 1920);
226 assert(img_pt.y() >= 0);
227 assert(img_pt.y() <= 1080);
228
229 image_pts.push_back(img_pt);
230 }
231
232 vpgl_perspective_camera<double> P;
233 P.set_calibration(trueK);
234
235 bool did_compute = vpgl_perspective_camera_compute::compute(image_pts, ground_pts, P);
236
237 TEST("Calibrate from ground<->image correspondences", did_compute, true);
238 TEST_NEAR(" C", vgl_distance(trueC, P.get_camera_center()), 0, 1e-6);
239 TEST_NEAR(" R", (trueR.as_matrix() - P.get_rotation().as_matrix()).frobenius_norm(), 0, 1e-6);
240 }
241
242 static void
test_calibration_compute_natural()243 test_calibration_compute_natural()
244 {
245 vpgl_calibration_matrix<double> trueK(1680, vgl_point_2d<double>(959.5, 539.5));
246 vgl_rotation_3d<double> trueR(vnl_vector_fixed<double, 3>(1.87379, 0.0215981, -0.0331475));
247 vgl_point_3d<double> trueC(14.5467, -6.71791, 4.79478);
248 vpgl_perspective_camera<double> trueP(trueK, trueC, trueR);
249
250 // generate some points on the ground
251 std::vector<vgl_point_2d<double>> ground_pts;
252 ground_pts.emplace_back(14.3256, 5.7912);
253 ground_pts.emplace_back(14.3256, 9.4488);
254 ground_pts.emplace_back(14.3256, 15.24);
255 ground_pts.emplace_back(5.7404, 9.398);
256 ground_pts.emplace_back(22.8092, 9.398);
257
258 // project them to the image
259 std::vector<vgl_point_2d<double>> image_pts;
260 for (auto & ground_pt : ground_pts)
261 {
262 vgl_homg_point_3d<double> world_pt(ground_pt.x(), ground_pt.y(), 0, 1);
263
264 vgl_point_2d<double> img_pt = trueP.project(world_pt);
265 assert(img_pt.x() >= 0);
266 assert(img_pt.x() <= 1920);
267 assert(img_pt.y() >= 0);
268 assert(img_pt.y() <= 1080);
269
270 image_pts.push_back(img_pt);
271 }
272
273 vpgl_calibration_matrix<double> K;
274 bool did_compute = vpgl_calibration_matrix_compute::natural(image_pts, ground_pts, trueK.principal_point(), K);
275
276 TEST("Calibrate natural intrinsics from correspondences", did_compute, true);
277 TEST_NEAR(" K Discrepancy", (trueK.get_matrix() - K.get_matrix()).frobenius_norm(), 0, 1e-6);
278 }
279
280 static void
test_compute_affine()281 test_compute_affine()
282 {
283 std::vector<vgl_point_3d<double>> pts_3d;
284 pts_3d.emplace_back(31.191099166870, 121.919998168945, 53.835998535156);
285 pts_3d.emplace_back(67.191101074219, 66.720397949219, 74.165496826172);
286 pts_3d.emplace_back(54.891101837158, 97.920303344727, 66.359802246094);
287 pts_3d.emplace_back(50.991100311279, 80.220397949219, 67.610298156738);
288 pts_3d.emplace_back(28.791099548340, 67.320396423340, 67.759902954102);
289 pts_3d.emplace_back(53.091098785400, 40.920299530029, 68.854202270508);
290 pts_3d.emplace_back(43.791099548340, 49.620399475098, 67.872703552246);
291 pts_3d.emplace_back(93.290100097656, 30.420299530029, 79.595397949219);
292 pts_3d.emplace_back(94.790100097656, 52.020401000977, 74.720100402832);
293 pts_3d.emplace_back(100.489997863770, 86.520401000977, 68.012802124023);
294 pts_3d.emplace_back(91.790100097656, 102.419998168945, 68.957199096680);
295 pts_3d.emplace_back(87.891098022461, 127.620002746582, 67.613098144531);
296 pts_3d.emplace_back(31.491100311279, 107.220001220703, 60.077598571777);
297 pts_3d.emplace_back(59.391101837158, 115.319999694824, 61.324699401855);
298 pts_3d.emplace_back(59.991100311279, 133.320007324219, 62.057498931885);
299 pts_3d.emplace_back(36.891101837158, 144.119995117188, 55.506599426270);
300 pts_3d.emplace_back(18.891099929810, 111.419998168945, 57.615100860596);
301 pts_3d.emplace_back(19.491100311279, 78.720397949219, 85.827499389648);
302 pts_3d.emplace_back(26.991100311279, 35.220401763916, 83.151901245117);
303 pts_3d.emplace_back(61.191101074219, 16.320400238037, 68.855102539063);
304 pts_3d.emplace_back(127.489997863770, 32.220401763916, 96.971298217773);
305 pts_3d.emplace_back(123.290000915527, 16.920299530029, 99.897903442383);
306 pts_3d.emplace_back(122.089996337891, 92.220397949219, 69.267196655273);
307 pts_3d.emplace_back(85.491096496582, 92.820396423340, 67.331802368164);
308 vnl_vector_fixed<double, 4> row00, row01;
309 row00[0] = 2.13641;
310 row00[1] = 0.000728937;
311 row00[2] = -0.103897;
312 row00[3] = 253.827;
313 row01[0] = -0.0107484;
314 row01[1] = -2.13683;
315 row01[2] = -0.229387;
316 row01[3] = 648.49;
317 vpgl_affine_camera<double> acam(row00, row01), fitted_acam;
318 std::vector<vgl_point_2d<double>> pts_2d;
319 for (const auto & i : pts_3d)
320 {
321 pts_2d.emplace_back(acam.project(i));
322 }
323 bool good = vpgl_affine_camera_compute::compute(pts_2d, pts_3d, fitted_acam);
324 double er0 = 0.0;
325 vnl_matrix_fixed<double, 3, 4> Mgt = acam.get_matrix(), Mfitted = fitted_acam.get_matrix();
326 for (size_t r = 0; r < 2; ++r)
327 for (size_t c = 0; c < 2; ++c)
328 {
329 er0 += fabs(Mgt[r][c] - Mfitted[r][c]);
330 }
331 good = good && er0 < 1.0e-6;
332 TEST("compute affine camera from pts", good, true);
333 }
334
335 static void
test_compute_rational()336 test_compute_rational()
337 {
338 double neu_u1[20] = { 8.96224e-005, 5.01302e-005, -7.67889e-006, -0.010342, -6.89891e-005,
339 7.97337e-006, 0.00149824, -1.38598e-005, 0.000188125, 0.980305,
340 -6.26076e-005, 2.9209e-006, -0.0021416, 3.32719e-008, -0.000322074,
341 -7.1486e-005, -2.9938e-007, 1.36383e-005, 0.023163, 0.0108915 };
342 double den_u1[20] = { 7.47152e-007,
343 1.90129e-006,
344 -1.90195e-007,
345 -1.31851e-005,
346 1.99123e-006,
347 -3.32589e-008,
348 -3.46297e-005,
349 -1.27463e-007,
350 5.46123e-006,
351 -0.000542706,
352 1.32208e-006,
353 -6.82432e-008,
354 7.26265e-005,
355 3.35976e-008,
356 -3.78315e-006,
357 -0.00145714,
358 0,
359 -1.42542e-005,
360 -0.000437505,
361 1 };
362 double neu_v1[20] = { -8.19635e-006, -4.83573e-005, 7.79866e-007, -0.00106039, -5.42129e-006, 0,
363 -0.00193506, -6.93011e-006, 4.74326e-005, -0.132446, -0.000200527, -2.45944e-006,
364 0.00300919, -5.85355e-005, 0.000148533, -1.12081, 5.08764e-007, -2.81721e-006,
365 0.0116155, -0.00375141 };
366 double den_v1[20] = { -6.9545e-007, -1.47008e-005, 2.14085e-007, 2.83534e-005, -0.000119086,
367 2.32147e-006, -3.8376e-005, -2.77014e-007, 5.07729e-007, -0.00280166,
368 -0.000309513, 9.6094e-006, -0.000178701, -2.26873e-007, 1.61618e-006,
369 -0.00102174, -2.21943e-008, 5.21377e-005, 0.000211917, 1 };
370
371 // Scale and offsets
372 double sx = 0.117798, ox = 44.2834;
373 double sy = 0.114598, oy = 33.2609;
374 double sz = 526.733, oz = 36.9502;
375 double su = 14106, ou = 13785;
376 double sv = 15402, ov = 15216;
377
378 vpgl_rational_camera<double> rcam(neu_u1, den_u1, neu_v1, den_v1, sx, ox, sy, oy, sz, oz, su, ou, sv, ov);
379 std::vector<vgl_point_2d<double>> image_pts;
380 std::vector<vgl_point_3d<double>> ground_pts;
381 size_t n_points = 1000;
382 vnl_random rng;
383 for (unsigned i = 0; i < n_points; i++)
384 {
385 double x = 2.0 * sx * rng.drand64() + (ox - sx);
386 double y = 2.0 * sy * rng.drand64() + (oy - sy);
387 double z = 2.0 * sz * rng.drand64() + (oz - sz);
388 vgl_point_3d<double> p3d(x, y, z);
389 ground_pts.push_back(p3d);
390 double u, v;
391 rcam.project(x, y, z, u, v);
392 image_pts.emplace_back(u, v);
393 }
394 vpgl_rational_camera<double> fcam;
395 bool good = vpgl_rational_camera_compute::compute(image_pts, ground_pts, fcam);
396 if (good)
397 {
398 double rms_er = 0.0;
399 for (size_t i = 0; i < n_points; ++i)
400 {
401 const vgl_point_3d<double> & p3 = ground_pts[i];
402 double u, v;
403 fcam.project(p3.x(), p3.y(), p3.z(), u, v);
404 double sq_err = (u - image_pts[i].x()) * (u - image_pts[i].x()) + (v - image_pts[i].y()) * (v - image_pts[i].y());
405 rms_er += sq_err;
406 }
407 rms_er /= n_points;
408 rms_er = sqrt(rms_er);
409 std::cout << "fitted rational camera rms projection error " << rms_er << std::endl;
410 good = good && (rms_er < 1.0e-5);
411 }
412 TEST("compute rational camera ", good, true);
413 }
414
415 static void
test_camera_compute()416 test_camera_compute()
417 {
418 test_camera_compute_setup();
419 test_perspective_compute();
420 test_perspective_compute_direct_linear_transform();
421 test_perspective_compute_ground();
422 test_calibration_compute_natural();
423 test_compute_affine();
424 test_compute_rational();
425 }
426
427 TESTMAIN(test_camera_compute);
428