3d_coordinates 测量世界坐标中的倾斜物体

• 测量世界坐标中的倾斜物体
• Initialize the program
  dev_close_window ()
  dev_open_window (0, 0, 768, 576, ‘black’, WindowHandle)
  dev_update_off ()
  dev_set_draw (‘margin’)
  dev_set_line_width (3)
  set_display_font (WindowHandle, 14, ‘mono’, ‘true’, ‘false’)
• 
  
  • Calibrate the camera
• 
  CalTabDescrFile := ‘caltab_big.descr’
  StartCamPar := [0.008,0,0.0000086,0.0000086,384,288,768,576]
  create_calib_data (‘calibration_object’, 1, 1, CalibDataID)
  set_calib_data_cam_param (CalibDataID, 0, ‘area_scan_division’, StartCamPar)
  set_calib_data_calib_object (CalibDataID, 0, CalTabDescrFile)
  NumImages := 10
  for I := 1 to NumImages by 1
  read_image (Image, ‘calib/calib-3d-coord-’ + I$’02d’)
  dev_display (Image)
  Message := ‘Find calibration plate in\nall calibration images (’ + I + ‘/’ + NumImages + ‘)’
  disp_message (WindowHandle, Message, ‘window’, 12, 12, ‘black’, ‘true’)
  
  • Find the calibration plate
    find_calib_object (Image, CalibDataID, 0, 0, I - 1, [], [])
    get_calib_data (CalibDataID, ‘camera’, 0, ‘init_params’, StartCamPar)
    get_calib_data_observ_points (CalibDataID, 0, 0, I - 1, Row, Column, Index, Pose)
    get_calib_data_observ_contours (Contours, CalibDataID, ‘caltab’, 0, 0, I - 1)
    gen_cross_contour_xld (Cross, Row, Column, 6, 0.785398)
    dev_set_color (‘green’)
    dev_display (Contours)
    dev_set_color (‘yellow’)
    dev_display (Cross)
    endfor
    disp_continue_message (WindowHandle, ‘black’, ‘true’)
    stop ()
    calibrate_cameras (CalibDataID, Error)
    get_calib_data (CalibDataID, ‘camera’, 0, ‘params’, CamParam)
• 此时得到了Pose和CamParam
• Perform measurements
• 
  for I := 1 to NumImages by 1
  read_image (Image, ‘calib/calib-3d-coord-’ + I$’02d’)
  
  • 现在找到标定板的黑色边缘
  • Now, measure the size of the black border of the plate
    get_measure_positions (Image, PlateRegion, CalibDataID, I, Distance, Phi, RowCenter, ColumnCenter)
    *用矩形的形状创建一个XLD轮廓，依次为矩形的XLD轮廓，中心的行坐标，矩形的中心坐标，矩形的主轴的方向
    gen_rectangle2_contour_xld (Rectangle, RowCenter, ColumnCenter, Phi, Distance * 0.52, 8)
    gen_measure_rectangle2 (RowCenter, ColumnCenter, Phi, Distance * 0.52, 8, 768, 576, ‘nearest_neighbor’, MeasureHandle)
  • 提取垂直于矩形或环形弧的直边共18个点
    measure_pos (Image, MeasureHandle, 1, 40, ‘all’, ‘all’, RowEdge, ColumnEdge, Amplitude, Distance1)
    close_measure (MeasureHandle)
    Rows := [RowEdge[0],RowEdge[|RowEdge| - 1]]
    Columns := [ColumnEdge[0],ColumnEdge[|RowEdge| - 1]]
    gen_cross_contour_xld (Cross, Rows, Columns, 16, Phi)
  • 将两个边界点转换为世界坐标系
  • Transform the two border points into the world coordinate system
    get_calib_data (CalibDataID, ‘calib_obj_pose’, [0,I - 1], ‘pose’, Pose)
    *将图像点转换为z=0的世界坐标系
    image_points_to_world_plane (CamParam, Pose, Rows, Columns, ‘m’, SX, SY)
    distance_pp (SY[0], SX[0], SY[1], SX[1], Width)
  • 
    
    • Display results of width measurement
      dev_display (Image)
      dev_set_color (‘white’)
      dev_set_line_width (3)
      dev_display (Rectangle)
      dev_set_color (‘green’)
      dev_set_draw (‘fill’)
      dev_set_line_width (2)
      dev_display (Cross)
      dev_set_draw (‘margin’)
      disp_message (WindowHandle, ‘Width = ’ + (Width * 100)$’8.3f’ + ‘cm’, ‘window’, 12, 12, ‘black’, ‘true’)
      disp_continue_message (WindowHandle, ‘black’, ‘true’)
      stop ()
  • 现在，测量校准标记点的尺寸
  • Now, measure the size of the calibration marks
  • 提取图像中的圆
  • Extract the ellipses in the image用环状结构元素侵蚀一个区域
    erosion_circle (PlateRegion, ROI, 17.5)
    reduce_domain (Image, ROI, ImageReduced)
    edges_sub_pix (ImageReduced, Edges, ‘canny’, 1, 20, 60)
    select_contours_xld (Edges, SelectedEdges, ‘contour_length’, 20, 99999999, -0.5, 0.5)
  • Fit ellipses to extracted edges，拟合圆
    fit_ellipse_contour_xld (SelectedEdges, ‘fitzgibbon’, -1, 2, 0, 200, 3, 2, Row, Column, Phi, Radius1, Radius2, StartPhi, EndPhi, PointOrder)
    *取均值
    MeanRadius1 := mean(Radius1)
    MeanRadius2 := mean(Radius2)
    DevRadius1 := deviation(Radius1)
    DevRadius2 := deviation(Radius2)
  • Transform the ellipses to world coordinates, where they should be circles
    *吧圆转换到世界坐标
  • and convert the circles from meters to millimeters so that we can see them.
    contour_to_world_plane_xld (SelectedEdges, WorldCircles, CamParam, Pose, ‘mm’)
  • Fit ellipses to the circles in world coordinates
    fit_ellipse_contour_xld (WorldCircles, ‘fitzgibbon’, -1, 2, 0, 200, 3, 2, Row, Column, Phi, RadiusW1, RadiusW2, StartPhi, EndPhi, PointOrder)
    MeanRadiusW1 := mean(RadiusW1)
    MeanRadiusW2 := mean(RadiusW2)
    DevRadiusW1 := deviation(RadiusW1)
    DevRadiusW2 := deviation(RadiusW2)
  • 
    
    • Display results of ellipse measurement
      dev_display (Image)
      dev_set_color (‘yellow’)
      dev_set_line_width (3)
      dev_display (SelectedEdges)
      Message := ‘Measured dimensions of the ellipses’
      Message[0] := ’ Mean Radius1; Mean Radius2; (Standard deviations [%])’
      Message[1] := ‘Image coordinates: ’ + MeanRadius1$'5.2f' + 'px; ' + MeanRadius2$’5.2f’ + ‘px (’ + (DevRadius1 / MeanRadius1 * 100)$'4.2f' + ', ' + (DevRadius2 / MeanRadius2 * 100)$’4.2f’ + ‘)’
      Message[2] := ‘World coordinates: ’ + (MeanRadiusW1 / 10)$'5.2f' + 'cm; ' + (MeanRadiusW2 / 10)$’5.2f’ + ‘cm (’ + (DevRadiusW1 / MeanRadiusW1 * 100)$'4.2f' + ', ' + (DevRadiusW2 / MeanRadiusW2 * 100)$’4.2f’ + ‘)’
      disp_message (WindowHandle, Message, ‘window’, 12, 12, ‘black’, ‘true’)
      if (I < 10)
      disp_continue_message (WindowHandle, ‘black’, ‘true’)
      stop ()
      endif
      endfor
      clear_calib_data (CalibDataID)