vpMbtPolygon.cpp

/****************************************************************************
 *
 * $Id: vpMbtPolygon.cpp 4056 2013-01-05 13:04:42Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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:
 * Make the complete tracking of an object by using its CAD model
 *
 * Authors:
 * Nicolas Melchior
 * Romain Tallonneau
 * Eric Marchand
 * Aurelien Yol
 *
 *****************************************************************************/

#include <limits.h>

#include <visp/vpConfig.h>
#include <visp/vpMbtPolygon.h>

vpMbtPolygon::vpMbtPolygon()
{
  nbpt = 0 ;
  p = NULL ;
  isappearing = false;
  isvisible = false;
}

vpMbtPolygon::~vpMbtPolygon()
{
  if (p !=NULL)
  {
    delete[] p;
    p = NULL;
  }
}

vpPoint &
vpMbtPolygon::getPoint(const unsigned int _index)
{
  if(_index >= nbpt){
    throw vpException(vpException::dimensionError, "index out of range");
  }
  return p[_index];
}

void
vpMbtPolygon::setNbPoint(const unsigned int nb)
{
  nbpt = nb ;
  if (p != NULL)
    delete[] p;
  p = new vpPoint[nb] ;
}

void
vpMbtPolygon::addPoint(const unsigned int n, const vpPoint &P)
{
  //if( p!NULL && n < nbpt )
    p[n] = P ;
}

void
vpMbtPolygon::changeFrame(const vpHomogeneousMatrix &cMo)
{
  for (unsigned int i = 0 ; i < nbpt ; i++)
  {
    p[i].changeFrame(cMo) ;
    p[i].projection() ;
  }
}

bool
vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const bool &depthTest)
{
  changeFrame(cMo) ;
  
  if(depthTest)
    for (unsigned int i = 0 ; i < nbpt ; i++){
      if(p[i].get_Z() < 0){
        isappearing = false;
        isvisible = false ;
        return false ;
      }
    }
  
  if(nbpt <= 2){
    /* a line is allways visible */
    isvisible = true;
    isappearing = false;
    return  true ;
  }

  vpColVector e1(3) ;
  vpColVector e2(3) ;
  vpColVector facenormal(3) ;

  e1[0] = p[1].get_X() - p[0].get_X() ;
  e1[1] = p[1].get_Y() - p[0].get_Y() ;
  e1[2] = p[1].get_Z() - p[0].get_Z() ;

  e2[0] = p[2].get_X() - p[1].get_X() ;
  e2[1] = p[2].get_Y() - p[1].get_Y() ;
  e2[2] = p[2].get_Z() - p[1].get_Z() ;

  facenormal = vpColVector::crossProd(e1,e2) ;

  double angle = p[0].get_X()*facenormal[0] +  p[0].get_Y()*facenormal[1]  +  p[0].get_Z()*facenormal[2]  ;
  
  if(angle < -0.0001 )
  {
    isvisible = true;
    isappearing = false;
    
    return  true ;
  }
  else
  {
    if (angle < 0.0000001 ){
      isappearing = true;
    }
    else {
      isappearing = false;
    }
    isvisible = false ;
    return false ;
  }
}

bool 
vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha)
{
  //   std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl;
  if(nbpt <= 2){
    /* a line is allways visible */
    isvisible = true;
    isappearing = false;
    return  true ;
  }

  changeFrame(cMo);

  vpColVector e1(3) ;
  vpColVector e2(3) ;
  vpColVector facenormal(3) ;

  e1[0] = p[1].get_X() - p[0].get_X() ;
  e1[1] = p[1].get_Y() - p[0].get_Y() ;
  e1[2] = p[1].get_Z() - p[0].get_Z() ;

  e2[0] = p[2].get_X() - p[1].get_X() ;
  e2[1] = p[2].get_Y() - p[1].get_Y() ;
  e2[2] = p[2].get_Z() - p[1].get_Z() ;
 
  e1.normalize();
  e2.normalize();
  
  facenormal = vpColVector::crossProd(e1,e2) ;
  facenormal.normalize();
 
  vpColVector e4(3) ;
  vpPoint pt;
  for (unsigned int i = 0; i < nbpt; i += 1){
    pt.set_X(pt.get_X() + p[i].get_X());
    pt.set_Y(pt.get_Y() + p[i].get_Y());
    pt.set_Z(pt.get_Z() + p[i].get_Z());
  }
  e4[0] = -pt.get_X()/(double)nbpt; e4[1] = -pt.get_Y()/(double)nbpt; e4[2] = -pt.get_Z()/(double)nbpt; 
  e4.normalize();
  
  double angle2 = vpColVector::dotProd (e4, facenormal);
  double my_angle = acos(angle2);
  
//   std::cout << angle2 << "/" << vpMath::deg(my_angle) << std::endl;
  
  if( my_angle < alpha ){
    isvisible = true;
    isappearing = false;
    return true;
  }
  
  isvisible = false;
  isappearing = false;
  return false;
}

std::vector<vpImagePoint> 
vpMbtPolygon::getRoi(const vpCameraParameters &_cam)
{
  std::vector<vpImagePoint> roi;
  for (unsigned int i = 0; i < nbpt; i ++){
    vpImagePoint ip;
    vpMeterPixelConversion::convertPoint(_cam, p[i].get_x(), p[i].get_y(), ip);
    roi.push_back(ip);
  }
  return roi;
}

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

void                
vpMbtPolygon::getMinMaxRoi(const std::vector<vpImagePoint> &roi, int & i_min, int &i_max, int &j_min, int &j_max)
{
  // i_min_d = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
  double i_min_d = (double) INT_MAX;
  double i_max_d = 0;
  double j_min_d = (double) INT_MAX;
  double j_max_d = 0;

  for (unsigned int i = 0; i < roi.size(); i += 1){
    if(i_min_d > roi[i].get_i())
      i_min_d = roi[i].get_i();
    
    if(roi[i].get_i() < 0)
      i_min_d = 1;
    
    if((roi[i].get_i() > 0) && (i_max_d < roi[i].get_i()))
      i_max_d = roi[i].get_i();
    
    if(j_min_d > roi[i].get_j())
      j_min_d = roi[i].get_j();
    
    if(roi[i].get_j() < 0)
      j_min_d = 1;//border
      
    if((roi[i].get_j() > 0) && j_max_d < roi[i].get_j())
      j_max_d = roi[i].get_j();
  }
  i_min = static_cast<int> (i_min_d);
  i_max = static_cast<int> (i_max_d);
  j_min = static_cast<int> (j_min_d);
  j_max = static_cast<int> (j_max_d);
}

bool
vpMbtPolygon::roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners)
{
  for(unsigned int i=0; i<corners.size(); ++i){
    if((corners[i].get_i() < 0) || (corners[i].get_j() < 0) ||
       (corners[i].get_i() > I.getHeight()) || (corners[i].get_j() > I.getWidth())){
      return false;
    }
  }
  return true;
}