1 #include <iostream>
2 #include "testlib/testlib_test.h"
3 #ifdef _MSC_VER
4 #  include "vcl_msvc_warnings.h"
5 #endif
6 #include "vgl/vgl_point_2d.h"
7 #include "vgl/vgl_point_3d.h"
8 #include "vpgl/vpgl_rational_camera.h"
9 #include <vpgl/algo/vpgl_rational_adjust.h>
10 
11 static void
test_rational_adjust()12 test_rational_adjust()
13 {
14   // Make the rational cameras
15   // Rational polynomial coefficients
16   // from digital globe image  02JUL05075233-P1BS-005630614010_01_P001
17   double neu_u[20] = { -1.45002e-005,
18                        8.88441e-005,
19                        -1.69845e-006,
20                        0.000125672,
21                        -1.3334e-005,
22                        1.66753e-006,
23                        -0.00191472,
24                        -5.49752e-006,
25                        0.000626966,
26                        1.00493,
27                        0.000149249,
28                        5.56542e-007,
29                        -0.00243291,
30                        -4.19472e-007,
31                        -0.000322203,
32                        0.00751094,
33                        0,
34                        1.0456e-005,
35                        0.00335227,
36                        -0.00171778 };
37   double den_u[20] = { -1.11389e-007, 1.12986e-007,  2.60026e-008,  1.37888e-006,
38                        1.59082e-006,  -2.23928e-007, -4.61017e-005, 0,
39                        2.26253e-006,  0.00161218,    -1.99999e-006, 3.05839e-007,
40                        -4.65254e-006, -3.57289e-008, -1.38706e-006, 0.00195803,
41                        1.95078e-008,  -6.02895e-006, -0.000621955,  1 };
42   double neu_v[20] = { -1.34506e-007, 3.30571e-006,  1.12006e-007,  -0.000720573, 2.18505e-005,
43                        -1.04205e-006, -6.89949e-005, 6.89221e-008,  7.62809e-006, -0.0233525,
44                        1.04041e-005,  -1.01309e-005, 0.00294398,    2.71637e-006, 7.06142e-005,
45                        -1.00083,      -6.37659e-008, -4.99594e-006, 0.0227909,    -0.00222785 };
46   double den_v[20] = { 5.17035e-008,
47                        1.79981e-007,
48                        -1.70495e-008,
49                        -2.0714e-006,
50                        3.631e-006,
51                        -9.81742e-008,
52                        1.38937e-005,
53                        0,
54                        -5.91773e-007,
55                        -0.000137415,
56                        0.000102628,
57                        -3.96336e-006,
58                        -2.34085e-006,
59                        6.79756e-008,
60                        -3.48424e-006,
61                        0.000686196,
62                        0,
63                        -2.33034e-006,
64                        0.000124036,
65                        1 };
66   // Scale and offsets
67 
68   double sx1 = 0.1038, ox1 = 44.3526;
69   double sy1 = 0.1131, oy1 = 33.3405;
70   double sz1 = 501, oz1 = 35.0;
71   double su = 13827, ou = 13793;
72   double sv = 15657, ov = 15296;
73 
74   vpgl_rational_camera<double> rcam(neu_u, den_u, neu_v, den_v, sx1, ox1, sy1, oy1, sz1, oz1, su, ou, sv, ov);
75   //
76   // test 3-d registration
77   // world points
78   vgl_point_3d<double> c0(44.363757, 33.289254, 31.000366);
79   vgl_point_3d<double> c1(44.369636, 33.293019, 35.749939);
80   vgl_point_3d<double> c2(44.373331, 33.292956, 32.587067);
81   vgl_point_3d<double> c3(44.395048, 33.292964, 41.883163);
82   vgl_point_3d<double> c4(44.395221, 33.289763, 38.543938);
83   vgl_point_3d<double> c5(44.384865, 33.290064, 39.703369);
84   vgl_point_3d<double> c6(44.351024, 33.290036, 36.203430);
85   vgl_point_3d<double> c7(44.341150, 33.293502, 38.745071);
86   vgl_point_3d<double> c8(44.352379, 33.294193, 44.503021);
87   std::vector<vgl_point_3d<double>> geo_points;
88   geo_points.push_back(c0);
89   geo_points.push_back(c1);
90   geo_points.push_back(c2);
91   geo_points.push_back(c3);
92   geo_points.push_back(c4);
93   geo_points.push_back(c5);
94   geo_points.push_back(c6);
95   geo_points.push_back(c7);
96   geo_points.push_back(c8);
97   // corresponding image points for rcam
98   vgl_point_2d<double> p0(14960.3, 22724.9);
99   vgl_point_2d<double> p1(15754.8, 22182.7);
100   vgl_point_2d<double> p2(16247.6, 22177.6);
101   vgl_point_2d<double> p3(19160.2, 22103.7);
102   vgl_point_2d<double> p4(19179.8, 22544.4);
103   vgl_point_2d<double> p5(17791.3, 22541.2);
104   vgl_point_2d<double> p6(13253.2, 22665.5);
105   vgl_point_2d<double> p7(11932.3, 22221.1);
106   vgl_point_2d<double> p8(13441.2, 22089.2);
107 
108   std::vector<vgl_point_2d<double>> img_points;
109   img_points.push_back(p0);
110   img_points.push_back(p1);
111   img_points.push_back(p2);
112   img_points.push_back(p3);
113   img_points.push_back(p4);
114   img_points.push_back(p5);
115   img_points.push_back(p6);
116   img_points.push_back(p7);
117   img_points.push_back(p8);
118 
119   // New 3-d alignment
120   vpgl_rational_camera<double> adj_rcam;
121   vpgl_rational_adjust::adjust(rcam, img_points, geo_points, adj_rcam);
122   std::cout << "\nInitial Cam\n" << rcam << '\n' << "\nAdjusted Cam\n" << adj_rcam << '\n';
123   double zoff = adj_rcam.offset(vpgl_rational_camera<double>::Z_INDX);
124   TEST_NEAR("test adjust_geo", zoff, 35.1515, 0.001);
125 }
126 
127 TESTMAIN(test_rational_adjust);
128