Visual Servoing Platform version 3.5.0
vpImageSimulator.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: Class which enables to project an image in the 3D space
32 * and get the view of a virtual camera.
33 *
34 * Authors:
35 * Amaury Dame
36 * Nicolas Melchior
37 *
38 *****************************************************************************/
39
40#include <visp3/core/vpImageConvert.h>
41#include <visp3/core/vpMatrixException.h>
42#include <visp3/core/vpMeterPixelConversion.h>
43#include <visp3/core/vpPixelMeterConversion.h>
44#include <visp3/core/vpPolygon3D.h>
45#include <visp3/core/vpRotationMatrix.h>
46#include <visp3/robot/vpImageSimulator.h>
47
48#ifdef VISP_HAVE_MODULE_IO
49#include <visp3/io/vpImageIo.h>
50#endif
51
63 : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(), distance(1.),
64 visible_result(1.), visible(false), X0_2_optim(NULL), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
65 vbase_v(), vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(), colorI(col), Ig(), Ic(),
66 rect(), cleanPrevImage(false), setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
67{
68 for (int i = 0; i < 4; i++)
69 X[i].resize(3);
70
71 for (int i = 0; i < 4; i++)
72 X2[i].resize(3);
73
74 normal_obj.resize(3);
75 visible = false;
76 normal_Cam.resize(3);
77
78 // Xinter.resize(3);
79
80 vbase_u.resize(3);
81 vbase_v.resize(3);
82
83 focal.resize(3);
84 focal = 0;
85 focal[2] = 1;
86
87 normal_Cam_optim = new double[3];
88 X0_2_optim = new double[3];
89 vbase_u_optim = new double[3];
90 vbase_v_optim = new double[3];
91 Xinter_optim = new double[3];
92
93 pt.resize(4);
94}
95
100 : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(), distance(1.),
101 visible_result(1.), visible(false), X0_2_optim(NULL), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
102 vbase_v(), vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(), colorI(GRAY_SCALED), Ig(),
103 Ic(), rect(), cleanPrevImage(false), setBackgroundTexture(false), bgColor(vpColor::white), focal(),
104 needClipping(false)
105{
106 pt.resize(4);
107 for (unsigned int i = 0; i < 4; i++) {
108 X[i] = text.X[i];
109 pt[i] = text.pt[i];
110 }
111
112 for (int i = 0; i < 4; i++)
113 X2[i].resize(3);
114
115 Ic = text.Ic;
116 Ig = text.Ig;
117
118 focal.resize(3);
119 focal = 0;
120 focal[2] = 1;
121
122 normal_obj = text.normal_obj;
123 frobeniusNorm_u = text.frobeniusNorm_u;
124 fronbniusNorm_v = text.fronbniusNorm_v;
125
126 normal_Cam.resize(3);
127 vbase_u.resize(3);
128 vbase_v.resize(3);
129
130 normal_Cam_optim = new double[3];
131 X0_2_optim = new double[3];
132 vbase_u_optim = new double[3];
133 vbase_v_optim = new double[3];
134 Xinter_optim = new double[3];
135
136 colorI = text.colorI;
137 interp = text.interp;
138 bgColor = text.bgColor;
139 cleanPrevImage = text.cleanPrevImage;
140 setBackgroundTexture = false;
141
142 setCameraPosition(text.cMt);
143}
144
149{
150 delete[] normal_Cam_optim;
151 delete[] X0_2_optim;
152 delete[] vbase_u_optim;
153 delete[] vbase_v_optim;
154 delete[] Xinter_optim;
155}
156
158{
159 for (unsigned int i = 0; i < 4; i++) {
160 X[i] = sim.X[i];
161 pt[i] = sim.pt[i];
162 }
163
164 Ic = sim.Ic;
165 Ig = sim.Ig;
166
167 bgColor = sim.bgColor;
168 cleanPrevImage = sim.cleanPrevImage;
169
170 focal = sim.focal;
171
172 normal_obj = sim.normal_obj;
173 frobeniusNorm_u = sim.frobeniusNorm_u;
174 fronbniusNorm_v = sim.fronbniusNorm_v;
175
176 colorI = sim.colorI;
177 interp = sim.interp;
178
179 setCameraPosition(sim.cMt);
180
181 return *this;
182}
183
190{
191 if (setBackgroundTexture)
192 // The Ig has been set to a previously defined background texture
193 I = Ig;
194 else {
195 if (cleanPrevImage) {
196 unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
197 for (unsigned int i = 0; i < I.getHeight(); i++) {
198 for (unsigned int j = 0; j < I.getWidth(); j++) {
199 I[i][j] = col;
200 }
201 }
202 }
203 }
204
205 if (visible) {
206 if (!needClipping)
207 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
208 else
209 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
210
211 double top = rect.getTop();
212 double bottom = rect.getBottom();
213 double left = rect.getLeft();
214 double right = rect.getRight();
215
216 unsigned char *bitmap = I.bitmap;
217 unsigned int width = I.getWidth();
218 vpImagePoint ip;
219 int nb_point_dessine = 0;
220
221 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
222 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
223 double x = 0, y = 0;
224 ip.set_ij(i, j);
226 ip.set_ij(y, x);
227 if (colorI == GRAY_SCALED) {
228 unsigned char Ipixelplan = 0;
229 if (getPixel(ip, Ipixelplan)) {
230 *(bitmap + i * width + j) = Ipixelplan;
231 nb_point_dessine++;
232 }
233 } else if (colorI == COLORED) {
234 vpRGBa Ipixelplan;
235 if (getPixel(ip, Ipixelplan)) {
236 unsigned char pixelgrey =
237 (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
238 *(bitmap + i * width + j) = pixelgrey;
239 nb_point_dessine++;
240 }
241 }
242 }
243 }
244 }
245}
246
257{
258 if (cleanPrevImage) {
259 unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
260 for (unsigned int i = 0; i < I.getHeight(); i++) {
261 for (unsigned int j = 0; j < I.getWidth(); j++) {
262 I[i][j] = col;
263 }
264 }
265 }
266 if (visible) {
267 if (!needClipping)
268 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
269 else
270 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
271
272 double top = rect.getTop();
273 double bottom = rect.getBottom();
274 double left = rect.getLeft();
275 double right = rect.getRight();
276
277 unsigned char *bitmap = I.bitmap;
278 unsigned int width = I.getWidth();
279 vpImagePoint ip;
280 int nb_point_dessine = 0;
281
282 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
283 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
284 double x = 0, y = 0;
285 ip.set_ij(i, j);
287 ip.set_ij(y, x);
288 unsigned char Ipixelplan = 0;
289 if (getPixel(Isrc, ip, Ipixelplan)) {
290 *(bitmap + i * width + j) = Ipixelplan;
291 nb_point_dessine++;
292 }
293 }
294 }
295 }
296}
297
314{
315 if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
317 " zBuffer must have the same size as the image I ! "));
318
319 if (cleanPrevImage) {
320 unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
321 for (unsigned int i = 0; i < I.getHeight(); i++) {
322 for (unsigned int j = 0; j < I.getWidth(); j++) {
323 I[i][j] = col;
324 }
325 }
326 }
327 if (visible) {
328 if (!needClipping)
329 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
330 else
331 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
332
333 double top = rect.getTop();
334 double bottom = rect.getBottom();
335 double left = rect.getLeft();
336 double right = rect.getRight();
337
338 unsigned char *bitmap = I.bitmap;
339 unsigned int width = I.getWidth();
340 vpImagePoint ip;
341 int nb_point_dessine = 0;
342
343 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
344 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
345 double x = 0, y = 0;
346 ip.set_ij(i, j);
348 ip.set_ij(y, x);
349 if (colorI == GRAY_SCALED) {
350 unsigned char Ipixelplan;
351 if (getPixel(ip, Ipixelplan)) {
352 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
353 *(bitmap + i * width + j) = Ipixelplan;
354 nb_point_dessine++;
355 zBuffer[i][j] = Xinter_optim[2];
356 }
357 }
358 } else if (colorI == COLORED) {
359 vpRGBa Ipixelplan;
360 if (getPixel(ip, Ipixelplan)) {
361 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
362 unsigned char pixelgrey =
363 (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
364 *(bitmap + i * width + j) = pixelgrey;
365 nb_point_dessine++;
366 zBuffer[i][j] = Xinter_optim[2];
367 }
368 }
369 }
370 }
371 }
372 }
373}
374
383{
384 if (cleanPrevImage) {
385 for (unsigned int i = 0; i < I.getHeight(); i++) {
386 for (unsigned int j = 0; j < I.getWidth(); j++) {
387 I[i][j] = bgColor;
388 }
389 }
390 }
391
392 if (visible) {
393 if (!needClipping)
394 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
395 else
396 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
397
398 double top = rect.getTop();
399 double bottom = rect.getBottom();
400 double left = rect.getLeft();
401 double right = rect.getRight();
402
403 vpRGBa *bitmap = I.bitmap;
404 unsigned int width = I.getWidth();
405 vpImagePoint ip;
406 int nb_point_dessine = 0;
407
408 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
409 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
410 double x = 0, y = 0;
411 ip.set_ij(i, j);
413 ip.set_ij(y, x);
414 if (colorI == GRAY_SCALED) {
415 unsigned char Ipixelplan;
416 if (getPixel(ip, Ipixelplan)) {
417 vpRGBa pixelcolor;
418 pixelcolor.R = Ipixelplan;
419 pixelcolor.G = Ipixelplan;
420 pixelcolor.B = Ipixelplan;
421 *(bitmap + i * width + j) = pixelcolor;
422 nb_point_dessine++;
423 }
424 } else if (colorI == COLORED) {
425 vpRGBa Ipixelplan;
426 if (getPixel(ip, Ipixelplan)) {
427 *(bitmap + i * width + j) = Ipixelplan;
428 nb_point_dessine++;
429 }
430 }
431 }
432 }
433 }
434}
435
446{
447 if (cleanPrevImage) {
448 for (unsigned int i = 0; i < I.getHeight(); i++) {
449 for (unsigned int j = 0; j < I.getWidth(); j++) {
450 I[i][j] = bgColor;
451 }
452 }
453 }
454
455 if (visible) {
456 if (!needClipping)
457 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
458 else
459 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
460
461 double top = rect.getTop();
462 double bottom = rect.getBottom();
463 double left = rect.getLeft();
464 double right = rect.getRight();
465
466 vpRGBa *bitmap = I.bitmap;
467 unsigned int width = I.getWidth();
468 vpImagePoint ip;
469 int nb_point_dessine = 0;
470
471 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
472 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
473 double x = 0, y = 0;
474 ip.set_ij(i, j);
476 ip.set_ij(y, x);
477 vpRGBa Ipixelplan;
478 if (getPixel(Isrc, ip, Ipixelplan)) {
479 *(bitmap + i * width + j) = Ipixelplan;
480 nb_point_dessine++;
481 }
482 }
483 }
484 }
485}
486
503{
504 if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
506 " zBuffer must have the same size as the image I ! "));
507
508 if (cleanPrevImage) {
509 for (unsigned int i = 0; i < I.getHeight(); i++) {
510 for (unsigned int j = 0; j < I.getWidth(); j++) {
511 I[i][j] = bgColor;
512 }
513 }
514 }
515 if (visible) {
516 if (!needClipping)
517 getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
518 else
519 getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
520
521 double top = rect.getTop();
522 double bottom = rect.getBottom();
523 double left = rect.getLeft();
524 double right = rect.getRight();
525
526 vpRGBa *bitmap = I.bitmap;
527 unsigned int width = I.getWidth();
528 vpImagePoint ip;
529 int nb_point_dessine = 0;
530
531 for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
532 for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
533 double x = 0, y = 0;
534 ip.set_ij(i, j);
536 ip.set_ij(y, x);
537 if (colorI == GRAY_SCALED) {
538 unsigned char Ipixelplan;
539 if (getPixel(ip, Ipixelplan)) {
540 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
541 vpRGBa pixelcolor;
542 pixelcolor.R = Ipixelplan;
543 pixelcolor.G = Ipixelplan;
544 pixelcolor.B = Ipixelplan;
545 *(bitmap + i * width + j) = pixelcolor;
546 nb_point_dessine++;
547 zBuffer[i][j] = Xinter_optim[2];
548 }
549 }
550 } else if (colorI == COLORED) {
551 vpRGBa Ipixelplan;
552 if (getPixel(ip, Ipixelplan)) {
553 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
554 *(bitmap + i * width + j) = Ipixelplan;
555 nb_point_dessine++;
556 zBuffer[i][j] = Xinter_optim[2];
557 }
558 }
559 }
560 }
561 }
562 }
563}
564
669void vpImageSimulator::getImage(vpImage<unsigned char> &I, std::list<vpImageSimulator> &list,
670 const vpCameraParameters &cam)
671{
672
673 unsigned int width = I.getWidth();
674 unsigned int height = I.getHeight();
675
676 unsigned int nbsimList = (unsigned int)list.size();
677
678 if (nbsimList < 1)
679 return;
680
681 vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
682
683 double topFinal = height + 1;
684 ;
685 double bottomFinal = -1;
686 double leftFinal = width + 1;
687 double rightFinal = -1;
688
689 unsigned int unvisible = 0;
690 unsigned int indexSimu = 0;
691 for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
692 vpImageSimulator *sim = &(*it);
693 if (sim->visible)
694 simList[indexSimu] = sim;
695 else
696 unvisible++;
697 }
698 nbsimList = nbsimList - unvisible;
699
700 if (nbsimList < 1) {
701 delete[] simList;
702 return;
703 }
704
705 for (unsigned int i = 0; i < nbsimList; i++) {
706 if (!simList[i]->needClipping)
707 simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
708 else
709 simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
710
711 if (topFinal > simList[i]->rect.getTop())
712 topFinal = simList[i]->rect.getTop();
713 if (bottomFinal < simList[i]->rect.getBottom())
714 bottomFinal = simList[i]->rect.getBottom();
715 if (leftFinal > simList[i]->rect.getLeft())
716 leftFinal = simList[i]->rect.getLeft();
717 if (rightFinal < simList[i]->rect.getRight())
718 rightFinal = simList[i]->rect.getRight();
719 }
720
721 double zmin = -1;
722 int indice = -1;
723 unsigned char *bitmap = I.bitmap;
724 vpImagePoint ip;
725
726 for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++) {
727 for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++) {
728 zmin = -1;
729 double x = 0, y = 0;
730 ip.set_ij(i, j);
732 ip.set_ij(y, x);
733 for (int k = 0; k < (int)nbsimList; k++) {
734 double z = 0;
735 if (simList[k]->getPixelDepth(ip, z)) {
736 if (z < zmin || zmin < 0) {
737 zmin = z;
738 indice = k;
739 }
740 }
741 }
742 if (indice >= 0) {
743 if (simList[indice]->colorI == GRAY_SCALED) {
744 unsigned char Ipixelplan = 255;
745 simList[indice]->getPixel(ip, Ipixelplan);
746 *(bitmap + i * width + j) = Ipixelplan;
747 } else if (simList[indice]->colorI == COLORED) {
748 vpRGBa Ipixelplan(255, 255, 255);
749 simList[indice]->getPixel(ip, Ipixelplan);
750 unsigned char pixelgrey =
751 (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
752 *(bitmap + i * width + j) = pixelgrey;
753 }
754 }
755 }
756 }
757
758 delete[] simList;
759}
760
867void vpImageSimulator::getImage(vpImage<vpRGBa> &I, std::list<vpImageSimulator> &list, const vpCameraParameters &cam)
868{
869
870 unsigned int width = I.getWidth();
871 unsigned int height = I.getHeight();
872
873 unsigned int nbsimList = (unsigned int)list.size();
874
875 if (nbsimList < 1)
876 return;
877
878 vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
879
880 double topFinal = height + 1;
881 ;
882 double bottomFinal = -1;
883 double leftFinal = width + 1;
884 double rightFinal = -1;
885
886 unsigned int unvisible = 0;
887 unsigned int indexSimu = 0;
888 for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
889 vpImageSimulator *sim = &(*it);
890 if (sim->visible)
891 simList[indexSimu] = sim;
892 else
893 unvisible++;
894 }
895
896 nbsimList = nbsimList - unvisible;
897
898 if (nbsimList < 1) {
899 delete[] simList;
900 return;
901 }
902
903 for (unsigned int i = 0; i < nbsimList; i++) {
904 if (!simList[i]->needClipping)
905 simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
906 else
907 simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
908
909 if (topFinal > simList[i]->rect.getTop())
910 topFinal = simList[i]->rect.getTop();
911 if (bottomFinal < simList[i]->rect.getBottom())
912 bottomFinal = simList[i]->rect.getBottom();
913 if (leftFinal > simList[i]->rect.getLeft())
914 leftFinal = simList[i]->rect.getLeft();
915 if (rightFinal < simList[i]->rect.getRight())
916 rightFinal = simList[i]->rect.getRight();
917 }
918
919 double zmin = -1;
920 int indice = -1;
921 vpRGBa *bitmap = I.bitmap;
922 vpImagePoint ip;
923
924 for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++) {
925 for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++) {
926 zmin = -1;
927 double x = 0, y = 0;
928 ip.set_ij(i, j);
930 ip.set_ij(y, x);
931 for (int k = 0; k < (int)nbsimList; k++) {
932 double z = 0;
933 if (simList[k]->getPixelDepth(ip, z)) {
934 if (z < zmin || zmin < 0) {
935 zmin = z;
936 indice = k;
937 }
938 }
939 }
940 if (indice >= 0) {
941 if (simList[indice]->colorI == GRAY_SCALED) {
942 unsigned char Ipixelplan = 255;
943 simList[indice]->getPixel(ip, Ipixelplan);
944 vpRGBa pixelcolor;
945 pixelcolor.R = Ipixelplan;
946 pixelcolor.G = Ipixelplan;
947 pixelcolor.B = Ipixelplan;
948 *(bitmap + i * width + j) = pixelcolor;
949 } else if (simList[indice]->colorI == COLORED) {
950 vpRGBa Ipixelplan(255, 255, 255);
951 simList[indice]->getPixel(ip, Ipixelplan);
952 // unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 *
953 // Ipixelplan.G + 0.0722 * Ipixelplan.B;
954 *(bitmap + i * width + j) = Ipixelplan;
955 }
956 }
957 }
958 }
959
960 delete[] simList;
961}
962
969{
970 cMt = cMt_;
972 cMt.extract(R);
973 needClipping = false;
974
975 normal_Cam = R * normal_obj;
976
977 visible_result = vpColVector::dotProd(normal_Cam, focal);
978
979 for (unsigned int i = 0; i < 4; i++)
980 pt[i].track(cMt);
981
982 vpColVector e1(3);
983 vpColVector e2(3);
984 vpColVector facenormal(3);
985
986 e1[0] = pt[1].get_X() - pt[0].get_X();
987 e1[1] = pt[1].get_Y() - pt[0].get_Y();
988 e1[2] = pt[1].get_Z() - pt[0].get_Z();
989
990 e2[0] = pt[2].get_X() - pt[1].get_X();
991 e2[1] = pt[2].get_Y() - pt[1].get_Y();
992 e2[2] = pt[2].get_Z() - pt[1].get_Z();
993
994 facenormal = vpColVector::crossProd(e1, e2);
995
996 double angle = pt[0].get_X() * facenormal[0] + pt[0].get_Y() * facenormal[1] + pt[0].get_Z() * facenormal[2];
997
998 if (angle > 0) {
999 visible = true;
1000 } else {
1001 visible = false;
1002 }
1003
1004 if (visible) {
1005 for (unsigned int i = 0; i < 4; i++) {
1006 project(X[i], cMt, X2[i]);
1007 pt[i].track(cMt);
1008 if (pt[i].get_Z() < 0)
1009 needClipping = true;
1010 }
1011
1012 vbase_u = X2[1] - X2[0];
1013 vbase_v = X2[3] - X2[0];
1014
1015 distance = vpColVector::dotProd(normal_Cam, X2[1]);
1016
1017 if (distance < 0) {
1018 visible = false;
1019 return;
1020 }
1021
1022 for (unsigned int i = 0; i < 3; i++) {
1023 normal_Cam_optim[i] = normal_Cam[i];
1024 X0_2_optim[i] = X2[0][i];
1025 vbase_u_optim[i] = vbase_u[i];
1026 vbase_v_optim[i] = vbase_v[i];
1027 }
1028
1029 std::vector<vpPoint> *ptPtr = &pt;
1030 if (needClipping) {
1032 ptPtr = &ptClipped;
1033 }
1034
1035 listTriangle.clear();
1036 for (unsigned int i = 1; i < (*ptPtr).size() - 1; i++) {
1037 vpImagePoint ip1, ip2, ip3;
1038 ip1.set_j((*ptPtr)[0].get_x());
1039 ip1.set_i((*ptPtr)[0].get_y());
1040
1041 ip2.set_j((*ptPtr)[i].get_x());
1042 ip2.set_i((*ptPtr)[i].get_y());
1043
1044 ip3.set_j((*ptPtr)[i + 1].get_x());
1045 ip3.set_i((*ptPtr)[i + 1].get_y());
1046
1047 vpTriangle tri(ip1, ip2, ip3);
1048 listTriangle.push_back(tri);
1049 }
1050 }
1051}
1052
1053void vpImageSimulator::initPlan(vpColVector *X_)
1054{
1055 for (unsigned int i = 0; i < 4; i++) {
1056 X[i] = X_[i];
1057 pt[i].setWorldCoordinates(X_[i][0], X_[i][1], X_[i][2]);
1058 }
1059
1060 normal_obj = vpColVector::crossProd(X[1] - X[0], X[3] - X[0]);
1061 normal_obj = normal_obj / normal_obj.frobeniusNorm();
1062
1063 frobeniusNorm_u = (X[1] - X[0]).frobeniusNorm();
1064 fronbniusNorm_v = (X[3] - X[0]).frobeniusNorm();
1065}
1066
1082{
1083 Ig = I;
1085 initPlan(X_);
1086}
1087
1103{
1104 Ic = I;
1106 initPlan(X_);
1107}
1108
1109#ifdef VISP_HAVE_MODULE_IO
1125void vpImageSimulator::init(const char *file_image, vpColVector *X_)
1126{
1127 vpImageIo::read(Ig, file_image);
1128 vpImageIo::read(Ic, file_image);
1129 initPlan(X_);
1130}
1131#endif
1132
1148void vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoint> &X_)
1149{
1150 if (X_.size() != 4) {
1151 throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1152 }
1153 vpColVector Xvec[4];
1154 for (unsigned int i = 0; i < 4; ++i) {
1155 Xvec[i].resize(3);
1156 Xvec[i][0] = X_[i].get_oX();
1157 Xvec[i][1] = X_[i].get_oY();
1158 Xvec[i][2] = X_[i].get_oZ();
1159 }
1160
1161 Ig = I;
1163 initPlan(Xvec);
1164}
1180void vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint> &X_)
1181{
1182 if (X_.size() != 4) {
1183 throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1184 }
1185 vpColVector Xvec[4];
1186 for (unsigned int i = 0; i < 4; ++i) {
1187 Xvec[i].resize(3);
1188 Xvec[i][0] = X_[i].get_oX();
1189 Xvec[i][1] = X_[i].get_oY();
1190 Xvec[i][2] = X_[i].get_oZ();
1191 }
1192
1193 Ic = I;
1195 initPlan(Xvec);
1196}
1197
1198#ifdef VISP_HAVE_MODULE_IO
1215void vpImageSimulator::init(const char *file_image, const std::vector<vpPoint> &X_)
1216{
1217 if (X_.size() != 4) {
1218 throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1219 }
1220 vpColVector Xvec[4];
1221 for (unsigned int i = 0; i < 4; ++i) {
1222 Xvec[i].resize(3);
1223 Xvec[i][0] = X_[i].get_oX();
1224 Xvec[i][1] = X_[i].get_oY();
1225 Xvec[i][2] = X_[i].get_oZ();
1226 }
1227
1228 vpImageIo::read(Ig, file_image);
1229 vpImageIo::read(Ic, file_image);
1230 initPlan(Xvec);
1231}
1232#endif
1233
1234bool vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan)
1235{
1236 // std::cout << "In get Pixel" << std::endl;
1237 // test si pixel dans zone projetee
1238 bool inside = false;
1239 for (unsigned int i = 0; i < listTriangle.size(); i++)
1240 if (listTriangle[i].inTriangle(iP)) {
1241 inside = true;
1242 break;
1243 }
1244 if (!inside)
1245 return false;
1246
1247 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
1250 // return false;}
1251
1252 // methoed algebrique
1253 double z;
1254
1255 // calcul de la profondeur de l'intersection
1256 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1257 // calcul coordonnees 3D intersection
1258 Xinter_optim[0] = iP.get_u() * z;
1259 Xinter_optim[1] = iP.get_v() * z;
1260 Xinter_optim[2] = z;
1261
1262 // recuperation des coordonnes de l'intersection dans le plan objet
1263 // repere plan object :
1264 // centre = X0_2_optim[i] (premier point definissant le plan)
1265 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1266 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1267 // simplement obtenu par un produit scalaire
1268 double u = 0, v = 0;
1269 for (unsigned int i = 0; i < 3; i++) {
1270 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1271 u += diff * vbase_u_optim[i];
1272 v += diff * vbase_v_optim[i];
1273 }
1274 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1275 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1276
1277 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1278 double i2, j2;
1279 i2 = v * (Ig.getHeight() - 1);
1280 j2 = u * (Ig.getWidth() - 1);
1281 if (interp == BILINEAR_INTERPOLATION)
1282 Ipixelplan = Ig.getValue(i2, j2);
1283 else if (interp == SIMPLE)
1284 Ipixelplan = Ig[(unsigned int)i2][(unsigned int)j2];
1285 return true;
1286 } else
1287 return false;
1288}
1289
1290bool vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP, unsigned char &Ipixelplan)
1291{
1292 // test si pixel dans zone projetee
1293 bool inside = false;
1294 for (unsigned int i = 0; i < listTriangle.size(); i++)
1295 if (listTriangle[i].inTriangle(iP)) {
1296 inside = true;
1297 break;
1298 }
1299 if (!inside)
1300 return false;
1301
1302 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1303 // return false;
1304
1305 // methoed algebrique
1306 double z;
1307
1308 // calcul de la profondeur de l'intersection
1309 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1310 // calcul coordonnees 3D intersection
1311 Xinter_optim[0] = iP.get_u() * z;
1312 Xinter_optim[1] = iP.get_v() * z;
1313 Xinter_optim[2] = z;
1314
1315 // recuperation des coordonnes de l'intersection dans le plan objet
1316 // repere plan object :
1317 // centre = X0_2_optim[i] (premier point definissant le plan)
1318 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1319 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1320 // simplement obtenu par un produit scalaire
1321 double u = 0, v = 0;
1322 for (unsigned int i = 0; i < 3; i++) {
1323 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1324 u += diff * vbase_u_optim[i];
1325 v += diff * vbase_v_optim[i];
1326 }
1327 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1328 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1329
1330 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1331 double i2, j2;
1332 i2 = v * (Isrc.getHeight() - 1);
1333 j2 = u * (Isrc.getWidth() - 1);
1334 if (interp == BILINEAR_INTERPOLATION)
1335 Ipixelplan = Isrc.getValue(i2, j2);
1336 else if (interp == SIMPLE)
1337 Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
1338 return true;
1339 } else
1340 return false;
1341}
1342
1343bool vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan)
1344{
1345 // test si pixel dans zone projetee
1346 bool inside = false;
1347 for (unsigned int i = 0; i < listTriangle.size(); i++)
1348 if (listTriangle[i].inTriangle(iP)) {
1349 inside = true;
1350 break;
1351 }
1352 if (!inside)
1353 return false;
1354 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1355 // return false;
1356
1357 // methoed algebrique
1358 double z;
1359
1360 // calcul de la profondeur de l'intersection
1361 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1362 // calcul coordonnees 3D intersection
1363 Xinter_optim[0] = iP.get_u() * z;
1364 Xinter_optim[1] = iP.get_v() * z;
1365 Xinter_optim[2] = z;
1366
1367 // recuperation des coordonnes de l'intersection dans le plan objet
1368 // repere plan object :
1369 // centre = X0_2_optim[i] (premier point definissant le plan)
1370 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1371 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1372 // simplement obtenu par un produit scalaire
1373 double u = 0, v = 0;
1374 for (unsigned int i = 0; i < 3; i++) {
1375 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1376 u += diff * vbase_u_optim[i];
1377 v += diff * vbase_v_optim[i];
1378 }
1379 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1380 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1381
1382 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1383 double i2, j2;
1384 i2 = v * (Ic.getHeight() - 1);
1385 j2 = u * (Ic.getWidth() - 1);
1386 if (interp == BILINEAR_INTERPOLATION)
1387 Ipixelplan = Ic.getValue(i2, j2);
1388 else if (interp == SIMPLE)
1389 Ipixelplan = Ic[(unsigned int)i2][(unsigned int)j2];
1390 return true;
1391 } else
1392 return false;
1393}
1394
1395bool vpImageSimulator::getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP, vpRGBa &Ipixelplan)
1396{
1397 // test si pixel dans zone projetee
1398 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1399 // return false;
1400 bool inside = false;
1401 for (unsigned int i = 0; i < listTriangle.size(); i++)
1402 if (listTriangle[i].inTriangle(iP)) {
1403 inside = true;
1404 break;
1405 }
1406 if (!inside)
1407 return false;
1408
1409 // methoed algebrique
1410 double z;
1411
1412 // calcul de la profondeur de l'intersection
1413 z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1414 // calcul coordonnees 3D intersection
1415 Xinter_optim[0] = iP.get_u() * z;
1416 Xinter_optim[1] = iP.get_v() * z;
1417 Xinter_optim[2] = z;
1418
1419 // recuperation des coordonnes de l'intersection dans le plan objet
1420 // repere plan object :
1421 // centre = X0_2_optim[i] (premier point definissant le plan)
1422 // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1423 // ici j'ai considere que le plan est un rectangle => coordonnees sont
1424 // simplement obtenu par un produit scalaire
1425 double u = 0, v = 0;
1426 for (unsigned int i = 0; i < 3; i++) {
1427 double diff = (Xinter_optim[i] - X0_2_optim[i]);
1428 u += diff * vbase_u_optim[i];
1429 v += diff * vbase_v_optim[i];
1430 }
1431 u = u / (frobeniusNorm_u * frobeniusNorm_u);
1432 v = v / (fronbniusNorm_v * fronbniusNorm_v);
1433
1434 if (u > 0 && v > 0 && u < 1. && v < 1.) {
1435 double i2, j2;
1436 i2 = v * (Isrc.getHeight() - 1);
1437 j2 = u * (Isrc.getWidth() - 1);
1438 if (interp == BILINEAR_INTERPOLATION)
1439 Ipixelplan = Isrc.getValue(i2, j2);
1440 else if (interp == SIMPLE)
1441 Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
1442 return true;
1443 } else
1444 return false;
1445}
1446
1447bool vpImageSimulator::getPixelDepth(const vpImagePoint &iP, double &Zpixelplan)
1448{
1449 // test si pixel dans zone projetee
1450 bool inside = false;
1451 for (unsigned int i = 0; i < listTriangle.size(); i++)
1452 if (listTriangle[i].inTriangle(iP)) {
1453 inside = true;
1454 break;
1455 }
1456 if (!inside)
1457 return false;
1458 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1459 // return false;
1460
1461 Zpixelplan = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1462 return true;
1463}
1464
1465bool vpImageSimulator::getPixelVisibility(const vpImagePoint &iP, double &Visipixelplan)
1466{
1467 // test si pixel dans zone projetee
1468 bool inside = false;
1469 for (unsigned int i = 0; i < listTriangle.size(); i++)
1470 if (listTriangle[i].inTriangle(iP)) {
1471 inside = true;
1472 break;
1473 }
1474 if (!inside)
1475 return false;
1476 // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1477 // return false;
1478
1479 Visipixelplan = visible_result;
1480 return true;
1481}
1482
1483void vpImageSimulator::project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt, vpColVector &_vout)
1484{
1485 vpColVector XH(4);
1486 getHomogCoord(_vin, XH);
1487 getCoordFromHomog(_cMt * XH, _vout);
1488}
1489
1490void vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH)
1491{
1492 for (unsigned int i = 0; i < 3; i++)
1493 _vH[i] = _v[i];
1494 _vH[3] = 1.;
1495}
1496
1497void vpImageSimulator::getCoordFromHomog(const vpColVector &_vH, vpColVector &_v)
1498{
1499 for (unsigned int i = 0; i < 3; i++)
1500 _v[i] = _vH[i] / _vH[3];
1501}
1502
1503void vpImageSimulator::getRoi(const unsigned int &Iwidth, const unsigned int &Iheight, const vpCameraParameters &cam,
1504 const std::vector<vpPoint> &point, vpRect &rectangle)
1505{
1506 double top = Iheight + 1;
1507 double bottom = -1;
1508 double right = -1;
1509 double left = Iwidth + 1;
1510
1511 for (unsigned int i = 0; i < point.size(); i++) {
1512 double u = 0, v = 0;
1513 vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), u, v);
1514 if (v < top)
1515 top = v;
1516 if (v > bottom)
1517 bottom = v;
1518 if (u < left)
1519 left = u;
1520 if (u > right)
1521 right = u;
1522 }
1523 if (top < 0)
1524 top = 0;
1525 if (top >= Iheight)
1526 top = Iheight - 1;
1527 if (bottom < 0)
1528 bottom = 0;
1529 if (bottom >= Iheight)
1530 bottom = Iheight - 1;
1531 if (left < 0)
1532 left = 0;
1533 if (left >= Iwidth)
1534 left = Iwidth - 1;
1535 if (right < 0)
1536 right = 0;
1537 if (right >= Iwidth)
1538 right = Iwidth - 1;
1539
1540 rectangle.setTop(top);
1541 rectangle.setBottom(bottom);
1542 rectangle.setLeft(left);
1543 rectangle.setRight(right);
1544}
1545
1547{
1548 std::vector<vpColVector> X_;
1549 for (int i = 0; i < 4; i++)
1550 X_.push_back(X[i]);
1551 return X_;
1552}
1553
1554VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/)
1555{
1556 os << "";
1557 return os;
1558}
unsigned int getCols() const
Definition: vpArray2D.h:279
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int getRows() const
Definition: vpArray2D.h:289
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
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)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
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)
Definition: vpImageIo.cpp:149
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_j(double jj)
Definition: vpImagePoint.h:177
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_u() const
Definition: vpImagePoint.h:262
double get_v() const
Definition: vpImagePoint.h:273
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)
unsigned int getWidth() const
Definition: vpImage.h:246
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
unsigned int getHeight() const
Definition: vpImage.h:188
error that can be emited by the vpMatrix class and its derivates
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
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)
Definition: vpRGBa.h:67
unsigned char B
Blue component.
Definition: vpRGBa.h:150
unsigned char R
Red component.
Definition: vpRGBa.h:148
unsigned char G
Green component.
Definition: vpRGBa.h:149
Defines a rectangle in the plane.
Definition: vpRect.h:80
void setTop(double pos)
Definition: vpRect.h:358
double getLeft() const
Definition: vpRect.h:174
void setLeft(double pos)
Definition: vpRect.h:322
void setRight(double pos)
Definition: vpRect.h:349
double getRight() const
Definition: vpRect.h:180
double getBottom() const
Definition: vpRect.h:98
void setBottom(double pos)
Definition: vpRect.h:289
double getTop() const
Definition: vpRect.h:193
Implementation of a rotation matrix and operations on such kind of matrices.
Defines a 2D triangle.
Definition: vpTriangle.h:59