35 #include <visp3/core/vpDisplay.h>
36 #include <visp3/core/vpImagePoint.h>
37 #include <visp3/core/vpMath.h>
38 #include <visp3/core/vpMeterPixelConversion.h>
39 #include <visp3/visual_features/vpBasicFeature.h>
40 #include <visp3/visual_features/vpFeatureSegment.h>
43 #include <visp3/core/vpException.h>
46 #include <visp3/core/vpDebug.h>
78 : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
185 if (
flags[i] ==
false) {
188 vpTRACE(
"Warning !!! The interaction matrix is computed but xc "
192 vpTRACE(
"Warning !!! The interaction matrix is computed but Yc "
196 vpTRACE(
"Warning !!! The interaction matrix is computed but l was "
200 vpTRACE(
"Warning !!! The interaction matrix is computed but alpha "
204 vpTRACE(
"Warning !!! The interaction matrix is computed but Z1 "
208 vpTRACE(
"Warning !!! The interaction matrix is computed but Z2 "
212 vpTRACE(
"Problem during the reading of the variable flags");
219 double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_);
220 double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_);
227 double lambda = -lambda1 * ln;
228 double Zn_inv = lambda2 * ln;
229 double lc = cos_a_ / ln;
230 double ls = sin_a_ / ln;
231 double xnalpha = xn * cos_a_ + yn * sin_a_;
232 double lnc = cos_a_ * ln;
233 double lns = sin_a_ * ln;
237 Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
238 Lxn[0][1] = lambda * xn * sin_a_;
239 Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
240 Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
241 Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
248 Lyn[0][0] = lambda * yn * cos_a_;
249 Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
250 Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
251 Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
252 Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
259 Lln[0][0] = lambda * lnc;
260 Lln[0][1] = lambda * lns;
261 Lln[0][2] = -(Zn_inv + lambda * xnalpha);
262 Lln[0][3] = -yn - xnalpha * sin_a_;
263 Lln[0][4] = xn + xnalpha * cos_a_;
271 Lalpha[0][0] = -lambda1 * sin_a_ * l_;
272 Lalpha[0][1] = lambda1 * cos_a_ * l_;
273 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
274 Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
275 Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
283 Lxc[0][0] = -lambda2;
285 Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
286 Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
287 Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
295 Lyc[0][1] = -lambda2;
296 Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
297 Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
298 Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
305 Ll[0][0] = lambda1 * cos_a_;
306 Ll[0][1] = lambda1 * sin_a_;
307 Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
308 Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
309 Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
315 Lalpha[0][0] = -lambda1 * sin_a_ / l_;
316 Lalpha[0][1] = lambda1 * cos_a_ / l_;
317 Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
318 Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
319 Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
358 exc[0] = xc_ - s_star[0];
364 eyc[0] = yc_ - s_star[1];
370 eL[0] = l_ - s_star[2];
376 eAlpha[0] = alpha_ - s_star[3];
377 while (eAlpha[0] < -M_PI)
378 eAlpha[0] += 2 * M_PI;
379 while (eAlpha[0] > M_PI)
380 eAlpha[0] -= 2 * M_PI;
415 std::cout <<
"vpFeatureSegment: (";
418 std::cout <<
"xn = ";
420 std::cout <<
"xc = ";
421 std::cout <<
s[0] <<
"; ";
425 std::cout <<
"yn = ";
427 std::cout <<
"yc = ";
428 std::cout <<
s[1] <<
"; ";
432 std::cout <<
"ln = ";
435 std::cout <<
s[2] <<
"; ";
440 std::cout <<
")" << std::endl;
470 unsigned int thickness)
const
484 double x1 = x - (l / 2.) * cos_a_;
485 double x2 = x + (l / 2.) * cos_a_;
487 double y1 = y - (l / 2.) * sin_a_;
488 double y2 = y + (l / 2.) * sin_a_;
508 unsigned int thickness)
const
522 double x1 = x - (l / 2.) * cos_a_;
523 double x2 = x + (l / 2.) * cos_a_;
525 double y1 = y - (l / 2.) * sin_a_;
526 double y2 = y + (l / 2.) * sin_a_;
554 double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
555 double x_c = (x1 + x2) / 2.;
556 double y_c = (y1 + y2) / 2.;
557 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 vp_override
static unsigned int selectAlpha()
void buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
static unsigned int selectXc()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
void setAlpha(double val)
static unsigned int selectYc()
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
vpFeatureSegment(bool normalized=false)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
vpFeatureSegment * duplicate() const vp_override
Feature duplication.
static unsigned int selectL()
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)