44 #include <visp/vpImageSimulator.h>
45 #include <visp/vpRotationMatrix.h>
46 #include <visp/vpImageIo.h>
47 #include <visp/vpImageConvert.h>
48 #include <visp/vpPixelMeterConversion.h>
49 #include <visp/vpMeterPixelConversion.h>
50 #include <visp/vpMatrixException.h>
82 normal_Cam_optim =
new double[3];
83 X0_2_optim =
new double[3];
84 vbase_u_optim =
new double[3];
85 vbase_v_optim =
new double[3];
86 Xinter_optim =
new double[3];
91 cleanPrevImage =
false;
92 setBackgroundTexture =
false;
117 normal_obj = text.normal_obj;
118 euclideanNorm_u = text.euclideanNorm_u;
119 euclideanNorm_v = text.euclideanNorm_v;
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];
132 colorI = text.colorI;
133 interp = text.interp;
134 bgColor = text.bgColor;
135 cleanPrevImage = text.cleanPrevImage;
136 setBackgroundTexture =
false;
146 delete[] normal_Cam_optim;
148 delete[] vbase_u_optim;
149 delete[] vbase_v_optim;
150 delete[] Xinter_optim;
166 bgColor = sim.bgColor;
167 cleanPrevImage = sim.cleanPrevImage;
171 normal_obj = sim.normal_obj;
172 euclideanNorm_u = sim.euclideanNorm_u;
173 euclideanNorm_v = sim.euclideanNorm_v;
192 int nb_point_dessine = 0;
193 if (setBackgroundTexture)
200 unsigned char col = (
unsigned char) (0.2126 * bgColor.
R
201 + 0.7152 * bgColor.
G + 0.0722 * bgColor.
B);
202 for (
unsigned int i = 0; i < I.
getHeight(); i++)
204 for (
unsigned int j = 0; j < I.
getWidth(); j++)
216 double top = rect.
getTop();
221 unsigned char *bitmap = I.
bitmap;
225 for (
unsigned int i = (
unsigned int)top; i < (
unsigned int)bottom; i++)
227 for (
unsigned int j = (
unsigned int)left; j < (
unsigned int)right; j++)
235 unsigned char Ipixelplan = 0;
236 if(getPixel(ip,Ipixelplan))
238 *(bitmap+i*width+j)=Ipixelplan;
245 if(getPixel(ip,Ipixelplan))
247 unsigned char pixelgrey = (
unsigned char)(0.2126 * Ipixelplan.
R + 0.7152 * Ipixelplan.
G + 0.0722 * Ipixelplan.
B);
248 *(bitmap+i*width+j)=pixelgrey;
270 int nb_point_dessine = 0;
273 unsigned char col = (
unsigned char)(0.2126 * bgColor.
R + 0.7152 * bgColor.
G + 0.0722 * bgColor.
B);
274 for (
unsigned int i = 0; i < I.
getHeight(); i++)
276 for (
unsigned int j = 0; j < I.
getWidth(); j++)
286 double top = rect.
getTop();
291 unsigned char *bitmap = I.
bitmap;
295 for (
unsigned int i = (
unsigned int)top; i < (
unsigned int)bottom; i++)
297 for (
unsigned int j = (
unsigned int)left; j < (
unsigned int)right; j++)
303 unsigned char Ipixelplan = 0;
304 if(getPixel(Isrc,ip,Ipixelplan))
306 *(bitmap+i*width+j)=Ipixelplan;
330 int nb_point_dessine = 0;
333 unsigned char col = (
unsigned char)(0.2126 * bgColor.
R + 0.7152 * bgColor.
G + 0.0722 * bgColor.
B);
334 for (
unsigned int i = 0; i < I.
getHeight(); i++)
336 for (
unsigned int j = 0; j < I.
getWidth(); j++)
346 double top = rect.
getTop();
351 unsigned char *bitmap = I.
bitmap;
355 for (
unsigned int i = (
unsigned int)top; i < (
unsigned int)bottom; i++)
357 for (
unsigned int j = (
unsigned int)left; j < (
unsigned int)right; j++)
365 unsigned char Ipixelplan;
366 if(getPixel(ip,Ipixelplan))
368 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
370 *(bitmap+i*width+j)=Ipixelplan;
372 zBuffer[i][j] = Xinter_optim[2];
379 if(getPixel(ip,Ipixelplan))
381 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
383 unsigned char pixelgrey = (
unsigned char)(0.2126 * Ipixelplan.
R + 0.7152 * Ipixelplan.
G + 0.0722 * Ipixelplan.
B);
384 *(bitmap+i*width+j)=pixelgrey;
386 zBuffer[i][j] = Xinter_optim[2];
404 int nb_point_dessine = 0;
407 for (
unsigned int i = 0; i < I.
getHeight(); i++)
409 for (
unsigned int j = 0; j < I.
getWidth(); j++)
420 double top = rect.
getTop();
429 for (
unsigned int i = (
unsigned int)top; i < (
unsigned int)bottom; i++)
431 for (
unsigned int j = (
unsigned int)left; j < (
unsigned int)right; j++)
439 unsigned char Ipixelplan;
440 if(getPixel(ip,Ipixelplan))
443 pixelcolor.
R = Ipixelplan;
444 pixelcolor.
G = Ipixelplan;
445 pixelcolor.
B = Ipixelplan;
446 *(bitmap+i*width+j) = pixelcolor;
453 if(getPixel(ip,Ipixelplan))
455 *(bitmap+i*width+j) = Ipixelplan;
476 int nb_point_dessine = 0;
479 for (
unsigned int i = 0; i < I.
getHeight(); i++)
481 for (
unsigned int j = 0; j < I.
getWidth(); j++)
492 double top = rect.
getTop();
501 for (
unsigned int i = (
unsigned int)top; i < (
unsigned int)bottom; i++)
503 for (
unsigned int j = (
unsigned int)left; j < (
unsigned int)right; j++)
510 if(getPixel(Isrc,ip,Ipixelplan))
512 *(bitmap+i*width+j) = Ipixelplan;
536 int nb_point_dessine = 0;
539 for (
unsigned int i = 0; i < I.
getHeight(); i++)
541 for (
unsigned int j = 0; j < I.
getWidth(); j++)
551 double top = rect.
getTop();
560 for (
unsigned int i = (
unsigned int)top; i < (
unsigned int)bottom; i++)
562 for (
unsigned int j = (
unsigned int)left; j < (
unsigned int)right; j++)
570 unsigned char Ipixelplan;
571 if(getPixel(ip,Ipixelplan))
573 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
576 pixelcolor.
R = Ipixelplan;
577 pixelcolor.
G = Ipixelplan;
578 pixelcolor.
B = Ipixelplan;
579 *(bitmap+i*width+j) = pixelcolor;
581 zBuffer[i][j] = Xinter_optim[2];
588 if(getPixel(ip,Ipixelplan))
590 if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
592 *(bitmap+i*width+j) = Ipixelplan;
594 zBuffer[i][j] = Xinter_optim[2];
603 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
726 double topFinal = height+1;;
727 double bottomFinal = -1;
728 double leftFinal = width+1;
729 double rightFinal = -1;
733 unsigned int unvisible = 0;
734 for (
unsigned int i = 0; i < nbsimList; i++)
743 nbsimList = nbsimList - unvisible;
752 for (
unsigned int i = 0; i < nbsimList; i++)
755 simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
757 if (topFinal > simList[i]->rect.
getTop()) topFinal = simList[i]->rect.
getTop();
758 if (bottomFinal < simList[i]->rect.
getBottom()) bottomFinal = simList[i]->rect.
getBottom();
759 if (leftFinal > simList[i]->rect.
getLeft()) leftFinal = simList[i]->rect.
getLeft();
760 if (rightFinal < simList[i]->rect.
getRight()) rightFinal = simList[i]->rect.
getRight();
765 unsigned char *bitmap = I.
bitmap;
768 for (
unsigned int i = (
unsigned int)topFinal; i < (
unsigned int)bottomFinal; i++)
770 for (
unsigned int j = (
unsigned int)leftFinal; j < (
unsigned int)rightFinal; j++)
777 for (
int k = 0; k < (int)nbsimList; k++)
780 if(simList[k]->getPixelDepth(ip,z))
782 if (z < zmin || zmin < 0)
793 unsigned char Ipixelplan = 255;
794 simList[indice]->getPixel(ip,Ipixelplan);
795 *(bitmap+i*width+j)=Ipixelplan;
797 else if (simList[indice]->colorI ==
COLORED)
799 vpRGBa Ipixelplan(255,255,255);
800 simList[indice]->getPixel(ip,Ipixelplan);
801 unsigned char pixelgrey = (
unsigned char)(0.2126 * Ipixelplan.
R + 0.7152 * Ipixelplan.
G + 0.0722 * Ipixelplan.
B);
802 *(bitmap+i*width+j)=pixelgrey;
932 double topFinal = height+1;;
933 double bottomFinal = -1;
934 double leftFinal = width+1;
935 double rightFinal = -1;
939 unsigned int unvisible = 0;
940 for (
unsigned int i = 0; i < nbsimList; i++)
949 nbsimList = nbsimList - unvisible;
957 for (
unsigned int i = 0; i < nbsimList; i++)
960 simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
962 if (topFinal > simList[i]->rect.
getTop()) topFinal = simList[i]->rect.
getTop();
963 if (bottomFinal < simList[i]->rect.
getBottom()) bottomFinal = simList[i]->rect.
getBottom();
964 if (leftFinal > simList[i]->rect.
getLeft()) leftFinal = simList[i]->rect.
getLeft();
965 if (rightFinal < simList[i]->rect.
getRight()) rightFinal = simList[i]->rect.
getRight();
973 for (
unsigned int i = (
unsigned int)topFinal; i < (
unsigned int)bottomFinal; i++)
975 for (
unsigned int j = (
unsigned int)leftFinal; j < (
unsigned int)rightFinal; j++)
982 for (
int k = 0; k < (int)nbsimList; k++)
985 if(simList[k]->getPixelDepth(ip,z))
987 if (z < zmin || zmin < 0)
998 unsigned char Ipixelplan = 255;
999 simList[indice]->getPixel(ip,Ipixelplan);
1001 pixelcolor.
R = Ipixelplan;
1002 pixelcolor.
G = Ipixelplan;
1003 pixelcolor.
B = Ipixelplan;
1004 *(bitmap+i*width+j) = pixelcolor;
1006 else if (simList[indice]->colorI ==
COLORED)
1008 vpRGBa Ipixelplan(255,255,255);
1009 simList[indice]->getPixel(ip,Ipixelplan);
1011 *(bitmap+i*width+j)=Ipixelplan;
1123 std::list<vpImageSimulator> &list,
1130 unsigned int nbsimList = (
unsigned int)list.size();
1137 double topFinal = height+1;;
1138 double bottomFinal = -1;
1139 double leftFinal = width+1;
1140 double rightFinal = -1;
1142 unsigned int unvisible = 0;
1143 unsigned int indexSimu=0;
1144 for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
1147 simList[indexSimu] = sim;
1151 nbsimList = nbsimList - unvisible;
1160 for (
unsigned int i = 0; i < nbsimList; i++)
1163 simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
1165 if (topFinal > simList[i]->rect.
getTop()) topFinal = simList[i]->rect.
getTop();
1166 if (bottomFinal < simList[i]->rect.
getBottom()) bottomFinal = simList[i]->rect.
getBottom();
1167 if (leftFinal > simList[i]->rect.
getLeft()) leftFinal = simList[i]->rect.
getLeft();
1168 if (rightFinal < simList[i]->rect.
getRight()) rightFinal = simList[i]->rect.
getRight();
1173 unsigned char *bitmap = I.
bitmap;
1176 for (
unsigned int i = (
unsigned int)topFinal; i < (
unsigned int)bottomFinal; i++)
1178 for (
unsigned int j = (
unsigned int)leftFinal; j < (
unsigned int)rightFinal; j++)
1185 for (
int k = 0; k < (int)nbsimList; k++)
1188 if(simList[k]->getPixelDepth(ip,z))
1190 if (z < zmin || zmin < 0)
1201 unsigned char Ipixelplan = 255;
1202 simList[indice]->getPixel(ip,Ipixelplan);
1203 *(bitmap+i*width+j)=Ipixelplan;
1205 else if (simList[indice]->colorI ==
COLORED)
1207 vpRGBa Ipixelplan(255,255,255);
1208 simList[indice]->getPixel(ip,Ipixelplan);
1209 unsigned char pixelgrey = (
unsigned char)(0.2126 * Ipixelplan.
R + 0.7152 * Ipixelplan.
G + 0.0722 * Ipixelplan.
B);
1210 *(bitmap+i*width+j)=pixelgrey;
1323 std::list<vpImageSimulator> &list,
1330 unsigned int nbsimList = (
unsigned int)list.size();
1337 double topFinal = height+1;;
1338 double bottomFinal = -1;
1339 double leftFinal = width+1;
1340 double rightFinal = -1;
1342 unsigned int unvisible = 0;
1343 unsigned int indexSimu = 0;
1344 for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
1347 simList[indexSimu] = sim;
1352 nbsimList = nbsimList - unvisible;
1360 for (
unsigned int i = 0; i < nbsimList; i++)
1363 simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
1365 if (topFinal > simList[i]->rect.
getTop()) topFinal = simList[i]->rect.
getTop();
1366 if (bottomFinal < simList[i]->rect.
getBottom()) bottomFinal = simList[i]->rect.
getBottom();
1367 if (leftFinal > simList[i]->rect.
getLeft()) leftFinal = simList[i]->rect.
getLeft();
1368 if (rightFinal < simList[i]->rect.
getRight()) rightFinal = simList[i]->rect.
getRight();
1376 for (
unsigned int i = (
unsigned int)topFinal; i < (
unsigned int)bottomFinal; i++)
1378 for (
unsigned int j = (
unsigned int)leftFinal; j < (
unsigned int)rightFinal; j++)
1385 for (
int k = 0; k < (int)nbsimList; k++)
1388 if(simList[k]->getPixelDepth(ip,z))
1390 if (z < zmin || zmin < 0)
1401 unsigned char Ipixelplan = 255;
1402 simList[indice]->getPixel(ip,Ipixelplan);
1404 pixelcolor.
R = Ipixelplan;
1405 pixelcolor.
G = Ipixelplan;
1406 pixelcolor.
B = Ipixelplan;
1407 *(bitmap+i*width+j) = pixelcolor;
1409 else if (simList[indice]->colorI ==
COLORED)
1411 vpRGBa Ipixelplan(255,255,255);
1412 simList[indice]->getPixel(ip,Ipixelplan);
1414 *(bitmap+i*width+j)=Ipixelplan;
1435 normal_Cam = R * normal_obj;
1439 for(
int i = 0; i < 4; i++)
1456 double angle = pt[0].
get_X()*facenormal[0] + pt[0].
get_Y()*facenormal[1] + pt[0].
get_Z()*facenormal[2] ;
1465 for(
int i = 0; i < 4; i++)
1467 project(X[i],cMt,X2[i]);
1471 vbase_u = X2[1]-X2[0];
1472 vbase_v = X2[3]-X2[0];
1482 for(
unsigned int i = 0; i < 3; i++)
1484 normal_Cam_optim[i] = normal_Cam[i];
1485 X0_2_optim[i] = X2[0][i];
1486 vbase_u_optim[i] = vbase_u[i];
1487 vbase_v_optim[i] = vbase_v[i];
1491 for(
unsigned int i = 0; i < 4; i++)
1493 iPa[i].
set_j(X2[i][0]/X2[i][2]);
1494 iPa[i].
set_i(X2[i][1]/X2[i][2]);
1505 for (
unsigned int i = 0; i < 4; i++)
1514 euclideanNorm_u=(X[1]-X[0]).euclideanNorm();
1515 euclideanNorm_v=(X[3]-X[0]).euclideanNorm();
1602 for(
unsigned int i=0; i<4; ++i){
1604 Xvec[i][0] = _X[i].get_oX();
1605 Xvec[i][1] = _X[i].get_oY();
1606 Xvec[i][2] = _X[i].get_oZ();
1634 for(
unsigned int i=0; i<4; ++i){
1636 Xvec[i][0] = _X[i].get_oX();
1637 Xvec[i][1] = _X[i].get_oY();
1638 Xvec[i][2] = _X[i].get_oZ();
1666 for(
unsigned int i=0; i<4; ++i){
1668 Xvec[i][0] = _X[i].get_oX();
1669 Xvec[i][1] = _X[i].get_oY();
1670 Xvec[i][2] = _X[i].get_oZ();
1679 vpImageSimulator::getPixel(
const vpImagePoint &iP,
unsigned char &Ipixelplan)
1689 z = distance/(normal_Cam_optim[0]*iP.
get_u()+normal_Cam_optim[1]*iP.
get_v()+normal_Cam_optim[2]);
1691 Xinter_optim[0]=iP.
get_u()*z;
1692 Xinter_optim[1]=iP.
get_v()*z;
1700 double u = 0, v = 0;
1702 for(
unsigned int i = 0; i < 3; i++)
1704 diff = (Xinter_optim[i]-X0_2_optim[i]);
1705 u += diff*vbase_u_optim[i];
1706 v += diff*vbase_v_optim[i];
1708 u = u/(euclideanNorm_u*euclideanNorm_u);
1709 v = v/(euclideanNorm_v*euclideanNorm_v);
1711 if( u > 0 && v > 0 && u < 1. && v < 1.)
1718 else if (interp ==
SIMPLE)
1719 Ipixelplan = Ig[(
unsigned int)i2][(
unsigned int)j2];
1738 z = distance/(normal_Cam_optim[0]*iP.
get_u()+normal_Cam_optim[1]*iP.
get_v()+normal_Cam_optim[2]);
1740 Xinter_optim[0]=iP.
get_u()*z;
1741 Xinter_optim[1]=iP.
get_v()*z;
1749 double u = 0, v = 0;
1751 for(
unsigned int i = 0; i < 3; i++)
1753 diff = (Xinter_optim[i]-X0_2_optim[i]);
1754 u += diff*vbase_u_optim[i];
1755 v += diff*vbase_v_optim[i];
1757 u = u/(euclideanNorm_u*euclideanNorm_u);
1758 v = v/(euclideanNorm_v*euclideanNorm_v);
1760 if( u > 0 && v > 0 && u < 1. && v < 1.)
1767 else if (interp ==
SIMPLE)
1768 Ipixelplan = Isrc[(
unsigned int)i2][(
unsigned int)j2];
1787 z = distance/(normal_Cam_optim[0]*iP.
get_u()+normal_Cam_optim[1]*iP.
get_v()+normal_Cam_optim[2]);
1789 Xinter_optim[0]=iP.
get_u()*z;
1790 Xinter_optim[1]=iP.
get_v()*z;
1798 double u = 0, v = 0;
1800 for(
unsigned int i = 0; i < 3; i++)
1802 diff = (Xinter_optim[i]-X0_2_optim[i]);
1803 u += diff*vbase_u_optim[i];
1804 v += diff*vbase_v_optim[i];
1806 u = u/(euclideanNorm_u*euclideanNorm_u);
1807 v = v/(euclideanNorm_v*euclideanNorm_v);
1809 if( u > 0 && v > 0 && u < 1. && v < 1.)
1816 else if (interp ==
SIMPLE)
1817 Ipixelplan = Ic[(
unsigned int)i2][(
unsigned int)j2];
1836 z = distance/(normal_Cam_optim[0]*iP.
get_u()+normal_Cam_optim[1]*iP.
get_v()+normal_Cam_optim[2]);
1838 Xinter_optim[0]=iP.
get_u()*z;
1839 Xinter_optim[1]=iP.
get_v()*z;
1847 double u = 0, v = 0;
1849 for(
unsigned int i = 0; i < 3; i++)
1851 diff = (Xinter_optim[i]-X0_2_optim[i]);
1852 u += diff*vbase_u_optim[i];
1853 v += diff*vbase_v_optim[i];
1855 u = u/(euclideanNorm_u*euclideanNorm_u);
1856 v = v/(euclideanNorm_v*euclideanNorm_v);
1858 if( u > 0 && v > 0 && u < 1. && v < 1.)
1865 else if (interp ==
SIMPLE)
1866 Ipixelplan = Isrc[(
unsigned int)i2][(
unsigned int)j2];
1874 vpImageSimulator::getPixelDepth(
const vpImagePoint &iP,
double &Zpixelplan)
1880 Zpixelplan = distance/(normal_Cam_optim[0]*iP.
get_u()+normal_Cam_optim[1]*iP.
get_v()+normal_Cam_optim[2]);
1885 vpImageSimulator::getPixelVisibility(
const vpImagePoint &iP,
1886 double &Visipixelplan)
1892 Visipixelplan = visible_result;
1897 vpImageSimulator::project(
const vpColVector &_vin,
1901 getHomogCoord(_vin,XH);
1902 getCoordFromHomog(_cMt*XH,_vout);
1908 for(
unsigned int i=0;i<3;i++)
1916 for(
unsigned int i=0;i<3;i++)
1917 _v[i]=_vH[i]/_vH[3];
1922 vpImageSimulator::getRoi(
const unsigned int &Iwidth,
1923 const unsigned int &Iheight,
1927 double top = Iheight+1;
1930 double left= Iwidth+1;
1931 for(
int i = 0; i < 4; i++)
1935 if (v < top) top = v;
1936 if (v > bottom) bottom = v;
1937 if (u < left) left = u;
1938 if (u > right) right = u;
1940 if (top < 0) top = 0;
1941 if(top >= Iheight) top = Iheight-1;
1942 if (bottom < 0) bottom = 0;
1943 if(bottom >= Iheight) bottom = Iheight-1;
1944 if(left < 0) left = 0;
1945 if(left >= Iwidth) left = Iwidth-1;
1946 if(right < 0) right = 0;
1947 if(right >= Iwidth) right = Iwidth-1;