Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpMbtDistanceCircle.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Make the complete tracking of an object by using its CAD model. Circle
32  * tracking.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
46 #include <stdlib.h>
47 #include <algorithm>
48 
49 #include <visp3/mbt/vpMbtDistanceCircle.h>
50 #include <visp3/core/vpPlane.h>
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/core/vpPixelMeterConversion.h>
53 #include <visp3/visual_features/vpFeatureBuilder.h>
54 #include <visp3/visual_features/vpFeatureEllipse.h>
55 #include <visp3/vision/vpPose.h>
56 
61  : name(), index(0), cam(), me(NULL), wmean(1),
62  featureEllipse(), isTrackedCircle(true), meEllipse(NULL),
63  circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
64  L(), error(), nbFeature(0), Reinit(false),
65  hiddenface(NULL), index_polygon(-1), isvisible(false)
66 {
67 }
68 
73 {
74  if (meEllipse != NULL) delete meEllipse;
75  if (circle != NULL) delete circle;
76  if (p1 != NULL) delete p1;
77  if (p2 != NULL) delete p2;
78  if (p3 != NULL) delete p3;
79 }
80 
86 void
87 vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo)
88 {
89  circle->project(cMo) ;
90 }
91 
92 
103 void
104 vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
105 {
106  circle = new vpCircle ;
107  p1 = new vpPoint ;
108  p2 = new vpPoint ;
109  p3 = new vpPoint ;
110 
111  // Get the points
112  *p1 = _p1;
113  *p2 = _p2;
114  *p3 = _p3;
115 
116  // Get the radius
117  radius = r;
118 
119  vpPlane plane(*p1, *p2, *p3, vpPlane::object_frame);
120 
121  // Build our circle
122  circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
123 }
124 
125 
131 void
133 {
134  me = _me ;
135  if (meEllipse != NULL)
136  {
137  meEllipse->setMe(me) ;
138  }
139 }
140 
149 bool
151 {
152  if(isvisible){
153  // Perspective projection
154  circle->changeFrame(cMo);
155 
156  try{
157  circle->projection();
158  }
159  catch(...){
160  std::cout<<"Problem when projecting circle\n";
161  return false;
162  }
163 
164  // Create the moving edges containers
165  meEllipse = new vpMbtMeEllipse;
166  meEllipse->setMe(me) ;
167 
168  //meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
169  meEllipse->setInitRange(me->getRange()); // TODO: check because set to zero for lines
170 
171  try
172  {
173  vpImagePoint ic;
174  double mu20_p, mu11_p, mu02_p;
175  vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
176  meEllipse->initTracking(I, ic, mu20_p, mu11_p, mu02_p);
177  }
178  catch(...)
179  {
180  //vpTRACE("the circle can't be initialized");
181  return false;
182  }
183  }
184  return true;
185 }
186 
193 void
195 {
196  if(isvisible){
197  try
198  {
199  meEllipse->track(I) ;
200  }
201  catch(...)
202  {
203  //std::cout << "Track meEllipse failed" << std::endl;
204  meEllipse->reset();
205  Reinit = true;
206  }
207 
208  // Update the number of features
209  nbFeature = (unsigned int)meEllipse->getMeList().size();
210  }
211 }
212 
213 
222 void
224 {
225  if(isvisible){
226  // Perspective projection
227  circle->changeFrame(cMo);
228 
229  try{
230  circle->projection();
231  }
232  catch(...){std::cout<<"Problem when projecting circle\n";}
233 
234  try
235  {
236 
237  vpImagePoint ic;
238  double mu20_p, mu11_p, mu02_p;
239  vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
240  meEllipse->updateParameters(I, ic, mu20_p, mu11_p, mu02_p);
241  }
242  catch(...)
243  {
244  Reinit = true;
245  }
246  nbFeature = (unsigned int)meEllipse->getMeList().size();
247  }
248 }
249 
250 
259 void
261 {
262  if(meEllipse!= NULL)
263  delete meEllipse;
264 
265  meEllipse = NULL;
266 
267  if(initMovingEdge(I,cMo) == false)
268  Reinit = true;
269 
270  Reinit = false;
271 }
272 
273 
285 void
287  const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
288  const bool displayFullModel )
289 {
290  if( (isvisible && isTrackedCircle) || displayFullModel){
291  // Perspective projection
292  circle->changeFrame(cMo);
293 
294  try{
295  circle->projection();
296  }
297  catch(...){std::cout<<"Cannot project the circle";}
298 
299  vpImagePoint center;
300  double mu20_p, mu11_p, mu02_p;
301  vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
302  vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
303  }
304 }
305 
317 void
319  const vpCameraParameters &camera, const vpColor col,
320  const unsigned int thickness, const bool displayFullModel)
321 {
322  if( (isvisible && isTrackedCircle) || displayFullModel){
323  // Perspective projection
324  circle->changeFrame(cMo);
325 
326  try{
327  circle->projection();
328  }
329  catch(...){std::cout<<"Cannot project the circle";}
330 
331  vpImagePoint center;
332  double mu20_p, mu11_p, mu02_p;
333  vpMeterPixelConversion::convertEllipse(camera, *circle, center, mu20_p, mu11_p, mu02_p);
334  vpDisplay::displayEllipse(I, center, mu20_p, mu11_p, mu02_p, true, col, thickness);
335  }
336 }
337 
338 
349 void
351 {
352  if (meEllipse != NULL)
353  {
354  meEllipse->display(I); // display the me
355  if (vpDEBUG_ENABLE(3))
356  vpDisplay::flush(I);
357  }
358 }
359 
363 void
365 {
366  if (isvisible == true)
367  {
368  nbFeature = (unsigned int)meEllipse->getMeList().size();
369  L.resize(nbFeature, 6);
371  }
372  else
373  nbFeature = 0 ;
374 }
375 
379 void
381 {
382  if (isvisible)
383  {
384  // Perspective projection
385  circle->changeFrame(cMo) ;
386  try{
387  circle->projection();
388  }
389  catch(...){std::cout<<"Problem projection circle\n";}
390 
391  vpFeatureBuilder::create(featureEllipse, *circle);
392 
393  vpMatrix H1 = featureEllipse.interaction();
394 
395  vpRowVector H(5);
396  double x=0, y=0;
397 
398  // Get the parameters of the ellipse in the image plane
399  double xg = circle->p[0];
400  double yg = circle->p[1];
401  double mu20 = circle->p[2];
402  double mu11 = circle->p[3];
403  double mu02 = circle->p[4];
404 
405  unsigned int j = 0;
406 
407  for(std::list<vpMeSite>::const_iterator it=meEllipse->getMeList().begin(); it!=meEllipse->getMeList().end(); ++it){
408  vpPixelMeterConversion::convertPoint(cam, it->j, it->i, x, y);
409  H[0] = 2*(mu11*(y-yg)+mu02*(xg-x));
410  H[1] = 2*(mu20*(yg-y)+mu11*(x-xg));
411  H[2] = vpMath::sqr(y-yg)-mu02;
412  H[3] = 2*(yg*(x-xg)+y*xg+mu11-x*y);
413  H[4] = vpMath::sqr(x-xg)-mu20;
414 
415  for (unsigned int k=0; k<6; k++)
416  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];
417 
418  error[j] = mu02*vpMath::sqr(x) + mu20*vpMath::sqr(y) - 2*mu11*x*y
419  + 2*(mu11*yg-mu02*xg)*x + 2*(mu11*xg-mu20*yg)*y
420  + mu02*vpMath::sqr(xg) + mu20*vpMath::sqr(yg) - 2*mu11*xg*yg
421  + vpMath::sqr(mu11) - mu20*mu02;
422 
423  j++;
424  }
425  }
426 }
427 
unsigned int getRange() const
Definition: vpMe.h:167
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
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)
unsigned int nbFeature
The number of moving edges.
vpPoint * p3
An other point on the plane containing the circle.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:269
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:526
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:70
vpMatrix L
The interaction matrix.
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
vpColVector error
The error vector.
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:449
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
vpMbtMeEllipse * meEllipse
The moving edge containers.
Definition: vpMe.h:59
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
vpPoint * p2
A point on the plane containing the circle.
static void flush(const vpImage< unsigned char > &I)
Class that defines what is a point.
Definition: vpPoint.h:59
void projection()
Definition: vpCircle.cpp:155
bool Reinit
Indicates if the circle has to be reinitialized.
bool isvisible
Indicates if the circle is visible or not.
static double sqr(double x)
Definition: vpMath.h:110
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Generic class defining intrinsic camera parameters.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:451
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpCircle * circle
The circle to track.
static void convertEllipse(const vpCameraParameters &cam, const vpCircle &circle, vpImagePoint &center, double &mu20_p, double &mu11_p, double &mu02_p)
void displayMovingEdges(const vpImage< unsigned char > &I)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:447
double getB() const
Definition: vpPlane.h:108
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
double getA() const
Definition: vpPlane.h:106
double getC() const
Definition: vpPlane.h:110
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
double radius
The radius of the circle.
Class that defines what is a circle.
Definition: vpCircle.h:57
vpPoint * p1
The center of the circle.
vpColVector p
Definition: vpTracker.h:73
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:62
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)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225