(* The following pascal/delphi procedure implements the triangulation algorithm. ********************************************************************************** procedure triangulation( dx, dy: double; xres, yres: integer; var X,Y,Z: double); ********************************************************************************** The procedure takes as inputs: 1. the position of the unknown point in the current camera frame (dx, dy) in pixels 2. the size of the current camera frams (xres, yres) in pixels end returns the 3D coordinates of the unknown point in the world coordinate system The procedure assumes three global parameters: 1. LASER_ANGLE: the fixed laser device angle with respect to the horizontal X axis in radians 2. CAMERA_ANGLE: the fixed camera angle with respect to the horizontal X axis in radians 3. CAMERA_FOV: the field of view of the camera in radians ©2006 George P. Pavlidis, gpavlid@ceti.gr *) {************************************* TRIANGULATION *****************************} procedure triangulation( dx, dy: double; xres, yres: integer; var X,Y,Z: double); {*********************************************************************************} var thetax, a: double; thetay: double; // not actually needed; it is not used in the algorithm begin //****** compute the radius of the camera frame circumcircle a := sqrt( xres*xres + yres*yres) / 2; //****** compute the X distance of the unknown point from the center of the frame dx := (xres/2)-dx; //****** compute the Y distance of the unknown point from the center of the frame dy := (yres/2)-dy; //****** compute the angle that corresponds to the X displacement from the center thetax := arctan( dx / a * tan( CAMERA_FOV/2)); //****** compute the angle that corresponds to the X displacement from the center thetay := arctan( dy / a * tan( CAMERA_FOV/2)); //****** copmpute the distance of the unknown point on the measured object from the camera Z := LASER_CAMERA_DISTANCE * sin( LASER_ANGLE) * cos( thetax) / sin( LASER_ANGLE + CAMERA_ANGLE + thetax); //****** deduce the X coordinate using Z and trigonometry X := Z * dx/a * tan( CAMERA_FOV/2); //****** deduce the Y coordinate using Z and trigonometry; Y := Z * dy/a * tan( CAMERA_FOV/2); end;