技术点说明相机标定内参 外参Pose是高精度拼接的基础平面校正Rectification将斜视图像校正为正射视图消除透视畸变世界坐标对齐通过齐次变换矩阵将两相机的坐标系统一无缝拼接精确控制第二幅图的校正原点使其左边界 第一幅图右边界高度补偿通过DiffHeight修正标定平面与物体平面的高度差高效运行校正映射Map只需计算一次后续帧可快速应用* This example program shows how high precision mosaicking * can be performed with calibrated cameras. * * Further information can be found in the * Solution Guide III-C 3D Vision, Chapters 9 and 10. * This example program is used in chapter 9 to * illustrate calibrated mosaicking. * dev_update_off () ImgPath : 3d_machine_vision/multiple_cameras/ * * Open two windows for the left and the right image. dev_close_window () read_image (Image1, ImgPath camera1_ref) get_image_size (Image1, Width, Height) WindowScale : 0.66 dev_open_window (0, 0, Width * WindowScale, Height * WindowScale, black, WindowHandle1) dev_open_window (0, Width * WindowScale 6, Width * WindowScale, Height * WindowScale, black, WindowHandle2) * * Set some parameters for both windows. dev_set_window (WindowHandle1) dev_set_color (green) dev_set_draw (margin) dev_set_line_width (2) dev_set_part (0, 0, Height - 1, Width - 1) set_display_font (WindowHandle1, 16, mono, true, false) * dev_set_window (WindowHandle2) dev_set_color (green) dev_set_draw (margin) dev_set_line_width (2) dev_set_part (0, 0, Height - 1, Width - 1) set_display_font (WindowHandle2, 16, mono, true, false) * * We assume that the two cameras are already calibrated * (we know the internal camera parameters). gen_cam_par_area_scan_division (0.01619, -734.789, 7.402e-006, 7.4e-006, 324.911, 256.894, 640, 480, CamParam1) gen_cam_par_area_scan_division (0.0162584, -763.35, 7.39842e-006, 7.4e-006, 324.176, 245.371, 640, 480, CamParam2) * * Read the images and display them. read_image (Image1, ImgPath camera1_ref) read_image (Image2, ImgPath camera2_ref) dev_set_window (WindowHandle1) dev_display (Image1) dev_set_window (WindowHandle2) dev_display (Image2) * * Prepare the camera calibration. CaltabName : caltab_30mm.descr create_calib_data (calibration_object, 2, 1, CalibDataID) set_calib_data_calib_object (CalibDataID, 0, CaltabName) set_calib_data_cam_param (CalibDataID, 0, [], CamParam1) set_calib_data_cam_param (CalibDataID, 1, [], CamParam2) * * Find and display the calibration plate in the images. dev_set_window (WindowHandle1) find_calib_object (Image1, CalibDataID, 0, 0, 0, [], []) get_calib_data_observ_points (CalibDataID, 0, 0, 0, RowCoord1, ColumnCoord1, Index1, Pose1) get_calib_data_observ_contours (Caltab1, CalibDataID, caltab, 0, 0, 0) dev_display (Caltab1) * dev_set_window (WindowHandle2) find_calib_object (Image2, CalibDataID, 1, 0, 0, [], []) get_calib_data_observ_points (CalibDataID, 1, 0, 0, RowCoord2, ColumnCoord2, Index2, Pose2) get_calib_data_observ_contours (Caltab2, CalibDataID, caltab, 1, 0, 0) dev_display (Caltab2) * disp_message (WindowHandle1, Calibration successful, window, 12, 12, black, true) disp_continue_message (WindowHandle1, black, true) stop () clear_calib_data (CalibDataID) * * * 补偿高度差标定板 vs 物体表面 ThicknessCaliper : 2.9 / 1000.0 //卡尺厚度米 ThicknessPlate : 5.65 / 1000.0 // 标定板厚度米 ****DiffHeight0.0029 DiffHeight : ThicknessPlate - ThicknessCaliper // 高度补偿值 DistancePlates : 0.06488 // 两标定板之间的已知距离米 * * Define the pixel size of the mosaic image (in meters). * * PixelSize : 0.0001 //定义拼接图像的像素物理尺寸米/像素 即 0.1 mm/pixel * 计算第一幅左图校正图像的位置和尺寸 * For the first (left) image, the determination * of the necessary shift of the pose is straightforward. * You can define the upper left corner of the rectified image * in image coordinates * based on a preselected border width. *自定义图像区域 BorderInPercent : 7 get_image_size (Image1, WidthImage1, HeightImage1) ******定义左图校正区域的左上角预留边框 UpperRow : HeightImage1 * BorderInPercent / 100.0 LeftColumn : WidthImage1 * BorderInPercent / 100.0 * * Then, this point must be transformed into world coordinates. ****将该点从图像坐标 → 世界坐标基于 Pose1 image_points_to_world_plane (CamParam1, Pose1, UpperRow, LeftColumn, m, LeftX, UpperY) * For the determination of the height of the rectified image * we need to define a point that lies near the lower border * of the first image. * LowerRow : HeightImage1 * (100 - BorderInPercent) / 100.0 * * Again, this point must be transformed * into the world coordinate system. ****左上角图像坐标 基于位姿pose1转化为世界坐标 image_points_to_world_plane (CamParam1, Pose1, LowerRow, LeftColumn, m, X1, LowerY) * * The height can be determined as the vertical distance * between the upper left point and the point near the * lower image border, expressed in pixels of the rectified image. HeightRect : int((LowerY - UpperY) / PixelSize) //计算校正图像的高度上下边界距离 * * Analogously, the width can be determined from a point t * hat lies in the overlapping area of the two images, * i.e., near the right border of the first image. * OverlapInPercent : 20 RightColumn : WidthImage1 * (100 - OverlapInPercent / 2.0) / 100.0 ****依据位姿pose1 得到图像右上角坐标对应的 世界坐标 image_points_to_world_plane (CamParam1, Pose1, UpperRow, RightColumn, m, RightX, Y1) ********计算校正图像的宽度利用重叠区 WidthRect : int((RightX - LeftX) / PixelSize) * * With the shifted pose and the size of the rectified image, * the rectification map for the first image can be derived. *生成左图的校正映射Map 将世界坐标原点移到校正图像左上角并补偿高度。 set_origin_pose (Pose1, LeftX, UpperY, DiffHeight, PoseNewOrigin1) *****【核心】以上步骤 主要是 从规定映射图像区域和标定板 得到映射图像区域所对应的位姿 PoseNewOrigin1 *********图像坐标系-相机内参相机外参【依据标定板pose并平移到校正图像对应的左上角】---得到每个像素点的MapSingle1映射 gen_image_to_world_plane_map (MapSingle1, CamParam1, PoseNewOrigin1, Width, Height, WidthRect, HeightRect, PixelSize, bilinear) * * Generate a new homogeneous transformation matrix. ******计算第二幅右图的校正映射 —— 关键与左图无缝拼接 *构建齐次变换矩阵 hom_mat3d_identity (HomMat3DIdentity) **************hom_mat3d_translate_local【基于相机1的pose 进行变换】 **************hom_mat3d_translate【基于世界坐标 进行变换】 此案例未知 ****cp1Hur1: 从标定板1坐标系 → 第一幅校正图右上角世界坐标 hom_mat3d_translate_local (HomMat3DIdentity, LeftX PixelSize * WidthRect, UpperY, DiffHeight, cp1Hur1) *****// cp1Hcp2: 从标定板1 → 标定板2 的已知变换由标定物物理结构决定 hom_mat3d_translate_local (HomMat3DIdentity, DistancePlates, 0, 0, cp1Hcp2) * *推导右图校正所需的新原点 hom_mat3d_invert (cp1Hcp2, cp2Hcp1) // 反向变换 hom_mat3d_compose (cp2Hcp1, cp1Hur1, cp2Hul2) // 得到标定板2 → 第二幅图左上角 * * 更新 Pose2 的原点 pose_to_hom_mat3d (Pose2, cam2Hcp2) hom_mat3d_compose (cam2Hcp2, cp2Hul2, cam2Hul2) hom_mat3d_to_pose (cam2Hul2, PoseNewOrigin2) * *生成右图校正映射【核心】 ******核心思想让右图校正后的左上角 左图校正后的右上角 → 实现无缝拼接 gen_image_to_world_plane_map (MapSingle2, CamParam2, PoseNewOrigin2, Width, Height, WidthRect, HeightRect, PixelSize, bilinear) * * Open a new Graphics Window for the merged image. dev_open_window (Height * WindowScale, 0, Width * 2 * WindowScale, Height * WindowScale, black, WindowHandleCombined) set_display_font (WindowHandleCombined, 16, mono, true, false) dev_set_color (green) dev_set_draw (margin) ScalePlot : 200 RowPlot : 400 Coord : [0:2000] * * Process all image pairs in a loop. ***** 拼接并显示结果循环处理多组图像 for I : 1 to 3 by 1 * * Display both images. dev_set_window (WindowHandle1) read_image (Image1, ImgPath camera1_ I$02d) get_image_size (Image1, WidthImage1, HeightImage1) dev_set_part (0, 0, HeightImage1 - 1, WidthImage1 - 1) dev_display (Image1) dev_set_window (WindowHandle2) read_image (Image2, ImgPath camera2_ I$02d) get_image_size (Image2, WidthImage2, HeightImage2) dev_set_part (0, 0, HeightImage2 - 1, WidthImage2 - 1) dev_display (Image2) * * Start the time measurement. count_seconds (TimeStart1) * * Rectify the image pair from the two-camera setup with map_image. map_image (Image1, MapSingle1, RectifiedImage1) // 应用校正映射 map_image (Image2, MapSingle2, RectifiedImage2) concat_obj (RectifiedImage1, RectifiedImage2, Concat) * End the time measurement and calculate the difference. count_seconds (TimeEnd1) Time1 : TimeEnd1 - TimeStart1 * dev_set_window (WindowHandleCombined) * Start the time measurement again. count_seconds (TimeStart2) * * Tile both images into one large image. tile_images (Concat, Combined, 2, vertical) // 水平拼接 * * End the time measurement again and calculate the difference. count_seconds (TimeEnd2) Time2 : TimeEnd2 - TimeStart2 * * Display the combined image and the time measurement. get_image_size (Combined, WidthComb, HeightComb) dev_set_part (0, 0, HeightComb - 1, WidthComb - 1) dev_display (Combined) disp_message (WindowHandle1, Merge cameras: (1000 * (Time1 Time2))$.3 ms, window, 12, 12, black, true) * * In addition, we plot the accuracy of the mosaicking with a procedure. plot_mosaicking_accuracy (Combined, WidthRect, HeightRect, WindowHandleCombined, Coord, ScalePlot, RowPlot) // 绘制拼接精度如边缘对齐误差 if (I 3) disp_continue_message (WindowHandleCombined, black, true) stop () endif endfor