vpMbScanLine.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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://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:
 * Make the complete tracking of an object by using its CAD model
 *
 * Authors:
 * Aurelien Yol
 *
 *****************************************************************************/

#include <visp3/core/vpConfig.h>

#if defined _MSC_VER && _MSC_VER >= 1200
#  define NOMINMAX
#endif

#include <cmath>
#include <algorithm>
#include <cstddef>
#include <iostream>
#include <utility>

#include <visp3/mbt/vpMbScanLine.h>
#include <visp3/core/vpMeterPixelConversion.h>

#if defined(DEBUG_DISP)
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#endif


#ifndef DOXYGEN_SHOULD_SKIP_THIS

vpMbScanLine::vpMbScanLine()
  : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(),
    visibility_samples(), depthTreshold(1e-06)
#if defined(DEBUG_DISP)
  ,dispMaskDebug(NULL), dispLineDebug(NULL), linedebugImg()
#endif
{
#if defined(VISP_HAVE_X11) && defined(DEBUG_DISP)
    dispLineDebug = new vpDisplayX();
    dispMaskDebug = new vpDisplayX();
#elif defined(VISP_HAVE_GDI) && defined(DEBUG_DISP)
    dispLineDebug = new vpDisplayGDI();
    dispMaskDebug = new vpDisplayGDI();
#endif
}

vpMbScanLine::~vpMbScanLine()
{
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
  if (dispLineDebug != NULL) delete dispLineDebug;
  if (dispMaskDebug != NULL) delete dispMaskDebug;
#endif
}
void vpMbScanLine::drawLineY(const vpColVector &a,
               const vpColVector &b,
               const vpMbScanLineEdge &edge,
               const int ID,
               std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
  double x0 = a[0] / a[2];
  double y0 = a[1] / a[2];
  double z0 = a[2];
  double x1 = b[0] / b[2];
  double y1 = b[1] / b[2];
  double z1 = b[2];
  if (y0 > y1)
  {
    std::swap(x0, x1);
    std::swap(y0, y1);
    std::swap(z0, z1);
  }

  //if (y0 >= h - 1 || y1 < 0 || y1 == y0)
  if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits<double>::epsilon())
      return;

  const unsigned int _y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(y0)));
  const double _y1 = std::min<double>(h, y1);

  const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));

  for(unsigned int y = _y0 ; y < _y1 ; ++y)
  {
      const double x = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
      const double alpha = getAlpha(y, y0 * z0, z0, y1 * z1, z1);
      vpMbScanLineSegment s;
      s.p = x;
      s.type = POINT;
      s.Z2 = s.Z1 = mix(z0, z1, alpha);
      s.P2 = s.P1 = s.p * s.Z1;
      s.ID = ID;
      s.edge = edge;
      s.b_sample_Y = b_sample_Y;
      scanlines[y].push_back(s);
  }
}

void vpMbScanLine::drawLineX(const vpColVector &a,
               const vpColVector &b,
               const vpMbScanLineEdge &edge,
               const int ID,
               std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
  double x0 = a[0] / a[2];
  double y0 = a[1] / a[2];
  double z0 = a[2];
  double x1 = b[0] / b[2];
  double y1 = b[1] / b[2];
  double z1 = b[2];
  if (x0 > x1)
  {
    std::swap(x0, x1);
    std::swap(y0, y1);
    std::swap(z0, z1);
  }

  //if (x0 >= w - 1 || x1 < 0 || x1 == x0)
  if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits<double>::epsilon())
      return;

  const unsigned int _x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(x0)));
  const double _x1 = std::min<double>(w, x1);

  const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1));

  for(unsigned int x = _x0 ; x < _x1 ; ++x)
  {
      const double y = y0 + (y1 - y0) * (x - x0) / (x1 - x0);
      const double alpha = getAlpha(x, x0 * z0, z0, x1 * z1, z1);
      vpMbScanLineSegment s;
      s.p = y;
      s.type = POINT;
      s.Z2 = s.Z1 = mix(z0, z1, alpha);
      s.P2 = s.P1 = s.p * s.Z1;
      s.ID = ID;
      s.edge = edge;
      s.b_sample_Y = b_sample_Y;
      scanlines[x].push_back(s);
  }
}


void
vpMbScanLine::drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
                  const int ID,
                  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
  if (polygon.size() < 2)
    return;

  if (polygon.size() == 2)
  {
    vpColVector p1, p2;
    createVectorFromPoint(polygon.front().first, p1, K);
    createVectorFromPoint(polygon.back().first, p2, K);

    drawLineY(p1,
              p2,
              makeMbScanLineEdge(polygon.front().first, polygon.back().first),
              ID,
              scanlines);
    return;
  }

  std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
  local_scanlines.resize(h);

  for(size_t i = 0 ; i < polygon.size() ; ++i)
  {
    vpColVector p1, p2;
    createVectorFromPoint(polygon[i].first, p1, K);
    createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);

    drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
  }

  createScanLinesFromLocals(scanlines,local_scanlines,h);
}

void
vpMbScanLine::drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
                  const int ID,
                  std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
  if (polygon.size() < 2)
      return;

  if (polygon.size() == 2)
  {
    vpColVector p1, p2;
    createVectorFromPoint(polygon.front().first, p1, K);
    createVectorFromPoint(polygon.back().first, p2, K);

    drawLineX(p1,
              p2,
              makeMbScanLineEdge(polygon.front().first, polygon.back().first),
              ID,
              scanlines);
    return;
  }

  std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
  local_scanlines.resize(w);

  for(size_t i = 0 ; i < polygon.size() ; ++i)
  {
    vpColVector p1, p2;
    createVectorFromPoint(polygon[i].first, p1, K);
    createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);

    drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
  }

  createScanLinesFromLocals(scanlines,local_scanlines,w);
}

void
vpMbScanLine::createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
                                        std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
                                        const unsigned int &size)
{
  for(unsigned int j = 0 ; j < size ; ++j)
  {
      std::vector<vpMbScanLineSegment> &scanline = localScanlines[j];
      sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator()); // Not sure its necessary

      bool b_start = true;
      for(size_t i = 0 ; i < scanline.size() ; ++i)
      {
          vpMbScanLineSegment s = scanline[i];
          if (b_start)
          {
              s.type = START;
              s.P1 = s.p * s.Z1;
              b_start = false;
          }
          else
          {
              vpMbScanLineSegment &prev = scanlines[j].back();
              s.type = END;
              s.P1 = prev.P1;
              s.Z1 = prev.Z1;
              s.P2 = s.p * s.Z2;
              prev.P2 = s.P2;
              prev.Z2 = s.Z2;
              b_start = true;
          }
          scanlines[j].push_back(s);
      }
  }
}

void
vpMbScanLine::drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > &polygons,
                        std::vector<int> listPolyIndices,
                        const vpCameraParameters &cam, unsigned int width, unsigned int height)
{
  this->w = width;
  this->h = height;
  this->K = cam;

  visibility_samples.clear();

  std::vector<std::vector<vpMbScanLineSegment> > scanlinesY;
  scanlinesY.resize(h);
  std::vector<std::vector<vpMbScanLineSegment> > scanlinesX;
  scanlinesX.resize(w);

  mask.resize(h,w,0);

  vpImage<unsigned char> maskY(h,w,0);
  vpImage<unsigned char> maskX(h,w,0);

  primitive_ids.resize(h, w, -1);

  for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
  {
      drawPolygonY(*(polygons[ID]), listPolyIndices[ID], scanlinesY);
      drawPolygonX(*(polygons[ID]), listPolyIndices[ID], scanlinesX);
  }

  // Y
  int last_ID = -1;
  vpMbScanLineSegment last_visible;
  for(unsigned int y = 0 ; y < scanlinesY.size() ; ++y)
  {
      std::vector<vpMbScanLineSegment> &scanline = scanlinesY[y];
      sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());

      std::vector<std::pair<double, vpMbScanLineSegment> > stack;
      for(size_t i = 0 ; i < scanline.size() ; ++i)
      {
          const vpMbScanLineSegment &s = scanline[i];

          switch(s.type)
          {
          case START:
              stack.push_back(std::make_pair(s.Z1, s));
              break;
          case END:
              for(size_t j = 0 ; j < stack.size() ; ++j)
                  if (stack[j].second.ID == s.ID)
                  {
                      stack[j] = stack.back();
                      stack.pop_back();
                      break;
                  }
              break;
          case POINT:
              break;
          }

          for(size_t j = 0 ; j < stack.size() ; ++j)
          {
              const vpMbScanLineSegment &s0 = stack[j].second;
              stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
          }
          sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());

          int new_ID = stack.empty() ? -1 : stack.front().second.ID;

          if (new_ID != last_ID || s.type == POINT)
          {
              if (s.b_sample_Y)
                  switch(s.type)
                  {
                  case POINT:
                      if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
                          visibility_samples[s.edge].insert((int)y);
                      break;
                  case START:
                      if (new_ID == s.ID)
                          visibility_samples[s.edge].insert((int)y);
                      break;
                  case END:
                      if (last_ID == s.ID)
                          visibility_samples[s.edge].insert((int)y);
                      break;
                  }

              // This part will only be used for MbKltTracking
              if (last_ID != -1)
              {
                  const unsigned int x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
                  const double x1 = std::min<double>(w, s.p);
                  for(unsigned int x = x0 + maskBorder ; x < x1 - maskBorder; ++x)
                  {
                      primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;

                      if(maskBorder != 0)
                        maskY[(unsigned int)y][(unsigned int)x] = 255;
                      else
                        mask[(unsigned int)y][(unsigned int)x] = 255;
                  }
              }

              last_ID = new_ID;
              if (!stack.empty())
              {
                  last_visible = stack.front().second;
                  last_visible.p = s.p;
              }
          }
      }
  }

  // X
  last_ID = -1;
  for(unsigned int x = 0 ; x < scanlinesX.size() ; ++x)
  {
      std::vector<vpMbScanLineSegment> &scanline = scanlinesX[x];
      sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());

      std::vector<std::pair<double, vpMbScanLineSegment> > stack;
      for(size_t i = 0 ; i < scanline.size() ; ++i)
      {
          const vpMbScanLineSegment &s = scanline[i];

          switch(s.type)
          {
          case START:
              stack.push_back(std::make_pair(s.Z1, s));
              break;
          case END:
              for(size_t j = 0 ; j < stack.size() ; ++j)
                  if (stack[j].second.ID == s.ID)
                  {
                      stack[j] = stack.back();
                      stack.pop_back();
                      break;
                  }
              break;
          case POINT:
              break;
          }

          for(size_t j = 0 ; j < stack.size() ; ++j)
          {
              const vpMbScanLineSegment &s0 = stack[j].second;
              stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
          }
          sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());

          int new_ID = stack.empty() ? -1 : stack.front().second.ID;

          if (new_ID != last_ID || s.type == POINT)
          {
              if (!s.b_sample_Y)
                  switch(s.type)
                  {
                  case POINT:
                      if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
                          visibility_samples[s.edge].insert((int)x);
                      break;
                  case START:
                      if (new_ID == s.ID)
                          visibility_samples[s.edge].insert((int)x);
                      break;
                  case END:
                      if (last_ID == s.ID)
                          visibility_samples[s.edge].insert((int)x);
                      break;
                  }

              // This part will only be used for MbKltTracking
              if (maskBorder != 0 && last_ID != -1)
              {
                  const unsigned int y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
                  const double y1 = std::min<double>(h, s.p);
                  for(unsigned int y = y0 + maskBorder ; y < y1 - maskBorder; ++y)
                  {
                      //primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
                      maskX[(unsigned int)y][(unsigned int)x] = 255;
                  }
              }

              last_ID = new_ID;
              if (!stack.empty())
              {
                  last_visible = stack.front().second;
                  last_visible.p = s.p;
              }
          }
      }
  }

  if(maskBorder != 0)
    for(unsigned int i = 0 ; i < h ; i++)
      for(unsigned int j = 0 ; j < w ; j++)
        if(maskX[i][j] == 255 && maskY[i][j] == 255)
          mask[i][j] = 255;

#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
  if(!dispMaskDebug->isInitialised()){
    dispMaskDebug->init(mask, 800, 600);
  }

  vpDisplay::display(mask);

  for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
  {
    for(unsigned int i = 0 ; i < polygons[ID]->size() ; i++){
      vpPoint p1 = (*(polygons[ID]))[i].first;
      vpPoint p2 = (*(polygons[ID]))[(i+1)%polygons[ID]->size()].first;
      double i1=0,j1=0,i2=0,j2=0;
      p1.project();
      p2.project();
      vpMeterPixelConversion::convertPoint(K,p1.get_x(), p1.get_y(),j1,i1);
      vpMeterPixelConversion::convertPoint(K,p2.get_x(), p2.get_y(),j2,i2);

      vpDisplay::displayLine(mask,i1,j1,i2,j2,vpColor::red,3);
    }
  }

  vpDisplay::flush(mask);


  if(!dispLineDebug->isInitialised()){
    linedebugImg.resize(h,w,0);
    dispLineDebug->init(linedebugImg, 800, 100);
  }
  vpDisplay::display(linedebugImg);
#endif

}

void
vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
                                  std::vector<std::pair<vpPoint, vpPoint> > &lines,
                                  const bool &displayResults)
{
  vpColVector _a, _b;
  createVectorFromPoint(a, _a, K);
  createVectorFromPoint(b, _b, K);

  double x0 = _a[0] / _a[2];
  double y0 = _a[1] / _a[2];
  double z0 = _a[2];
  double x1 = _b[0] / _b[2];
  double y1 = _b[1] / _b[2];
  double z1 = _b[2];

  vpMbScanLineEdge edge = makeMbScanLineEdge(a, b);
  lines.clear();

  if(displayResults){
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
    double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
    vpPoint a_(a), b_(b);
    a_.project();
    b_.project();
    vpMeterPixelConversion::convertPoint(K,a_.get_x(), a_.get_y(),j1,i1);
    vpMeterPixelConversion::convertPoint(K,b_.get_x(), b_.get_y(),j2,i2);

    vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::yellow,3);
#endif
  }

  if (!visibility_samples.count(edge))
      return;

  // Initialized as the biggest difference between the two points is on the X-axis
  double *v0(&x0), *w0(&z0);
  double *v1(&x1), *w1(&z1);
  unsigned int size(w);

  if (std::fabs(y0 - y1) > std::fabs(x0 - x1)) // Test if the biggest difference is on the Y-axis
  {
    v0 = &y0;
    v1 = &y1;
    size = h;
  }

  // Cannot call swap(a,b) since a and b are const
  // The fix consists in 2 new points that contain the right points
  vpPoint a_;
  vpPoint b_;

  if (*v0 > *v1)
  {
      std::swap(v0, v1);
      std::swap(w0, w1);
      //std::swap(a, b);
      // Cannot call swap(a,b) since a and b are const
      // Instead of swap we set the right address of the corresponding pointers
      a_ = b;
      b_ = a;
  }
  else {
    a_ = a;
    b_ = b;
  }

  //if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0)
  if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits<double>::epsilon())
      return;

  const int _v0 = std::max(0, int(std::ceil(*v0)));
  const int _v1 = std::min<int>((int)(size - 1), (int)(std::ceil(*v1) - 1));

  const std::set<int> &visible_samples = visibility_samples[edge];
  int last = _v0;
  vpPoint line_start;
  vpPoint line_end;
  bool b_line_started = false;
  for(std::set<int>::const_iterator it = visible_samples.begin() ; it != visible_samples.end() ; ++it)
  {
      const int v = *it;
      const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
      //const vpPoint p = mix(a, b, alpha);
      const vpPoint p = mix(a_, b_, alpha);
      if (last + 1 != v)
      {
          if(b_line_started)
            lines.push_back(std::make_pair(line_start, line_end));
          b_line_started = false;
      }
      if (v == _v0)
      {
          //line_start = a;
          line_start = a_;
          line_end = p;
          b_line_started = true;
      }
      else if (v == _v1)
      {
          //line_end = b;
          line_end = b_;
          if (!b_line_started)
              line_start = p;
          b_line_started = true;
      }
      else
      {
          line_end = p;
          if (!b_line_started)
              line_start = p;
          b_line_started = true;
      }
      last = v;
  }
  if (b_line_started)
      lines.push_back(std::make_pair(line_start, line_end));

  if(displayResults){
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
    double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
    for(unsigned int i = 0 ; i < lines.size() ; i++){
      lines[i].first.project();
      lines[i].second.project();
      vpMeterPixelConversion::convertPoint(K,lines[i].first.get_x(), lines[i].first.get_y(),j1,i1);
      vpMeterPixelConversion::convertPoint(K,lines[i].second.get_x(), lines[i].second.get_y(),j2,i2);

      vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::red,3);
    }
    vpDisplay::flush(linedebugImg);
#endif
  }
}

vpMbScanLine::vpMbScanLineEdge
vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b)
{
  vpColVector _a(3);
  vpColVector _b(3);

  _a[0] = std::ceil((a.get_X() * 1e8) * 1e-6);
  _a[1] = std::ceil((a.get_Y() * 1e8) * 1e-6);
  _a[2] = std::ceil((a.get_Z() * 1e8) * 1e-6);

  _b[0] = std::ceil((b.get_X() * 1e8) * 1e-6);
  _b[1] = std::ceil((b.get_Y() * 1e8) * 1e-6);
  _b[2] = std::ceil((b.get_Z() * 1e8) * 1e-6);

  bool b_comp = false;
  for(unsigned int i = 0 ; i < 3 ; ++i)
    if (_a[i] < _b[i])
    {
      b_comp = true;
      break;
    }
    else if(_a[i] > _b[i])
      break;

  if (b_comp)
    return std::make_pair(_a, _b);

  return std::make_pair(_b, _a);
}

void
vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K)
{
    v = vpColVector(3);

    v[0] = p.get_X() * K.get_px() + K.get_u0() * p.get_Z();
    v[1] = p.get_Y() * K.get_py() + K.get_v0() * p.get_Z();
    v[2] = p.get_Z();
}

double
vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, double Z1)
{
  const double N = X0 - x * Z0;
  const double D = x * (Z1 - Z0) - (X1 - X0);
  double alpha = N / D;
  if (vpMath::isNaN(alpha) || vpMath::isInf(alpha))
    return 0.0;

  alpha = std::min(1.0,alpha);
  alpha = std::max(0.0,alpha);
  return alpha;
}

double
vpMbScanLine::mix(double a, double b, double alpha)
{
  return a + (b - a) * alpha;
}

vpPoint
vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
{
  vpPoint res;
  res.set_X(a.get_X() + ( b.get_X() - a.get_X() ) * alpha);
  res.set_Y(a.get_Y() + ( b.get_Y() - a.get_Y() ) * alpha);
  res.set_Z(a.get_Z() + ( b.get_Z() - a.get_Z() ) * alpha);

  return res;
}

double
vpMbScanLine::norm(const vpPoint &a, const vpPoint &b)
{
  return sqrt(vpMath::sqr(a.get_X()-b.get_X()) + vpMath::sqr(a.get_Y() - b.get_Y()) + vpMath::sqr(a.get_Z() - b.get_Z()));
}

#endif