Visual Servoing Platform  version 3.6.1 under development (2024-12-06)
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 
48 BEGIN_VISP_NAMESPACE
60  : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(), distance(1.),
61  visible_result(1.), visible(false), X0_2_optim(nullptr), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
62  vbase_v(), vbase_u_optim(nullptr), vbase_v_optim(nullptr), Xinter_optim(nullptr), listTriangle(), colorI(col), Ig(), Ic(),
63  rect(), cleanPrevImage(false), setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
64 {
65  for (int i = 0; i < 4; i++)
66  X[i].resize(3);
67 
68  for (int i = 0; i < 4; i++)
69  X2[i].resize(3);
70 
71  normal_obj.resize(3);
72  visible = false;
73  normal_Cam.resize(3);
74 
75  // Xinter.resize(3);
76 
77  vbase_u.resize(3);
78  vbase_v.resize(3);
79 
80  focal.resize(3);
81  focal = 0;
82  focal[2] = 1;
83 
84  normal_Cam_optim = new double[3];
85  X0_2_optim = new double[3];
86  vbase_u_optim = new double[3];
87  vbase_v_optim = new double[3];
88  Xinter_optim = new double[3];
89 
90  pt.resize(4);
91 }
92 
97  : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(), distance(1.),
98  visible_result(1.), visible(false), X0_2_optim(nullptr), frobeniusNorm_u(0.), fronbniusNorm_v(0.), vbase_u(),
99  vbase_v(), vbase_u_optim(nullptr), vbase_v_optim(nullptr), Xinter_optim(nullptr), listTriangle(), colorI(GRAY_SCALED), Ig(),
100  Ic(), rect(), cleanPrevImage(false), setBackgroundTexture(false), bgColor(vpColor::white), focal(),
101  needClipping(false)
102 {
103  pt.resize(4);
104  for (unsigned int i = 0; i < 4; i++) {
105  X[i] = text.X[i];
106  pt[i] = text.pt[i];
107  }
108 
109  for (int i = 0; i < 4; i++)
110  X2[i].resize(3);
111 
112  Ic = text.Ic;
113  Ig = text.Ig;
114 
115  focal.resize(3);
116  focal = 0;
117  focal[2] = 1;
118 
119  normal_obj = text.normal_obj;
120  frobeniusNorm_u = text.frobeniusNorm_u;
121  fronbniusNorm_v = text.fronbniusNorm_v;
122 
123  normal_Cam.resize(3);
124  vbase_u.resize(3);
125  vbase_v.resize(3);
126 
127  normal_Cam_optim = new double[3];
128  X0_2_optim = new double[3];
129  vbase_u_optim = new double[3];
130  vbase_v_optim = new double[3];
131  Xinter_optim = new double[3];
132 
133  colorI = text.colorI;
134  interp = text.interp;
135  bgColor = text.bgColor;
136  cleanPrevImage = text.cleanPrevImage;
137  setBackgroundTexture = false;
138 
139  setCameraPosition(text.cMt);
140 }
141 
146 {
147  delete[] normal_Cam_optim;
148  delete[] X0_2_optim;
149  delete[] vbase_u_optim;
150  delete[] vbase_v_optim;
151  delete[] Xinter_optim;
152 }
153 
155 {
156  for (unsigned int i = 0; i < 4; i++) {
157  X[i] = sim.X[i];
158  pt[i] = sim.pt[i];
159  }
160 
161  Ic = sim.Ic;
162  Ig = sim.Ig;
163 
164  bgColor = sim.bgColor;
165  cleanPrevImage = sim.cleanPrevImage;
166 
167  focal = sim.focal;
168 
169  normal_obj = sim.normal_obj;
170  frobeniusNorm_u = sim.frobeniusNorm_u;
171  fronbniusNorm_v = sim.fronbniusNorm_v;
172 
173  colorI = sim.colorI;
174  interp = sim.interp;
175 
176  setCameraPosition(sim.cMt);
177 
178  return *this;
179 }
180 
187 {
188  if (setBackgroundTexture)
189  // The Ig has been set to a previously defined background texture
190  I = Ig;
191  else {
192  if (cleanPrevImage) {
193  unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
194  for (unsigned int i = 0; i < I.getHeight(); i++) {
195  for (unsigned int j = 0; j < I.getWidth(); j++) {
196  I[i][j] = col;
197  }
198  }
199  }
200  }
201 
202  if (visible) {
203  if (!needClipping)
204  getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
205  else
206  getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
207 
208  double top = rect.getTop();
209  double bottom = rect.getBottom();
210  double left = rect.getLeft();
211  double right = rect.getRight();
212 
213  unsigned char *bitmap = I.bitmap;
214  unsigned int width = I.getWidth();
215  vpImagePoint ip;
216 
217  for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
218  for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
219  double x = 0, y = 0;
220  ip.set_ij(i, j);
222  ip.set_ij(y, x);
223  if (colorI == GRAY_SCALED) {
224  unsigned char Ipixelplan = 0;
225  if (getPixel(ip, Ipixelplan)) {
226  *(bitmap + i * width + j) = Ipixelplan;
227  }
228  }
229  else if (colorI == COLORED) {
230  vpRGBa Ipixelplan;
231  if (getPixel(ip, Ipixelplan)) {
232  unsigned char pixelgrey =
233  (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
234  *(bitmap + i * width + j) = pixelgrey;
235  }
236  }
237  }
238  }
239  }
240 }
241 
252 {
253  if (cleanPrevImage) {
254  unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
255  for (unsigned int i = 0; i < I.getHeight(); i++) {
256  for (unsigned int j = 0; j < I.getWidth(); j++) {
257  I[i][j] = col;
258  }
259  }
260  }
261  if (visible) {
262  if (!needClipping)
263  getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
264  else
265  getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
266 
267  double top = rect.getTop();
268  double bottom = rect.getBottom();
269  double left = rect.getLeft();
270  double right = rect.getRight();
271 
272  unsigned char *bitmap = I.bitmap;
273  unsigned int width = I.getWidth();
274  vpImagePoint ip;
275 
276  for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
277  for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
278  double x = 0, y = 0;
279  ip.set_ij(i, j);
281  ip.set_ij(y, x);
282  unsigned char Ipixelplan = 0;
283  if (getPixel(Isrc, ip, Ipixelplan)) {
284  *(bitmap + i * width + j) = Ipixelplan;
285  }
286  }
287  }
288  }
289 }
290 
307 {
308  if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
310  " zBuffer must have the same size as the image I ! "));
311 
312  if (cleanPrevImage) {
313  unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
314  for (unsigned int i = 0; i < I.getHeight(); i++) {
315  for (unsigned int j = 0; j < I.getWidth(); j++) {
316  I[i][j] = col;
317  }
318  }
319  }
320  if (visible) {
321  if (!needClipping)
322  getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
323  else
324  getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
325 
326  double top = rect.getTop();
327  double bottom = rect.getBottom();
328  double left = rect.getLeft();
329  double right = rect.getRight();
330 
331  unsigned char *bitmap = I.bitmap;
332  unsigned int width = I.getWidth();
333  vpImagePoint ip;
334 
335  for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
336  for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
337  double x = 0, y = 0;
338  ip.set_ij(i, j);
340  ip.set_ij(y, x);
341  if (colorI == GRAY_SCALED) {
342  unsigned char Ipixelplan;
343  if (getPixel(ip, Ipixelplan)) {
344  if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
345  *(bitmap + i * width + j) = Ipixelplan;
346  zBuffer[i][j] = Xinter_optim[2];
347  }
348  }
349  }
350  else if (colorI == COLORED) {
351  vpRGBa Ipixelplan;
352  if (getPixel(ip, Ipixelplan)) {
353  if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
354  unsigned char pixelgrey =
355  (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
356  *(bitmap + i * width + j) = pixelgrey;
357  zBuffer[i][j] = Xinter_optim[2];
358  }
359  }
360  }
361  }
362  }
363  }
364 }
365 
374 {
375  if (cleanPrevImage) {
376  for (unsigned int i = 0; i < I.getHeight(); i++) {
377  for (unsigned int j = 0; j < I.getWidth(); j++) {
378  I[i][j] = bgColor;
379  }
380  }
381  }
382 
383  if (visible) {
384  if (!needClipping)
385  getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
386  else
387  getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
388 
389  double top = rect.getTop();
390  double bottom = rect.getBottom();
391  double left = rect.getLeft();
392  double right = rect.getRight();
393 
394  vpRGBa *bitmap = I.bitmap;
395  unsigned int width = I.getWidth();
396  vpImagePoint ip;
397 
398  for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
399  for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
400  double x = 0, y = 0;
401  ip.set_ij(i, j);
403  ip.set_ij(y, x);
404  if (colorI == GRAY_SCALED) {
405  unsigned char Ipixelplan;
406  if (getPixel(ip, Ipixelplan)) {
407  vpRGBa pixelcolor;
408  pixelcolor.R = Ipixelplan;
409  pixelcolor.G = Ipixelplan;
410  pixelcolor.B = Ipixelplan;
411  *(bitmap + i * width + j) = pixelcolor;
412  }
413  }
414  else if (colorI == COLORED) {
415  vpRGBa Ipixelplan;
416  if (getPixel(ip, Ipixelplan)) {
417  *(bitmap + i * width + j) = Ipixelplan;
418  }
419  }
420  }
421  }
422  }
423 }
424 
435 {
436  if (cleanPrevImage) {
437  for (unsigned int i = 0; i < I.getHeight(); i++) {
438  for (unsigned int j = 0; j < I.getWidth(); j++) {
439  I[i][j] = bgColor;
440  }
441  }
442  }
443 
444  if (visible) {
445  if (!needClipping)
446  getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
447  else
448  getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
449 
450  double top = rect.getTop();
451  double bottom = rect.getBottom();
452  double left = rect.getLeft();
453  double right = rect.getRight();
454 
455  vpRGBa *bitmap = I.bitmap;
456  unsigned int width = I.getWidth();
457  vpImagePoint ip;
458 
459  for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
460  for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
461  double x = 0, y = 0;
462  ip.set_ij(i, j);
464  ip.set_ij(y, x);
465  vpRGBa Ipixelplan;
466  if (getPixel(Isrc, ip, Ipixelplan)) {
467  *(bitmap + i * width + j) = Ipixelplan;
468  }
469  }
470  }
471  }
472 }
473 
490 {
491  if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
493  " zBuffer must have the same size as the image I ! "));
494 
495  if (cleanPrevImage) {
496  for (unsigned int i = 0; i < I.getHeight(); i++) {
497  for (unsigned int j = 0; j < I.getWidth(); j++) {
498  I[i][j] = bgColor;
499  }
500  }
501  }
502  if (visible) {
503  if (!needClipping)
504  getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
505  else
506  getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
507 
508  double top = rect.getTop();
509  double bottom = rect.getBottom();
510  double left = rect.getLeft();
511  double right = rect.getRight();
512 
513  vpRGBa *bitmap = I.bitmap;
514  unsigned int width = I.getWidth();
515  vpImagePoint ip;
516 
517  for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
518  for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
519  double x = 0, y = 0;
520  ip.set_ij(i, j);
522  ip.set_ij(y, x);
523  if (colorI == GRAY_SCALED) {
524  unsigned char Ipixelplan;
525  if (getPixel(ip, Ipixelplan)) {
526  if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
527  vpRGBa pixelcolor;
528  pixelcolor.R = Ipixelplan;
529  pixelcolor.G = Ipixelplan;
530  pixelcolor.B = Ipixelplan;
531  *(bitmap + i * width + j) = pixelcolor;
532  zBuffer[i][j] = Xinter_optim[2];
533  }
534  }
535  }
536  else if (colorI == COLORED) {
537  vpRGBa Ipixelplan;
538  if (getPixel(ip, Ipixelplan)) {
539  if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
540  *(bitmap + i * width + j) = Ipixelplan;
541  zBuffer[i][j] = Xinter_optim[2];
542  }
543  }
544  }
545  }
546  }
547  }
548 }
549 
657 void vpImageSimulator::getImage(vpImage<unsigned char> &I, std::list<vpImageSimulator> &list,
658  const vpCameraParameters &cam)
659 {
660 
661  unsigned int width = I.getWidth();
662  unsigned int height = I.getHeight();
663 
664  unsigned int nbsimList = (unsigned int)list.size();
665 
666  if (nbsimList < 1)
667  return;
668 
669  vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
670 
671  double topFinal = height + 1;
672  ;
673  double bottomFinal = -1;
674  double leftFinal = width + 1;
675  double rightFinal = -1;
676 
677  unsigned int unvisible = 0;
678  unsigned int indexSimu = 0;
679  for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
680  vpImageSimulator *sim = &(*it);
681  if (sim->visible)
682  simList[indexSimu] = sim;
683  else
684  unvisible++;
685  }
686  nbsimList = nbsimList - unvisible;
687 
688  if (nbsimList < 1) {
689  delete[] simList;
690  return;
691  }
692 
693  for (unsigned int i = 0; i < nbsimList; i++) {
694  if (!simList[i]->needClipping)
695  simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
696  else
697  simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
698 
699  if (topFinal > simList[i]->rect.getTop())
700  topFinal = simList[i]->rect.getTop();
701  if (bottomFinal < simList[i]->rect.getBottom())
702  bottomFinal = simList[i]->rect.getBottom();
703  if (leftFinal > simList[i]->rect.getLeft())
704  leftFinal = simList[i]->rect.getLeft();
705  if (rightFinal < simList[i]->rect.getRight())
706  rightFinal = simList[i]->rect.getRight();
707  }
708 
709  double zmin = -1;
710  int indice = -1;
711  unsigned char *bitmap = I.bitmap;
712  vpImagePoint ip;
713 
714  for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++) {
715  for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++) {
716  zmin = -1;
717  double x = 0, y = 0;
718  ip.set_ij(i, j);
720  ip.set_ij(y, x);
721  for (int k = 0; k < (int)nbsimList; k++) {
722  double z = 0;
723  if (simList[k]->getPixelDepth(ip, z)) {
724  if (z < zmin || zmin < 0) {
725  zmin = z;
726  indice = k;
727  }
728  }
729  }
730  if (indice >= 0) {
731  if (simList[indice]->colorI == GRAY_SCALED) {
732  unsigned char Ipixelplan = 255;
733  simList[indice]->getPixel(ip, Ipixelplan);
734  *(bitmap + i * width + j) = Ipixelplan;
735  }
736  else if (simList[indice]->colorI == COLORED) {
737  vpRGBa Ipixelplan(255, 255, 255);
738  simList[indice]->getPixel(ip, Ipixelplan);
739  unsigned char pixelgrey =
740  (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
741  *(bitmap + i * width + j) = pixelgrey;
742  }
743  }
744  }
745  }
746 
747  delete[] simList;
748 }
749 
859 void vpImageSimulator::getImage(vpImage<vpRGBa> &I, std::list<vpImageSimulator> &list, const vpCameraParameters &cam)
860 {
861 
862  unsigned int width = I.getWidth();
863  unsigned int height = I.getHeight();
864 
865  unsigned int nbsimList = (unsigned int)list.size();
866 
867  if (nbsimList < 1)
868  return;
869 
870  vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
871 
872  double topFinal = height + 1;
873  ;
874  double bottomFinal = -1;
875  double leftFinal = width + 1;
876  double rightFinal = -1;
877 
878  unsigned int unvisible = 0;
879  unsigned int indexSimu = 0;
880  for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
881  vpImageSimulator *sim = &(*it);
882  if (sim->visible)
883  simList[indexSimu] = sim;
884  else
885  unvisible++;
886  }
887 
888  nbsimList = nbsimList - unvisible;
889 
890  if (nbsimList < 1) {
891  delete[] simList;
892  return;
893  }
894 
895  for (unsigned int i = 0; i < nbsimList; i++) {
896  if (!simList[i]->needClipping)
897  simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
898  else
899  simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
900 
901  if (topFinal > simList[i]->rect.getTop())
902  topFinal = simList[i]->rect.getTop();
903  if (bottomFinal < simList[i]->rect.getBottom())
904  bottomFinal = simList[i]->rect.getBottom();
905  if (leftFinal > simList[i]->rect.getLeft())
906  leftFinal = simList[i]->rect.getLeft();
907  if (rightFinal < simList[i]->rect.getRight())
908  rightFinal = simList[i]->rect.getRight();
909  }
910 
911  double zmin = -1;
912  int indice = -1;
913  vpRGBa *bitmap = I.bitmap;
914  vpImagePoint ip;
915 
916  for (unsigned int i = (unsigned int)topFinal; i < (unsigned int)bottomFinal; i++) {
917  for (unsigned int j = (unsigned int)leftFinal; j < (unsigned int)rightFinal; j++) {
918  zmin = -1;
919  double x = 0, y = 0;
920  ip.set_ij(i, j);
922  ip.set_ij(y, x);
923  for (int k = 0; k < (int)nbsimList; k++) {
924  double z = 0;
925  if (simList[k]->getPixelDepth(ip, z)) {
926  if (z < zmin || zmin < 0) {
927  zmin = z;
928  indice = k;
929  }
930  }
931  }
932  if (indice >= 0) {
933  if (simList[indice]->colorI == GRAY_SCALED) {
934  unsigned char Ipixelplan = 255;
935  simList[indice]->getPixel(ip, Ipixelplan);
936  vpRGBa pixelcolor;
937  pixelcolor.R = Ipixelplan;
938  pixelcolor.G = Ipixelplan;
939  pixelcolor.B = Ipixelplan;
940  *(bitmap + i * width + j) = pixelcolor;
941  }
942  else if (simList[indice]->colorI == COLORED) {
943  vpRGBa Ipixelplan(255, 255, 255);
944  simList[indice]->getPixel(ip, Ipixelplan);
945  // unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 *
946  // Ipixelplan.G + 0.0722 * Ipixelplan.B;
947  *(bitmap + i * width + j) = Ipixelplan;
948  }
949  }
950  }
951  }
952 
953  delete[] simList;
954 }
955 
962 {
963  cMt = cMt_;
965  cMt.extract(R);
966  needClipping = false;
967 
968  normal_Cam = R * normal_obj;
969 
970  visible_result = vpColVector::dotProd(normal_Cam, focal);
971 
972  for (unsigned int i = 0; i < 4; i++)
973  pt[i].track(cMt);
974 
975  vpColVector e1(3);
976  vpColVector e2(3);
977  vpColVector facenormal(3);
978 
979  e1[0] = pt[1].get_X() - pt[0].get_X();
980  e1[1] = pt[1].get_Y() - pt[0].get_Y();
981  e1[2] = pt[1].get_Z() - pt[0].get_Z();
982 
983  e2[0] = pt[2].get_X() - pt[1].get_X();
984  e2[1] = pt[2].get_Y() - pt[1].get_Y();
985  e2[2] = pt[2].get_Z() - pt[1].get_Z();
986 
987  facenormal = vpColVector::crossProd(e1, e2);
988 
989  double angle = pt[0].get_X() * facenormal[0] + pt[0].get_Y() * facenormal[1] + pt[0].get_Z() * facenormal[2];
990 
991  if (angle > 0) {
992  visible = true;
993  }
994  else {
995  visible = false;
996  }
997 
998  if (visible) {
999  for (unsigned int i = 0; i < 4; i++) {
1000  project(X[i], cMt, X2[i]);
1001  pt[i].track(cMt);
1002  if (pt[i].get_Z() < 0)
1003  needClipping = true;
1004  }
1005 
1006  vbase_u = X2[1] - X2[0];
1007  vbase_v = X2[3] - X2[0];
1008 
1009  distance = vpColVector::dotProd(normal_Cam, X2[1]);
1010 
1011  if (distance < 0) {
1012  visible = false;
1013  return;
1014  }
1015 
1016  for (unsigned int i = 0; i < 3; i++) {
1017  normal_Cam_optim[i] = normal_Cam[i];
1018  X0_2_optim[i] = X2[0][i];
1019  vbase_u_optim[i] = vbase_u[i];
1020  vbase_v_optim[i] = vbase_v[i];
1021  }
1022 
1023  std::vector<vpPoint> *ptPtr = &pt;
1024  if (needClipping) {
1026  ptPtr = &ptClipped;
1027  }
1028 
1029  listTriangle.clear();
1030  for (unsigned int i = 1; i < (*ptPtr).size() - 1; i++) {
1031  vpImagePoint ip1, ip2, ip3;
1032  ip1.set_j((*ptPtr)[0].get_x());
1033  ip1.set_i((*ptPtr)[0].get_y());
1034 
1035  ip2.set_j((*ptPtr)[i].get_x());
1036  ip2.set_i((*ptPtr)[i].get_y());
1037 
1038  ip3.set_j((*ptPtr)[i + 1].get_x());
1039  ip3.set_i((*ptPtr)[i + 1].get_y());
1040 
1041  vpTriangle tri(ip1, ip2, ip3);
1042  listTriangle.push_back(tri);
1043  }
1044  }
1045 }
1046 
1047 void vpImageSimulator::initPlan(vpColVector *X_)
1048 {
1049  for (unsigned int i = 0; i < 4; i++) {
1050  X[i] = X_[i];
1051  pt[i].setWorldCoordinates(X_[i][0], X_[i][1], X_[i][2]);
1052  }
1053 
1054  normal_obj = vpColVector::crossProd(X[1] - X[0], X[3] - X[0]);
1055  normal_obj = normal_obj / normal_obj.frobeniusNorm();
1056 
1057  frobeniusNorm_u = (X[1] - X[0]).frobeniusNorm();
1058  fronbniusNorm_v = (X[3] - X[0]).frobeniusNorm();
1059 }
1060 
1076 {
1077  Ig = I;
1078  vpImageConvert::convert(I, Ic);
1079  initPlan(X_);
1080 }
1081 
1097 {
1098  Ic = I;
1099  vpImageConvert::convert(I, Ig);
1100  initPlan(X_);
1101 }
1102 
1103 #ifdef VISP_HAVE_MODULE_IO
1119 void vpImageSimulator::init(const char *file_image, vpColVector *X_)
1120 {
1121  vpImageIo::read(Ig, file_image);
1122  vpImageIo::read(Ic, file_image);
1123  initPlan(X_);
1124 }
1125 #endif
1126 
1142 void vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoint> &X_)
1143 {
1144  if (X_.size() != 4) {
1145  throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1146  }
1147  vpColVector Xvec[4];
1148  for (unsigned int i = 0; i < 4; ++i) {
1149  Xvec[i].resize(3);
1150  Xvec[i][0] = X_[i].get_oX();
1151  Xvec[i][1] = X_[i].get_oY();
1152  Xvec[i][2] = X_[i].get_oZ();
1153  }
1154 
1155  Ig = I;
1156  vpImageConvert::convert(I, Ic);
1157  initPlan(Xvec);
1158 }
1174 void vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint> &X_)
1175 {
1176  if (X_.size() != 4) {
1177  throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1178  }
1179  vpColVector Xvec[4];
1180  for (unsigned int i = 0; i < 4; ++i) {
1181  Xvec[i].resize(3);
1182  Xvec[i][0] = X_[i].get_oX();
1183  Xvec[i][1] = X_[i].get_oY();
1184  Xvec[i][2] = X_[i].get_oZ();
1185  }
1186 
1187  Ic = I;
1188  vpImageConvert::convert(I, Ig);
1189  initPlan(Xvec);
1190 }
1191 
1192 #ifdef VISP_HAVE_MODULE_IO
1209 void vpImageSimulator::init(const char *file_image, const std::vector<vpPoint> &X_)
1210 {
1211  if (X_.size() != 4) {
1212  throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
1213  }
1214  vpColVector Xvec[4];
1215  for (unsigned int i = 0; i < 4; ++i) {
1216  Xvec[i].resize(3);
1217  Xvec[i][0] = X_[i].get_oX();
1218  Xvec[i][1] = X_[i].get_oY();
1219  Xvec[i][2] = X_[i].get_oZ();
1220  }
1221 
1222  vpImageIo::read(Ig, file_image);
1223  vpImageIo::read(Ic, file_image);
1224  initPlan(Xvec);
1225 }
1226 #endif
1227 
1228 bool vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan)
1229 {
1230  // std::cout << "In get Pixel" << std::endl;
1231  // test si pixel dans zone projetee
1232  bool inside = false;
1233  for (unsigned int i = 0; i < listTriangle.size(); i++)
1234  if (listTriangle[i].inTriangle(iP)) {
1235  inside = true;
1236  break;
1237  }
1238  if (!inside)
1239  return false;
1240 
1241  // if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
1244  // return false;}
1245 
1246  // methoed algebrique
1247  double z;
1248 
1249  // calcul de la profondeur de l'intersection
1250  z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1251  // calcul coordonnees 3D intersection
1252  Xinter_optim[0] = iP.get_u() * z;
1253  Xinter_optim[1] = iP.get_v() * z;
1254  Xinter_optim[2] = z;
1255 
1256  // recuperation des coordonnes de l'intersection dans le plan objet
1257  // repere plan object :
1258  // centre = X0_2_optim[i] (premier point definissant le plan)
1259  // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1260  // ici j'ai considere que le plan est un rectangle => coordonnees sont
1261  // simplement obtenu par un produit scalaire
1262  double u = 0, v = 0;
1263  for (unsigned int i = 0; i < 3; i++) {
1264  double diff = (Xinter_optim[i] - X0_2_optim[i]);
1265  u += diff * vbase_u_optim[i];
1266  v += diff * vbase_v_optim[i];
1267  }
1268  u = u / (frobeniusNorm_u * frobeniusNorm_u);
1269  v = v / (fronbniusNorm_v * fronbniusNorm_v);
1270 
1271  if (u > 0 && v > 0 && u < 1. && v < 1.) {
1272  double i2, j2;
1273  i2 = v * (Ig.getHeight() - 1);
1274  j2 = u * (Ig.getWidth() - 1);
1275  if (interp == BILINEAR_INTERPOLATION)
1276  Ipixelplan = Ig.getValue(i2, j2);
1277  else if (interp == SIMPLE)
1278  Ipixelplan = Ig[(unsigned int)i2][(unsigned int)j2];
1279  return true;
1280  }
1281  else
1282  return false;
1283 }
1284 
1285 bool vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP, unsigned char &Ipixelplan)
1286 {
1287  // test si pixel dans zone projetee
1288  bool inside = false;
1289  for (unsigned int i = 0; i < listTriangle.size(); i++)
1290  if (listTriangle[i].inTriangle(iP)) {
1291  inside = true;
1292  break;
1293  }
1294  if (!inside)
1295  return false;
1296 
1297  // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1298  // return false;
1299 
1300  // methoed algebrique
1301  double z;
1302 
1303  // calcul de la profondeur de l'intersection
1304  z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1305  // calcul coordonnees 3D intersection
1306  Xinter_optim[0] = iP.get_u() * z;
1307  Xinter_optim[1] = iP.get_v() * z;
1308  Xinter_optim[2] = z;
1309 
1310  // recuperation des coordonnes de l'intersection dans le plan objet
1311  // repere plan object :
1312  // centre = X0_2_optim[i] (premier point definissant le plan)
1313  // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1314  // ici j'ai considere que le plan est un rectangle => coordonnees sont
1315  // simplement obtenu par un produit scalaire
1316  double u = 0, v = 0;
1317  for (unsigned int i = 0; i < 3; i++) {
1318  double diff = (Xinter_optim[i] - X0_2_optim[i]);
1319  u += diff * vbase_u_optim[i];
1320  v += diff * vbase_v_optim[i];
1321  }
1322  u = u / (frobeniusNorm_u * frobeniusNorm_u);
1323  v = v / (fronbniusNorm_v * fronbniusNorm_v);
1324 
1325  if (u > 0 && v > 0 && u < 1. && v < 1.) {
1326  double i2, j2;
1327  i2 = v * (Isrc.getHeight() - 1);
1328  j2 = u * (Isrc.getWidth() - 1);
1329  if (interp == BILINEAR_INTERPOLATION)
1330  Ipixelplan = Isrc.getValue(i2, j2);
1331  else if (interp == SIMPLE)
1332  Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
1333  return true;
1334  }
1335  else
1336  return false;
1337 }
1338 
1339 bool vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan)
1340 {
1341  // test si pixel dans zone projetee
1342  bool inside = false;
1343  for (unsigned int i = 0; i < listTriangle.size(); i++)
1344  if (listTriangle[i].inTriangle(iP)) {
1345  inside = true;
1346  break;
1347  }
1348  if (!inside)
1349  return false;
1350  // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1351  // return false;
1352 
1353  // methoed algebrique
1354  double z;
1355 
1356  // calcul de la profondeur de l'intersection
1357  z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1358  // calcul coordonnees 3D intersection
1359  Xinter_optim[0] = iP.get_u() * z;
1360  Xinter_optim[1] = iP.get_v() * z;
1361  Xinter_optim[2] = z;
1362 
1363  // recuperation des coordonnes de l'intersection dans le plan objet
1364  // repere plan object :
1365  // centre = X0_2_optim[i] (premier point definissant le plan)
1366  // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1367  // ici j'ai considere que le plan est un rectangle => coordonnees sont
1368  // simplement obtenu par un produit scalaire
1369  double u = 0, v = 0;
1370  for (unsigned int i = 0; i < 3; i++) {
1371  double diff = (Xinter_optim[i] - X0_2_optim[i]);
1372  u += diff * vbase_u_optim[i];
1373  v += diff * vbase_v_optim[i];
1374  }
1375  u = u / (frobeniusNorm_u * frobeniusNorm_u);
1376  v = v / (fronbniusNorm_v * fronbniusNorm_v);
1377 
1378  if (u > 0 && v > 0 && u < 1. && v < 1.) {
1379  double i2, j2;
1380  i2 = v * (Ic.getHeight() - 1);
1381  j2 = u * (Ic.getWidth() - 1);
1382  if (interp == BILINEAR_INTERPOLATION)
1383  Ipixelplan = Ic.getValue(i2, j2);
1384  else if (interp == SIMPLE)
1385  Ipixelplan = Ic[(unsigned int)i2][(unsigned int)j2];
1386  return true;
1387  }
1388  else
1389  return false;
1390 }
1391 
1392 bool vpImageSimulator::getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP, vpRGBa &Ipixelplan)
1393 {
1394  // test si pixel dans zone projetee
1395  // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1396  // return false;
1397  bool inside = false;
1398  for (unsigned int i = 0; i < listTriangle.size(); i++)
1399  if (listTriangle[i].inTriangle(iP)) {
1400  inside = true;
1401  break;
1402  }
1403  if (!inside)
1404  return false;
1405 
1406  // methoed algebrique
1407  double z;
1408 
1409  // calcul de la profondeur de l'intersection
1410  z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1411  // calcul coordonnees 3D intersection
1412  Xinter_optim[0] = iP.get_u() * z;
1413  Xinter_optim[1] = iP.get_v() * z;
1414  Xinter_optim[2] = z;
1415 
1416  // recuperation des coordonnes de l'intersection dans le plan objet
1417  // repere plan object :
1418  // centre = X0_2_optim[i] (premier point definissant le plan)
1419  // base = u:(X[1]-X[0]) et v:(X[3]-X[0])
1420  // ici j'ai considere que le plan est un rectangle => coordonnees sont
1421  // simplement obtenu par un produit scalaire
1422  double u = 0, v = 0;
1423  for (unsigned int i = 0; i < 3; i++) {
1424  double diff = (Xinter_optim[i] - X0_2_optim[i]);
1425  u += diff * vbase_u_optim[i];
1426  v += diff * vbase_v_optim[i];
1427  }
1428  u = u / (frobeniusNorm_u * frobeniusNorm_u);
1429  v = v / (fronbniusNorm_v * fronbniusNorm_v);
1430 
1431  if (u > 0 && v > 0 && u < 1. && v < 1.) {
1432  double i2, j2;
1433  i2 = v * (Isrc.getHeight() - 1);
1434  j2 = u * (Isrc.getWidth() - 1);
1435  if (interp == BILINEAR_INTERPOLATION)
1436  Ipixelplan = Isrc.getValue(i2, j2);
1437  else if (interp == SIMPLE)
1438  Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
1439  return true;
1440  }
1441  else
1442  return false;
1443 }
1444 
1445 bool vpImageSimulator::getPixelDepth(const vpImagePoint &iP, double &Zpixelplan)
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  Zpixelplan = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
1460  return true;
1461 }
1462 
1463 bool vpImageSimulator::getPixelVisibility(const vpImagePoint &iP, double &Visipixelplan)
1464 {
1465  // test si pixel dans zone projetee
1466  bool inside = false;
1467  for (unsigned int i = 0; i < listTriangle.size(); i++)
1468  if (listTriangle[i].inTriangle(iP)) {
1469  inside = true;
1470  break;
1471  }
1472  if (!inside)
1473  return false;
1474  // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
1475  // return false;
1476 
1477  Visipixelplan = visible_result;
1478  return true;
1479 }
1480 
1481 void vpImageSimulator::project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt, vpColVector &_vout)
1482 {
1483  vpColVector XH(4);
1484  getHomogCoord(_vin, XH);
1485  getCoordFromHomog(_cMt * XH, _vout);
1486 }
1487 
1488 void vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH)
1489 {
1490  for (unsigned int i = 0; i < 3; i++)
1491  _vH[i] = _v[i];
1492  _vH[3] = 1.;
1493 }
1494 
1495 void vpImageSimulator::getCoordFromHomog(const vpColVector &_vH, vpColVector &_v)
1496 {
1497  for (unsigned int i = 0; i < 3; i++)
1498  _v[i] = _vH[i] / _vH[3];
1499 }
1500 
1501 void vpImageSimulator::getRoi(const unsigned int &Iwidth, const unsigned int &Iheight, const vpCameraParameters &cam,
1502  const std::vector<vpPoint> &point, vpRect &rectangle)
1503 {
1504  double top = Iheight + 1;
1505  double bottom = -1;
1506  double right = -1;
1507  double left = Iwidth + 1;
1508 
1509  for (unsigned int i = 0; i < point.size(); i++) {
1510  double u = 0, v = 0;
1511  vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), u, v);
1512  if (v < top)
1513  top = v;
1514  if (v > bottom)
1515  bottom = v;
1516  if (u < left)
1517  left = u;
1518  if (u > right)
1519  right = u;
1520  }
1521  if (top < 0)
1522  top = 0;
1523  if (top >= Iheight)
1524  top = Iheight - 1;
1525  if (bottom < 0)
1526  bottom = 0;
1527  if (bottom >= Iheight)
1528  bottom = Iheight - 1;
1529  if (left < 0)
1530  left = 0;
1531  if (left >= Iwidth)
1532  left = Iwidth - 1;
1533  if (right < 0)
1534  right = 0;
1535  if (right >= Iwidth)
1536  right = Iwidth - 1;
1537 
1538  rectangle.setTop(top);
1539  rectangle.setBottom(bottom);
1540  rectangle.setLeft(left);
1541  rectangle.setRight(right);
1542 }
1543 
1545 {
1546  std::vector<vpColVector> X_;
1547  for (int i = 0; i < 4; i++)
1548  X_.push_back(X[i]);
1549  return X_;
1550 }
1551 
1552 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/)
1553 {
1554  os << "";
1555  return os;
1556 }
1557 END_VISP_NAMESPACE
unsigned int getCols() const
Definition: vpArray2D.h:337
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:614
unsigned int getRows() const
Definition: vpArray2D.h:347
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
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:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
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:147
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:309
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:320
void set_i(double ii)
Definition: vpImagePoint.h:298
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)
VP_EXPLICIT 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:242
Type getValue(unsigned int i, unsigned int j) const
Type * bitmap
points toward the bitmap
Definition: vpImage.h:135
unsigned int getHeight() const
Definition: vpImage.h:181
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:169
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:65
unsigned char B
Blue component.
Definition: vpRGBa.h:169
unsigned char R
Red component.
Definition: vpRGBa.h:167
unsigned char G
Green component.
Definition: vpRGBa.h:168
Defines a rectangle in the plane.
Definition: vpRect.h:79
void setTop(double pos)
Definition: vpRect.h:357
double getLeft() const
Definition: vpRect.h:173
void setLeft(double pos)
Definition: vpRect.h:321
void setRight(double pos)
Definition: vpRect.h:348
double getRight() const
Definition: vpRect.h:179
double getBottom() const
Definition: vpRect.h:97
void setBottom(double pos)
Definition: vpRect.h:288
double getTop() const
Definition: vpRect.h:192
Implementation of a rotation matrix and operations on such kind of matrices.
Defines a 2D triangle.
Definition: vpTriangle.h:54