intersect_lines_of_sight_targets

* This program demonstrates the measurement of the height of a
* workpiece by using targets and a stereo setup.
* First, the targets are detected in both the left
* and right images. Then, the 3D coordinates of
* the points are computed by the operator intersect_line_of_sight.
* Finally, the height of the workpiece is determined using
* these 3D coordinates.
* 
dev_update_off ()
dev_close_window ()
Scale := .7
* 
* Set the camera parameter and the relative pose
gen_cam_par_area_scan_division (0.0128583, -533.075, 7.40044e-006, 7.4e-006, 310.243, 243.807, 640, 480, CamParamLeft)
gen_cam_par_area_scan_division (0.0130659, -512.699, 7.41508e-006, 7.4e-006, 333.347, 237.374, 640, 480, CamParamRight)
create_pose (0.154132, -0.00585068, 0.0429294, 359.932, 336.422, 359.901, 'Rp+T', 'gba', 'point', RelPose)
* 
* Read and display images
read_image (ImageLeft, 'stereo/targets/targets_l')
read_image (ImageRight, 'stereo/targets/targets_r')
get_image_size (ImageLeft, Width, Height)
* 
dev_open_window (0, 0, Width * Scale, Height * Scale, 'black', WindowHandleLeft)
dev_set_color ('yellow')
dev_set_line_width (2)
set_display_font (WindowHandleLeft, 14, 'mono', 'true', 'false')
dev_display (ImageLeft)
disp_message (WindowHandleLeft, 'Left camera', 'window', 12, 12, 'black', 'true')
* 
dev_open_window (0, Width * Scale + 12, Width * Scale, Height * Scale, 'black', WindowHandleRight)
dev_set_color ('yellow')
dev_set_line_width (2)
set_display_font (WindowHandleRight, 14, 'mono', 'true', 'false')
dev_display (ImageRight)
disp_message (WindowHandleRight, 'Right camera', 'window', 12, 12, 'black', 'true')
* 
* Detect the targets
* 
* Left image
critical_points_sub_pix (ImageLeft, 'facet', 2.0, 5, RowMin, ColMin, RowMax, ColMax, RowPointLeft, ColumnPointLeft)
gen_cross_contour_xld (CrossLeft, RowPointLeft, ColumnPointLeft, 20, rad(45))
dev_set_window (WindowHandleLeft)
dev_display (CrossLeft)
* 
* Right image
critical_points_sub_pix (ImageRight, 'facet', 2.0, 5, RowMin, ColMin, RowMax, ColMax, RowPointRight, ColumnPointRight)
gen_cross_contour_xld (CrossRight, RowPointRight, ColumnPointRight, 20, rad(45))
dev_set_window (WindowHandleRight)
dev_display (CrossRight)
disp_continue_message (WindowHandleLeft, 'black', 'true')
stop ()
* 
* Compute the 3D coordinates of the
* targets and convert them into [mm]
intersect_lines_of_sight (CamParamLeft, CamParamRight, RelPose, RowPointLeft, ColumnPointLeft, RowPointRight, ColumnPointRight, X, Y, Z, Dist)
X := X * 1000
Y := Y * 1000
Z := Z * 1000
* 
* Determine the height of the workpiece
* First, define a new (non orthogonal) coordinate system
* whose plane z=0 is defined by the vectors NX=P2P0 and NY=P2P3
NX := [X[3] - X[2],Y[3] - Y[2],Z[3] - Z[2]]
NY := [X[0] - X[2],Y[0] - Y[2],Z[0] - Z[2]]
* Determine a normal vector NZ on this plane by computing
* the cross product of NX and NX
NZ := [NX[1] * NY[2] - NX[2] * NY[1],NX[2] * NY[0] - NY[2] * NX[0],NX[0] * NY[1] - NY[0] * NX[1]]
NZ := NZ / sqrt(sum(NZ * NZ))
* The height of the workpiece is obtained by computing the
* projection of the vector P2P1 onto the vector NZ. This is
* done by computing the dot product P2P1.NZ
P2P1 := [X[1] - X[2],Y[1] - Y[2],Z[1] - Z[2]]
WorkpieceHeight := sum(NZ * P2P1)
* 
* Display the results
gen_arrow_contour_xld (ArrowNXRight, RowPointRight[2], ColumnPointRight[2], RowPointRight[0], ColumnPointRight[0], 20, 20)
gen_arrow_contour_xld (ArrowNYRight, RowPointRight[2], ColumnPointRight[2], RowPointRight[3], ColumnPointRight[3], 20, 20)
gen_arrow_contour_xld (ArrowRight1, 225.5, 322.5, 225.5, 365.5, 15, 15)
gen_arrow_contour_xld (ArrowRight2, 225.5, 365.5, 225.5, 322.5, 15, 15)
dev_set_window (WindowHandleLeft)
dev_display (ImageLeft)
dev_display (CrossLeft)
disp_message (WindowHandleLeft, 'Left camera', 'window', 12, 12, 'black', 'true')
for I := 0 to 3 by 1
    String := 'x = ' + X[I]$'5.1f' + ' mm'
    String[1] := 'y = ' + Y[I]$'5.1f' + ' mm'
    String[2] := 'z = ' + Z[I]$'5.1f' + ' mm'
    disp_message (WindowHandleLeft, String, 'image', RowPointLeft[I] + 20, ColumnPointLeft[I] - 100, 'black', 'true')
    disp_message (WindowHandleLeft, 'P' + I, 'image', RowPointLeft[I] - 30, ColumnPointLeft[I] - 30, 'yellow', 'false')
    disp_message (WindowHandleRight, 'P' + I, 'image', RowPointRight[I] - 30, ColumnPointRight[I] - 30, 'yellow', 'false')
endfor
dev_set_window (WindowHandleRight)
dev_set_color ('blue')
dev_display (ArrowNXRight)
dev_display (ArrowNYRight)
dev_set_color ('yellow')
dev_display (CrossRight)
dev_display (ArrowRight1)
dev_display (ArrowRight2)
disp_message (WindowHandleRight, 'Height = ' + WorkpieceHeight$'2.1f' + ' mm', 'image', 213, 389, 'yellow', 'false')