少有人走的路勇哥注：想搞清楚halcon的手眼标定，3d位姿的知识是基本功。
本篇文章是其入门扫盲贴。
程序运行后，有四个坐标系：
相机坐标系（Camera coordinate system）
世界坐标系（World coordinate system）
对象坐标系（Object coordinate system）
机器人基坐标系（Robot base coordinate system）
你可以用鼠标指向四个坐标系的位置，则在右下角的会显示对像在这个坐标系下的位姿信息（３个平移３个旋转）。
初始化的代码初始化了四个坐标系的位姿：
WorldCSPose世界坐标系初始化位姿
ObjInWorldPose对像在世界坐标系的位姿
RobotInWorldPose机器人基坐在世界坐标系下的位姿
CamInWorldPose相机在世界坐标系下的位姿
create_pose (0.0, 0.0, 0.0, 0, 0, 0.0, 'Rp+T', 'gba', 'point', WorldCSPose) create_pose (0.05, 0.20, 0.0, 180, 0, 45, 'Rp+T', 'gba', 'point', ObjInWorldPose) create_pose (-0.30, 0.20, 0.0, 90, 0, 0, 'Rp+T', 'gba', 'point', RobotInWorldPose) create_pose (0.1, -0.15, 0.0, -90, 0, 0, 'Rp+T', 'gba', 'point', CamInWorldPose)
外部函数disp_3d_coord_system_in_image显示xyz坐标系轴的图标。
下图是四个坐标系的轴图标的状态。
大家对照这个图，下面勇哥说说四条create_pos语句是怎么样把四个坐标系轴图标创建出来的。
第一条语句创建的就是世界坐标系的位姿，这个位置移动与旋转都为0，它做为原点。
create_pose (0.0, 0.0, 0.0, 0, 0, 0.0, 'Rp+T', 'gba', 'point', WorldCSPose)
第二条语句是把世界坐标系（World coordinate system）进行移动与旋转。
x轴正方向移动50mm,y轴正方向移动200mm，然后再按x, y, z的顺序依次旋转180度，0度，45度。
create_pose (0.05, 0.20, 0.0, 180, 0, 45, 'Rp+T', 'gba', 'point', ObjInWorldPose)
这样操作后世界坐标系（World coordinate system）就会变成上图的对象坐标系（object coordinate system）完全重合。
以上过程如果在ug, 3dsmax等3d建模软件里面实验一下会更容易明白。有空闲勇哥会截几张图示意一下。
第三与第四条语句意义类同，这里就不赘述了。
当鼠标指向四个坐标系的矩形区域范围内时，调用用户定义函数show_pose_in_selected_cs显示结果。
在它里面是一个switch分支。
当鼠标指向不同的坐标系矩形范围时，触发动作。
switch (ToBeDisplayed) case 0: * Nothing selected: Do nothing break case 1: * 鼠标指向相机坐标系 SelectedString := 'camera' SelectedOrientation := [CamInWorldPose[3],CamInWorldPose[4],CamInWorldPose[5]] SelectedRow := CamRow SelectedColumn := CamColumn * 反转对象位姿到相机坐标系下 pose_invert (CamInWorldPose, WorldInCamPose) *计算对象在相机坐标系下的位姿(ObjInCamPose). pose_compose (WorldInCamPose, ObjInWorldPose, ObjInCamPose) DisplayedPose := ObjInCamPose display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break case 2: * 鼠标指向世界坐标系 SelectedString := 'robot' SelectedOrientation := [RobotInWorldPose[3],RobotInWorldPose[4],RobotInWorldPose[5]] SelectedRow := RobotRow SelectedColumn := RobotColumn pose_invert (RobotInWorldPose, WorldInRobotPose) pose_compose (WorldInRobotPose, ObjInWorldPose, ObjInRobotPose) DisplayedPose := ObjInRobotPose display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break case 4: *鼠标指向世界坐标系 SelectedString := 'world' SelectedOrientation := [WorldCSPose[3],WorldCSPose[4],WorldCSPose[5]] SelectedRow := WorldRow SelectedColumn := WorldColumn DisplayedPose := ObjInWorldPose display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break case 8: * 鼠标指向对象坐标系 SelectedString := 'object' SelectedOrientation := [ObjInWorldPose[3],ObjInWorldPose[4],ObjInWorldPose[5]] SelectedRow := ObjRow SelectedColumn := ObjColumn DisplayedPose := gen_tuple_const(7,0) display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break default: break endswitch
先来看case 1的动作，鼠标指向相机坐标系。
计算结果是求出“对象在相机坐标系下的位姿”(Pose of object in camera coordinates)
tx=-0.05, ty=0, tz=0.35, rx=270, ry=0, rz=45
这个计算结果由下面两条核心算子完成。
*位姿取反 pose_invert (CamInWorldPose, WorldInCamPose) *矩阵相乘，用于已知两个位姿，求出第三个位姿。 pose_compose (WorldInCamPose, ObjInWorldPose, ObjInCamPose)
pose_invert算子是把 "相机在世界坐标系下的位姿" 转换为 “世界坐标系在相机坐标系下的位姿"。
pose_compose算子是把 “世界坐标系在相机坐标系下的位姿” 乘以 “对象在世界坐标系下的位姿” ， 求出“对象在相机坐标系下的位姿” 。
你是不是还是满头雾水不知道所云？勇哥画个示意图你就明白了，如下：
在下图中WorldInCamPose（世界坐标系在相机坐标系下的位姿）相当于三角上边那根红色边的关系,
ObjInWorldPose（对象在世界坐标系下的位姿）代表下边那条红色边的关系，
而三角形蓝色那条边的关系就是我们要求的ObjInCamPose（对象在相机坐标系下的位姿）
到此步勇哥暂时不想再深入研究上面的两个算子的数学原理，我只记了凡是要做坐标系位置的转换、或者已经两个坐标系位置求第三个坐标系位姿就用上面两个算子。
其它的转换还有：
对象在世界坐标系下的位姿
对象在机器人基坐标系下的位姿
原理是一样的，这里勇哥就不赘述了。
唯一不同的是”对象在对象坐标系下的位姿“（Pose of object in object coordinates）。
这个肯定是移动与旋转都为0呀。
本例子halcon程序代码如下：
主程序
*物体的姿态总是取决于所选择的参考坐标系。这个例子显示了物体在不同坐标系下的姿态。
*用户通过将鼠标指针放置在坐标系统的标签上来选择参考坐标系统。
*物体的局部坐标系总是显示出来。
*根据鼠标指针的位置显示相应的坐标系统。
*根据该坐标系给出的对象的位姿显示在对象右侧的框中。使用相对距离。
*在窗口中点击鼠标右键可以停止该程序
dev_close_window ()
dev_get_preferences ('graphics_window_context_menu', GraphicsWindowContextMenu)
dev_set_preferences ('graphics_window_context_menu', 'false')
Width := 640
Height := 480
dev_open_window (0, 0, Width, Height, 'white', WindowHandle)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
gen_robot_cam_object (Image, WindowHandle)
dev_display (Image)
dev_set_line_width (2)
* 世界坐标系的位姿-原点
* 前三个平移单位是米, 后面三个旋转的单位是度
* gba方式是旋转顺序为x,y,z
create_pose (0.0, 0.0, 0.0, 0, 0, 0.0, 'Rp+T', 'gba', 'point', WorldCSPose)
* 物体在世界坐标系中的位姿
create_pose (0.05, 0.20, 0.0, 180, 0, 45, 'Rp+T', 'gba', 'point', ObjInWorldPose)
*机器人基坐在世界坐标系中的位姿
create_pose (-0.30, 0.20, 0.0, 90, 0, 0, 'Rp+T', 'gba', 'point', RobotInWorldPose)
* 相机在世界坐标系中的位姿
create_pose (0.1, -0.15, 0.0, -90, 0, 0, 'Rp+T', 'gba', 'point', CamInWorldPose)
* 显示所选择的坐标系和物体相对于所选择的坐标系的姿态。
show_pose_in_selected_cs (Image, WindowHandle, ObjInWorldPose, RobotInWorldPose, CamInWorldPose, WorldCSPose)
* 在窗口中点击鼠标右键停止程序
wait_seconds (0.5)
dev_set_preferences ('graphics_window_context_menu', GraphicsWindowContextMenu)gen_robot_cam_object 画界面的图形
*
* Draws the stylized camera and the robot into the given window.
* The contents of the window is dumped into an image
* object and returned.
dev_clear_window ()
dev_set_color ('black')
dev_set_line_width (3)
gen_rectangle2_contour_xld (Rectangle, 69.5, 449.5, rad(-90), 34, 22.8293)
gen_contour_polygon_xld (Triangle, [105,131,131,131,105,131], [449,426,426,469,449,469])
dev_display (Rectangle)
dev_display (Triangle)
dev_set_line_width (4)
gen_rectangle1 (Robot1, 423.5, 0.875, 432.5, 99.5)
dev_display (Robot1)
gen_circle_contour_xld (Robot2, 422.5, 46.5, 5.38516, 0, 6.28318, 'positive', 1)
dev_display (Robot2)
gen_circle_contour_xld (Robot3, 332.5, 16.5, 6, 0, 6.28318, 'positive', 1)
dev_display (Robot3)
gen_circle_contour_xld (Robot4, 255, 151, 5.5, 0, 6.28318, 'positive', 1)
dev_display (Robot4)
gen_circle_contour_xld (Robot5, 253.5, 251, 5.09902, 0, 6.28318, 'positive', 1)
dev_display (Robot5)
gen_contour_polygon_xld (Robot6, [417,337], [44.5,17.5])
dev_display (Robot6)
gen_contour_polygon_xld (Robot7, [329,257], [21,147])
dev_display (Robot7)
gen_contour_polygon_xld (Robot8, [253.5,253.5], [156,245.5])
dev_display (Robot8)
gen_contour_polygon_xld (Robot9, [254.75,297], [256,299])
dev_display (Robot9)
gen_contour_polygon_xld (Robot10, [284,310], [314.25,287.75])
dev_display (Robot10)
gen_contour_polygon_xld (Robot11, [284.25,298.5], [314,328])
dev_display (Robot11)
gen_contour_polygon_xld (Robot12, [309.5,324], [287.75,302])
dev_display (Robot12)
dump_window_image (Image, WindowHandle)
rgb1_to_gray (Image, Image)
read_image (ObjectImage, 'clamp_sloped/clamp_sloped_02')
get_image_size (Image, Width, Height)
Scale := 0.3
hom_mat2d_identity (HomMat2DIdentity)
hom_mat2d_scale (HomMat2DIdentity, Scale, Scale, 0, 0, HomMat2DScale)
affine_trans_image_size (ObjectImage, ImageScaled, HomMat2DScale, 'constant', Width * Scale, Height * Scale)
concat_obj (Image, ImageScaled, Images)
ImagePosRow := 330
ImagePosCol := 350
* compute current position of the caltab image in the Title Images
tile_images_offset (Images, Image, [0,ImagePosRow], [0,ImagePosCol], [-1,0], [-1,30], [-1,125], [-1,160], Width, Height)
dev_set_line_width (2)
return ()show_pose_in_selected_cs
* * This procedure tests the regions defined below whether they * currently contain the mouse pointer. Then the pose of the object * is computed with respect to the coordinate system that is * enclose by the region where the mouse pointer currently is. * All input poses are given with respect to the world coordinate system. * * Set default camera parameter (needed for projecting the * coordinate systems) *此过程测试下面定义的区域当前是否包含鼠标指针。然后根据包含鼠标指针所在区域的坐标系统计算对象的姿态。 *所有输入位姿都是在世界坐标系下给出的。 *设置默认的相机参数(用于投影坐标系统) gen_cam_par_area_scan_division (0.01, 0, 7.32e-006, 7.4e-6, 323.6, 245.555, 640, 480, CamParam) dev_update_off () * To avoid flickering, set 'flush' to false. get_window_param (WindowHandle, 'flush', Flush) set_window_param (WindowHandle, 'flush', 'false') * Depending on mouse position show the object pose in * the corresponding coordinate system get_window_extents (WindowHandle, Row, Column, Width, Height) * Set x,y,z color ColorX := 'red' ColorY := 'forest green' ColorZ := 'blue' TextColor := 'black' * Image coordinates for showing the object coordinate system ObjRow := 425 ObjColumn := 385 * Image coordinates for showing the world coordinate system WorldRow := 220 WorldColumn := 310 * Image coordinates for showing the robot coordinate system RobotRow := 425 RobotColumn := 46 * Image coordinates for showing the camera coordinate system CamRow := 70 CamColumn := 450 * Offset of the label with repect to coordinate in image coordinates LabelOffsetColumn := 85 LabelOffsetRow := -15 * Image coordinates of the label for the object coordinate system LabelObjInImage := [ObjRow + LabelOffsetRow,ObjColumn + LabelOffsetColumn] * Image coordinates of the label for the world coordinate system LabelWorldInImage := [WorldRow + LabelOffsetRow,WorldColumn + LabelOffsetColumn] * Image coordinates of the label for the robot base coordinate system LabelRobotInImage := [RobotRow + LabelOffsetRow,RobotColumn + LabelOffsetColumn] * Image coordinates of the label for the camera coordinate system LabelCamInImage := [CamRow + LabelOffsetRow,CamColumn + LabelOffsetColumn] * Generate rectangles for which a mouse reaction is defined AreaSizeRow := 150 AreaSizeColumn := 220 StartOffset := -80 gen_rectangle1 (ROIObj, ObjRow + StartOffset, ObjColumn + StartOffset, ObjRow + AreaSizeRow, ObjColumn + AreaSizeColumn) gen_rectangle1 (ROICam, CamRow + StartOffset, CamColumn + StartOffset, CamRow + AreaSizeRow, CamColumn + AreaSizeColumn) gen_rectangle1 (ROIRob, RobotRow + StartOffset, RobotColumn + StartOffset, RobotRow + AreaSizeRow, RobotColumn + AreaSizeColumn) gen_rectangle1 (ROIWorld, WorldRow + StartOffset, WorldColumn + StartOffset, WorldRow + AreaSizeRow, WorldColumn + AreaSizeColumn) * Images coordinates for displaying the pose values PoseTextRow := 260 PoseTextColumn := 460 Orientation3D := [ObjInWorldPose[3],ObjInWorldPose[4],ObjInWorldPose[5]] Colors := [TextColor,TextColor,TextColor,ColorX,ColorY,ColorZ,ColorX,ColorY,ColorZ] CurrentlyDisplayed := 0 ToBeDisplayed := 1 Button := 0 while (Button != 4) * Update display, if a new region has been selected if (ToBeDisplayed != CurrentlyDisplayed) CurrentlyDisplayed := ToBeDisplayed switch (ToBeDisplayed) case 0: * Nothing selected: Do nothing break case 1: * Compute object pose in camera coordinate system SelectedString := 'camera' SelectedOrientation := [CamInWorldPose[3],CamInWorldPose[4],CamInWorldPose[5]] SelectedRow := CamRow SelectedColumn := CamColumn * This inverted pose is needed for the transformation of * the object pose into the camera coordinate systems. pose_invert (CamInWorldPose, WorldInCamPose) * Compute the pose of the object in camera coordinates (ObjInCamPose). pose_compose (WorldInCamPose, ObjInWorldPose, ObjInCamPose) DisplayedPose := ObjInCamPose display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break case 2: * Compute object pose in robot coordinate system SelectedString := 'robot' SelectedOrientation := [RobotInWorldPose[3],RobotInWorldPose[4],RobotInWorldPose[5]] SelectedRow := RobotRow SelectedColumn := RobotColumn * This inverted pose is needed for the transformation of * the object pose into the robot coordinate systems. pose_invert (RobotInWorldPose, WorldInRobotPose) * Compute the pose of the object in robot base coordinates (ObjInRobotPose). pose_compose (WorldInRobotPose, ObjInWorldPose, ObjInRobotPose) DisplayedPose := ObjInRobotPose display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break case 4: * Show object pose in world coordinate system * No computation is needed since the input pose is already * the pose of the object in the world coordinate system. SelectedString := 'world' SelectedOrientation := [WorldCSPose[3],WorldCSPose[4],WorldCSPose[5]] SelectedRow := WorldRow SelectedColumn := WorldColumn DisplayedPose := ObjInWorldPose display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break case 8: * Object coordinate system SelectedString := 'object' SelectedOrientation := [ObjInWorldPose[3],ObjInWorldPose[4],ObjInWorldPose[5]] SelectedRow := ObjRow SelectedColumn := ObjColumn * The pose of the object in the object attached coordinate system equal * its origin. DisplayedPose := gen_tuple_const(7,0) display_poses (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage, CamParam, Orientation3D, ObjRow, ObjColumn, SelectedOrientation, SelectedRow, SelectedColumn, SelectedString, DisplayedPose, PoseTextRow, PoseTextColumn, Colors, Message) break default: * This branch can only be reached, if the selection * regions overlap and the mouse points to an * overlapping part. * In this case, do nothing. break endswitch endif * Flush the content of the graphics window. flush_buffer (WindowHandle) * Get mouse position try get_mposition (WindowHandle, MouseRow, MouseColumn, Button) catch (Exception) continue endtry * Test if mouse is in region defined for the camera test_region_point (ROICam, MouseRow, MouseColumn, MouseOnCam) test_region_point (ROIRob, MouseRow, MouseColumn, MouseOnRob) test_region_point (ROIWorld, MouseRow, MouseColumn, MouseOnWorld) test_region_point (ROIObj, MouseRow, MouseColumn, MouseOnObj) * Calculate index depending on mouse position ToBeDisplayed := MouseOnCam + 2 * MouseOnRob + 4 * MouseOnWorld + 8 * MouseOnObj endwhile set_window_param (WindowHandle, 'flush', Flush) return ()
display_poses
* * Display poses reset_visualization (Image, WindowHandle, LabelObjInImage, LabelRobotInImage, LabelCamInImage, LabelWorldInImage) dev_set_colored (3) disp_3d_coord_system_in_image (WindowHandle, CamParam, Orientation3D, ObjRow, ObjColumn) disp_3d_coord_system_in_image (WindowHandle, CamParam, SelectedOrientation, SelectedRow, SelectedColumn) Message := ['Pose of object','in ' + SelectedString,'coordinates'] Message[3] := 'Tx = ' + DisplayedPose[0]$'.02f' + ' m' Message[4] := 'Ty = ' + DisplayedPose[1]$'.02f' + ' m' Message[5] := 'Tz = ' + DisplayedPose[2]$'.02f' + ' m' Message[6] := 'Rx = ' + DisplayedPose[3]$'.02f' + ' deg' Message[7] := 'Ry = ' + DisplayedPose[4]$'.02f' + ' deg' Message[8] := 'Rz = ' + DisplayedPose[5]$'.02f' + ' deg' disp_message (WindowHandle, Message, 'image', PoseTextRow, PoseTextColumn, Colors, 'true') return ()
---------------------
作者：hackpig
来源：www.skcircle.com
版权声明：本文为博主原创文章，转载请附上博文链接！
本文出自勇哥的网站《少有人走的路》wwww.skcircle.com，转载请注明出处！讨论可扫码加群：
