39 #ifndef DOXYGEN_SHOULD_SKIP_THIS
41 #include <visp3/mbt/vpMbtMeEllipse.h>
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpImagePoint.h>
45 #include <visp3/core/vpRobust.h>
46 #include <visp3/core/vpTrackingException.h>
47 #include <visp3/me/vpMe.h>
56 vpMbtMeEllipse::vpMbtMeEllipse()
57 : iPc(), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
64 vpMbtMeEllipse::vpMbtMeEllipse(
const vpMbtMeEllipse &meellipse)
65 :
vpMeTracker(meellipse), iPc(meellipse.iPc), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.),
66 thresholdWeight(0.), expecteddensity(0.)
75 mu11 = meellipse.mu11;
76 mu20 = meellipse.mu20;
77 mu02 = meellipse.mu02;
79 expecteddensity = meellipse.expecteddensity;
85 vpMbtMeEllipse::~vpMbtMeEllipse() { list.clear(); }
103 unsigned int &_nbFeatures,
105 bool display,
unsigned int length,
106 unsigned int thickness)
111 double offset = std::floor(SobelX.
getRows() / 2.0f);
116 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
117 double iSite = it->ifloat;
118 double jSite = it->jfloat;
123 double theta = atan((-mu02 * jSite + mu02 * iPc.get_j() + mu11 * iSite - mu11 * iPc.get_i()) /
124 (mu20 * iSite - mu11 * jSite + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
127 double deltaNormalized = theta;
128 while (deltaNormalized < 0)
129 deltaNormalized += M_PI;
130 while (deltaNormalized > M_PI)
131 deltaNormalized -= M_PI;
134 vecSite[0] = cos(deltaNormalized);
135 vecSite[1] = sin(deltaNormalized);
138 double gradientX = 0;
139 double gradientY = 0;
141 for (
unsigned int i = 0; i < SobelX.
getRows(); i++) {
142 double iImg = iSite + (i - offset);
143 for (
unsigned int j = 0; j < SobelX.
getCols(); j++) {
144 double jImg = jSite + (j - offset);
156 gradientX += SobelX[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
160 for (
unsigned int i = 0; i < SobelY.
getRows(); i++) {
161 double iImg = iSite + (i - offset);
162 for (
unsigned int j = 0; j < SobelY.
getCols(); j++) {
163 double jImg = jSite + (j - offset);
175 gradientY += SobelY[i][j] * _I((
unsigned int)iImg, (
unsigned int)jImg);
179 double angle = atan2(gradientY, gradientX);
198 vecGrad[0] = cos(angle);
199 vecGrad[1] = sin(angle);
202 double angle1 = acos(vecSite * vecGrad);
203 double angle2 = acos(vecSite * (-vecGrad));
207 (
int)(it->get_j() + length*sin(deltaNormalized)),
vpColor::blue,
208 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
209 if (angle1 < angle2) {
212 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
215 (
int)(it->get_j() + length*sin(angle+M_PI)),
vpColor::red,
216 length >= 20 ? length/5 : 4, length >= 20 ? length/10 : 2, thickness);
220 _sumErrorRad += (std::min)(angle1, angle2);
241 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
249 if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
250 std::cout <<
"In vpMbtMeEllipse::sample: ";
251 std::cout <<
"function called with sample step = 0";
258 double t = (a - b) / (a + b);
260 int nb_points_to_track = (int)(circumference / me->getSampleStep());
261 double incr = 2 * M_PI / nb_points_to_track;
270 for (
int pt = 0; pt < nb_points_to_track; pt++) {
271 double j = a * cos(k);
272 double i = b * sin(k);
274 double iP_j = iPc.get_j() + ce * j - se * i;
275 double iP_i = iPc.get_i() + se * j + ce * i;
284 double theta = atan((-mu02 * iP_j + mu02 * iPc.get_j() + mu11 * iP_i - mu11 * iPc.get_i()) /
285 (mu20 * iP_i - mu11 * iP_j + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
289 pix.
init((
int)iP_i, (
int)iP_j, theta);
319 vpDERROR_TRACE(2,
"Tracking error: Moving edges not initialized");
323 unsigned int n = numberOfSignal();
324 if ((
double)n < 0.9 * expecteddensity) {
335 void vpMbtMeEllipse::updateTheta()
338 for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
345 double theta = atan((-mu02 * p_me.
jfloat + mu02 * iPc.get_j() + mu11 * p_me.
ifloat - mu11 * iPc.get_i()) /
346 (mu20 * p_me.
ifloat - mu11 * p_me.
jfloat + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
358 void vpMbtMeEllipse::suppressPoints()
361 for (std::list<vpMeSite>::iterator itList = list.begin(); itList != list.end();) {
364 itList = list.erase(itList);
393 if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
396 a = sqrt((mu20_p + mu02_p + val_p) / 2);
397 b = sqrt((mu20_p + mu02_p - val_p) / 2);
399 e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
411 sample(I, doNotTrack);
433 if (m_mask != NULL) {
435 expecteddensity = (double)list.size();
443 double mu11_p,
double mu02_p)
450 if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
453 a = sqrt((mu20_p + mu02_p + val_p) / 2);
454 b = sqrt((mu20_p + mu02_p - val_p) / 2);
456 e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
475 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS