|
CVD 0.8
|
00001 /* 00002 This file is part of the CVD Library. 00003 00004 Copyright (C) 2005 The Authors 00005 00006 This library is free software; you can redistribute it and/or 00007 modify it under the terms of the GNU Lesser General Public 00008 License as published by the Free Software Foundation; either 00009 version 2.1 of the License, or (at your option) any later version. 00010 00011 This library is distributed in the hope that it will be useful, 00012 but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00014 Lesser General Public License for more details. 00015 00016 You should have received a copy of the GNU Lesser General Public 00017 License along with this library; if not, write to the Free Software 00018 Foundation, Inc., 00019 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00020 */ 00021 //-*- c++ -*- 00022 00023 #ifndef __CAMERA_H 00024 #define __CAMERA_H 00025 00026 #include <cmath> 00027 #include <TooN/TooN.h> 00028 #include <TooN/helpers.h> 00029 00030 template<class T> 00031 inline T SAT(T x){return (x<-1.0/3?-1e9:x);} 00032 00035 namespace Camera { 00036 00039 class Linear { 00040 public: 00042 static const int num_parameters=4; 00043 00046 inline void load(std::istream& is); 00049 inline void save(std::ostream& os) const; 00050 00052 inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale=1) const; 00054 inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const; 00056 inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const; 00057 00060 inline TooN::Matrix<2,2> get_derivative() const; 00061 00063 inline TooN::Matrix<num_parameters,2> get_parameter_derivs() const ; 00064 00068 inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const ; 00069 00073 inline void update(const TooN::Vector<num_parameters>& updates); 00074 00077 inline TooN::Vector<num_parameters>& get_parameters() {return my_camera_parameters;} 00078 inline const TooN::Vector<num_parameters>& get_parameters() const {return my_camera_parameters;} 00079 00080 private: 00081 TooN::Vector<num_parameters> my_camera_parameters; 00082 mutable TooN::Vector<2> my_last_camframe; 00083 }; 00084 00085 00088 class Cubic { 00089 public: 00091 static const int num_parameters=5; 00092 00095 inline void load(std::istream& is); 00098 inline void save(std::ostream& os) const; 00099 00101 inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale=1) const; 00103 inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const; 00105 inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const; 00106 00109 inline TooN::Matrix<2,2> get_derivative() const; 00110 00112 inline TooN::Matrix<num_parameters,2> get_parameter_derivs() const ; 00113 00117 inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const ; 00118 00122 inline void update(const TooN::Vector<num_parameters>& updates); 00123 00126 inline TooN::Vector<num_parameters>& get_parameters() {return my_camera_parameters;} 00127 inline const TooN::Vector<num_parameters>& get_parameters() const {return my_camera_parameters;} 00128 00129 private: 00130 TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0 00131 mutable TooN::Vector<2> my_last_camframe; 00132 }; 00133 00134 00137 class Quintic { 00138 public: 00140 static const int num_parameters=6; 00141 00144 inline void load(std::istream& is); 00147 inline void save(std::ostream& os) const; 00148 00150 inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale=1) const ; 00152 00153 inline TooN::Vector<2> project_vector(const TooN::Vector<2>& x, const TooN::Vector<2>& d) const { 00154 const TooN::DefaultPrecision xsq = x*x; 00155 const TooN::DefaultPrecision& a = my_camera_parameters[4]; 00156 const TooN::DefaultPrecision& b = my_camera_parameters[5]; 00157 return (2 * (a + 2*b*xsq) * (x*d) * TooN::diagmult(my_camera_parameters.slice<0,2>(), x) + 00158 (1 + a*xsq + b*xsq*xsq)*TooN::diagmult(my_camera_parameters.slice<0,2>(), d)); 00159 } 00160 00161 inline TooN::Vector<2> project_vector(const TooN::Vector<2>& d) const { 00162 return diagmult(my_camera_parameters.slice<0,2>(), d); 00163 } 00164 inline TooN::Vector<2> unproject_vector(const TooN::Vector<2>& d) const { 00165 TooN::Vector<2> v; 00166 v[0] = d[0]/my_camera_parameters[0]; 00167 v[1] = d[1]/my_camera_parameters[1]; 00168 return v; 00169 } 00170 inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const; 00171 inline std::pair<TooN::Vector<2>, TooN::Matrix<2> > project(const TooN::Vector<2>& camframe, const TooN::Matrix<2>& R) const; 00172 00174 inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const; 00175 00176 inline std::pair<TooN::Vector<2>, TooN::Matrix<2> > unproject(const TooN::Vector<2>& imframe, const TooN::Matrix<2>& R) const; 00177 00180 inline TooN::Matrix<2,2> get_derivative() const; 00181 inline TooN::Matrix<2,2> get_derivative(const TooN::Vector<2>& x) const; 00182 00183 inline TooN::Matrix<2,2> get_inv_derivative() const; 00184 inline TooN::Matrix<2,2> get_inv_derivative(const TooN::Vector<2>& x) const; 00185 00187 inline TooN::Matrix<num_parameters,2> get_parameter_derivs() const ; 00188 00192 inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const ; 00193 00197 inline void update(const TooN::Vector<num_parameters>& updates); 00198 00201 inline const TooN::Vector<num_parameters>& get_parameters() const {return my_camera_parameters;} 00204 inline TooN::Vector<num_parameters>& get_parameters() {return my_camera_parameters;} 00205 00206 private: 00207 TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0 00208 mutable TooN::Vector<2> my_last_camframe; 00209 00210 inline TooN::DefaultPrecision sat(TooN::DefaultPrecision x) 00211 { 00212 TooN::DefaultPrecision a; 00213 a = (-3*my_camera_parameters[4] - sqrt(9*my_camera_parameters[4]*my_camera_parameters[4] - 20 * my_camera_parameters[5]))/(10*my_camera_parameters[5]); 00214 00215 if(x < a) 00216 return x; 00217 else 00218 return 1e9; //(inf) 00219 } 00220 }; 00221 00232 class Harris{ 00233 00234 private: 00235 TooN::Vector<2> radial_distort(const TooN::Vector<2>& camframe) const 00236 { 00237 TooN::DefaultPrecision r2 = camframe*camframe; 00238 return camframe / sqrt(1 + my_camera_parameters[4] * r2); 00239 } 00240 00241 00242 public: 00244 static const int num_parameters=5; 00245 00248 inline void load(std::istream& is) 00249 { 00250 is >> my_camera_parameters; 00251 } 00252 00255 inline void save(std::ostream& os) const 00256 { 00257 os << my_camera_parameters; 00258 } 00259 00260 00262 inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale=1) const 00263 { 00264 return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00265 } 00266 00268 inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const 00269 { 00270 my_last_camframe = camframe; 00271 return linearproject(radial_distort(camframe)); 00272 } 00273 00275 inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const 00276 { 00277 //Undo the focal length and optic axis. 00278 TooN::Vector<2> mod_camframe; 00279 mod_camframe[0] = (imframe[0]-my_camera_parameters[2])/my_camera_parameters[0]; 00280 mod_camframe[1] = (imframe[1]-my_camera_parameters[3])/my_camera_parameters[1]; 00281 TooN::DefaultPrecision rprime2 = mod_camframe*mod_camframe; 00282 my_last_camframe = mod_camframe / sqrt(1 - my_camera_parameters[4] * rprime2); 00283 return my_last_camframe; 00284 } 00285 00288 inline TooN::Matrix<2,2> get_derivative() const 00289 { 00290 return get_derivative(my_last_camframe); 00291 } 00292 00295 inline TooN::Matrix<2,2> get_derivative(const TooN::Vector<2>& pt) const 00296 { 00297 TooN::Matrix<2,2> J; 00298 00299 TooN::DefaultPrecision xc = pt[0]; 00300 TooN::DefaultPrecision yc = pt[1]; 00301 00302 TooN::DefaultPrecision fu= my_camera_parameters[0]; 00303 TooN::DefaultPrecision fv= my_camera_parameters[1]; 00304 TooN::DefaultPrecision a = my_camera_parameters[4]; 00305 00306 TooN::DefaultPrecision g = 1/sqrt(1 + a * (xc*xc + yc*yc)); 00307 TooN::DefaultPrecision g3= g*g*g; 00308 00309 J[0][0] = fu * (g - a * xc*xc*g3); 00310 J[0][1] = - fu * a * xc * yc * g3; 00311 J[1][0] = - fv * a * xc * yc * g3; 00312 J[1][1] = fv * (g - a * yc*yc*g3); 00313 00314 return J; 00315 } 00316 00318 inline TooN::Matrix<num_parameters,2> get_parameter_derivs() const 00319 { 00320 return get_parameter_derivs_at(my_last_camframe); 00321 } 00322 00325 inline TooN::Matrix<num_parameters,2> get_parameter_derivs_at(const TooN::Vector<2>& pt) const 00326 { 00327 TooN::Vector<2> mod_camframe = radial_distort(pt); 00328 00329 TooN::Matrix<5, 2> result; 00330 00331 TooN::DefaultPrecision xc = pt[0]; 00332 TooN::DefaultPrecision yc = pt[1]; 00333 TooN::DefaultPrecision r2 = xc*xc + yc*yc; 00334 00335 TooN::DefaultPrecision fu= my_camera_parameters[0]; 00336 TooN::DefaultPrecision fv= my_camera_parameters[1]; 00337 TooN::DefaultPrecision a = my_camera_parameters[4]; 00338 00339 TooN::DefaultPrecision g = 1/sqrt(1 + a * r2); 00340 TooN::DefaultPrecision g3= g*g*g; 00341 00342 //Derivatives of x_image: 00343 result[0][0] = mod_camframe[0]; 00344 result[1][0] = 0; 00345 result[2][0] = 1; 00346 result[3][0] = 0; 00347 result[4][0] = - fu * xc * r2 / 2 * g3; 00348 00349 //Derivatives of y_image: 00350 result[0][1] = 0; 00351 result[1][1] = mod_camframe[1]; 00352 result[2][1] = 0; 00353 result[3][1] = 1; 00354 result[4][1] = - fv * yc * r2 / 2 * g3; 00355 00356 return result; 00357 } 00358 00362 inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const 00363 { 00364 return get_parameter_derivs() * direction; 00365 } 00366 00370 //inline void update(const TooN::Vector<num_parameters>& updates); 00371 00374 inline TooN::Vector<num_parameters>& get_parameters() 00375 { 00376 return my_camera_parameters; 00377 } 00378 inline const TooN::Vector<num_parameters>& get_parameters() const 00379 { 00380 return my_camera_parameters; 00381 } 00382 00383 private: 00384 TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0, alpha 00385 mutable TooN::Vector<2> my_last_camframe; 00386 }; 00387 00390 class SquareHarris{ 00391 00392 private: 00393 TooN::Vector<2> radial_distort(const TooN::Vector<2>& camframe) const 00394 { 00395 TooN::DefaultPrecision r2 = camframe*camframe; 00396 return camframe / sqrt(1 + my_camera_parameters[4] * r2); 00397 } 00398 00399 00400 public: 00402 static const int num_parameters=5; 00403 00406 inline void load(std::istream& is) 00407 { 00408 is >> my_camera_parameters; 00409 } 00410 00413 inline void save(std::ostream& os) const 00414 { 00415 os << my_camera_parameters; 00416 } 00417 00418 00420 inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale=1) const 00421 { 00422 return TooN::Vector<2>(scale * (camframe* my_camera_parameters[0]) + my_camera_parameters.slice<2,2>()); 00423 } 00424 00426 inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const 00427 { 00428 my_last_camframe = camframe; 00429 return linearproject(radial_distort(camframe)); 00430 } 00431 00433 inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const 00434 { 00435 //Undo the focal length and optic axis. 00436 TooN::Vector<2> mod_camframe; 00437 mod_camframe[0] = (imframe[0]-my_camera_parameters[2])/my_camera_parameters[0]; 00438 mod_camframe[1] = (imframe[1]-my_camera_parameters[3])/my_camera_parameters[0]; 00439 TooN::DefaultPrecision rprime2 = mod_camframe*mod_camframe; 00440 my_last_camframe = mod_camframe / sqrt(1 - my_camera_parameters[4] * rprime2); 00441 return my_last_camframe; 00442 } 00443 00446 inline TooN::Matrix<2,2> get_derivative() const 00447 { 00448 TooN::Matrix<2,2> J; 00449 00450 TooN::DefaultPrecision xc = my_last_camframe[0]; 00451 TooN::DefaultPrecision yc = my_last_camframe[1]; 00452 00453 TooN::DefaultPrecision f= my_camera_parameters[0]; 00454 TooN::DefaultPrecision a = my_camera_parameters[4]; 00455 00456 TooN::DefaultPrecision g = 1/sqrt(1 + a * (xc*xc + yc*yc)); 00457 TooN::DefaultPrecision g3= g*g*g; 00458 00459 J[0][0] = f * (g - 2 * a * xc*xc*g3); 00460 J[0][1] = -2 * f * a * xc * yc * g3; 00461 J[1][0] = -2 * f * a * xc * yc * g3; 00462 J[1][1] = f * (g - 2 * a * yc*yc*g3); 00463 00464 return J; 00465 } 00466 00468 inline TooN::Matrix<num_parameters,2> get_parameter_derivs() const 00469 { 00470 TooN::Vector<2> mod_camframe = radial_distort(my_last_camframe); 00471 00472 TooN::Matrix<5, 2> result; 00473 00474 TooN::DefaultPrecision xc = my_last_camframe[0]; 00475 TooN::DefaultPrecision yc = my_last_camframe[1]; 00476 TooN::DefaultPrecision r2 = xc*xc + yc*yc; 00477 00478 TooN::DefaultPrecision f= my_camera_parameters[0]; 00479 TooN::DefaultPrecision a = my_camera_parameters[4]; 00480 00481 TooN::DefaultPrecision g = 1/sqrt(1 + a * r2); 00482 TooN::DefaultPrecision g3= g*g*g; 00483 00484 //Derivatives of x_image: 00485 result[0][0] = mod_camframe[0]; 00486 result[1][0] = 0; 00487 result[2][0] = 1; 00488 result[3][0] = 0; 00489 result[4][0] = - f * xc * r2 / 2 * g3; 00490 00491 00492 00493 //Derivatives of y_image: 00494 result[0][1] = mod_camframe[1]; 00495 result[1][1] = 0; 00496 result[2][1] = 0; 00497 result[3][1] = 1; 00498 result[4][1] = - f * yc * r2 / 2 * g3; 00499 00500 return result; 00501 } 00502 00506 inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const 00507 { 00508 return get_parameter_derivs() * direction; 00509 } 00510 00514 //inline void update(const TooN::Vector<num_parameters>& updates); 00515 00518 inline TooN::Vector<num_parameters>& get_parameters() 00519 { 00520 my_camera_parameters[1] = 0; 00521 return my_camera_parameters; 00522 } 00523 00524 private: 00525 TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0, alpha 00526 mutable TooN::Vector<2> my_last_camframe; 00527 }; 00528 00538 class ArcTan{ 00539 00540 private: 00541 TooN::Vector<2> radial_distort(const TooN::Vector<2>& camframe) const 00542 { 00543 const TooN::DefaultPrecision r2 = camframe*camframe; 00544 const TooN::DefaultPrecision w2 = my_camera_parameters[4] * my_camera_parameters[4]; 00545 const TooN::DefaultPrecision factor = w2*r2; 00546 TooN::DefaultPrecision term = 1.0; 00547 TooN::DefaultPrecision scale = term; 00548 term *= factor; 00549 scale -= term/3.0; 00550 term *= factor; 00551 scale += term/5.0; 00552 term *= factor; 00553 scale -= term/7.0; 00554 return (scale * camframe); 00555 } 00556 00557 00558 public: 00560 static const int num_parameters=5; 00561 00564 inline void load(std::istream& is) 00565 { 00566 is >> my_camera_parameters; 00567 } 00568 00571 inline void save(std::ostream& os) const 00572 { 00573 os << my_camera_parameters; 00574 } 00575 00577 inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale=1) const 00578 { 00579 return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00580 } 00581 00583 inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const 00584 { 00585 my_last_camframe = camframe; 00586 return linearproject(radial_distort(camframe)); 00587 } 00588 00590 inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe) const 00591 { 00592 //Undo the focal length and optic axis. 00593 TooN::Vector<2> mod_camframe; 00594 mod_camframe[0] = (imframe[0]-my_camera_parameters[2])/my_camera_parameters[0]; 00595 mod_camframe[1] = (imframe[1]-my_camera_parameters[3])/my_camera_parameters[1]; 00596 const TooN::DefaultPrecision rprime2 = mod_camframe*mod_camframe; 00597 00598 // first guess 00599 TooN::DefaultPrecision scale = mod_camframe*mod_camframe; 00600 00601 const TooN::DefaultPrecision w2 = my_camera_parameters[4]* my_camera_parameters[4]; 00602 const TooN::DefaultPrecision k3 = -w2/ 3.0; 00603 const TooN::DefaultPrecision k5 = w2*w2/ 5.0; 00604 const TooN::DefaultPrecision k7 = -w2*w2*w2/ 7.0; 00605 00606 // 3 iterations of Newton-Raphson 00607 for(int i=0; i<3; i++){ 00608 TooN::DefaultPrecision temp=1+scale*(k3 + scale*(k5 + scale*k7)); 00609 TooN::DefaultPrecision error = rprime2 - scale*temp*temp; 00610 TooN::DefaultPrecision deriv = temp*(temp+2*scale*(k3 + 2*scale*(k5 + 1.5*scale*k7))); 00611 scale += error/deriv; 00612 } 00613 my_last_camframe = mod_camframe/(1+scale*(k3+scale*(k5+scale*k7))); 00614 00615 return my_last_camframe; 00616 } 00617 00620 inline TooN::Matrix<2,2> get_derivative() const 00621 { 00622 return get_derivative(my_last_camframe); 00623 } 00624 00627 inline TooN::Matrix<2,2> get_derivative(const TooN::Vector<2>& pt) const 00628 { 00629 TooN::Matrix<2,2> J = TooN::Identity; 00630 TooN::DefaultPrecision r2=pt*pt; 00631 const TooN::DefaultPrecision w2 = my_camera_parameters[4]* my_camera_parameters[4]; 00632 const TooN::DefaultPrecision k3 = -w2/ 3.0; 00633 const TooN::DefaultPrecision k5 = w2*w2/ 5.0; 00634 const TooN::DefaultPrecision k7 = -w2*w2*w2/ 7.0; 00635 J *= (1 + k3*r2 + k5*r2*r2 + k7*r2*r2*r2); 00636 J += ((2*k3 + 4*k5*r2 + 6*k7*r2*r2) * pt.as_col()) * pt.as_row(); 00637 J[0] *= my_camera_parameters[0]; 00638 J[1] *= my_camera_parameters[1]; 00639 return J; 00640 } 00641 00643 inline TooN::Matrix<num_parameters,2> get_parameter_derivs() const 00644 { 00645 return get_parameter_derivs_at(my_last_camframe); 00646 } 00647 00650 inline TooN::Matrix<num_parameters,2> get_parameter_derivs_at(const TooN::Vector<2>& pt) const 00651 { 00652 TooN::Vector<2> mod_camframe = radial_distort(pt); 00653 00654 TooN::Matrix<5, 2> result; 00655 00656 const TooN::DefaultPrecision xc = pt[0]; 00657 const TooN::DefaultPrecision yc = pt[1]; 00658 const TooN::DefaultPrecision r2 = xc*xc + yc*yc; 00659 const TooN::DefaultPrecision r4 = r2*r2; 00660 const TooN::DefaultPrecision r6 = r4*r2; 00661 00662 const TooN::DefaultPrecision fu= my_camera_parameters[0]; 00663 const TooN::DefaultPrecision fv= my_camera_parameters[1]; 00664 00665 const TooN::DefaultPrecision w = my_camera_parameters[4]; 00666 const TooN::DefaultPrecision w3 = w*w*w; 00667 const TooN::DefaultPrecision w5 = w3*w*w; 00668 00669 const TooN::DefaultPrecision k1 = -(2.0/3.0)*w*r2; 00670 const TooN::DefaultPrecision k2 = (4.0/5.0)*w3*r4; 00671 const TooN::DefaultPrecision k3 = -(6.0/7.0)*w5*r6; 00672 00673 //Derivatives of x_image: 00674 result[0][0] = mod_camframe[0]; 00675 result[1][0] = 0; 00676 result[2][0] = 1; 00677 result[3][0] = 0; 00678 result[4][0] = fu * (k1 + k2 + k3) * xc; 00679 00680 //Derivatives of y_image: 00681 result[0][1] = 0; 00682 result[1][1] = mod_camframe[1]; 00683 result[2][1] = 0; 00684 result[3][1] = 1; 00685 result[4][1] = fv * (k1 + k2 + k3) * yc; 00686 00687 return result; 00688 } 00689 00693 inline TooN::Vector<num_parameters> get_parameter_derivs(const TooN::Vector<2>& direction) const 00694 { 00695 return get_parameter_derivs() * direction; 00696 } 00697 00701 //inline void update(const TooN::Vector<num_parameters>& updates); 00702 00705 inline TooN::Vector<num_parameters>& get_parameters() 00706 { 00707 return my_camera_parameters; 00708 } 00709 inline const TooN::Vector<num_parameters>& get_parameters() const 00710 { 00711 return my_camera_parameters; 00712 } 00713 00714 private: 00715 TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0, omega 00716 mutable TooN::Vector<2> my_last_camframe; 00717 }; 00718 00719 } 00720 00721 00722 00723 00725 // Camera::Linear inline functions // 00727 00728 void Camera::Linear::load(std::istream& is) { 00729 is >> my_camera_parameters; 00730 } 00731 00732 void Camera::Linear::save(std::ostream& os) const { 00733 os << my_camera_parameters; 00734 } 00735 00736 inline TooN::Vector<2> Camera::Linear::linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale) const { 00737 return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00738 } 00739 00740 inline TooN::Vector<2> Camera::Linear::project(const TooN::Vector<2>& camframe) const { 00741 my_last_camframe = camframe; 00742 return TooN::Vector<2>(diagmult(camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00743 } 00744 00745 inline TooN::Vector<2> Camera::Linear::unproject(const TooN::Vector<2>& imframe) const { 00746 my_last_camframe[0] = (imframe[0]-my_camera_parameters[2])/my_camera_parameters[0]; 00747 my_last_camframe[1] = (imframe[1]-my_camera_parameters[3])/my_camera_parameters[1]; 00748 return my_last_camframe; 00749 } 00750 00751 TooN::Matrix<2,2> Camera::Linear::get_derivative() const { 00752 TooN::Matrix<2,2> result; 00753 result(0,0) = my_camera_parameters[0]; 00754 result(1,1) = my_camera_parameters[1]; 00755 result(0,1) = result(1,0) = 0; 00756 return result; 00757 } 00758 00759 TooN::Matrix<Camera::Linear::num_parameters,2> Camera::Linear::get_parameter_derivs() const { 00760 TooN::Matrix<num_parameters,2> result; 00761 result(0,0) = my_last_camframe[0]; 00762 result(0,1) = 0; 00763 result(1,0) = 0; 00764 result(1,1) = my_last_camframe[1]; 00765 result(2,0) = 1; 00766 result(2,1) = 0; 00767 result(3,0) = 0; 00768 result(3,1) = 1; 00769 return result; 00770 } 00771 00772 TooN::Vector<Camera::Linear::num_parameters> Camera::Linear::get_parameter_derivs(const TooN::Vector<2>& direction) const { 00773 TooN::Vector<num_parameters> result; 00774 result[0] = my_last_camframe[0] * direction[0]; 00775 result[1] = my_last_camframe[1] * direction[1]; 00776 result[2] = direction[0]; 00777 result[3] = direction[1]; 00778 00779 return result; 00780 } 00781 00782 void Camera::Linear::update(const TooN::Vector<num_parameters>& updates){ 00783 my_camera_parameters+=updates; 00784 } 00785 00786 00788 // Camera::Cubic inline functions // 00790 00791 void Camera::Cubic::load(std::istream& is) { 00792 is >> my_camera_parameters; 00793 } 00794 00795 void Camera::Cubic::save(std::ostream& os) const { 00796 os << my_camera_parameters; 00797 } 00798 00799 inline TooN::Vector<2> Camera::Cubic::linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale) const { 00800 return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00801 } 00802 00803 inline TooN::Vector<2> Camera::Cubic::project(const TooN::Vector<2>& camframe) const{ 00804 my_last_camframe = camframe; 00805 TooN::Vector<2> mod_camframe = camframe * (1+SAT(my_camera_parameters[4]*(camframe*camframe))); 00806 return TooN::Vector<2>(diagmult(mod_camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00807 } 00808 00809 inline TooN::Vector<2> Camera::Cubic::unproject(const TooN::Vector<2>& imframe) const { 00810 TooN::Vector<2> mod_camframe; 00811 mod_camframe[0] = (imframe[0]-my_camera_parameters[2])/my_camera_parameters[0]; 00812 mod_camframe[1] = (imframe[1]-my_camera_parameters[3])/my_camera_parameters[1]; 00813 00814 // first guess 00815 TooN::DefaultPrecision scale = 1+my_camera_parameters[4]*(mod_camframe*mod_camframe); 00816 00817 // 3 iterations of Newton-Rapheson 00818 for(int i=0; i<3; i++){ 00819 TooN::DefaultPrecision error = my_camera_parameters[4]*(mod_camframe*mod_camframe) - scale*scale*(scale-1); 00820 TooN::DefaultPrecision deriv = (3*scale -2)*scale; 00821 scale += error/deriv; 00822 } 00823 my_last_camframe = mod_camframe/scale; 00824 00825 return my_last_camframe; 00826 } 00827 00828 TooN::Matrix<2,2> Camera::Cubic::get_derivative() const { 00829 TooN::Matrix<2,2> result = TooN::Identity; 00830 result *= 1+my_camera_parameters[4]*(my_last_camframe*my_last_camframe); 00831 result += (2*my_camera_parameters[4]*my_last_camframe.as_col()) * my_last_camframe.as_row(); 00832 result[0] *= my_camera_parameters[0]; 00833 result[1] *= my_camera_parameters[1]; 00834 return result; 00835 } 00836 00837 TooN::Matrix<Camera::Cubic::num_parameters,2> Camera::Cubic::get_parameter_derivs() const { 00838 TooN::Vector<2> mod_camframe = my_last_camframe * (1+my_camera_parameters[4]*(my_last_camframe*my_last_camframe)); 00839 TooN::Matrix<num_parameters,2> result; 00840 result(0,0) = mod_camframe[0]*my_camera_parameters[0]; 00841 result(0,1) = 0; 00842 result(1,0) = 0; 00843 result(1,1) = mod_camframe[1]*my_camera_parameters[1]; 00844 result(2,0) = 1*my_camera_parameters[0]; 00845 result(2,1) = 0; 00846 result(3,0) = 0; 00847 result(3,1) = 1*my_camera_parameters[1]; 00848 result[4] = diagmult(my_last_camframe,my_camera_parameters.slice<0,2>())*(my_last_camframe*my_last_camframe); 00849 return result; 00850 } 00851 00852 TooN::Vector<Camera::Cubic::num_parameters> Camera::Cubic::get_parameter_derivs(const TooN::Vector<2>& direction) const { 00853 TooN::Vector<2> mod_camframe = my_last_camframe * (1+my_camera_parameters[4]*(my_last_camframe*my_last_camframe)); 00854 TooN::Vector<num_parameters> result; 00855 result[0] = mod_camframe[0] * direction[0] *my_camera_parameters[0]; 00856 result[1] = mod_camframe[1] * direction[1] *my_camera_parameters[1]; 00857 result[2] = direction[0] *my_camera_parameters[0]; 00858 result[3] = direction[1] *my_camera_parameters[1]; 00859 result[4] = (diagmult(my_last_camframe,my_camera_parameters.slice<0,2>())*direction)*(my_last_camframe*my_last_camframe); 00860 return result; 00861 } 00862 00863 void Camera::Cubic::update(const TooN::Vector<num_parameters>& updates){ 00864 TooN::DefaultPrecision fu=my_camera_parameters[0]; 00865 TooN::DefaultPrecision fv=my_camera_parameters[1]; 00866 my_camera_parameters[0]+=fu*updates[0]; 00867 my_camera_parameters[1]+=fv*updates[1]; 00868 my_camera_parameters[2]+=fu*updates[2]; 00869 my_camera_parameters[3]+=fv*updates[3]; 00870 my_camera_parameters[4]+=updates[4]; 00871 //my_camera_parameters+=updates; 00872 } 00873 00875 // Camera::Quintic inline functions // 00877 00878 void Camera::Quintic::load(std::istream& is) { 00879 is >> my_camera_parameters; 00880 } 00881 00882 void Camera::Quintic::save(std::ostream& os) const { 00883 os << my_camera_parameters; 00884 } 00885 00886 inline TooN::Vector<2> Camera::Quintic::linearproject(const TooN::Vector<2>& camframe, TooN::DefaultPrecision scale) const { 00887 return TooN::Vector<2>(scale * diagmult(camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00888 } 00889 00890 inline TooN::Vector<2> Camera::Quintic::project(const TooN::Vector<2>& camframe) const { 00891 my_last_camframe = camframe; 00892 TooN::DefaultPrecision sc = /*sat*/(camframe*camframe); 00893 TooN::Vector<2> mod_camframe = camframe * (1 + sc*(my_camera_parameters[4] + sc*my_camera_parameters[5])); 00894 return TooN::Vector<2>(diagmult(mod_camframe, my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>()); 00895 } 00896 00897 inline std::pair<TooN::Vector<2>, TooN::Matrix<2> > Camera::Quintic::project(const TooN::Vector<2>& camframe, const TooN::Matrix<2>& R) const 00898 { 00899 std::pair<TooN::Vector<2>, TooN::Matrix<2> > result; 00900 result.first = this->project(camframe); 00901 const TooN::Matrix<2> J = this->get_derivative(); 00902 result.second = J * R * J.T(); 00903 return result; 00904 } 00905 00906 inline TooN::Vector<2> Camera::Quintic::unproject(const TooN::Vector<2>& imframe) const { 00907 TooN::Vector<2> mod_camframe; 00908 mod_camframe[0] = (imframe[0]-my_camera_parameters[2])/my_camera_parameters[0]; 00909 mod_camframe[1] = (imframe[1]-my_camera_parameters[3])/my_camera_parameters[1]; 00910 00911 // first guess 00912 TooN::DefaultPrecision scale = mod_camframe*mod_camframe; 00913 00914 // 3 iterations of Newton-Rapheson 00915 for(int i=0; i<3; i++){ 00916 TooN::DefaultPrecision temp=1+scale*(my_camera_parameters[4]+my_camera_parameters[5]*scale); 00917 TooN::DefaultPrecision error = mod_camframe*mod_camframe - scale*temp*temp; 00918 TooN::DefaultPrecision deriv = temp*(temp+2*scale*(my_camera_parameters[4]+2*my_camera_parameters[5]*scale)); 00919 scale += error/deriv; 00920 } 00921 my_last_camframe = mod_camframe/(1+scale*(my_camera_parameters[4]+my_camera_parameters[5]*scale)); 00922 00923 //std::cout<<"Done inverse on "<<imframe<<" - when reprojected get "<<project(my_last_camframe)<<std::endl; 00924 00925 return my_last_camframe; 00926 } 00927 00928 inline std::pair<TooN::Vector<2>, TooN::Matrix<2> > Camera::Quintic::unproject(const TooN::Vector<2>& imframe, const TooN::Matrix<2>& R) const 00929 { 00930 std::pair<TooN::Vector<2>, TooN::Matrix<2> > result; 00931 result.first = this->unproject(imframe); 00932 TooN::Matrix<2> J = get_derivative(); 00933 TooN::DefaultPrecision rdet = 1/ (J[0][0] * J[1][1] - J[0][1] * J[1][0]); 00934 TooN::Matrix<2> Jinv; 00935 Jinv[0][0] = rdet * J[1][1]; 00936 Jinv[1][1] = rdet * J[0][0]; 00937 Jinv[0][1] = -rdet * J[0][1]; 00938 Jinv[1][0] = -rdet * J[1][0]; 00939 result.second = Jinv * R * Jinv.T(); 00940 return result; 00941 } 00942 00943 00944 TooN::Matrix<2,2> Camera::Quintic::get_derivative() const { 00945 TooN::Matrix<2,2> result = TooN::Identity; 00946 TooN::DefaultPrecision temp1=my_last_camframe*my_last_camframe; 00947 TooN::DefaultPrecision temp2=my_camera_parameters[5]*temp1; 00948 result *= 1+temp1*(my_camera_parameters[4]+temp2); 00949 result += (2*(my_camera_parameters[4]+2*temp2)*my_last_camframe.as_col()) * my_last_camframe.as_row(); 00950 result[0] *= my_camera_parameters[0]; 00951 result[1] *= my_camera_parameters[1]; 00952 return result; 00953 } 00954 00955 00956 00957 TooN::Matrix<2,2> Camera::Quintic::get_inv_derivative() const { 00958 TooN::Matrix<2,2> result = TooN::Identity; 00959 TooN::DefaultPrecision temp1=my_last_camframe*my_last_camframe; 00960 TooN::DefaultPrecision temp2=my_camera_parameters[5]*temp1; 00961 TooN::DefaultPrecision temp3=2*(my_camera_parameters[4]+2*temp2); 00962 00963 result *= 1+temp1*(my_camera_parameters[4]+temp2); 00964 00965 result[0][0] += my_last_camframe[1]*my_last_camframe[1]*temp3; 00966 result[0][1] =-(temp3*my_last_camframe[0]*my_last_camframe[1]); 00967 00968 result[1][1] += my_last_camframe[0]*my_last_camframe[0]*temp3; 00969 result[1][0] =-(temp3*my_last_camframe[0]*my_last_camframe[1]); 00970 00971 (result.T())[0] *= my_camera_parameters[1]; 00972 (result.T())[1] *= my_camera_parameters[0]; 00973 00974 result /= (result[0][0]*result[1][1] - result[1][0]*result[0][1]); 00975 00976 return result; 00977 } 00978 00979 TooN::Matrix<2,2> Camera::Quintic::get_inv_derivative(const TooN::Vector<2>& x) const 00980 { 00981 00982 TooN::Matrix<2,2> result = TooN::Identity; 00983 TooN::DefaultPrecision temp1=x*x; 00984 TooN::DefaultPrecision temp2=my_camera_parameters[5]*temp1; 00985 TooN::DefaultPrecision temp3=2*(my_camera_parameters[4]+2*temp2); 00986 00987 result *= 1+temp1*(my_camera_parameters[4]+temp2); 00988 //Identity(result,1+temp1*(my_camera_parameters[4]+temp2)); 00989 00990 result[0][0] += x[1]*x[1]*temp3; 00991 result[0][1] =-(temp3*x[0]*x[1]); 00992 00993 result[1][1] += x[0]*x[0]*temp3; 00994 result[1][0] =-(temp3*x[0]*x[1]); 00995 00996 (result.T())[0] *= my_camera_parameters[1]; 00997 (result.T())[1] *= my_camera_parameters[0]; 00998 00999 result /= (result[0][0]*result[1][1] - result[1][0]*result[0][1]); 01000 01001 return result; 01002 01003 } 01004 01005 TooN::Matrix<2,2> Camera::Quintic::get_derivative(const TooN::Vector<2>& x) const { 01006 TooN::Matrix<2,2> result = TooN::Identity; 01007 TooN::DefaultPrecision temp1=x*x; 01008 TooN::DefaultPrecision temp2=my_camera_parameters[5]*temp1; 01009 result *= 1+temp1*(my_camera_parameters[4]+temp2); 01010 //Identity(result,1+temp1*(my_camera_parameters[4]+temp2)); 01011 result += (2*(my_camera_parameters[4]+2*temp2)*x.as_col()) * x.as_row(); 01012 result[0] *= my_camera_parameters[0]; 01013 result[1] *= my_camera_parameters[1]; 01014 return result; 01015 } 01016 01017 TooN::Matrix<Camera::Quintic::num_parameters,2> Camera::Quintic::get_parameter_derivs() const { 01018 TooN::Matrix<num_parameters,2> result; 01019 TooN::DefaultPrecision r2 = my_last_camframe * my_last_camframe; 01020 TooN::DefaultPrecision r4 = r2 * r2; 01021 TooN::Vector<2> mod_camframe = my_last_camframe * (1+ r2 * (my_camera_parameters[4] + r2 * my_camera_parameters[5])); 01022 01023 result(0,0) = mod_camframe[0]; 01024 result(1,0) = 0; 01025 result(2,0) = 1; 01026 result(3,0) = 0; 01027 result(4,0) = my_camera_parameters[0]*my_last_camframe[0]*r2; 01028 result(5,0) = my_camera_parameters[0]*my_last_camframe[0]*r4; 01029 01030 result(0,1) = 0; 01031 result(1,1) = mod_camframe[1]; 01032 result(2,1) = 0; 01033 result(3,1) = 1; 01034 result(4,1) = my_camera_parameters[1]*my_last_camframe[1]*r2; 01035 result(5,1) = my_camera_parameters[1]*my_last_camframe[1]*r4; 01036 01037 //cout<<"Finish me!\n"; 01038 return result; 01039 } 01040 01041 TooN::Vector<Camera::Quintic::num_parameters> Camera::Quintic::get_parameter_derivs(const TooN::Vector<2>& direction) const { 01042 //TooN::Vector<num_parameters> result; 01043 //cout<<"Finish me!\n"; 01044 //FIXME improve this somewhat 01045 return get_parameter_derivs() * direction; 01046 } 01047 01048 void Camera::Quintic::update(const TooN::Vector<num_parameters>& updates){ 01049 TooN::DefaultPrecision fu = my_camera_parameters[0]; 01050 TooN::DefaultPrecision fv = my_camera_parameters[1]; 01051 01052 my_camera_parameters[0]+=updates[0]*fu; 01053 my_camera_parameters[1]+=updates[1]*fv; 01054 my_camera_parameters[2]+=updates[2]*fu; 01055 my_camera_parameters[3]+=updates[3]*fv; 01056 my_camera_parameters[4]+=updates[4]; 01057 my_camera_parameters[5]+=updates[5]; 01058 } 01059 01060 #endif
1.7.4