Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpMbtDistanceCircle.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
36 
37 #include <visp3/core/vpConfig.h>
38 
44 #include <algorithm>
45 #include <stdlib.h>
46 
47 #include <visp3/core/vpMeterPixelConversion.h>
48 #include <visp3/core/vpPixelMeterConversion.h>
49 #include <visp3/core/vpPlane.h>
50 #include <visp3/mbt/vpMbtDistanceCircle.h>
51 #include <visp3/vision/vpPose.h>
52 #include <visp3/visual_features/vpFeatureBuilder.h>
53 #include <visp3/visual_features/vpFeatureEllipse.h>
54 
59  : name(), index(0), cam(), me(nullptr), wmean(1), featureEllipse(), isTrackedCircle(true), meEllipse(nullptr), circle(nullptr),
60  radius(0.), p1(nullptr), p2(nullptr), p3(nullptr), L(), error(), nbFeature(0), Reinit(false), hiddenface(nullptr),
61  index_polygon(-1), isvisible(false)
62 { }
63 
68 {
69  if (meEllipse != nullptr)
70  delete meEllipse;
71  if (circle != nullptr)
72  delete circle;
73  if (p1 != nullptr)
74  delete p1;
75  if (p2 != nullptr)
76  delete p2;
77  if (p3 != nullptr)
78  delete p3;
79 }
80 
87 void vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo) { circle->project(cMo); }
88 
99 void vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
100 {
101  circle = new vpCircle;
102  p1 = new vpPoint;
103  p2 = new vpPoint;
104  p3 = new vpPoint;
105 
106  // Get the points
107  *p1 = _p1;
108  *p2 = _p2;
109  *p3 = _p3;
110 
111  // Get the radius
112  radius = r;
113 
114  vpPlane plane(*p1, *p2, *p3, vpPlane::object_frame);
115 
116  // Build our circle
117  circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
118 }
119 
126 {
127  me = _me;
128  if (meEllipse != nullptr) {
129  meEllipse->setMe(me);
130  }
131 }
132 
145  bool doNotTrack, const vpImage<bool> *mask)
146 {
147  if (isvisible) {
148  // Perspective projection
149  circle->changeFrame(cMo);
150 
151  try {
152  circle->projection();
153  }
154  catch (...) {
155  std::cout << "Problem when projecting circle\n";
156  return false;
157  }
158 
159  // Create the moving edges containers
160  meEllipse = new vpMbtMeEllipse;
161  meEllipse->setMask(*mask);
162  meEllipse->setMe(me);
163 
164  // meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
165  meEllipse->setInitRange(me->getRange()); // TODO: check because set to zero for lines
166 
167  try {
168  vpImagePoint ic;
169  double n20_p, n11_p, n02_p;
170  vpMeterPixelConversion::convertEllipse(cam, *circle, ic, n20_p, n11_p, n02_p);
171  meEllipse->initTracking(I, ic, n20_p, n11_p, n02_p, doNotTrack);
172  }
173  catch (...) {
174  // vpTRACE("the circle can't be initialized");
175  return false;
176  }
177  }
178  return true;
179 }
180 
188 {
189  if (isvisible) {
190  try {
191  meEllipse->track(I);
192  }
193  catch (...) {
194  // std::cout << "Track meEllipse failed" << std::endl;
195  meEllipse->reset();
196  Reinit = true;
197  }
198 
199  // Update the number of features
200  nbFeature = (unsigned int)meEllipse->getMeList().size();
201  }
202 }
203 
213 {
214  if (isvisible) {
215  // Perspective projection
216  circle->changeFrame(cMo);
217 
218  try {
219  circle->projection();
220  }
221  catch (...) {
222  std::cout << "Problem when projecting circle\n";
223  }
224 
225  try {
226 
227  vpImagePoint ic;
228  double n20_p, n11_p, n02_p;
229  vpMeterPixelConversion::convertEllipse(cam, *circle, ic, n20_p, n11_p, n02_p);
230  meEllipse->updateParameters(I, ic, n20_p, n11_p, n02_p);
231  }
232  catch (...) {
233  Reinit = true;
234  }
235  nbFeature = (unsigned int)meEllipse->getMeList().size();
236  }
237 }
238 
251  const vpImage<bool> *mask)
252 {
253  if (meEllipse != nullptr)
254  delete meEllipse;
255 
256  meEllipse = nullptr;
257 
258  if (!initMovingEdge(I, cMo, false, mask))
259  Reinit = true;
260 
261  Reinit = false;
262 }
263 
276  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
277  bool displayFullModel)
278 {
279  std::vector<double> params = getModelForDisplay(cMo, camera, displayFullModel);
280 
281  vpImagePoint center(params[0], params[1]);
282  double n20_p = params[2];
283  double n11_p = params[3];
284  double n02_p = params[4];
285  vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, col, thickness);
286 }
287 
300  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
301  bool displayFullModel)
302 {
303  std::vector<double> params = getModelForDisplay(cMo, camera, displayFullModel);
304 
305  vpImagePoint center(params[1], params[2]);
306  double n20_p = params[3];
307  double n11_p = params[4];
308  double n02_p = params[5];
309  vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, col, thickness);
310 }
311 
316 std::vector<std::vector<double> > vpMbtDistanceCircle::getFeaturesForDisplay()
317 {
318  std::vector<std::vector<double> > features;
319 
320  if (meEllipse != nullptr) {
321  for (std::list<vpMeSite>::const_iterator it = meEllipse->getMeList().begin(); it != meEllipse->getMeList().end();
322  ++it) {
323  vpMeSite p_me = *it;
324 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
325  std::vector<double> params = { 0, //ME
326  p_me.get_ifloat(),
327  p_me.get_jfloat(),
328  static_cast<double>(p_me.getState()) };
329 #else
330  std::vector<double> params;
331  params.push_back(0); //ME
332  params.push_back(p_me.get_ifloat());
333  params.push_back(p_me.get_jfloat());
334  params.push_back(static_cast<double>(p_me.getState()));
335 #endif
336 
337  features.push_back(params);
338  }
339  }
340 
341  return features;
342 }
343 
356  const vpCameraParameters &camera, bool displayFullModel)
357 {
358  std::vector<double> params;
359 
360  if ((isvisible && isTrackedCircle) || displayFullModel) {
361  // Perspective projection
362  circle->changeFrame(cMo);
363 
364  try {
365  circle->projection();
366  }
367  catch (...) {
368  std::cout << "Cannot project the circle";
369  }
370 
371  vpImagePoint center;
372  double n20_p, n11_p, n02_p;
373  vpMeterPixelConversion::convertEllipse(camera, *circle, center, n20_p, n11_p, n02_p);
374  params.push_back(1); // 1 for ellipse parameters
375  params.push_back(center.get_i());
376  params.push_back(center.get_j());
377  params.push_back(n20_p);
378  params.push_back(n11_p);
379  params.push_back(n02_p);
380  }
381 
382  return params;
383 }
384 
397 {
398  if (meEllipse != nullptr) {
399  meEllipse->display(I); // display the me
400  if (vpDEBUG_ENABLE(3))
401  vpDisplay::flush(I);
402  }
403 }
404 
406 {
407  if (meEllipse != nullptr) {
408  meEllipse->display(I); // display the me
409  if (vpDEBUG_ENABLE(3))
410  vpDisplay::flush(I);
411  }
412 }
413 
418 {
419  if (isvisible) {
420  nbFeature = (unsigned int)meEllipse->getMeList().size();
421  L.resize(nbFeature, 6);
423  }
424  else
425  nbFeature = 0;
426 }
427 
433 {
434  if (isvisible) {
435  // Perspective projection
436  circle->changeFrame(cMo);
437  try {
438  circle->projection();
439  }
440  catch (...) {
441  std::cout << "Problem projection circle\n";
442  }
443 
444  vpFeatureBuilder::create(featureEllipse, *circle);
445 
446  vpMatrix H1 = featureEllipse.interaction();
447 
448  vpRowVector H(5);
449  double x = 0, y = 0;
450 
451  // Get the parameters of the ellipse in the image plane
452  double xg = circle->p[0];
453  double yg = circle->p[1];
454  double n20 = circle->p[2];
455  double n11 = circle->p[3];
456  double n02 = circle->p[4];
457 
458  unsigned int j = 0;
459 
460  for (std::list<vpMeSite>::const_iterator it = meEllipse->getMeList().begin(); it != meEllipse->getMeList().end();
461  ++it) {
462  vpPixelMeterConversion::convertPoint(cam, it->m_j, it->m_i, x, y);
463  // TRO Chaumette 2004 eq 25
464  H[0] = 2 * (n11 * (y - yg) + n02 * (xg - x));
465  H[1] = 2 * (n20 * (yg - y) + n11 * (x - xg));
466  H[2] = vpMath::sqr(y - yg) - 4.0 * n02;
467  H[3] = 2 * (yg * (x - xg) + y * xg + 4.0 * n11 - x * y);
468  H[4] = vpMath::sqr(x - xg) - 4.0 * n20;
469 
470  for (unsigned int k = 0; k < 6; k++)
471  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];
472 
473  error[j] = n02 * vpMath::sqr(x) + n20 * vpMath::sqr(y) - 2 * n11 * x * y + 2 * (n11 * yg - n02 * xg) * x +
474  2 * (n11 * xg - n20 * yg) * y + n02 * vpMath::sqr(xg) + n20 * vpMath::sqr(yg) - 2 * n11 * xg * yg +
475  4.0 * vpMath::sqr(n11) - 4.0 * n20 * n02;
476  j++;
477  }
478  }
479 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:86
void projection() vp_override
Definition: vpCircle.cpp:137
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpCircle.cpp:57
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const vp_override
Definition: vpCircle.cpp:246
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void flush(const vpImage< unsigned char > &I)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
compute the interaction matrix from a subset a the possible features
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_i() const
Definition: vpImagePoint.h:114
static double sqr(double x)
Definition: vpMath.h:201
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getFeaturesForDisplay()
vpColVector error
The error vector.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
vpMatrix L
The interaction matrix.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpCircle * circle
The circle to track.
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
bool isvisible
Indicates if the circle is visible or not.
void displayMovingEdges(const vpImage< unsigned char > &I)
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
bool Reinit
Indicates if the circle has to be reinitialized.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the circle.
vpPoint * p3
An other point on the plane containing the circle.
std::vector< double > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
vpMbtMeEllipse * meEllipse
The moving edge containers.
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
vpMeSiteState getState() const
Definition: vpMeSite.h:261
double get_ifloat() const
Definition: vpMeSite.h:192
double get_jfloat() const
Definition: vpMeSite.h:198
Definition: vpMe.h:124
unsigned int getRange() const
Definition: vpMe.h:282
static void convertEllipse(const vpCameraParameters &cam, const vpSphere &sphere, vpImagePoint &center_p, double &n20_p, double &n11_p, double &n02_p)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:54
@ object_frame
Definition: vpPlane.h:65
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
double getB() const
Definition: vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:450
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:454
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:452
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:107
vpColVector p
Definition: vpTracker.h:67
#define vpDEBUG_ENABLE(level)
Definition: vpDebug.h:524