Visual Servoing Platform  version 3.4.0
vpMbtDistanceCylinder.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  * Make the complete tracking of an object by using its CAD model. Cylinder
33  * tracking.
34  *
35  * Authors:
36  * Nicolas Melchior
37  * Romain Tallonneau
38  * Eric Marchand
39  * Bertrand Delabarre
40  *
41  *****************************************************************************/
42 
43 #include <visp3/core/vpConfig.h>
44 
50 #include <algorithm>
51 #include <stdlib.h>
52 #include <visp3/core/vpMeterPixelConversion.h>
53 #include <visp3/core/vpPixelMeterConversion.h>
54 #include <visp3/core/vpPlane.h>
55 #include <visp3/mbt/vpMbtDistanceCylinder.h>
56 #include <visp3/visual_features/vpFeatureBuilder.h>
57 #include <visp3/visual_features/vpFeatureEllipse.h>
58 
59 #include <visp3/vision/vpPose.h>
60 
65  : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1), featureline1(), featureline2(), isTrackedCylinder(true),
66  meline1(NULL), meline2(NULL), cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(), error(),
67  nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(NULL), hiddenface(NULL), index_polygon(-1),
68  isvisible(false)
69 {
70 }
71 
76 {
77  // cout << "Deleting cylinder " << index << endl;
78 
79  if (p1 != NULL)
80  delete p1;
81  if (p2 != NULL)
82  delete p2;
83  if (c != NULL)
84  delete c;
85  if (meline1 != NULL)
86  delete meline1;
87  if (meline2 != NULL)
88  delete meline2;
89  if (cercle1 != NULL)
90  delete cercle1;
91  if (cercle2 != NULL)
92  delete cercle2;
93 }
94 
102 void vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
103 {
104  c->project(cMo);
105  p1->project(cMo);
106  p2->project(cMo);
107  cercle1->project(cMo);
108  cercle2->project(cMo);
109 }
110 
119 void vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
120 {
121  c = new vpCylinder;
122  p1 = new vpPoint;
123  p2 = new vpPoint;
124  cercle1 = new vpCircle;
125  cercle2 = new vpCircle;
126 
127  // Get the points
128  *p1 = _p1;
129  *p2 = _p2;
130 
131  // Get the radius
132  radius = r;
133 
134  vpColVector ABC(3);
135  vpColVector V1(3);
136  vpColVector V2(3);
137 
138  V1[0] = _p1.get_oX();
139  V1[1] = _p1.get_oY();
140  V1[2] = _p1.get_oZ();
141  V2[0] = _p2.get_oX();
142  V2[1] = _p2.get_oY();
143  V2[2] = _p2.get_oZ();
144 
145  // Get the axis of the cylinder
146  ABC = V1 - V2;
147 
148  // Build our extremity circles
149  cercle1->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
150  cercle2->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p2.get_oX(), _p2.get_oY(), _p2.get_oZ(), r);
151 
152  // Build our cylinder
153  c->setWorldCoordinates(ABC[0], ABC[1], ABC[2], (_p1.get_oX() + _p2.get_oX()) / 2.0,
154  (_p1.get_oY() + _p2.get_oY()) / 2.0, (_p1.get_oZ() + _p2.get_oZ()) / 2.0, r);
155 }
156 
163 {
164  me = _me;
165  if (meline1 != NULL) {
166  meline1->setMe(me);
167  }
168  if (meline2 != NULL) {
169  meline2->setMe(me);
170  }
171 }
172 
185  const vpImage<bool> *mask)
186 {
187  if (isvisible) {
188  // Perspective projection
189  p1->changeFrame(cMo);
190  p2->changeFrame(cMo);
191  cercle1->changeFrame(cMo);
192  cercle2->changeFrame(cMo);
193  c->changeFrame(cMo);
194 
195  p1->projection();
196  p2->projection();
197  try {
198  cercle1->projection();
199  } catch (...) {
200  // std::cout<<"Problem when projecting circle 1\n";
201  return false;
202  }
203  try {
204  cercle2->projection();
205  } catch (...) {
206  // std::cout<<"Problem when projecting circle 2\n";
207  return false;
208  }
209  c->projection();
210 
211  double rho1, theta1;
212  double rho2, theta2;
213 
214  // Create the moving edges containers
215  meline1 = new vpMbtMeLine;
216  meline1->setMask(*mask);
217  meline1->setMe(me);
218  meline2 = new vpMbtMeLine;
219  meline1->setMask(*mask);
220  meline2->setMe(me);
221 
222  // meline->setDisplay(vpMeSite::RANGE_RESULT);
223  meline1->setInitRange(0);
224  meline2->setInitRange(0);
225 
226  // Conversion meter to pixels
227  vpMeterPixelConversion::convertLine(cam, c->getRho1(), c->getTheta1(), rho1, theta1);
228  vpMeterPixelConversion::convertLine(cam, c->getRho2(), c->getTheta2(), rho2, theta2);
229 
230  // Determine intersections between circles and limbos
231  double i11, i12, i21, i22, j11, j12, j21, j22;
232  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
233  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
234  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
235  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
236 
237  // Create the image points
238  vpImagePoint ip11, ip12, ip21, ip22;
239  ip11.set_ij(i11, j11);
240  ip12.set_ij(i12, j12);
241  ip21.set_ij(i21, j21);
242  ip22.set_ij(i22, j22);
243 
244  // update limits of the melines.
245  int marge = /*10*/ 5; // ou 5 normalement
246  if (ip11.get_j() < ip12.get_j()) {
247  meline1->jmin = (int)ip11.get_j() - marge;
248  meline1->jmax = (int)ip12.get_j() + marge;
249  } else {
250  meline1->jmin = (int)ip12.get_j() - marge;
251  meline1->jmax = (int)ip11.get_j() + marge;
252  }
253  if (ip11.get_i() < ip12.get_i()) {
254  meline1->imin = (int)ip11.get_i() - marge;
255  meline1->imax = (int)ip12.get_i() + marge;
256  } else {
257  meline1->imin = (int)ip12.get_i() - marge;
258  meline1->imax = (int)ip11.get_i() + marge;
259  }
260 
261  if (ip21.get_j() < ip22.get_j()) {
262  meline2->jmin = (int)ip21.get_j() - marge;
263  meline2->jmax = (int)ip22.get_j() + marge;
264  } else {
265  meline2->jmin = (int)ip22.get_j() - marge;
266  meline2->jmax = (int)ip21.get_j() + marge;
267  }
268  if (ip21.get_i() < ip22.get_i()) {
269  meline2->imin = (int)ip21.get_i() - marge;
270  meline2->imax = (int)ip22.get_i() + marge;
271  } else {
272  meline2->imin = (int)ip22.get_i() - marge;
273  meline2->imax = (int)ip21.get_i() + marge;
274  }
275 
276  // Initialize the tracking
277  while (theta1 > M_PI) {
278  theta1 -= M_PI;
279  }
280  while (theta1 < -M_PI) {
281  theta1 += M_PI;
282  }
283 
284  if (theta1 < -M_PI / 2.0)
285  theta1 = -theta1 - 3 * M_PI / 2.0;
286  else
287  theta1 = M_PI / 2.0 - theta1;
288 
289  while (theta2 > M_PI) {
290  theta2 -= M_PI;
291  }
292  while (theta2 < -M_PI) {
293  theta2 += M_PI;
294  }
295 
296  if (theta2 < -M_PI / 2.0)
297  theta2 = -theta2 - 3 * M_PI / 2.0;
298  else
299  theta2 = M_PI / 2.0 - theta2;
300 
301  try {
302  meline1->initTracking(I, ip11, ip12, rho1, theta1, doNotTrack);
303  } catch (...) {
304  // vpTRACE("the line can't be initialized");
305  return false;
306  }
307  try {
308  meline2->initTracking(I, ip21, ip22, rho2, theta2, doNotTrack);
309  } catch (...) {
310  // vpTRACE("the line can't be initialized");
311  return false;
312  }
313  }
314  return true;
315 }
316 
324 {
325  if (isvisible) {
326  try {
327  meline1->track(I);
328  } catch (...) {
329  // std::cout << "Track meline1 failed" << std::endl;
330  meline1->reset();
331  Reinit = true;
332  }
333  try {
334  meline2->track(I);
335  } catch (...) {
336  // std::cout << "Track meline2 failed" << std::endl;
337  meline2->reset();
338  Reinit = true;
339  }
340 
341  // Update the number of features
342  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
343  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
345  }
346 }
347 
355 {
356  if (isvisible) {
357  // Perspective projection
358  p1->changeFrame(cMo);
359  p2->changeFrame(cMo);
360  cercle1->changeFrame(cMo);
361  cercle2->changeFrame(cMo);
362  c->changeFrame(cMo);
363 
364  p1->projection();
365  p2->projection();
366  try {
367  cercle1->projection();
368  } catch (...) {
369  std::cout << "Probleme projection cercle 1\n";
370  }
371  try {
372  cercle2->projection();
373  } catch (...) {
374  std::cout << "Probleme projection cercle 2\n";
375  }
376  c->projection();
377 
378  // Get the limbos
379  double rho1, theta1;
380  double rho2, theta2;
381 
382  // Conversion meter to pixels
383  vpMeterPixelConversion::convertLine(cam, c->getRho1(), c->getTheta1(), rho1, theta1);
384  vpMeterPixelConversion::convertLine(cam, c->getRho2(), c->getTheta2(), rho2, theta2);
385 
386  // Determine intersections between circles and limbos
387  double i11, i12, i21, i22, j11, j12, j21, j22;
388 
389  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
390  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
391 
392  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
393  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
394 
395  // Create the image points
396  vpImagePoint ip11, ip12, ip21, ip22;
397  ip11.set_ij(i11, j11);
398  ip12.set_ij(i12, j12);
399  ip21.set_ij(i21, j21);
400  ip22.set_ij(i22, j22);
401 
402  // update limits of the meline.
403  int marge = /*10*/ 5; // ou 5 normalement
404  if (ip11.get_j() < ip12.get_j()) {
405  meline1->jmin = (int)ip11.get_j() - marge;
406  meline1->jmax = (int)ip12.get_j() + marge;
407  } else {
408  meline1->jmin = (int)ip12.get_j() - marge;
409  meline1->jmax = (int)ip11.get_j() + marge;
410  }
411  if (ip11.get_i() < ip12.get_i()) {
412  meline1->imin = (int)ip11.get_i() - marge;
413  meline1->imax = (int)ip12.get_i() + marge;
414  } else {
415  meline1->imin = (int)ip12.get_i() - marge;
416  meline1->imax = (int)ip11.get_i() + marge;
417  }
418 
419  if (ip21.get_j() < ip22.get_j()) {
420  meline2->jmin = (int)ip21.get_j() - marge;
421  meline2->jmax = (int)ip22.get_j() + marge;
422  } else {
423  meline2->jmin = (int)ip22.get_j() - marge;
424  meline2->jmax = (int)ip21.get_j() + marge;
425  }
426  if (ip21.get_i() < ip22.get_i()) {
427  meline2->imin = (int)ip21.get_i() - marge;
428  meline2->imax = (int)ip22.get_i() + marge;
429  } else {
430  meline2->imin = (int)ip22.get_i() - marge;
431  meline2->imax = (int)ip21.get_i() + marge;
432  }
433 
434  // Initialize the tracking
435  while (theta1 > M_PI) {
436  theta1 -= M_PI;
437  }
438  while (theta1 < -M_PI) {
439  theta1 += M_PI;
440  }
441 
442  if (theta1 < -M_PI / 2.0)
443  theta1 = -theta1 - 3 * M_PI / 2.0;
444  else
445  theta1 = M_PI / 2.0 - theta1;
446 
447  while (theta2 > M_PI) {
448  theta2 -= M_PI;
449  }
450  while (theta2 < -M_PI) {
451  theta2 += M_PI;
452  }
453 
454  if (theta2 < -M_PI / 2.0)
455  theta2 = -theta2 - 3 * M_PI / 2.0;
456  else
457  theta2 = M_PI / 2.0 - theta2;
458 
459  try {
460  // meline1->updateParameters(I,rho1,theta1);
461  meline1->updateParameters(I, ip11, ip12, rho1, theta1);
462  } catch (...) {
463  Reinit = true;
464  }
465  try {
466  // meline2->updateParameters(I,rho2,theta2);
467  meline2->updateParameters(I, ip21, ip22, rho2, theta2);
468  } catch (...) {
469  Reinit = true;
470  }
471 
472  // Update the numbers of features
473  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
474  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
476  }
477 }
478 
490  const vpImage<bool> *mask)
491 {
492  if (meline1 != NULL)
493  delete meline1;
494  if (meline2 != NULL)
495  delete meline2;
496 
497  meline1 = NULL;
498  meline2 = NULL;
499 
500  if (!initMovingEdge(I, cMo, false, mask))
501  Reinit = true;
502 
503  Reinit = false;
504 }
505 
517  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
518  bool displayFullModel)
519 {
520  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(),
521  cMo, camera, displayFullModel);
522 
523  for (size_t i = 0; i < models.size(); i++) {
524  vpImagePoint ip1(models[i][1], models[i][2]);
525  vpImagePoint ip2(models[i][3], models[i][4]);
526 
527  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
528  }
529 }
530 
542  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
543  bool displayFullModel)
544 {
545  std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(),
546  cMo, camera, displayFullModel);
547 
548  for (size_t i = 0; i < models.size(); i++) {
549  vpImagePoint ip1(models[i][1], models[i][2]);
550  vpImagePoint ip2(models[i][3], models[i][4]);
551 
552  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
553  }
554 }
555 
560 std::vector<std::vector<double> > vpMbtDistanceCylinder::getFeaturesForDisplay()
561 {
562  std::vector<std::vector<double> > features;
563 
564  if (meline1 != NULL) {
565  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end(); ++it) {
566  vpMeSite p_me = *it;
567 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
568  std::vector<double> params = {0, //ME
569  p_me.get_ifloat(),
570  p_me.get_jfloat(),
571  static_cast<double>(p_me.getState())};
572 #else
573  std::vector<double> params;
574  params.push_back(0); //ME
575  params.push_back(p_me.get_ifloat());
576  params.push_back(p_me.get_jfloat());
577  params.push_back(static_cast<double>(p_me.getState()));
578 #endif
579  features.push_back(params);
580  }
581  }
582 
583  if (meline2 != NULL) {
584  for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end(); ++it) {
585  vpMeSite p_me = *it;
586 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
587  std::vector<double> params = {0, //ME
588  p_me.get_ifloat(),
589  p_me.get_jfloat(),
590  static_cast<double>(p_me.getState())};
591 #else
592  std::vector<double> params;
593  params.push_back(0); //ME
594  params.push_back(p_me.get_ifloat());
595  params.push_back(p_me.get_jfloat());
596  params.push_back(static_cast<double>(p_me.getState()));
597 #endif
598  features.push_back(params);
599  }
600  }
601 
602  return features;
603 }
604 
615 std::vector<std::vector<double> > vpMbtDistanceCylinder::getModelForDisplay(unsigned int, unsigned int,
616  const vpHomogeneousMatrix &cMo,
617  const vpCameraParameters &camera,
618  bool displayFullModel)
619 {
620  std::vector<std::vector<double> > models;
621 
622  if ((isvisible && isTrackedCylinder) || displayFullModel) {
623  // Perspective projection
624  p1->changeFrame(cMo);
625  p2->changeFrame(cMo);
626  cercle1->changeFrame(cMo);
627  cercle2->changeFrame(cMo);
628  c->changeFrame(cMo);
629 
630  p1->projection();
631  p2->projection();
632  try {
633  cercle1->projection();
634  } catch (...) {
635  std::cout << "Problem projection circle 1";
636  }
637  try {
638  cercle2->projection();
639  } catch (...) {
640  std::cout << "Problem projection circle 2";
641  }
642  c->projection();
643 
644  double rho1, theta1;
645  double rho2, theta2;
646 
647  // Meters to pixels conversion
648  vpMeterPixelConversion::convertLine(camera, c->getRho1(), c->getTheta1(), rho1, theta1);
649  vpMeterPixelConversion::convertLine(camera, c->getRho2(), c->getTheta2(), rho2, theta2);
650 
651  // Determine intersections between circles and limbos
652  double i11, i12, i21, i22, j11, j12, j21, j22;
653 
654  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
655  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
656 
657  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
658  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
659 
660  // Create the image points
661  vpImagePoint ip11, ip12, ip21, ip22;
662  ip11.set_ij(i11, j11);
663  ip12.set_ij(i12, j12);
664  ip21.set_ij(i21, j21);
665  ip22.set_ij(i22, j22);
666 
667 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
668  std::vector<double> params1 = {0,
669  ip11.get_i(),
670  ip11.get_j(),
671  ip12.get_i(),
672  ip12.get_j()};
673 
674  std::vector<double> params2 = {0,
675  ip21.get_i(),
676  ip21.get_j(),
677  ip22.get_i(),
678  ip22.get_j()};
679 #else
680  std::vector<double> params1, params2;
681  params1.push_back(0);
682  params1.push_back(ip11.get_i());
683  params1.push_back(ip11.get_j());
684  params1.push_back(ip12.get_i());
685  params1.push_back(ip12.get_j());
686 
687  params2.push_back(0);
688  params2.push_back(ip11.get_i());
689  params2.push_back(ip11.get_j());
690  params2.push_back(ip12.get_i());
691  params2.push_back(ip12.get_j());
692 #endif
693 
694  models.push_back(params1);
695  models.push_back(params2);
696  }
697 
698  return models;
699 }
700 
716 {
717  if (meline1 != NULL) {
718  meline1->display(I);
719  }
720  if (meline2 != NULL) {
721  meline2->display(I);
722  }
723 }
724 
726 {
727  if (meline1 != NULL) {
728  meline1->display(I);
729  }
730  if (meline2 != NULL) {
731  meline2->display(I);
732  }
733 }
734 
739 {
740  if (isvisible) {
741  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
742  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
744  L.resize(nbFeature, 6);
746  } else {
747  nbFeature = 0;
748  nbFeaturel1 = 0;
749  nbFeaturel2 = 0;
750  }
751 }
752 
758  const vpImage<unsigned char> &I)
759 {
760  if (isvisible) {
761  // Perspective projection
762  c->changeFrame(cMo);
763  c->projection();
764  cercle1->changeFrame(cMo);
765  cercle1->changeFrame(cMo);
766  try {
767  cercle1->projection();
768  } catch (...) {
769  std::cout << "Problem projection circle 1\n";
770  }
771  try {
772  cercle2->projection();
773  } catch (...) {
774  std::cout << "Problem projection circle 2\n";
775  }
776 
777  bool disp = false;
778  bool disp2 = false;
779  if (disp || disp2)
780  vpDisplay::flush(I);
781 
782  // Build the lines
785 
786  double rho1 = featureline1.getRho();
787  double theta1 = featureline1.getTheta();
788  double rho2 = featureline2.getRho();
789  double theta2 = featureline2.getTheta();
790 
791  double co1 = cos(theta1);
792  double si1 = sin(theta1);
793  double co2 = cos(theta2);
794  double si2 = sin(theta2);
795 
796  double mx = 1.0 / cam.get_px();
797  double my = 1.0 / cam.get_py();
798  double xc = cam.get_u0();
799  double yc = cam.get_v0();
800 
801  vpMatrix H1;
802  H1 = featureline1.interaction();
803  vpMatrix H2;
804  H2 = featureline2.interaction();
805 
806  vpMeSite p;
807  unsigned int j = 0;
808  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
809  ++it) {
810  double x = (double)it->j;
811  double y = (double)it->i;
812 
813  x = (x - xc) * mx;
814  y = (y - yc) * my;
815 
816  double alpha1 = x * si1 - y * co1;
817 
818  double *Lrho = H1[0];
819  double *Ltheta = H1[1];
820  // Calculate interaction matrix for a distance
821  for (unsigned int k = 0; k < 6; k++) {
822  L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
823  }
824  error[j] = rho1 - (x * co1 + y * si1);
825 
826  if (disp)
827  vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::orange, 1);
828 
829  j++;
830  }
831 
832  for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
833  ++it) {
834  double x = (double)it->j;
835  double y = (double)it->i;
836 
837  x = (x - xc) * mx;
838  y = (y - yc) * my;
839 
840  double alpha2 = x * si2 - y * co2;
841 
842  double *Lrho = H2[0];
843  double *Ltheta = H2[1];
844  // Calculate interaction matrix for a distance
845  for (unsigned int k = 0; k < 6; k++) {
846  L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
847  }
848  error[j] = rho2 - (x * co2 + y * si2);
849 
850  if (disp)
851  vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::red, 1);
852 
853  j++;
854  }
855  }
856 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:92
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpCircle.cpp:245
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:60
void projection()
Definition: vpCircle.cpp:140
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:358
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor orange
Definition: vpColor.h:227
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:103
double getRho1() const
Definition: vpCylinder.h:132
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpCylinder.cpp:249
void projection()
Definition: vpCylinder.cpp:154
double getTheta1() const
Definition: vpCylinder.h:138
double getTheta2() const
Definition: vpCylinder.h:151
double getRho2() const
Definition: vpCylinder.h:145
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:65
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
double getTheta() const
vpMatrix interaction(unsigned int select=FEATURE_ALL)
double getRho() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_j() const
Definition: vpImagePoint.h:214
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
vpCylinder * c
The cylinder.
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
vpPoint * p2
The second extremity on the axe.
vpCircle * cercle1
The upper circle limiting the cylinder.
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void displayMovingEdges(const vpImage< unsigned char > &I)
std::vector< std::vector< double > > getFeaturesForDisplay()
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
unsigned int nbFeature
The number of moving edges.
vpCircle * cercle2
The lower circle limiting the cylinder.
bool isvisible
Indicates if the cylinder is visible or not.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
int j
Definition: vpMeSite.h:87
vpMeSiteState getState() const
Definition: vpMeSite.h:190
double get_ifloat() const
Definition: vpMeSite.h:160
double get_jfloat() const
Definition: vpMeSite.h:167
Definition: vpMe.h:61
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:447
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:451
void projection(const vpColVector &_cP, vpColVector &_p) const
Definition: vpPoint.cpp:222
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:449
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239