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);
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionnalities.
static const vpColor cyan
static const vpColor yellow
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCircle(const vpImage< unsigned char > &I, const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
void print(unsigned int select=FEATURE_ALL) const
static unsigned int selectAlpha()
void buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
static unsigned int selectXc()
void setAlpha(double val)
static unsigned int selectYc()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpFeatureSegment * duplicate() const
Feature duplication.
vpFeatureSegment(bool normalized=false)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
static unsigned int selectL()
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double deg(double rad)
Implementation of a matrix and operations on matrices.
void stack(const vpMatrix &A)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)