Visual Servoing Platform  version 3.4.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 
669 void 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 
867 void 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 
1053 void 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;
1084  vpImageConvert::convert(I, Ic);
1085  initPlan(X_);
1086 }
1087 
1103 {
1104  Ic = I;
1105  vpImageConvert::convert(I, Ig);
1106  initPlan(X_);
1107 }
1108 
1109 #ifdef VISP_HAVE_MODULE_IO
1110 
1125 void 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 
1148 void 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;
1162  vpImageConvert::convert(I, Ic);
1163  initPlan(Xvec);
1164 }
1180 void 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;
1194  vpImageConvert::convert(I, Ig);
1195  initPlan(Xvec);
1196 }
1197 
1198 #ifdef VISP_HAVE_MODULE_IO
1199 
1215 void 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 
1234 bool 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 
1290 bool 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 
1343 bool 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 
1395 bool 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 
1447 bool 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 
1465 bool 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 
1483 void 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 
1490 void 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 
1497 void 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 
1503 void 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 
1554 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/)
1555 {
1556  os << "";
1557  return os;
1558 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
double frobeniusNorm() const
double getTop() const
Definition: vpRect.h:193
double get_v() const
Definition: vpImagePoint.h:273
void init(const vpImage< unsigned char > &I, vpColVector *X)
unsigned int getWidth() const
Definition: vpImage.h:246
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned char B
Blue component.
Definition: vpRGBa.h:150
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpImageSimulator &)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void setLeft(double pos)
Definition: vpRect.h:322
double get_u() const
Definition: vpImagePoint.h:262
Defines a 2D triangle.
Definition: vpTriangle.h:58
error that can be emited by ViSP classes.
Definition: vpException.h:71
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned char G
Green component.
Definition: vpRGBa.h:149
double getRight() const
Definition: vpRect.h:180
Definition: vpRGBa.h:66
Implementation of a rotation matrix and operations on such kind of matrices.
double getBottom() const
Definition: vpRect.h:98
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
void setTop(double pos)
Definition: vpRect.h:358
void setCameraPosition(const vpHomogeneousMatrix &cMt)
void set_i(double ii)
Definition: vpImagePoint.h:166
vpImageSimulator & operator=(const vpImageSimulator &sim)
Generic class defining intrinsic camera parameters.
Class which enables to project an image in the 3D space and get the view of a virtual camera...
void extract(vpRotationMatrix &R) const
unsigned int getRows() const
Definition: vpArray2D.h:289
vpImageSimulator(const vpColorPlan &col=COLORED)
void set_j(double jj)
Definition: vpImagePoint.h:177
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:244
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static double dotProd(const vpColVector &a, const vpColVector &b)
void setRight(double pos)
Definition: vpRect.h:349
std::vector< vpColVector > get3DcornersTextureRectangle()
unsigned char R
Red component.
Definition: vpRGBa.h:148
error that can be emited by the vpMatrix class and its derivates
unsigned int getHeight() const
Definition: vpImage.h:188
Defines a rectangle in the plane.
Definition: vpRect.h:79
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
double getLeft() const
Definition: vpRect.h:174
void setBottom(double pos)
Definition: vpRect.h:289