Visual Servoing Platform version 3.5.0
vpMeSite.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 * Moving edges.
33 *
34 * Authors:
35 * Eric Marchand
36 * Andrew Comport
37 * Aurelien Yol
38 *
39 *****************************************************************************/
40
46#include <cmath> // std::fabs
47#include <limits> // numeric_limits
48#include <stdlib.h>
49#include <visp3/core/vpTrackingException.h>
50#include <visp3/me/vpMe.h>
51#include <visp3/me/vpMeSite.h>
52
53#ifndef DOXYGEN_SHOULD_SKIP_THIS
54static bool horsImage(int i, int j, int half, int rows, int cols)
55{
56 // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
57 // > (cols - half - 3) )) ;
58 int half_1 = half + 1;
59 int half_3 = half + 3;
60 // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
61 // > (cols - half - 3) )) ;
62 return ((0 < (half_1 - i)) || ((i - rows + half_3) > 0) || (0 < (half_1 - j)) || ((j - cols + half_3) > 0));
63}
64#endif
65
67{
68 // Site components
69 alpha = 0.0;
70 convlt = 0.0;
71 weight = -1;
72
73 selectDisplay = NONE;
74
75 // Pixel components
76 i = 0;
77 j = 0;
78 v = 0;
79 ifloat = i;
80 jfloat = j;
81 i_1 = 0;
82 j_1 = 0;
83
84 mask_sign = 1;
85
86 normGradient = 0;
87
88 state = NO_SUPPRESSION;
89
90#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
91 suppress = 0;
92#endif
93}
94
96 : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
97 weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
98#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
99 ,
100 suppress(0)
101#endif
102{
103}
104
105vpMeSite::vpMeSite(double ip, double jp)
106 : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
107 weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
108#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
109 ,
110 suppress(0)
111#endif
112{
113 i = vpMath::round(ip);
114 j = vpMath::round(jp);
115 ifloat = ip;
116 jfloat = jp;
117}
118
123 : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
124 weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
125#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
126 ,
127 suppress(0)
128#endif
129{
130 *this = mesite;
131}
132
133// More an Update than init
134// For points in meter form (to avoid approximations)
135void vpMeSite::init(double ip, double jp, double alphap)
136{
137 // Note: keep old value of convlt, suppress and contrast
138 selectDisplay = NONE;
139
140 ifloat = ip;
141 i = vpMath::round(ip);
142 jfloat = jp;
143 j = vpMath::round(jp);
144 alpha = alphap;
145 mask_sign = 1;
146
147 v = 0;
148 i_1 = 0;
149 j_1 = 0;
150}
151
152// initialise with convolution
153void vpMeSite::init(double ip, double jp, double alphap, double convltp)
154{
155 selectDisplay = NONE;
156 ifloat = ip;
157 i = (int)ip;
158 jfloat = jp;
159 j = (int)jp;
160 alpha = alphap;
161 convlt = convltp;
162 mask_sign = 1;
163
164 v = 0;
165 i_1 = 0;
166 j_1 = 0;
167}
168// initialise with convolution and sign
169void vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
170{
171 selectDisplay = NONE;
172 ifloat = ip;
173 i = (int)ip;
174 jfloat = jp;
175 j = (int)jp;
176 alpha = alphap;
177 convlt = convltp;
178 mask_sign = sign;
179
180 v = 0;
181 i_1 = 0;
182 j_1 = 0;
183}
184
186{
187 i = m.i;
188 j = m.j;
189 i_1 = m.i_1;
190 j_1 = m.j_1;
191 ifloat = m.ifloat;
192 jfloat = m.jfloat;
193 v = m.v;
195 alpha = m.alpha;
196 convlt = m.convlt;
198 weight = m.weight;
199 selectDisplay = m.selectDisplay;
200 state = m.state;
201
202#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
203 suppress = m.suppress;
204#endif
205
206 return *this;
207}
208
209// ===================================================================
217// ===================================================================
218
220{
221 unsigned int range_ = static_cast<unsigned int>(range);
222 // Size of query list includes the point on the line
223 vpMeSite *list_query_pixels = new vpMeSite[2 * range_ + 1];
224
225 // range : +/- the range within which the pixel's
226 // correspondent will be sought
227
228 double salpha = sin(alpha);
229 double calpha = cos(alpha);
230 int n = 0;
231 vpImagePoint ip;
232
233 for (int k = -range; k <= range; k++) {
234 double ii = (ifloat + k * salpha);
235 double jj = (jfloat + k * calpha);
236
237 // Display
238 if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RANGE)) {
239 ip.set_i(ii);
240 ip.set_j(jj);
242 }
243
244 // Copy parent's convolution
245 vpMeSite pel;
246 pel.init(ii, jj, alpha, convlt, mask_sign);
247 pel.setDisplay(selectDisplay); // Display
248
249 // Add site to the query list
250 list_query_pixels[n] = pel;
251 n++;
252 }
253
254 return (list_query_pixels);
255}
256
257#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
258// ===================================================================
266// ===================================================================
267void vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
268{
269
270 int k;
271
272 double salpha = sin(alpha);
273 double calpha = cos(alpha);
274
275 // First extremity
276 k = -range;
277 unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
278 unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
279
280 // Second extremity
281 k = range;
282 unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
283 unsigned int j2 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
284
285 // TODO: Here check if i1,j1,i2,j2 > 0 else ??
286 if (I[i1][j1] > I[i2][j2])
287 mask_sign = 1;
288 else
289 mask_sign = -1;
290}
291#endif
292
293// Specific function for ME
295{
296 int half;
297 int height_ = static_cast<int>(I.getHeight());
298 int width_ = static_cast<int>(I.getWidth());
299
300 double conv = 0.0;
301 unsigned int msize = me->getMaskSize();
302 half = (static_cast<int>(msize) - 1) >> 1;
303
304 if (horsImage(i, j, half + me->getStrip(), height_, width_)) {
305 conv = 0.0;
306 i = 0;
307 j = 0;
308 } else {
309 // Calculate tangent angle from normal
310 double theta = alpha + M_PI / 2;
311 // Move tangent angle to within 0->M_PI for a positive
312 // mask index
313 while (theta < 0)
314 theta += M_PI;
315 while (theta > M_PI)
316 theta -= M_PI;
317
318 // Convert radians to degrees
319 int thetadeg = vpMath::round(theta * 180 / M_PI);
320
321 if (abs(thetadeg) == 180) {
322 thetadeg = 0;
323 }
324
325 unsigned int index_mask = (unsigned int)(thetadeg / (double)me->getAngleStep());
326
327 unsigned int i_ = static_cast<unsigned int>(i);
328 unsigned int j_ = static_cast<unsigned int>(j);
329 unsigned int half_ = static_cast<unsigned int>(half);
330
331 unsigned int ihalf = i_ - half_;
332 unsigned int jhalf = j_ - half_;
333
334 for (unsigned int a = 0; a < msize; a++) {
335 unsigned int ihalfa = ihalf + a;
336 for (unsigned int b = 0; b < msize; b++) {
337 conv += mask_sign * me->getMask()[index_mask][a][b] *
338 // I(i-half+a,j-half+b) ;
339 I(ihalfa, jhalf + b);
340 }
341 }
342 }
343
344 return (conv);
345}
346
355void vpMeSite::track(const vpImage<unsigned char> &I, const vpMe *me, bool test_contraste)
356{
357 // vpMeSite *list_query_pixels ;
358 // int max_rank =0 ;
359 // int max_rank1=0 ;
360 // int max_rank2 = 0;
361 // double convolution = 0 ;
362 // double max_convolution = 0 ;
363 // double max = 0 ;
364 // double contraste = 0;
365 // // vpDisplay::display(I) ;
366 // // vpERROR_TRACE("getclcik %d",me->getRange()) ;
367 // // vpDisplay::getClick(I) ;
368 //
369 // // range = +/- range of pixels within which the correspondent
370 // // of the current pixel will be sought
371 // int range = me->getRange() ;
372 //
373 // // std::cout << i << " " << j<<" " << range << " " << suppress <<
374 // std::endl ; list_query_pixels = getQueryList(I, range) ;
375 //
376 // double contraste_max = 1 + me->getMu2();
377 // double contraste_min = 1 - me->getMu1();
378 //
379 // // array in which likelihood ratios will be stored
380 // double *likelihood= new double[ 2 * range + 1 ] ;
381 //
382 // int ii_1 = i ;
383 // int jj_1 = j ;
384 // i_1 = i ;
385 // j_1 = j ;
386 // double threshold;
387 // threshold = me->getThreshold();
388 //
389 // // std::cout <<"---------------------"<<std::endl ;
390 // for(int n = 0 ; n < 2 * range + 1 ; n++)
391 // {
392 //
393 // // convolution results
394 // convolution = list_query_pixels[n].convolution(I, me) ;
395 //
396 // // luminance ratio of reference pixel to potential correspondent
397 // pixel
398 // // the luminance must be similar, hence the ratio value should
399 // // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
400 // if( test_contraste )
401 // {
402 // // Include this to eliminate temporal calculation
403 // if (convlt==0)
404 // {
405 // std::cout << "vpMeSite::track : Division by zero convlt = 0" <<
406 // std::endl ; delete []list_query_pixels ; delete
407 // []likelihood;
408 // throw(vpTrackingException(vpTrackingException::initializationError,
409 // "Division by zero")) ;
410 // }
411 //
412 // // contraste = fabs(convolution / convlt) ;
413 // contraste = convolution / convlt ;
414 // // likelihood ratios
415 // if((contraste > contraste_min) && (contraste < contraste_max))
416 // likelihood[n] = fabs(convolution + convlt ) ;
417 // else
418 // likelihood[n] = 0 ;
419 // }
420 // else
421 // likelihood[n] = fabs(2*convolution) ;
422 //
423 //
424 // // establishment of the maximal likelihood ratios's rank
425 // // in the array, the value of the likelihood ratio can now be
426 // // referenced by its rank in the array
427 // if (likelihood[n] > max)
428 // {
429 // max_convolution= convolution;
430 // max = likelihood[n] ;
431 // max_rank = n ;
432 // max_rank2 = max_rank1;
433 // max_rank1 = max_rank;
434 // }
435 //
436 // }
437 //
438 // // test on the likelihood threshold if threshold==-1 then
439 // // the me->threshold is selected
440 //
441 // vpImagePoint ip;
442 //
443 // // if (test_contrast)
444 // if(max > threshold)
445 // {
446 // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
447 // {
448 // ip.set_i( list_query_pixels[max_rank].i );
449 // ip.set_j( list_query_pixels[max_rank].j );
450 // vpDisplay::displayPoint(I, ip, vpColor::red);
451 // }
452 //
453 // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by
454 // the vpMeSite of max likelihood normGradient =
455 // vpMath::sqr(max_convolution);
456 //
457 // convlt = max_convolution;
458 // i_1 = ii_1; //list_query_pixels[max_rank].i ;
459 // j_1 = jj_1; //list_query_pixels[max_rank].j ;
460 // delete []list_query_pixels ;
461 // delete []likelihood;
462 // }
463 // else //none of the query sites is better than the threshold
464 // {
465 // if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
466 // {
467 // ip.set_i( list_query_pixels[max_rank].i );
468 // ip.set_j( list_query_pixels[max_rank].j );
469 // vpDisplay::displayPoint(I, ip, vpColor::green);
470 // }
471 // normGradient = 0 ;
472 // if(max == 0)
473 // suppress = 1; // contrast suppression
474 // else
475 // suppress = 2; // threshold suppression
476 //
477 // delete []list_query_pixels ;
478 // delete []likelihood; // modif portage
479 // }
480
481 int max_rank = -1;
482 // int max_rank1=-1 ;
483 // int max_rank2 = -1;
484 double max_convolution = 0;
485 double max = 0;
486 double contraste = 0;
487 // vpDisplay::display(I) ;
488 // vpERROR_TRACE("getclcik %d",me->range) ;
489 // vpDisplay::getClick(I) ;
490
491 // range = +/- range of pixels within which the correspondent
492 // of the current pixel will be sought
493 unsigned int range = me->getRange();
494
495 // std::cout << i << " " << j<<" " << range << " " << suppress <<
496 // std::endl ;
497 vpMeSite *list_query_pixels = getQueryList(I, (int)range);
498
499 double contraste_max = 1 + me->getMu2();
500 double contraste_min = 1 - me->getMu1();
501
502 // array in which likelihood ratios will be stored
503 double *likelihood = new double[2 * range + 1];
504
505 int ii_1 = i;
506 int jj_1 = j;
507 i_1 = i;
508 j_1 = j;
509 double threshold;
510 threshold = me->getThreshold();
511 double diff = 1e6;
512
513 // std::cout <<"---------------------"<<std::endl ;
514 for (unsigned int n = 0; n < 2 * range + 1; n++) {
515 // convolution results
516 double convolution_ = list_query_pixels[n].convolution(I, me);
517
518 // luminance ratio of reference pixel to potential correspondent pixel
519 // the luminance must be similar, hence the ratio value should
520 // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
521 if (test_contraste) {
522 likelihood[n] = fabs(convolution_ + convlt);
523 if (likelihood[n] > threshold) {
524 contraste = convolution_ / convlt;
525 if ((contraste > contraste_min) && (contraste < contraste_max) && fabs(1 - contraste) < diff) {
526 diff = fabs(1 - contraste);
527 max_convolution = convolution_;
528 max = likelihood[n];
529 max_rank = (int)n;
530 // max_rank2 = max_rank1;
531 // max_rank1 = max_rank;
532 }
533 }
534 }
535
536 else {
537 likelihood[n] = fabs(2 * convolution_);
538 if (likelihood[n] > max && likelihood[n] > threshold) {
539 max_convolution = convolution_;
540 max = likelihood[n];
541 max_rank = (int)n;
542 // max_rank2 = max_rank1;
543 // max_rank1 = max_rank;
544 }
545 }
546 }
547
548 // test on the likelihood threshold if threshold==-1 then
549 // the me->threshold is selected
550
551 vpImagePoint ip;
552
553 // if (test_contrast)
554 if (max_rank >= 0) {
555 if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
556 ip.set_i(list_query_pixels[max_rank].i);
557 ip.set_j(list_query_pixels[max_rank].j);
559 }
560
561 *this = list_query_pixels[max_rank]; // The vpMeSite2 is replaced by the
562 // vpMeSite2 of max likelihood
563 normGradient = vpMath::sqr(max_convolution);
564
565 convlt = max_convolution;
566 i_1 = ii_1; // list_query_pixels[max_rank].i ;
567 j_1 = jj_1; // list_query_pixels[max_rank].j ;
568 delete[] list_query_pixels;
569 delete[] likelihood;
570 } else // none of the query sites is better than the threshold
571 {
572 if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
573 ip.set_i(list_query_pixels[0].i);
574 ip.set_j(list_query_pixels[0].j);
576 }
577 normGradient = 0;
578 // if(contraste != 0)
579 if (std::fabs(contraste) > std::numeric_limits<double>::epsilon())
580 state = CONSTRAST; // contrast suppression
581 else
582 state = THRESHOLD; // threshold suppression
583
584 delete[] list_query_pixels;
585 delete[] likelihood; // modif portage
586 }
587}
588
589int vpMeSite::operator!=(const vpMeSite &m) { return ((m.i != i) || (m.j != j)); }
590
591VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS)
592{
593#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
594 return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Flag: " << vpMeS.suppress
595 << " Weight: " << vpMeS.weight);
596#else
597 return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Weight: " << vpMeS.weight);
598#endif
599}
600
602{
603 vpMeSite::display(I, ifloat, jfloat, state);
604}
605
607{
608 vpMeSite::display(I, ifloat, jfloat, state);
609}
610
611// Static functions
612
631void vpMeSite::display(const vpImage<unsigned char> &I, const double &i, const double &j, const vpMeSiteState &state)
632{
633 switch (state) {
634 case NO_SUPPRESSION:
636 break;
637
638 case CONSTRAST:
640 break;
641
642 case THRESHOLD:
644 break;
645
646 case M_ESTIMATOR:
648 break;
649
650 case TOO_NEAR:
652 break;
653
654 default:
656 }
657}
658
677void vpMeSite::display(const vpImage<vpRGBa> &I, const double &i, const double &j, const vpMeSiteState &state)
678{
679 switch (state) {
680 case NO_SUPPRESSION:
682 break;
683
684 case CONSTRAST:
686 break;
687
688 case THRESHOLD:
690 break;
691
692 case M_ESTIMATOR:
694 break;
695
696 case TOO_NEAR:
698 break;
699
700 default:
702 }
703}
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor yellow
Definition: vpColor.h:225
static const vpColor green
Definition: vpColor.h:220
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_j(double jj)
Definition: vpImagePoint.h:177
void set_i(double ii)
Definition: vpImagePoint.h:166
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double sqr(double x)
Definition: vpMath.h:116
static int round(double x)
Definition: vpMath.h:247
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
int j
Definition: vpMeSite.h:87
vpMeSiteState
Moving-edge site state.
Definition: vpMeSite.h:77
@ CONSTRAST
Point removed due to a contrast problem.
Definition: vpMeSite.h:79
@ TOO_NEAR
Point removed because too near image borders.
Definition: vpMeSite.h:82
@ THRESHOLD
Point removed due to a threshold problem.
Definition: vpMeSite.h:80
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
void setDisplay(vpMeSiteDisplayType select)
Definition: vpMeSite.h:139
double weight
Definition: vpMeSite.h:99
int operator!=(const vpMeSite &m)
Definition: vpMeSite.cpp:589
void display(const vpImage< unsigned char > &I)
Definition: vpMeSite.cpp:601
void track(const vpImage< unsigned char > &im, const vpMe *me, bool test_contraste=true)
Definition: vpMeSite.cpp:355
double ifloat
Definition: vpMeSite.h:89
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:294
@ NONE
Definition: vpMeSite.h:74
@ RANGE_RESULT
Definition: vpMeSite.h:74
@ RESULT
Definition: vpMeSite.h:74
@ RANGE
Definition: vpMeSite.h:74
int i
Definition: vpMeSite.h:87
int mask_sign
Definition: vpMeSite.h:91
vpMeSite & operator=(const vpMeSite &m)
Definition: vpMeSite.cpp:185
int suppress
Definition: vpMeSite.h:249
vp_deprecated void getSign(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:267
int j_1
Definition: vpMeSite.h:88
double normGradient
Definition: vpMeSite.h:97
double alpha
Definition: vpMeSite.h:93
void init()
Definition: vpMeSite.cpp:66
double jfloat
Definition: vpMeSite.h:89
vpMeSite * getQueryList(const vpImage< unsigned char > &I, const int range)
Definition: vpMeSite.cpp:219
double convlt
Definition: vpMeSite.h:95
unsigned char v
Definition: vpMeSite.h:90
int i_1
Definition: vpMeSite.h:88
vpMeSite()
Definition: vpMeSite.cpp:95
Definition: vpMe.h:61
unsigned int getAngleStep() const
Definition: vpMe.h:114
vpMatrix * getMask() const
Definition: vpMe.h:120
double getMu1() const
Definition: vpMe.h:155
int getStrip() const
Definition: vpMe.h:185
double getMu2() const
Definition: vpMe.h:161
double getThreshold() const
Definition: vpMe.h:193
unsigned int getMaskSize() const
Definition: vpMe.h:142
unsigned int getRange() const
Definition: vpMe.h:179