40 #include <visp3/core/vpDisplay.h>
41 #include <visp3/core/vpImagePoint.h>
42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 #include <visp3/visual_features/vpBasicFeature.h>
45 #include <visp3/visual_features/vpFeatureSegment.h>
48 #include <visp3/core/vpException.h>
51 #include <visp3/core/vpDebug.h>
86 : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
195 if (
flags[i] ==
false) {
198 vpTRACE(
"Warning !!! The interaction matrix is computed but xc "
202 vpTRACE(
"Warning !!! The interaction matrix is computed but Yc "
206 vpTRACE(
"Warning !!! The interaction matrix is computed but l was "
210 vpTRACE(
"Warning !!! The interaction matrix is computed but alpha "
214 vpTRACE(
"Warning !!! The interaction matrix is computed but Z1 "
218 vpTRACE(
"Warning !!! The interaction matrix is computed but Z2 "
222 vpTRACE(
"Problem during the reading of the variable flags");
229 double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_);
230 double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_);
237 double lambda = -lambda1 * ln;
238 double Zn_inv = lambda2 * ln;
239 double lc = cos_a_ / ln;
240 double ls = sin_a_ / ln;
241 double xnalpha = xn * cos_a_ + yn * sin_a_;
242 double lnc = cos_a_ * ln;
243 double lns = sin_a_ * ln;
247 Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
248 Lxn[0][1] = lambda * xn * sin_a_;
249 Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
250 Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
251 Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
258 Lyn[0][0] = lambda * yn * cos_a_;
259 Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
260 Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
261 Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
262 Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
269 Lln[0][0] = lambda * lnc;
270 Lln[0][1] = lambda * lns;
271 Lln[0][2] = -(Zn_inv + lambda * xnalpha);
272 Lln[0][3] = -yn - xnalpha * sin_a_;
273 Lln[0][4] = xn + xnalpha * cos_a_;
281 Lalpha[0][0] = -lambda1 * sin_a_ * l_;
282 Lalpha[0][1] = lambda1 * cos_a_ * l_;
283 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
284 Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
285 Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
292 Lxc[0][0] = -lambda2;
294 Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
295 Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
296 Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
304 Lyc[0][1] = -lambda2;
305 Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
306 Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
307 Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
314 Ll[0][0] = lambda1 * cos_a_;
315 Ll[0][1] = lambda1 * sin_a_;
316 Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
317 Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
318 Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
324 Lalpha[0][0] = -lambda1 * sin_a_ / l_;
325 Lalpha[0][1] = lambda1 * cos_a_ / l_;
326 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
327 Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
328 Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
368 exc[0] = xc_ - s_star[0];
374 eyc[0] = yc_ - s_star[1];
380 eL[0] = l_ - s_star[2];
386 eAlpha[0] = alpha_ - s_star[3];
387 while (eAlpha[0] < -M_PI)
388 eAlpha[0] += 2 * M_PI;
389 while (eAlpha[0] > M_PI)
390 eAlpha[0] -= 2 * M_PI;
425 std::cout <<
"vpFeatureSegment: (";
428 std::cout <<
"xn = ";
430 std::cout <<
"xc = ";
431 std::cout <<
s[0] <<
"; ";
435 std::cout <<
"yn = ";
437 std::cout <<
"yc = ";
438 std::cout <<
s[1] <<
"; ";
442 std::cout <<
"ln = ";
445 std::cout <<
s[2] <<
"; ";
450 std::cout <<
")" << std::endl;
483 unsigned int thickness)
const
496 double x1 = x - (l / 2.) * cos_a_;
497 double x2 = x + (l / 2.) * cos_a_;
499 double y1 = y - (l / 2.) * sin_a_;
500 double y2 = y + (l / 2.) * sin_a_;
521 unsigned int thickness)
const
534 double x1 = x - (l / 2.) * cos_a_;
535 double x2 = x + (l / 2.) * cos_a_;
537 double y1 = y - (l / 2.) * sin_a_;
538 double y2 = y + (l / 2.) * sin_a_;
567 double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
568 double x_c = (x1 + x2) / 2.;
569 double y_c = (y1 + y2) / 2.;
570 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 functionalities.
static const vpColor cyan
static const vpColor yellow
static void displayCircle(const vpImage< unsigned char > &I, const vpImageCircle &circle, const vpColor &color, bool fill=false, unsigned int thickness=1)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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)