31 #include <visp3/rbt/vpRBSilhouetteControlPoint.h>
32 #include <visp3/core/vpTrackingException.h>
33 #include <visp3/visual_features/vpFeatureBuilder.h>
34 #include <visp3/core/vpRobust.h>
36 #define VISP_DEBUG_RB_CONTROL_POINT 1
50 m_candidates.resize(1);
54 m_isSilhouette =
false;
66 m_me = meTracker.m_me;
68 m_numCandidates = meTracker.m_numCandidates;
69 m_cam = meTracker.m_cam;
73 norm = meTracker.norm;
74 normw = meTracker.normw;
80 m_isSilhouette = meTracker.m_isSilhouette;
82 theta = meTracker.theta;
83 thetaInit = meTracker.thetaInit;
84 delta = meTracker.delta;
85 sign = meTracker.sign;
86 featureline = meTracker.featureline;
87 line = meTracker.line;
88 m_valid = meTracker.m_valid;
99 m_me = std::move(meTracker.m_me);
100 s = std::move(meTracker.s);
101 thetaInit = std::move(meTracker.thetaInit);
102 m_numCandidates = std::move(meTracker.m_numCandidates);
103 m_cam = std::move(meTracker.m_cam);
104 icpoint = std::move(meTracker.icpoint);
105 cpoint = std::move(meTracker.cpoint);
106 cpointo = std::move(meTracker.cpointo);
107 norm = std::move(meTracker.norm);
108 normw = std::move(meTracker.normw);
109 xs = std::move(meTracker.xs);
110 ys = std::move(meTracker.ys);
111 nxs = std::move(meTracker.nxs);
112 nys = std::move(meTracker.nys);
113 Zs = std::move(meTracker.Zs);
114 m_isSilhouette = std::move(meTracker.m_isSilhouette);
115 rho = std::move(meTracker.rho);
116 theta = std::move(meTracker.theta);
117 delta = std::move(meTracker.delta);
118 sign = std::move(meTracker.sign);
119 featureline = std::move(meTracker.featureline);
120 line = std::move(meTracker.line);
121 m_valid = std::move(meTracker.m_valid);
125 int vpRBSilhouetteControlPoint::outOfImage(
int i,
int j,
int half,
int rows,
int cols)
const
127 return (!((i> half+2) &&
128 (i< rows -(half+2)) &&
134 int vpRBSilhouetteControlPoint::outOfImage(
const vpImagePoint &iP,
int half,
int rows,
int cols)
const
138 return (!((i> half+2) &&
139 (i< rows -(half+2)) &&
151 s.
track(I, m_me,
false);
154 s.
track(I, m_me,
false);
158 vpERROR_TRACE(
"caught a tracking exception, ignoring me point...");
174 vpERROR_TRACE(
"caught a tracking exception, ignoring me point...");
198 buildPlane(
cpoint, norm, plane);
204 if (abs(theta) > 1e-2) {
217 buildPlane(
cpointo, norm2, plane2);
218 buildPlane(
cpointo, normw, plane1);
235 double px = m_cam->
get_px();
236 double py = m_cam->
get_py();
244 rho = x*cos(theta)+y*sin(theta);
253 m_valid = isLineDegenerate();
265 double px = m_cam->
get_px();
266 double py = m_cam->
get_py();
286 #if VISP_DEBUG_RB_CONTROL_POINT
288 std::cerr <<
"Line in camera frame = " << line.
cP << std::endl;
293 m_valid = isLineDegenerate();
301 double px = m_cam->
get_px();
302 double py = m_cam->
get_py();
303 double uc = m_cam->
get_u0();
304 double vc = m_cam->
get_v0();
316 const double px = m_cam->
get_px();
317 const double py = m_cam->
get_py();
318 const double uc = m_cam->
get_u0();
319 const double vc = m_cam->
get_v0();
334 m_valid = !isLineDegenerate();
338 #if VISP_DEBUG_RB_CONTROL_POINT
339 if (std::isnan(theta)) {
343 if (fabs(theta - thetaInit) < M_PI / 2.0) {
357 double delta = theta;
359 if (m_me !=
nullptr) {
363 const double contrastThreshold = fabs(convolution) * marginRatio;
372 double c = cos(theta);
373 double s = sin(theta);
374 for (
int n = -range; n <= range; n++) {
375 unsigned int ii =
static_cast<unsigned int>(round(
icpoint.
get_i() + s * n));
376 unsigned int jj =
static_cast<unsigned int>(round(
icpoint.
get_j() + c * n));
377 unsigned int isBg =
static_cast<unsigned int>(I[ii][jj] == 0.f);
380 m_isSilhouette = k > 2;
392 if (!isLineDegenerate()) {
396 double rho0 = featureline.
getRho();
397 double theta0 = featureline.
getTheta();
398 #if VISP_DEBUG_RB_CONTROL_POINT
399 if (std::isnan(theta0)) {
400 std::cerr <<
"Line in camera frame = " << line.
cP.
t() << std::endl;
401 std::cerr <<
"Line in object frame = " << line.
oP.
t() << std::endl;
406 double co = cos(theta0);
407 double si = sin(theta0);
409 double mx = 1.0 / m_cam->
get_px();
410 double my = 1.0 / m_cam->
get_py();
411 double xc = m_cam->
get_u0();
412 double yc = m_cam->
get_v0();
416 double x = (double)s.
m_j, y = (
double)s.
m_i;
421 const double alpha = x*si - y*co;
424 double *Ltheta = H[1];
426 for (
unsigned int k = 0; k < 6; k++) {
427 L[i][k] = (Lrho[k] + alpha*Ltheta[k]);
429 e[i] = rho0 - (x*co + y*si);
435 for (
unsigned int k = 0; k < 6; k++) {
447 if (!isLineDegenerate()) {
451 const double rho0 = featureline.
getRho();
452 const double theta0 = featureline.
getTheta();
453 #if VISP_DEBUG_RB_CONTROL_POINT
454 if (std::isnan(theta0)) {
459 const double co = cos(theta0);
460 const double si = sin(theta0);
462 const double mx = 1.0 / m_cam->
get_px();
463 const double my = 1.0 / m_cam->
get_py();
464 const double xc = m_cam->
get_u0();
465 const double yc = m_cam->
get_v0();
468 double errormin = std::numeric_limits<double>::max();
470 const std::vector<vpMeSite> &cs = m_candidates;
471 xmin = (s.
m_j - xc) * mx;
472 ymin = (s.
m_i - yc) * my;
473 for (
unsigned int l = 0; l < m_numCandidates; l++)
478 const double x = (Pk.
m_j - xc) * mx;
479 const double y = (Pk.
m_i - yc) * my;
480 const double err = fabs(rho0 - (x * co + y * si));
481 if (err <= errormin) {
490 e[i] = rho0 - (xmin * co + ymin * si);
491 const double alpha = xmin * si - ymin * co;
493 const double *Lrho = H[0];
494 const double *Ltheta = H[1];
496 for (
unsigned int k = 0; k < 6; k++) {
497 L[i][k] = (Lrho[k] + alpha * Ltheta[k]);
502 for (
unsigned int k = 0; k < 6; k++) {
509 bool vpRBSilhouetteControlPoint::isLineDegenerate()
const
512 a = line.
cP[4] * line.
cP[3] - line.
cP[0] * line.
cP[7];
513 b = line.
cP[5] * line.
cP[3] - line.
cP[1] * line.
cP[7];
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_uv(double u, double v)
void projection() VP_OVERRIDE
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
static int round(double x)
Implementation of a matrix and operations on matrices.
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
@ THRESHOLD
Point not tracked due to the likelihood that is below the threshold, but retained in the ME list.
@ NO_SUPPRESSION
Point successfully tracked.
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
double m_convlt
Convolution of Site in previous image.
void trackMultipleHypotheses(const vpImage< unsigned char > &I, const vpMe &me, const bool &test_contrast, std::vector< vpMeSite > &outputHypotheses, const unsigned numCandidates)
vpMeSiteState getState() const
int m_j
Integer coordinates along j of a site.
int m_i
Integer coordinate along i of a site.
void setContrastThreshold(const double &thresh, const vpMe &me)
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
void setState(const vpMeSiteState &flag)
double getThresholdMarginRatio() const
This class defines the container for a plane geometrical structure.
vpPlane & init(const vpPoint &P, const vpColVector &normal, vpPlaneFrame frame=camera_frame)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
void projection(const vpColVector &_cP, vpColVector &_p) const VP_OVERRIDE
double get_y() const
Get the point y coordinate in the image plane.
double get_Y() const
Get the point cY coordinate in the camera frame.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
double get_Z() const
Get the point cZ coordinate in the camera frame.
double get_oY() const
Get the point oY coordinate in the object frame.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
double get_X() const
Get the point cX coordinate in the camera frame.
void setWorldCoordinates(double oX, double oY, double oZ)
Trackable silhouette point representation.
void buildSilhouettePoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam)
void detectSilhouette(const vpImage< float > &I)
vpRBSilhouetteControlPoint & operator=(const vpRBSilhouetteControlPoint &meTracker)
void updateSilhouettePoint(const vpHomogeneousMatrix &_cMo)
void trackMultipleHypotheses(const vpImage< unsigned char > &I)
Track the moving edge and retain the best numCandidates hypotheses.
vpRBSilhouetteControlPoint()
void buildPoint(int n, int m, const double &Z, double orient, const vpColVector &normo, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam, const vpMe &me)
void computeMeInteractionMatrixError(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e)
void track(const vpImage< unsigned char > &I)
Track the moving edge at this point retaining only the hypothesis with the highest likelihood.
void initControlPoint(const vpImage< unsigned char > &I, double cvlt)
void update(const vpHomogeneousMatrix &_cMo)
void computeMeInteractionMatrixErrorMH(const vpHomogeneousMatrix &cMo, unsigned int i, vpMatrix &L, vpColVector &e)
Implementation of a rotation matrix and operations on such kind of matrices.
Error that can be emitted by the vpTracker class and its derivatives.