Visual Servoing Platform  version 3.6.1 under development (2024-05-26)
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(nullptr), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
61  vbase_v(), vbase_u_optim(nullptr), vbase_v_optim(nullptr), Xinter_optim(nullptr), 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(nullptr), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
98  vbase_v(), vbase_u_optim(nullptr), vbase_v_optim(nullptr), Xinter_optim(nullptr), 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 
649 void 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 
847 void 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 
1033 void 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;
1064  vpImageConvert::convert(I, Ic);
1065  initPlan(X_);
1066 }
1067 
1083 {
1084  Ic = I;
1085  vpImageConvert::convert(I, Ig);
1086  initPlan(X_);
1087 }
1088 
1089 #ifdef VISP_HAVE_MODULE_IO
1105 void 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 
1128 void 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;
1142  vpImageConvert::convert(I, Ic);
1143  initPlan(Xvec);
1144 }
1160 void 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;
1174  vpImageConvert::convert(I, Ig);
1175  initPlan(Xvec);
1176 }
1177 
1178 #ifdef VISP_HAVE_MODULE_IO
1195 void 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 
1214 bool 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 
1270 bool 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 
1323 bool 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 
1375 bool 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 
1427 bool 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 
1445 bool 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 
1463 void 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 
1470 void 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 
1477 void 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 
1483 void 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 
1534 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/)
1535 {
1536  os << "";
1537  return os;
1538 }
unsigned int getCols() const
Definition: vpArray2D.h:329
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:603
unsigned int getRows() const
Definition: vpArray2D.h:339
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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:1058
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:70
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:143
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void set_j(double jj)
Definition: vpImagePoint.h:305
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:316
void set_i(double ii)
Definition: vpImagePoint.h:294
double get_u() const
Definition: vpImagePoint.h:136
double get_v() const
Definition: vpImagePoint.h:147
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:245
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1863
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:146
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:61
unsigned char B
Blue component.
Definition: vpRGBa.h:139
unsigned char R
Red component.
Definition: vpRGBa.h:137
unsigned char G
Green component.
Definition: vpRGBa.h:138
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:54