Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpImageSimulator.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: Class which enables to project an image in the 3D space
32 * and get the view of a virtual camera.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpImageConvert.h>
37#include <visp3/core/vpMatrixException.h>
38#include <visp3/core/vpMeterPixelConversion.h>
39#include <visp3/core/vpPixelMeterConversion.h>
40#include <visp3/core/vpPolygon3D.h>
41#include <visp3/core/vpRotationMatrix.h>
42#include <visp3/robot/vpImageSimulator.h>
43
44#ifdef VISP_HAVE_MODULE_IO
45#include <visp3/io/vpImageIo.h>
46#endif
47
59 : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(), distance(1.),
60 visible_result(1.), visible(false), X0_2_optim(NULL), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
61 vbase_v(), vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(), colorI(col), Ig(), Ic(),
62 rect(), cleanPrevImage(false), setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
63{
64 for (int i = 0; i < 4; i++)
65 X[i].resize(3);
66
67 for (int i = 0; i < 4; i++)
68 X2[i].resize(3);
69
70 normal_obj.resize(3);
71 visible = false;
72 normal_Cam.resize(3);
73
74 // Xinter.resize(3);
75
76 vbase_u.resize(3);
77 vbase_v.resize(3);
78
79 focal.resize(3);
80 focal = 0;
81 focal[2] = 1;
82
83 normal_Cam_optim = new double[3];
84 X0_2_optim = new double[3];
85 vbase_u_optim = new double[3];
86 vbase_v_optim = new double[3];
87 Xinter_optim = new double[3];
88
89 pt.resize(4);
90}
91
96 : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(), distance(1.),
97 visible_result(1.), visible(false), X0_2_optim(NULL), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
98 vbase_v(), vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(), colorI(GRAY_SCALED), Ig(),
99 Ic(), rect(), cleanPrevImage(false), setBackgroundTexture(false), bgColor(vpColor::white), focal(),
100 needClipping(false)
101{
102 pt.resize(4);
103 for (unsigned int i = 0; i < 4; i++) {
104 X[i] = text.X[i];
105 pt[i] = text.pt[i];
106 }
107
108 for (int i = 0; i < 4; i++)
109 X2[i].resize(3);
110
111 Ic = text.Ic;
112 Ig = text.Ig;
113
114 focal.resize(3);
115 focal = 0;
116 focal[2] = 1;
117
118 normal_obj = text.normal_obj;
119 frobeniusNorm_u = text.frobeniusNorm_u;
120 fronbniusNorm_v = text.fronbniusNorm_v;
121
122 normal_Cam.resize(3);
123 vbase_u.resize(3);
124 vbase_v.resize(3);
125
126 normal_Cam_optim = new double[3];
127 X0_2_optim = new double[3];
128 vbase_u_optim = new double[3];
129 vbase_v_optim = new double[3];
130 Xinter_optim = new double[3];
131
132 colorI = text.colorI;
133 interp = text.interp;
134 bgColor = text.bgColor;
135 cleanPrevImage = text.cleanPrevImage;
136 setBackgroundTexture = false;
137
138 setCameraPosition(text.cMt);
139}
140
145{
146 delete[] normal_Cam_optim;
147 delete[] X0_2_optim;
148 delete[] vbase_u_optim;
149 delete[] vbase_v_optim;
150 delete[] Xinter_optim;
151}
152
154{
155 for (unsigned int i = 0; i < 4; i++) {
156 X[i] = sim.X[i];
157 pt[i] = sim.pt[i];
158 }
159
160 Ic = sim.Ic;
161 Ig = sim.Ig;
162
163 bgColor = sim.bgColor;
164 cleanPrevImage = sim.cleanPrevImage;
165
166 focal = sim.focal;
167
168 normal_obj = sim.normal_obj;
169 frobeniusNorm_u = sim.frobeniusNorm_u;
170 fronbniusNorm_v = sim.fronbniusNorm_v;
171
172 colorI = sim.colorI;
173 interp = sim.interp;
174
175 setCameraPosition(sim.cMt);
176
177 return *this;
178}
179
186{
187 if (setBackgroundTexture)
188 // The Ig has been set to a previously defined background texture
189 I = Ig;
190 else {
191 if (cleanPrevImage) {
192 unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
193 for (unsigned int i = 0; i < I.getHeight(); i++) {
194 for (unsigned int j = 0; j < I.getWidth(); j++) {
195 I[i][j] = col;
196 }
197 }
198 }
199 }
200
201 if (visible) {
202 if (!needClipping)
203 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
204 else
205 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
206
207 double top = rect.getTop();
208 double bottom = rect.getBottom();
209 double left = rect.getLeft();
210 double right = rect.getRight();
211
212 unsigned char *bitmap = I.bitmap;
213 unsigned int width = I.getWidth();
214 vpImagePoint ip;
215
216 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
217 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
218 double x = 0, y = 0;
219 ip.set_ij(i, j);
221 ip.set_ij(y, x);
222 if (colorI == GRAY_SCALED) {
223 unsigned char Ipixelplan = 0;
224 if (getPixel(ip, Ipixelplan)) {
225 *(bitmap + i * width + j) = Ipixelplan;
226 }
227 } else if (colorI == COLORED) {
228 vpRGBa Ipixelplan;
229 if (getPixel(ip, Ipixelplan)) {
230 unsigned char pixelgrey =
231 (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
232 *(bitmap + i * width + j) = pixelgrey;
233 }
234 }
235 }
236 }
237 }
238}
239
250{
251 if (cleanPrevImage) {
252 unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
253 for (unsigned int i = 0; i < I.getHeight(); i++) {
254 for (unsigned int j = 0; j < I.getWidth(); j++) {
255 I[i][j] = col;
256 }
257 }
258 }
259 if (visible) {
260 if (!needClipping)
261 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
262 else
263 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
264
265 double top = rect.getTop();
266 double bottom = rect.getBottom();
267 double left = rect.getLeft();
268 double right = rect.getRight();
269
270 unsigned char *bitmap = I.bitmap;
271 unsigned int width = I.getWidth();
272 vpImagePoint ip;
273
274 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
275 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
276 double x = 0, y = 0;
277 ip.set_ij(i, j);
279 ip.set_ij(y, x);
280 unsigned char Ipixelplan = 0;
281 if (getPixel(Isrc, ip, Ipixelplan)) {
282 *(bitmap + i * width + j) = Ipixelplan;
283 }
284 }
285 }
286 }
287}
288
305{
306 if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
308 " zBuffer must have the same size as the image I ! "));
309
310 if (cleanPrevImage) {
311 unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
312 for (unsigned int i = 0; i < I.getHeight(); i++) {
313 for (unsigned int j = 0; j < I.getWidth(); j++) {
314 I[i][j] = col;
315 }
316 }
317 }
318 if (visible) {
319 if (!needClipping)
320 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
321 else
322 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
323
324 double top = rect.getTop();
325 double bottom = rect.getBottom();
326 double left = rect.getLeft();
327 double right = rect.getRight();
328
329 unsigned char *bitmap = I.bitmap;
330 unsigned int width = I.getWidth();
331 vpImagePoint ip;
332
333 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
334 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
335 double x = 0, y = 0;
336 ip.set_ij(i, j);
338 ip.set_ij(y, x);
339 if (colorI == GRAY_SCALED) {
340 unsigned char Ipixelplan;
341 if (getPixel(ip, Ipixelplan)) {
342 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
343 *(bitmap + i * width + j) = Ipixelplan;
344 zBuffer[i][j] = Xinter_optim[2];
345 }
346 }
347 } else if (colorI == COLORED) {
348 vpRGBa Ipixelplan;
349 if (getPixel(ip, Ipixelplan)) {
350 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
351 unsigned char pixelgrey =
352 (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
353 *(bitmap + i * width + j) = pixelgrey;
354 zBuffer[i][j] = Xinter_optim[2];
355 }
356 }
357 }
358 }
359 }
360 }
361}
362
371{
372 if (cleanPrevImage) {
373 for (unsigned int i = 0; i < I.getHeight(); i++) {
374 for (unsigned int j = 0; j < I.getWidth(); j++) {
375 I[i][j] = bgColor;
376 }
377 }
378 }
379
380 if (visible) {
381 if (!needClipping)
382 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
383 else
384 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
385
386 double top = rect.getTop();
387 double bottom = rect.getBottom();
388 double left = rect.getLeft();
389 double right = rect.getRight();
390
391 vpRGBa *bitmap = I.bitmap;
392 unsigned int width = I.getWidth();
393 vpImagePoint ip;
394
395 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
396 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
397 double x = 0, y = 0;
398 ip.set_ij(i, j);
400 ip.set_ij(y, x);
401 if (colorI == GRAY_SCALED) {
402 unsigned char Ipixelplan;
403 if (getPixel(ip, Ipixelplan)) {
404 vpRGBa pixelcolor;
405 pixelcolor.R = Ipixelplan;
406 pixelcolor.G = Ipixelplan;
407 pixelcolor.B = Ipixelplan;
408 *(bitmap + i * width + j) = pixelcolor;
409 }
410 } else if (colorI == COLORED) {
411 vpRGBa Ipixelplan;
412 if (getPixel(ip, Ipixelplan)) {
413 *(bitmap + i * width + j) = Ipixelplan;
414 }
415 }
416 }
417 }
418 }
419}
420
431{
432 if (cleanPrevImage) {
433 for (unsigned int i = 0; i < I.getHeight(); i++) {
434 for (unsigned int j = 0; j < I.getWidth(); j++) {
435 I[i][j] = bgColor;
436 }
437 }
438 }
439
440 if (visible) {
441 if (!needClipping)
442 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
443 else
444 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
445
446 double top = rect.getTop();
447 double bottom = rect.getBottom();
448 double left = rect.getLeft();
449 double right = rect.getRight();
450
451 vpRGBa *bitmap = I.bitmap;
452 unsigned int width = I.getWidth();
453 vpImagePoint ip;
454
455 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
456 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
457 double x = 0, y = 0;
458 ip.set_ij(i, j);
460 ip.set_ij(y, x);
461 vpRGBa Ipixelplan;
462 if (getPixel(Isrc, ip, Ipixelplan)) {
463 *(bitmap + i * width + j) = Ipixelplan;
464 }
465 }
466 }
467 }
468}
469
486{
487 if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
489 " zBuffer must have the same size as the image I ! "));
490
491 if (cleanPrevImage) {
492 for (unsigned int i = 0; i < I.getHeight(); i++) {
493 for (unsigned int j = 0; j < I.getWidth(); j++) {
494 I[i][j] = bgColor;
495 }
496 }
497 }
498 if (visible) {
499 if (!needClipping)
500 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
501 else
502 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
503
504 double top = rect.getTop();
505 double bottom = rect.getBottom();
506 double left = rect.getLeft();
507 double right = rect.getRight();
508
509 vpRGBa *bitmap = I.bitmap;
510 unsigned int width = I.getWidth();
511 vpImagePoint ip;
512
513 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
514 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
515 double x = 0, y = 0;
516 ip.set_ij(i, j);
518 ip.set_ij(y, x);
519 if (colorI == GRAY_SCALED) {
520 unsigned char Ipixelplan;
521 if (getPixel(ip, Ipixelplan)) {
522 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
523 vpRGBa pixelcolor;
524 pixelcolor.R = Ipixelplan;
525 pixelcolor.G = Ipixelplan;
526 pixelcolor.B = Ipixelplan;
527 *(bitmap + i * width + j) = pixelcolor;
528 zBuffer[i][j] = Xinter_optim[2];
529 }
530 }
531 } else if (colorI == COLORED) {
532 vpRGBa Ipixelplan;
533 if (getPixel(ip, Ipixelplan)) {
534 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
535 *(bitmap + i * width + j) = Ipixelplan;
536 zBuffer[i][j] = Xinter_optim[2];
537 }
538 }
539 }
540 }
541 }
542 }
543}
544
649void vpImageSimulator::getImage(vpImage<unsigned char> &I, std::list<vpImageSimulator> &list,
650 const vpCameraParameters &cam)
651{
652
653 unsigned int width = I.getWidth();
654 unsigned int height = I.getHeight();
655
656 unsigned int nbsimList = (unsigned int)list.size();
657
658 if (nbsimList < 1)
659 return;
660
661 vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
662
663 double topFinal = height + 1;
664 ;
665 double bottomFinal = -1;
666 double leftFinal = width + 1;
667 double rightFinal = -1;
668
669 unsigned int unvisible = 0;
670 unsigned int indexSimu = 0;
671 for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
672 vpImageSimulator *sim = &(*it);
673 if (sim->visible)
674 simList[indexSimu] = sim;
675 else
676 unvisible++;
677 }
678 nbsimList = nbsimList - unvisible;
679
680 if (nbsimList < 1) {
681 delete[] simList;
682 return;
683 }
684
685 for (unsigned int i = 0; i < nbsimList; i++) {
686 if (!simList[i]->needClipping)
687 simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
688 else
689 simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
690
691 if (topFinal > simList[i]->rect.getTop())
692 topFinal = simList[i]->rect.getTop();
693 if (bottomFinal < simList[i]->rect.getBottom())
694 bottomFinal = simList[i]->rect.getBottom();
695 if (leftFinal > simList[i]->rect.getLeft())
696 leftFinal = simList[i]->rect.getLeft();
697 if (rightFinal < simList[i]->rect.getRight())
698 rightFinal = simList[i]->rect.getRight();
699 }
700
701 double zmin = -1;
702 int indice = -1;
703 unsigned char *bitmap = I.bitmap;
704 vpImagePoint ip;
705
706 for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++) {
707 for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++) {
708 zmin = -1;
709 double x = 0, y = 0;
710 ip.set_ij(i, j);
712 ip.set_ij(y, x);
713 for (int k = 0; k < (int)nbsimList; k++) {
714 double z = 0;
715 if (simList[k]->getPixelDepth(ip, z)) {
716 if (z < zmin || zmin < 0) {
717 zmin = z;
718 indice = k;
719 }
720 }
721 }
722 if (indice >= 0) {
723 if (simList[indice]->colorI == GRAY_SCALED) {
724 unsigned char Ipixelplan = 255;
725 simList[indice]->getPixel(ip, Ipixelplan);
726 *(bitmap + i * width + j) = Ipixelplan;
727 } else if (simList[indice]->colorI == COLORED) {
728 vpRGBa Ipixelplan(255, 255, 255);
729 simList[indice]->getPixel(ip, Ipixelplan);
730 unsigned char pixelgrey =
731 (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
732 *(bitmap + i * width + j) = pixelgrey;
733 }
734 }
735 }
736 }
737
738 delete[] simList;
739}
740
847void vpImageSimulator::getImage(vpImage<vpRGBa> &I, std::list<vpImageSimulator> &list, const vpCameraParameters &cam)
848{
849
850 unsigned int width = I.getWidth();
851 unsigned int height = I.getHeight();
852
853 unsigned int nbsimList = (unsigned int)list.size();
854
855 if (nbsimList < 1)
856 return;
857
858 vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
859
860 double topFinal = height + 1;
861 ;
862 double bottomFinal = -1;
863 double leftFinal = width + 1;
864 double rightFinal = -1;
865
866 unsigned int unvisible = 0;
867 unsigned int indexSimu = 0;
868 for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
869 vpImageSimulator *sim = &(*it);
870 if (sim->visible)
871 simList[indexSimu] = sim;
872 else
873 unvisible++;
874 }
875
876 nbsimList = nbsimList - unvisible;
877
878 if (nbsimList < 1) {
879 delete[] simList;
880 return;
881 }
882
883 for (unsigned int i = 0; i < nbsimList; i++) {
884 if (!simList[i]->needClipping)
885 simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
886 else
887 simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
888
889 if (topFinal > simList[i]->rect.getTop())
890 topFinal = simList[i]->rect.getTop();
891 if (bottomFinal < simList[i]->rect.getBottom())
892 bottomFinal = simList[i]->rect.getBottom();
893 if (leftFinal > simList[i]->rect.getLeft())
894 leftFinal = simList[i]->rect.getLeft();
895 if (rightFinal < simList[i]->rect.getRight())
896 rightFinal = simList[i]->rect.getRight();
897 }
898
899 double zmin = -1;
900 int indice = -1;
901 vpRGBa *bitmap = I.bitmap;
902 vpImagePoint ip;
903
904 for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++) {
905 for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++) {
906 zmin = -1;
907 double x = 0, y = 0;
908 ip.set_ij(i, j);
910 ip.set_ij(y, x);
911 for (int k = 0; k < (int)nbsimList; k++) {
912 double z = 0;
913 if (simList[k]->getPixelDepth(ip, z)) {
914 if (z < zmin || zmin < 0) {
915 zmin = z;
916 indice = k;
917 }
918 }
919 }
920 if (indice >= 0) {
921 if (simList[indice]->colorI == GRAY_SCALED) {
922 unsigned char Ipixelplan = 255;
923 simList[indice]->getPixel(ip, Ipixelplan);
924 vpRGBa pixelcolor;
925 pixelcolor.R = Ipixelplan;
926 pixelcolor.G = Ipixelplan;
927 pixelcolor.B = Ipixelplan;
928 *(bitmap + i * width + j) = pixelcolor;
929 } else if (simList[indice]->colorI == COLORED) {
930 vpRGBa Ipixelplan(255, 255, 255);
931 simList[indice]->getPixel(ip, Ipixelplan);
932 // unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 *
933 // Ipixelplan.G + 0.0722 * Ipixelplan.B;
934 *(bitmap + i * width + j) = Ipixelplan;
935 }
936 }
937 }
938 }
939
940 delete[] simList;
941}
942
949{
950 cMt = cMt_;
952 cMt.extract(R);
953 needClipping = false;
954
955 normal_Cam = R * normal_obj;
956
957 visible_result = vpColVector::dotProd(normal_Cam, focal);
958
959 for (unsigned int i = 0; i < 4; i++)
960 pt[i].track(cMt);
961
962 vpColVector e1(3);
963 vpColVector e2(3);
964 vpColVector facenormal(3);
965
966 e1[0] = pt[1].get_X() - pt[0].get_X();
967 e1[1] = pt[1].get_Y() - pt[0].get_Y();
968 e1[2] = pt[1].get_Z() - pt[0].get_Z();
969
970 e2[0] = pt[2].get_X() - pt[1].get_X();
971 e2[1] = pt[2].get_Y() - pt[1].get_Y();
972 e2[2] = pt[2].get_Z() - pt[1].get_Z();
973
974 facenormal = vpColVector::crossProd(e1, e2);
975
976 double angle = pt[0].get_X() * facenormal[0] + pt[0].get_Y() * facenormal[1] + pt[0].get_Z() * facenormal[2];
977
978 if (angle > 0) {
979 visible = true;
980 } else {
981 visible = false;
982 }
983
984 if (visible) {
985 for (unsigned int i = 0; i < 4; i++) {
986 project(X[i], cMt, X2[i]);
987 pt[i].track(cMt);
988 if (pt[i].get_Z() < 0)
989 needClipping = true;
990 }
991
992 vbase_u = X2[1] - X2[0];
993 vbase_v = X2[3] - X2[0];
994
995 distance = vpColVector::dotProd(normal_Cam, X2[1]);
996
997 if (distance < 0) {
998 visible = false;
999 return;
1000 }
1001
1002 for (unsigned int i = 0; i < 3; i++) {
1003 normal_Cam_optim[i] = normal_Cam[i];
1004 X0_2_optim[i] = X2[0][i];
1005 vbase_u_optim[i] = vbase_u[i];
1006 vbase_v_optim[i] = vbase_v[i];
1007 }
1008
1009 std::vector<vpPoint> *ptPtr = &pt;
1010 if (needClipping) {
1012 ptPtr = &ptClipped;
1013 }
1014
1015 listTriangle.clear();
1016 for (unsigned int i = 1; i < (*ptPtr).size() - 1; i++) {
1017 vpImagePoint ip1, ip2, ip3;
1018 ip1.set_j((*ptPtr)[0].get_x());
1019 ip1.set_i((*ptPtr)[0].get_y());
1020
1021 ip2.set_j((*ptPtr)[i].get_x());
1022 ip2.set_i((*ptPtr)[i].get_y());
1023
1024 ip3.set_j((*ptPtr)[i + 1].get_x());
1025 ip3.set_i((*ptPtr)[i + 1].get_y());
1026
1027 vpTriangle tri(ip1, ip2, ip3);
1028 listTriangle.push_back(tri);
1029 }
1030 }
1031}
1032
1033void vpImageSimulator::initPlan(vpColVector *X_)
1034{
1035 for (unsigned int i = 0; i < 4; i++) {
1036 X[i] = X_[i];
1037 pt[i].setWorldCoordinates(X_[i][0], X_[i][1], X_[i][2]);
1038 }
1039
1040 normal_obj = vpColVector::crossProd(X[1] - X[0], X[3] - X[0]);
1041 normal_obj = normal_obj / normal_obj.frobeniusNorm();
1042
1043 frobeniusNorm_u = (X[1] - X[0]).frobeniusNorm();
1044 fronbniusNorm_v = (X[3] - X[0]).frobeniusNorm();
1045}
1046
1062{
1063 Ig = I;
1065 initPlan(X_);
1066}
1067
1083{
1084 Ic = I;
1086 initPlan(X_);
1087}
1088
1089#ifdef VISP_HAVE_MODULE_IO
1105void vpImageSimulator::init(const char *file_image, vpColVector *X_)
1106{
1107 vpImageIo::read(Ig, file_image);
1108 vpImageIo::read(Ic, file_image);
1109 initPlan(X_);
1110}
1111#endif
1112
1128void vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoint> &X_)
1129{
1130 if (X_.size() != 4) {
1131 throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1132 }
1133 vpColVector Xvec[4];
1134 for (unsigned int i = 0; i < 4; ++i) {
1135 Xvec[i].resize(3);
1136 Xvec[i][0] = X_[i].get_oX();
1137 Xvec[i][1] = X_[i].get_oY();
1138 Xvec[i][2] = X_[i].get_oZ();
1139 }
1140
1141 Ig = I;
1143 initPlan(Xvec);
1144}
1160void vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint> &X_)
1161{
1162 if (X_.size() != 4) {
1163 throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1164 }
1165 vpColVector Xvec[4];
1166 for (unsigned int i = 0; i < 4; ++i) {
1167 Xvec[i].resize(3);
1168 Xvec[i][0] = X_[i].get_oX();
1169 Xvec[i][1] = X_[i].get_oY();
1170 Xvec[i][2] = X_[i].get_oZ();
1171 }
1172
1173 Ic = I;
1175 initPlan(Xvec);
1176}
1177
1178#ifdef VISP_HAVE_MODULE_IO
1195void vpImageSimulator::init(const char *file_image, const std::vector<vpPoint> &X_)
1196{
1197 if (X_.size() != 4) {
1198 throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1199 }
1200 vpColVector Xvec[4];
1201 for (unsigned int i = 0; i < 4; ++i) {
1202 Xvec[i].resize(3);
1203 Xvec[i][0] = X_[i].get_oX();
1204 Xvec[i][1] = X_[i].get_oY();
1205 Xvec[i][2] = X_[i].get_oZ();
1206 }
1207
1208 vpImageIo::read(Ig, file_image);
1209 vpImageIo::read(Ic, file_image);
1210 initPlan(Xvec);
1211}
1212#endif
1213
1214bool vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan)
1215{
1216 // std::cout << "In get Pixel" << std::endl;
1217 // test si pixel dans zone projetee
1218 bool inside = false;
1219 for (unsigned int i = 0; i < listTriangle.size(); i++)
1220 if (listTriangle[i].inTriangle(iP)) {
1221 inside = true;
1222 break;
1223 }
1224 if (!inside)
1225 return false;
1226
1227 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
1230 // return false;}
1231
1232 // methoed algebrique
1233 double z;
1234
1235 // calcul de la profondeur de l'intersection
1236 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1237 // calcul coordonnees 3D intersection
1238 Xinter_optim[0] = iP.get_u() * z;
1239 Xinter_optim[1] = iP.get_v() * z;
1240 Xinter_optim[2] = z;
1241
1242 // recuperation des coordonnes de l'intersection dans le plan objet
1243 // repere plan object :
1244 // centre = X0_2_optim[i] (premier point definissant le plan)
1245 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1246 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1247 // simplement obtenu par un produit scalaire
1248 double u = 0, v = 0;
1249 for (unsigned int i = 0; i < 3; i++) {
1250 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1251 u += diff * vbase_u_optim[i];
1252 v += diff * vbase_v_optim[i];
1253 }
1254 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1255 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1256
1257 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1258 double i2, j2;
1259 i2 = v * (Ig.getHeight() - 1);
1260 j2 = u * (Ig.getWidth() - 1);
1261 if (interp == BILINEAR_INTERPOLATION)
1262 Ipixelplan = Ig.getValue(i2, j2);
1263 else if (interp == SIMPLE)
1264 Ipixelplan = Ig[(unsigned int)i2][(unsigned int)j2];
1265 return true;
1266 } else
1267 return false;
1268}
1269
1270bool vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP, unsigned char &Ipixelplan)
1271{
1272 // test si pixel dans zone projetee
1273 bool inside = false;
1274 for (unsigned int i = 0; i < listTriangle.size(); i++)
1275 if (listTriangle[i].inTriangle(iP)) {
1276 inside = true;
1277 break;
1278 }
1279 if (!inside)
1280 return false;
1281
1282 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1283 // return false;
1284
1285 // methoed algebrique
1286 double z;
1287
1288 // calcul de la profondeur de l'intersection
1289 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1290 // calcul coordonnees 3D intersection
1291 Xinter_optim[0] = iP.get_u() * z;
1292 Xinter_optim[1] = iP.get_v() * z;
1293 Xinter_optim[2] = z;
1294
1295 // recuperation des coordonnes de l'intersection dans le plan objet
1296 // repere plan object :
1297 // centre = X0_2_optim[i] (premier point definissant le plan)
1298 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1299 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1300 // simplement obtenu par un produit scalaire
1301 double u = 0, v = 0;
1302 for (unsigned int i = 0; i < 3; i++) {
1303 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1304 u += diff * vbase_u_optim[i];
1305 v += diff * vbase_v_optim[i];
1306 }
1307 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1308 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1309
1310 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1311 double i2, j2;
1312 i2 = v * (Isrc.getHeight() - 1);
1313 j2 = u * (Isrc.getWidth() - 1);
1314 if (interp == BILINEAR_INTERPOLATION)
1315 Ipixelplan = Isrc.getValue(i2, j2);
1316 else if (interp == SIMPLE)
1317 Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
1318 return true;
1319 } else
1320 return false;
1321}
1322
1323bool vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan)
1324{
1325 // test si pixel dans zone projetee
1326 bool inside = false;
1327 for (unsigned int i = 0; i < listTriangle.size(); i++)
1328 if (listTriangle[i].inTriangle(iP)) {
1329 inside = true;
1330 break;
1331 }
1332 if (!inside)
1333 return false;
1334 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1335 // return false;
1336
1337 // methoed algebrique
1338 double z;
1339
1340 // calcul de la profondeur de l'intersection
1341 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1342 // calcul coordonnees 3D intersection
1343 Xinter_optim[0] = iP.get_u() * z;
1344 Xinter_optim[1] = iP.get_v() * z;
1345 Xinter_optim[2] = z;
1346
1347 // recuperation des coordonnes de l'intersection dans le plan objet
1348 // repere plan object :
1349 // centre = X0_2_optim[i] (premier point definissant le plan)
1350 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1351 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1352 // simplement obtenu par un produit scalaire
1353 double u = 0, v = 0;
1354 for (unsigned int i = 0; i < 3; i++) {
1355 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1356 u += diff * vbase_u_optim[i];
1357 v += diff * vbase_v_optim[i];
1358 }
1359 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1360 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1361
1362 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1363 double i2, j2;
1364 i2 = v * (Ic.getHeight() - 1);
1365 j2 = u * (Ic.getWidth() - 1);
1366 if (interp == BILINEAR_INTERPOLATION)
1367 Ipixelplan = Ic.getValue(i2, j2);
1368 else if (interp == SIMPLE)
1369 Ipixelplan = Ic[(unsigned int)i2][(unsigned int)j2];
1370 return true;
1371 } else
1372 return false;
1373}
1374
1375bool vpImageSimulator::getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP, vpRGBa &Ipixelplan)
1376{
1377 // test si pixel dans zone projetee
1378 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1379 // return false;
1380 bool inside = false;
1381 for (unsigned int i = 0; i < listTriangle.size(); i++)
1382 if (listTriangle[i].inTriangle(iP)) {
1383 inside = true;
1384 break;
1385 }
1386 if (!inside)
1387 return false;
1388
1389 // methoed algebrique
1390 double z;
1391
1392 // calcul de la profondeur de l'intersection
1393 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1394 // calcul coordonnees 3D intersection
1395 Xinter_optim[0] = iP.get_u() * z;
1396 Xinter_optim[1] = iP.get_v() * z;
1397 Xinter_optim[2] = z;
1398
1399 // recuperation des coordonnes de l'intersection dans le plan objet
1400 // repere plan object :
1401 // centre = X0_2_optim[i] (premier point definissant le plan)
1402 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1403 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1404 // simplement obtenu par un produit scalaire
1405 double u = 0, v = 0;
1406 for (unsigned int i = 0; i < 3; i++) {
1407 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1408 u += diff * vbase_u_optim[i];
1409 v += diff * vbase_v_optim[i];
1410 }
1411 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1412 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1413
1414 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1415 double i2, j2;
1416 i2 = v * (Isrc.getHeight() - 1);
1417 j2 = u * (Isrc.getWidth() - 1);
1418 if (interp == BILINEAR_INTERPOLATION)
1419 Ipixelplan = Isrc.getValue(i2, j2);
1420 else if (interp == SIMPLE)
1421 Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
1422 return true;
1423 } else
1424 return false;
1425}
1426
1427bool vpImageSimulator::getPixelDepth(const vpImagePoint &iP, double &Zpixelplan)
1428{
1429 // test si pixel dans zone projetee
1430 bool inside = false;
1431 for (unsigned int i = 0; i < listTriangle.size(); i++)
1432 if (listTriangle[i].inTriangle(iP)) {
1433 inside = true;
1434 break;
1435 }
1436 if (!inside)
1437 return false;
1438 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1439 // return false;
1440
1441 Zpixelplan = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1442 return true;
1443}
1444
1445bool vpImageSimulator::getPixelVisibility(const vpImagePoint &iP, double &Visipixelplan)
1446{
1447 // test si pixel dans zone projetee
1448 bool inside = false;
1449 for (unsigned int i = 0; i < listTriangle.size(); i++)
1450 if (listTriangle[i].inTriangle(iP)) {
1451 inside = true;
1452 break;
1453 }
1454 if (!inside)
1455 return false;
1456 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1457 // return false;
1458
1459 Visipixelplan = visible_result;
1460 return true;
1461}
1462
1463void vpImageSimulator::project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt, vpColVector &_vout)
1464{
1465 vpColVector XH(4);
1466 getHomogCoord(_vin, XH);
1467 getCoordFromHomog(_cMt * XH, _vout);
1468}
1469
1470void vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH)
1471{
1472 for (unsigned int i = 0; i < 3; i++)
1473 _vH[i] = _v[i];
1474 _vH[3] = 1.;
1475}
1476
1477void vpImageSimulator::getCoordFromHomog(const vpColVector &_vH, vpColVector &_v)
1478{
1479 for (unsigned int i = 0; i < 3; i++)
1480 _v[i] = _vH[i] / _vH[3];
1481}
1482
1483void vpImageSimulator::getRoi(const unsigned int &Iwidth, const unsigned int &Iheight, const vpCameraParameters &cam,
1484 const std::vector<vpPoint> &point, vpRect &rectangle)
1485{
1486 double top = Iheight + 1;
1487 double bottom = -1;
1488 double right = -1;
1489 double left = Iwidth + 1;
1490
1491 for (unsigned int i = 0; i < point.size(); i++) {
1492 double u = 0, v = 0;
1493 vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), u, v);
1494 if (v < top)
1495 top = v;
1496 if (v > bottom)
1497 bottom = v;
1498 if (u < left)
1499 left = u;
1500 if (u > right)
1501 right = u;
1502 }
1503 if (top < 0)
1504 top = 0;
1505 if (top >= Iheight)
1506 top = Iheight - 1;
1507 if (bottom < 0)
1508 bottom = 0;
1509 if (bottom >= Iheight)
1510 bottom = Iheight - 1;
1511 if (left < 0)
1512 left = 0;
1513 if (left >= Iwidth)
1514 left = Iwidth - 1;
1515 if (right < 0)
1516 right = 0;
1517 if (right >= Iwidth)
1518 right = Iwidth - 1;
1519
1520 rectangle.setTop(top);
1521 rectangle.setBottom(bottom);
1522 rectangle.setLeft(left);
1523 rectangle.setRight(right);
1524}
1525
1527{
1528 std::vector<vpColVector> X_;
1529 for (int i = 0; i < 4; i++)
1530 X_.push_back(X[i]);
1531 return X_;
1532}
1533
1534VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/)
1535{
1536 os << "";
1537 return os;
1538}
unsigned int getCols() const
Definition vpArray2D.h:280
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
void set_ij(double ii, double jj)
void set_i(double ii)
double get_u() const
double get_v() const
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
vpImageSimulator(const vpColorPlan &col=COLORED)
std::vector< vpColVector > get3DcornersTextureRectangle()
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
vpImageSimulator & operator=(const vpImageSimulator &sim)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
Type getValue(unsigned int i, unsigned int j) const
Definition vpImage.h:1592
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
unsigned int getHeight() const
Definition vpImage.h:184
error that can be emitted by the vpMatrix class and its derivatives
@ incorrectMatrixSizeError
Incorrect matrix size.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
static void getClippedPolygon(const std::vector< vpPoint > &ptIn, std::vector< vpPoint > &ptOut, const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags, const vpCameraParameters &cam=vpCameraParameters(), const double &znear=0.001, const double &zfar=100)
unsigned char B
Blue component.
Definition vpRGBa.h:140
unsigned char R
Red component.
Definition vpRGBa.h:138
unsigned char G
Green component.
Definition vpRGBa.h:139
Defines a rectangle in the plane.
Definition vpRect.h:76
void setTop(double pos)
Definition vpRect.h:354
double getLeft() const
Definition vpRect.h:170
void setLeft(double pos)
Definition vpRect.h:318
void setRight(double pos)
Definition vpRect.h:345
double getRight() const
Definition vpRect.h:176
double getBottom() const
Definition vpRect.h:94
void setBottom(double pos)
Definition vpRect.h:285
double getTop() const
Definition vpRect.h:189
Implementation of a rotation matrix and operations on such kind of matrices.
Defines a 2D triangle.
Definition vpTriangle.h:53