vpMbtDistanceKltPoints.cpp

/****************************************************************************
 *
 * $Id: vpMbtDistanceKltPoints.cpp 4661 2014-02-10 19:34:58Z fspindle $
 *
 * 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:
 * Klt polygon, containing points of interest.
 *
 * Authors:
 * Romain Tallonneau
 * Aurelien Yol
 *
 *****************************************************************************/

#include <visp/vpMbtDistanceKltPoints.h>

#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))

vpMbtDistanceKltPoints::vpMbtDistanceKltPoints()
  : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(), curPoints(), curPointsInd(),
    nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), dt(1.), d0(1.),
    cam(), polygon(NULL)
{
  initPoints = std::map<int, vpImagePoint>();
  curPoints = std::map<int, vpImagePoint>();
  curPointsInd = std::map<int, int>();
}

vpMbtDistanceKltPoints::~vpMbtDistanceKltPoints()
{}

void
vpMbtDistanceKltPoints::init(const vpKltOpencv& _tracker)
{
  // extract ids of the points in the face
  nbPointsInit = 0;
  nbPointsCur = 0;
  initPoints = std::map<int, vpImagePoint>();
  curPoints = std::map<int, vpImagePoint>();
  curPointsInd = std::map<int, int>();
  std::vector<vpImagePoint> roi;
  polygon->getRoiClipped(cam, roi);

  for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i ++){
    int id;
    float x_tmp, y_tmp;
    _tracker.getFeature((int)i, id, x_tmp, y_tmp);

    if(isInside(roi, y_tmp, x_tmp)){
      initPoints[id] = vpImagePoint(y_tmp, x_tmp);
      curPoints[id] = vpImagePoint(y_tmp, x_tmp);
      curPointsInd[id] = (int)i;
      nbPointsInit++;
      nbPointsCur++;
    }
  }

  if(nbPointsCur >= minNbPoint) enoughPoints = true;
  else enoughPoints = false;

  // initialisation of the value for the computation in SE3
  vpPlane plan(polygon->getPoint(0), polygon->getPoint(1), polygon->getPoint(2));

  d0 = plan.getD();
  N = plan.getNormal();

  N.normalize();
  N_cur = N;
  invd0 = 1.0 / d0;
}

unsigned int
vpMbtDistanceKltPoints::computeNbDetectedCurrent(const vpKltOpencv& _tracker)
{
  int id;
  float x, y;
  nbPointsCur = 0;
  curPoints = std::map<int, vpImagePoint>();
  curPointsInd = std::map<int, int>();

  for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++){
    _tracker.getFeature((int)i, id, x, y);
    if(isTrackedFeature(id)){
      curPoints[id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
      curPointsInd[id] = (int)i;
      nbPointsCur++;
    }
  }

  if(nbPointsCur >= minNbPoint) enoughPoints = true;
  else enoughPoints = false;

  return nbPointsCur;
}

void
vpMbtDistanceKltPoints::computeInteractionMatrixAndResidu(vpColVector& _R, vpMatrix& _J)
{
  unsigned int index_ = 0;

  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
  for( ; iter != curPoints.end(); iter++){
    int id(iter->first);
    double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());

    double x_cur(0), y_cur(0);
    vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);

    vpImagePoint iP0 = initPoints[id];
    double x0(0), y0(0);
    vpPixelMeterConversion::convertPoint(cam, iP0, x0, y0);

    double x0_transform, y0_transform ;// equivalent x and y in the first image (reference)
    computeP_mu_t(x0, y0, x0_transform, y0_transform, H );

    double invZ = compute_1_over_Z(x_cur, y_cur);

    _J[2*index_][0] = - invZ;
    _J[2*index_][1] = 0;
    _J[2*index_][2] = x_cur * invZ;
    _J[2*index_][3] = x_cur * y_cur;
    _J[2*index_][4] = -(1+x_cur*x_cur);
    _J[2*index_][5] = y_cur;

    _J[2*index_+1][0] = 0;
    _J[2*index_+1][1] = - invZ;
    _J[2*index_+1][2] = y_cur * invZ;
    _J[2*index_+1][3] = (1+y_cur*y_cur);
    _J[2*index_+1][4] = - y_cur * x_cur;
    _J[2*index_+1][5] = - x_cur;

    _R[2*index_] =  (x0_transform - x_cur);
    _R[2*index_+1] = (y0_transform - y_cur);
    index_++;
  }
}

double
vpMbtDistanceKltPoints::compute_1_over_Z(const double x, const double y)
{
  double num = cRc0_0n[0] * x + cRc0_0n[1] * y + cRc0_0n[2];
  double den = -(d0 - dt);
  return num/den;
}

inline void
vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, const double y_in, double& x_out, double& y_out, const vpMatrix& _cHc0)
{
  double p_mu_t_2 = x_in * _cHc0[2][0] + y_in * _cHc0[2][1] + _cHc0[2][2];

  if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
    x_out = 0.0;
    y_out = 0.0;
    throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
  }

  x_out = (x_in * _cHc0[0][0] + y_in * _cHc0[0][1] + _cHc0[0][2]) / p_mu_t_2;
  y_out = (x_in * _cHc0[1][0] + y_in * _cHc0[1][1] + _cHc0[1][2]) / p_mu_t_2;
}

void
vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& _cHc0)
{
  vpRotationMatrix cRc0;
  vpTranslationVector ctransc0;

  _cTc0.extract(cRc0);
  _cTc0.extract(ctransc0);
  vpMatrix cHc0(_cHc0);

//   vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
  vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
  cHc0 /= cHc0[2][2];

  H = cHc0;

//   vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
//   vpQuaternionVector RotQuat(cRc0);
//   vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(), -RotQuat.z(), RotQuat.w());
//   vpQuaternionVector partial = RotQuat * NQuat;
//   vpQuaternionVector resQuat = (partial * RotQuatConj);
//
//   cRc0_0n = vpColVector(3);
//   cRc0_0n[0] = resQuat.x();
//   cRc0_0n[1] = resQuat.y();
//   cRc0_0n[2] = resQuat.z();

  cRc0_0n = cRc0*N;

//   vpPlane p(corners[0], corners[1], corners[2]);
//   vpColVector Ncur = p.getNormal();
//   Ncur.normalize();
  N_cur = cRc0_0n;
  dt = 0.0;
  for (unsigned int i = 0; i < 3; i += 1){
    dt += ctransc0[i] * (N_cur[i]);
  }
}

bool
vpMbtDistanceKltPoints::isTrackedFeature(const int _id)
{
//   std::map<int, vpImagePoint>::const_iterator iter = initPoints.begin();
//   while(iter != initPoints.end()){
//     if(iter->first == _id){
//       return true;
//     }
//     iter++;
//   }

  std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
  if(iter != initPoints.end())
    return true;

  return false;
}

void
vpMbtDistanceKltPoints::updateMask(
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
    cv::Mat &mask,
#else
    IplImage* mask,
#endif
    unsigned char nb, unsigned int shiftBorder)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
  int width  = mask.cols;
  int height = mask.rows;
#else
  int width  = mask->width;
  int height = mask->height;
#endif

  int i_min, i_max, j_min, j_max;
  std::vector<vpImagePoint> roi;
  polygon->getRoiClipped(cam, roi);
  vpMbtPolygon::getMinMaxRoi(roi, i_min, i_max, j_min,j_max);

  /* check image boundaries */
  if(i_min > height){ //underflow
    i_min = 0;
  }
  if(i_max > height){
    i_max = height;
  }
  if(j_min > width){ //underflow
    j_min = 0;
  }
  if(j_max > width){
    j_max = width;
  }

  double shiftBorder_d = (double) shiftBorder;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
  for(int i=i_min; i< i_max; i++){
    double i_d = (double) i;
    for(int j=j_min; j< j_max; j++){
      double j_d = (double) j;
      if(shiftBorder != 0){
        if( vpMbtDistanceKltPoints::isInside(roi, i_d, j_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
          mask.at<unsigned char>(i,j) = nb;
        }
      }
      else{
        if(vpMbtDistanceKltPoints::isInside(roi, i, j)){
          mask.at<unsigned char>(i,j) = nb;
        }
      }
    }
  }
#else
  unsigned char* ptrData = (unsigned char*)mask->imageData + i_min*mask->widthStep+j_min;
  for(int i=i_min; i< i_max; i++){
    double i_d = (double) i;
    for(int j=j_min; j< j_max; j++){
      double j_d = (double) j;
      if(shiftBorder != 0){
        if( vpMbtDistanceKltPoints::isInside(roi, i_d, j_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
            && vpMbtDistanceKltPoints::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
          *(ptrData++) = nb;
        }
        else{
          ptrData++;
        }
      }
      else{
        if(vpMbtDistanceKltPoints::isInside(roi, i, j)){
          *(ptrData++) = nb;
        }
        else{
          ptrData++;
        }
      }
    }
    ptrData += mask->widthStep - j_max + j_min;
  }
#endif
}

void
vpMbtDistanceKltPoints::removeOutliers(const vpColVector& _w, const double &threshold_outlier)
{
  std::map<int, vpImagePoint> tmp;
  std::map<int, int> tmp2;
  unsigned int nbSupp = 0;
  unsigned int k = 0;

  nbPointsCur = 0;
  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
  for( ; iter != curPoints.end(); iter++){
    if(_w[k] > threshold_outlier && _w[k+1] > threshold_outlier){
//     if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
      tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
      tmp2[iter->first] = curPointsInd[iter->first];
      nbPointsCur++;
    }
    else{
      nbSupp++;
      initPoints.erase(iter->first);
    }

    k+=2;
  }

  if(nbSupp != 0){
    curPoints = std::map<int, vpImagePoint>();
    curPointsInd = std::map<int, int>();

    curPoints = tmp;
    curPointsInd = tmp2;
    if(nbPointsCur >= minNbPoint) enoughPoints = true;
    else enoughPoints = false;
  }
}

void
vpMbtDistanceKltPoints::displayPrimitive(const vpImage<unsigned char>& _I)
{
  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
  for( ; iter != curPoints.end(); iter++){
    int id(iter->first);
    vpImagePoint iP;
    iP.set_i(static_cast<double>(iter->second.get_i()));
    iP.set_j(static_cast<double>(iter->second.get_j()));

    vpDisplay::displayCross(_I, iP, 10, vpColor::red);

    iP.set_i( vpMath::round( iP.get_i() + 7 ) );
    iP.set_j( vpMath::round( iP.get_j() + 7 ) );
    char ide[10];
    sprintf(ide, "%ld", static_cast<long int>(id));
    vpDisplay::displayText(_I, iP, ide, vpColor::red);
  }
}

void
vpMbtDistanceKltPoints::displayPrimitive(const vpImage<vpRGBa>& _I)
{
  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
  for( ; iter != curPoints.end(); iter++){
    int id(iter->first);
    vpImagePoint iP;
    iP.set_i(static_cast<double>(iter->second.get_i()));
    iP.set_j(static_cast<double>(iter->second.get_j()));

    vpDisplay::displayCross(_I, iP, 10, vpColor::red);

    iP.set_i( vpMath::round( iP.get_i() + 7 ) );
    iP.set_j( vpMath::round( iP.get_j() + 7 ) );
    char ide[10];
    sprintf(ide, "%ld", static_cast<long int>(id));
    vpDisplay::displayText(_I, iP, ide, vpColor::red);
  }
}

//###################################
//      Static functions
//###################################

bool
vpMbtDistanceKltPoints::intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double i_test, const double j_test, const double i, const double j)
{
  double dx = p2.get_j() - p1.get_j();
  double dy = p2.get_i() - p1.get_i();
  double ex = j - j_test;
  double ey = i - i_test;

  double den = dx * ey - dy * ex;
  double t = 0, u = 0;
  //if(den != 0){
  if(std::fabs(den) > std::fabs(den)*std::numeric_limits<double>::epsilon()){
    t = -( ey * ( p1.get_j() - j_test ) + ex * ( -p1.get_i() + i_test ) ) / den;
    u = -( dx * ( -p1.get_i() + i_test ) + dy * ( p1.get_j() - j_test ) ) / den;
  }
  else{
    throw vpException(vpException::divideByZeroError, "denominator null");
  }
  return ( t >= std::numeric_limits<double>::epsilon() && t < 1.0 && u >= std::numeric_limits<double>::epsilon() && u < 1.0);
}

bool
vpMbtDistanceKltPoints::isInside(const std::vector<vpImagePoint>& roi, const double i, const double j)
{
  double i_test = 100000.;
  double j_test = 100000.;
  unsigned int nbInter = 0;
  bool computeAgain = true;

  if(computeAgain){
    computeAgain = false;
    for(unsigned int k=0; k< roi.size(); k++){
      try{
        if(vpMbtDistanceKltPoints::intersect(roi[k], roi[(k+1)%roi.size()], i, j, i_test, j_test)){
          nbInter++;
        }
      }
      catch(...){
        computeAgain = true;
        break;
      }
    }

    if(computeAgain){
      i_test += 100;
      j_test -= 100;
      nbInter = 0;
    }
  }
  return ((nbInter%2) == 1);
}

#endif