Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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(NULL), wmean(1), featureEllipse(), isTrackedCircle(true), meEllipse(NULL), circle(NULL),
60 radius(0.), p1(NULL), p2(NULL), p3(NULL), L(), error(), nbFeature(0), Reinit(false), hiddenface(NULL),
61 index_polygon(-1), isvisible(false)
62{
63}
64
69{
70 if (meEllipse != NULL)
71 delete meEllipse;
72 if (circle != NULL)
73 delete circle;
74 if (p1 != NULL)
75 delete p1;
76 if (p2 != NULL)
77 delete p2;
78 if (p3 != NULL)
79 delete p3;
80}
81
88void vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo) { circle->project(cMo); }
89
100void vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
101{
102 circle = new vpCircle;
103 p1 = new vpPoint;
104 p2 = new vpPoint;
105 p3 = new vpPoint;
106
107 // Get the points
108 *p1 = _p1;
109 *p2 = _p2;
110 *p3 = _p3;
111
112 // Get the radius
113 radius = r;
114
115 vpPlane plane(*p1, *p2, *p3, vpPlane::object_frame);
116
117 // Build our circle
118 circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
119}
120
127{
128 me = _me;
129 if (meEllipse != NULL) {
130 meEllipse->setMe(me);
131 }
132}
133
146 bool doNotTrack, const vpImage<bool> *mask)
147{
148 if (isvisible) {
149 // Perspective projection
150 circle->changeFrame(cMo);
151
152 try {
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 } catch (...) {
173 // vpTRACE("the circle can't be initialized");
174 return false;
175 }
176 }
177 return true;
178}
179
187{
188 if (isvisible) {
189 try {
190 meEllipse->track(I);
191 } catch (...) {
192 // std::cout << "Track meEllipse failed" << std::endl;
193 meEllipse->reset();
194 Reinit = true;
195 }
196
197 // Update the number of features
198 nbFeature = (unsigned int)meEllipse->getMeList().size();
199 }
200}
201
211{
212 if (isvisible) {
213 // Perspective projection
214 circle->changeFrame(cMo);
215
216 try {
218 } catch (...) {
219 std::cout << "Problem when projecting circle\n";
220 }
221
222 try {
223
224 vpImagePoint ic;
225 double n20_p, n11_p, n02_p;
226 vpMeterPixelConversion::convertEllipse(cam, *circle, ic, n20_p, n11_p, n02_p);
227 meEllipse->updateParameters(I, ic, n20_p, n11_p, n02_p);
228 } catch (...) {
229 Reinit = true;
230 }
231 nbFeature = (unsigned int)meEllipse->getMeList().size();
232 }
233}
234
247 const vpImage<bool> *mask)
248{
249 if (meEllipse != NULL)
250 delete meEllipse;
251
252 meEllipse = NULL;
253
254 if (!initMovingEdge(I, cMo, false, mask))
255 Reinit = true;
256
257 Reinit = false;
258}
259
272 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
273 bool displayFullModel)
274{
275 std::vector<double> params = getModelForDisplay(cMo, camera, displayFullModel);
276
277 vpImagePoint center(params[0], params[1]);
278 double n20_p = params[2];
279 double n11_p = params[3];
280 double n02_p = params[4];
281 vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, col, thickness);
282}
283
296 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
297 bool displayFullModel)
298{
299 std::vector<double> params = getModelForDisplay(cMo, camera, displayFullModel);
300
301 vpImagePoint center(params[1], params[2]);
302 double n20_p = params[3];
303 double n11_p = params[4];
304 double n02_p = params[5];
305 vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, col, thickness);
306}
307
312std::vector<std::vector<double> > vpMbtDistanceCircle::getFeaturesForDisplay()
313{
314 std::vector<std::vector<double> > features;
315
316 if (meEllipse != NULL) {
317 for (std::list<vpMeSite>::const_iterator it = meEllipse->getMeList().begin(); it != meEllipse->getMeList().end();
318 ++it) {
319 vpMeSite p_me = *it;
320#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
321 std::vector<double> params = {0, // ME
322 p_me.get_ifloat(), p_me.get_jfloat(), static_cast<double>(p_me.getState())};
323#else
324 std::vector<double> params;
325 params.push_back(0); // ME
326 params.push_back(p_me.get_ifloat());
327 params.push_back(p_me.get_jfloat());
328 params.push_back(static_cast<double>(p_me.getState()));
329#endif
330 features.push_back(params);
331 }
332 }
333
334 return features;
335}
336
349 const vpCameraParameters &camera, bool displayFullModel)
350{
351 std::vector<double> params;
352
353 if ((isvisible && isTrackedCircle) || displayFullModel) {
354 // Perspective projection
355 circle->changeFrame(cMo);
356
357 try {
359 } catch (...) {
360 std::cout << "Cannot project the circle";
361 }
362
363 vpImagePoint center;
364 double n20_p, n11_p, n02_p;
365 vpMeterPixelConversion::convertEllipse(camera, *circle, center, n20_p, n11_p, n02_p);
366 params.push_back(1); // 1 for ellipse parameters
367 params.push_back(center.get_i());
368 params.push_back(center.get_j());
369 params.push_back(n20_p);
370 params.push_back(n11_p);
371 params.push_back(n02_p);
372 }
373
374 return params;
375}
376
389{
390 if (meEllipse != NULL) {
391 meEllipse->display(I); // display the me
392 if (vpDEBUG_ENABLE(3))
394 }
395}
396
398{
399 if (meEllipse != NULL) {
400 meEllipse->display(I); // display the me
401 if (vpDEBUG_ENABLE(3))
403 }
404}
405
410{
411 if (isvisible) {
412 nbFeature = (unsigned int)meEllipse->getMeList().size();
413 L.resize(nbFeature, 6);
415 } else
416 nbFeature = 0;
417}
418
424{
425 if (isvisible) {
426 // Perspective projection
427 circle->changeFrame(cMo);
428 try {
430 } catch (...) {
431 std::cout << "Problem projection circle\n";
432 }
433
434 vpFeatureBuilder::create(featureEllipse, *circle);
435
436 vpMatrix H1 = featureEllipse.interaction();
437
438 vpRowVector H(5);
439 double x = 0, y = 0;
440
441 // Get the parameters of the ellipse in the image plane
442 double xg = circle->p[0];
443 double yg = circle->p[1];
444 double n20 = circle->p[2];
445 double n11 = circle->p[3];
446 double n02 = circle->p[4];
447
448 unsigned int j = 0;
449
450 for (std::list<vpMeSite>::const_iterator it = meEllipse->getMeList().begin(); it != meEllipse->getMeList().end();
451 ++it) {
452 vpPixelMeterConversion::convertPoint(cam, it->j, it->i, x, y);
453 // TRO Chaumette 2004 eq 25
454 H[0] = 2 * (n11 * (y - yg) + n02 * (xg - x));
455 H[1] = 2 * (n20 * (yg - y) + n11 * (x - xg));
456 H[2] = vpMath::sqr(y - yg) - 4.0 * n02;
457 H[3] = 2 * (yg * (x - xg) + y * xg + 4.0 * n11 - x * y);
458 H[4] = vpMath::sqr(x - xg) - 4.0 * n20;
459
460 for (unsigned int k = 0; k < 6; k++)
461 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];
462
463 error[j] = n02 * vpMath::sqr(x) + n20 * vpMath::sqr(y) - 2 * n11 * x * y + 2 * (n11 * yg - n02 * xg) * x +
464 2 * (n11 * xg - n20 * yg) * y + n02 * vpMath::sqr(xg) + n20 * vpMath::sqr(yg) - 2 * n11 * xg * yg +
465 4.0 * vpMath::sqr(n11) - 4.0 * n20 * n02;
466 j++;
467 }
468 }
469}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
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:87
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
Definition vpCircle.cpp:246
void setWorldCoordinates(const vpColVector &oP)
Definition vpCircle.cpp:57
void projection()
Definition vpCircle.cpp:137
void resize(unsigned int i, bool flagNullify=true)
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)
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 ...
double get_j() const
double get_i() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
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.
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.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
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:231
double get_jfloat() const
Definition vpMeSite.h:238
Definition vpMe.h:122
unsigned int getRange() const
Definition vpMe.h:273
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:458
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
Implementation of row vector and the associated operations.
vpColVector p
Definition vpTracker.h:68
#define vpDEBUG_ENABLE(level)
Definition vpDebug.h:533