CamPoseCalib.hh

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef __BIAS_CamPoseCalib_HH
#define __BIAS_CamPoseCalib_HH

#include <bias_config.h>
#include <Base/Common/W32Compat.hh>
#include <math.h>

#include <vector>
#include <iostream>

//#include <Base/Common/BIASpragmaStart.hh>

#include <Base/Debug/Debug.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Matrix2x2.hh>
#include <Geometry/RMatrix.hh>
#include <Geometry/PMatrix.hh>

namespace BIAS {

#define D_BIAS_CAMPOSE_ESTIMATE_STEPS 1<<0
#define D_BIAS_CAMPOSE_ESTIMATE_EQSYS 1<<1
#define D_BIAS_CAMPOSE_INIT           1<<2
#define D_BIAS_CAMPOSE_APPLY_SOL      1<<3
#define D_BIAS_CAMPOSE_ESTIMATE_ITER  1<<4
#define D_BIAS_CAMPOSE_EQSYS          1<<5
#define D_BIAS_CAMPOSE_LM             1<<6
#define D_BIAS_CAMPOSE_INITGUESS      1<<7
#define D_BIAS_CAMPOSE_SECD           1<<8

  /** @class CamPoseCalib
      @ingroup g_geometry
      @brief this classs allows estimation of the 
      internal (except skew) and external camera parameters by an 
      iterative non-linear algorithm from given 2D-3D correspondences
           
      A linearisation of the rotation is used therefore more than one 
      iteration is necessary. Iteration 
      stops if the change drops below minChange
      or if a maximum of iterations is reached, while 
      the default iterations of Estimate() are more than enough usually.<br>
      Minimum number of correspondences are 4, while the model 
      points may even lie on one plane.
      It may also work with only 3 correspondences, but not guaranteed!

      usage:      
      - Set the initial camera either as complete PMatrix or just KMatrix
      - optional: Set parameters like additional estimation of f, principal
      - Add correspondences
      - optional: add 2D coVariances of the correspondences

      - Call Estimate()

      - Get the new estimated projection matrix with GetP() or
      get the resulting transformation by GetRotation() and GetTranslation()

      see also example ExampleCamPoseCalib.cc
      @author Daniel Grest, Dec 2004
  */

  class BIASGeometry_EXPORT CamPoseCalib : public BIAS::Debug
  {
  public:
    CamPoseCalib();
    ~CamPoseCalib(){};

    /** clears the correspondences, such that u can do a new estimate,      
     */
    void Reset(); 

    /** add a corresponding point pair 
     */
    int AddCorr(const BIAS::Vector3<double> &x, 
                const BIAS::Vector2<double> &y);

    /** add a corresponding 3D-point 2D-line pair 
     */
    int AddCorr(const BIAS::Vector3<double> &x, 
                const BIAS::Vector2<double> &pointOnLine,
                const BIAS::Vector2<double> &normalOfLine);

    /** activates an automatic initial guess for the projection matrix, 
        according to Christian Perwass' habil.
    */
    int SetInitialCamera(const BIAS::KMatrix &K);

    /** activates an automatic initial guess for the projection matrix, 
        according to Christian Perwass' habil.
    */
    int SetInitialCamera(double fx, double fy, double cx, double cy);

    /** Set the initial cam yourself if you have a good guess.
     */
    int SetInitialCamera(const BIAS::PMatrix &P);
    /** Set the initial cam yourself if you have a good guess.
     */
    int SetInitialCamera(double fx, double fy, double cx, double cy,
                         const BIAS::RMatrix &R, 
                         const BIAS::Vector3<double> &C);

    /** wether to do Levenberg-Maquardt estimation.
        This guarantees an error decrease in each iteration,
        but takes more time, default is on
    */
    inline void SetLM(bool on){ doLM_=on; }

    /** wether to estimate the focal length f also, default is NO
        @param val 0 NO, 1 YES, 2 BOTH
    */
    inline void SetEstimateFocal(unsigned char val) { estimateF_=val; }

    /** wether to estimate the principal point also, default is NO
     */
    inline void SetEstimatePrincipal(bool yes) { estimatePrincipal_=yes; }

    /* what kind of extrinsics translation to estimate, default is all
     */
    inline void SetEstimateTranslation(const bool estimateTransX,
                                       const bool estimateTransY,
                                       const bool estimateTransZ) {
      estimateTransX_=estimateTransX; estimateTransY_=estimateTransY;
      estimateTransZ_=estimateTransZ;
    }

    /** if the estimated translation in one iteration step is smaller
        than this value, iteration stops, default is 1E-9
    */
    inline void SetChangeEpsilon(double transEpsilon, double rotEpsilon) { 
      transChangeEpsilon_ = transEpsilon; 
      rotChangeEpsilon_ = rotEpsilon;
    }

    /** optional: set weights for each correspondence.<br>
     */
    inline void SetWeights(const std::vector<double> &weights)
    { weights_=weights; }

    inline void SetWeightsLine(const std::vector<double> &weights)
    { weightsLine_=weights; }

    /** optional: do robust estimation, if the error is larger than 
        k, an outlier is assumed <br>
        @param func: choose the M-Estimator(google for it) function <br>
        0 is Fair- function, 1 is Huber function and 2 is Tukey function
    */
    inline void SetWeighting(double k, int func=1) 
    { outlierError_=k; robustFunc_=func; }
  
    
    /** If uncertainties for the 2D points is available set them here.
        Expects the already inverted CoVariances, one for each 2D point.
     */
    inline void 
    SetVariances(const std::vector<BIAS::Matrix2x2<double> > &variances)
    { variances_= variances; }

    /** Does the calculation with a certain number of iterations.
        Less iterations may speed up the processing, but may result in a more
        inaccurate estimation. <br>
        If LM is active(default), 
        in each iteration there may be the same amount
        of LM iterations, such that the total number of 
        function evaluation is up to iterations^2
        @return 0=ok(epsilon reached), <0:error, >0: maxiter reached, 
               -5: all image points are on a line        
    */
    int Estimate(unsigned int iterations=20);

    /** returns the estimated new projection matrix 
     */
    void GetP(BIAS::PMatrix &P);

    /** this return the relative rotation from the initial camera orientation
        to the new estimated camera rotation 
    */
    inline BIAS::RMatrix GetRotation() const 
    { return resultR_.Transpose(); }

    /** this returns the translation from the initial camera center
        to the new camera center
    */
    inline BIAS::Vector3<double> GetTranslation() const 
    { return camR_* resultR_.Transpose() * 
        ((-1.0/scaleNumStabiltity_)*resultTrans_); }

    /** returns the covariance of the last estimate, which is
        (H=Hessian): H^{-1} * eps^T*eps * 1/(2n-p)
        The first 3 parameters are translation, then rotation.
        The covariance is given in the camera coord system, 
        not the world coord system !    
        Also see dissertation of Prof. Koch, page 67.
        @author grest, Jan. 2006 */
    int GetCoVarMatrix(BIAS::Matrix<double> &coVar);

    /** returns the covarinace of the last estimate in the world coo system
        @author woelk 04/2006 */
    void GetCov(BIAS::Matrix<double>& cov);

    /** @brief returns the result vector, corresponding to the covariance 
        matrix
        @author woelk 03/2006 */
    BIAS::Vector<double> GetResult() const;

    /** returns the avg error of the estimation,
        which is the average difference of 
        the projected points to their
        corresponding image points . Uses the Mahalanobis-distance,
        if variances are specified.
    */
    double GetAvgError();
    

    /** for analyzing only. Set the known groundtruth, used in outputPose_()
     */
    void SetGroundTruth(BIAS::Vector3<double> optAxis, 
                        BIAS::Vector3<double> upVector,
                        BIAS::Vector3<double> center) {
      truthUpVec_= upVector;
      truthCenter_=center; 
      truthOptAxis_=optAxis;
    }

    /** @author woelk 05/2006 */
    int TransformVec(const BIAS::Vector<double>& src, 
                      BIAS::Vector<double>& dst);

    
    //    inline void SetCalcSecondDerivatives(bool on) {
    //      calcSecD_=on;
    //    }

  protected:
    ///////////////////////// functions /////////////////////////////////////  
    int Solve_(BIAS::Matrix<double> &J, BIAS::Vector<double> &eps,
               BIAS::Vector<double> &result);

    int SolveLM_(BIAS::Matrix<double> &J,BIAS::Vector<double> &eps,
                 BIAS::Vector<double> &result, unsigned int iterations);


    /** returns the avg error of the estimation,
        which is the weighted average squared difference of 
        the projected points to their  corresponding image point  
    */
    double GetAvgSquaredWeightedError_();
   

    /** helper function for Estimate, creates the Jacobi matrix
     */
    int CreateEquationSys_();
    /** adds rows for 3D-point-2D-point corrs
     */
    int AddConstraints2DLine_(); 

    /** helper function for Estimate 
     */
    int ApplySolution_(BIAS::Vector<double> &result);

    /** for robust estimation
     */
    int SetWeightsByError_();
  
    // creates the second derivatives     
    int CreateSecDMatrix_();

    int GuessInitialCam_();

    int ApplyCoVariances_();

    // for logging and anaylze    
    void outputPose_(int iteration);
    BIAS::Vector3<double> truthUpVec_, truthCenter_, truthOptAxis_;

    std::vector<BIAS::Vector3<double> > mps_,initialMps_, origMps_;
    std::vector<BIAS::Vector2<double> > tps_; // target points 

    // model points for 2Dlines 
    std::vector<BIAS::Vector3<double> > mpsLine_,origMpsLine_,initialMpsLine_; 
    std::vector<BIAS::Vector2<double> > tpsLinePoint_,tpsLineNormal_;

    double transChangeEpsilon_, rotChangeEpsilon_;

    // minimum depth value of a modelpoint, else the corr is not used
    double minDepthModel_; 
    bool initialGuess_;

    // the scale of all model points, to achieve numerical stability
    // most stable, if all entries in the Jacobian are near 1.0
    double scaleNumStabiltity_;

    std::vector<double> weights_, weightsLine_;
    BIAS::SVD svd_; // for linear system sloving
    BIAS::Vector<double> solveVector_;
    BIAS::Matrix<double> solveMatrix_;
    BIAS::Matrix<double> secDMatrix_;

    // for switching on/off the use of second derivatives
    const bool calcSecD_; 

    BIAS::Matrix<double> coVarMat_; // J^T*J

    int paramToEst_;
    int estimateF_;
    bool estimateTransX_,estimateTransY_,estimateTransZ_;
    bool estimatePrincipal_;

    // camera values, sx is the focal length in pixel in x-dir
    double sx_,sy_,cx_,cy_;

    BIAS::Vector3<double> resultTrans_,camC_;
    BIAS::RMatrix camR_,camRT_,resultR_;
    BIAS::PMatrix camP_;

    BIAS::Vector3<double> center_,avgRay_,avgModel_,origAvgModel_;

    bool initialized_;

    // for robust estimation
    double outlierError_;
    int robustFunc_;

    std::vector<BIAS::Matrix2x2<double> > variances_;

    /// for LM algorithm extension
    bool doLM_;
    double mu_; // the weighting value for the Identity matrix;
    double initMu_;
    double lastError_;
    double errorIncreaseForLM_;
    BIAS::RMatrix lastRot_;
  };

}


//#include <Base/Common/BIASpragmaEnd.hh>


#endif