halcon三维点云数据处理(五)创建代表工具和机器人底座的3D模型

目录

  • [一、gen_robot_tool_and_base_object_model_3d 函数调用](#一、gen_robot_tool_and_base_object_model_3d 函数调用)
  • [二、gen_arrow_object_model_3d 函数调用](#二、gen_arrow_object_model_3d 函数调用)

首先说明一下这部分代码在find_box_3d这个例程中,非常好用的一个坐标系生成函数。

一、gen_robot_tool_and_base_object_model_3d 函数调用

csharp 复制代码
RobotToolSize := 0.03
gen_robot_tool_and_base_object_model_3d (RobotToolSize * 0.05, RobotToolSize, OM3DToolOrigin, OM3DBase)
csharp 复制代码
* This procedure creates 3D models that represent the tool and the base
* of the robot.
* 
if (ArrowThickness <= 0)
    throw ('ArrowThickness should be > 0')
endif
if (ArrowLength <= 0)
    throw ('ArrowLength should be > 0')
endif
create_pose (0, 0, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', IdentityPose)
* 
* 3D model for the tool.
create_pose (ArrowLength, 0, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransXPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransXPose, OM3DToolXOrigin)
create_pose (0, ArrowLength, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransYPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransYPose, OM3DToolYOrigin)
create_pose (0, 0, ArrowLength, 0, 0, 0, 'Rp+T', 'gba', 'point', TransZPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransZPose, OM3DToolZOrigin)
OM3DToolOrigin := [OM3DToolXOrigin,OM3DToolYOrigin,OM3DToolZOrigin]
* 
* 3D model for the base.
FactorVisBase := ArrowThickness * 10
gen_box_object_model_3d (IdentityPose, FactorVisBase * 1.5, FactorVisBase * 1.5, FactorVisBase / 12.0, OM3DBasePlate)
create_pose (ArrowLength, 0, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransXPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransXPose, OM3DBaseX)
create_pose (0, ArrowLength, 0, 0, 0, 0, 'Rp+T', 'gba', 'point', TransYPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransYPose, OM3DBaseY)
create_pose (0, 0, ArrowLength, 0, 0, 0, 'Rp+T', 'gba', 'point', TransZPose)
gen_arrow_object_model_3d (ArrowThickness, IdentityPose, TransZPose, OM3DBaseZ)
OM3DBase := [OM3DBaseX,OM3DBaseY,OM3DBaseZ,OM3DBasePlate]
return ()

二、gen_arrow_object_model_3d 函数调用

这个函数绘制一个箭头,从ArrowStart点开始,到ArrowEnd点结束。

csharp 复制代码
* 
* This procedure draws an arrow that starts at the point ArrowStart and ends at ArrowEnd.
* 
* Get parameters.
DirectionVector := ArrowEnd[0:2] - ArrowStart[0:2]
ArrowLength := sqrt(DirectionVector[0] * DirectionVector[0] + DirectionVector[1] * DirectionVector[1] + DirectionVector[2] * DirectionVector[2])
ConeRadius := 2.0 * ArrowThickness
ConeLength := min([2.0 * ConeRadius,ArrowLength * 0.9])
CylinderLength := ArrowLength - ConeLength
* 
* Create cone.
pi := rad(180)
X := 0
Y := 0
Z := CylinderLength + ConeLength
for Index := 0 to 2 * pi by 0.1
    X := [X,ConeRadius * cos(Index)]
    Y := [Y,ConeRadius * sin(Index)]
    Z := [Z,CylinderLength]
endfor
gen_object_model_3d_from_points (X, Y, Z, OM3DConeTmp)
convex_hull_object_model_3d (OM3DConeTmp, OM3DCone)
clear_object_model_3d (OM3DConeTmp)
* 
* Create cylinder.
X := []
Y := []
for Index := 0 to 2 * pi by 0.1
    X := [X,ArrowThickness * cos(Index)]
    Y := [Y,ArrowThickness * sin(Index)]
endfor
tuple_gen_const (|Y|, 0, ZZero)
tuple_gen_const (|Y|, CylinderLength, ZTop)
gen_object_model_3d_from_points ([X,X], [Y,Y], [ZZero,ZTop], OM3DCylinderTmp)
convex_hull_object_model_3d (OM3DCylinderTmp, OM3DCylinder)
clear_object_model_3d (OM3DCylinderTmp)
* 
* Union cone and cylinder Create arrow.
union_object_model_3d ([OM3DCone,OM3DCylinder], 'points_surface', OM3DArrowTmp)
clear_object_model_3d (OM3DCone)
clear_object_model_3d (OM3DCylinder)
Scale := CylinderLength / ArrowLength
OriginX := [0, 0, 0]
OriginY := [0, 0, 0]
OriginZ := [0,CylinderLength,ArrowLength]
TargetX := [ArrowStart[0],ArrowStart[0] + Scale * DirectionVector[0],ArrowEnd[0]]
TargetY := [ArrowStart[1],ArrowStart[1] + Scale * DirectionVector[1],ArrowEnd[1]]
TargetZ := [ArrowStart[2],ArrowStart[2] + Scale * DirectionVector[2],ArrowEnd[2]]
vector_to_hom_mat3d ('rigid', OriginX, OriginY, OriginZ, TargetX, TargetY, TargetZ, HomMat3D)
affine_trans_object_model_3d (OM3DArrowTmp, HomMat3D, OM3DArrow)
clear_object_model_3d (OM3DArrowTmp)
return ()

获取参数:

计算方向向量DirectionVector,这是ArrowEnd和ArrowStart的前两个坐标的差。

计算箭头的长度ArrowLength,这是方向向量的长度(即三维空间中的直线距离)。

计算锥体的半径ConeRadius,这是箭头粗细ArrowThickness的两倍。

计算锥体的长度ConeLength,这是箭头长度的90%和两倍锥体半径中的较小值。

计算圆柱体的长度CylinderLength,这是箭头总长度减去锥体长度。
创建锥体:

使用循环生成锥体底面的点,这些点位于一个圆上,圆的半径为ConeRadius。

使用这些点生成一个临时三维对象模型OM3DConeTmp。

使用convex_hull_object_model_3d函数从临时模型生成一个凸包模型OM3DCone,这是最终的锥体模型。

清除临时模型OM3DConeTmp。
创建圆柱体:

使用循环生成圆柱体侧面的点,这些点位于一个圆上,圆的半径为ArrowThickness。

生成两个常数值数组ZZero和ZTop,分别代表圆柱体的底部和顶部在Z轴上的位置。

使用这些点生成一个临时三维对象模型OM3DCylinderTmp。

使用convex_hull_object_model_3d函数从临时模型生成一个凸包模型OM3DCylinder,这是最终的圆柱体模型。

清除临时模型OM3DCylinderTmp。
合并锥体和圆柱体以创建箭头:

使用union_object_model_3d函数将锥体和圆柱体合并成一个临时模型OM3DArrowTmp。

清除原始的锥体和圆柱体模型OM3DCone和OM3DCylinder。
调整箭头的位置和方向:

计算缩放比例Scale,这是圆柱体长度与箭头总长度的比例。

计算原点和目标点的坐标,用于将箭头从原点(临时模型的位置)变换到实际的位置和方向。

使用vector_to_hom_mat3d函数计算一个从原点到目标点的刚体变换矩阵HomMat3D。

使用affine_trans_object_model_3d函数应用这个变换矩阵,将箭头从临时模型OM3DArrowTmp变换到最终的位置和方向,生成最终的箭头模型OM3DArrow。

清除临时模型OM3DArrowTmp。

相关推荐
SunflowerCoder17 小时前
WPF迁移avalonia之图像处理(一)
图像处理·wpf·avalonia
那雨倾城1 天前
PiscCode轨迹跟踪Mediapipe + OpenCV进阶:速度估算
图像处理·人工智能·python·opencv·计算机视觉
LeonDL1682 天前
【通用视觉框架】基于Python+OpenCV+PyQt5开发的视觉框架软件,全套源码,开箱即用
图像处理·人工智能·python·opencv·pyqt5·通用视觉框架软件·机器视觉软件框架
siliconstorm.ai2 天前
开源与闭源的再对决:从Grok到中国力量,AI生态走向何方?
大数据·图像处理·人工智能·语言模型·ai作画·云计算·机器翻译
山烛3 天前
OpenCV 图像轮廓检测
图像处理·人工智能·python·opencv·计算机视觉·轮廓检测
聚客AI3 天前
95%企业AI失败?揭秘LangGraph+OceanBase融合数据层如何破局!
大数据·图像处理·人工智能·机器学习·语言模型·自然语言处理·数据分析
蚊子爱喝水4 天前
高性能多线程 PHP 图像处理库 PHP-VIPS:颠覆你对图像处理的认知
开发语言·图像处理·php
WSSWWWSSW4 天前
Python OpenCV图像处理与深度学习:Python OpenCV性能优化与高效图像处理
图像处理·python·opencv
卡尔曼的BD SLAMer4 天前
计算机视觉与深度学习 | 传统图像处理技术的未来发展前景分析
图像处理·算法·计算机视觉
封奚泽优4 天前
提取动漫图像轮廓并拟合为样条曲线(MATLAB)
图像处理·人工智能·计算机视觉·matlab·deepseek