vpMbtDistanceLine.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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 https://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:
 * Romain Tallonneau
 */
#include <visp3/core/vpConfig.h>

 
#include <stdlib.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPlane.h>
#include <visp3/mbt/vpMbtDistanceLine.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
 
BEGIN_VISP_NAMESPACE
 
namespace
{
const unsigned int defaultRange = 0U;
}
 
void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L);

vpMbtDistanceLine::vpMbtDistanceLine()
  : name(), index(0), cam(), me(nullptr), isTrackedLine(true), isTrackedLineWithVisibility(true), wmean(1), featureline(),
  poly(), useScanLine(false), meline(), line(nullptr), p1(nullptr), p2(nullptr), L(), error(), nbFeature(), nbFeatureTotal(0),
  Reinit(false), hiddenface(nullptr), Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
{ }

vpMbtDistanceLine::~vpMbtDistanceLine()
{
  if (line != nullptr)
    delete line;
 
  for (unsigned int i = 0; i < meline.size(); i++)
    if (meline[i] != nullptr)
      delete meline[i];
 
  meline.clear();
}

void vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
{
  line->project(cMo);
  p1->project(cMo);
  p2->project(cMo);
}

void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
{
  vpColVector a(3);
  vpColVector b(3);
 
  // Calculate vector corresponding to PQ
  a[0] = P.get_oX() - Q.get_oX();
  a[1] = P.get_oY() - Q.get_oY();
  a[2] = P.get_oZ() - Q.get_oZ();
 
  // Calculate vector corresponding to PR
  b[0] = P.get_oX() - R.get_oX();
  b[1] = P.get_oY() - R.get_oY();
  b[2] = P.get_oZ() - R.get_oZ();
 
  // Calculate normal vector to plane PQ x PR
  vpColVector n = vpColVector::cross(a, b);
 
  // Equation of the plane is given by:
  double A = n[0];
  double B = n[1];
  double C = n[2];
  double D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
 
  double norm = sqrt(A * A + B * B + C * C);
  plane.setA(A / norm);
  plane.setB(B / norm);
  plane.setC(C / norm);
  plane.setD(D / norm);
}

void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
{
  vpPlane plane1;
  vpPlane plane2;
  buildPlane(P1, P2, P3, plane1);
  buildPlane(P1, P2, P4, plane2);
 
  L.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(), plane2.getA(), plane2.getB(),
                        plane2.getC(), plane2.getD());
}

void vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
{
  if (line == nullptr) {
    line = new vpLine;
  }
 
  poly.setNbPoint(2);
  poly.addPoint(0, _p1);
  poly.addPoint(1, _p2);
 
  p1 = &poly.p[0];
  p2 = &poly.p[1];
 
  vpColVector V1(3);
  vpColVector V2(3);
 
  V1[0] = p1->get_oX();
  V1[1] = p1->get_oY();
  V1[2] = p1->get_oZ();
  V2[0] = p2->get_oX();
  V2[1] = p2->get_oY();
  V2[2] = p2->get_oZ();
 
  // if((V1-V2).sumSquare()!=0)
  if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
    vpColVector V3(3);
    V3[0] = double(rand_gen.next() % 1000) / 100;
    V3[1] = double(rand_gen.next() % 1000) / 100;
    V3[2] = double(rand_gen.next() % 1000) / 100;
 
    vpColVector v_tmp1, v_tmp2;
    v_tmp1 = V2 - V1;
    v_tmp2 = V3 - V1;
    vpColVector V4 = vpColVector::cross(v_tmp1, v_tmp2);
 
    vpPoint P3(V3[0], V3[1], V3[2]);
    vpPoint P4(V4[0], V4[1], V4[2]);
    buildLine(*p1, *p2, P3, P4, *line);
  }
  else {
    vpPoint P3(V1[0], V1[1], V1[2]);
    vpPoint P4(V2[0], V2[1], V2[2]);
    buildLine(*p1, *p2, P3, P4, *line);
  }
}

void vpMbtDistanceLine::addPolygon(const int &idx)
{
  Lindex_polygon.push_back(idx);
  Lindex_polygon_tracked.push_back(true);
}

void vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track)
{
  unsigned int ind = 0;
  for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
    if ((*hiddenface)[static_cast<unsigned int>(*itpoly)]->getName() == polyname) {
      Lindex_polygon_tracked[ind] = track;
    }
    ind++;
  }
 
  isTrackedLine = false;
  for (unsigned int i = 0; i < Lindex_polygon_tracked.size(); i++)
    if (Lindex_polygon_tracked[i]) {
      isTrackedLine = true;
      break;
    }
 
  if (!isTrackedLine) {
    isTrackedLineWithVisibility = false;
    return;
  }
 
  updateTracked();
}

void vpMbtDistanceLine::updateTracked()
{
  if (!isTrackedLine) {
    isTrackedLineWithVisibility = false;
    return;
  }
 
  unsigned int ind = 0;
  isTrackedLineWithVisibility = false;
  for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
    if ((*hiddenface)[static_cast<unsigned int>(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]) {
      isTrackedLineWithVisibility = true;
      break;
    }
    ind++;
  }
}

void vpMbtDistanceLine::setMovingEdge(vpMe *_me)
{
  me = _me;
 
  for (unsigned int i = 0; i < meline.size(); i++)
    if (meline[i] != nullptr) {
      //      nbFeature[i] = 0;
      meline[i]->reset();
      meline[i]->setMe(me);
    }
 
  //  nbFeatureTotal = 0;
}

bool vpMbtDistanceLine::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, bool doNotTrack,
                                       const vpImage<bool> *mask, const int &initRange)
{
  for (unsigned int i = 0; i < meline.size(); i++) {
    if (meline[i] != nullptr)
      delete meline[i];
  }
 
  meline.clear();
  nbFeature.clear();
  nbFeatureTotal = 0;
 
  if (isvisible) {
    p1->changeFrame(cMo);
    p2->changeFrame(cMo);
 
    if (poly.getClipping() > 3) // Contains at least one FOV constraint
      cam.computeFov(I.getWidth(), I.getHeight());
 
    poly.computePolygonClipped(cam);
 
    if (poly.polyClipped.size() == 2) { // Les points sont visibles.
 
      std::vector<std::pair<vpPoint, vpPoint> > linesLst;
 
      if (useScanLine) {
        hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
      }
      else {
        linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
      }
 
      if (linesLst.size() == 0) {
        return false;
      }
 
      line->changeFrame(cMo);
      try {
        line->projection();
      }
      catch (...) {
        isvisible = false;
        return false;
      }
      double rho, theta;
      // rho theta uv
      vpMeterPixelConversion::convertLine(cam, line->getRho(), line->getTheta(), rho, theta);
 
      while (theta > M_PI) {
        theta -= M_PI;
      }
      while (theta < -M_PI) {
        theta += M_PI;
      }
 
      if (theta < -M_PI / 2.0)
        theta = -theta - 3 * M_PI / 2.0;
      else
        theta = M_PI / 2.0 - theta;
 
      for (unsigned int i = 0; i < linesLst.size(); i++) {
        vpImagePoint ip1, ip2;
 
        linesLst[i].first.project();
        linesLst[i].second.project();
 
        vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
        vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
 
        unsigned int initRange_;
        if (initRange < 0) {
          initRange_ = defaultRange;
        }
        else {
          initRange_ = static_cast<unsigned int>(initRange);
        }
        int oldInitRange = me->getInitRange();
        me->setInitRange(initRange_);
        vpMbtMeLine *melinePt = new vpMbtMeLine;
        melinePt->setMask(*mask);
        melinePt->setMe(me);
 
        int marge = /*10*/ 5; // ou 5 normalement
        if (ip1.get_j() < ip2.get_j()) {
          melinePt->jmin = static_cast<int>(ip1.get_j()) - marge;
          melinePt->jmax = static_cast<int>(ip2.get_j()) + marge;
        }
        else {
          melinePt->jmin = static_cast<int>(ip2.get_j()) - marge;
          melinePt->jmax = static_cast<int>(ip1.get_j()) + marge;
        }
        if (ip1.get_i() < ip2.get_i()) {
          melinePt->imin = static_cast<int>(ip1.get_i()) - marge;
          melinePt->imax = static_cast<int>(ip2.get_i()) + marge;
        }
        else {
          melinePt->imin = static_cast<int>(ip2.get_i()) - marge;
          melinePt->imax = static_cast<int>(ip1.get_i()) + marge;
        }
 
        try {
          melinePt->initTracking(I, ip1, ip2, rho, theta, doNotTrack);
          me->setInitRange(oldInitRange);
          meline.push_back(melinePt);
          nbFeature.push_back(static_cast<unsigned int>(melinePt->getMeList().size()));
          nbFeatureTotal += nbFeature.back();
        }
        catch (...) {
          me->setInitRange(oldInitRange);
          delete melinePt;
          isvisible = false;
          return false;
        }
      }
    }
    else {
      isvisible = false;
    }
  }
 
  return true;
}

void vpMbtDistanceLine::trackMovingEdge(const vpImage<unsigned char> &I)
{
  int oldInitRange = me->getInitRange();
  me->setInitRange(defaultRange);
  if (isvisible) {
    try {
      nbFeature.clear();
      nbFeatureTotal = 0;
      for (size_t i = 0; i < meline.size(); i++) {
        meline[i]->track(I);
        nbFeature.push_back(static_cast<unsigned int>(meline[i]->getMeList().size()));
        nbFeatureTotal += static_cast<unsigned int>(meline[i]->getMeList().size());
      }
    }
    catch (...) {
      for (size_t i = 0; i < meline.size(); i++) {
        if (meline[i] != nullptr)
          delete meline[i];
      }
 
      nbFeature.clear();
      meline.clear();
      nbFeatureTotal = 0;
      Reinit = true;
      isvisible = false;
    }
  }
  me->setInitRange(oldInitRange);
}

void vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  int oldInitRange = me->getInitRange();
  if (isvisible) {
    p1->changeFrame(cMo);
    p2->changeFrame(cMo);
 
    if (poly.getClipping() > 3) // Contains at least one FOV constraint
      cam.computeFov(I.getWidth(), I.getHeight());
 
    poly.computePolygonClipped(cam);
 
    if (poly.polyClipped.size() == 2) { // Les points sont visibles.
 
      std::vector<std::pair<vpPoint, vpPoint> > linesLst;
 
      if (useScanLine) {
        hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
      }
      else {
        linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
      }
 
      if (linesLst.size() != meline.size() || linesLst.size() == 0) {
        for (size_t i = 0; i < meline.size(); i++) {
          if (meline[i] != nullptr)
            delete meline[i];
        }
 
        meline.clear();
        nbFeature.clear();
        nbFeatureTotal = 0;
        isvisible = false;
        Reinit = true;
      }
      else {
        line->changeFrame(cMo);
        try {
          line->projection();
        }
        catch (...) {
          for (size_t j = 0; j < meline.size(); j++) {
            if (meline[j] != nullptr)
              delete meline[j];
          }
 
          meline.clear();
          nbFeature.clear();
          nbFeatureTotal = 0;
          isvisible = false;
          Reinit = true;
          return;
        }
        double rho, theta;
        // rho theta uv
        vpMeterPixelConversion::convertLine(cam, line->getRho(), line->getTheta(), rho, theta);
 
        while (theta > M_PI) {
          theta -= M_PI;
        }
        while (theta < -M_PI) {
          theta += M_PI;
        }
 
        if (theta < -M_PI / 2.0)
          theta = -theta - 3 * M_PI / 2.0;
        else
          theta = M_PI / 2.0 - theta;
 
        try {
          for (unsigned int i = 0; i < linesLst.size(); i++) {
            vpImagePoint ip1, ip2;
 
            linesLst[i].first.project();
            linesLst[i].second.project();
 
            vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
            vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
 
            int marge = /*10*/ 5; // ou 5 normalement
            if (ip1.get_j() < ip2.get_j()) {
              meline[i]->jmin = static_cast<int>(ip1.get_j()) - marge;
              meline[i]->jmax = static_cast<int>(ip2.get_j()) + marge;
            }
            else {
              meline[i]->jmin = static_cast<int>(ip2.get_j()) - marge;
              meline[i]->jmax = static_cast<int>(ip1.get_j()) + marge;
            }
            if (ip1.get_i() < ip2.get_i()) {
              meline[i]->imin = static_cast<int>(ip1.get_i()) - marge;
              meline[i]->imax = static_cast<int>(ip2.get_i()) + marge;
            }
            else {
              meline[i]->imin = static_cast<int>(ip2.get_i()) - marge;
              meline[i]->imax = static_cast<int>(ip1.get_i()) + marge;
            }
            me->setInitRange(defaultRange);
            meline[i]->updateParameters(I, ip1, ip2, rho, theta);
            nbFeature[i] = static_cast<unsigned int>(meline[i]->getMeList().size());
            nbFeatureTotal += nbFeature[i];
          }
        }
        catch (...) {
          for (size_t j = 0; j < meline.size(); j++) {
            if (meline[j] != nullptr)
              delete meline[j];
          }
 
          meline.clear();
          nbFeature.clear();
          nbFeatureTotal = 0;
          isvisible = false;
          Reinit = true;
        }
      }
    }
    else {
      for (size_t i = 0; i < meline.size(); i++) {
        if (meline[i] != nullptr)
          delete meline[i];
      }
      nbFeature.clear();
      meline.clear();
      nbFeatureTotal = 0;
      isvisible = false;
    }
  }
  me->setInitRange(oldInitRange);
}

void vpMbtDistanceLine::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                         const vpImage<bool> *mask)
{
  for (size_t i = 0; i < meline.size(); i++) {
    if (meline[i] != nullptr)
      delete meline[i];
  }
 
  nbFeature.clear();
  meline.clear();
  nbFeatureTotal = 0;
 
  if (!initMovingEdge(I, cMo, false, mask))
    Reinit = true;
 
  Reinit = false;
}

void vpMbtDistanceLine::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
                                bool displayFullModel)
{
  std::vector<std::vector<double> > models =
    getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
 
  for (size_t i = 0; i < models.size(); i++) {
    vpImagePoint ip1(models[i][1], models[i][2]);
    vpImagePoint ip2(models[i][3], models[i][4]);
    vpDisplay::displayLine(I, ip1, ip2, col, thickness);
  }
}

void vpMbtDistanceLine::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
                                const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
                                bool displayFullModel)
{
  std::vector<std::vector<double> > models =
    getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
 
  for (size_t i = 0; i < models.size(); i++) {
    vpImagePoint ip1(models[i][1], models[i][2]);
    vpImagePoint ip2(models[i][3], models[i][4]);
    vpDisplay::displayLine(I, ip1, ip2, col, thickness);
  }
}

void vpMbtDistanceLine::displayMovingEdges(const vpImage<unsigned char> &I)
{
  for (size_t i = 0; i < meline.size(); i++) {
    if (meline[i] != nullptr) {
      meline[i]->display(I);
    }
  }
}
 
void vpMbtDistanceLine::displayMovingEdges(const vpImage<vpRGBa> &I)
{
  for (size_t i = 0; i < meline.size(); i++) {
    if (meline[i] != nullptr) {
      meline[i]->display(I);
    }
  }
}

std::vector<std::vector<double> > vpMbtDistanceLine::getFeaturesForDisplay()
{
  std::vector<std::vector<double> > features;
 
  for (size_t i = 0; i < meline.size(); i++) {
    vpMbtMeLine *me_l = meline[i];
    if (me_l != nullptr) {
      for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
        vpMeSite p_me_l = *it;
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
        std::vector<double> params = { 0, //ME
                                      p_me_l.get_ifloat(),
                                      p_me_l.get_jfloat(),
                                      static_cast<double>(p_me_l.getState()) };
#else
        std::vector<double> params;
        params.push_back(0); // ME
        params.push_back(p_me_l.get_ifloat());
        params.push_back(p_me_l.get_jfloat());
        params.push_back(static_cast<double>(p_me_l.getState()));
#endif
 
        features.push_back(params);
      }
    }
  }
 
  return features;
}

std::vector<std::vector<double> > vpMbtDistanceLine::getModelForDisplay(unsigned int width, unsigned int height,
                                                                        const vpHomogeneousMatrix &cMo,
                                                                        const vpCameraParameters &camera,
                                                                        bool displayFullModel)
{
  std::vector<std::vector<double> > models;
 
  if ((isvisible && isTrackedLine) || displayFullModel) {
    p1->changeFrame(cMo);
    p2->changeFrame(cMo);
 
    vpImagePoint ip1, ip2;
    vpCameraParameters c = camera;
    if (poly.getClipping() > 3) // Contains at least one FOV constraint
      c.computeFov(width, height);
 
    poly.computePolygonClipped(c);
 
    if (poly.polyClipped.size() == 2 &&
        ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
        ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
        ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
        ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
        ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
        ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
 
      std::vector<std::pair<vpPoint, vpPoint> > linesLst;
      if (useScanLine && !displayFullModel) {
        hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
      }
      else {
        linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
      }
 
      for (unsigned int i = 0; i < linesLst.size(); i++) {
        linesLst[i].first.project();
        linesLst[i].second.project();
 
        vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
        vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
 
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
        std::vector<double> params = { 0, //0 for line parameters
                                      ip1.get_i(),
                                      ip1.get_j(),
                                      ip2.get_i(),
                                      ip2.get_j() };
#else
        std::vector<double> params;
        params.push_back(0); //0 for line parameters
        params.push_back(ip1.get_i());
        params.push_back(ip1.get_j());
        params.push_back(ip2.get_i());
        params.push_back(ip2.get_j());
#endif
 
        models.push_back(params);
      }
    }
  }
 
  return models;
}

void vpMbtDistanceLine::initInteractionMatrixError()
{
  if (isvisible) {
    L.resize(nbFeatureTotal, 6);
    error.resize(nbFeatureTotal);
  }
  else {
    for (size_t i = 0; i < meline.size(); i++) {
      nbFeature[i] = 0;
      // To be consistent with nbFeature[i] = 0
      std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
      me_site_list.clear();
    }
    nbFeatureTotal = 0;
  }
}

void vpMbtDistanceLine::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
{
  if (isvisible) {
    try {
      // feature projection
      line->changeFrame(cMo);
      line->projection();
 
      vpFeatureBuilder::create(featureline, *line);
 
      double rho = featureline.getRho();
      double theta = featureline.getTheta();
 
      double co = cos(theta);
      double si = sin(theta);
 
      double mx = 1.0 / cam.get_px();
      double my = 1.0 / cam.get_py();
      double xc = cam.get_u0();
      double yc = cam.get_v0();
 
      double alpha_;
      vpMatrix H = featureline.interaction();
 
      double x, y;
      unsigned int j = 0;
 
      for (size_t i = 0; i < meline.size(); i++) {
        for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
             it != meline[i]->getMeList().end(); ++it) {
          x = static_cast<double>(it->m_j);
          y = static_cast<double>(it->m_i);
 
          x = (x - xc) * mx;
          y = (y - yc) * my;
 
          alpha_ = x * si - y * co;
 
          double *Lrho = H[0];
          double *Ltheta = H[1];
          // Calculate interaction matrix for a distance
          for (unsigned int k = 0; k < 6; k++) {
            L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
          }
          error[j] = rho - (x * co + y * si);
          j++;
        }
      }
    }
    catch (...) {
   // Handle potential exception: due to a degenerate case: the image of the straight line is a point!
   // Set the corresponding interaction matrix part to zero
      unsigned int j = 0;
      for (size_t i = 0; i < meline.size(); i++) {
        for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
             it != meline[i]->getMeList().end(); ++it) {
          for (unsigned int k = 0; k < 6; k++) {
            L[j][k] = 0.0;
          }
 
          error[j] = 0.0;
          j++;
        }
      }
    }
  }
}

bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
{
  if (threshold > I.getWidth() || threshold > I.getHeight()) {
    return true;
  }
  if (isvisible) {
 
    for (size_t i = 0; i < meline.size(); i++) {
      for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
           ++it) {
        int i_ = it->m_i;
        int j_ = it->m_j;
 
        if (i_ < 0 || j_ < 0) { // out of image.
          return true;
        }
 
        if ((static_cast<unsigned int>(i_) >(I.getHeight() - threshold)) || static_cast<unsigned int>(i_) < threshold ||
            (static_cast<unsigned int>(j_) > (I.getWidth() - threshold)) || static_cast<unsigned int>(j_) < threshold) {
          return true;
        }
      }
    }
  }
  return false;
}
END_VISP_NAMESPACE