Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
vpRBSilhouetteControlPoint.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  */
30 
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>
35 
36 #define VISP_DEBUG_RB_CONTROL_POINT 1
37 
38 BEGIN_VISP_NAMESPACE
39 
41 {
42  m_valid = false;
43 }
44 
46 {
47  init();
48  m_me = nullptr;
49  m_numCandidates = 1;
50  m_candidates.resize(1);
51  sign = 1;
52  norm.resize(3);
53  theta = 0;
54  m_isSilhouette = false;
55  m_valid = true;
56 }
57 
59 {
60  init();
61  *this = meTracker;
62 }
63 
65 {
66  m_me = meTracker.m_me;
67  s = meTracker.s;
68  m_numCandidates = meTracker.m_numCandidates;
69  m_cam = meTracker.m_cam;
70  icpoint = meTracker.icpoint;
71  cpoint = meTracker.cpoint;
72  cpointo = meTracker.cpointo;
73  norm = meTracker.norm;
74  normw = meTracker.normw;
75  xs = meTracker.xs;
76  ys = meTracker.ys;
77  nxs = meTracker.nxs;
78  nys = meTracker.nys;
79  Zs = meTracker.Zs;
80  m_isSilhouette = meTracker.m_isSilhouette;
81  rho = meTracker.rho;
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;
89  return *this;
90 }
91 
93 {
94  *this = meTracker;
95 }
96 
98 {
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);
122  return *this;
123 }
124 
125 int vpRBSilhouetteControlPoint::outOfImage(int i, int j, int half, int rows, int cols) const
126 {
127  return (!((i> half+2) &&
128  (i< rows -(half+2)) &&
129  (j>half+2) &&
130  (j<cols-(half+2)))
131  );
132 }
133 
134 int vpRBSilhouetteControlPoint::outOfImage(const vpImagePoint &iP, int half, int rows, int cols) const
135 {
136  int i = vpMath::round(iP.get_i());
137  int j = vpMath::round(iP.get_j());
138  return (!((i> half+2) &&
139  (i< rows -(half+2)) &&
140  (j>half+2) &&
141  (j<cols-(half+2)))
142  );
143 }
144 
146 {
147 
148  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
149  try {
150  if (s.m_convlt == 0) {
151  s.track(I, m_me, false);
152  }
153  else {
154  s.track(I, m_me, false);
155  }
156  }
157  catch (vpTrackingException &) {
158  vpERROR_TRACE("caught a tracking exception, ignoring me point...");
160  }
161  }
162 }
163 
165 {
166  // If element hasn't been suppressed
167  try {
168  if (s.getState() == vpMeSite::NO_SUPPRESSION) {
169  //const bool testContrast = s.m_convlt != 0.0;
170  s.trackMultipleHypotheses(I, *m_me, false, m_candidates, m_numCandidates);
171  }
172  }
173  catch (vpTrackingException &) {
174  vpERROR_TRACE("caught a tracking exception, ignoring me point...");
176  }
177 }
178 
186 void
187 vpRBSilhouetteControlPoint::buildPlane(const vpPoint &pointn, const vpColVector &normal, vpPlane &plane)
188 {
189  plane.init(pointn, normal, vpPlane::object_frame);
190 }
191 
192 void
193 vpRBSilhouetteControlPoint::buildPLine(const vpHomogeneousMatrix &oMc)
194 {
195  vpPlane plane;
196  vpPlane plane1;
197  vpPlane plane2;
198  buildPlane(cpoint, norm, plane);
200  oMc.extract(R);
201 
202  vpColVector V(3);
203  vpColVector Vo(3);
204  if (abs(theta) > 1e-2) {
205  V[0] = ((cpoint.get_oX()/cpoint.get_oZ())+1)*cpoint.get_oZ()-cpoint.get_oX();
206  V[1] = ((cpoint.get_oY()/cpoint.get_oZ())-cos(theta)/sin(theta))*cpoint.get_oZ()-cpoint.get_oY();
207  V[2] = (-plane.getD()-V[0]*plane.getA()-V[1]*plane.getB())/plane.getC()-cpoint.get_oZ();
208  }
209  else {
210  V[0] = ((cpoint.get_oX()/cpoint.get_oZ())+1)*cpoint.get_oZ()-cpoint.get_oX();
211  V[1] = ((cpoint.get_oY()/cpoint.get_oZ()))*cpoint.get_oZ()-cpoint.get_oY();
212  V[2] = (-plane.getD()-V[0]*plane.getA()-V[1]*plane.getB())/plane.getC()-cpoint.get_oZ();
213  }
214 
215  Vo = R*V;
216  vpColVector norm2 = vpColVector::cross(Vo, normw);
217  buildPlane(cpointo, norm2, plane2);
218  buildPlane(cpointo, normw, plane1);
219 
220  line.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(),
221  plane2.getA(), plane2.getB(), plane2.getC(), plane2.getD());
222 }
223 
224 void
225 vpRBSilhouetteControlPoint::buildPoint(int n, int m, const double &Z, double orient, const vpColVector &normo,
226  const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam, const vpMe &me)
227 {
228  m_cam = &cam;
229  m_me = &me;
230 
232  cMo.extract(R);
233  theta = orient;
234  thetaInit = theta;
235  double px = m_cam->get_px();
236  double py = m_cam->get_py();
237  int jc = m_cam->get_u0();
238  int ic = m_cam->get_v0();
239  icpoint.set_i(n);
240  icpoint.set_j(m);
241  double x, y;
242  x = (m-jc)/px;
243  y = (n-ic)/py;
244  rho = x*cos(theta)+y*sin(theta);
245  cpoint.setWorldCoordinates(x*Z, y*Z, Z);
246  cpoint.changeFrame(oMc);
248  normw = normo;
249  norm = R*normo;
250  nxs = cos(theta);
251  nys = sin(theta);
252  buildPLine(oMc);
253  m_valid = isLineDegenerate();
254 }
255 
256 void
257 vpRBSilhouetteControlPoint::buildSilhouettePoint(int n, int m, const double &Z, double orient, const vpColVector &normo,
258  const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &oMc, const vpCameraParameters &cam)
259 {
260  m_cam = &cam;
262  cMo.extract(R);
263  theta = orient;
264  thetaInit = theta;
265  double px = m_cam->get_px();
266  double py = m_cam->get_py();
267  int jc = m_cam->get_u0();
268  int ic = m_cam->get_v0();
269  icpoint.set_i(n);
270  icpoint.set_j(m);
271  xs = (m-jc)/px;
272  ys = (n-ic)/py;
273  Zs = Z;
274 
275  nxs = cos(theta);
276  nys = sin(theta);
277  double x, y;
278  x = (m-jc)/px;
279  y = (n-ic)/py;
280  cpoint.setWorldCoordinates(x * Z, y * Z, Z);
281  cpoint.changeFrame(oMc);
283  normw = normo;
284  norm = R * normo;
285  buildPLine(oMc);
286 #if VISP_DEBUG_RB_CONTROL_POINT
287  if (std::isnan(line.getTheta())) {
288  std::cerr << "Line in camera frame = " << line.cP << std::endl;
289  throw vpException(vpException::fatalError, "Incorrect line definition");
290 
291  }
292 #endif
293  m_valid = isLineDegenerate();
294 }
295 
296 void
298 {
299  cpointo.changeFrame(_cMo);
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();
305  double u, v;
306  v = py*cpointo.get_y()+vc;
307  u = px*cpointo.get_x()+uc;
308  icpoint.set_uv(u, v);
309 }
310 
311 void
313 {
314  cpointo.changeFrame(cMo);
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();
320  const double v = py * cpointo.get_y() + vc;
321  const double u = px * cpointo.get_x() + uc;
322  icpoint.set_uv(u, v);
323  xs = cpointo.get_x();
324  ys = cpointo.get_y();
325  Zs = cpointo.get_Z();
326  if (m_valid) {
327  try {
328  line.changeFrame(cMo);
329  line.projection();
330  }
331  catch (vpException &e) {
332  m_valid = false;
333  }
334  m_valid = !isLineDegenerate();
335  if (m_valid) {
336  vpFeatureBuilder::create(featureline, line);
337  theta = featureline.getTheta();
338 #if VISP_DEBUG_RB_CONTROL_POINT
339  if (std::isnan(theta)) {
340  throw vpException(vpException::fatalError, "Got nan theta in updateSilhouettePoint");
341  }
342 #endif
343  if (fabs(theta - thetaInit) < M_PI / 2.0) {
344  nxs = cos(theta);
345  nys = sin(theta);
346  }
347  else {
348  nxs = -cos(theta);
349  nys = -sin(theta);
350  }
351  }
352  }
353 }
354 
356 {
357  double delta = theta;
358  s.init((double)icpoint.get_i(), (double)icpoint.get_j(), delta, cvlt, sign);
359  if (m_me != nullptr) {
360  const double marginRatio = m_me->getThresholdMarginRatio();
361  const double convolution = s.convolution(I, m_me);
362  s.init((double)icpoint.get_i(), (double)icpoint.get_j(), delta, convolution, sign);
363  const double contrastThreshold = fabs(convolution) * marginRatio;
364  s.setContrastThreshold(contrastThreshold, *m_me);
365  }
366 }
367 
369 {
370  unsigned int k = 0;
371  int range = 4;
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);
378  k += isBg;
379  }
380  m_isSilhouette = k > 2;
381 }
382 
386 void
388 {
389  line.changeFrame(cMo);
390 
391  m_valid = false;
392  if (!isLineDegenerate()) {
393  line.projection();
394  vpFeatureBuilder::create(featureline, line);
395 
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;
402  featureline.print();
403  throw vpException(vpException::fatalError, "Got nan theta in computeInteractionMatrixError");
404  }
405 #endif
406  double co = cos(theta0);
407  double si = sin(theta0);
408 
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();
413 
414  vpMatrix H;
415  H = featureline.interaction();
416  double x = (double)s.m_j, y = (double)s.m_i;
417 
418  x = (x-xc)*mx;
419  y = (y-yc)*my;
420 
421  const double alpha = x*si - y*co;
422 
423  double *Lrho = H[0];
424  double *Ltheta = H[1];
425  // Calculate interaction matrix for a distance
426  for (unsigned int k = 0; k < 6; k++) {
427  L[i][k] = (Lrho[k] + alpha*Ltheta[k]);
428  }
429  e[i] = rho0 - (x*co + y*si);
430  m_valid = true;
431  }
432  else {
433  m_valid = false;
434  e[i] = 0;
435  for (unsigned int k = 0; k < 6; k++) {
436  L[i][k] = 0.0;
437  }
438  }
439 }
440 
441 void
443 {
444  line.changeFrame(cMo);
445 
446  m_valid = false;
447  if (!isLineDegenerate()) {
448  line.projection();
449 
450  vpFeatureBuilder::create(featureline, line);
451  const double rho0 = featureline.getRho();
452  const double theta0 = featureline.getTheta();
453 #if VISP_DEBUG_RB_CONTROL_POINT
454  if (std::isnan(theta0)) {
455  throw vpException(vpException::fatalError, "Got nan theta in computeInteractionMatrixMH");
456  }
457 #endif
458 
459  const double co = cos(theta0);
460  const double si = sin(theta0);
461 
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();
466  const vpMatrix &H = featureline.interaction();
467  double xmin, ymin;
468  double errormin = std::numeric_limits<double>::max();
469 
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++) //for each candidate of P
474  {
475  const vpMeSite &Pk = cs[l];
476 
477  if ((Pk.getState() == vpMeSite::NO_SUPPRESSION)) {
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) {
482  errormin = err;
483  xmin = x;
484  ymin = y;
485  m_valid = true;
486  }
487  }
488  }
489  if (m_valid) {
490  e[i] = rho0 - (xmin * co + ymin * si);
491  const double alpha = xmin * si - ymin * co;
492 
493  const double *Lrho = H[0];
494  const double *Ltheta = H[1];
495  // Calculate interaction matrix for a distance
496  for (unsigned int k = 0; k < 6; k++) {
497  L[i][k] = (Lrho[k] + alpha * Ltheta[k]);
498  }
499  }
500  else {
501  e[i] = 0;
502  for (unsigned int k = 0; k < 6; k++) {
503  L[i][k] = 0.0;
504  }
505  }
506  }
507 }
508 
509 bool vpRBSilhouetteControlPoint::isLineDegenerate() const
510 {
511  double a, b, d;
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];
514  d = a*a + b*b;
515  return d <= 1e-7;
516 }
517 
518 END_VISP_NAMESPACE
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpRowVector t() const
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:1264
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1148
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
double getTheta() const
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
double getRho() const
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 ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:309
double get_j() const
Definition: vpImagePoint.h:125
void set_i(double ii)
Definition: vpImagePoint.h:298
void set_uv(double u, double v)
Definition: vpImagePoint.h:357
double get_i() const
Definition: vpImagePoint.h:114
void projection() VP_OVERRIDE
Definition: vpLine.cpp:201
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
Definition: vpLine.cpp:348
double getTheta() const
Definition: vpLine.h:143
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)
Definition: vpLine.cpp:83
static int round(double x)
Definition: vpMath.h:410
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:68
@ THRESHOLD
Point not tracked due to the likelihood that is below the threshold, but retained in the ME list.
Definition: vpMeSite.h:91
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:86
double convolution(const vpImage< unsigned char > &ima, const vpMe *me)
Definition: vpMeSite.cpp:227
void init()
Definition: vpMeSite.cpp:70
double m_convlt
Convolution of Site in previous image.
Definition: vpMeSite.h:111
void trackMultipleHypotheses(const vpImage< unsigned char > &I, const vpMe &me, const bool &test_contrast, std::vector< vpMeSite > &outputHypotheses, const unsigned numCandidates)
Definition: vpMeSite.cpp:372
vpMeSiteState getState() const
Definition: vpMeSite.h:283
int m_j
Integer coordinates along j of a site.
Definition: vpMeSite.h:101
int m_i
Integer coordinate along i of a site.
Definition: vpMeSite.h:99
void setContrastThreshold(const double &thresh, const vpMe &me)
Definition: vpMeSite.h:303
void track(const vpImage< unsigned char > &I, const vpMe *me, const bool &test_contrast=true)
Definition: vpMeSite.cpp:282
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:273
Definition: vpMe.h:134
double getThresholdMarginRatio() const
Definition: vpMe.h:300
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:56
@ object_frame
Definition: vpPlane.h:64
vpPlane & init(const vpPoint &P, const vpColVector &normal, vpPlaneFrame frame=camera_frame)
Definition: vpPlane.cpp:157
double getD() const
Definition: vpPlane.h:106
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
double getB() const
Definition: vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:415
void projection(const vpColVector &_cP, vpColVector &_p) const VP_OVERRIDE
Definition: vpPoint.cpp:249
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:426
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:408
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:419
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:424
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:410
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:417
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
Definition: vpPoint.cpp:270
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:406
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
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.
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.
vpColVector cP
Definition: vpTracker.h:73
Error that can be emitted by the vpTracker class and its derivatives.