1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83
| int Stereo::GetDisparity(IplImage *disp_map, CvPoint right)
{
///////////////////////////////////////////////////////////////////////////////////
//The GetDisparity method get the value of disparity for a given 2D point
//The 2D point is a point of the right image.
//Knowing the disparity and the right coordinates of the points we will be able
//To compute the 3D coordinates of the point
//Reminder :
//Z=b/D =====> D = (y'-y), b is the remaining coeeficient of the right cam to left cam transform matrix (correspondng gto y axis tanslation)
//X=xZ
//Y=yZ
///////////////////////////////////////////////////////////////////////////////////
uchar disp_value;
disp_value = ((uchar*)(disp_map->imageData+disp_map->widthStep*right.y))[right.x];
//printf("Disp : %i\n",disp_value);
return (int)disp_value;
}
//////////////////////////////////////////////////////////////////
CvPoint3D32f Stereo::compute3Dcoordinates(CvPoint right, float b, int disparity)
{
double x,y,z;
float f = f_rect;
z = (b*f / disparity);
x = (right.x * z)/f /**1.02e-3*/;
y = (right.y * z)/f /**0.9375e-3*/;
//z = (b / disparity)*600;
//x = right.x * z *3e-4;
//y = right.y * z *3e-4;
if (x!=0 && y!=0 && disparity )
{
printf("x=%f , y=%f , z=%f \n",x,y,z);
}
/*z = z*600;
x = x*600*5.6e-4;
y = y*600*5.6e-4;
*/
//printf("x=%f cm, y=%f cm, z=%f cm \n",x*634*5.6e-4,y*634*5.6e-4,z*634);
return cvPoint3D32f(x,y,z);
} |
Partager