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...
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()
vpMatrix interaction(unsigned int select=FEATURE_ALL) override
vpFeatureSegment * duplicate() const override
Feature duplication.
void print(unsigned int select=FEATURE_ALL) const override
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const override
vpFeatureSegment(bool normalized=false)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) override
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)