Visual Servoing Platform version 3.5.0
vpMbtDistanceCylinder.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Make the complete tracking of an object by using its CAD model. Cylinder
33 * tracking.
34 *
35 * Authors:
36 * Nicolas Melchior
37 * Romain Tallonneau
38 * Eric Marchand
39 * Bertrand Delabarre
40 *
41 *****************************************************************************/
42
43#include <visp3/core/vpConfig.h>
44
50#include <algorithm>
51#include <stdlib.h>
52#include <visp3/core/vpMeterPixelConversion.h>
53#include <visp3/core/vpPixelMeterConversion.h>
54#include <visp3/core/vpPlane.h>
55#include <visp3/mbt/vpMbtDistanceCylinder.h>
56#include <visp3/visual_features/vpFeatureBuilder.h>
57#include <visp3/visual_features/vpFeatureEllipse.h>
58
59#include <visp3/vision/vpPose.h>
60
65 : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1), featureline1(), featureline2(), isTrackedCylinder(true),
66 meline1(NULL), meline2(NULL), cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(), error(),
67 nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(NULL), hiddenface(NULL), index_polygon(-1),
68 isvisible(false)
69{
70}
71
76{
77 // cout << "Deleting cylinder " << index << endl;
78
79 if (p1 != NULL)
80 delete p1;
81 if (p2 != NULL)
82 delete p2;
83 if (c != NULL)
84 delete c;
85 if (meline1 != NULL)
86 delete meline1;
87 if (meline2 != NULL)
88 delete meline2;
89 if (cercle1 != NULL)
90 delete cercle1;
91 if (cercle2 != NULL)
92 delete cercle2;
93}
94
102void 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
119void vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
120{
121 c = new vpCylinder;
122 p1 = new vpPoint;
123 p2 = new vpPoint;
124 cercle1 = new vpCircle;
125 cercle2 = new vpCircle;
126
127 // Get the points
128 *p1 = _p1;
129 *p2 = _p2;
130
131 // Get the radius
132 radius = r;
133
134 vpColVector ABC(3);
135 vpColVector V1(3);
136 vpColVector V2(3);
137
138 V1[0] = _p1.get_oX();
139 V1[1] = _p1.get_oY();
140 V1[2] = _p1.get_oZ();
141 V2[0] = _p2.get_oX();
142 V2[1] = _p2.get_oY();
143 V2[2] = _p2.get_oZ();
144
145 // Get the axis of the cylinder
146 ABC = V1 - V2;
147
148 // Build our extremity circles
149 cercle1->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
150 cercle2->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p2.get_oX(), _p2.get_oY(), _p2.get_oZ(), r);
151
152 // Build our cylinder
153 c->setWorldCoordinates(ABC[0], ABC[1], ABC[2], (_p1.get_oX() + _p2.get_oX()) / 2.0,
154 (_p1.get_oY() + _p2.get_oY()) / 2.0, (_p1.get_oZ() + _p2.get_oZ()) / 2.0, r);
155}
156
163{
164 me = _me;
165 if (meline1 != NULL) {
166 meline1->setMe(me);
167 }
168 if (meline2 != NULL) {
169 meline2->setMe(me);
170 }
171}
172
185 const vpImage<bool> *mask)
186{
187 if (isvisible) {
188 // Perspective projection
189 p1->changeFrame(cMo);
190 p2->changeFrame(cMo);
191 cercle1->changeFrame(cMo);
192 cercle2->changeFrame(cMo);
193 c->changeFrame(cMo);
194
195 p1->projection();
196 p2->projection();
197 try {
199 } catch (...) {
200 // std::cout<<"Problem when projecting circle 1\n";
201 return false;
202 }
203 try {
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 {
368 } catch (...) {
369 std::cout << "Probleme projection cercle 1\n";
370 }
371 try {
373 } catch (...) {
374 std::cout << "Probleme projection cercle 2\n";
375 }
376 c->projection();
377
378 // Get the limbos
379 double rho1, theta1;
380 double rho2, theta2;
381
382 // Conversion meter to pixels
383 vpMeterPixelConversion::convertLine(cam, c->getRho1(), c->getTheta1(), rho1, theta1);
384 vpMeterPixelConversion::convertLine(cam, c->getRho2(), c->getTheta2(), rho2, theta2);
385
386 // Determine intersections between circles and limbos
387 double i11, i12, i21, i22, j11, j12, j21, j22;
388
389 vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
390 vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
391
392 vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
393 vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
394
395 // Create the image points
396 vpImagePoint ip11, ip12, ip21, ip22;
397 ip11.set_ij(i11, j11);
398 ip12.set_ij(i12, j12);
399 ip21.set_ij(i21, j21);
400 ip22.set_ij(i22, j22);
401
402 // update limits of the meline.
403 int marge = /*10*/ 5; // ou 5 normalement
404 if (ip11.get_j() < ip12.get_j()) {
405 meline1->jmin = (int)ip11.get_j() - marge;
406 meline1->jmax = (int)ip12.get_j() + marge;
407 } else {
408 meline1->jmin = (int)ip12.get_j() - marge;
409 meline1->jmax = (int)ip11.get_j() + marge;
410 }
411 if (ip11.get_i() < ip12.get_i()) {
412 meline1->imin = (int)ip11.get_i() - marge;
413 meline1->imax = (int)ip12.get_i() + marge;
414 } else {
415 meline1->imin = (int)ip12.get_i() - marge;
416 meline1->imax = (int)ip11.get_i() + marge;
417 }
418
419 if (ip21.get_j() < ip22.get_j()) {
420 meline2->jmin = (int)ip21.get_j() - marge;
421 meline2->jmax = (int)ip22.get_j() + marge;
422 } else {
423 meline2->jmin = (int)ip22.get_j() - marge;
424 meline2->jmax = (int)ip21.get_j() + marge;
425 }
426 if (ip21.get_i() < ip22.get_i()) {
427 meline2->imin = (int)ip21.get_i() - marge;
428 meline2->imax = (int)ip22.get_i() + marge;
429 } else {
430 meline2->imin = (int)ip22.get_i() - marge;
431 meline2->imax = (int)ip21.get_i() + marge;
432 }
433
434 // Initialize the tracking
435 while (theta1 > M_PI) {
436 theta1 -= M_PI;
437 }
438 while (theta1 < -M_PI) {
439 theta1 += M_PI;
440 }
441
442 if (theta1 < -M_PI / 2.0)
443 theta1 = -theta1 - 3 * M_PI / 2.0;
444 else
445 theta1 = M_PI / 2.0 - theta1;
446
447 while (theta2 > M_PI) {
448 theta2 -= M_PI;
449 }
450 while (theta2 < -M_PI) {
451 theta2 += M_PI;
452 }
453
454 if (theta2 < -M_PI / 2.0)
455 theta2 = -theta2 - 3 * M_PI / 2.0;
456 else
457 theta2 = M_PI / 2.0 - theta2;
458
459 try {
460 // meline1->updateParameters(I,rho1,theta1);
461 meline1->updateParameters(I, ip11, ip12, rho1, theta1);
462 } catch (...) {
463 Reinit = true;
464 }
465 try {
466 // meline2->updateParameters(I,rho2,theta2);
467 meline2->updateParameters(I, ip21, ip22, rho2, theta2);
468 } catch (...) {
469 Reinit = true;
470 }
471
472 // Update the numbers of features
473 nbFeaturel1 = (unsigned int)meline1->getMeList().size();
474 nbFeaturel2 = (unsigned int)meline2->getMeList().size();
476 }
477}
478
490 const vpImage<bool> *mask)
491{
492 if (meline1 != NULL)
493 delete meline1;
494 if (meline2 != NULL)
495 delete meline2;
496
497 meline1 = NULL;
498 meline2 = NULL;
499
500 if (!initMovingEdge(I, cMo, false, mask))
501 Reinit = true;
502
503 Reinit = false;
504}
505
517 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
518 bool displayFullModel)
519{
520 std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(),
521 cMo, camera, displayFullModel);
522
523 for (size_t i = 0; i < models.size(); i++) {
524 vpImagePoint ip1(models[i][1], models[i][2]);
525 vpImagePoint ip2(models[i][3], models[i][4]);
526
527 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
528 }
529}
530
542 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
543 bool displayFullModel)
544{
545 std::vector<std::vector<double> > models = getModelForDisplay(I.getWidth(), I.getHeight(),
546 cMo, camera, displayFullModel);
547
548 for (size_t i = 0; i < models.size(); i++) {
549 vpImagePoint ip1(models[i][1], models[i][2]);
550 vpImagePoint ip2(models[i][3], models[i][4]);
551
552 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
553 }
554}
555
560std::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
615std::vector<std::vector<double> > vpMbtDistanceCylinder::getModelForDisplay(unsigned int, unsigned int,
616 const vpHomogeneousMatrix &cMo,
617 const vpCameraParameters &camera,
618 bool displayFullModel)
619{
620 std::vector<std::vector<double> > models;
621
622 if ((isvisible && isTrackedCylinder) || displayFullModel) {
623 // Perspective projection
624 p1->changeFrame(cMo);
625 p2->changeFrame(cMo);
626 cercle1->changeFrame(cMo);
627 cercle2->changeFrame(cMo);
628 c->changeFrame(cMo);
629
630 p1->projection();
631 p2->projection();
632 try {
634 } catch (...) {
635 std::cout << "Problem projection circle 1";
636 }
637 try {
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 {
768 } catch (...) {
769 std::cout << "Problem projection circle 1\n";
770 }
771 try {
773 } catch (...) {
774 std::cout << "Problem projection circle 2\n";
775 }
776
777 bool disp = false;
778 bool disp2 = false;
779 if (disp || disp2)
781
782 // Build the lines
785
786 double rho1 = featureline1.getRho();
787 double theta1 = featureline1.getTheta();
788 double rho2 = featureline2.getRho();
789 double theta2 = featureline2.getTheta();
790
791 double co1 = cos(theta1);
792 double si1 = sin(theta1);
793 double co2 = cos(theta2);
794 double si2 = sin(theta2);
795
796 double mx = 1.0 / cam.get_px();
797 double my = 1.0 / cam.get_py();
798 double xc = cam.get_u0();
799 double yc = cam.get_v0();
800
801 vpMatrix H1;
802 H1 = featureline1.interaction();
803 vpMatrix H2;
804 H2 = featureline2.interaction();
805
806 vpMeSite p;
807 unsigned int j = 0;
808 for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
809 ++it) {
810 double x = (double)it->j;
811 double y = (double)it->i;
812
813 x = (x - xc) * mx;
814 y = (y - yc) * my;
815
816 double alpha1 = x * si1 - y * co1;
817
818 double *Lrho = H1[0];
819 double *Ltheta = H1[1];
820 // Calculate interaction matrix for a distance
821 for (unsigned int k = 0; k < 6; k++) {
822 L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
823 }
824 error[j] = rho1 - (x * co1 + y * si1);
825
826 if (disp)
827 vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::orange, 1);
828
829 j++;
830 }
831
832 for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
833 ++it) {
834 double x = (double)it->j;
835 double y = (double)it->i;
836
837 x = (x - xc) * mx;
838 y = (y - yc) * my;
839
840 double alpha2 = x * si2 - y * co2;
841
842 double *Lrho = H2[0];
843 double *Ltheta = H2[1];
844 // Calculate interaction matrix for a distance
845 for (unsigned int k = 0; k < 6; k++) {
846 L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
847 }
848 error[j] = rho2 - (x * co2 + y * si2);
849
850 if (disp)
851 vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::red, 1);
852
853 j++;
854 }
855 }
856}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:92
void changeFrame(const vpHomogeneousMatrix &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:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor orange
Definition: vpColor.h:227
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:103
double getRho1() const
Definition: vpCylinder.h: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:88
double get_j() const
Definition: vpImagePoint.h:214
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
vpCylinder * c
The cylinder.
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
vpPoint * p2
The second extremity on the axe.
vpCircle * cercle1
The upper circle limiting the cylinder.
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void displayMovingEdges(const vpImage< unsigned char > &I)
std::vector< std::vector< double > > getFeaturesForDisplay()
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
unsigned int nbFeature
The number of moving edges.
vpCircle * cercle2
The lower circle limiting the cylinder.
bool isvisible
Indicates if the cylinder is visible or not.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
int j
Definition: vpMeSite.h:87
vpMeSiteState getState() const
Definition: vpMeSite.h:190
double get_ifloat() const
Definition: vpMeSite.h:160
double get_jfloat() const
Definition: vpMeSite.h:167
Definition: vpMe.h:61
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp: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