vpTemplateTrackerTriangle.cpp

/****************************************************************************
 *
 * $Id: vpTemplateTrackerTriangle.cpp 4636 2014-02-04 08:17: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:
 * Template tracker.
 *
 * Authors:
 * Amaury Dame
 * Aurelien Yol
 * Fabien Spindler
 *
 *****************************************************************************/
#include <visp/vpTemplateTrackerTriangle.h>

vpTemplateTrackerTriangle::vpTemplateTrackerTriangle()
  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
    uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001)
{
}

vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpTemplateTrackerTriangle& T)
  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
    uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001)
{
  *this = T;
}

vpTemplateTrackerTriangle & vpTemplateTrackerTriangle::operator=(const vpTemplateTrackerTriangle& T)
{
  minx_temp=T.minx_temp;
  miny_temp=T.miny_temp;

  l_t=T.l_t;
  h_t=T.h_t;
  C1.x=T.C1.x;
  C1.y=T.C1.y;
  C2.x=T.C2.x;
  C2.y=T.C2.y;
  C3.x=T.C3.x;
  C3.y=T.C3.y;
  //uvinv.resize(2,2);
  //uvinv=T.uvinv;
  //p_ds_uv.resize(2);
  //p_ds_uv=T.p_ds_uv;
  //ptempo.resize(2);
  //ptempo=T.ptempo;
  not_good=T.not_good;

  uvinv00=T.uvinv00;
  uvinv01=T.uvinv01;
  uvinv10=T.uvinv10;
  uvinv11=T.uvinv11;

  marge_triangle = T.marge_triangle;

  return (*this);
}

vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpColVector &c1,
                                                     const vpColVector &c2,
                                                     const vpColVector &c3)
  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
    uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001)
{
  init(c1[0],c1[1],c2[0],c2[1],c3[0],c3[1]);
}
vpTemplateTrackerTriangle vpTemplateTrackerTriangle::getPyramidDown() const
{
  vpTemplateTrackerTriangle Ttemp;
  Ttemp.init(C1.x/2.,C1.y/2.,C2.x/2.,C2.y/2.,C3.x/2.,C3.y/2.);
  return Ttemp;
}

vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(int x1,int y1, int x2,int y2, int x3,int y3)
  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
    uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001)
{
  init(x1,y1,x2,y2,x3,y3);
}

vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3)
  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
    uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001)
{
  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
}

vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1,double y1, double x2,double y2, double x3,double y3)
  : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
    uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001)
{
  init(x1,y1,x2,y2,x3,y3);
}
void vpTemplateTrackerTriangle::init(const vpColVector &c1,const vpColVector &c2,const vpColVector &c3)
{
  init(c1[0],c1[1],c2[0],c2[1],c3[0],c3[1]);
}
void vpTemplateTrackerTriangle::init(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3)
{
  init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
}

void vpTemplateTrackerTriangle::init(int x1, int y1, int x2,int y2, int x3, int y3)
{
  init((double)x1,(double)y1,(double)x2,(double)y2,(double)x3,(double)y3);
}

void vpTemplateTrackerTriangle::init(double x1, double y1, double x2,double y2, double x3, double y3)
{
  C1.x=x1;C1.y=y1;
  C2.x=x2;C2.y=y2;
  C3.x=x3;C3.y=y3;

  double minx,miny,maxx,maxy;
  //calcul du rectangle minimal contenant le triangle seletionn�
  minx=(x1<x2)?x1:x2;
  miny=(y1<y2)?y1:y2;
  minx=(minx<x3)?minx:x3;
  miny=(miny<y3)?miny:y3;
  maxx=(x1>x2)?x1:x2;
  maxy=(y1>y2)?y1:y2;
  maxx=(maxx>x3)?maxx:x3;
  maxy=(maxy>y3)?maxy:y3;

  vpColVector u;
  vpColVector v;
  u.resize(2);
  v.resize(2);
  vpMatrix uv(2,2);
  vpMatrix uvinv(2,2);

  u[0]=C2.x-C1.x;
  u[1]=C2.y-C1.y;

  v[0]=C3.x-C1.x;
  v[1]=C3.y-C1.y;

  uv[0][0]=u[0];uv[1][0]=v[0];
  uv[0][1]=u[1];uv[1][1]=v[1];
  try
  {
    uvinv=uv.inverseByLU();
    not_good=false;
  }
  catch(...)
  {
    not_good=true;
    std::cout<<"Triangle vide"<<std::endl;

  }
  uvinv00=uvinv[0][0];
  uvinv01=uvinv[0][1];
  uvinv10=uvinv[1][0];
  uvinv11=uvinv[1][1];

  l_t=maxx-minx;
  h_t=maxy-miny;
  minx_temp=minx;
  miny_temp=miny;

  marge_triangle = 0.00001;
}

//marge ajoutee a zone pour que sommet soit pris en compte

bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
{
  if(not_good)
    return false;

  /*ptempo[0]=j-C1.x;
  ptempo[1]=i-C1.y;

  p_ds_uv=ptempo*uvinv;
  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/

  double ptempo0=j-C1.x;
  double ptempo1=i-C1.y;
  double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
  double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
  return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
}

bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const
{
  if(not_good)
    return false;
  /*ptempo[0]=j-C1.x;
  ptempo[1]=i-C1.y;

  p_ds_uv=ptempo*uvinv;
  return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
  double ptempo0=j-C1.x;
  double ptempo1=i-C1.y;
  double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
  double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
  return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
}

bool vpTemplateTrackerTriangle::inTriangle(const vpImagePoint &ip) const
{
  return inTriangle(ip.get_i(), ip.get_j());
}
void vpTemplateTrackerTriangle::getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const
{
  c1.set_uv(C1.x, C1.y);
  c2.set_uv(C2.x, C2.y);
  c3.set_uv(C3.x, C3.y);
}

void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
{
  c.resize(3);
  c[0].set_uv(C1.x, C1.y);
  c[1].set_uv(C2.x, C2.y);
  c[2].set_uv(C3.x, C3.y);
}

void vpTemplateTrackerTriangle::getCorners(vpColVector &c1,vpColVector &c2,vpColVector &c3) const
{
  c1=getCorner1();
  c2=getCorner2();
  c3=getCorner3();
}

vpColVector vpTemplateTrackerTriangle::getCorner1() const
{
  vpColVector c(2);
  c[0]=C1.x;
  c[1]=C1.y;

  return c;
}
vpColVector vpTemplateTrackerTriangle::getCorner2() const
{
  vpColVector c(2);
  c[0]=C2.x;
  c[1]=C2.y;
  return c;
}

vpColVector vpTemplateTrackerTriangle::getCorner3() const
{
  vpColVector c(2);
  c[0]=C3.x;
  c[1]=C3.y;
  return c;
}

void vpTemplateTrackerTriangle::getSize(double &w,double &h) const
{
  w=l_t;
  h=h_t;
}
void vpTemplateTrackerTriangle::getSize(int &w,int &h) const
{
  w=(int)l_t+1;
  h=(int)h_t+1;
}

double vpTemplateTrackerTriangle::getMinx() const
{
  return minx_temp-1;
}
double vpTemplateTrackerTriangle::getMiny() const
{
  return miny_temp-1;
}
double vpTemplateTrackerTriangle::getMaxx() const
{
  return minx_temp+l_t+1;
}
double vpTemplateTrackerTriangle::getMaxy() const
{
  return miny_temp+h_t+1;
}