Visual Servoing Platform  version 3.2.0
vpMbtDistanceCircle.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Make the complete tracking of an object by using its CAD model. Circle
33  * tracking.
34  *
35  * Authors:
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpConfig.h>
41 
47 #include <algorithm>
48 #include <stdlib.h>
49 
50 #include <visp3/core/vpMeterPixelConversion.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPlane.h>
53 #include <visp3/mbt/vpMbtDistanceCircle.h>
54 #include <visp3/vision/vpPose.h>
55 #include <visp3/visual_features/vpFeatureBuilder.h>
56 #include <visp3/visual_features/vpFeatureEllipse.h>
57 
62  : name(), index(0), cam(), me(NULL), wmean(1), featureEllipse(), isTrackedCircle(true), meEllipse(NULL), circle(NULL),
63  radius(0.), p1(NULL), p2(NULL), p3(NULL), L(), error(), nbFeature(0), Reinit(false), hiddenface(NULL),
64  index_polygon(-1), isvisible(false)
65 {
66 }
67 
72 {
73  if (meEllipse != NULL)
74  delete meEllipse;
75  if (circle != NULL)
76  delete circle;
77  if (p1 != NULL)
78  delete p1;
79  if (p2 != NULL)
80  delete p2;
81  if (p3 != NULL)
82  delete p3;
83 }
84 
91 void vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo) { circle->project(cMo); }
92 
103 void vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
104 {
105  circle = new vpCircle;
106  p1 = new vpPoint;
107  p2 = new vpPoint;
108  p3 = new vpPoint;
109 
110  // Get the points
111  *p1 = _p1;
112  *p2 = _p2;
113  *p3 = _p3;
114 
115  // Get the radius
116  radius = r;
117 
118  vpPlane plane(*p1, *p2, *p3, vpPlane::object_frame);
119 
120  // Build our circle
121  circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
122 }
123 
130 {
131  me = _me;
132  if (meEllipse != NULL) {
133  meEllipse->setMe(me);
134  }
135 }
136 
148 bool vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack,
149  const vpImage<bool> *mask)
150 {
151  if (isvisible) {
152  // Perspective projection
153  circle->changeFrame(cMo);
154 
155  try {
156  circle->projection();
157  } catch (...) {
158  std::cout << "Problem when projecting circle\n";
159  return false;
160  }
161 
162  // Create the moving edges containers
163  meEllipse = new vpMbtMeEllipse;
164  meEllipse->setMask(*mask);
165  meEllipse->setMe(me);
166 
167  // meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
168  meEllipse->setInitRange(me->getRange()); // TODO: check because set to zero for lines
169 
170  try {
171  vpImagePoint ic;
172  double mu20_p, mu11_p, mu02_p;
173  vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
174  meEllipse->initTracking(I, ic, mu20_p, mu11_p, mu02_p, doNotTrack);
175  } catch (...) {
176  // vpTRACE("the circle can't be initialized");
177  return false;
178  }
179  }
180  return true;
181 }
182 
190 {
191  if (isvisible) {
192  try {
193  meEllipse->track(I);
194  } catch (...) {
195  // std::cout << "Track meEllipse failed" << std::endl;
196  meEllipse->reset();
197  Reinit = true;
198  }
199 
200  // Update the number of features
201  nbFeature = (unsigned int)meEllipse->getMeList().size();
202  }
203 }
204 
214 {
215  if (isvisible) {
216  // Perspective projection
217  circle->changeFrame(cMo);
218 
219  try {
220  circle->projection();
221  } catch (...) {
222  std::cout << "Problem when projecting circle\n";
223  }
224 
225  try {
226 
227  vpImagePoint ic;
228  double mu20_p, mu11_p, mu02_p;
229  vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
230  meEllipse->updateParameters(I, ic, mu20_p, mu11_p, mu02_p);
231  } catch (...) {
232  Reinit = true;
233  }
234  nbFeature = (unsigned int)meEllipse->getMeList().size();
235  }
236 }
237 
249 {
250  if (meEllipse != NULL)
251  delete meEllipse;
252 
253  meEllipse = NULL;
254 
255  if (!initMovingEdge(I, cMo, false, mask))
256  Reinit = true;
257 
258  Reinit = false;
259 }
260 
273  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
274  const bool displayFullModel)
275 {
276  if ((isvisible && isTrackedCircle) || displayFullModel) {
277  // Perspective projection
278  circle->changeFrame(cMo);
279 
280  try {
281  circle->projection();
282  } catch (...) {
283  std::cout << "Cannot project the circle";
284  }
285 
286  vpImagePoint center;
287  double mu20_p, mu11_p, mu02_p;
288  vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
289  vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
290  }
291 }
292 
305  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
306  const bool displayFullModel)
307 {
308  if ((isvisible && isTrackedCircle) || displayFullModel) {
309  // Perspective projection
310  circle->changeFrame(cMo);
311 
312  try {
313  circle->projection();
314  } catch (...) {
315  std::cout << "Cannot project the circle";
316  }
317 
318  vpImagePoint center;
319  double mu20_p, mu11_p, mu02_p;
320  vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
321  vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
322  }
323 }
324 
340 {
341  if (meEllipse != NULL) {
342  meEllipse->display(I); // display the me
343  if (vpDEBUG_ENABLE(3))
344  vpDisplay::flush(I);
345  }
346 }
347 
352 {
353  if (isvisible) {
354  nbFeature = (unsigned int)meEllipse->getMeList().size();
355  L.resize(nbFeature, 6);
357  } else
358  nbFeature = 0;
359 }
360 
366 {
367  if (isvisible) {
368  // Perspective projection
369  circle->changeFrame(cMo);
370  try {
371  circle->projection();
372  } catch (...) {
373  std::cout << "Problem projection circle\n";
374  }
375 
376  vpFeatureBuilder::create(featureEllipse, *circle);
377 
378  vpMatrix H1 = featureEllipse.interaction();
379 
380  vpRowVector H(5);
381  double x = 0, y = 0;
382 
383  // Get the parameters of the ellipse in the image plane
384  double xg = circle->p[0];
385  double yg = circle->p[1];
386  double mu20 = circle->p[2];
387  double mu11 = circle->p[3];
388  double mu02 = circle->p[4];
389 
390  unsigned int j = 0;
391 
392  for (std::list<vpMeSite>::const_iterator it = meEllipse->getMeList().begin(); it != meEllipse->getMeList().end();
393  ++it) {
394  vpPixelMeterConversion::convertPoint(cam, it->j, it->i, x, y);
395  H[0] = 2 * (mu11 * (y - yg) + mu02 * (xg - x));
396  H[1] = 2 * (mu20 * (yg - y) + mu11 * (x - xg));
397  H[2] = vpMath::sqr(y - yg) - mu02;
398  H[3] = 2 * (yg * (x - xg) + y * xg + mu11 - x * y);
399  H[4] = vpMath::sqr(x - xg) - mu20;
400 
401  for (unsigned int k = 0; k < 6; k++)
402  L[j][k] = H[0] * H1[0][k] + H[1] * H1[1][k] + H[2] * H1[2][k] + H[3] * H1[3][k] + H[4] * H1[4][k];
403 
404  error[j] = mu02 * vpMath::sqr(x) + mu20 * vpMath::sqr(y) - 2 * mu11 * x * y + 2 * (mu11 * yg - mu02 * xg) * x +
405  2 * (mu11 * xg - mu20 * yg) * y + mu02 * vpMath::sqr(xg) + mu20 * vpMath::sqr(yg) -
406  2 * mu11 * xg * yg + vpMath::sqr(mu11) - mu20 * mu02;
407 
408  j++;
409  }
410  }
411 }
vpMbtDistanceCircle::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCircle.cpp:188
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:243
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:107
vpCircle::projection
void projection()
Definition: vpCircle.cpp:137
vpPoint::get_oY
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:421
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpForwardProjection::project
void project()
Definition: vpForwardProjection.cpp:67
vpMbtDistanceCircle::isvisible
bool isvisible
Indicates if the circle is visible or not.
Definition: vpMbtDistanceCircle.h:104
vpMbtDistanceCircle::initMovingEdge
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceCircle.cpp:147
vpMbtDistanceCircle::p3
vpPoint * p3
An other point on the plane containing the circle.
Definition: vpMbtDistanceCircle.h:89
vpPoint::get_oZ
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:423
vpMbtDistanceCircle::circle
vpCircle * circle
The circle to track.
Definition: vpMbtDistanceCircle.h:79
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:92
vpCircle
Class that defines what is a circle.
Definition: vpCircle.h:57
vpMbtDistanceCircle::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceCircle.cpp:247
vpFeatureEllipse::interaction
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
Definition: vpFeatureEllipse.cpp:92
vpMbtDistanceCircle::buildFrom
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
Definition: vpMbtDistanceCircle.cpp:102
vpMe::getRange
unsigned int getRange() const
Definition: vpMe.h:178
vpPoint::get_oX
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:419
vpMbtDistanceCircle::nbFeature
unsigned int nbFeature
The number of moving edges.
Definition: vpMbtDistanceCircle.h:96
vpMe
Definition: vpMe.h:59
vpMeterPixelConversion::convertEllipse
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center, double &mu20_p, double &mu11_p, double &mu02_p)
Definition: vpMeterPixelConversion.cpp:143
vpMbtDistanceCircle::display
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Definition: vpMbtDistanceCircle.cpp:271
vpMbtDistanceCircle::p1
vpPoint * p1
The center of the circle.
Definition: vpMbtDistanceCircle.h:85
vpMbtDistanceCircle::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCircle.cpp:212
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:103
vpMbtDistanceCircle::vpMbtDistanceCircle
vpMbtDistanceCircle()
Definition: vpMbtDistanceCircle.cpp:60
vpMbtDistanceCircle::setMovingEdge
void setMovingEdge(vpMe *Me)
Definition: vpMbtDistanceCircle.cpp:128
vpMbtDistanceCircle::meEllipse
vpMbtMeEllipse * meEllipse
The moving edge containers.
Definition: vpMbtDistanceCircle.h:76
vpMbtDistanceCircle::radius
double radius
The radius of the circle.
Definition: vpMbtDistanceCircle.h:82
vpPixelMeterConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Definition: vpPixelMeterConversion.h:102
vpCircle::setWorldCoordinates
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:60
vpMbtDistanceCircle::L
vpMatrix L
The interaction matrix.
Definition: vpMbtDistanceCircle.h:92
vpTracker::p
vpColVector p
Definition: vpTracker.h:70
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
vpMbtDistanceCircle::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCircle.cpp:338
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
vpPlane::object_frame
Definition: vpPlane.h:69
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:652
vpRowVector
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:71
vpCircle::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:238
vpImage< unsigned char >
vpMbtDistanceCircle::~vpMbtDistanceCircle
virtual ~vpMbtDistanceCircle()
Definition: vpMbtDistanceCircle.cpp:70
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpArray2D::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:170
vpMbtDistanceCircle::computeInteractionMatrixError
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCircle.cpp:364
vpMbtDistanceCircle::initInteractionMatrixError
void initInteractionMatrixError()
Definition: vpMbtDistanceCircle.cpp:350
vpDisplay::displayEllipse
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:295
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:91
vpMbtDistanceCircle::error
vpColVector error
The error vector.
Definition: vpMbtDistanceCircle.h:94
vpDEBUG_ENABLE
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:537
vpMbtDistanceCircle::Reinit
bool Reinit
Indicates if the circle has to be reinitialized.
Definition: vpMbtDistanceCircle.h:98
vpMbtDistanceCircle::p2
vpPoint * p2
A point on the plane containing the circle.
Definition: vpMbtDistanceCircle.h:87