显示对象相对于世界、相机和机器人坐标系的姿势

勇哥注:想搞清楚halcon的手眼标定,3d位姿的知识是基本功。

本篇文章是其入门扫盲贴。



程序运行后,有四个坐标系:

  • 相机坐标系(Camera coordinate system)

  • 世界坐标系(World coordinate system)

  • 对象坐标系(Object coordinate system)

  • 机器人基坐标系(Robot base coordinate system)


32l27Efj9w.gif


你可以用鼠标指向四个坐标系的位置,则在右下角的会显示对像在这个坐标系下的位姿信息(3个平移3个旋转)。

image.png



初始化的代码初始化了四个坐标系的位姿:

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语句是怎么样把四个坐标系轴图标创建出来的。

image.png


第一条语句创建的就是世界坐标系的位姿,这个位置移动与旋转都为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的动作,鼠标指向相机坐标系。

image.png

计算结果是求出“对象在相机坐标系下的位姿”(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(对象在相机坐标系下的位姿)

image.png

到此步勇哥暂时不想再深入研究上面的两个算子的数学原理,我只记了凡是要做坐标系位置的转换、或者已经两个坐标系位置求第三个坐标系位姿就用上面两个算子。



其它的转换还有:

对象在世界坐标系下的位姿

image.png

对象在机器人基坐标系下的位姿

image.png


原理是一样的,这里勇哥就不赘述了。


唯一不同的是”对象在对象坐标系下的位姿“(Pose of object in object coordinates)。

这个肯定是移动与旋转都为0呀。 


image.png



本例子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

版权声明:本文为博主原创文章,转载请附上博文链接!


#转载请注明出处 www.skcircle.com 《少有人走的路》勇哥的工业自动化技术网站。如果需要本贴图片源码等资源,请向勇哥索取。
  • 评论列表:
  •  新远
     发布于 2020-07-23 18:08:37  回复该评论
  • 勇哥,能否做一下旋转中心的例子(手眼标定)
    •  勇哥,很想停止
       发布于 2020-07-25 09:01:47  回复该评论
    • 你想说的是九点标定,还是手眼标定。也许绝大部分人玩的是九点标定吧

发表评论:

◎欢迎参与讨论,请在这里发表您的看法、交流您的观点。

«   2020年8月   »
12
3456789
10111213141516
17181920212223
24252627282930
31
控制面板
您好,欢迎到访网站!
  查看权限
网站分类
标签列表
搜索
最新留言
最近发表
文章归档
友情链接
  • 订阅本站的 RSS 2.0 新闻聚合
  • 扫描加本站机器视觉QQ群,验证答案为:halcon勇哥的机器视觉
  • 扫描加站长微信:站长微信:abc496103864
  • 扫描加站长QQ:
  • 扫描赞赏本站:

Powered By Z-BlogPHP 1.6.0 Valyria

Copyright Your skcircle.com Rights Reserved.

鄂ICP备18008319号


站长QQ:496103864 微信:abc496103864