ProjectionParametersPerspectiveDepth.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_ProjectionParametersPerspectiveDepth_hh__
#define __BIAS_ProjectionParametersPerspectiveDepth_hh__

#include "bias_config.h"
#include <MathAlgo/Interpolator.hh>
#include <Base/Common/BIASpragmaStart.hh>
#include <Geometry/ProjectionParametersPerspective.hh>

namespace BIAS {

static unsigned nrOfSplineParameters_ =12;
static unsigned nrOfReflectivityParameters_=4;

  enum BIAS_TOF_DEPTH_ERROR_MODEL
    {
            DEPTH_MODEL_UNDEFINED = -1, // dummy for calibration software
      DEPTH_MODEL_POLYNOM = 0,   // standard depth error model
      DEPTH_MODEL_LINEAR  = 1,   // Linear depth error model
      DEPTH_MODEL_SPLINE  = 2,    // Splines depth error model
      DEPTH_MODEL_NONE  = 3    // No depth deviation
    };

  /** @class ProjectionParametersPerspectiveDepth
   *  @ingroup g_geometry
   *  @brief additional depth calibration parameters for a perspective depth camera
   *  to be used for ToF-(PMD) cameras. Models different depth distortion models which account
   *  for distortions due to sinusoidal assumptions and different surface reflectivities
   *  @author ischiller
   *  @date 01/2009-06/2010  */
  class BIASGeometry_EXPORT ProjectionParametersPerspectiveDepth
    : public  ProjectionParametersPerspective {
  public:

    /** \brief constructor with image width and height
     *  \param width[in]: image width (default=0)
     *  \param height[in]: image height (default=0)
     *  \author ischiller */
    ProjectionParametersPerspectiveDepth(const unsigned int width = 0,
                                         const unsigned int height = 0);     

    /** \brief constructor with depth distortion model
    *  \param model[in]: BIAS_TOF_DEPTH_ERROR_MODEL depth distortion model (Polynom or Spline...)
    *  \author ischiller */
    ProjectionParametersPerspectiveDepth(BIAS_TOF_DEPTH_ERROR_MODEL model);
    
    /** \brief copy constructor with ProjectionParametersPerspectiveDepth
    *  \param P[in]: ProjectionParametersPerspectiveDepth to init from
    *  \author ischiller */
    ProjectionParametersPerspectiveDepth(const ProjectionParametersPerspectiveDepth & P)
    { Init();  *this = P; };

    /** \brief copy constructor with ProjectionParametersPerspective
    *  \param P[in]: ProjectionParametersPerspective to init from
    *  \author ischiller */
    ProjectionParametersPerspectiveDepth(const ProjectionParametersPerspective & P)
    {Init(); *this = P;  };

    /** \brief operator = with ProjectionParametersPerspectiveDepth
    *  \param P[in]: ProjectionParametersPerspective to assign from
    *  \return assigned ProjectionParametersPerspectiveDepth
    *  \author ischiller */
    ProjectionParametersPerspectiveDepth&
      operator=(const ProjectionParametersPerspectiveDepth & P){
      ProjectionParametersPerspective::operator=(P);
      DepthCalibrationParameters_ = P.DepthCalibrationParameters_;
      listSplineKnotPoints_ = P.listSplineKnotPoints_;
      DepthCalibrationModel_= P.DepthCalibrationModel_;
   
      ReflectivityCalibrationParameters_ = P.ReflectivityCalibrationParameters_;
      depthIntNormValues_ = P.depthIntNormValues_;
      minNormIntCoeffs_ = P.minNormIntCoeffs_;
      maxNormIntCoeffs_= P.maxNormIntCoeffs_;
      radiusIntNormCoeffs_ = P.radiusIntNormCoeffs_;
      minDepth_ = P.minDepth_;
      maxDepth_ = P.maxDepth_;
      if(DepthCalibrationModel_ == DEPTH_MODEL_SPLINE)
        PrepareSpline_();
      return *this;
    };
 
    /** \brief operator = with ProjectionParametersPerspective
    *  \param P[in]: ProjectionParametersPerspective to assign from
    *  \return assigned ProjectionParametersPerspectiveDepth
    *  \author ischiller */
    ProjectionParametersPerspectiveDepth&
      operator=(const ProjectionParametersPerspective & P){
      ProjectionParametersPerspective::operator=(P);
      DepthCalibrationParameters_.resize(6, 0.0);
      nrOfReflectivityParameters_ = 3;
      nrOfSplineParameters_ = 12;
      DepthCalibrationModel_=DEPTH_MODEL_POLYNOM;
      // 3 for Reflectivity polynom f(), 2 for radial g()
      ReflectivityCalibrationParameters_.resize(nrOfReflectivityParameters_, 0.0);      
      minDepth_ = 0.0;
      maxDepth_ = 15000.0;
      return *this;
    };

    /** \brief destructor which clears all vectors
    *  \author ischiller */
    virtual ~ProjectionParametersPerspectiveDepth(){
      DepthCalibrationParameters_.clear();
      listSplineKnotPoints_.clear();
      ReflectivityCalibrationParameters_.clear();
      depthIntNormValues_.clear();
      minNormIntCoeffs_.clear();
      maxNormIntCoeffs_.clear();
      radiusIntNormCoeffs_.clear();
    };

    /** \brief Init function called by constructors*/
    void Init();

    /** \brief covariant virtual copy constructor for use in Projection */
    virtual ProjectionParametersPerspectiveDepth* Clone() const {
      return new ProjectionParametersPerspectiveDepth(*this);
    }

    /** \brief Set the depth calibration model, one of BIAS_TOF_DEPTH_ERROR_MODEL
     *  inits the vectors and reserves spaces
         *  \param model[in]: BIAS_TOF_DEPTH_ERROR_MODEL depth distortion model (Polynom or Spline...)
         *  \author ischiller */
    void SetDepthCalibrationModel(BIAS_TOF_DEPTH_ERROR_MODEL model);

    /** \brief Set the depth calibration parameters
     *  \param params[in]: the depth distortion parameters must be of correct length according to set model
     *  \param minDepth[in]: the minimum depth used for calibration (default=0.0)
     *  \param maxDepth[in]: the maximum depth used for calibration (default=15000.0)
     *  \author ischiller */
    void SetDepthCalibrationParameters (const std::vector<double>& params,
                                        float minDepth=0.0, 
                                        float maxDepth=15000.0);

    /** \brief Set the depth calibration parameters for B-Spline model
    *  \param knots[in]: the knot points of distortion spline must be of correct length (see GetNrOfSplineParameters())
    *  \param controls[in]: the control points of distortion spline must be of correct length (see GetNrOfSplineParameters())
    *  \param minDepth[in]: the minimum depth used for calibration (default=0.0)
    *  \param maxDepth[in]: the maximum depth used for calibration (default=15000.0)
    *  \author ischiller */
    void SetDepthCalibrationParametersSpline (const std::vector<double>& knots,
                                        const std::vector<double>& controls,
                                        float minDepth=0.0, 
                                        float maxDepth=15000.0);

    /** \brief Get the depth calibration model
      *  \author ischiller */
    BIAS_TOF_DEPTH_ERROR_MODEL GetDepthCalibrationModel(){
      return DepthCalibrationModel_;
    }

    /** \brief Get the depth calibration parameters
      *  \param params[out]: the depth distortion parameters, these are the control points in spline model
      *  \author ischiller */
    void GetDepthCalibrationParameters(std::vector<double> &params){
      params.clear();           
      for(unsigned i=0;i<DepthCalibrationParameters_.size();i++){
        params.push_back(DepthCalibrationParameters_[i]);
      }
    }

    /** \brief Get the depth calibration spline knot points
    *  \param knots[out]: the knot points in spline model
    *  \author ischiller */
    void GetDepthCalibrationSplineKnots(std::vector<double> &knots){
      knots.clear();           
      for(unsigned i=0;i<listSplineKnotPoints_.size();i++){
        knots.push_back(listSplineKnotPoints_[i]);
      }
    }
    /** \brief get the minimum and maximum depth values with which the calibration has been done
     *  \param minDepth[out]: minimum depth
     *  \param maxDepth[out]: maximum depth
     *  \author ischiller */
    void GetMinMaxDepth(float& minDepth, float& maxDepth){
      minDepth = minDepth_;
      maxDepth = maxDepth_;
    }

    /** \brief sets the reflectivity calibration parameters and Fit polynomials
     *  to radial and reflectivity normalization parameters
     * \return 0 on sucess, < 0 if polynomial fitting fails
     * \author ischiller*/
    int SetReflectivityNormalizationParameters(const std::vector<double>& depthValues,
                                             const std::vector<double>& minInt, 
                                             const std::vector<double>& maxInt,
                                             const std::vector<double>& radiusValues,
                                             const std::vector<double>& radiusIntensities);
    /**\brief Set the reflectivity calibration parameters
    * \param params[in]: the parameters of the reflectivity correction polynomial
    * \author ischiller
    * \date 02/2010 */
    void SetReflectivityCalibrationParameters (const std::vector<double>& params);

    /**\brief Get the reflectivity calibration parameters
    * \param params[out]: the parameters of the reflectivity correction polynomial
    * \author ischiller
    * \date 02/2010 */
    void GetReflectivityCalibrationParameters(std::vector<double> &params){
        params.clear();
      for(unsigned i=0;i<ReflectivityCalibrationParameters_.size();i++)
        params.push_back(ReflectivityCalibrationParameters_[i]);
    }
    /**\brief Get the reflectivity calibration parameters which define the min reflectivity for
     * all depths intervals
    * \param params[out]: the parameters of the reflectivity correction polynomial
    * \author ischiller
    * \date 02/2010 */
    void GetReflectivityMinReflectivities(std::vector<double> &params){
        params.clear();
        for(unsigned i=0;i<minNormIntCoeffs_.size();i++)
            params.push_back(minNormIntCoeffs_[i]);
    }

    /**\brief Get the reflectivity calibration parameters which define the max reflectivity for
    * all depths intervals
    * \param params[out]: the parameters of the reflectivity correction polynomial
    * \author ischiller
    * \date 02/2010 */
    void GetReflectivityMaxReflectivities(std::vector<double> &params){
        params.clear();
        for(unsigned i=0;i<maxNormIntCoeffs_.size();i++)
            params.push_back(maxNormIntCoeffs_[i]);
    }

    /**\brief Get the reflectivity calibration parameters which define the radius vignetting function
    * \param params[out]: the parameters of the reflectivity correction polynomial
    * \author ischiller
    * \date 02/2010 */
    void GetReflectivityRadiusNormalization(std::vector<double> &params){
        params.clear();
        for(unsigned i=0;i<radiusIntNormCoeffs_.size();i++)
            params.push_back(radiusIntNormCoeffs_[i]);
    }

    /**\brief Distort depth value d at image position pos
     * \param pos[in]: the position in the image
     * \param d[in][out]: the depth to distort
     * \param bIsInCartesianCoords[in]: indicates whether image has been transformed to cartesian coordinates */
    void DistortDepth (HomgPoint2D const &pos, float &d,bool bIsInCartesianCoords=false);

    /**\brief UnDistort depth value d at image position pos
     * \param pos[in]: the position in the image
     * \param d[in][out]: the depth to undistort
     * \param bIsInCartesianCoords[in]: indicates whether image has been transformed to cartesian coordinates */
    void UnDistortDepth (HomgPoint2D const &pos, float &d,bool bIsInCartesianCoords=false);

    /** \brief Undistorts the passed depth map in place. Uses method UnDistortDepth.
     * \return 0 if everything went smoothly, -1 if parameter's size didn't fit the input image's size.
     * \param depthMap[in][out]: the depth image to undistort, will be altered
     * \param bIsInCartesianCoords[in]: indicates whether image has been transformed to cartesian coordinates
     * \author ischiller **/
    int UnDistortDepthMapIP(BIAS::Image<float>& depthMap,bool bIsInCartesianCoords=false) ;

    /** \brief Distorts the passed depth map in place. Uses method DistortDepth.
     * \return 0 if everything went smoothly, -1 if parameter's size didn't fit the input image's size.
     * \param depthMap[in][out]: the depth image to distort, will be altered
     * \param bIsInCartesianCoords[in]: indicates whether image has been transformed to cartesian coordinates
     * \author ischiller **/
    int DistortDepthMapIP(BIAS::Image<float>& depthMap,bool bIsInCartesianCoords=false) ;


    /** \brief Distort Reflectivity value at depth at image offset form center radius
     *  \param depth[in][out]: the depth value, will be altered
     *  \param Reflectivity[in]: the reflectivity at that position
     *  \param radius[in]: current offset from principle point
     *  \author ischiller
     *  \date 02/2010 */
    void DistortReflectivity(HomgPoint2D const &pos,float &depth,
            float &Reflectivity,bool bIsInCartesianCoords=false);

    /** \brief UnDistort Reflectivity value i at image position pos
     *  \param depth[in][out]: the depth value, will be altered
     *  \param Reflectivity[in]: the reflectivity at that position
     *  \param radius[in]: current offset from principle point
     *  \author ischiller
     *  \date 02/2010 */
    void UnDistortReflectivity(HomgPoint2D const &pos,float &depth,
            float &Reflectivity,bool bIsInCartesianCoords=false);

    /** \brief Distort Reflectivity error in depth map
     *  \param depthMap[in][out]: the depth map, will be altered
     *  \param ReflectivityImage[in]: the reflectivity image
     *  \author ischiller
     *  \date 02/2010 */
    int DistortReflectivityDepthIP(BIAS::Image<float>& depthMap, 
                                       BIAS::Image<float>& ReflectivityImage,
                                   bool bIsInCartesianCoords=false);

    /** \brief Undistort Reflectivity error in depth map
    *  \param depthMap[in][out]: the depth map, will be altered
    *  \param ReflectivityImage[in]: the reflectivity image
    *  \author ischiller
    *  \date 02/2010 */
    int UnDistortReflectivityDepthIP(BIAS::Image<float>& depthMap, 
                                     BIAS::Image<float>& ReflectivityImage,
                                     bool bIsInCartesianCoords=false);

    /** \brief Sets depth distortion to 0
     *  \author ischiller
     *  \date 02/2010 */
    void SetDepthDistortionToZero();

    /** \brief Sets depth reflectivity distortion to 0
    *  \author ischiller
    *  \date 02/2010 */
    void SetReflectivityDepthDistortionToZero();

    /** \brief checks whether depth distortion is present
    *  \return true if depth distortion is present, false otherwise
    *  \author ischiller
    *  \date 02/2010 */
    bool HasDepthDistortion() const;

    /** \brief checks whether depth reflectivity distortion is present
    *  \return true if depth distortion is present, false otherwise
    *  \author ischiller
    *  \date 02/2010 */
    bool HasReflectivityDistortion() const;

    /** \brief returns the number of depth error model parameters
     * \param model[in]: the model for which the number of parameters is requested
    *  \return number of parameters
    *  \author ischiller
    *  \date 02/2010 */
    static unsigned GetNrOfDepthErrormodelParameters(BIAS_TOF_DEPTH_ERROR_MODEL model);

    /** \brief returns the number of depth reflectivity error model parameters
    *  \return number of parameters
    *  \author ischiller
    *  \date 02/2010 */
    static unsigned GetNrOfReflectivityParameters();

    static std::string GetDepthDistModelString(BIAS_TOF_DEPTH_ERROR_MODEL model);

#ifdef BIAS_HAVE_XML2
    /** @brief specialization of XML block name function */
    virtual int XMLGetClassName(std::string& TopLevelTag,
                                double& Version) const;

    /** @brief specialization of XML write function */
    virtual int XMLOut(const xmlNodePtr Node, XMLIO& XMLObject) const;

    /** @brief specialization of XML read function */
    virtual int XMLIn(const xmlNodePtr Node, XMLIO& XMLObject);
#endif

    friend std::ostream& operator<<(std::ostream &os,
                                    const ProjectionParametersPerspectiveDepth& p);

    
  protected:
    long int D_DEPTH_DISTORTION;

    float minDepth_,maxDepth_;
    std::vector<double> depthIntNormValues_,minNormIntCoeffs_,maxNormIntCoeffs_;
    std::vector<double> radiusIntNormValues_, radiusIntNormCoeffs_;
    void PrepareSpline_();
    bool bSplinePrepared_;
    BIAS::Interpolator  splineInterpolator_;
    BIAS::Interpolator  splineInterpolatorUndist_;
    std::vector<double> listSplineKnotPoints_;
    std::vector<double> DepthCalibrationParameters_;
    std::vector<double> ReflectivityCalibrationParameters_;
    BIAS_TOF_DEPTH_ERROR_MODEL DepthCalibrationModel_;
  };

  /////////////// inline code ///////////////////////

  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersPerspectiveDepth& p)
  {
    ProjectionParametersPerspective ppp = p;
    os << ppp << std::endl;
    os << "ProjectionParametersPerspectiveDepth:" << std::endl;
    switch(p.DepthCalibrationModel_){
    case DEPTH_MODEL_POLYNOM:
      os << "Model:"<<"Polynom"<<std::endl;
      break;
    case DEPTH_MODEL_LINEAR:
      os << "Model:"<<"Linear"<<std::endl;
      break;
    case DEPTH_MODEL_SPLINE:
      os << "Model:"<<"Spline"<<std::endl;
      break;
    case DEPTH_MODEL_NONE:
      os << "Model:"<<"None"<<std::endl;
      break;
    case DEPTH_MODEL_UNDEFINED:
      os << "Model:"<<"Undefined"<<std::endl;
      break;
    }

    if(p.DepthCalibrationModel_!=DEPTH_MODEL_NONE){
      os<<"MinDepth:"<<p.minDepth_<<" MaxDepth_:"<<p.maxDepth_<<std::endl;
      if(p.DepthCalibrationParameters_.size()>0)
        os<<"DepthCalibration:"<<std::endl;
      for (unsigned int i=0; i<p.DepthCalibrationParameters_.size(); i++) {
        os << p.DepthCalibrationParameters_[i]<<" ";
      }
      os << std::endl;
      if(p.listSplineKnotPoints_.size()>0 && p.DepthCalibrationModel_==DEPTH_MODEL_SPLINE)
        os<<"Spline Knot Points:"<<std::endl;
      for (unsigned int i=0; i<p.listSplineKnotPoints_.size(); i++) {
        os << p.listSplineKnotPoints_[i]<<" ";
      }
      os << std::endl;
    }
    if(p.ReflectivityCalibrationParameters_.size()>0){
        os<<"Reflectivity Calibration:"<<std::endl;
        for (unsigned int i=0; i<p.ReflectivityCalibrationParameters_.size(); i++) {
            os << p.ReflectivityCalibrationParameters_[i]<<" ";
        }
        os << std::endl;
    }
    if(p.minNormIntCoeffs_.size()>0){
        os<<"Minimum Reflectivity Polynom :"<<std::endl;
        for (unsigned int i=0; i<p.minNormIntCoeffs_.size(); i++) {        
            os << "cmin" << i<<": ";
            os << p.minNormIntCoeffs_[i]<<" ";
        }
        os << std::endl;
    }
    if(p.maxNormIntCoeffs_.size()>0){
        os<<"Maximum Reflectivity Polynom :"<<std::endl;
        for (unsigned int i=0; i<p.maxNormIntCoeffs_.size(); i++) {       
            os << "cmax" << i<<": ";
            os << p.maxNormIntCoeffs_[i]<<" ";
        }
        os << std::endl;
    }
    if(p.radiusIntNormCoeffs_.size()>0){
        os<<"Radial Vignetting Polynom :"<<std::endl;
        for (unsigned int i=0; i<p.radiusIntNormCoeffs_.size(); i++) {
            os << "crad" << i<<": ";
            os<< p.radiusIntNormCoeffs_[i]<<" ";
        }
        os << std::endl;
    }

    return os;
  }



} // end namespace

#include <Base/Common/BIASpragmaEnd.hh>

#endif