三维点云 拼接的 代码 与 运行效果 -某培训班内部 视频

ObjectSOut := []
* PartNum:=2
*三维彩色显示的参数
GenParamNames := ['lut' ,'intensity' ,'light_position', \
'disp_pose', 'alpha']
GenParamValues := ['color1' ,'coord_z' ,'0.0 0.0 -0.3 1.0', 'true', 1]
stop()
*图片存放地址
FileAddress:='./data/Part'+PartNum+'/SecondOrigin'
*拼接的三张点云图
for I:=1 to 3 by 1
try
if(I==7)
continue
endif
*读取深度图 xyz 三通道的 图像
read_image(Image, (FileAddress+'/Scene/Depth'+I))
*读取拍照点坐标的tuple 文件
*此时坐标是 直角坐标系 或者 法兰盘坐标
read_tuple(FileAddress+'/Pose/pose'+I+'.dat', CapturePose1)
CapturePose2:=CapturePose1[0:5]
CapturePose2[6]:=2
*根据 工具坐标末端的偏离值 ， 换算到 实际的拍照点坐标
ZJToolPose:=[-64.5,-1.506,534.301,0.911,-26.629,-3.594,2]
*两个pose 相乘
pose_compose(CapturePose2,ZJToolPose, CapturePose)
decompose3(Image,xImg, yImg, zImg)
*图像换算到点云 效果
xyz_to_object_model_3d(xImg, yImg, zImg, Object3D1)
*根据 z坐标的区间 筛选出 需要的三维点云
select_points_object_model_3d(Object3D1, 'point_coord_z',200, 800, ObjectX)
*读取标定文件--眼在手上的标定文件
read_pose ('./data/Calib/ToolInCamPose.dat', ToolInCamPose)
*将tool与相机的关系 ，逆变换为 相机在工具的位置关系
pose_invert (ToolInCamPose, CamInToolPose)
*读取拍照点，拍照点 转变 旋转轴状态 2转为0 ，欧拉角格式转换
convert_pose_type(CapturePose, 'Rp+T', 'gba', 'point', CapturePose1)
* 1.原始点云仿射
* OM3DScene--选取点云
select_points_object_model_3d (ObjectX, 'point_coord_z', -40, 3780, OM3DScene1)
*两次仿射 得到 机器人坐标系 的点云 效果
rigid_trans_object_model_3d (OM3DScene1, CamInToolPose, AffineTranA1)
rigid_trans_object_model_3d (AffineTranA1,CapturePose1, Trans3D4)
*循环三次 ， 合并到 数组集合
ObjectSOut := [ObjectSOut,Trans3D4]
* visualize_object_model_3d(WindowHandle,ObjectX, [], [], GenParamNames,GenParamValues, [], [], [], PoseOut)
catch (Exception)
endtry
endfor
select_points_object_model_3d (ObjectSOut, 'point_coord_z', 400, 1680, OM3DScene1)
pra_name:=['color_0','color_1',\
'point_size_0','disp_pose']
pra_value:=['magenta','green',1.0,'true']
*显示三维合并效果
visualize_object_model_3d(WindowHandle,ObjectSOut, [], [], pra_name,pra_value, [], [], [], PoseOut)
return ()