vpFeatureSegment.cpp

/****************************************************************************
 *
 * $Id: vpFeatureThetaU.cpp 3530 2012-01-03 10:52:12Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 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 visual feature.
 *
 * Authors:
 * Filip Novotny
 * Fabien Spindler
 *
 *****************************************************************************/


#include <visp/vpBasicFeature.h>
#include <visp/vpFeatureSegment.h>
#include <visp/vpImagePoint.h>
#include <visp/vpMeterPixelConversion.h>
#include <visp/vpMath.h>
#include <visp/vpDisplay.h>
#include <cmath>

// Exception
#include <visp/vpException.h>
#include <visp/vpMatrixException.h>

// Debug trace
#include <visp/vpDebug.h>


void
vpFeatureSegment::init()
{
  //feature dimension
  dim_s = 4 ;
  nbParameters = 6;

  // memory allocation
  s.resize(dim_s) ;
  if (flags == NULL)
    flags = new bool[nbParameters];
  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
}

vpFeatureSegment::vpFeatureSegment(bool normalized):
  vpBasicFeature()
{
  this->normalized_ = normalized;

  init();
}

vpMatrix
vpFeatureSegment::interaction( const unsigned int select )
{

  vpMatrix L ;
  L.resize(0,6) ;

  if (deallocate == vpBasicFeature::user)  
  {
    for (unsigned int i = 0; i < nbParameters; i++)
    {
      if (flags[i] == false)
      {
        switch(i){
        case 0:
          vpTRACE("Warning !!!  The interaction matrix is computed but xc was not set yet");
        break;
        case 1:
          vpTRACE("Warning !!!  The interaction matrix is computed but Yc was not set yet");
        break;
        case 2:
          vpTRACE("Warning !!!  The interaction matrix is computed but l was not set yet");
        break;
        case 3:
          vpTRACE("Warning !!!  The interaction matrix is computed but alpha was not set yet");
        break;
        case 4:
          vpTRACE("Warning !!!  The interaction matrix is computed but Z1 was not set yet");
        break;
        case 5:
          vpTRACE("Warning !!!  The interaction matrix is computed but Z2 was not set yet");
        break;
        default:
          vpTRACE("Problem during the reading of the variable flags");
        }
      }
    }
  }

  //This version is a simplification
  double lambda1 = (Z1_-Z2_)/(Z1_*Z2_); // -l * lambda
  double lambda2 = (Z1_+Z2_)/(2*Z1_*Z2_); // 1/Zm
  
  if (normalized_)
  {
    // here var xc_ contains xc/l, yc contains yc/l and l contains 1/l
    double xn = xc_;
    double yn = yc_;
    double ln = l_;
    double lambda = -lambda1 * ln;
    double Zn_inv = lambda2 * ln;
    double lc = cos_a_ / ln;
    double ls = sin_a_ / ln;
    double xnalpha = xn*cos_a_+yn*sin_a_;
    double lnc = cos_a_ *  ln;
    double lns = sin_a_ *  ln;

    if (vpFeatureSegment::selectXc() & select ){
      vpMatrix Lxn(1,6);
      Lxn[0][0] = -Zn_inv +  lambda * xn * cos_a_;
      Lxn[0][1] = lambda * xn * sin_a_ ;
      Lxn[0][2] = lambda1 * (xn*xnalpha - cos_a_ /4.);
      Lxn[0][3] = sin_a_*cos_a_/4/ln - xn*xnalpha*sin_a_/ln;
      Lxn[0][4] = -ln*(1.+lc*lc/4.) + xn*xnalpha*cos_a_/ln ;
      Lxn[0][5] = yn ;
      L = vpMatrix::stackMatrices(L, Lxn) ;
    }

    if (vpFeatureSegment::selectYc() & select ){
      vpMatrix Lyn(1,6);
      Lyn[0][0] = lambda*yn*cos_a_ ;
      Lyn[0][1] = -Zn_inv + lambda*yn*sin_a_ ;
      Lyn[0][2] = lambda1 * (yn*xnalpha - sin_a_/4.);
      Lyn[0][3] = ln*(1+ls*ls/4.)-yn*xnalpha*sin_a_/ln ;
      Lyn[0][4] = -sin_a_*cos_a_/4/ln + yn*xnalpha*cos_a_/ln;
      Lyn[0][5] = -xn ;
      L = vpMatrix::stackMatrices(L, Lyn) ;
    }

    if (vpFeatureSegment::selectL() & select ){
      vpMatrix Lln(1,6);
      Lln[0][0] = lambda * lnc ;
      Lln[0][1] = lambda * lns ;
      Lln[0][2] = -(Zn_inv + lambda*xnalpha);
      Lln[0][3] = -yn-xnalpha*sin_a_ ;
      Lln[0][4] = xn + xnalpha*cos_a_ ;
      Lln[0][5] = 0 ;
      L = vpMatrix::stackMatrices(L, Lln) ;
    }

  }
  else
  {
    if (vpFeatureSegment::selectXc() & select ){
      vpMatrix Lxc(1,6);
      Lxc[0][0] = -lambda2 ;
      Lxc[0][1] = 0. ;
      Lxc[0][2] = lambda2*xc_ - lambda1*l_*cos_a_/4.;
      //Lxc[0][3] = xc_*xc_ + l_*l_*cos_a_*sin_a_/4. ; // orig bug
      Lxc[0][3] = xc_*yc_ + l_*l_*cos_a_*sin_a_/4. ;
      Lxc[0][4] = -(1+xc_*xc_+l_*l_*cos_a_*cos_a_/4.) ;
      Lxc[0][5] = yc_ ;
      L = vpMatrix::stackMatrices(L,Lxc) ;
    }

    if (vpFeatureSegment::selectYc() & select ){
      vpMatrix Lyc(1,6);
      Lyc[0][0] = 0. ;
      Lyc[0][1] = -lambda2 ;
      Lyc[0][2] = lambda2*yc_ - lambda1*l_*sin_a_/4.;
      Lyc[0][3] = 1+yc_*yc_+l_*l_*sin_a_*sin_a_/4. ;
      Lyc[0][4] = -xc_*yc_-l_*l_*cos_a_*sin_a_/4. ;
      Lyc[0][5] = -xc_ ;
      L = vpMatrix::stackMatrices(L,Lyc) ;
    }

    if (vpFeatureSegment::selectL() & select ){
      vpMatrix Ll(1,6);
      Ll[0][0] = lambda1*cos_a_ ;
      Ll[0][1] = lambda1*sin_a_ ;
      Ll[0][2] = lambda2*l_-lambda1*(xc_*cos_a_+yc_*sin_a_);
      Ll[0][3] = l_*(xc_*cos_a_*sin_a_ + yc_*(1+sin_a_*sin_a_)) ;
      Ll[0][4] = -l_*(xc_*(1+cos_a_*cos_a_)+yc_*cos_a_*sin_a_) ;
      Ll[0][5] = 0 ;
      L = vpMatrix::stackMatrices(L,Ll) ;
    }
  }
  if (vpFeatureSegment::selectAlpha() & select ){
    vpMatrix Lalpha(1,6);
      Lalpha[0][0] = -lambda1*sin_a_/l_ ;
      Lalpha[0][1] = lambda1*cos_a_/l_ ;
      Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_)/l_;
      Lalpha[0][3] = -xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_;
      Lalpha[0][4] = xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_ ;
      Lalpha[0][5] = -1 ;
    L = vpMatrix::stackMatrices(L,Lalpha) ;
  }

  return L ;
}

vpColVector
vpFeatureSegment::error( const vpBasicFeature &s_star, const unsigned int select )
{ 
  vpColVector e(0) ;

  if (vpFeatureSegment::selectXc() & select ){
    vpColVector exc(1) ;
    exc[0] = xc_-s_star[0];
    e = vpMatrix::stackMatrices(e,exc) ;
  }

  if (vpFeatureSegment::selectYc() & select ){
    vpColVector eyc(1) ;
    eyc[0] = yc_ - s_star[1];
    e = vpMatrix::stackMatrices(e,eyc) ;
  }

  if (vpFeatureSegment::selectL() & select ){
    vpColVector eL(1) ;
    eL[0] = l_ - s_star[2];
    e = vpMatrix::stackMatrices(e,eL) ;
  }

  if (vpFeatureSegment::selectAlpha() & select ){
    vpColVector eAlpha(1) ;
    eAlpha[0] = alpha_ - s_star[3];
    while (eAlpha[0] < -M_PI) eAlpha[0] += 2*M_PI ;
    while (eAlpha[0] > M_PI) eAlpha[0] -= 2*M_PI ;
    e = vpMatrix::stackMatrices(e,eAlpha) ;
  }
  return e ;
}

void
vpFeatureSegment::print( const unsigned int select ) const
{
  std::cout <<"vpFeatureSegment: (";
  if (vpFeatureSegment::selectXc() & select ) {
    if (normalized_)
      std::cout << "xn = ";
    else
      std::cout << "xc = ";
    std::cout << s[0] << "; ";
  }
  if (vpFeatureSegment::selectYc() & select ) {
    if (normalized_)
      std::cout << "yn = ";
    else
      std::cout << "yc = ";
    std::cout << s[1] << "; ";
  }
  if (vpFeatureSegment::selectL() & select ) {
    if (normalized_)
      std::cout << "ln = ";
    else
      std::cout << "l = ";
    std::cout << s[2] << "; ";
  }
  if (vpFeatureSegment::selectAlpha() & select ) {
    std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
  }
  std::cout << ")" << std::endl;
}

vpFeatureSegment *vpFeatureSegment::duplicate() const
{
  vpFeatureSegment *feature;
  
  feature = new vpFeatureSegment(*this) ;    
  return feature ;
}

void
vpFeatureSegment::display(const vpCameraParameters &cam ,
                          const vpImage<unsigned char> & I,
                          const vpColor &color,
                          unsigned int  thickness ) const
{
  double l, x, y;
  if (normalized_) {
    l = 1./l_;
    x = xc_ * l;
    y = yc_ * l;
  }
  else {
    l = l_;
    x = xc_;
    y = yc_;
  }

  double x1 = x - (l/2.)*cos_a_;
  double x2 = x + (l/2.)*cos_a_;

  double y1 = y - (l/2.)*sin_a_;
  double y2 = y + (l/2.)*sin_a_;
  vpImagePoint ip1,ip2;

  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
  vpDisplay::displayCircle(I, ip1, 5, color, true);
  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
}

void
vpFeatureSegment::display(const vpCameraParameters & cam ,
                          const vpImage<vpRGBa> & I,
                          const vpColor &color,
                          unsigned int thickness ) const
{
  double l, x, y;
  if (normalized_) {
    l = 1./l_;
    x = xc_ * l;
    y = yc_ * l;
  }
  else {
    l = l_;
    x = xc_;
    y = yc_;
  }

  double x1 = x - (l/2.)*cos_a_;
  double x2 = x + (l/2.)*cos_a_;

  double y1 = y - (l/2.)*sin_a_;
  double y2 = y + (l/2.)*sin_a_;
  vpImagePoint ip1,ip2;

  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
  vpDisplay::displayCircle(I, ip1, 5,vpColor::cyan, true);
  vpDisplay::displayCircle(I, ip2, 5,vpColor::yellow, true);
} 

void vpFeatureSegment::buildFrom(const double x1, const double y1, const double Z1,
                                 const double x2, const double y2, const double Z2)
{
  double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
  double x_c = (x1+x2)/2.;
  double y_c = (y1+y2)/2.;
  double alpha = atan2(y1-y2,x1-x2);

  if (normalized_) {
    setXc( x_c / l );
    setYc( y_c / l );
    setL( 1/l );
    setAlpha( alpha );

    setZ1(Z1);
    setZ2(Z2);
  }
  else {
    setXc( x_c );
    setYc( y_c );
    setL( l );
    setAlpha( alpha );

    setZ1(Z1);
    setZ2(Z2);
  }
}