vpFeatureBuilderVanishingPoint.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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:
 * Conversion between tracker and visual feature vanishing point.
 *
 * Authors:
 * Odile Bourquardez
 *
 *****************************************************************************/

#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureException.h>

void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &p, unsigned int select)
{
  if ((vpFeatureVanishingPoint::selectX() & select) || (vpFeatureVanishingPoint::selectY() & select)) {
    s.set_x(p.get_x());
    s.set_y(p.get_y());
  }
  else if((vpFeatureVanishingPoint::selectOneOverRho() & select) || (vpFeatureVanishingPoint::selectAlpha() & select)) {
    double x = p.get_x();
    double y = p.get_y();

    s.setOneOverRho( 1. / sqrt(x * x  + y * y) );
    s.setAlpha( atan2(y, x) );
  }
}

void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpFeatureLine &L1, const vpFeatureLine &L2, unsigned int select)
{
  if ((vpFeatureVanishingPoint::selectX() & select) || (vpFeatureVanishingPoint::selectY() & select)) {
    double rho_l = L1.getRho();
    double rho_r = L2.getRho();
    double theta_l = L1.getTheta();
    double theta_r = L2.getTheta();
    double c_l = cos(theta_l);
    double c_r = cos(theta_r);
    double s_l = sin(theta_l);
    double s_r = sin(theta_r);

    double min = 0.0001;
    if (fabs(theta_r - theta_l) < min || fabs(fabs(theta_r - theta_l) - M_PI) < min ||
        fabs(fabs(theta_r - theta_l) - 2 * M_PI) < min) {
      vpCERROR << "There is no vanishing point : the lines are parallel in the "
                  "image plane"
               << std::endl;
      throw(vpFeatureException(vpFeatureException::badInitializationError, "There is no vanishing point : the lines are "
                                                                           "parallel in the image plane"));
    }

    double y = (rho_r * c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l);
    double x = (rho_r * s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l);

    s.set_x(x);
    s.set_y(y);
  }
  else if((vpFeatureVanishingPoint::selectOneOverRho() & select) || (vpFeatureVanishingPoint::selectAlpha() & select)) {
    double rho_1   = L1.getRho();
    double theta_1 = L1.getTheta();
    double rho_2   = L2.getRho();
    double theta_2 = L2.getTheta();

    double theta_diff = theta_1 - theta_2;

    double denom = sqrt( rho_1*rho_1 + rho_2*rho_2 - 2*rho_1*rho_2 * cos(theta_diff) );
    double one_over_rho = sin(theta_diff) / denom;
    double alpha = atan2( rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2*sin(theta_1) - rho_1 * sin(theta_2));

    s.setOneOverRho(one_over_rho);
    s.setAlpha(alpha);
  }
}

void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpLine &L1, const vpLine &L2, unsigned int select)
{
  vpFeatureLine l1, l2;
  vpFeatureBuilder::create(l1, L1);
  vpFeatureBuilder::create(l2, L2);

  vpFeatureBuilder::create(s, l1, l2, select);
}

void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpCameraParameters &cam,
                              const vpImagePoint &line1_ip1, const vpImagePoint &line1_ip2,
                              const vpImagePoint &line2_ip1, const vpImagePoint &line2_ip2,
                              unsigned int select)
{
  double x1 = 0, y1 = 0;
  double x2 = 0, y2 = 0;

  // First line
  vpPixelMeterConversion::convertPoint(cam, line1_ip1, x1, y1);
  vpPixelMeterConversion::convertPoint(cam, line1_ip2, x2, y2);

  double theta_1 = atan2(-(x1-x2), y1 - y2);
  double rho_1 = ( (x1+x2)*cos(theta_1) + (y1+y2)*sin(theta_1) ) / 2.;

  // Second line
  vpPixelMeterConversion::convertPoint(cam, line2_ip1, x1, y1);
  vpPixelMeterConversion::convertPoint(cam, line2_ip2, x2, y2);

  double theta_2 = atan2(-(x1-x2), y1 - y2);
  double rho_2 = ( (x1+x2)*cos(theta_2) + (y1+y2)*sin(theta_2) ) / 2.;

  if ((vpFeatureVanishingPoint::selectX() & select) || (vpFeatureVanishingPoint::selectY() & select)) {
    double min = 0.0001;
    double theta_diff = theta_1 - theta_2;

    if (fabs(theta_diff) < min || fabs(fabs(theta_diff) - M_PI) < min || fabs(fabs(theta_diff) - 2 * M_PI) < min) {
      throw(vpException(vpException::fatalError, "There is no vanishing point : the lines are parallel in the image plane"));
    }

    double x = (sin(theta_1) * rho_2 - sin(theta_2)*rho_1 ) / sin(theta_diff);
    double y = (cos(theta_2) * rho_1 - cos(theta_1)*rho_2 ) / sin(theta_diff);

    s.set_x(x);
    s.set_y(y);
  }
  else if((vpFeatureVanishingPoint::selectOneOverRho() & select)) {
    double theta_diff = theta_1 - theta_2;
    double denom = sqrt( rho_1*rho_1 + rho_2*rho_2 - 2*rho_1*rho_2 * cos(theta_diff) );
    double one_over_rho = sin(theta_diff) / denom;
//    if (one_over_rho < 0) {
//      one_over_rho = -one_over_rho;
//    }
    double alpha = atan2( rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2*sin(theta_1) - rho_1 * sin(theta_2));

    s.setOneOverRho(one_over_rho);
    s.setAlpha(alpha);
  }
  else if((vpFeatureVanishingPoint::selectAtanOneOverRho() & select)) {
    double theta_diff = theta_1 - theta_2;
    double denom = sqrt( rho_1*rho_1 + rho_2*rho_2 - 2*rho_1*rho_2 * cos(theta_diff) );
    double alpha = atan2( rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2*sin(theta_1) - rho_1 * sin(theta_2));
    double one_over_rho = sin(theta_diff) / denom;
    double atan_one_over_rho = atan2(sin(theta_diff), denom);

//    if (one_over_rho < 0) {
//      one_over_rho = -one_over_rho;
//    }

    s.setOneOverRho(one_over_rho);
    s.setAtanOneOverRho(atan_one_over_rho);
    s.setAlpha(alpha);
  }
}