Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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, const 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, const unsigned int thickness,
518  const 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, const unsigned int thickness,
543  const 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  const 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 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void displayMovingEdges(const vpImage< unsigned char > &I)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpCylinder * c
The cylinder.
void projection(const vpColVector &_cP, vpColVector &_p)
Definition: vpPoint.cpp:216
double get_u0() const
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
double get_jfloat() const
Definition: vpMeSite.h:166
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
double get_i() const
Definition: vpImagePoint.h:204
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:239
unsigned int getWidth() const
Definition: vpImage.h:237
bool isvisible
Indicates if the cylinder is visible or not.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:422
double getRho2() const
Definition: vpCylinder.h:140
Definition: vpMe.h:60
unsigned int nbFeature
The number of moving edges.
double getRho() const
double get_py() const
double getTheta() const
vpPoint * p1
The first extremity on the axe.
static void flush(const vpImage< unsigned char > &I)
double get_j() const
Definition: vpImagePoint.h:215
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:272
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
Definition: vpMeSite.h:189
static const vpColor orange
Definition: vpColor.h:190
unsigned int nbFeaturel1
The number of moving edges on line 1.
double getTheta1() const
Definition: vpCylinder.h:133
void projection()
Definition: vpCircle.cpp:138
std::vector< std::vector< double > > getFeaturesForDisplay()
vpCircle * cercle2
The lower circle limiting the cylinder.
double get_v0() const
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:332
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:424
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
double get_px() const
void projection()
Definition: vpCylinder.cpp:167
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
vpPoint * p2
The second extremity on the axe.
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:420
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
Class that defines what is a cylinder.
Definition: vpCylinder.h:96
int j
Definition: vpMeSite.h:86
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double get_ifloat() const
Definition: vpMeSite.h:159
double getRho1() const
Definition: vpCylinder.h:127
double getTheta2() const
Definition: vpCylinder.h:146
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
unsigned int getHeight() const
Definition: vpImage.h:179
vpColVector error
The error vector.
vpCircle * cercle1
The upper circle limiting the cylinder.
bool Reinit
Indicates if the line has to be reinitialized.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double radius
The radius of the cylinder.
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:71
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
unsigned int nbFeaturel2
The number of moving edges on line 2.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:233
Class that defines what is a circle.
Definition: vpCircle.h:58
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:189
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:61
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310