1 // Some tests for vgl_affine_coordinates
2 // J.L. Mundy Jan 31, 2016
3
4 #include <iostream>
5 #include <fstream>
6 #include <cstdlib>
7 #include <vector>
8 #include <cmath>
9 #ifdef _MSC_VER
10 # include "vcl_msvc_warnings.h"
11 #endif
12 #include "testlib/testlib_test.h"
13 #include "vgl/vgl_affine_coordinates.h"
14 #include "vgl/vgl_point_2d.h"
15 #include "vgl/vgl_point_3d.h"
16 #include "vgl/vgl_box_2d.h"
17 #include "vgl/vgl_pointset_3d.h"
18 static double
rnd(double r)19 rnd(double r)
20 {
21 double b = (2.0 * r * double(rand()) / RAND_MAX) - r;
22 return b;
23 }
24 static vgl_vector_2d<double>
rand_vector(double r)25 rand_vector(double r)
26 {
27 return { rnd(r), rnd(r) };
28 }
29 static vgl_point_3d<double>
trans_3d(std::vector<std::vector<double>> const & T,std::vector<double> const & t,vgl_point_3d<double> const & P)30 trans_3d(std::vector<std::vector<double>> const & T, std::vector<double> const & t, vgl_point_3d<double> const & P)
31 {
32 double px = T[0][0] * P.x() + T[0][1] * P.y() + T[0][2] * P.z();
33 double py = T[1][0] * P.x() + T[1][1] * P.y() + T[1][2] * P.z();
34 double pz = T[2][0] * P.x() + T[2][1] * P.y() + T[2][2] * P.z();
35 px += t[0];
36 py += t[1];
37 pz += t[2];
38 return { px, py, pz };
39 }
40 // C is a 2x3 affine camera matrix without the translation column, t is the 2x1 translation vector
41 static vgl_point_2d<double>
proj_3d(std::vector<std::vector<double>> const & C,std::vector<double> const & t,vgl_point_3d<double> const & P)42 proj_3d(std::vector<std::vector<double>> const & C, std::vector<double> const & t, vgl_point_3d<double> const & P)
43 {
44 double px = C[0][0] * P.x() + C[0][1] * P.y() + C[0][2] * P.z() + t[0];
45 double py = C[1][0] * P.x() + C[1][1] * P.y() + C[1][2] * P.z() + t[1];
46 return { px, py };
47 }
48 static void
test_all()49 test_all()
50 {
51 // basis
52 vgl_point_2d<double> p0(1.0, 2.0), p1(3.0, 2.0), p2(1.0, 4.0);
53 vgl_point_2d<double> p(4.0, 3.0);
54 double a00 = 0.5, a11 = 0.5, a10 = -0.3, a01 = 0.3, tx = 10.0, ty = 5.0;
55 double q0x = a00 * p0.x() + a01 * p0.y() + tx;
56 double q0y = a10 * p0.x() + a11 * p0.y() + ty;
57 vgl_point_2d<double> q0(q0x, q0y);
58
59 double q1x = a00 * p1.x() + a01 * p1.y() + tx;
60 double q1y = a10 * p1.x() + a11 * p1.y() + ty;
61 vgl_point_2d<double> q1(q1x, q1y);
62
63 double q2x = a00 * p2.x() + a01 * p2.y() + tx;
64 double q2y = a10 * p2.x() + a11 * p2.y() + ty;
65 vgl_point_2d<double> q2(q2x, q2y);
66
67 double qx = a00 * p.x() + a01 * p.y() + tx;
68 double qy = a10 * p.x() + a11 * p.y() + ty;
69 vgl_point_2d<double> q(qx, qy);
70 std::vector<vgl_point_2d<double>> p_pts, affine_p_pts;
71 p_pts.push_back(p0);
72 p_pts.push_back(p1);
73 p_pts.push_back(p2);
74 p_pts.push_back(p);
75
76 std::vector<vgl_point_2d<double>> q_pts, affine_q_pts;
77 q_pts.push_back(q0);
78 q_pts.push_back(q1);
79 q_pts.push_back(q2);
80 q_pts.push_back(q);
81
82 vgl_affine_coordinates_2d(p_pts, affine_p_pts);
83 vgl_affine_coordinates_2d(q_pts, affine_q_pts);
84 bool good = true;
85 for (unsigned i = 0; i < affine_p_pts.size(); ++i)
86 {
87 std::cout << affine_p_pts[i] << ' ' << affine_q_pts[i] << std::endl;
88 double d = (affine_p_pts[i] - affine_q_pts[i]).length();
89 good = good && d < 1.0e-9;
90 }
91 TEST("invariance of affine 2d coords", good, true);
92 std::vector<vgl_point_3d<double>> Ppts, Qpts;
93 vgl_point_3d<double> P0(1.0, 2.0, 3.0), P1(3.0, 2.5, 4.0), P2(1.5, 4.0, -1.0), P3(0.0, 10.0, 7.0);
94 vgl_point_3d<double> P(4.0, 3.0, 10.0), Pm(-4.0, 3.0, 10.0);
95 Ppts.push_back(P0);
96 Ppts.push_back(P1);
97 Ppts.push_back(P2);
98 Ppts.push_back(P3);
99 Ppts.push_back(P);
100 Ppts.push_back(Pm);
101 std::vector<std::vector<double>> T(3);
102 T[0] = std::vector<double>(3);
103 T[1] = std::vector<double>(3);
104 T[2] = std::vector<double>(3);
105 T[0][0] = 0.5;
106 T[1][1] = 1.5;
107 T[2][2] = 1.0;
108 T[0][1] = -0.7;
109 T[0][2] = -0.5;
110 T[1][0] = 0.7;
111 T[1][2] = -0.3;
112 T[2][0] = 0.5;
113 T[2][1] = 0.3;
114 std::vector<double> t(3);
115 t[0] = 10.0;
116 t[1] = 20.0;
117 t[2] = 100.0;
118 vgl_point_3d<double> Q0 = trans_3d(T, t, P0), Q1 = trans_3d(T, t, P1), Q2 = trans_3d(T, t, P2);
119 vgl_point_3d<double> Q3 = trans_3d(T, t, P3), Q = trans_3d(T, t, P), Qm = trans_3d(T, t, Pm);
120 Qpts.push_back(Q0);
121 Qpts.push_back(Q1);
122 Qpts.push_back(Q2);
123 Qpts.push_back(Q3);
124 Qpts.push_back(Q);
125 Qpts.push_back(Qm);
126 std::vector<vgl_point_3d<double>> affine_P_coords, affine_Q_coords;
127 vgl_affine_coordinates_3d(Ppts, affine_P_coords);
128 vgl_affine_coordinates_3d(Qpts, affine_Q_coords);
129 good = true;
130 for (unsigned i = 0; i < Ppts.size(); ++i)
131 {
132 std::cout << affine_P_coords[i] << ' ' << affine_Q_coords[i] << std::endl;
133 double d = (affine_P_coords[i] - affine_Q_coords[i]).length();
134 good = good && d < 1.0e-9;
135 }
136 TEST("invariance of affine 3d coords", good, true);
137 // project Pi into view1
138 std::vector<vgl_point_2d<double>> ppts1, ppts2;
139 std::vector<std::vector<double>> C1(2);
140 C1[0] = std::vector<double>(3);
141 C1[1] = std::vector<double>(3);
142 std::vector<double> tc(2, 0.0);
143 C1[0][0] = 0.5;
144 C1[0][1] = 0.0;
145 C1[0][2] = 2.0;
146 C1[1][0] = 0.0;
147 C1[1][1] = 2.0;
148 C1[1][2] = 3.0;
149 std::vector<std::vector<double>> C2 = C1;
150 C2[0][0] = 1.5;
151 C2[0][1] = 0.707;
152 C2[0][2] = 4.0;
153 C2[1][0] = -0.707;
154 C2[1][1] = 0.5;
155 C2[1][2] = 5.0;
156 for (const auto & Ppt : Ppts)
157 {
158 vgl_point_2d<double> p1 = proj_3d(C1, tc, Ppt);
159 p1.set(p1.x(), p1.y());
160 ppts1.push_back(p1);
161 vgl_point_2d<double> p2 = proj_3d(C2, tc, Ppt);
162 ppts2.push_back(p2);
163 }
164 std::vector<vgl_point_3d<double>> recon_affine_pts;
165 vgl_affine_coordinates_3d(ppts1, ppts2, recon_affine_pts);
166 good = true;
167 for (unsigned i = 0; i < ppts1.size(); ++i)
168 {
169 std::cout << affine_P_coords[i] << ' ' << recon_affine_pts[i] << std::endl;
170 double d = (affine_P_coords[i] - recon_affine_pts[i]).length();
171 good = good && d < 1.0e-9;
172 }
173 TEST("affine 3-d coords from 2-d views", good, true);
174 // image test JF - below are image coordinates on three face images of the same person
175 std::vector<vgl_point_2d<double>> JF0, JF1, JF2;
176 vgl_point_2d<double> p00(402.056, 429.889), p01(421.611, 373), p02(383.389, 362.778), p03(427.389, 304.556),
177 p04(490.056, 301.889);
178 JF0.push_back(p00);
179 JF0.push_back(p01);
180 JF0.push_back(p02);
181 JF0.push_back(p03);
182 JF0.push_back(p04);
183 vgl_point_2d<double> p10(235.444, 343.889), p11(253.222, 309.222), p12(236.333, 291), p13(259.889, 253.222),
184 p14(303, 251.889);
185 JF1.push_back(p10);
186 JF1.push_back(p11);
187 JF1.push_back(p12);
188 JF1.push_back(p13);
189 JF1.push_back(p14);
190 vgl_point_2d<double> p20(293.667, 301), p21(307, 270.778), p22(282.556, 257.444), p23(319.444, 219.667),
191 p24(351.444, 220.556);
192 JF2.push_back(p20);
193 JF2.push_back(p21);
194 JF2.push_back(p22);
195 JF2.push_back(p23);
196 JF2.push_back(p24);
197 #if 0
198 for(unsigned i = 0; i<5; ++i){
199 //roughly normalize the coordinates
200 vgl_point_2d<double>& p0 = JF0[i];
201 p0.set((p0.x()-275.0)/200.0, (p0.y()-275.0)/200.0);
202 vgl_point_2d<double>& p1 = JF1[i];
203 p1.set((p1.x()-275.0)/200.0, (p1.y()-275.0)/200.0);
204 vgl_point_2d<double>& p2 = JF2[i];
205 p2.set((p1.x()-275.0)/200.0, (p2.y()-275.0)/200.0);
206 }
207 #endif
208 std::vector<vgl_point_3d<double>> affine_01, affine_02;
209 vgl_affine_coordinates_3d(JF0, JF1, affine_01);
210 vgl_affine_coordinates_3d(JF0, JF2, affine_02);
211 for (unsigned i = 0; i < affine_01.size(); ++i)
212 {
213 std::cout << affine_01[i] << ' ' << affine_02[i] << std::endl;
214 }
215 std::cout << "Christina sensitivity test" << std::endl;
216 // sensitivity test 3-d points from a face scan for a realistic test case
217
218 vgl_point_3d<double> c0(-33.633899688721, 23.875200271606, -31.615800857544); // right lobe
219 vgl_point_3d<double> c1(21.458400726318, 27.088699340820, 73.859703063965); // alar tip
220 vgl_point_3d<double> c2(10.649299621582, 67.683601379395, 50.533798217773); // right medial canthus
221 vgl_point_3d<double> c3(21.124300003052, 37.831298828125, 84.872703552246); // nose apex
222 vgl_point_3d<double> c4(-18.988700866699, 71.193702697754, 39.596099853516); // right lateral canthus
223 vgl_point_3d<double> c5(67.848396301270, 69.642601013184, 49.217899322510); // left lateral canthus
224 std::vector<vgl_point_3d<double>> OrigCpts, Cpts;
225 OrigCpts.push_back(c0);
226 OrigCpts.push_back(c1);
227 OrigCpts.push_back(c2);
228 OrigCpts.push_back(c3);
229 OrigCpts.push_back(c4);
230 OrigCpts.push_back(c5);
231 Cpts.push_back(c3);
232 Cpts.push_back(c1);
233 Cpts.push_back(c2);
234 Cpts.push_back(c0);
235 Cpts.push_back(c4);
236 Cpts.push_back(c5);
237 std::vector<vgl_point_3d<double>> affine_C_coords;
238 vgl_affine_coordinates_3d(Cpts, affine_C_coords);
239 for (unsigned i = 0; i < Cpts.size(); ++i)
240 {
241 std::cout << affine_C_coords[i] << std::endl;
242 }
243 std::vector<vgl_point_2d<double>> cpts1, cpts2;
244 std::vector<std::vector<double>> CC1(2);
245 CC1[0] = std::vector<double>(3);
246 CC1[1] = std::vector<double>(3);
247 std::vector<double> tcc(2, 0.0);
248 // set up camera looking along +X
249 CC1[0][0] = 0.0;
250 CC1[0][1] = 0.0;
251 CC1[0][2] = 1.0;
252 CC1[1][0] = 0.0;
253 CC1[1][1] = -1.0;
254 CC1[1][2] = 0.0;
255
256 std::vector<std::vector<double>> CC2 = CC1;
257
258 // rotate 15deg about Y
259 CC1[0][0] = 0.9659258; // sqrt(2.0)/2.0;
260 CC2[0][2] = 0.2588190; // CC1[0][0];
261 for (const auto & Cpt : Cpts)
262 {
263 vgl_point_2d<double> p1 = proj_3d(CC1, tcc, Cpt);
264 cpts1.push_back(p1);
265 vgl_point_2d<double> p2 = proj_3d(CC2, tcc, Cpt);
266 cpts2.push_back(p2);
267 }
268
269 std::vector<vgl_point_3d<double>> c_affine_pts;
270 vgl_affine_coordinates_3d(cpts1, cpts2, c_affine_pts);
271 good = true;
272 for (unsigned i = 0; i < cpts1.size(); ++i)
273 {
274 std::cout << affine_C_coords[i] << ' ' << c_affine_pts[i] << std::endl;
275 double d = (affine_C_coords[i] - c_affine_pts[i]).length();
276 good = good && d < 1.0e-9;
277 }
278 TEST("Affine coordinates for face pts", good, true);
279 // Determine coordinate sensitivity
280 vgl_box_2d<double> box1, box2;
281 for (unsigned i = 0; i < cpts1.size(); ++i)
282 {
283 vgl_point_2d<double> p1 = cpts1[i];
284 vgl_point_2d<double> p2 = cpts2[i];
285 box1.add(p1);
286 box2.add(p2);
287 }
288 vgl_point_2d<double> min_p1 = box1.min_point();
289 vgl_point_2d<double> max_p1 = box1.max_point();
290 double diag1 = (max_p1 - min_p1).length();
291 vgl_point_2d<double> min_p2 = box2.min_point();
292 vgl_point_2d<double> max_p2 = box2.max_point();
293 double diag2 = (max_p2 - min_p2).length();
294 double diam = diag1;
295 if (diam < diag2)
296 diam = diag2;
297 unsigned nr = 1000;
298 double norm_radius = 0.02;
299 double r = norm_radius * diam;
300 std::cout << "diameter " << diam << " Norm radius " << norm_radius << " Abs radius " << r << std::endl;
301 vgl_pointset_3d<double> pset4, pset5;
302 vgl_point_3d<double> z(0.0, 0.0, 0.0);
303 double L4 = (affine_C_coords[4] - z).length();
304 double L5 = (affine_C_coords[5] - z).length();
305 double max_diff = 0.0;
306 for (unsigned k = 0; k < nr; ++k)
307 {
308 std::vector<vgl_point_2d<double>> rpts1, rpts2;
309 for (unsigned i = 0; i < cpts1.size(); ++i)
310 {
311 vgl_point_2d<double> p1 = cpts1[i];
312 vgl_point_2d<double> p2 = cpts2[i];
313 p1 = p1 + rand_vector(r);
314 p2 = p2 + rand_vector(r);
315 rpts1.push_back(p1);
316 rpts2.push_back(p2);
317 }
318 std::vector<vgl_point_3d<double>> aff_pts_3d;
319 vgl_affine_coordinates_3d(rpts1, rpts2, aff_pts_3d);
320 pset4.add_point(aff_pts_3d[4]);
321 pset5.add_point(aff_pts_3d[5]);
322 double d1 = (aff_pts_3d[4] - affine_C_coords[4]).length();
323 double d2 = (aff_pts_3d[5] - affine_C_coords[5]).length();
324 d1 /= L4;
325 d2 /= L5;
326 if (d1 > max_diff)
327 max_diff = d1;
328 if (d2 > max_diff)
329 max_diff = d2;
330 }
331 std::cout << "max error " << max_diff << std::endl;
332 // write pset to file
333 std::string pfile4 = "D:/VisionSystems/Janus/Invariants/pt4_spread.txt";
334 std::string pfile5 = "D:/VisionSystems/Janus/Invariants/pt5_spread.txt";
335 std::ofstream ostr4(pfile4.c_str());
336 ostr4 << pset4;
337 ostr4.close();
338 std::ofstream ostr5(pfile5.c_str());
339 ostr5 << pset5;
340 ostr5.close();
341 // linden face test
342 std::cout << "linden test" << std::endl;
343 vgl_point_3d<double> l0(-100.140998840332, -63.093498229980, -97.007896423340); // right lobe
344 vgl_point_3d<double> l1(-35.828701019287, -45.380100250244, -8.269379615784); // alar tip
345 vgl_point_3d<double> l2(-51.182998657227, -2.846539974213, -30.991199493408); // right medial canthus
346 vgl_point_3d<double> l3(-32.956798553467, -32.738601684570, 5.117280006409); // nose apex
347 vgl_point_3d<double> l4(-82.580596923828, 0.867244005203, -42.126300811768); // right lateral canthus
348 vgl_point_3d<double> l5(15.527600288391, -4.037390232086, -39.543399810791); // left lateral canthus
349 std::vector<vgl_point_3d<double>> Lpts;
350 Lpts.push_back(l0);
351 Lpts.push_back(l1);
352 Lpts.push_back(l2);
353 Lpts.push_back(l3);
354 Lpts.push_back(l4);
355 Lpts.push_back(l5);
356 std::vector<vgl_point_3d<double>> affine_L_coords;
357 vgl_affine_coordinates_3d(Lpts, affine_L_coords);
358 for (unsigned i = 0; i < Lpts.size(); ++i)
359 {
360 std::cout << affine_L_coords[i] << std::endl;
361 }
362 }
363 void
test_affine_coordinates()364 test_affine_coordinates()
365 {
366 std::cout << "*****************************\n"
367 << " Testing vgl_affine_coordinates\n"
368 << "*****************************\n\n";
369
370 test_all();
371 }
372
373
374 TESTMAIN(test_affine_coordinates);
375