“9点标定法”
基于2D图像的像素坐标与机器人坐标(XY值),使用vector_to_hom_mat2d直接求解。
针对3D相机而言,vector_to_hom_mat2d算子可以替换为vector_to_hom_mat3d,输入变为3D相机获取的XYZ值,以及机器人坐标的XYZ值
*相机坐标系与机器人坐标系下对应点坐标值
inputImageX:=[1,2,3]
inputImageY:=[1,2,3]
inputImageZ:=[1,1,1]
inputRobotX:=[3,4,5]
inputRobotY:=[3,4,5]
inputRobotZ:=[1,1,1]
vector_to_hom_mat3d ('rigid', inputImageX, inputImageY, inputImageZ, inputRobotX, inputRobotY, inputRobotZ, outputHomMat3D)
*通过变换矩阵 准备计算重投影误差数据
affine_trans_point_3d (outputHomMat3D, inputImageX, inputImageY, inputImageZ, robotPointsX, robotPointsY,robotPointsZ)
*计算重投影误差
err2 := []
for Index := 0 to |robotPointsX|-1 by 1
*空间中点欧氏距离
err2 := [err2, sqrt(pow(robotPointsX[Index]-inputRobotX[Index], 2) + pow(robotPointsY[Index]-robotPointsY[Index],2)) +pow(robotPointsZ[Index]-robotPointsZ[Index],2)]
endfor
outputErrMean := mean(err2)
outputErrMax := max(err2)