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