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 mean_gray_level = twinDot.mean_gray_level;
146 gray_level_min = twinDot.gray_level_min;
147 gray_level_max = twinDot.gray_level_max;
148 grayLevelPrecision = twinDot.grayLevelPrecision;
149 gamma = twinDot.gamma; ;
150 sizePrecision = twinDot.sizePrecision;
151 ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision ;
152 maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision;
153 allowedBadPointsPercentage_ = twinDot.allowedBadPointsPercentage_;
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;
171 compute_moment = twinDot.compute_moment;
172 graphics = twinDot.graphics;
173 thickness = twinDot.thickness;
175 direction_list = twinDot.direction_list;
176 ip_edges_list = twinDot.ip_edges_list;
198 unsigned int thickness)
201 std::list<vpImagePoint>::const_iterator it;
203 for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
245 unsigned int i = (
unsigned int)cog.
get_i();
246 unsigned int j = (
unsigned int)cog.
get_j();
248 double Ip = pow((
double)I[i][j]/255,1/gamma);
250 if(Ip - (1 - grayLevelPrecision)<0){
254 gray_level_min = (
unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
255 if (gray_level_min > 255)
256 gray_level_min = 255;
258 gray_level_max = (
unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
259 if (gray_level_max > 255)
260 gray_level_max = 255;
307 unsigned int i = (
unsigned int)cog.
get_i();
308 unsigned int j = (
unsigned int)cog.
get_j();
310 double Ip = pow((
double)I[i][j]/255,1/gamma);
312 if(Ip - (1 - grayLevelPrecision)<0){
316 gray_level_min = (
unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
317 if (gray_level_min > 255)
318 gray_level_min = 255;
320 gray_level_max = (
unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
321 if (gray_level_max > 255)
322 gray_level_max = 255;
378 unsigned int gray_level_min,
379 unsigned int gray_level_max,
384 this->gray_level_min = gray_level_min;
385 this->gray_level_max = gray_level_max;
461 found = computeParameters(I, cog.
get_u(), cog.
get_v());
465 found = isValid( I, wantedDot);
484 double searchWindowWidth, searchWindowHeight;
486 if( std::fabs(
getWidth()) <= std::numeric_limits<double>::epsilon() || std::fabs(
getHeight()) <= std::numeric_limits<double>::epsilon() )
488 searchWindowWidth = 80.;
489 searchWindowHeight = 80.;
496 std::list<vpDot2> candidates;
498 (
int)(this->cog.
get_u()-searchWindowWidth /2.0),
499 (int)(this->cog.
get_v()-searchWindowHeight/2.0),
500 (
unsigned int)searchWindowWidth,
501 (
unsigned int)searchWindowHeight,
506 if( candidates.empty() )
510 "No dot was found")) ;
514 vpDot2 movingDot = candidates.front();
530 bbox_u_min = movingDot.bbox_u_min;
531 bbox_u_max = movingDot.bbox_u_max;
532 bbox_v_min = movingDot.bbox_v_min;
533 bbox_v_max = movingDot.bbox_v_max;
553 if( !isInImage( I ) )
555 vpERROR_TRACE(
"The center of gravity of the dot is not in the image") ;
557 "No dot was found")) ;
569 if(Ip - (1 - grayLevelPrecision)<0){
573 gray_level_min = (
unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
574 if (gray_level_min > 255)
575 gray_level_min = 255;
577 gray_level_max = (
unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
578 if (gray_level_max > 255)
579 gray_level_max = 255;
645 return fabs(surface);
655 return grayLevelPrecision;
665 return sizePrecision;
677 return ellipsoidShapePrecision;
686 return maxSizeSearchDistancePrecision;
695 double diff_u = this->cog.
get_u() - cogDistantDot.
get_u();
696 double diff_v = this->cog.
get_v() - cogDistantDot.
get_v();
697 return sqrt( diff_u*diff_u + diff_v*diff_v );
730 this->height = height;
745 this->surface = surface;
766 double epsilon = 0.05;
767 if( grayLevelPrecision<epsilon )
769 this->grayLevelPrecision = epsilon;
771 else if( grayLevelPrecision>1 )
773 this->grayLevelPrecision = 1.0;
777 this->grayLevelPrecision = grayLevelPrecision;
796 if( sizePrecision<0 )
798 this->sizePrecision = 0;
800 else if( sizePrecision>1 )
802 this->sizePrecision = 1.0;
806 this->sizePrecision = sizePrecision;
841 if( ellipsoidShapePrecision<0 )
843 this->ellipsoidShapePrecision = 0;
845 else if( ellipsoidShapePrecision>1 )
847 this->ellipsoidShapePrecision = 1.0;
851 this->ellipsoidShapePrecision = ellipsoidShapePrecision;
870 double epsilon = 0.05;
871 if( maxSizeSearchDistancePrecision<epsilon )
873 this-> maxSizeSearchDistancePrecision = epsilon;
875 else if( maxSizeSearchDistancePrecision >1 )
877 this->maxSizeSearchDistancePrecision = 1.0;
881 this->maxSizeSearchDistancePrecision = maxSizeSearchDistancePrecision;
914 unsigned int w,
unsigned int h)
916 unsigned int image_w = I.
getWidth();
921 else if (u >= (
int)image_w) u = (int)image_w - 1;
923 else if (v >= (
int)image_h) v = (
int)image_h - 1;
925 if (((
unsigned int)u + w) > image_w) w = image_w - (
unsigned int)u - 1;
926 if (((
unsigned int)v + h) > image_h) h = image_h - (
unsigned int)v - 1;
939 vpDot2::setArea(
const vpRect & a)
946 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1015 unsigned int area_w = I.
getWidth();
1017 setArea(I, area_u, area_v, area_w, area_h);
1020 unsigned int gridWidth;
1021 unsigned int gridHeight;
1022 getGridSize( gridWidth, gridHeight );
1036 vpDot2* dotToTest = NULL;
1039 unsigned int area_u_min = (
unsigned int) area.
getLeft();
1040 unsigned int area_u_max = (
unsigned int) area.
getRight();
1041 unsigned int area_v_min = (
unsigned int) area.
getTop();
1042 unsigned int area_v_max = (
unsigned int) area.
getBottom();
1047 for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
1049 for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
1054 if( !hasGoodLevel(I, u, v) )
continue;
1058 bool good_germ =
true;
1059 niceDotsVector->
front();
1060 while( !niceDotsVector->
outside() && good_germ ==
true) {
1061 tmpDot = niceDotsVector->
value();
1063 cogTmpDot = tmpDot.
getCog();
1064 double u0 = cogTmpDot.
get_u();
1065 double v0 = cogTmpDot.
get_v();
1066 double half_w = tmpDot.
getWidth() / 2.;
1067 double half_h = tmpDot.
getHeight() / 2.;
1069 if ( u >= (u0-half_w) && u <= (u0+half_w) &&
1070 v >= (v0-half_h) && v <= (v0+half_h) ) {
1074 niceDotsVector->
next();
1081 unsigned int border_u;
1082 unsigned int border_v;
1083 if(findFirstBorder(I, u, v, border_u, border_v) ==
false){
1091 badDotsVector->
front();
1092 #define vpBAD_DOT_VALUE (badDotsVector->value())
1095 std::list<vpImagePoint>::const_iterator it_edges;
1096 while( !badDotsVector->
outside() && good_germ ==
true)
1098 if( (
double)u >= vpBAD_DOT_VALUE.bbox_u_min
1099 && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
1100 (
double)v >= vpBAD_DOT_VALUE.bbox_v_min
1101 && (double)v <= vpBAD_DOT_VALUE.bbox_v_max)
1104 it_edges = ip_edges_list.begin();
1105 while (it_edges != ip_edges_list.end() && good_germ ==
true)
1110 cogBadDot = *it_edges;
1112 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() )
1114 (std::fabs(v - cogBadDot.
get_v()) <=
vpMath::maximum(std::fabs((
double)v), std::fabs(cogBadDot.
get_v()))*std::numeric_limits<double>::epsilon() ))
1121 badDotsVector->
next();
1123 #undef vpBAD_DOT_VALUE
1132 vpTRACE(4,
"Try germ (%d, %d)", u, v);
1140 if( dotToTest != NULL )
delete dotToTest;
1141 dotToTest = getInstance();
1142 dotToTest->setCog( germ );
1147 dotToTest->setGraphics( graphics );
1148 dotToTest->setGraphicsThickness( thickness );
1149 dotToTest->setComputeMoments(
true );
1150 dotToTest->setArea( area );
1151 dotToTest->setEllipsoidShapePrecision( ellipsoidShapePrecision );
1156 if( dotToTest->computeParameters( I ) == false ) {
1163 if( dotToTest->isValid( I, *
this ) )
1171 double area_center_u = area_u + area_w/2.0 - 0.5;
1172 double area_center_v = area_v + area_h/2.0 - 0.5;
1174 double thisDiff_u = cogDotToTest.
get_u() - area_center_u;
1175 double thisDiff_v = cogDotToTest.
get_v() - area_center_v;
1176 double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
1178 bool stopLoop =
false;
1179 niceDotsVector->
front();
1181 while( !niceDotsVector->
outside() && stopLoop == false )
1186 double epsilon = 3.0;
1189 cogTmpDot = tmpDot.
getCog();
1191 if( fabs( cogTmpDot.
get_u() - cogDotToTest.
get_u() ) < epsilon &&
1192 fabs( cogTmpDot.
get_v() - cogDotToTest.
get_v() ) < epsilon )
1201 double otherDiff_u = cogTmpDot.
get_u() - area_center_u;
1202 double otherDiff_v = cogTmpDot.
get_v() - area_center_v;
1203 double otherDist = sqrt( otherDiff_u*otherDiff_u +
1204 otherDiff_v*otherDiff_v );
1210 if( otherDist > thisDist )
1212 niceDotsVector->
addLeft( *dotToTest );
1213 niceDotsVector->
next();
1220 niceDotsVector->
next();
1222 vpTRACE(4,
"End while (%d, %d)", u, v);
1226 if( niceDotsVector->
outside() && stopLoop == false )
1228 niceDotsVector->
end();
1229 niceDotsVector->
addRight( *dotToTest );
1234 badDotsVector->
front();
1235 badDotsVector->
addRight( *dotToTest );
1239 if( dotToTest != NULL )
delete dotToTest;
1241 delete badDotsVector;
1247 return niceDotsVector;
1276 unsigned int area_w,
1277 unsigned int area_h)
1282 setArea(I, area_u, area_v, area_w, area_h);
1285 unsigned int gridWidth;
1286 unsigned int gridHeight;
1287 getGridSize( gridWidth, gridHeight );
1301 vpDot2* dotToTest = NULL;
1304 unsigned int area_u_min = (
unsigned int) area.
getLeft();
1305 unsigned int area_u_max = (
unsigned int) area.
getRight();
1306 unsigned int area_v_min = (
unsigned int) area.
getTop();
1307 unsigned int area_v_max = (
unsigned int) area.
getBottom();
1312 for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
1314 for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
1319 if( !hasGoodLevel(I, u, v) )
continue;
1323 bool good_germ =
true;
1324 niceDotsVector->
front();
1325 while( !niceDotsVector->
outside() && good_germ ==
true) {
1326 tmpDot = niceDotsVector->
value();
1328 cogTmpDot = tmpDot.
getCog();
1329 double u0 = cogTmpDot.
get_u();
1330 double v0 = cogTmpDot.
get_v();
1331 double half_w = tmpDot.
getWidth() / 2.;
1332 double half_h = tmpDot.
getHeight() / 2.;
1334 if ( u >= (u0-half_w) && u <= (u0+half_w) &&
1335 v >= (v0-half_h) && v <= (v0+half_h) ) {
1339 niceDotsVector->
next();
1346 unsigned int border_u;
1347 unsigned int border_v;
1348 if(findFirstBorder(I, u, v, border_u, border_v) ==
false){
1356 badDotsVector->
front();
1357 #define vpBAD_DOT_VALUE (badDotsVector->value())
1360 std::list<vpImagePoint>::const_iterator it_edges;
1361 while( !badDotsVector->
outside() && good_germ ==
true)
1363 if( (
double)u >= vpBAD_DOT_VALUE.bbox_u_min
1364 && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
1365 (
double)v >= vpBAD_DOT_VALUE.bbox_v_min
1366 && (double)v <= vpBAD_DOT_VALUE.bbox_v_max)
1369 it_edges = ip_edges_list.begin();
1370 while (it_edges != ip_edges_list.end() && good_germ ==
true)
1375 cogBadDot = *it_edges;
1377 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() )
1379 (std::fabs(v - cogBadDot.
get_v()) <=
vpMath::maximum(std::fabs((
double)v), std::fabs(cogBadDot.
get_v()))*std::numeric_limits<double>::epsilon() ))
1386 badDotsVector->
next();
1388 #undef vpBAD_DOT_VALUE
1397 vpTRACE(4,
"Try germ (%d, %d)", u, v);
1405 if( dotToTest != NULL )
delete dotToTest;
1406 dotToTest = getInstance();
1407 dotToTest->setCog( germ );
1412 dotToTest->setGraphics( graphics );
1413 dotToTest->setGraphicsThickness( thickness );
1414 dotToTest->setComputeMoments(
true );
1415 dotToTest->setArea( area );
1416 dotToTest->setEllipsoidShapePrecision( ellipsoidShapePrecision );
1421 if( dotToTest->computeParameters( I ) == false ) {
1428 if( dotToTest->isValid( I, *
this ) )
1436 double area_center_u = area_u + area_w/2.0 - 0.5;
1437 double area_center_v = area_v + area_h/2.0 - 0.5;
1439 double thisDiff_u = cogDotToTest.
get_u() - area_center_u;
1440 double thisDiff_v = cogDotToTest.
get_v() - area_center_v;
1441 double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
1443 bool stopLoop =
false;
1444 niceDotsVector->
front();
1446 while( !niceDotsVector->
outside() && stopLoop == false )
1451 double epsilon = 3.0;
1454 cogTmpDot = tmpDot.
getCog();
1456 if( fabs( cogTmpDot.
get_u() - cogDotToTest.
get_u() ) < epsilon &&
1457 fabs( cogTmpDot.
get_v() - cogDotToTest.
get_v() ) < epsilon )
1466 double otherDiff_u = cogTmpDot.
get_u() - area_center_u;
1467 double otherDiff_v = cogTmpDot.
get_v() - area_center_v;
1468 double otherDist = sqrt( otherDiff_u*otherDiff_u +
1469 otherDiff_v*otherDiff_v );
1475 if( otherDist > thisDist )
1477 niceDotsVector->
addLeft( *dotToTest );
1478 niceDotsVector->
next();
1485 niceDotsVector->
next();
1487 vpTRACE(4,
"End while (%d, %d)", u, v);
1491 if( niceDotsVector->
outside() && stopLoop == false )
1493 niceDotsVector->
end();
1494 niceDotsVector->
addRight( *dotToTest );
1499 badDotsVector->
front();
1500 badDotsVector->
addRight( *dotToTest );
1504 if( dotToTest != NULL )
delete dotToTest;
1506 delete badDotsVector;
1508 return niceDotsVector;
1510 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS
1588 unsigned int area_w,
1589 unsigned int area_h,
1590 std::list<vpDot2> &niceDots)
1598 setArea(I, area_u, area_v, area_w, area_h);
1601 unsigned int gridWidth;
1602 unsigned int gridHeight;
1603 getGridSize( gridWidth, gridHeight );
1618 std::list<vpDot2> badDotsVector;
1619 std::list<vpDot2>::iterator itnice;
1620 std::list<vpDot2>::iterator itbad;
1622 vpDot2* dotToTest = NULL;
1625 unsigned int area_u_min = (
unsigned int) area.
getLeft();
1626 unsigned int area_u_max = (
unsigned int) area.
getRight();
1627 unsigned int area_v_min = (
unsigned int) area.
getTop();
1628 unsigned int area_v_max = (
unsigned int) area.
getBottom();
1633 for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
1635 for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
1640 if( !hasGoodLevel(I, u, v) )
continue;
1644 bool good_germ =
true;
1646 itnice = niceDots.begin();
1647 while( itnice != niceDots.end() && good_germ ==
true) {
1650 cogTmpDot = tmpDot.
getCog();
1651 double u0 = cogTmpDot.
get_u();
1652 double v0 = cogTmpDot.
get_v();
1653 double half_w = tmpDot.
getWidth() / 2.;
1654 double half_h = tmpDot.
getHeight() / 2.;
1656 if ( u >= (u0-half_w) && u <= (u0+half_w) &&
1657 v >= (v0-half_h) && v <= (v0+half_h) ) {
1668 unsigned int border_u;
1669 unsigned int border_v;
1670 if(findFirstBorder(I, u, v, border_u, border_v) ==
false){
1678 itbad = badDotsVector.begin();
1679 #define vpBAD_DOT_VALUE (*itbad)
1682 while( itbad != badDotsVector.end() && good_germ ==
true) {
1683 if( (
double)u >= vpBAD_DOT_VALUE.bbox_u_min
1684 && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
1685 (
double)v >= vpBAD_DOT_VALUE.bbox_v_min
1686 && (double)v <= vpBAD_DOT_VALUE.bbox_v_max){
1687 std::list<vpImagePoint>::const_iterator it_edges = ip_edges_list.begin();
1688 while (it_edges != ip_edges_list.end() && good_germ ==
true){
1692 cogBadDot = *it_edges;
1694 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() )
1696 (std::fabs(v - cogBadDot.
get_v()) <=
vpMath::maximum(std::fabs((
double)v), std::fabs(cogBadDot.
get_v()))*std::numeric_limits<double>::epsilon() )) {
1704 #undef vpBAD_DOT_VALUE
1713 vpTRACE(4,
"Try germ (%d, %d)", u, v);
1721 if( dotToTest != NULL )
delete dotToTest;
1722 dotToTest = getInstance();
1723 dotToTest->
setCog( germ );
1731 dotToTest->setArea( area );
1737 if( dotToTest->computeParameters( I ) == false ) {
1744 if( dotToTest->isValid( I, *
this ) )
1752 double area_center_u = area_u + area_w/2.0 - 0.5;
1753 double area_center_v = area_v + area_h/2.0 - 0.5;
1755 double thisDiff_u = cogDotToTest.
get_u() - area_center_u;
1756 double thisDiff_v = cogDotToTest.
get_v() - area_center_v;
1757 double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
1759 bool stopLoop =
false;
1760 itnice = niceDots.begin();
1762 while( itnice != niceDots.end() && stopLoop == false )
1767 double epsilon = 3.0;
1770 cogTmpDot = tmpDot.
getCog();
1772 if( fabs( cogTmpDot.
get_u() - cogDotToTest.
get_u() ) < epsilon &&
1773 fabs( cogTmpDot.
get_v() - cogDotToTest.
get_v() ) < epsilon )
1782 double otherDiff_u = cogTmpDot.
get_u() - area_center_u;
1783 double otherDiff_v = cogTmpDot.
get_v() - area_center_v;
1784 double otherDist = sqrt( otherDiff_u*otherDiff_u +
1785 otherDiff_v*otherDiff_v );
1791 if( otherDist > thisDist )
1793 niceDots.insert(itnice, *dotToTest );
1803 vpTRACE(4,
"End while (%d, %d)", u, v);
1807 if( itnice == niceDots.end() && stopLoop == false )
1809 niceDots.push_back( *dotToTest );
1814 badDotsVector.push_front( *dotToTest );
1818 if( dotToTest != NULL )
delete dotToTest;
1845 double epsilon = 0.001;
1854 if ( (std::fabs(wantedDot.
getWidth()) > std::numeric_limits<double>::epsilon())
1856 (std::fabs(wantedDot.
getHeight()) > std::numeric_limits<double>::epsilon())
1858 (std::fabs(wantedDot.
getSurface()) > std::numeric_limits<double>::epsilon()) )
1860 if (std::fabs(sizePrecision) > std::numeric_limits<double>::epsilon()){
1862 std::cout <<
"test size precision......................\n";
1863 std::cout <<
"wanted dot: " <<
"w=" << wantedDot.
getWidth()
1866 <<
" precision=" << sizePrecision
1867 <<
" epsilon=" << epsilon << std::endl;
1868 std::cout <<
"dot found: " <<
"w=" <<
getWidth()
1877 printf(
"Bad width > for dot (%g, %g)\n", cog.
get_u(), cog.
get_v());
1882 if( (
getWidth() < wantedDot.
getWidth()/(sizePrecision+epsilon ) )== false )
1887 printf(
"Bad width %g > %g for dot (%g, %g)\n",
1899 printf(
"Bad height %g > %g for dot (%g, %g)\n",
1911 printf(
"Bad height %g > %g for dot (%g, %g)\n",
1923 printf(
"Bad surface %g > %g for dot (%g, %g)\n",
1924 wantedDot.
getSurface()*(sizePrecision*sizePrecision)-epsilon,
1936 printf(
"Bad surface %g < %g for dot (%g, %g)\n",
1948 int nb_point_to_test = 20;
1949 int nb_bad_points = 0;
1950 int nb_max_bad_points = (int)(nb_point_to_test*allowedBadPointsPercentage_);
1951 double step_angle = 2*M_PI / nb_point_to_test;
1954 if (std::fabs(ellipsoidShapePrecision) > std::numeric_limits<double>::epsilon() && compute_moment) {
1973 double Sqrt = sqrt(tmp1*tmp1 + 4*tmp2*tmp2);
1984 double innerCoef = ellipsoidShapePrecision ;
1986 double cog_u = this->cog.
get_u();
1987 double cog_v = this->cog.
get_v();
1991 for(
double theta = 0. ; theta<2*M_PI ; theta+= step_angle ) {
1992 u = (
unsigned int) (cog_u + innerCoef*(a1*cos(alpha)*cos(theta)-a2*sin(alpha)*sin(theta)));
1993 v = (
unsigned int) (cog_v + innerCoef*(a1*sin(alpha)*cos(theta)+a2*cos(alpha)*sin(theta)));
1994 if( ! this->hasGoodLevel( I, u, v) ) {
1998 printf(
"Inner cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
1999 u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
2005 for (
unsigned int t=0; t< thickness; t++) {
2016 if (nb_bad_points > nb_max_bad_points)
2019 printf(
"Inner ellipse has %d bad points. Max allowed is %d\n",
2020 nb_bad_points, nb_max_bad_points);
2029 double outCoef = 2-ellipsoidShapePrecision;
2031 for(
double theta=0. ; theta<2*M_PI ; theta+= step_angle ) {
2032 u = (
unsigned int) (cog_u + outCoef*(a1*cos(alpha)*cos(theta)-a2*sin(alpha)*sin(theta)));
2033 v = (
unsigned int) (cog_v + outCoef*(a1*sin(alpha)*cos(theta)+a2*cos(alpha)*sin(theta)));
2044 if( ! this->hasReverseLevel( I, u, v ) ) {
2048 printf(
"Outside cercle pixel (%d, %d) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
2049 u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
2055 for(
unsigned int t=0; t<thickness; t++) {
2064 if (nb_bad_points > nb_max_bad_points)
2067 printf(
"Outside ellipse has %d bad points. Max allowed is %d\n",
2068 nb_bad_points, nb_max_bad_points);
2096 const unsigned int &u,
2097 const unsigned int &v)
const
2099 if( !isInArea( u, v ) )
2102 if( I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
2126 const unsigned int &u,
2127 const unsigned int &v)
const
2130 if( !isInArea( u, v ) )
2133 if( I[v][u] < gray_level_min || I[v][u] > gray_level_max)
2152 vpDot2* vpDot2::getInstance()
2158 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
2178 std::list<unsigned int>::const_iterator it;
2179 freeman_chain.
kill();
2180 for (it = direction_list.begin(); it != direction_list.end(); ++it) {
2181 freeman_chain += *it;
2203 freeman_chain = direction_list;
2251 direction_list.clear();
2252 ip_edges_list.clear();
2259 if( std::fabs(est_u + 1.0) <=
vpMath::maximum(std::fabs(est_u),1.)*std::numeric_limits<double>::epsilon() )
2261 est_u = this->cog.
get_u();
2266 if( std::fabs(est_v + 1.0) <=
vpMath::maximum(std::fabs(est_v),1.)*std::numeric_limits<double>::epsilon() )
2268 est_v = this->cog.
get_v();
2273 if( !isInArea( (
unsigned int) est_u, (
unsigned int) est_v ) )
2275 vpDEBUG_TRACE(3,
"Initial pixel coordinates (%d, %d) for dot tracking are not in the area",
2276 (
int) est_u, (
int) est_v) ;
2287 if( !hasGoodLevel( I, (
unsigned int) est_u, (
unsigned int) est_v ) )
2289 vpDEBUG_TRACE(3,
"Can't find a dot from pixel (%d, %d) coordinates",
2290 (
int) est_u, (
int) est_v) ;
2296 if(!findFirstBorder(I, (
unsigned int) est_u, (
unsigned int) est_v,
2297 this->firstBorder_u, this->firstBorder_v)) {
2299 vpDEBUG_TRACE(3,
"Can't find first border (%d, %d) coordinates",
2300 (
int) est_u, (
int) est_v) ;
2304 unsigned int dir = 6;
2307 computeFreemanChainElement(I, this->firstBorder_u, this->firstBorder_v, dir);
2308 unsigned int firstDir = dir;
2311 if( !isInArea( this->firstBorder_u, this->firstBorder_v ) )
2313 vpDEBUG_TRACE(3,
"Border pixel coordinates (%d, %d) of the dot are not in the area",
2314 this->firstBorder_u, this->firstBorder_v);
2319 direction_list.push_back( dir );
2321 ip.
set_u( this->firstBorder_u );
2322 ip.
set_v( this->firstBorder_v );
2324 ip_edges_list.push_back( ip );
2326 int border_u = (int)this->firstBorder_u;
2327 int border_v = (int)this->firstBorder_v;
2333 float dS, dMu, dMv, dMuv, dMu2, dMv2;
2344 for(
int t=0; t< (int)thickness; t++) {
2345 ip.
set_u ( border_u + t);
2346 ip.
set_v ( border_v );
2357 computeFreemanParameters(border_u, border_v, dir, du, dv,
2368 if (compute_moment) {
2374 if( !isInArea( (
unsigned int)border_u, (
unsigned int)border_v ) ) {
2376 vpDEBUG_TRACE(3,
"Dot (%d, %d) is not in the area", border_u, border_v);
2383 direction_list.push_back( dir );
2385 ip.
set_u( border_u );
2386 ip.
set_v( border_v );
2387 ip_edges_list.push_back( ip );
2392 if( border_v < bbox_v_min ) bbox_v_min = border_v;
2393 if( border_v > bbox_v_max ) bbox_v_max = border_v;
2394 if( border_u < bbox_u_min ) bbox_u_min = border_u;
2395 if( border_u > bbox_u_max ) bbox_u_max = border_u;
2398 if (computeFreemanChainElement(I, (
unsigned int)border_u, (
unsigned int)border_v, dir) ==
false) {
2399 vpDEBUG_TRACE(3,
"Can't compute Freeman chain for dot (%d, %d)",
2400 border_u, border_v);
2407 while( (getFirstBorder_u() != (
unsigned int)border_u
2408 || getFirstBorder_v() != (
unsigned int)border_v
2409 || firstDir != dir) &&
2410 isInArea( (
unsigned int)border_u, (
unsigned int)border_v ) );
2413 #if VP_DEBUG_MODE == 3
2421 if( std::fabs(
m00) <= std::numeric_limits<double>::epsilon()
2422 || std::fabs(
m00 - 1.) <=
vpMath::maximum(std::fabs(
m00), 1.)*std::numeric_limits<double>::epsilon() )
2424 vpDEBUG_TRACE(3,
"The center of gravity of the dot wasn't properly detected");
2430 double tmpCenter_u =
m10 /
m00;
2431 double tmpCenter_v = m01 /
m00;
2450 cog.
set_u( tmpCenter_u );
2451 cog.
set_v( tmpCenter_v );
2454 width = bbox_u_max - bbox_u_min + 1;
2455 height = bbox_v_max - bbox_v_min + 1;
2458 computeMeanGrayLevel(I);
2480 const unsigned int &u,
2481 const unsigned int &v,
2482 unsigned int &border_u,
2483 unsigned int &border_v)
2493 double epsilon =0.001;
2496 std::cout <<
"gray level: " << gray_level_min <<
" " << gray_level_max << std::endl;
2498 while( hasGoodLevel( I, border_u+1, border_v ) &&
2504 vpDEBUG_TRACE(3,
"The found dot (%d, %d, %d) has a greater width than the required one", u, v, border_u);
2538 const unsigned int &u,
2539 const unsigned int &v,
2540 unsigned int &element)
2543 if (hasGoodLevel( I, u, v )) {
2544 unsigned int _u = u;
2545 unsigned int _v = v;
2547 updateFreemanPosition( _u, _v, (element + 2) %8 );
2548 if (hasGoodLevel( I, _u, _v )) {
2549 element = (element + 2) % 8;
2552 unsigned int _u = u;
2553 unsigned int _v = v;
2554 updateFreemanPosition( _u, _v, (element + 1) %8 );
2556 if ( hasGoodLevel( I, _u, _v )) {
2557 element = (element + 1) % 8;
2560 unsigned int _u = u;
2561 unsigned int _v = v;
2562 updateFreemanPosition( _u, _v, element );
2564 if ( hasGoodLevel( I, _u, _v )) {
2568 unsigned int _u = u;
2569 unsigned int _v = v;
2570 updateFreemanPosition( _u, _v, (element + 7) %8 );
2572 if ( hasGoodLevel( I, _u, _v )) {
2573 element = (element + 7) %8;
2576 unsigned int _u = u;
2577 unsigned int _v = v;
2578 updateFreemanPosition( _u, _v, (element + 6) %8 );
2580 if ( hasGoodLevel( I, _u, _v )) {
2581 element = (element + 6) %8 ;
2584 unsigned int _u = u;
2585 unsigned int _v = v;
2586 updateFreemanPosition( _u, _v, (element + 5) %8 );
2588 if ( hasGoodLevel( I, _u, _v )) {
2589 element = (element + 5) %8 ;
2592 unsigned int _u = u;
2593 unsigned int _v = v;
2594 updateFreemanPosition( _u, _v, (element + 4) %8 );
2596 if ( hasGoodLevel( I, _u, _v )) {
2597 element = (element + 4) %8 ;
2600 unsigned int _u = u;
2601 unsigned int _v = v;
2602 updateFreemanPosition( _u, _v, (element + 3) %8 );
2604 if ( hasGoodLevel( I, _u, _v )) {
2605 element = (element + 3) %8 ;
2661 vpDot2::computeFreemanParameters(
const int &u_p,
2663 unsigned int &element,
2666 float &dMu,
float &dMv,
2668 float &dMu2,
float &dMv2)
2690 dMv = (float)(0.5 * v_p * v_p);
2691 if (compute_moment) {
2692 dMuv = (float)(0.25 * v_p * v_p * (2 * u_p + 1));
2694 dMv2 = (float)(1.0/ 3. * v_p * v_p * v_p);
2701 dS = (float)(v_p + 0.5);
2702 dMu = - (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
2703 dMv = (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
2704 if (compute_moment) {
2705 float half_u_p = (float)(0.5*u_p);
2706 dMuv = (float)(v_p*v_p*(0.25+half_u_p) + v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
2707 dMu2 = (float)(-1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1./12.0);
2708 dMv2 = (float)( 1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) + 1./12.0);
2715 dMu = (float)(- 0.5 * u_p * u_p);
2717 if (compute_moment) {
2719 dMu2 = (float)(-1.0/ 3. * u_p * u_p * u_p);
2727 dS = (float)(- v_p - 0.5);
2728 dMu = - (float)(0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
2729 dMv = - (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
2730 if (compute_moment) {
2731 float half_u_p = (float)(0.5*u_p);
2732 dMuv = (float)(v_p*v_p*(0.25-half_u_p) + v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
2733 dMu2 = (float)(-1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
2734 dMv2 = (float)(-1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1./12.0);
2740 dS = (float)(- v_p);
2741 dMv = (float)(- 0.5 * v_p * v_p);
2743 if (compute_moment) {
2744 dMuv = (float)(-0.25 * v_p * v_p * (2 * u_p - 1));
2746 dMv2 = (float)(-1.0/ 3. * v_p * v_p * v_p);
2753 dS = (float)(- v_p + 0.5);
2754 dMu = (float)( 0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
2755 dMv = (float)(- (0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0));
2756 if (compute_moment) {
2757 float half_u_p = (float)(0.5*u_p);
2758 dMuv = (float)(v_p*v_p*(0.25-half_u_p) - v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
2759 dMu2 = (float)( 1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
2760 dMv2 = (float)(-1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
2767 dMu = (float)(0.5 * u_p * u_p);
2769 if (compute_moment) {
2771 dMu2 = (float)(1.0/ 3. * u_p * u_p * u_p);
2779 dS = (float)(v_p - 0.5);
2780 dMu = (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
2781 dMv = (float)(0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0);
2782 if (compute_moment) {
2783 float half_u_p = (float)(0.5*u_p);
2784 dMuv = (float)(v_p*v_p*(0.25+half_u_p) - v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
2785 dMu2 = (float)(1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1./12.0);
2786 dMv2 = (float)(1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
2806 void vpDot2::updateFreemanPosition(
unsigned int& u,
unsigned int& v,
2807 const unsigned int &dir )
2810 case 0: u += 1;
break;
2811 case 1: u += 1; v += 1;
break;
2812 case 2: v += 1;
break;
2813 case 3: u -= 1; v += 1;
break;
2814 case 4: u -= 1;
break;
2815 case 5: u -= 1; v -= 1;
break;
2816 case 6: v -= 1;
break;
2817 case 7: u += 1; v -= 1;
break;
2834 return isInImage( I, cog);
2853 double u = ip.
get_u();
2854 double v = ip.
get_v();
2856 if( u < 0 || u >= width )
return false;
2857 if( v < 0 || v >= height )
return false;
2872 bool vpDot2::isInArea(
const unsigned int &u,
const unsigned int &v)
const
2874 unsigned int area_u_min = (
unsigned int) area.
getLeft();
2875 unsigned int area_u_max = (
unsigned int) area.
getRight();
2876 unsigned int area_v_min = (
unsigned int) area.
getTop();
2877 unsigned int area_v_max = (
unsigned int) area.
getBottom();
2879 if( u < area_u_min || u > area_u_max )
return false;
2880 if( v < area_v_min || v > area_v_max )
return false;
2896 void vpDot2::getGridSize(
unsigned int &gridWidth,
unsigned int &gridHeight )
2906 if( gridWidth == 0 ) gridWidth = 1;
2907 if( gridHeight == 0 ) gridHeight = 1;
2926 int cog_u = (int)cog.
get_u();
2927 int cog_v = (int)cog.
get_v();
2929 unsigned int sum_value =0;
2930 unsigned int nb_pixels =0;
2932 for(
unsigned int i=(
unsigned int)this->bbox_u_min; i <=(
unsigned int)this->bbox_u_max ; i++){
2933 unsigned int pixel_gray =(
unsigned int) I[(
unsigned int)cog_v][i];
2935 sum_value += pixel_gray;
2939 for(
unsigned int i=(
unsigned int)this->bbox_v_min; i <=(
unsigned int)this->bbox_v_max ; i++){
2940 unsigned char pixel_gray =I[i][(
unsigned int)cog_u];
2942 sum_value += pixel_gray;
2949 if( (cog_u - bbox_u_min) > (cog_v - bbox_v_min)){
2950 imin=cog_v - bbox_v_min;
2952 else{ imin = cog_u - bbox_u_min;}
2953 if( (bbox_u_max - cog_u) > (bbox_v_max - cog_v)){
2954 imax=bbox_v_max - cog_v;
2956 else{ imax = bbox_u_max - cog_u;}
2957 for(
int i=-imin; i <=imax ; i++){
2958 unsigned int pixel_gray =(
unsigned int) I[(
unsigned int)(cog_v + i)][(
unsigned int)(cog_u + i)];
2960 sum_value += pixel_gray;
2965 if( (cog_u - bbox_u_min) > (bbox_v_max - cog_v)){
2966 imin = bbox_v_max - cog_v;
2968 else{ imin = cog_u - bbox_u_min;}
2969 if( (bbox_u_max - cog_u) > (cog_v - bbox_v_min)){
2970 imax = cog_v - bbox_v_min;
2972 else{ imax = bbox_u_max - cog_u;}
2974 for(
int i=-imin; i <=imax ; i++){
2975 unsigned char pixel_gray =I[(
unsigned int)(cog_v - i)][(
unsigned int)(cog_u + i)];
2977 sum_value += pixel_gray;
2988 mean_gray_level = sum_value/nb_pixels;
3017 std::cout << Cogs.
getRows() <<
" dots loaded from file " << dotFile << std::endl;
3023 std::cout <<
"Dot file has a wrong number of dots : redefining them" << std::endl;
3034 cog.
set_uv(Cogs[i][0], Cogs[i][1]);
3047 std::cout <<
"Cannot track dots from file" << std::endl;
3054 for(i=0;i<n && fromFile;++i)
3057 for(
unsigned int j=0;j<n && fromFile;++j)
3062 std::cout <<
"Dots from file seem incoherent" << std::endl;
3072 std::cout <<
"Click on the " << n <<
" dots clockwise starting from upper/left dot..." << std::endl;
3073 for (i = 0; i < n; i++)
3086 Cogs[i][0] = cog.
get_u();
3087 Cogs[i][1] = cog.
get_v();
3093 if (!fromFile & (dotFile !=
""))
3096 std::cout << Cogs.
getRows() <<
" dots written to file " << dotFile << std::endl;
3122 cogs.push_back(dot[i].
getCog());
3125 for(i=n;i<cogs.size();++i)
3155 const std::list<vpImagePoint> &edges_list,
vpColor color,
3156 unsigned int thickness)
3159 std::list<vpImagePoint>::const_iterator it;
3161 for (it = edges_list.begin(); it != edges_list.end(); ++it)
3182 const std::list<vpImagePoint> &edges_list,
vpColor color,
3183 unsigned int thickness)
3186 std::list<vpImagePoint>::const_iterator it;
3188 for (it = edges_list.begin(); it != edges_list.end(); ++it)