1 // This is bbas/bpgl/bpgl_comp_rational_camera.hxx
2 #ifndef bpgl_comp_rational_camera_hxx_
3 #define bpgl_comp_rational_camera_hxx_
4 //:
5 // \file
6 #include <iostream>
7 #include <cmath>
8 #include <vector>
9 #include <fstream>
10 #include "bpgl_comp_rational_camera.h"
11 #ifdef _MSC_VER
12 # include <vcl_msvc_warnings.h>
13 #endif
14 #include <vgl/vgl_point_2d.h>
15 #include <vgl/vgl_point_3d.h>
16 //--------------------------------------
17 // Constructors
18 //
19
20 // Create an identity projection, i.e. (x,y,z) identically maps to (x,y)
21 template <class T>
bpgl_comp_rational_camera()22 bpgl_comp_rational_camera<T>::bpgl_comp_rational_camera():
23 vpgl_rational_camera<T>()
24 {
25 matrix_.set_identity();
26 }
27
28 //: Constructor from a rational camera and an affine matrix
29 template <class T>
30 bpgl_comp_rational_camera<T>::
bpgl_comp_rational_camera(vnl_matrix_fixed<T,3,3> const & M,vpgl_rational_camera<T> const & rcam)31 bpgl_comp_rational_camera(vnl_matrix_fixed<T, 3,3> const& M,
32 vpgl_rational_camera<T> const& rcam) :
33 vpgl_rational_camera<T>(rcam), matrix_(M)
34 {
35 }
36
37 //: Constructor from translation only
38 template <class T>
39 bpgl_comp_rational_camera<T>::
bpgl_comp_rational_camera(const T tu,const T tv,vpgl_rational_camera<T> const & rcam)40 bpgl_comp_rational_camera(const T tu, const T tv,
41 vpgl_rational_camera<T> const& rcam) :
42 vpgl_rational_camera<T>(rcam)
43 {
44 matrix_.set_identity();
45 set_translation(tu,tv);
46 }
47
48 //: Constructor from translation rotation only first rotate then translate
49 template <class T>
50 bpgl_comp_rational_camera<T>::
bpgl_comp_rational_camera(const T tu,const T tv,const T angle_in_radians,vpgl_rational_camera<T> const & rcam)51 bpgl_comp_rational_camera(const T tu, const T tv, const T angle_in_radians,
52 vpgl_rational_camera<T> const& rcam):
53 vpgl_rational_camera<T>(rcam)
54 {
55 this->set_trans_rotation(tu, tv, angle_in_radians);
56 set_translation(T(0),T(0));
57 }
58
59 //: Constructor with all transform parameters
60 template <class T>
61 bpgl_comp_rational_camera<T>::
bpgl_comp_rational_camera(const T tu,const T tv,const T angle_in_radians,const T su,const T sv,vpgl_rational_camera<T> const & rcam)62 bpgl_comp_rational_camera(const T tu, const T tv, const T angle_in_radians,
63 const T su, const T sv,
64 vpgl_rational_camera<T> const& rcam):
65 vpgl_rational_camera<T>(rcam)
66 {
67 this->set_all(tu, tv, angle_in_radians, su, sv);
68 set_translation(T(0),T(0));
69 }
70
71 //: read from a file
72 template <class T>
bpgl_comp_rational_camera(std::string cam_path)73 bpgl_comp_rational_camera<T>::bpgl_comp_rational_camera(std::string cam_path)
74 {
75 vpgl_rational_camera<T> *rcam = read_rational_camera<T >(cam_path);
76
77 std::ifstream file_inp;
78 file_inp.open(cam_path.c_str());
79 if (!file_inp.good()) {
80 std::cout << "error: bad filename: " << cam_path << std::endl;
81 return;
82 }
83 vnl_matrix_fixed<T, 3,3> M;
84 std::string input;
85 bool good = false;
86 while (!file_inp.eof()&&!good) {
87 file_inp >> input;
88 if (input=="affine_matrix")
89 {
90 file_inp >> M;
91 good = true;
92 }
93 }
94 if (!good)
95 {
96 //std::cout << "error: not a composite rational camera file\n";
97 return;
98 }
99 *this = bpgl_comp_rational_camera<T>(M, *rcam);
100 }
101
102 template <class T>
clone(void) const103 bpgl_comp_rational_camera<T>* bpgl_comp_rational_camera<T>::clone(void) const
104 {
105 return new bpgl_comp_rational_camera<T>(*this);
106 }
107
108 // Base projection method
109 template <class T>
project(const T x,const T y,const T z,T & u,T & v) const110 void bpgl_comp_rational_camera<T>::project(const T x, const T y, const T z,
111 T& u, T& v) const
112 {
113 //first project with the rational camera
114 T ur, vr;
115 vpgl_rational_camera<T>::project(x, y, z, ur, vr);
116 //transform by affine map
117 vnl_vector_fixed<T, 3> p, pt;
118 p[0]=ur; p[1]=vr; p[2]=(T)1;
119 pt = matrix_*p;
120 u = pt[0];
121 v = pt[1];
122 }
123
124 //vnl interface methods
125 template <class T>
126 vnl_vector_fixed<T, 2>
project(vnl_vector_fixed<T,3> const & world_point) const127 bpgl_comp_rational_camera<T>::project(vnl_vector_fixed<T, 3> const& world_point)const
128 {
129 vnl_vector_fixed<T, 2> image_point;
130 this->project(world_point[0], world_point[1], world_point[2],
131 image_point[0], image_point[1]);
132 return image_point;
133 }
134
135 //vgl interface methods
136 template <class T>
project(vgl_point_3d<T> world_point) const137 vgl_point_2d<T> bpgl_comp_rational_camera<T>::project(vgl_point_3d<T> world_point)const
138 {
139 T u = 0, v = 0;
140 this->project(world_point.x(), world_point.y(), world_point.z(), u, v);
141 return vgl_point_2d<T>(u, v);
142 }
143
144 //: print the camera parameters
145 template <class T>
print(std::ostream & s) const146 void bpgl_comp_rational_camera<T>::print(std::ostream& s) const
147 {
148 vpgl_scale_offset<T> sox = this->scale_offsets()[this->X_INDX];
149 vpgl_scale_offset<T> soy = this->scale_offsets()[this->Y_INDX];
150 vpgl_scale_offset<T> soz = this->scale_offsets()[this->Z_INDX];
151 vpgl_scale_offset<T> sou = this->scale_offsets()[this->U_INDX];
152 vpgl_scale_offset<T> sov = this->scale_offsets()[this->V_INDX];
153
154 s << "bpgl_comp_rational_camera:\n"
155 << "------------------------\n"
156 << "xoff = " << sox.offset()
157 << " yoff = " << soy.offset()
158 << " zoff = " << soz.offset() << '\n'
159 << "xscale = " << sox.scale()
160 << " yscale = " << soy.scale()
161 << " zscale = " << soz.scale() << '\n'
162
163 << "uoff = " << sou.offset()
164 << " voff = " << sov.offset() << '\n'
165 << "uscale = " << sou.scale()
166 << " vscale = " << sov.scale() << "\n\n"
167
168 << "U Numerator\n"
169 << "[0] " << this->coefficient_matrix()[0][0]
170 << " [1] " << this->coefficient_matrix()[0][1]
171 << " [2] " << this->coefficient_matrix()[0][2]
172 << " [3] " << this->coefficient_matrix()[0][3] <<'\n'
173 << "[4] " << this->coefficient_matrix()[0][4]
174 << " [5] " << this->coefficient_matrix()[0][5]
175 << " [6] " << this->coefficient_matrix()[0][6]
176 << " [7] " << this->coefficient_matrix()[0][7] <<'\n'
177 << "[8] " << this->coefficient_matrix()[0][8]
178 << " [9] " << this->coefficient_matrix()[0][9]
179 << " [10] " << this->coefficient_matrix()[0][10]
180 << " [11] " << this->coefficient_matrix()[0][11] <<'\n'
181 << "[12] " << this->coefficient_matrix()[0][12]
182 << " [13] " << this->coefficient_matrix()[0][13]
183 << " [14] " << this->coefficient_matrix()[0][14]
184 << " [15] " << this->coefficient_matrix()[0][15] <<'\n'
185 << "[16] " << this->coefficient_matrix()[0][16]
186 << " [17] " << this->coefficient_matrix()[0][17]
187 << " [18] " << this->coefficient_matrix()[0][18]
188 << " [19] " << this->coefficient_matrix()[0][19] <<"\n\n"
189
190 << "U Denominator\n"
191 << "[0] " << this->coefficient_matrix()[1][0]
192 << " [1] " << this->coefficient_matrix()[1][1]
193 << " [2] " << this->coefficient_matrix()[1][2]
194 << " [3] " << this->coefficient_matrix()[1][3] <<'\n'
195 << "[4] " << this->coefficient_matrix()[1][4]
196 << " [5] " << this->coefficient_matrix()[1][5]
197 << " [6] " << this->coefficient_matrix()[1][6]
198 << " [7] " << this->coefficient_matrix()[1][7] <<'\n'
199 << "[8] " << this->coefficient_matrix()[1][8]
200 << " [9] " << this->coefficient_matrix()[1][9]
201 << " [10] " << this->coefficient_matrix()[1][10]
202 << " [11] " << this->coefficient_matrix()[1][11] <<'\n'
203 << "[12] " << this->coefficient_matrix()[1][12]
204 << " [13] " << this->coefficient_matrix()[1][13]
205 << " [14] " << this->coefficient_matrix()[1][14]
206 << " [15] " << this->coefficient_matrix()[1][15] <<'\n'
207 << "[16] " << this->coefficient_matrix()[1][16]
208 << " [17] " << this->coefficient_matrix()[1][17]
209 << " [18] " << this->coefficient_matrix()[1][18]
210 << " [19] " << this->coefficient_matrix()[1][19] <<"\n\n"
211
212 << "V Numerator\n"
213 << "[0] " << this->coefficient_matrix()[2][0]
214 << " [1] " << this->coefficient_matrix()[2][1]
215 << " [2] " << this->coefficient_matrix()[2][2]
216 << " [3] " << this->coefficient_matrix()[2][3]<<'\n'
217 << "[4] " << this->coefficient_matrix()[2][4]
218 << " [5] " << this->coefficient_matrix()[2][5]
219 << " [6] " << this->coefficient_matrix()[2][6]
220 << " [7] " << this->coefficient_matrix()[2][7] <<'\n'
221 << "[8] " << this->coefficient_matrix()[2][8]
222 << " [9] " << this->coefficient_matrix()[2][9]
223 << " [10] " << this->coefficient_matrix()[2][10]
224 << " [11] " << this->coefficient_matrix()[2][11] <<'\n'
225 << "[12] " << this->coefficient_matrix()[2][12]
226 << " [13] " << this->coefficient_matrix()[2][13]
227 << " [14] " << this->coefficient_matrix()[2][14]
228 << " [15] " << this->coefficient_matrix()[2][15]<<'\n'
229 << "[16] " << this->coefficient_matrix()[2][16]
230 << " [17] " << this->coefficient_matrix()[2][17]
231 << " [18] " << this->coefficient_matrix()[2][18]
232 << " [19] " << this->coefficient_matrix()[2][19] <<"\n\n"
233
234 << "V Denominator\n"
235 << "[0] " << this->coefficient_matrix()[3][0]
236 << " [1] " << this->coefficient_matrix()[3][1]
237 << " [2] " << this->coefficient_matrix()[3][2]
238 << " [3] " << this->coefficient_matrix()[3][3]<<'\n'
239 << "[4] " << this->coefficient_matrix()[3][4]
240 << " [5] " << this->coefficient_matrix()[3][5]
241 << " [6] " << this->coefficient_matrix()[3][6]
242 << " [7] " << this->coefficient_matrix()[3][7] <<'\n'
243 << "[8] " << this->coefficient_matrix()[3][8]
244 << " [9] " << this->coefficient_matrix()[3][9]
245 << " [10] " << this->coefficient_matrix()[3][10]
246 << " [11] " << this->coefficient_matrix()[3][11] <<'\n'
247 << "[12] " << this->coefficient_matrix()[3][12]
248 << " [13] " << this->coefficient_matrix()[3][13]
249 << " [14] " << this->coefficient_matrix()[3][14]
250 << " [15] " << this->coefficient_matrix()[3][15]<<'\n'
251 << "[16] " << this->coefficient_matrix()[3][16]
252 << " [17] " << this->coefficient_matrix()[3][17]
253 << " [18] " << this->coefficient_matrix()[3][18]
254 << " [19] " << this->coefficient_matrix()[3][19] <<'\n'
255 << "\nAffine Matrix\n"
256 << matrix_ << '\n'
257 <<"------------------------------------------------\n\n";
258 }
259
260 template <class T>
save(std::string cam_path)261 bool bpgl_comp_rational_camera<T>::save(std::string cam_path)
262 {
263 std::ofstream file_out;
264 file_out.open(cam_path.c_str());
265 if (!file_out.good()) {
266 std::cerr << "error: bad filename: " << cam_path << std::endl;
267 return false;
268 }
269 file_out.precision(12);
270
271 int map[20];
272 map[0]=19;
273 map[1]=9;
274 map[2]=15;
275 map[3]=18;
276 map[4]=6;
277 map[5]=8;
278 map[6]=14;
279 map[7]=3;
280 map[8]=12;
281 map[9]=17;
282 map[10]=5;
283 map[11]=0;
284 map[12]=4;
285 map[13]=7;
286 map[14]=1;
287 map[15]=10;
288 map[16]=13;
289 map[17]=2;
290 map[18]=11;
291 map[19]=16;
292
293 file_out << "satId = \"????\";\n"
294 << "bandId = \"RGB\";\n"
295 << "SpecId = \"RPC00B\";\n"
296 << "BEGIN_GROUP = IMAGE\n"
297 << "\n\n" // skip errBias and errRand fields
298 << " lineOffset = " << this->offset(this->V_INDX) << '\n'
299 << " sampOffset = " << this->offset(this->U_INDX) << '\n'
300 << " latOffset = " << this->offset(this->Y_INDX) << '\n'
301 << " longOffset = " << this->offset(this->X_INDX) << '\n'
302 << " heightOffset = " << this->offset(this->Z_INDX) << '\n'
303 << " lineScale = " << this->scale(this->V_INDX) << '\n'
304 << " sampScale = " << this->scale(this->U_INDX) << '\n'
305 << " latScale = " << this->scale(this->Y_INDX) << '\n'
306 << " longScale = " << this->scale(this->X_INDX) << '\n'
307 << " heightScale = " << this->scale(this->Z_INDX) << '\n';
308 vnl_matrix_fixed<double,4,20> coeffs = this->coefficient_matrix();
309 file_out << " lineNumCoef = (";
310 for (int i=0; i<20; i++) {
311 file_out << "\n " << coeffs[this->NEU_V][map[i]];
312 if (i < 19)
313 file_out << ',';
314 }
315 file_out << ");\n lineDenCoef = (";
316 for (int i=0; i<20; i++) {
317 file_out << "\n " << coeffs[this->DEN_V][map[i]];
318 if (i < 19)
319 file_out << ',';
320 }
321 file_out << ");\n sampNumCoef = (";
322 for (int i=0; i<20; i++) {
323 file_out << "\n " << coeffs[this->NEU_U][map[i]];
324 if (i < 19)
325 file_out << ',';
326 }
327 file_out << ");\n sampDenCoef = (";
328 for (int i=0; i<20; i++) {
329 file_out << "\n " << coeffs[this->DEN_U][map[i]];
330 if (i < 19)
331 file_out << ',';
332 }
333 file_out << ");\n"
334 << "END_GROUP = IMAGE\n"
335 << "END;\n"
336
337 << "affine_matrix\n"
338 << matrix_;
339 return true;
340 }
341
342
343 template <class T>
344 void
set_transform(vnl_matrix_fixed<T,3,3> const & M)345 bpgl_comp_rational_camera<T>::set_transform(vnl_matrix_fixed<T, 3,3> const& M)
346 {
347 matrix_ = M;
348 }
349
350 template <class T>
351 void
set_translation(const T tu,const T tv)352 bpgl_comp_rational_camera<T>::set_translation(const T tu, const T tv)
353 {
354 matrix_[0][2]=tu;
355 matrix_[1][2]=tv;
356 }
357
358 template <class T>
359 void
set_trans_rotation(const T tu,const T tv,const T angle_in_radians)360 bpgl_comp_rational_camera<T>::set_trans_rotation(const T tu, const T tv,
361 const T angle_in_radians)
362 {
363 double theta_d = static_cast<double>(angle_in_radians);
364 double c = std::cos(theta_d), s = std::sin(theta_d);
365 T ct = static_cast<T>(c), st = static_cast<T>(s);
366 matrix_[0][0] = ct; matrix_[0][1] = -st; matrix_[0][2] = tu;
367 matrix_[1][0] = st; matrix_[1][1] = ct; matrix_[1][2] = tv;
368 }
369
370 template <class T>
371 void
set_all(const T tu,const T tv,const T angle_in_radians,const T su,const T sv)372 bpgl_comp_rational_camera<T>::set_all(const T tu, const T tv,
373 const T angle_in_radians,
374 const T su, const T sv)
375 {
376 double theta_d = static_cast<double>(angle_in_radians);
377 double c = std::cos(theta_d), s = std::sin(theta_d);
378 T ct = static_cast<T>(c), st = static_cast<T>(s);
379 matrix_[0][0] = ct*su; matrix_[0][1] = -st*sv; matrix_[0][2] = tu;
380 matrix_[1][0] = st*su; matrix_[1][1] = ct*sv; matrix_[1][2] = tv;
381 }
382
383 template <class T>
transform()384 vnl_matrix_fixed<T, 3,3> bpgl_comp_rational_camera<T>::transform()
385 {
386 return matrix_;
387 }
388
389 template <class T>
translation(T & tu,T & tv)390 void bpgl_comp_rational_camera<T>::translation(T& tu, T& tv)
391 {
392 tu = matrix_[0][2];
393 tv = matrix_[1][2];
394 }
395
396 template <class T>
rotation_in_radians()397 T bpgl_comp_rational_camera<T>::rotation_in_radians()
398 {
399 double y = static_cast<double>(matrix_[1][0]);
400 double x = static_cast<double>(matrix_[0][0]);
401 double angle = std::atan2(y, x);
402 return static_cast<T>(angle);
403 }
404
405 //: note that scale factors are not negative
406 template <class T>
image_scale(T & su,T & sv)407 void bpgl_comp_rational_camera<T>::image_scale(T& su, T& sv)
408 {
409 double su_sq = static_cast<double>(matrix_[0][0]*matrix_[0][0] +
410 matrix_[1][0]*matrix_[1][0]);
411 double sv_sq = static_cast<double>(matrix_[0][1]*matrix_[0][1] +
412 matrix_[1][1]*matrix_[1][1]);
413 double sud = std::sqrt(su_sq);
414 double svd = std::sqrt(sv_sq);
415 su = static_cast<T>(sud);
416 sv = static_cast<T>(svd);
417 }
418
419 //: Write to stream
420 template <class T>
operator <<(std::ostream & s,const bpgl_comp_rational_camera<T> & c)421 std::ostream& operator<<(std::ostream& s, const bpgl_comp_rational_camera<T >& c )
422 {
423 c.print(s);
424 return s;
425 }
426
427
428 // Code for easy instantiation.
429 #undef BPGL_COMP_RATIONAL_CAMERA_INSTANTIATE
430 #define BPGL_COMP_RATIONAL_CAMERA_INSTANTIATE(T) \
431 template class bpgl_comp_rational_camera<T >; \
432 template std::ostream& operator<<(std::ostream&, const bpgl_comp_rational_camera<T >&)
433
434
435 #endif // bpgl_comp_rational_camera_hxx_
436