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