41 #include <visp3/core/vpDisplay.h>
42 #include <visp3/core/vpImagePoint.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpMeterPixelConversion.h>
45 #include <visp3/visual_features/vpBasicFeature.h>
46 #include <visp3/visual_features/vpFeatureSegment.h>
49 #include <visp3/core/vpException.h>
52 #include <visp3/core/vpDebug.h>
87 : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
196 if (
flags[i] ==
false) {
199 vpTRACE(
"Warning !!! The interaction matrix is computed but xc "
203 vpTRACE(
"Warning !!! The interaction matrix is computed but Yc "
207 vpTRACE(
"Warning !!! The interaction matrix is computed but l was "
211 vpTRACE(
"Warning !!! The interaction matrix is computed but alpha "
215 vpTRACE(
"Warning !!! The interaction matrix is computed but Z1 "
219 vpTRACE(
"Warning !!! The interaction matrix is computed but Z2 "
223 vpTRACE(
"Problem during the reading of the variable flags");
230 double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_);
231 double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_);
238 double lambda = -lambda1 * ln;
239 double Zn_inv = lambda2 * ln;
240 double lc = cos_a_ / ln;
241 double ls = sin_a_ / ln;
242 double xnalpha = xn * cos_a_ + yn * sin_a_;
243 double lnc = cos_a_ * ln;
244 double lns = sin_a_ * ln;
248 Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
249 Lxn[0][1] = lambda * xn * sin_a_;
250 Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
251 Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
252 Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
259 Lyn[0][0] = lambda * yn * cos_a_;
260 Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
261 Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
262 Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
263 Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
270 Lln[0][0] = lambda * lnc;
271 Lln[0][1] = lambda * lns;
272 Lln[0][2] = -(Zn_inv + lambda * xnalpha);
273 Lln[0][3] = -yn - xnalpha * sin_a_;
274 Lln[0][4] = xn + xnalpha * cos_a_;
282 Lalpha[0][0] = -lambda1 * sin_a_ * l_;
283 Lalpha[0][1] = lambda1 * cos_a_ * l_;
284 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
285 Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
286 Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
293 Lxc[0][0] = -lambda2;
295 Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
296 Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
297 Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
305 Lyc[0][1] = -lambda2;
306 Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
307 Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
308 Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
315 Ll[0][0] = lambda1 * cos_a_;
316 Ll[0][1] = lambda1 * sin_a_;
317 Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
318 Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
319 Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
325 Lalpha[0][0] = -lambda1 * sin_a_ / l_;
326 Lalpha[0][1] = lambda1 * cos_a_ / l_;
327 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
328 Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
329 Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
369 exc[0] = xc_ - s_star[0];
375 eyc[0] = yc_ - s_star[1];
381 eL[0] = l_ - s_star[2];
387 eAlpha[0] = alpha_ - s_star[3];
388 while (eAlpha[0] < -M_PI)
389 eAlpha[0] += 2 * M_PI;
390 while (eAlpha[0] > M_PI)
391 eAlpha[0] -= 2 * M_PI;
426 std::cout <<
"vpFeatureSegment: (";
429 std::cout <<
"xn = ";
431 std::cout <<
"xc = ";
432 std::cout <<
s[0] <<
"; ";
436 std::cout <<
"yn = ";
438 std::cout <<
"yc = ";
439 std::cout <<
s[1] <<
"; ";
443 std::cout <<
"ln = ";
446 std::cout <<
s[2] <<
"; ";
451 std::cout <<
")" << std::endl;
484 unsigned int thickness)
const
497 double x1 = x - (l / 2.) * cos_a_;
498 double x2 = x + (l / 2.) * cos_a_;
500 double y1 = y - (l / 2.) * sin_a_;
501 double y2 = y + (l / 2.) * sin_a_;
522 unsigned int thickness)
const
535 double x1 = x - (l / 2.) * cos_a_;
536 double x2 = x + (l / 2.) * cos_a_;
538 double y1 = y - (l / 2.) * sin_a_;
539 double y2 = y + (l / 2.) * sin_a_;
568 double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
569 double x_c = (x1 + x2) / 2.;
570 double y_c = (y1 + y2) / 2.;
571 double alpha = atan2(y1 - y2, x1 - x2);