51 #include <visp/vpDisplay.h>
54 #include <visp/vpTrackingException.h>
55 #include <visp/vpMath.h>
56 #include <visp/vpIoTools.h>
58 #include <visp/vpDot2.h>
85 grayLevelPrecision = 0.80;
89 ellipsoidShapePrecision = 0.65;
90 maxSizeSearchDistancePrecision = 0.65;
94 bbox_u_min = bbox_u_max = bbox_v_min = bbox_v_max = 0;
99 compute_moment = false ;
142 width = twinDot.width;
143 height = twinDot.height;
144 surface = twinDot.surface;
145 gray_level_min = twinDot.gray_level_min;
146 gray_level_max = twinDot.gray_level_max;
147 mean_gray_level = twinDot.mean_gray_level;
148 grayLevelPrecision = twinDot.grayLevelPrecision;
149 gamma = twinDot.gamma; ;
150 sizePrecision = twinDot.sizePrecision;
151 ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision ;
152 maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision;
153 allowedBadPointsPercentage_ = twinDot.allowedBadPointsPercentage_;
156 direction_list = twinDot.direction_list;
157 ip_edges_list = twinDot.ip_edges_list;
159 compute_moment = twinDot.compute_moment;
160 graphics = twinDot.graphics;
161 thickness = twinDot.thickness;
163 bbox_u_min = twinDot.bbox_u_min;
164 bbox_u_max = twinDot.bbox_u_max;
165 bbox_v_min = twinDot.bbox_v_min;
166 bbox_v_max = twinDot.bbox_v_max;
168 firstBorder_u = twinDot.firstBorder_u;
169 firstBorder_v = twinDot.firstBorder_v;
202 unsigned int thickness)
205 std::list<vpImagePoint>::const_iterator it;
207 for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
249 unsigned int i = (
unsigned int)cog.
get_i();
250 unsigned int j = (
unsigned int)cog.
get_j();
252 double Ip = pow((
double)I[i][j]/255,1/gamma);
254 if(Ip - (1 - grayLevelPrecision)<0){
258 gray_level_min = (
unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
259 if (gray_level_min > 255)
260 gray_level_min = 255;
262 gray_level_max = (
unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
263 if (gray_level_max > 255)
264 gray_level_max = 255;
311 unsigned int i = (
unsigned int)cog.
get_i();
312 unsigned int j = (
unsigned int)cog.
get_j();
314 double Ip = pow((
double)I[i][j]/255,1/gamma);
316 if(Ip - (1 - grayLevelPrecision)<0){
320 gray_level_min = (
unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
321 if (gray_level_min > 255)
322 gray_level_min = 255;
324 gray_level_max = (
unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
325 if (gray_level_max > 255)
326 gray_level_max = 255;
382 unsigned int gray_level_min,
383 unsigned int gray_level_max,
388 this->gray_level_min = gray_level_min;
389 this->gray_level_max = gray_level_max;
465 found = computeParameters(I, cog.
get_u(), cog.
get_v());
469 found = isValid( I, wantedDot);
488 double searchWindowWidth, searchWindowHeight;
490 if( std::fabs(
getWidth()) <= std::numeric_limits<double>::epsilon() || std::fabs(
getHeight()) <= std::numeric_limits<double>::epsilon() )
492 searchWindowWidth = 80.;
493 searchWindowHeight = 80.;
500 std::list<vpDot2> candidates;
502 (
int)(this->cog.
get_u()-searchWindowWidth /2.0),
503 (int)(this->cog.
get_v()-searchWindowHeight/2.0),
504 (
unsigned int)searchWindowWidth,
505 (
unsigned int)searchWindowHeight,
510 if( candidates.empty() )
514 "No dot was found")) ;
518 vpDot2 movingDot = candidates.front();
534 bbox_u_min = movingDot.bbox_u_min;
535 bbox_u_max = movingDot.bbox_u_max;
536 bbox_v_min = movingDot.bbox_v_min;
537 bbox_v_max = movingDot.bbox_v_max;
557 if( !isInImage( I ) )
559 vpERROR_TRACE(
"The center of gravity of the dot is not in the image") ;
561 "No dot was found")) ;
573 if(Ip - (1 - grayLevelPrecision)<0){
577 gray_level_min = (
unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
578 if (gray_level_min > 255)
579 gray_level_min = 255;
581 gray_level_max = (
unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
582 if (gray_level_max > 255)
583 gray_level_max = 255;
654 return fabs(surface);
663 return fabs(surface);
673 return grayLevelPrecision;
683 return sizePrecision;
695 return ellipsoidShapePrecision;
704 return maxSizeSearchDistancePrecision;
713 double diff_u = this->cog.
get_u() - cogDistantDot.
get_u();
714 double diff_v = this->cog.
get_v() - cogDistantDot.
get_v();
715 return sqrt( diff_u*diff_u + diff_v*diff_v );
748 this->height = height;
764 this->surface = surface;
777 this->surface = area;
798 double epsilon = 0.05;
799 if( grayLevelPrecision<epsilon )
801 this->grayLevelPrecision = epsilon;
803 else if( grayLevelPrecision>1 )
805 this->grayLevelPrecision = 1.0;
809 this->grayLevelPrecision = grayLevelPrecision;
828 if( sizePrecision<0 )
830 this->sizePrecision = 0;
832 else if( sizePrecision>1 )
834 this->sizePrecision = 1.0;
838 this->sizePrecision = sizePrecision;
873 if( ellipsoidShapePrecision<0 )
875 this->ellipsoidShapePrecision = 0;
877 else if( ellipsoidShapePrecision>1 )
879 this->ellipsoidShapePrecision = 1.0;
883 this->ellipsoidShapePrecision = ellipsoidShapePrecision;
902 double epsilon = 0.05;
903 if( maxSizeSearchDistancePrecision<epsilon )
905 this-> maxSizeSearchDistancePrecision = epsilon;
907 else if( maxSizeSearchDistancePrecision >1 )
909 this->maxSizeSearchDistancePrecision = 1.0;
913 this->maxSizeSearchDistancePrecision = maxSizeSearchDistancePrecision;
946 unsigned int w,
unsigned int h)
948 unsigned int image_w = I.
getWidth();
953 else if (u >= (
int)image_w) u = (int)image_w - 1;
955 else if (v >= (
int)image_h) v = (
int)image_h - 1;
957 if (((
unsigned int)u + w) > image_w) w = image_w - (
unsigned int)u - 1;
958 if (((
unsigned int)v + h) > image_h) h = image_h - (
unsigned int)v - 1;
978 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1047 unsigned int area_w = I.
getWidth();
1049 setArea(I, area_u, area_v, area_w, area_h);
1052 unsigned int gridWidth;
1053 unsigned int gridHeight;
1054 getGridSize( gridWidth, gridHeight );
1068 vpDot2* dotToTest = NULL;
1071 unsigned int area_u_min = (
unsigned int) area.
getLeft();
1072 unsigned int area_u_max = (
unsigned int) area.
getRight();
1073 unsigned int area_v_min = (
unsigned int) area.
getTop();
1074 unsigned int area_v_max = (
unsigned int) area.
getBottom();
1079 for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
1081 for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
1086 if( !hasGoodLevel(I, u, v) )
continue;
1090 bool good_germ =
true;
1091 niceDotsVector->
front();
1092 while( !niceDotsVector->
outside() && good_germ ==
true) {
1093 tmpDot = niceDotsVector->
value();
1095 cogTmpDot = tmpDot.
getCog();
1096 double u0 = cogTmpDot.
get_u();
1097 double v0 = cogTmpDot.
get_v();
1098 double half_w = tmpDot.
getWidth() / 2.;
1099 double half_h = tmpDot.
getHeight() / 2.;
1101 if ( u >= (u0-half_w) && u <= (u0+half_w) &&
1102 v >= (v0-half_h) && v <= (v0+half_h) ) {
1106 niceDotsVector->
next();
1113 unsigned int border_u;
1114 unsigned int border_v;
1115 if(findFirstBorder(I, u, v, border_u, border_v) ==
false){
1123 badDotsVector->
front();
1124 #define vpBAD_DOT_VALUE (badDotsVector->value())
1127 std::list<vpImagePoint>::const_iterator it_edges;
1128 while( !badDotsVector->
outside() && good_germ ==
true)
1130 if( (
double)u >= vpBAD_DOT_VALUE.bbox_u_min
1131 && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
1132 (
double)v >= vpBAD_DOT_VALUE.bbox_v_min
1133 && (double)v <= vpBAD_DOT_VALUE.bbox_v_max)
1136 it_edges = ip_edges_list.begin();
1137 while (it_edges != ip_edges_list.end() && good_germ ==
true)
1142 cogBadDot = *it_edges;
1144 if( (std::fabs(border_u - cogBadDot.
get_u()) <=
vpMath::maximum(std::fabs((
double)border_u), std::fabs(cogBadDot.
get_u()))*std::numeric_limits<double>::epsilon() )
1146 (std::fabs(v - cogBadDot.
get_v()) <=
vpMath::maximum(std::fabs((
double)v), std::fabs(cogBadDot.
get_v()))*std::numeric_limits<double>::epsilon() ))
1153 badDotsVector->
next();
1155 #undef vpBAD_DOT_VALUE
1164 vpTRACE(4,
"Try germ (%d, %d)", u, v);
1172 if( dotToTest != NULL )
delete dotToTest;
1173 dotToTest = getInstance();
1174 dotToTest->setCog( germ );
1179 dotToTest->setGraphics( graphics );
1180 dotToTest->setGraphicsThickness( thickness );
1181 dotToTest->setComputeMoments(
true );
1182 dotToTest->setArea( area );
1183 dotToTest->setEllipsoidShapePrecision( ellipsoidShapePrecision );
1188 if( dotToTest->computeParameters( I ) == false ) {
1195 if( dotToTest->isValid( I, *
this ) )
1203 double area_center_u = area_u + area_w/2.0 - 0.5;
1204 double area_center_v = area_v + area_h/2.0 - 0.5;
1206 double thisDiff_u = cogDotToTest.
get_u() - area_center_u;
1207 double thisDiff_v = cogDotToTest.
get_v() - area_center_v;
1208 double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
1210 bool stopLoop =
false;
1211 niceDotsVector->
front();
1213 while( !niceDotsVector->
outside() && stopLoop == false )
1218 double epsilon = 3.0;
1221 cogTmpDot = tmpDot.
getCog();
1223 if( fabs( cogTmpDot.
get_u() - cogDotToTest.
get_u() ) < epsilon &&
1224 fabs( cogTmpDot.
get_v() - cogDotToTest.
get_v() ) < epsilon )
1233 double otherDiff_u = cogTmpDot.
get_u() - area_center_u;
1234 double otherDiff_v = cogTmpDot.
get_v() - area_center_v;
1235 double otherDist = sqrt( otherDiff_u*otherDiff_u +
1236 otherDiff_v*otherDiff_v );
1242 if( otherDist > thisDist )
1244 niceDotsVector->
addLeft( *dotToTest );
1245 niceDotsVector->
next();
1252 niceDotsVector->
next();
1254 vpTRACE(4,
"End while (%d, %d)", u, v);
1258 if( niceDotsVector->
outside() && stopLoop == false )
1260 niceDotsVector->
end();
1261 niceDotsVector->
addRight( *dotToTest );
1266 badDotsVector->
front();
1267 badDotsVector->
addRight( *dotToTest );
1271 if( dotToTest != NULL )
delete dotToTest;
1273 delete badDotsVector;
1279 return niceDotsVector;
1308 unsigned int area_w,
1309 unsigned int area_h)
1314 setArea(I, area_u, area_v, area_w, area_h);
1317 unsigned int gridWidth;
1318 unsigned int gridHeight;
1319 getGridSize( gridWidth, gridHeight );
1333 vpDot2* dotToTest = NULL;
1336 unsigned int area_u_min = (
unsigned int) area.
getLeft();
1337 unsigned int area_u_max = (
unsigned int) area.
getRight();
1338 unsigned int area_v_min = (
unsigned int) area.
getTop();
1339 unsigned int area_v_max = (
unsigned int) area.
getBottom();
1344 for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
1346 for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
1351 if( !hasGoodLevel(I, u, v) )
continue;
1355 bool good_germ =
true;
1356 niceDotsVector->
front();
1357 while( !niceDotsVector->
outside() && good_germ ==
true) {
1358 tmpDot = niceDotsVector->
value();
1360 cogTmpDot = tmpDot.
getCog();
1361 double u0 = cogTmpDot.
get_u();
1362 double v0 = cogTmpDot.
get_v();
1363 double half_w = tmpDot.
getWidth() / 2.;
1364 double half_h = tmpDot.
getHeight() / 2.;
1366 if ( u >= (u0-half_w) && u <= (u0+half_w) &&
1367 v >= (v0-half_h) && v <= (v0+half_h) ) {
1371 niceDotsVector->
next();
1378 unsigned int border_u;
1379 unsigned int border_v;
1380 if(findFirstBorder(I, u, v, border_u, border_v) ==
false){
1388 badDotsVector->
front();
1389 #define vpBAD_DOT_VALUE (badDotsVector->value())
1392 std::list<vpImagePoint>::const_iterator it_edges;
1393 while( !badDotsVector->
outside() && good_germ ==
true)
1395 if( (
double)u >= vpBAD_DOT_VALUE.bbox_u_min
1396 && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
1397 (
double)v >= vpBAD_DOT_VALUE.bbox_v_min
1398 && (double)v <= vpBAD_DOT_VALUE.bbox_v_max)
1401 it_edges = ip_edges_list.begin();
1402 while (it_edges != ip_edges_list.end() && good_germ ==
true)
1407 cogBadDot = *it_edges;
1409 if( (std::fabs(border_u - cogBadDot.
get_u()) <=
vpMath::maximum(std::fabs((
double)border_u), std::fabs(cogBadDot.
get_u()))*std::numeric_limits<double>::epsilon() )
1411 (std::fabs(v - cogBadDot.
get_v()) <=
vpMath::maximum(std::fabs((
double)v), std::fabs(cogBadDot.
get_v()))*std::numeric_limits<double>::epsilon() ))
1418 badDotsVector->
next();
1420 #undef vpBAD_DOT_VALUE
1429 vpTRACE(4,
"Try germ (%d, %d)", u, v);
1437 if( dotToTest != NULL )
delete dotToTest;
1438 dotToTest = getInstance();
1439 dotToTest->setCog( germ );
1444 dotToTest->setGraphics( graphics );
1445 dotToTest->setGraphicsThickness( thickness );
1446 dotToTest->setComputeMoments(
true );
1447 dotToTest->setArea( area );
1448 dotToTest->setEllipsoidShapePrecision( ellipsoidShapePrecision );
1453 if( dotToTest->computeParameters( I ) == false ) {
1460 if( dotToTest->isValid( I, *
this ) )
1468 double area_center_u = area_u + area_w/2.0 - 0.5;
1469 double area_center_v = area_v + area_h/2.0 - 0.5;
1471 double thisDiff_u = cogDotToTest.
get_u() - area_center_u;
1472 double thisDiff_v = cogDotToTest.
get_v() - area_center_v;
1473 double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
1475 bool stopLoop =
false;
1476 niceDotsVector->
front();
1478 while( !niceDotsVector->
outside() && stopLoop == false )
1483 double epsilon = 3.0;
1486 cogTmpDot = tmpDot.
getCog();
1488 if( fabs( cogTmpDot.
get_u() - cogDotToTest.
get_u() ) < epsilon &&
1489 fabs( cogTmpDot.
get_v() - cogDotToTest.
get_v() ) < epsilon )
1498 double otherDiff_u = cogTmpDot.
get_u() - area_center_u;
1499 double otherDiff_v = cogTmpDot.
get_v() - area_center_v;
1500 double otherDist = sqrt( otherDiff_u*otherDiff_u +
1501 otherDiff_v*otherDiff_v );
1507 if( otherDist > thisDist )
1509 niceDotsVector->
addLeft( *dotToTest );
1510 niceDotsVector->
next();
1517 niceDotsVector->
next();
1519 vpTRACE(4,
"End while (%d, %d)", u, v);
1523 if( niceDotsVector->
outside() && stopLoop == false )
1525 niceDotsVector->
end();
1526 niceDotsVector->
addRight( *dotToTest );
1531 badDotsVector->
front();
1532 badDotsVector->
addRight( *dotToTest );
1536 if( dotToTest != NULL )
delete dotToTest;
1538 delete badDotsVector;
1540 return niceDotsVector;
1542 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS
1622 unsigned int area_w,
1623 unsigned int area_h,
1624 std::list<vpDot2> &niceDots)
1632 setArea(I, area_u, area_v, area_w, area_h);
1635 unsigned int gridWidth;
1636 unsigned int gridHeight;
1637 getGridSize( gridWidth, gridHeight );
1652 std::list<vpDot2> badDotsVector;
1653 std::list<vpDot2>::iterator itnice;
1654 std::list<vpDot2>::iterator itbad;
1656 vpDot2* dotToTest = NULL;
1659 unsigned int area_u_min = (
unsigned int) area.
getLeft();
1660 unsigned int area_u_max = (
unsigned int) area.
getRight();
1661 unsigned int area_v_min = (
unsigned int) area.
getTop();
1662 unsigned int area_v_max = (
unsigned int) area.
getBottom();
1667 for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
1669 for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
1674 if( !hasGoodLevel(I, u, v) )
continue;
1678 bool good_germ =
true;
1680 itnice = niceDots.begin();
1681 while( itnice != niceDots.end() && good_germ ==
true) {
1684 cogTmpDot = tmpDot.
getCog();
1685 double u0 = cogTmpDot.
get_u();
1686 double v0 = cogTmpDot.
get_v();
1687 double half_w = tmpDot.
getWidth() / 2.;
1688 double half_h = tmpDot.
getHeight() / 2.;
1690 if ( u >= (u0-half_w) && u <= (u0+half_w) &&
1691 v >= (v0-half_h) && v <= (v0+half_h) ) {
1702 unsigned int border_u;
1703 unsigned int border_v;
1704 if(findFirstBorder(I, u, v, border_u, border_v) ==
false){
1712 itbad = badDotsVector.begin();
1713 #define vpBAD_DOT_VALUE (*itbad)
1716 while( itbad != badDotsVector.end() && good_germ ==
true) {
1717 if( (
double)u >= vpBAD_DOT_VALUE.bbox_u_min
1718 && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
1719 (
double)v >= vpBAD_DOT_VALUE.bbox_v_min
1720 && (double)v <= vpBAD_DOT_VALUE.bbox_v_max){
1721 std::list<vpImagePoint>::const_iterator it_edges = ip_edges_list.begin();
1722 while (it_edges != ip_edges_list.end() && good_germ ==
true){
1726 cogBadDot = *it_edges;
1728 if( (std::fabs(border_u - cogBadDot.
get_u()) <=
vpMath::maximum(std::fabs((
double)border_u), std::fabs(cogBadDot.
get_u()))*std::numeric_limits<double>::epsilon() )
1730 (std::fabs(v - cogBadDot.
get_v()) <=
vpMath::maximum(std::fabs((
double)v), std::fabs(cogBadDot.
get_v()))*std::numeric_limits<double>::epsilon() )) {
1738 #undef vpBAD_DOT_VALUE
1747 vpTRACE(4,
"Try germ (%d, %d)", u, v);
1755 if( dotToTest != NULL )
delete dotToTest;
1756 dotToTest = getInstance();
1757 dotToTest->
setCog( germ );
1771 if( dotToTest->computeParameters( I ) == false ) {
1778 if( dotToTest->isValid( I, *
this ) )
1786 double area_center_u = area_u + area_w/2.0 - 0.5;
1787 double area_center_v = area_v + area_h/2.0 - 0.5;
1789 double thisDiff_u = cogDotToTest.
get_u() - area_center_u;
1790 double thisDiff_v = cogDotToTest.
get_v() - area_center_v;
1791 double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
1793 bool stopLoop =
false;
1794 itnice = niceDots.begin();
1796 while( itnice != niceDots.end() && stopLoop == false )
1801 double epsilon = 3.0;
1804 cogTmpDot = tmpDot.
getCog();
1806 if( fabs( cogTmpDot.
get_u() - cogDotToTest.
get_u() ) < epsilon &&
1807 fabs( cogTmpDot.
get_v() - cogDotToTest.
get_v() ) < epsilon )
1816 double otherDiff_u = cogTmpDot.
get_u() - area_center_u;
1817 double otherDiff_v = cogTmpDot.
get_v() - area_center_v;
1818 double otherDist = sqrt( otherDiff_u*otherDiff_u +
1819 otherDiff_v*otherDiff_v );
1825 if( otherDist > thisDist )
1827 niceDots.insert(itnice, *dotToTest );
1837 vpTRACE(4,
"End while (%d, %d)", u, v);
1841 if( itnice == niceDots.end() && stopLoop == false )
1843 niceDots.push_back( *dotToTest );
1848 badDotsVector.push_front( *dotToTest );
1852 if( dotToTest != NULL )
delete dotToTest;
1879 double epsilon = 0.001;
1888 if ( (std::fabs(wantedDot.
getWidth()) > std::numeric_limits<double>::epsilon())
1890 (std::fabs(wantedDot.
getHeight()) > std::numeric_limits<double>::epsilon())
1892 (std::fabs(wantedDot.
getArea()) > std::numeric_limits<double>::epsilon()) )
1894 if (std::fabs(sizePrecision) > std::numeric_limits<double>::epsilon()){
1896 std::cout <<
"test size precision......................\n";
1897 std::cout <<
"wanted dot: " <<
"w=" << wantedDot.
getWidth()
1899 <<
" s=" << wantedDot.
getArea()
1900 <<
" precision=" << sizePrecision
1901 <<
" epsilon=" << epsilon << std::endl;
1902 std::cout <<
"dot found: " <<
"w=" <<
getWidth()
1904 <<
" s=" <<
getArea() << std::endl;
1911 printf(
"Bad width > for dot (%g, %g)\n", cog.
get_u(), cog.
get_v());
1916 if( (
getWidth() < wantedDot.
getWidth()/(sizePrecision+epsilon ) )== false )
1921 printf(
"Bad width %g > %g for dot (%g, %g)\n",
1933 printf(
"Bad height %g > %g for dot (%g, %g)\n",
1945 printf(
"Bad height %g > %g for dot (%g, %g)\n",
1952 if( ( wantedDot.
getArea()*(sizePrecision*sizePrecision)-epsilon <
getArea() ) ==
false )
1957 printf(
"Bad surface %g > %g for dot (%g, %g)\n",
1958 wantedDot.
getArea()*(sizePrecision*sizePrecision)-epsilon,
1965 if( (
getArea() < wantedDot.
getArea()/(sizePrecision*sizePrecision+epsilon )) == false )
1970 printf(
"Bad surface %g < %g for dot (%g, %g)\n",
1971 getArea(), wantedDot.
getArea()/(sizePrecision*sizePrecision+epsilon),
1982 int nb_point_to_test = 20;
1983 int nb_bad_points = 0;
1984 int nb_max_bad_points = (int)(nb_point_to_test*allowedBadPointsPercentage_);
1985 double step_angle = 2*M_PI / nb_point_to_test;
1988 if (std::fabs(ellipsoidShapePrecision) > std::numeric_limits<double>::epsilon() && compute_moment) {
2007 double Sqrt = sqrt(tmp1*tmp1 + 4*tmp2*tmp2);
2018 double innerCoef = ellipsoidShapePrecision ;
2020 double cog_u = this->cog.
get_u();
2021 double cog_v = this->cog.
get_v();
2025 for(
double theta = 0. ; theta<2*M_PI ; theta+= step_angle ) {
2026 u = (
unsigned int) (cog_u + innerCoef*(a1*cos(alpha)*cos(theta)-a2*sin(alpha)*sin(theta)));
2027 v = (
unsigned int) (cog_v + innerCoef*(a1*sin(alpha)*cos(theta)+a2*cos(alpha)*sin(theta)));
2028 if( ! this->hasGoodLevel( I, u, v) ) {
2032 printf(
"Inner cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
2033 u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
2039 for (
unsigned int t=0; t< thickness; t++) {
2050 if (nb_bad_points > nb_max_bad_points)
2053 printf(
"Inner ellipse has %d bad points. Max allowed is %d\n",
2054 nb_bad_points, nb_max_bad_points);
2063 double outCoef = 2-ellipsoidShapePrecision;
2065 for(
double theta=0. ; theta<2*M_PI ; theta+= step_angle ) {
2066 u = (
unsigned int) (cog_u + outCoef*(a1*cos(alpha)*cos(theta)-a2*sin(alpha)*sin(theta)));
2067 v = (
unsigned int) (cog_v + outCoef*(a1*sin(alpha)*cos(theta)+a2*cos(alpha)*sin(theta)));
2078 if( ! this->hasReverseLevel( I, u, v ) ) {
2082 printf(
"Outside cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
2083 u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
2089 for(
unsigned int t=0; t<thickness; t++) {
2098 if (nb_bad_points > nb_max_bad_points)
2101 printf(
"Outside ellipse has %d bad points. Max allowed is %d\n",
2102 nb_bad_points, nb_max_bad_points);
2130 const unsigned int &u,
2131 const unsigned int &v)
const
2133 if( !isInArea( u, v ) )
2136 if( I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
2160 const unsigned int &u,
2161 const unsigned int &v)
const
2164 if( !isInArea( u, v ) )
2167 if( I[v][u] < gray_level_min || I[v][u] > gray_level_max)
2186 vpDot2* vpDot2::getInstance()
2192 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
2212 std::list<unsigned int>::const_iterator it;
2213 freeman_chain.
kill();
2214 for (it = direction_list.begin(); it != direction_list.end(); ++it) {
2215 freeman_chain += *it;
2237 freeman_chain = direction_list;
2285 direction_list.clear();
2286 ip_edges_list.clear();
2293 if( std::fabs(est_u + 1.0) <=
vpMath::maximum(std::fabs(est_u),1.)*std::numeric_limits<double>::epsilon() )
2295 est_u = this->cog.
get_u();
2300 if( std::fabs(est_v + 1.0) <=
vpMath::maximum(std::fabs(est_v),1.)*std::numeric_limits<double>::epsilon() )
2302 est_v = this->cog.
get_v();
2307 if( !isInArea( (
unsigned int) est_u, (
unsigned int) est_v ) )
2309 vpDEBUG_TRACE(3,
"Initial pixel coordinates (%d, %d) for dot tracking are not in the area",
2310 (
int) est_u, (
int) est_v) ;
2321 if( !hasGoodLevel( I, (
unsigned int) est_u, (
unsigned int) est_v ) )
2323 vpDEBUG_TRACE(3,
"Can't find a dot from pixel (%d, %d) coordinates",
2324 (
int) est_u, (
int) est_v) ;
2330 if(!findFirstBorder(I, (
unsigned int) est_u, (
unsigned int) est_v,
2331 this->firstBorder_u, this->firstBorder_v)) {
2333 vpDEBUG_TRACE(3,
"Can't find first border (%d, %d) coordinates",
2334 (
int) est_u, (
int) est_v) ;
2338 unsigned int dir = 6;
2341 computeFreemanChainElement(I, this->firstBorder_u, this->firstBorder_v, dir);
2342 unsigned int firstDir = dir;
2345 if( !isInArea( this->firstBorder_u, this->firstBorder_v ) )
2347 vpDEBUG_TRACE(3,
"Border pixel coordinates (%d, %d) of the dot are not in the area",
2348 this->firstBorder_u, this->firstBorder_v);
2353 direction_list.push_back( dir );
2355 ip.
set_u( this->firstBorder_u );
2356 ip.
set_v( this->firstBorder_v );
2358 ip_edges_list.push_back( ip );
2360 int border_u = (int)this->firstBorder_u;
2361 int border_v = (int)this->firstBorder_v;
2367 float dS, dMu, dMv, dMuv, dMu2, dMv2;
2378 for(
int t=0; t< (int)thickness; t++) {
2379 ip.
set_u ( border_u + t);
2380 ip.
set_v ( border_v );
2391 computeFreemanParameters(border_u, border_v, dir, du, dv,
2402 if (compute_moment) {
2408 if( !isInArea( (
unsigned int)border_u, (
unsigned int)border_v ) ) {
2410 vpDEBUG_TRACE(3,
"Dot (%d, %d) is not in the area", border_u, border_v);
2417 direction_list.push_back( dir );
2419 ip.
set_u( border_u );
2420 ip.
set_v( border_v );
2421 ip_edges_list.push_back( ip );
2426 if( border_v < bbox_v_min ) bbox_v_min = border_v;
2427 if( border_v > bbox_v_max ) bbox_v_max = border_v;
2428 if( border_u < bbox_u_min ) bbox_u_min = border_u;
2429 if( border_u > bbox_u_max ) bbox_u_max = border_u;
2432 if (computeFreemanChainElement(I, (
unsigned int)border_u, (
unsigned int)border_v, dir) ==
false) {
2433 vpDEBUG_TRACE(3,
"Can't compute Freeman chain for dot (%d, %d)",
2434 border_u, border_v);
2441 while( (getFirstBorder_u() != (
unsigned int)border_u
2442 || getFirstBorder_v() != (
unsigned int)border_v
2443 || firstDir != dir) &&
2444 isInArea( (
unsigned int)border_u, (
unsigned int)border_v ) );
2447 #if VP_DEBUG_MODE == 3
2455 if( std::fabs(
m00) <= std::numeric_limits<double>::epsilon()
2456 || std::fabs(
m00 - 1.) <=
vpMath::maximum(std::fabs(
m00), 1.)*std::numeric_limits<double>::epsilon() )
2458 vpDEBUG_TRACE(3,
"The center of gravity of the dot wasn't properly detected");
2464 double tmpCenter_u =
m10 /
m00;
2465 double tmpCenter_v = m01 /
m00;
2484 cog.
set_u( tmpCenter_u );
2485 cog.
set_v( tmpCenter_v );
2488 width = bbox_u_max - bbox_u_min + 1;
2489 height = bbox_v_max - bbox_v_min + 1;
2492 computeMeanGrayLevel(I);
2514 const unsigned int &u,
2515 const unsigned int &v,
2516 unsigned int &border_u,
2517 unsigned int &border_v)
2527 double epsilon =0.001;
2530 std::cout <<
"gray level: " << gray_level_min <<
" " << gray_level_max << std::endl;
2532 while( hasGoodLevel( I, border_u+1, border_v ) &&
2538 vpDEBUG_TRACE(3,
"The found dot (%d, %d, %d) has a greater width than the required one", u, v, border_u);
2572 const unsigned int &u,
2573 const unsigned int &v,
2574 unsigned int &element)
2577 if (hasGoodLevel( I, u, v )) {
2578 unsigned int _u = u;
2579 unsigned int _v = v;
2581 updateFreemanPosition( _u, _v, (element + 2) %8 );
2582 if (hasGoodLevel( I, _u, _v )) {
2583 element = (element + 2) % 8;
2586 unsigned int _u = u;
2587 unsigned int _v = v;
2588 updateFreemanPosition( _u, _v, (element + 1) %8 );
2590 if ( hasGoodLevel( I, _u, _v )) {
2591 element = (element + 1) % 8;
2594 unsigned int _u = u;
2595 unsigned int _v = v;
2596 updateFreemanPosition( _u, _v, element );
2598 if ( hasGoodLevel( I, _u, _v )) {
2602 unsigned int _u = u;
2603 unsigned int _v = v;
2604 updateFreemanPosition( _u, _v, (element + 7) %8 );
2606 if ( hasGoodLevel( I, _u, _v )) {
2607 element = (element + 7) %8;
2610 unsigned int _u = u;
2611 unsigned int _v = v;
2612 updateFreemanPosition( _u, _v, (element + 6) %8 );
2614 if ( hasGoodLevel( I, _u, _v )) {
2615 element = (element + 6) %8 ;
2618 unsigned int _u = u;
2619 unsigned int _v = v;
2620 updateFreemanPosition( _u, _v, (element + 5) %8 );
2622 if ( hasGoodLevel( I, _u, _v )) {
2623 element = (element + 5) %8 ;
2626 unsigned int _u = u;
2627 unsigned int _v = v;
2628 updateFreemanPosition( _u, _v, (element + 4) %8 );
2630 if ( hasGoodLevel( I, _u, _v )) {
2631 element = (element + 4) %8 ;
2634 unsigned int _u = u;
2635 unsigned int _v = v;
2636 updateFreemanPosition( _u, _v, (element + 3) %8 );
2638 if ( hasGoodLevel( I, _u, _v )) {
2639 element = (element + 3) %8 ;
2695 vpDot2::computeFreemanParameters(
const int &u_p,
2697 unsigned int &element,
2700 float &dMu,
float &dMv,
2702 float &dMu2,
float &dMv2)
2724 dMv = (float)(0.5 * v_p * v_p);
2725 if (compute_moment) {
2726 dMuv = (float)(0.25 * v_p * v_p * (2 * u_p + 1));
2728 dMv2 = (float)(1.0/ 3. * v_p * v_p * v_p);
2735 dS = (float)(v_p + 0.5);
2736 dMu = - (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
2737 dMv = (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
2738 if (compute_moment) {
2739 float half_u_p = (float)(0.5*u_p);
2740 dMuv = (float)(v_p*v_p*(0.25+half_u_p) + v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
2741 dMu2 = (float)(-1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1./12.0);
2742 dMv2 = (float)( 1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) + 1./12.0);
2749 dMu = (float)(- 0.5 * u_p * u_p);
2751 if (compute_moment) {
2753 dMu2 = (float)(-1.0/ 3. * u_p * u_p * u_p);
2761 dS = (float)(- v_p - 0.5);
2762 dMu = - (float)(0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
2763 dMv = - (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
2764 if (compute_moment) {
2765 float half_u_p = (float)(0.5*u_p);
2766 dMuv = (float)(v_p*v_p*(0.25-half_u_p) + v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
2767 dMu2 = (float)(-1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
2768 dMv2 = (float)(-1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1./12.0);
2774 dS = (float)(- v_p);
2775 dMv = (float)(- 0.5 * v_p * v_p);
2777 if (compute_moment) {
2778 dMuv = (float)(-0.25 * v_p * v_p * (2 * u_p - 1));
2780 dMv2 = (float)(-1.0/ 3. * v_p * v_p * v_p);
2787 dS = (float)(- v_p + 0.5);
2788 dMu = (float)( 0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
2789 dMv = (float)(- (0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0));
2790 if (compute_moment) {
2791 float half_u_p = (float)(0.5*u_p);
2792 dMuv = (float)(v_p*v_p*(0.25-half_u_p) - v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
2793 dMu2 = (float)( 1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
2794 dMv2 = (float)(-1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
2801 dMu = (float)(0.5 * u_p * u_p);
2803 if (compute_moment) {
2805 dMu2 = (float)(1.0/ 3. * u_p * u_p * u_p);
2813 dS = (float)(v_p - 0.5);
2814 dMu = (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
2815 dMv = (float)(0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0);
2816 if (compute_moment) {
2817 float half_u_p = (float)(0.5*u_p);
2818 dMuv = (float)(v_p*v_p*(0.25+half_u_p) - v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
2819 dMu2 = (float)(1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1./12.0);
2820 dMv2 = (float)(1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
2840 void vpDot2::updateFreemanPosition(
unsigned int& u,
unsigned int& v,
2841 const unsigned int &dir )
2844 case 0: u += 1;
break;
2845 case 1: u += 1; v += 1;
break;
2846 case 2: v += 1;
break;
2847 case 3: u -= 1; v += 1;
break;
2848 case 4: u -= 1;
break;
2849 case 5: u -= 1; v -= 1;
break;
2850 case 6: v -= 1;
break;
2851 case 7: u += 1; v -= 1;
break;
2868 return isInImage( I, cog);
2887 double u = ip.
get_u();
2888 double v = ip.
get_v();
2890 if( u < 0 || u >= width )
return false;
2891 if( v < 0 || v >= height )
return false;
2906 bool vpDot2::isInArea(
const unsigned int &u,
const unsigned int &v)
const
2908 unsigned int area_u_min = (
unsigned int) area.
getLeft();
2909 unsigned int area_u_max = (
unsigned int) area.
getRight();
2910 unsigned int area_v_min = (
unsigned int) area.
getTop();
2911 unsigned int area_v_max = (
unsigned int) area.
getBottom();
2913 if( u < area_u_min || u > area_u_max )
return false;
2914 if( v < area_v_min || v > area_v_max )
return false;
2930 void vpDot2::getGridSize(
unsigned int &gridWidth,
unsigned int &gridHeight )
2940 if( gridWidth == 0 ) gridWidth = 1;
2941 if( gridHeight == 0 ) gridHeight = 1;
2960 int cog_u = (int)cog.
get_u();
2961 int cog_v = (int)cog.
get_v();
2963 unsigned int sum_value =0;
2964 unsigned int nb_pixels =0;
2966 for(
unsigned int i=(
unsigned int)this->bbox_u_min; i <=(
unsigned int)this->bbox_u_max ; i++){
2967 unsigned int pixel_gray =(
unsigned int) I[(
unsigned int)cog_v][i];
2969 sum_value += pixel_gray;
2973 for(
unsigned int i=(
unsigned int)this->bbox_v_min; i <=(
unsigned int)this->bbox_v_max ; i++){
2974 unsigned char pixel_gray =I[i][(
unsigned int)cog_u];
2976 sum_value += pixel_gray;
2983 if( (cog_u - bbox_u_min) > (cog_v - bbox_v_min)){
2984 imin=cog_v - bbox_v_min;
2986 else{ imin = cog_u - bbox_u_min;}
2987 if( (bbox_u_max - cog_u) > (bbox_v_max - cog_v)){
2988 imax=bbox_v_max - cog_v;
2990 else{ imax = bbox_u_max - cog_u;}
2991 for(
int i=-imin; i <=imax ; i++){
2992 unsigned int pixel_gray =(
unsigned int) I[(
unsigned int)(cog_v + i)][(
unsigned int)(cog_u + i)];
2994 sum_value += pixel_gray;
2999 if( (cog_u - bbox_u_min) > (bbox_v_max - cog_v)){
3000 imin = bbox_v_max - cog_v;
3002 else{ imin = cog_u - bbox_u_min;}
3003 if( (bbox_u_max - cog_u) > (cog_v - bbox_v_min)){
3004 imax = cog_v - bbox_v_min;
3006 else{ imax = bbox_u_max - cog_u;}
3008 for(
int i=-imin; i <=imax ; i++){
3009 unsigned char pixel_gray =I[(
unsigned int)(cog_v - i)][(
unsigned int)(cog_u + i)];
3011 sum_value += pixel_gray;
3022 mean_gray_level = sum_value/nb_pixels;
3051 std::cout << Cogs.
getRows() <<
" dots loaded from file " << dotFile << std::endl;
3057 std::cout <<
"Dot file has a wrong number of dots : redefining them" << std::endl;
3068 cog.
set_uv(Cogs[i][0], Cogs[i][1]);
3081 std::cout <<
"Cannot track dots from file" << std::endl;
3088 for(i=0;i<n && fromFile;++i)
3091 for(
unsigned int j=0;j<n && fromFile;++j)
3096 std::cout <<
"Dots from file seem incoherent" << std::endl;
3106 std::cout <<
"Click on the " << n <<
" dots clockwise starting from upper/left dot..." << std::endl;
3107 for (i = 0; i < n; i++)
3120 Cogs[i][0] = cog.
get_u();
3121 Cogs[i][1] = cog.
get_v();
3127 if (!fromFile & (dotFile !=
""))
3130 std::cout << Cogs.
getRows() <<
" dots written to file " << dotFile << std::endl;
3156 cogs.push_back(dot[i].
getCog());
3159 for(i=n;i<cogs.size();++i)
3189 const std::list<vpImagePoint> &edges_list,
vpColor color,
3190 unsigned int thickness)
3193 std::list<vpImagePoint>::const_iterator it;
3195 for (it = edges_list.begin(); it != edges_list.end(); ++it)
3216 const std::list<vpImagePoint> &edges_list,
vpColor color,
3217 unsigned int thickness)
3220 std::list<vpImagePoint>::const_iterator it;
3222 for (it = edges_list.begin(); it != edges_list.end(); ++it)