vpPose.h

/****************************************************************************
 *
 * $Id: vpPose.h 5211 2015-01-26 17:44:35Z strinh $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Pose computation.
 *
 * Authors:
 * Eric Marchand
 * Francois Chaumette
 * Aurelien Yol
 *
 *****************************************************************************/

#ifndef vpPOSE_HH
#define vpPOSE_HH

#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpHomography.h>
#include <visp/vpPoint.h>
#include <visp/vpRGBa.h>
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
#  include <visp/vpList.h>
#endif

#include <math.h>
#include <list>
#include <vector>

class VISP_EXPORT vpPose
{  
public:
  typedef enum
    {
      LAGRANGE         ,
      DEMENTHON        ,
      LOWE             ,
      RANSAC           ,
      LAGRANGE_LOWE    ,
      DEMENTHON_LOWE   ,
      VIRTUAL_VS       ,
      DEMENTHON_VIRTUAL_VS,
      LAGRANGE_VIRTUAL_VS
    } vpPoseMethodType;

  unsigned int npt ;       
  std::list<vpPoint> listP ;     

  double residual ;     

protected :
  double lambda ;

private:
  int vvsIterMax ; 
  std::vector<vpPoint> c3d ;
  bool computeCovariance;
  vpMatrix covarianceMatrix;
  
  unsigned int ransacNbInlierConsensus;
  int ransacMaxTrials;
  std::vector<vpPoint> ransacInliers;
  double ransacThreshold;

protected:
  double computeResidualDementhon(const vpHomogeneousMatrix &cMo) ;

  // method used in poseDementhonPlan()
  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo) ;

public:
  // constructor
  vpPose() ;
  virtual ~vpPose() ;
  void addPoint(const vpPoint& P) ;
  void clearPoint() ;

  bool computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
  double computeResidual(const vpHomogeneousMatrix &cMo) const ;
  bool coplanar(int &coplanar_plane_type) ;
  void displayModel(vpImage<unsigned char> &I,
                    vpCameraParameters &cam,
                    vpColor col=vpColor::none) ;
  void displayModel(vpImage<vpRGBa> &I,
                    vpCameraParameters &cam,
                    vpColor col=vpColor::none) ;
  double distanceToPlaneForCoplanarityTest ;
  void init() ;
  void poseDementhonPlan(vpHomogeneousMatrix &cMo) ;
  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ;
  void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0) ;
  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) ;
  void poseLowe(vpHomogeneousMatrix & cMo) ;
  bool poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
  void poseVirtualVSrobust(vpHomogeneousMatrix & cMo) ;
  void poseVirtualVS(vpHomogeneousMatrix & cMo) ;
  void printPoint() ; 
  void setDistanceToPlaneForCoplanarityTest(double d) ;
  void setLambda(double a) { lambda = a ; }
  void setVvsIterMax(int nb) { vvsIterMax = nb ; }
  
  void setRansacNbInliersToReachConsensus(const unsigned int &nbC){ ransacNbInlierConsensus = nbC; }
  void setRansacThreshold(const double &t) {
    //Test whether or not t is > 0
    if(t > 0) {
      ransacThreshold = t;
    } else {
      throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
    }
  }
  void setRansacMaxTrials(const int &rM){ ransacMaxTrials = rM; }
  unsigned int getRansacNbInliers() const { return (unsigned int) ransacInliers.size(); }
  std::vector<vpPoint> getRansacInliers() const{ return ransacInliers; }
  
  void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
  
  vpMatrix getCovarianceMatrix() const { 
    if(!computeCovariance)
      vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
    
    return covarianceMatrix; 
  }
  
  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo,
                      vpCameraParameters &cam, double size,
                      vpColor col=vpColor::none) ;
  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo,
                      vpCameraParameters &cam, double size,
                      vpColor col=vpColor::none) ;
  static double poseFromRectangle(vpPoint &p1,vpPoint &p2,
                                  vpPoint &p3,vpPoint &p4,
                                  double lx, vpCameraParameters & cam,
                                  vpHomogeneousMatrix & cMo) ;
                     
  static void findMatch(std::vector<vpPoint> &p2D, 
                     std::vector<vpPoint> &p3D, 
                     const unsigned int &numberOfInlierToReachAConsensus,
                     const double &threshold,
                     unsigned int &ninliers,
                     std::vector<vpPoint> &listInliers,
                     vpHomogeneousMatrix &cMo,
                     const int &maxNbTrials = 10000);

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
private:
  static void initRansac(const unsigned int n,
        const double *x, const double *y,
        const unsigned int m,
        const double *X, const double *Y, const double *Z,
        vpColVector &data) ;

public:
  static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M) ;
  
  static double computeResidual(const vpColVector &x,  const vpColVector &M, vpColVector &d) ;
  
  static bool degenerateConfiguration(vpColVector &x, unsigned int *ind) ;
  
  static void ransac(const unsigned int n,
                     const double *x, const double *y,
                     const unsigned int m,
                     const double *X, const double *Y, const double *Z,
                     const int numberOfInlierToReachAConsensus,
                     const double threshold,
                     unsigned int &ninliers,
                     vpColVector &xi,  vpColVector &yi,
                     vpColVector &Xi,  vpColVector &Yi,  vpColVector &Zi,
                     vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
                     
  vp_deprecated static void ransac(const unsigned int n,
                     const vpPoint *p,
                     const unsigned int m,
                     const vpPoint *P,
                     const int numberOfInlierToReachAConsensus,
                     const double threshold,
                     unsigned int &ninliers,
                     std::list<vpPoint> &Pi,
                     vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;

  vp_deprecated static void ransac(std::list<vpPoint> &p,
                     std::list<vpPoint> &P,
                     const int numberOfInlierToReachAConsensus,
                     const double threshold,
                     unsigned int &ninliers,
                     std::list<vpPoint> &lPi,
                     vpHomogeneousMatrix &cMo, const int maxNbTrials = 10000) ;
#endif
} ;


#endif


/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */