Image Component Library (ICL)
|
00001 /******************************************************************** 00002 ** Image Component Library (ICL) ** 00003 ** ** 00004 ** Copyright (C) 2006-2013 CITEC, University of Bielefeld ** 00005 ** Neuroinformatics Group ** 00006 ** Website: www.iclcv.org and ** 00007 ** http://opensource.cit-ec.de/projects/icl ** 00008 ** ** 00009 ** File : ICLIO/src/ICLIO/IntrinsicCalibrator.h ** 00010 ** Module : ICLIO ** 00011 ** Authors: Christian Groszewski ** 00012 ** ** 00013 ** ** 00014 ** GNU LESSER GENERAL PUBLIC LICENSE ** 00015 ** This file may be used under the terms of the GNU Lesser General ** 00016 ** Public License version 3.0 as published by the ** 00017 ** ** 00018 ** Free Software Foundation and appearing in the file LICENSE.LGPL ** 00019 ** included in the packaging of this file. Please review the ** 00020 ** following information to ensure the license requirements will ** 00021 ** be met: http://www.gnu.org/licenses/lgpl-3.0.txt ** 00022 ** ** 00023 ** The development of this software was supported by the ** 00024 ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** 00025 ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** 00026 ** Forschungsgemeinschaft (DFG) in the context of the German ** 00027 ** Excellence Initiative. ** 00028 ** ** 00029 ********************************************************************/ 00030 00031 #pragma once 00032 00033 #include <ICLUtils/BasicTypes.h> 00034 #include <ICLUtils/Macros.h> 00035 #include <ICLUtils/Uncopyable.h> 00036 #include <ICLUtils/ConfigFile.h> 00037 #include <ICLUtils/StackTimer.h> 00038 #include <ICLUtils/Array2D.h> 00039 #include <ICLMath/DynMatrix.h> 00040 #include <ICLMath/DynMatrixUtils.h> 00041 #include <ICLCore/Img.h> 00042 #include <ICLIO/ImageUndistortion.h> 00043 00044 namespace icl{ 00045 namespace io{ 00046 00054 class ICLIO_API IntrinsicCalibrator : public utils::Uncopyable{ 00055 00056 public: 00058 struct ICLIO_API Result : public ImageUndistortion{ 00059 00061 Result(){} 00062 00064 Result(const std::vector<double> ¶ms, const utils::Size &size): 00065 ImageUndistortion("MatlabModel5Params",params,size){} 00066 00068 00070 double getFocalLengthX() const { return getParams()[0]; } 00072 00074 double getFocalLengthY() const { return getParams()[1]; } 00076 00078 double getPrincipalX() const { return getParams()[2]; } 00080 00083 double getPrincipalY() const { return getParams()[3]; } 00085 00088 double getSkew() const { return getParams()[4]; } 00089 00091 00094 double getK1() const { return getParams()[5]; } 00096 00099 double getK2() const { return getParams()[6]; } 00101 00104 double getK3() const { return getParams()[9]; } 00106 00109 double getP1() const { return getParams()[7]; } 00111 00114 double getP2() const { return getParams()[8]; } 00115 }; 00116 private: 00118 class Data; 00119 Data *m_data; 00121 Result m_calres; 00122 00124 00132 void rigid_motion(const math::DynMatrix<icl64f> &X, 00133 const math::DynMatrix<icl64f> &om, 00134 const math::DynMatrix<icl64f> &T, 00135 math::DynMatrix<icl64f> &Y, math::DynMatrix<icl64f> &dYdom, math::DynMatrix<icl64f> &dYdT); 00136 00138 00154 void project_points2(const math::DynMatrix<icl64f> &X, const math::DynMatrix<icl64f> &om, const math::DynMatrix<icl64f> &T, 00155 const math::DynMatrix<icl64f> &f, const math::DynMatrix<icl64f> &c, const math::DynMatrix<icl64f> &k, const double alpha, 00156 math::DynMatrix<icl64f> &xp, math::DynMatrix<icl64f> &dxpdom, math::DynMatrix<icl64f> &dxpdT, math::DynMatrix<icl64f> &dxpdf, 00157 math::DynMatrix<icl64f> &dxpdc, math::DynMatrix<icl64f> &dxpdk, math::DynMatrix<icl64f> &dxpdalpha); 00158 00160 00165 void comp_distortion_oulu(const math::DynMatrix<icl64f> xd,const math::DynMatrix<icl64f> k, math::DynMatrix<icl64f> &x); 00166 00168 00173 void compute_homography(const math::DynMatrix<icl64f> &m, const math::DynMatrix<icl64f> &M, math::DynMatrix<icl64f> &H); 00174 00176 00181 void optimize(const math::DynMatrix<icl64f> &impoints, const math::DynMatrix<icl64f> &worldPoints, double *params); 00182 00184 00192 void init_intrinsic_param(const math::DynMatrix<icl64f> &x, const math::DynMatrix<icl64f> &X, 00193 math::DynMatrix<icl64f> &fc, math::DynMatrix<icl64f> &cc, math::DynMatrix<icl64f> &kc, double &alpha_c); 00194 00196 00208 void comp_ext_calib(const math::DynMatrix<icl64f> &x_kk, const math::DynMatrix<icl64f> &X_kk, const math::DynMatrix<icl64f> &fc, 00209 const math::DynMatrix<icl64f> &cc, const math::DynMatrix<icl64f> &kc, const double alpha_c, const double thresh_cond, 00210 math::DynMatrix<icl64f> &omckk, math::DynMatrix<icl64f> &Tckk, math::DynMatrix<icl64f> &Rckk); 00211 00213 00221 void normalize_pixel(const math::DynMatrix<icl64f> x_kk, const math::DynMatrix<icl64f> fc, const math::DynMatrix<icl64f> cc, 00222 const math::DynMatrix<icl64f> kc,const double alpha_c, math::DynMatrix<icl64f> &xn); 00223 00225 00230 void mean(const math::DynMatrix<icl64f> &x_k, math::DynMatrix<icl64f> &res); 00231 00233 /* 00234 *@param in rotation matrix or vector 00235 *@param out rotation matrix or vector 00236 *@param dout derivative of rotation 00237 */ 00238 00239 void rodrigues(const math::DynMatrix<icl64f> &in,math::DynMatrix<icl64f> &out, math::DynMatrix<icl64f> &dout); 00240 00242 00253 void compute_extrinsic_init(const math::DynMatrix<icl64f> &x_kk, const math::DynMatrix<icl64f> &X_kk, const math::DynMatrix<icl64f> &fc, 00254 const math::DynMatrix<icl64f> &cc, const math::DynMatrix<icl64f> &kc, const double &alpha_c, 00255 math::DynMatrix<icl64f> &omckk, math::DynMatrix<icl64f> &Tckk, math::DynMatrix<icl64f> &Rckk); 00256 00258 /* 00259 * @param x_kk image coordinates 00260 * @param X_kk world coordinates 00261 * @param fc focal length 00262 * @param cc principal point 00263 * @param kc distortion coefficients 00264 * @param alpha_c skew 00265 * @param MaxIter maximum number of iterations for optimazation 00266 * @param omckk rotation vector 00267 * @param Tckk translation vector 00268 * @param Rckk rotation matricx 00269 */ 00270 void compute_extrinsic_refine(const math::DynMatrix<icl64f> &omc_init, const math::DynMatrix<icl64f> &Tc_init, 00271 const math::DynMatrix<icl64f> &x_kk, const math::DynMatrix<icl64f> &X_kk, const math::DynMatrix<icl64f> &fc,const math::DynMatrix<icl64f> &cc, 00272 const math::DynMatrix<icl64f> &kc,const double alpha_c, const int MaxIter, double thresh_cond, 00273 math::DynMatrix<icl64f> &omckk, math::DynMatrix<icl64f> &Tckk, math::DynMatrix<icl64f> &Rckk, math::DynMatrix<icl64f> &JJ); 00274 00275 public: 00276 00278 00285 IntrinsicCalibrator(unsigned int boardWidth=6, unsigned int boardHeight=9, unsigned int boardCount=8, 00286 unsigned int imageWidth=640, unsigned int imageHeight=480); 00287 00289 ~IntrinsicCalibrator(); 00290 00292 00297 Result calibrate(const math::DynMatrix<icl64f> &impoints, const math::DynMatrix<icl64f> &worldpoints); 00298 00300 00303 void saveIntrinsics(const std::string &filename); 00304 00306 00309 void loadIntrinsics(const std::string &filename); 00310 00312 00319 void resetData(unsigned int boardWidth, unsigned int boardHeight, 00320 unsigned int boardCount,unsigned int imageWidth ,unsigned int imageHeight); 00321 00323 Result getResult() const { 00324 return m_calres; 00325 } 00326 00327 //2d coordinates in picture 00328 typedef math::FixedMatrix<double,1,2> Pos2D; 00329 //grid of 2d coords in picture 00330 typedef utils::Array2D<Pos2D> DetectedGrid; 00331 00332 struct CalibrationData{ 00333 // list of detected images (row-major order of points) 00334 std::vector<DetectedGrid> data; 00335 //imagesize of data 00336 utils::Size imageSize; 00337 }; 00338 00340 00343 static Result optimize(const CalibrationData &data); 00344 00345 }; 00346 } // namespace io 00347 } 00348