IntrinsicCalibrator.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLIO/src/ICLIO/IntrinsicCalibrator.h                  **
** Module : ICLIO                                                  **
** Authors: Christian Groszewski                                   **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <ICLUtils/BasicTypes.h>
#include <ICLUtils/Macros.h>
#include <ICLUtils/Uncopyable.h>
#include <ICLUtils/ConfigFile.h>
#include <ICLUtils/StackTimer.h>
#include <ICLUtils/Array2D.h>
#include <ICLMath/DynMatrix.h>
#include <ICLMath/DynMatrixUtils.h>
#include <ICLCore/Img.h>
#include <ICLIO/ImageUndistortion.h>

namespace icl{
  namespace io{

    class ICLIO_API IntrinsicCalibrator : public utils::Uncopyable{

      public:
      struct ICLIO_API Result : public ImageUndistortion{

        Result(){}

        Result(const std::vector<double> &params, const utils::Size &size):
        ImageUndistortion("MatlabModel5Params",params,size){}


        double getFocalLengthX() const { return getParams()[0]; }

        double getFocalLengthY() const { return getParams()[1]; }

        double getPrincipalX() const { return getParams()[2]; }

        double getPrincipalY() const { return getParams()[3]; }

        double getSkew() const { return getParams()[4]; }


        double getK1() const { return getParams()[5]; }

        double getK2() const { return getParams()[6]; }

        double getK3() const { return getParams()[9]; }

        double getP1() const { return getParams()[7]; }

        double getP2() const { return getParams()[8]; }
      };
      private:
      class Data;
      Data *m_data;
      Result m_calres;


      void rigid_motion(const math::DynMatrix<icl64f> &X,
                        const math::DynMatrix<icl64f> &om,
                        const math::DynMatrix<icl64f> &T,
                        math::DynMatrix<icl64f> &Y, math::DynMatrix<icl64f> &dYdom, math::DynMatrix<icl64f> &dYdT);


      void project_points2(const math::DynMatrix<icl64f> &X, const math::DynMatrix<icl64f> &om, const math::DynMatrix<icl64f> &T,
                           const math::DynMatrix<icl64f> &f, const math::DynMatrix<icl64f> &c, const math::DynMatrix<icl64f> &k, const double alpha,
                           math::DynMatrix<icl64f> &xp, math::DynMatrix<icl64f> &dxpdom, math::DynMatrix<icl64f> &dxpdT, math::DynMatrix<icl64f> &dxpdf,
                           math::DynMatrix<icl64f> &dxpdc, math::DynMatrix<icl64f> &dxpdk, math::DynMatrix<icl64f> &dxpdalpha);


      void comp_distortion_oulu(const math::DynMatrix<icl64f> xd,const math::DynMatrix<icl64f> k, math::DynMatrix<icl64f> &x);


      void compute_homography(const math::DynMatrix<icl64f> &m, const math::DynMatrix<icl64f> &M, math::DynMatrix<icl64f> &H);


      void optimize(const math::DynMatrix<icl64f> &impoints, const math::DynMatrix<icl64f> &worldPoints, double *params);


      void init_intrinsic_param(const math::DynMatrix<icl64f> &x, const math::DynMatrix<icl64f> &X,
                                math::DynMatrix<icl64f> &fc, math::DynMatrix<icl64f> &cc, math::DynMatrix<icl64f> &kc, double &alpha_c);


      void comp_ext_calib(const math::DynMatrix<icl64f> &x_kk, const math::DynMatrix<icl64f> &X_kk, const math::DynMatrix<icl64f> &fc,
                        const math::DynMatrix<icl64f> &cc, const math::DynMatrix<icl64f> &kc, const double alpha_c, const double thresh_cond,
                        math::DynMatrix<icl64f> &omckk, math::DynMatrix<icl64f> &Tckk, math::DynMatrix<icl64f> &Rckk);


      void normalize_pixel(const math::DynMatrix<icl64f> x_kk, const math::DynMatrix<icl64f> fc, const math::DynMatrix<icl64f> cc,
                           const math::DynMatrix<icl64f> kc,const double alpha_c, math::DynMatrix<icl64f> &xn);


      void mean(const math::DynMatrix<icl64f> &x_k, math::DynMatrix<icl64f> &res);

      /*
          *@param in rotation matrix or vector
          *@param out rotation matrix or vector
          *@param dout derivative of rotation
          */

      void rodrigues(const math::DynMatrix<icl64f> &in,math::DynMatrix<icl64f> &out, math::DynMatrix<icl64f> &dout);


      void compute_extrinsic_init(const math::DynMatrix<icl64f> &x_kk, const math::DynMatrix<icl64f> &X_kk, const math::DynMatrix<icl64f> &fc,
                                  const math::DynMatrix<icl64f> &cc, const math::DynMatrix<icl64f> &kc, const double &alpha_c,
                                  math::DynMatrix<icl64f> &omckk, math::DynMatrix<icl64f> &Tckk, math::DynMatrix<icl64f> &Rckk);

      /*
          * @param x_kk image coordinates
          * @param X_kk world coordinates
          * @param fc focal length
          * @param cc principal point
          * @param kc distortion coefficients
          * @param alpha_c skew
          * @param MaxIter maximum number of iterations for optimazation
          * @param omckk rotation vector
          * @param Tckk translation vector
          * @param Rckk rotation matricx
          */
      void compute_extrinsic_refine(const math::DynMatrix<icl64f> &omc_init, const math::DynMatrix<icl64f> &Tc_init,
                                    const math::DynMatrix<icl64f> &x_kk, const math::DynMatrix<icl64f> &X_kk, const math::DynMatrix<icl64f> &fc,const math::DynMatrix<icl64f> &cc,
                                    const math::DynMatrix<icl64f> &kc,const double alpha_c, const int MaxIter, double thresh_cond,
                                    math::DynMatrix<icl64f> &omckk, math::DynMatrix<icl64f> &Tckk, math::DynMatrix<icl64f> &Rckk, math::DynMatrix<icl64f> &JJ);

      public:


      IntrinsicCalibrator(unsigned int boardWidth=6, unsigned int boardHeight=9, unsigned int boardCount=8,
                        unsigned int imageWidth=640, unsigned int imageHeight=480);

      ~IntrinsicCalibrator();


      Result calibrate(const math::DynMatrix<icl64f> &impoints, const math::DynMatrix<icl64f> &worldpoints);


      void saveIntrinsics(const std::string &filename);


      void loadIntrinsics(const std::string &filename);


      void resetData(unsigned int boardWidth, unsigned int boardHeight,
                     unsigned int boardCount,unsigned int imageWidth ,unsigned int imageHeight);

      Result getResult() const {
        return m_calres;
      }

      //2d coordinates in picture
      typedef math::FixedMatrix<double,1,2> Pos2D;
      //grid of 2d coords in picture
      typedef utils::Array2D<Pos2D> DetectedGrid;

      struct CalibrationData{
        // list of detected images (row-major order of points)
        std::vector<DetectedGrid> data;
        //imagesize of data
        utils::Size imageSize;
      };


      static Result optimize(const CalibrationData &data);

    };
  } // namespace io
}