Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
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  bool doNotTrack, 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 
491  const vpImage<bool> *mask)
492 {
493  if (meline1 != NULL)
494  delete meline1;
495  if (meline2 != NULL)
496  delete meline2;
497 
498  meline1 = NULL;
499  meline2 = NULL;
500 
501  if (!initMovingEdge(I, cMo, false, mask))
502  Reinit = true;
503 
504  Reinit = false;
505 }
506 
518  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
519  bool displayFullModel)
520 {
521  std::vector<std::vector<double> > models =
522  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
523 
524  for (size_t i = 0; i < models.size(); i++) {
525  vpImagePoint ip1(models[i][1], models[i][2]);
526  vpImagePoint ip2(models[i][3], models[i][4]);
527 
528  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
529  }
530 }
531 
543  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
544  bool displayFullModel)
545 {
546  std::vector<std::vector<double> > models =
547  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
548 
549  for (size_t i = 0; i < models.size(); i++) {
550  vpImagePoint ip1(models[i][1], models[i][2]);
551  vpImagePoint ip2(models[i][3], models[i][4]);
552 
553  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
554  }
555 }
556 
561 std::vector<std::vector<double> > vpMbtDistanceCylinder::getFeaturesForDisplay()
562 {
563  std::vector<std::vector<double> > features;
564 
565  if (meline1 != NULL) {
566  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
567  ++it) {
568  vpMeSite p_me = *it;
569 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
570  std::vector<double> params = {0, // ME
571  p_me.get_ifloat(), p_me.get_jfloat(), 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();
585  ++it) {
586  vpMeSite p_me = *it;
587 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
588  std::vector<double> params = {0, // ME
589  p_me.get_ifloat(), p_me.get_jfloat(), static_cast<double>(p_me.getState())};
590 #else
591  std::vector<double> params;
592  params.push_back(0); // ME
593  params.push_back(p_me.get_ifloat());
594  params.push_back(p_me.get_jfloat());
595  params.push_back(static_cast<double>(p_me.getState()));
596 #endif
597  features.push_back(params);
598  }
599  }
600 
601  return features;
602 }
603 
614 std::vector<std::vector<double> > vpMbtDistanceCylinder::getModelForDisplay(unsigned int, unsigned int,
615  const vpHomogeneousMatrix &cMo,
616  const vpCameraParameters &camera,
617  bool displayFullModel)
618 {
619  std::vector<std::vector<double> > models;
620 
621  if ((isvisible && isTrackedCylinder) || displayFullModel) {
622  // Perspective projection
623  p1->changeFrame(cMo);
624  p2->changeFrame(cMo);
625  cercle1->changeFrame(cMo);
626  cercle2->changeFrame(cMo);
627  c->changeFrame(cMo);
628 
629  p1->projection();
630  p2->projection();
631  try {
632  cercle1->projection();
633  } catch (...) {
634  std::cout << "Problem projection circle 1";
635  }
636  try {
637  cercle2->projection();
638  } catch (...) {
639  std::cout << "Problem projection circle 2";
640  }
641  c->projection();
642 
643  double rho1, theta1;
644  double rho2, theta2;
645 
646  // Meters to pixels conversion
647  vpMeterPixelConversion::convertLine(camera, c->getRho1(), c->getTheta1(), rho1, theta1);
648  vpMeterPixelConversion::convertLine(camera, c->getRho2(), c->getTheta2(), rho2, theta2);
649 
650  // Determine intersections between circles and limbos
651  double i11, i12, i21, i22, j11, j12, j21, j22;
652 
653  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
654  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
655 
656  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
657  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
658 
659  // Create the image points
660  vpImagePoint ip11, ip12, ip21, ip22;
661  ip11.set_ij(i11, j11);
662  ip12.set_ij(i12, j12);
663  ip21.set_ij(i21, j21);
664  ip22.set_ij(i22, j22);
665 
666 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
667  std::vector<double> params1 = {0, ip11.get_i(), ip11.get_j(), ip12.get_i(), ip12.get_j()};
668 
669  std::vector<double> params2 = {0, ip21.get_i(), ip21.get_j(), ip22.get_i(), ip22.get_j()};
670 #else
671  std::vector<double> params1, params2;
672  params1.push_back(0);
673  params1.push_back(ip11.get_i());
674  params1.push_back(ip11.get_j());
675  params1.push_back(ip12.get_i());
676  params1.push_back(ip12.get_j());
677 
678  params2.push_back(0);
679  params2.push_back(ip11.get_i());
680  params2.push_back(ip11.get_j());
681  params2.push_back(ip12.get_i());
682  params2.push_back(ip12.get_j());
683 #endif
684 
685  models.push_back(params1);
686  models.push_back(params2);
687  }
688 
689  return models;
690 }
691 
707 {
708  if (meline1 != NULL) {
709  meline1->display(I);
710  }
711  if (meline2 != NULL) {
712  meline2->display(I);
713  }
714 }
715 
717 {
718  if (meline1 != NULL) {
719  meline1->display(I);
720  }
721  if (meline2 != NULL) {
722  meline2->display(I);
723  }
724 }
725 
730 {
731  if (isvisible) {
732  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
733  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
735  L.resize(nbFeature, 6);
737  } else {
738  nbFeature = 0;
739  nbFeaturel1 = 0;
740  nbFeaturel2 = 0;
741  }
742 }
743 
749  const vpImage<unsigned char> &I)
750 {
751  if (isvisible) {
752  // Perspective projection
753  c->changeFrame(cMo);
754  c->projection();
755  cercle1->changeFrame(cMo);
756  cercle1->changeFrame(cMo);
757  try {
758  cercle1->projection();
759  } catch (...) {
760  std::cout << "Problem projection circle 1\n";
761  }
762  try {
763  cercle2->projection();
764  } catch (...) {
765  std::cout << "Problem projection circle 2\n";
766  }
767 
768  bool disp = false;
769  bool disp2 = false;
770  if (disp || disp2)
771  vpDisplay::flush(I);
772 
773  // Build the lines
776 
777  double rho1 = featureline1.getRho();
778  double theta1 = featureline1.getTheta();
779  double rho2 = featureline2.getRho();
780  double theta2 = featureline2.getTheta();
781 
782  double co1 = cos(theta1);
783  double si1 = sin(theta1);
784  double co2 = cos(theta2);
785  double si2 = sin(theta2);
786 
787  double mx = 1.0 / cam.get_px();
788  double my = 1.0 / cam.get_py();
789  double xc = cam.get_u0();
790  double yc = cam.get_v0();
791 
792  vpMatrix H1;
793  H1 = featureline1.interaction();
794  vpMatrix H2;
795  H2 = featureline2.interaction();
796 
797  vpMeSite p;
798  unsigned int j = 0;
799  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
800  ++it) {
801  double x = (double)it->j;
802  double y = (double)it->i;
803 
804  x = (x - xc) * mx;
805  y = (y - yc) * my;
806 
807  double alpha1 = x * si1 - y * co1;
808 
809  double *Lrho = H1[0];
810  double *Ltheta = H1[1];
811  // Calculate interaction matrix for a distance
812  for (unsigned int k = 0; k < 6; k++) {
813  L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
814  }
815  error[j] = rho1 - (x * co1 + y * si1);
816 
817  if (disp)
818  vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::orange, 1);
819 
820  j++;
821  }
822 
823  for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
824  ++it) {
825  double x = (double)it->j;
826  double y = (double)it->i;
827 
828  x = (x - xc) * mx;
829  y = (y - yc) * my;
830 
831  double alpha2 = x * si2 - y * co2;
832 
833  double *Lrho = H2[0];
834  double *Ltheta = H2[1];
835  // Calculate interaction matrix for a distance
836  for (unsigned int k = 0; k < 6; k++) {
837  L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
838  }
839  error[j] = rho2 - (x * co2 + y * si2);
840 
841  if (disp)
842  vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::red, 1);
843 
844  j++;
845  }
846  }
847 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
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 &noMo, vpColVector &noP) const
Definition: vpCircle.cpp:248
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:398
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
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:136
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpCylinder.cpp:249
void projection()
Definition: vpCylinder.cpp:154
double getTheta1() const
Definition: vpCylinder.h:142
double getTheta2() const
Definition: vpCylinder.h:155
double getRho2() const
Definition: vpCylinder.h:149
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:89
double get_j() const
Definition: vpImagePoint.h:132
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:320
double get_i() const
Definition: vpImagePoint.h:121
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:461
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
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:463
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239