Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpMbtDistanceCylinder.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  * Romain Tallonneau
37  * Bertrand Delabarre
38  *
39 *****************************************************************************/
40 
41 #include <visp3/core/vpConfig.h>
42 
48 #include <algorithm>
49 #include <stdlib.h>
50 #include <visp3/core/vpMeterPixelConversion.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPlane.h>
53 #include <visp3/mbt/vpMbtDistanceCylinder.h>
54 #include <visp3/visual_features/vpFeatureBuilder.h>
55 #include <visp3/visual_features/vpFeatureEllipse.h>
56 
57 #include <visp3/vision/vpPose.h>
58 
63  : name(), index(0), cam(), me(nullptr), wmean1(1), wmean2(1), featureline1(), featureline2(), isTrackedCylinder(true),
64  meline1(nullptr), meline2(nullptr), cercle1(nullptr), cercle2(nullptr), radius(0), p1(nullptr), p2(nullptr), L(), error(),
65  nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(nullptr), hiddenface(nullptr), index_polygon(-1),
66  isvisible(false)
67 { }
68 
73 {
74  if (p1 != nullptr)
75  delete p1;
76  if (p2 != nullptr)
77  delete p2;
78  if (c != nullptr)
79  delete c;
80  if (meline1 != nullptr)
81  delete meline1;
82  if (meline2 != nullptr)
83  delete meline2;
84  if (cercle1 != nullptr)
85  delete cercle1;
86  if (cercle2 != nullptr)
87  delete cercle2;
88 }
89 
97 void vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
98 {
99  c->project(cMo);
100  p1->project(cMo);
101  p2->project(cMo);
102  cercle1->project(cMo);
103  cercle2->project(cMo);
104 }
105 
114 void vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
115 {
116  c = new vpCylinder;
117  p1 = new vpPoint;
118  p2 = new vpPoint;
119  cercle1 = new vpCircle;
120  cercle2 = new vpCircle;
121 
122  // Get the points
123  *p1 = _p1;
124  *p2 = _p2;
125 
126  // Get the radius
127  radius = r;
128 
129  vpColVector ABC(3);
130  vpColVector V1(3);
131  vpColVector V2(3);
132 
133  V1[0] = _p1.get_oX();
134  V1[1] = _p1.get_oY();
135  V1[2] = _p1.get_oZ();
136  V2[0] = _p2.get_oX();
137  V2[1] = _p2.get_oY();
138  V2[2] = _p2.get_oZ();
139 
140  // Get the axis of the cylinder
141  ABC = V1 - V2;
142 
143  // Build our extremity circles
144  cercle1->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
145  cercle2->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p2.get_oX(), _p2.get_oY(), _p2.get_oZ(), r);
146 
147  // Build our cylinder
148  c->setWorldCoordinates(ABC[0], ABC[1], ABC[2], (_p1.get_oX() + _p2.get_oX()) / 2.0,
149  (_p1.get_oY() + _p2.get_oY()) / 2.0, (_p1.get_oZ() + _p2.get_oZ()) / 2.0, r);
150 }
151 
158 {
159  me = _me;
160  if (meline1 != nullptr) {
161  meline1->setMe(me);
162  }
163  if (meline2 != nullptr) {
164  meline2->setMe(me);
165  }
166 }
167 
180  bool doNotTrack, const vpImage<bool> *mask)
181 {
182  if (isvisible) {
183  // Perspective projection
184  p1->changeFrame(cMo);
185  p2->changeFrame(cMo);
186  cercle1->changeFrame(cMo);
187  cercle2->changeFrame(cMo);
188  c->changeFrame(cMo);
189 
190  p1->projection();
191  p2->projection();
192  try {
193  cercle1->projection();
194  }
195  catch (...) {
196  // std::cout<<"Problem when projecting circle 1\n";
197  return false;
198  }
199  try {
200  cercle2->projection();
201  }
202  catch (...) {
203  // std::cout<<"Problem when projecting circle 2\n";
204  return false;
205  }
206  c->projection();
207 
208  double rho1, theta1;
209  double rho2, theta2;
210 
211  // Create the moving edges containers
212  meline1 = new vpMbtMeLine;
213  meline1->setMask(*mask);
214  meline1->setMe(me);
215  meline2 = new vpMbtMeLine;
216  meline1->setMask(*mask);
217  meline2->setMe(me);
218 
219  // meline->setDisplay(vpMeSite::RANGE_RESULT);
220  meline1->setInitRange(0);
221  meline2->setInitRange(0);
222 
223  // Conversion meter to pixels
224  vpMeterPixelConversion::convertLine(cam, c->getRho1(), c->getTheta1(), rho1, theta1);
225  vpMeterPixelConversion::convertLine(cam, c->getRho2(), c->getTheta2(), rho2, theta2);
226 
227  // Determine intersections between circles and limbos
228  double i11, i12, i21, i22, j11, j12, j21, j22;
229  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
230  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
231  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
232  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
233 
234  // Create the image points
235  vpImagePoint ip11, ip12, ip21, ip22;
236  ip11.set_ij(i11, j11);
237  ip12.set_ij(i12, j12);
238  ip21.set_ij(i21, j21);
239  ip22.set_ij(i22, j22);
240 
241  // update limits of the melines.
242  int marge = /*10*/ 5; // ou 5 normalement
243  if (ip11.get_j() < ip12.get_j()) {
244  meline1->jmin = (int)ip11.get_j() - marge;
245  meline1->jmax = (int)ip12.get_j() + marge;
246  }
247  else {
248  meline1->jmin = (int)ip12.get_j() - marge;
249  meline1->jmax = (int)ip11.get_j() + marge;
250  }
251  if (ip11.get_i() < ip12.get_i()) {
252  meline1->imin = (int)ip11.get_i() - marge;
253  meline1->imax = (int)ip12.get_i() + marge;
254  }
255  else {
256  meline1->imin = (int)ip12.get_i() - marge;
257  meline1->imax = (int)ip11.get_i() + marge;
258  }
259 
260  if (ip21.get_j() < ip22.get_j()) {
261  meline2->jmin = (int)ip21.get_j() - marge;
262  meline2->jmax = (int)ip22.get_j() + marge;
263  }
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  }
272  else {
273  meline2->imin = (int)ip22.get_i() - marge;
274  meline2->imax = (int)ip21.get_i() + marge;
275  }
276 
277  // Initialize the tracking
278  while (theta1 > M_PI) {
279  theta1 -= M_PI;
280  }
281  while (theta1 < -M_PI) {
282  theta1 += M_PI;
283  }
284 
285  if (theta1 < -M_PI / 2.0)
286  theta1 = -theta1 - 3 * M_PI / 2.0;
287  else
288  theta1 = M_PI / 2.0 - theta1;
289 
290  while (theta2 > M_PI) {
291  theta2 -= M_PI;
292  }
293  while (theta2 < -M_PI) {
294  theta2 += M_PI;
295  }
296 
297  if (theta2 < -M_PI / 2.0)
298  theta2 = -theta2 - 3 * M_PI / 2.0;
299  else
300  theta2 = M_PI / 2.0 - theta2;
301 
302  try {
303  meline1->initTracking(I, ip11, ip12, rho1, theta1, doNotTrack);
304  }
305  catch (...) {
306  // vpTRACE("the line can't be initialized");
307  return false;
308  }
309  try {
310  meline2->initTracking(I, ip21, ip22, rho2, theta2, doNotTrack);
311  }
312  catch (...) {
313  // vpTRACE("the line can't be initialized");
314  return false;
315  }
316  }
317  return true;
318 }
319 
327 {
328  if (isvisible) {
329  try {
330  meline1->track(I);
331  }
332  catch (...) {
333  // std::cout << "Track meline1 failed" << std::endl;
334  meline1->reset();
335  Reinit = true;
336  }
337  try {
338  meline2->track(I);
339  }
340  catch (...) {
341  // std::cout << "Track meline2 failed" << std::endl;
342  meline2->reset();
343  Reinit = true;
344  }
345 
346  // Update the number of features
347  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
348  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
350  }
351 }
352 
360 {
361  if (isvisible) {
362  // Perspective projection
363  p1->changeFrame(cMo);
364  p2->changeFrame(cMo);
365  cercle1->changeFrame(cMo);
366  cercle2->changeFrame(cMo);
367  c->changeFrame(cMo);
368 
369  p1->projection();
370  p2->projection();
371  try {
372  cercle1->projection();
373  }
374  catch (...) {
375  std::cout << "Probleme projection cercle 1\n";
376  }
377  try {
378  cercle2->projection();
379  }
380  catch (...) {
381  std::cout << "Probleme projection cercle 2\n";
382  }
383  c->projection();
384 
385  // Get the limbos
386  double rho1, theta1;
387  double rho2, theta2;
388 
389  // Conversion meter to pixels
390  vpMeterPixelConversion::convertLine(cam, c->getRho1(), c->getTheta1(), rho1, theta1);
391  vpMeterPixelConversion::convertLine(cam, c->getRho2(), c->getTheta2(), rho2, theta2);
392 
393  // Determine intersections between circles and limbos
394  double i11, i12, i21, i22, j11, j12, j21, j22;
395 
396  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
397  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
398 
399  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
400  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
401 
402  // Create the image points
403  vpImagePoint ip11, ip12, ip21, ip22;
404  ip11.set_ij(i11, j11);
405  ip12.set_ij(i12, j12);
406  ip21.set_ij(i21, j21);
407  ip22.set_ij(i22, j22);
408 
409  // update limits of the meline.
410  int marge = /*10*/ 5; // ou 5 normalement
411  if (ip11.get_j() < ip12.get_j()) {
412  meline1->jmin = (int)ip11.get_j() - marge;
413  meline1->jmax = (int)ip12.get_j() + marge;
414  }
415  else {
416  meline1->jmin = (int)ip12.get_j() - marge;
417  meline1->jmax = (int)ip11.get_j() + marge;
418  }
419  if (ip11.get_i() < ip12.get_i()) {
420  meline1->imin = (int)ip11.get_i() - marge;
421  meline1->imax = (int)ip12.get_i() + marge;
422  }
423  else {
424  meline1->imin = (int)ip12.get_i() - marge;
425  meline1->imax = (int)ip11.get_i() + marge;
426  }
427 
428  if (ip21.get_j() < ip22.get_j()) {
429  meline2->jmin = (int)ip21.get_j() - marge;
430  meline2->jmax = (int)ip22.get_j() + marge;
431  }
432  else {
433  meline2->jmin = (int)ip22.get_j() - marge;
434  meline2->jmax = (int)ip21.get_j() + marge;
435  }
436  if (ip21.get_i() < ip22.get_i()) {
437  meline2->imin = (int)ip21.get_i() - marge;
438  meline2->imax = (int)ip22.get_i() + marge;
439  }
440  else {
441  meline2->imin = (int)ip22.get_i() - marge;
442  meline2->imax = (int)ip21.get_i() + marge;
443  }
444 
445  // Initialize the tracking
446  while (theta1 > M_PI) {
447  theta1 -= M_PI;
448  }
449  while (theta1 < -M_PI) {
450  theta1 += M_PI;
451  }
452 
453  if (theta1 < -M_PI / 2.0)
454  theta1 = -theta1 - 3 * M_PI / 2.0;
455  else
456  theta1 = M_PI / 2.0 - theta1;
457 
458  while (theta2 > M_PI) {
459  theta2 -= M_PI;
460  }
461  while (theta2 < -M_PI) {
462  theta2 += M_PI;
463  }
464 
465  if (theta2 < -M_PI / 2.0)
466  theta2 = -theta2 - 3 * M_PI / 2.0;
467  else
468  theta2 = M_PI / 2.0 - theta2;
469 
470  try {
471  // meline1->updateParameters(I,rho1,theta1);
472  meline1->updateParameters(I, ip11, ip12, rho1, theta1);
473  }
474  catch (...) {
475  Reinit = true;
476  }
477  try {
478  // meline2->updateParameters(I,rho2,theta2);
479  meline2->updateParameters(I, ip21, ip22, rho2, theta2);
480  }
481  catch (...) {
482  Reinit = true;
483  }
484 
485  // Update the numbers of features
486  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
487  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
489  }
490 }
491 
504  const vpImage<bool> *mask)
505 {
506  if (meline1 != nullptr)
507  delete meline1;
508  if (meline2 != nullptr)
509  delete meline2;
510 
511  meline1 = nullptr;
512  meline2 = nullptr;
513 
514  if (!initMovingEdge(I, cMo, false, mask))
515  Reinit = true;
516 
517  Reinit = false;
518 }
519 
531  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
532  bool displayFullModel)
533 {
534  std::vector<std::vector<double> > models =
535  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
536 
537  for (size_t i = 0; i < models.size(); i++) {
538  vpImagePoint ip1(models[i][1], models[i][2]);
539  vpImagePoint ip2(models[i][3], models[i][4]);
540 
541  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
542  }
543 }
544 
556  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
557  bool displayFullModel)
558 {
559  std::vector<std::vector<double> > models =
560  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
561 
562  for (size_t i = 0; i < models.size(); i++) {
563  vpImagePoint ip1(models[i][1], models[i][2]);
564  vpImagePoint ip2(models[i][3], models[i][4]);
565 
566  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
567  }
568 }
569 
574 std::vector<std::vector<double> > vpMbtDistanceCylinder::getFeaturesForDisplay()
575 {
576  std::vector<std::vector<double> > features;
577 
578  if (meline1 != nullptr) {
579  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
580  ++it) {
581  vpMeSite p_me = *it;
582 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
583  std::vector<double> params = { 0, //ME
584  p_me.get_ifloat(),
585  p_me.get_jfloat(),
586  static_cast<double>(p_me.getState()) };
587 #else
588  std::vector<double> params;
589  params.push_back(0); //ME
590  params.push_back(p_me.get_ifloat());
591  params.push_back(p_me.get_jfloat());
592  params.push_back(static_cast<double>(p_me.getState()));
593 #endif
594 
595  features.push_back(params);
596  }
597  }
598 
599  if (meline2 != nullptr) {
600  for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
601  ++it) {
602  vpMeSite p_me = *it;
603 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
604  std::vector<double> params = { 0, //ME
605  p_me.get_ifloat(),
606  p_me.get_jfloat(),
607  static_cast<double>(p_me.getState()) };
608 #else
609  std::vector<double> params;
610  params.push_back(0); //ME
611  params.push_back(p_me.get_ifloat());
612  params.push_back(p_me.get_jfloat());
613  params.push_back(static_cast<double>(p_me.getState()));
614 #endif
615 
616  features.push_back(params);
617  }
618  }
619 
620  return features;
621 }
622 
633 std::vector<std::vector<double> > vpMbtDistanceCylinder::getModelForDisplay(unsigned int, unsigned int,
634  const vpHomogeneousMatrix &cMo,
635  const vpCameraParameters &camera,
636  bool displayFullModel)
637 {
638  std::vector<std::vector<double> > models;
639 
640  if ((isvisible && isTrackedCylinder) || displayFullModel) {
641  // Perspective projection
642  p1->changeFrame(cMo);
643  p2->changeFrame(cMo);
644  cercle1->changeFrame(cMo);
645  cercle2->changeFrame(cMo);
646  c->changeFrame(cMo);
647 
648  p1->projection();
649  p2->projection();
650  try {
651  cercle1->projection();
652  }
653  catch (...) {
654  std::cout << "Problem projection circle 1";
655  }
656  try {
657  cercle2->projection();
658  }
659  catch (...) {
660  std::cout << "Problem projection circle 2";
661  }
662  c->projection();
663 
664  double rho1, theta1;
665  double rho2, theta2;
666 
667  // Meters to pixels conversion
668  vpMeterPixelConversion::convertLine(camera, c->getRho1(), c->getTheta1(), rho1, theta1);
669  vpMeterPixelConversion::convertLine(camera, c->getRho2(), c->getTheta2(), rho2, theta2);
670 
671  // Determine intersections between circles and limbos
672  double i11, i12, i21, i22, j11, j12, j21, j22;
673 
674  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
675  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
676 
677  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
678  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
679 
680  // Create the image points
681  vpImagePoint ip11, ip12, ip21, ip22;
682  ip11.set_ij(i11, j11);
683  ip12.set_ij(i12, j12);
684  ip21.set_ij(i21, j21);
685  ip22.set_ij(i22, j22);
686 
687 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
688  std::vector<double> params1 = { 0,
689  ip11.get_i(),
690  ip11.get_j(),
691  ip12.get_i(),
692  ip12.get_j() };
693 
694  std::vector<double> params2 = { 0,
695  ip21.get_i(),
696  ip21.get_j(),
697  ip22.get_i(),
698  ip22.get_j() };
699 #else
700  std::vector<double> params1, params2;
701  params1.push_back(0);
702  params1.push_back(ip11.get_i());
703  params1.push_back(ip11.get_j());
704  params1.push_back(ip12.get_i());
705  params1.push_back(ip12.get_j());
706 
707  params2.push_back(0);
708  params2.push_back(ip11.get_i());
709  params2.push_back(ip11.get_j());
710  params2.push_back(ip12.get_i());
711  params2.push_back(ip12.get_j());
712 #endif
713 
714  models.push_back(params1);
715  models.push_back(params2);
716  }
717 
718  return models;
719 }
720 
736 {
737  if (meline1 != nullptr) {
738  meline1->display(I);
739  }
740  if (meline2 != nullptr) {
741  meline2->display(I);
742  }
743 }
744 
746 {
747  if (meline1 != nullptr) {
748  meline1->display(I);
749  }
750  if (meline2 != nullptr) {
751  meline2->display(I);
752  }
753 }
754 
759 {
760  if (isvisible) {
761  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
762  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
764  L.resize(nbFeature, 6);
766  }
767  else {
768  nbFeature = 0;
769  nbFeaturel1 = 0;
770  nbFeaturel2 = 0;
771  }
772 }
773 
779  const vpImage<unsigned char> &I)
780 {
781  if (isvisible) {
782  // Perspective projection
783  c->changeFrame(cMo);
784  c->projection();
785  cercle1->changeFrame(cMo);
786  cercle1->changeFrame(cMo);
787  try {
788  cercle1->projection();
789  }
790  catch (...) {
791  std::cout << "Problem projection circle 1\n";
792  }
793  try {
794  cercle2->projection();
795  }
796  catch (...) {
797  std::cout << "Problem projection circle 2\n";
798  }
799 
800  bool disp = false;
801  bool disp2 = false;
802  if (disp || disp2)
803  vpDisplay::flush(I);
804 
805  // Build the lines
808 
809  double rho1 = featureline1.getRho();
810  double theta1 = featureline1.getTheta();
811  double rho2 = featureline2.getRho();
812  double theta2 = featureline2.getTheta();
813 
814  double co1 = cos(theta1);
815  double si1 = sin(theta1);
816  double co2 = cos(theta2);
817  double si2 = sin(theta2);
818 
819  double mx = 1.0 / cam.get_px();
820  double my = 1.0 / cam.get_py();
821  double xc = cam.get_u0();
822  double yc = cam.get_v0();
823 
824  vpMatrix H1;
825  H1 = featureline1.interaction();
826  vpMatrix H2;
827  H2 = featureline2.interaction();
828 
829  vpMeSite p;
830  unsigned int j = 0;
831  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
832  ++it) {
833  double x = (double)it->m_j;
834  double y = (double)it->m_i;
835 
836  x = (x - xc) * mx;
837  y = (y - yc) * my;
838 
839  double alpha1 = x * si1 - y * co1;
840 
841  double *Lrho = H1[0];
842  double *Ltheta = H1[1];
843  // Calculate interaction matrix for a distance
844  for (unsigned int k = 0; k < 6; k++) {
845  L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
846  }
847  error[j] = rho1 - (x * co1 + y * si1);
848 
849  if (disp)
850  vpDisplay::displayCross(I, it->m_i, it->m_j, (unsigned int)(error[j] * 100), vpColor::orange, 1);
851 
852  j++;
853  }
854 
855  for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
856  ++it) {
857  double x = (double)it->m_j;
858  double y = (double)it->m_i;
859 
860  x = (x - xc) * mx;
861  y = (y - yc) * my;
862 
863  double alpha2 = x * si2 - y * co2;
864 
865  double *Lrho = H2[0];
866  double *Ltheta = H2[1];
867  // Calculate interaction matrix for a distance
868  for (unsigned int k = 0; k < 6; k++) {
869  L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
870  }
871  error[j] = rho2 - (x * co2 + y * si2);
872 
873  if (disp)
874  vpDisplay::displayCross(I, it->m_i, it->m_j, (unsigned int)(error[j] * 100), vpColor::red, 1);
875 
876  j++;
877  }
878  }
879 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
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:86
void projection() vp_override
Definition: vpCircle.cpp:137
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpCircle.cpp:57
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const vp_override
Definition: vpCircle.cpp:246
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:396
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor orange
Definition: vpColor.h:221
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:99
double getRho1() const
Definition: vpCylinder.h:132
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpCylinder.cpp:62
double getTheta1() const
Definition: vpCylinder.h:138
double getTheta2() const
Definition: vpCylinder.h:151
double getRho2() const
Definition: vpCylinder.h:145
void projection() vp_override
Definition: vpCylinder.cpp:146
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpCylinder.cpp:241
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) vp_override
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:82
double get_j() const
Definition: vpImagePoint.h:125
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:315
double get_i() const
Definition: vpImagePoint.h:114
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:184
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
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.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
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:65
vpMeSiteState getState() const
Definition: vpMeSite.h:261
int m_j
Integer coordinates along j of a site.
Definition: vpMeSite.h:98
double get_ifloat() const
Definition: vpMeSite.h:192
double get_jfloat() const
Definition: vpMeSite.h:198
Definition: vpMe.h:124
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:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:450
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:454
void projection(const vpColVector &_cP, vpColVector &_p) const vp_override
Definition: vpPoint.cpp:223
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:452
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpPoint.cpp:240