vpFeatureBuilderSegment.cpp

/****************************************************************************
 *
 * $Id: vpFeatureBuilderLine.cpp 3530 2012-01-03 10:52:12Z 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:
 * Segment creation out of dots.
 *
 * Authors:
 * Filip Novotny
 * Fabien Spindler
 *
 *****************************************************************************/


#include <visp/vpFeatureBuilder.h>
#include <visp/vpMath.h>


void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
                              const vpDot &d1, const vpDot &d2 )
{
  double x1=0, y1=0, x2=0, y2=0;

  vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
  vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;

  double xc = (x1+x2)/2.;
  double yc = (y1+y2)/2.;
  double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));

  if (s.isNormalized())
  {
    s.setXc( xc/l );
    s.setYc( yc/l );
    s.setL (  1/l );
  }
  else
  {
    s.setXc( xc );
    s.setYc( yc );
    s.setL ( l  );
  }

  s.setAlpha(atan2(y1-y2,x1-x2));
}

void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
                              const vpDot2 &d1, const vpDot2 &d2)
{
  double x1=0, y1=0, x2=0, y2=0;

  vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
  vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;

  double xc = (x1+x2)/2.;
  double yc = (y1+y2)/2.;
  double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));

  if (s.isNormalized())
  {
    s.setXc( xc/l );
    s.setYc( yc/l );
    s.setL (  1/l );
  }
  else
  {
    s.setXc( xc );
    s.setYc( yc );
    s.setL ( l  );
  }

  s.setAlpha(atan2(y1-y2,x1-x2));
}

void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
                              const vpImagePoint &ip1, const vpImagePoint &ip2)
{
  double x1=0, y1=0, x2=0, y2=0;

  vpPixelMeterConversion::convertPoint(cam, ip1, x1, y1) ;
  vpPixelMeterConversion::convertPoint(cam, ip2, x2, y2) ;

  double xc = (x1+x2)/2.;
  double yc = (y1+y2)/2.;
  double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));

  if (s.isNormalized())
  {
    s.setXc( xc/l );
    s.setYc( yc/l );
    s.setL (  1/l );
  }
  else
  {
    s.setXc( xc );
    s.setYc( yc );
    s.setL ( l  );
  }

  s.setAlpha(atan2(y1-y2,x1-x2));
}

void vpFeatureBuilder::create(vpFeatureSegment &s, vpPoint& P1, vpPoint& P2)
{
  double x1 = P1.get_x();
  double y1 = P1.get_y();
  double x2 = P2.get_x();
  double y2 = P2.get_y();

  double Z1 = P1.cP[2]/P1.cP[3];
  double Z2 = P2.cP[2]/P2.cP[3];

  s.buildFrom(x1, y1, Z1, x2, y2, Z2);
}