Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpMbtDistanceKltCylinder.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Klt cylinder, containing points of interest.
33  *
34  * Authors:
35  * Aurelien Yol
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpPolygon.h>
40 #include <visp3/mbt/vpMbtDistanceKltCylinder.h>
41 #include <visp3/mbt/vpMbtDistanceKltPoints.h>
42 
43 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
44 
45 #if defined(VISP_HAVE_CLIPPER)
46 #include <clipper.hpp> // clipper private library
47 #endif
48 
49 #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
50 #include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
51 #endif
52 
58  : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(), initPoints(), initPoints3D(), curPoints(),
59  curPointsInd(), nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), cam(),
60  isTrackedKltCylinder(true), listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false)
61 {
62 }
63 
69 
70 void vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
71 {
72  p1Ext = p1;
73  p2Ext = p2;
74 
75  vpColVector ABC(3);
76  vpColVector V1(3);
77  vpColVector V2(3);
78 
79  V1[0] = p1.get_oX();
80  V1[1] = p1.get_oY();
81  V1[2] = p1.get_oZ();
82  V2[0] = p2.get_oX();
83  V2[1] = p2.get_oY();
84  V2[2] = p2.get_oZ();
85 
86  // Get the axis of the cylinder
87  ABC = V1 - V2;
88 
89  // Build our extremity circles
90  circle1.setWorldCoordinates(ABC[0], ABC[1], ABC[2], p1.get_oX(), p1.get_oY(), p1.get_oZ(), r);
91  circle2.setWorldCoordinates(ABC[0], ABC[1], ABC[2], p2.get_oX(), p2.get_oY(), p2.get_oZ(), r);
92 
93  // Build our cylinder
94  cylinder.setWorldCoordinates(ABC[0], ABC[1], ABC[2], (p1.get_oX() + p2.get_oX()) / 2.0,
95  (p1.get_oY() + p2.get_oY()) / 2.0, (p1.get_oZ() + p2.get_oZ()) / 2.0, r);
96 }
97 
107 {
108  c0Mo = cMo;
109  cylinder.changeFrame(cMo);
110 
111  // extract ids of the points in the face
112  nbPointsInit = 0;
113  nbPointsCur = 0;
114  initPoints = std::map<int, vpImagePoint>();
115  initPoints3D = std::map<int, vpPoint>();
116  curPoints = std::map<int, vpImagePoint>();
117  curPointsInd = std::map<int, int>();
118 
119  for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
120  long id;
121  float x_tmp, y_tmp;
122  _tracker.getFeature((int)i, id, x_tmp, y_tmp);
123 
124  bool add = false;
125 
126  if (useScanLine) {
127  if ((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
128  (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth()) {
129  for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++)
130  if (hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] ==
132  add = true;
133  break;
134  }
135  }
136  } else {
137  std::vector<vpImagePoint> roi;
138  for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) {
139  hiddenface->getPolygon()[(size_t)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
140  if (vpPolygon::isInside(roi, y_tmp, x_tmp)) {
141  add = true;
142  break;
143  }
144  roi.clear();
145  }
146  }
147 
148  if (add) {
149 
150  double xm = 0, ym = 0;
151  vpPixelMeterConversion::convertPoint(cam, x_tmp, y_tmp, xm, ym);
152  double Z = computeZ(xm, ym);
153  if (!vpMath::isNaN(Z)) {
154 #if TARGET_OS_IPHONE
155  initPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
156  curPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
157  curPointsInd[(int)id] = (int)i;
158 #else
159  initPoints[id] = vpImagePoint(y_tmp, x_tmp);
160  curPoints[id] = vpImagePoint(y_tmp, x_tmp);
161  curPointsInd[id] = (int)i;
162 #endif
163  nbPointsInit++;
164  nbPointsCur++;
165 
166  vpPoint p;
167  p.setWorldCoordinates(xm * Z, ym * Z, Z);
168 #if TARGET_OS_IPHONE
169  initPoints3D[(int)id] = p;
170 #else
171  initPoints3D[id] = p;
172 #endif
173  // std::cout << "Computed Z for : " << xm << "," << ym << " : " <<
174  // computeZ(xm,ym) << std::endl;
175  }
176  }
177  }
178 
179  if (nbPointsCur >= minNbPoint)
180  enoughPoints = true;
181  else
182  enoughPoints = false;
183 
184  // std::cout << "Nb detected points in cylinder : " << nbPointsCur <<
185  // std::endl;
186 }
187 
197 {
198  long id;
199  float x, y;
200  nbPointsCur = 0;
201  curPoints = std::map<int, vpImagePoint>();
202  curPointsInd = std::map<int, int>();
203 
204  for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
205  _tracker.getFeature((int)i, id, x, y);
206  if (isTrackedFeature((int)id)) {
207 #if TARGET_OS_IPHONE
208  curPoints[(int)id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
209  curPointsInd[(int)id] = (int)i;
210 #else
211  curPoints[id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
212  curPointsInd[id] = (int)i;
213 #endif
214  nbPointsCur++;
215  }
216  }
217 
218  if (nbPointsCur >= minNbPoint)
219  enoughPoints = true;
220  else
221  enoughPoints = false;
222 
223  return nbPointsCur;
224 }
225 
234 void vpMbtDistanceKltCylinder::removeOutliers(const vpColVector &_w, const double &threshold_outlier)
235 {
236  std::map<int, vpImagePoint> tmp;
237  std::map<int, int> tmp2;
238  unsigned int nbSupp = 0;
239  unsigned int k = 0;
240 
241  nbPointsCur = 0;
242  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
243  for (; iter != curPoints.end(); ++iter) {
244  if (_w[k] > threshold_outlier && _w[k + 1] > threshold_outlier) {
245  // if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
246  tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
247  tmp2[iter->first] = curPointsInd[iter->first];
248  nbPointsCur++;
249  } else {
250  nbSupp++;
251  initPoints.erase(iter->first);
252  }
253 
254  k += 2;
255  }
256 
257  if (nbSupp != 0) {
258  curPoints = std::map<int, vpImagePoint>();
259  curPointsInd = std::map<int, int>();
260 
261  curPoints = tmp;
262  curPointsInd = tmp2;
263  if (nbPointsCur >= minNbPoint)
264  enoughPoints = true;
265  else
266  enoughPoints = false;
267  }
268 }
269 
282  vpMatrix &_J)
283 {
284  unsigned int index_ = 0;
285 
286  cylinder.changeFrame(_cMc0 * c0Mo);
287 
288  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
289  for (; iter != curPoints.end(); ++iter) {
290  int id(iter->first);
291  double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
292 
293  double x_cur(0), y_cur(0);
294  vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);
295 
296  vpPoint p0 = initPoints3D[id];
297  p0.changeFrame(_cMc0);
298  p0.project();
299 
300  double x0_transform(p0.get_x()), y0_transform(p0.get_y());
301 
302  double Z = computeZ(x_cur, y_cur);
303 
304  if (vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()) {
305  // std::cout << "Z is Nan : " << A << " , " << B << " , " << C << "
306  // | " << Z << " | " << x_cur << " , " << y_cur << std::endl;
307  // std::cout << std::sqrt(B*B - A*C) << " , " << B*B - A*C <<
308  // std::endl;
309 
310  _J[2 * index_][0] = 0;
311  _J[2 * index_][1] = 0;
312  _J[2 * index_][2] = 0;
313  _J[2 * index_][3] = 0;
314  _J[2 * index_][4] = 0;
315  _J[2 * index_][5] = 0;
316 
317  _J[2 * index_ + 1][0] = 0;
318  _J[2 * index_ + 1][1] = 0;
319  _J[2 * index_ + 1][2] = 0;
320  _J[2 * index_ + 1][3] = 0;
321  _J[2 * index_ + 1][4] = 0;
322  _J[2 * index_ + 1][5] = 0;
323 
324  _R[2 * index_] = (x0_transform - x_cur);
325  _R[2 * index_ + 1] = (y0_transform - y_cur);
326  index_++;
327  } else {
328  double invZ = 1.0 / Z;
329 
330  _J[2 * index_][0] = -invZ;
331  _J[2 * index_][1] = 0;
332  _J[2 * index_][2] = x_cur * invZ;
333  _J[2 * index_][3] = x_cur * y_cur;
334  _J[2 * index_][4] = -(1 + x_cur * x_cur);
335  _J[2 * index_][5] = y_cur;
336 
337  _J[2 * index_ + 1][0] = 0;
338  _J[2 * index_ + 1][1] = -invZ;
339  _J[2 * index_ + 1][2] = y_cur * invZ;
340  _J[2 * index_ + 1][3] = (1 + y_cur * y_cur);
341  _J[2 * index_ + 1][4] = -y_cur * x_cur;
342  _J[2 * index_ + 1][5] = -x_cur;
343 
344  _R[2 * index_] = (x0_transform - x_cur);
345  _R[2 * index_ + 1] = (y0_transform - y_cur);
346  index_++;
347  }
348  }
349 }
350 
358 bool vpMbtDistanceKltCylinder::isTrackedFeature(int _id)
359 {
360  std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
361  if (iter != initPoints.end())
362  return true;
363 
364  return false;
365 }
366 
377 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
378  cv::Mat &mask,
379 #else
380  IplImage *mask,
381 #endif
382  unsigned char nb, unsigned int shiftBorder)
383 {
384 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
385  int width = mask.cols;
386  int height = mask.rows;
387 #else
388  int width = mask->width;
389  int height = mask->height;
390 #endif
391 
392  for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) {
393  if ((*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->isVisible() &&
394  (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getNbPoint() > 2) {
395  int i_min, i_max, j_min, j_max;
396  std::vector<vpImagePoint> roi;
397  (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
398 
399  double shiftBorder_d = (double)shiftBorder;
400 #if defined(VISP_HAVE_CLIPPER)
401  std::vector<vpImagePoint> roi_offset;
402 
403  ClipperLib::Path path;
404  for (std::vector<vpImagePoint>::const_iterator it = roi.begin(); it != roi.end(); ++it) {
405  path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), (ClipperLib::cInt)it->get_v()));
406  }
407 
408  ClipperLib::Paths solution;
409  ClipperLib::ClipperOffset co;
410  co.AddPath(path, ClipperLib::jtRound, ClipperLib::etClosedPolygon);
411  co.Execute(solution, -shiftBorder_d);
412 
413  // Keep biggest polygon by area
414  if (!solution.empty()) {
415  size_t index_max = 0;
416 
417  if (solution.size() > 1) {
418  double max_area = 0;
419  vpPolygon polygon_area;
420 
421  for (size_t i = 0; i < solution.size(); i++) {
422  std::vector<vpImagePoint> corners;
423 
424  for (size_t j = 0; j < solution[i].size(); j++) {
425  corners.push_back(vpImagePoint((double)(solution[i][j].Y), (double)(solution[i][j].X)));
426  }
427 
428  polygon_area.buildFrom(corners);
429  if (polygon_area.getArea() > max_area) {
430  max_area = polygon_area.getArea();
431  index_max = i;
432  }
433  }
434  }
435 
436  for (size_t i = 0; i < solution[index_max].size(); i++) {
437  roi_offset.push_back(vpImagePoint((double)(solution[index_max][i].Y), (double)(solution[index_max][i].X)));
438  }
439  } else {
440  roi_offset = roi;
441  }
442 
443  vpPolygon polygon_test(roi_offset);
444  vpImagePoint imPt;
445 #endif
446 
447 #if defined(VISP_HAVE_CLIPPER)
448  vpPolygon3D::getMinMaxRoi(roi_offset, i_min, i_max, j_min, j_max);
449 #else
450  vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min, j_max);
451 #endif
452 
453  /* check image boundaries */
454  if (i_min > height) { // underflow
455  i_min = 0;
456  }
457  if (i_max > height) {
458  i_max = height;
459  }
460  if (j_min > width) { // underflow
461  j_min = 0;
462  }
463  if (j_max > width) {
464  j_max = width;
465  }
466 
467 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
468  for (int i = i_min; i < i_max; i++) {
469  double i_d = (double)i;
470 
471  for (int j = j_min; j < j_max; j++) {
472  double j_d = (double)j;
473 
474 #if defined(VISP_HAVE_CLIPPER)
475  imPt.set_ij(i_d, j_d);
476  if (polygon_test.isInside(imPt)) {
477  mask.ptr<uchar>(i)[j] = nb;
478  }
479 #else
480  if (shiftBorder != 0) {
481  if (vpPolygon::isInside(roi, i_d, j_d) &&
482  vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d + shiftBorder_d) &&
483  vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d + shiftBorder_d) &&
484  vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d - shiftBorder_d) &&
485  vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d - shiftBorder_d)) {
486  mask.at<unsigned char>(i, j) = nb;
487  }
488  } else {
489  if (vpPolygon::isInside(roi, i, j)) {
490  mask.at<unsigned char>(i, j) = nb;
491  }
492  }
493 #endif
494  }
495  }
496 #else
497  unsigned char *ptrData = (unsigned char *)mask->imageData + i_min * mask->widthStep + j_min;
498  for (int i = i_min; i < i_max; i++) {
499  double i_d = (double)i;
500  for (int j = j_min; j < j_max; j++) {
501  double j_d = (double)j;
502  if (shiftBorder != 0) {
503  if (vpPolygon::isInside(roi, i_d, j_d) &&
504  vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d + shiftBorder_d) &&
505  vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d + shiftBorder_d) &&
506  vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d - shiftBorder_d) &&
507  vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d - shiftBorder_d)) {
508  *(ptrData++) = nb;
509  } else {
510  ptrData++;
511  }
512  } else {
513  if (vpPolygon::isInside(roi, i, j)) {
514  *(ptrData++) = nb;
515  } else {
516  ptrData++;
517  }
518  }
519  }
520  ptrData += mask->widthStep - j_max + j_min;
521  }
522 #endif
523  }
524  }
525 }
526 
533 {
534  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
535  for (; iter != curPoints.end(); ++iter) {
536  int id(iter->first);
537  vpImagePoint iP;
538  iP.set_i(static_cast<double>(iter->second.get_i()));
539  iP.set_j(static_cast<double>(iter->second.get_j()));
540 
542 
543  iP.set_i(vpMath::round(iP.get_i() + 7));
544  iP.set_j(vpMath::round(iP.get_j() + 7));
545  std::stringstream ss;
546  ss << id;
547  vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
548  }
549 }
550 
557 {
558  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
559  for (; iter != curPoints.end(); ++iter) {
560  int id(iter->first);
561  vpImagePoint iP;
562  iP.set_i(static_cast<double>(iter->second.get_i()));
563  iP.set_j(static_cast<double>(iter->second.get_j()));
564 
566 
567  iP.set_i(vpMath::round(iP.get_i() + 7));
568  iP.set_j(vpMath::round(iP.get_j() + 7));
569  std::stringstream ss;
570  ss << id;
571  vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
572  }
573 }
574 
576  const vpCameraParameters &camera, const vpColor &col,
577  unsigned int thickness, const bool /*displayFullModel*/)
578 {
579  std::vector<std::vector<double> > models = getModelForDisplay(cMo, camera);
580 
581  for (size_t i = 0; i < models.size(); i++) {
582  vpImagePoint ip1(models[i][1], models[i][2]);
583  vpImagePoint ip2(models[i][3], models[i][4]);
584  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
585  }
586 }
587 
589  const vpCameraParameters &camera, const vpColor &col,
590  unsigned int thickness, const bool /*displayFullModel*/)
591 {
592  std::vector<std::vector<double> > models = getModelForDisplay(cMo, camera);
593 
594  for (size_t i = 0; i < models.size(); i++) {
595  vpImagePoint ip1(models[i][1], models[i][2]);
596  vpImagePoint ip2(models[i][3], models[i][4]);
597 
598  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
599  }
600 }
601 
607 std::vector<std::vector<double> > vpMbtDistanceKltCylinder::getFeaturesForDisplay()
608 {
609  std::vector<std::vector<double> > features;
610 
611  std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
612  for (; iter != curPoints.end(); ++iter) {
613  int id(iter->first);
614  vpImagePoint iP;
615  iP.set_i(static_cast<double>(iter->second.get_i()));
616  iP.set_j(static_cast<double>(iter->second.get_j()));
617 
618  vpImagePoint iP2;
619  iP2.set_i(vpMath::round(iP.get_i() + 7));
620  iP2.set_j(vpMath::round(iP.get_j() + 7));
621 
622 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
623  std::vector<double> params = {1, //KLT
624  iP.get_i(),
625  iP.get_j(),
626  iP2.get_i(),
627  iP2.get_j(),
628  static_cast<double>(id)};
629 #else
630  std::vector<double> params;
631  params.push_back(1); //KLT
632  params.push_back(iP.get_i());
633  params.push_back(iP.get_j());
634  params.push_back(iP2.get_i());
635  params.push_back(iP2.get_j());
636  params.push_back(static_cast<double>(id));
637 #endif
638  features.push_back(params);
639  }
640 
641  return features;
642 }
643 
652 std::vector<std::vector<double> > vpMbtDistanceKltCylinder::getModelForDisplay(const vpHomogeneousMatrix &cMo,
653  const vpCameraParameters &camera)
654 {
655  std::vector<std::vector<double> > models;
656 
657  // if(isvisible || displayFullModel)
658  {
659  // Perspective projection
660  circle1.changeFrame(cMo);
661  circle2.changeFrame(cMo);
662  cylinder.changeFrame(cMo);
663 
664  try {
665  circle1.projection();
666  } catch (...) {
667  std::cout << "Problem projection circle 1";
668  }
669  try {
670  circle2.projection();
671  } catch (...) {
672  std::cout << "Problem projection circle 2";
673  }
674 
675  cylinder.projection();
676 
677  double rho1, theta1;
678  double rho2, theta2;
679 
680  // Meters to pixels conversion
681  vpMeterPixelConversion::convertLine(camera, cylinder.getRho1(), cylinder.getTheta1(), rho1, theta1);
682  vpMeterPixelConversion::convertLine(camera, cylinder.getRho2(), cylinder.getTheta2(), rho2, theta2);
683 
684  // Determine intersections between circles and limbos
685  double i11, i12, i21, i22, j11, j12, j21, j22;
686 
687  vpCircle::computeIntersectionPoint(circle1, camera, rho1, theta1, i11, j11);
688  vpCircle::computeIntersectionPoint(circle2, camera, rho1, theta1, i12, j12);
689 
690  vpCircle::computeIntersectionPoint(circle1, camera, rho2, theta2, i21, j21);
691  vpCircle::computeIntersectionPoint(circle2, camera, rho2, theta2, i22, j22);
692 
693  // Create the image points
694  vpImagePoint ip11, ip12, ip21, ip22;
695  ip11.set_ij(i11, j11);
696  ip12.set_ij(i12, j12);
697  ip21.set_ij(i21, j21);
698  ip22.set_ij(i22, j22);
699 
700 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
701  std::vector<double> params1 = {0, //line parameters
702  ip11.get_i(),
703  ip11.get_j(),
704  ip12.get_i(),
705  ip12.get_j()};
706  models.push_back(params1);
707 
708  std::vector<double> params2 = {0, //line parameters
709  ip21.get_i(),
710  ip21.get_j(),
711  ip22.get_i(),
712  ip22.get_j()};
713 #else
714  std::vector<double> params1, params2;
715  params1.push_back(0); //line parameters
716  params1.push_back(ip11.get_i());
717  params1.push_back(ip11.get_j());
718  params1.push_back(ip12.get_i());
719  params1.push_back(ip12.get_j());
720 
721  params2.push_back(0); //line parameters
722  params2.push_back(ip21.get_i());
723  params2.push_back(ip21.get_j());
724  params2.push_back(ip22.get_i());
725  params2.push_back(ip22.get_j());
726 #endif
727  models.push_back(params1);
728  models.push_back(params2);
729  }
730 
731  return models;
732 }
733 
734 // ######################
735 // Private Functions
736 // ######################
737 
738 double vpMbtDistanceKltCylinder::computeZ(const double &x, const double &y)
739 {
740  // double A = x*x + y*y + 1 -
741  // ((cylinder.getA()*x+cylinder.getB()*y+cylinder.getC()) *
742  // (cylinder.getA()*x+cylinder.getB()*y+cylinder.getC())); double B = (x *
743  // cylinder.getX() + y * cylinder.getY() + cylinder.getZ()); double C =
744  // cylinder.getX() * cylinder.getX() + cylinder.getY() * cylinder.getY() +
745  // cylinder.getZ() * cylinder.getZ() - cylinder.getR() * cylinder.getR();
746  //
747  // return (B - std::sqrt(B*B - A*C))/A;
748 
749  return cylinder.computeZ(x, y);
750 }
751 #elif !defined(VISP_BUILD_SHARED_LIBS)
752 // Work arround to avoid warning:
753 // libvisp_mbt.a(vpMbtDistanceKltCylinder.cpp.o) has no symbols
754 void dummy_vpMbtDistanceKltCylinder(){};
755 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
double get_i() const
Definition: vpImagePoint.h:204
void displayPrimitive(const vpImage< unsigned char > &_I)
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:424
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:237
Implementation of an homogeneous matrix and operations on such kind of matrices.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
double getArea() const
Definition: vpPolygon.h:161
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
vpMbScanLine & getMbScanLineRenderer()
double computeZ(double x, double y) const
Definition: vpCylinder.cpp:363
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:422
static void getMinMaxRoi(const std::vector< vpImagePoint > &roi, int &i_min, int &i_max, int &j_min, int &j_max)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:270
static const vpColor red
Definition: vpColor.h:179
Class that defines what is a point.
Definition: vpPoint.h:58
double getTheta2() const
Definition: vpCylinder.h:145
Defines a generic 2D polygon.
Definition: vpPolygon.h:103
void projection()
Definition: vpCircle.cpp:136
void set_i(double ii)
Definition: vpImagePoint.h:167
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:330
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:309
double get_j() const
Definition: vpImagePoint.h:215
Generic class defining intrinsic camera parameters.
std::vector< PolygonType * > & getPolygon()
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:426
static bool isNaN(double value)
Definition: vpMath.cpp:84
bool useScanLine
Use scanline rendering.
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
void projection()
Definition: vpCylinder.cpp:165
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
static int round(double x)
Definition: vpMath.h:241
void set_j(double jj)
Definition: vpImagePoint.h:178
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void getFeature(const int &index, long &id, float &x, float &y) const
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:189
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:431
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:433
double getRho1() const
Definition: vpCylinder.h:126
std::vector< std::vector< double > > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:71
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:233
double getTheta1() const
Definition: vpCylinder.h:132
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
std::vector< std::vector< double > > getFeaturesForDisplay()
double getRho2() const
Definition: vpCylinder.h:139
void buildFrom(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:131
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:61