OpenCV:如何通过solvePnP估算距离?

时间:2015-10-30 13:16:15

标签: opencv computer-vision pose-estimation

我试着估计相机与我用于校准的棋盘之间的距离。但是我对棋盘上的点的Z值(993 - 1021 mm)有很大的不同。棋盘与摄像机平行,因此我预计摄像机中心与棋盘平面上任意点之间的Z值应相同。

我做错了什么?

代码:

import numpy
import cv2

#from cv2.calibrateCamera.
imagePoints = numpy.array([[185.38619995, 88.96965027],
                           [215.52053833, 89.59921265],
                           [245.42134094, 90.34884644],
                           [275.23474121, 90.67272186],
                           [304.63644409, 91.27327728],
                           [333.90859985, 91.75786591],
                           [185.34030151, 118.99983978],
                           [215.52850342, 119.46482849],
                           [245.514328, 119.89020538],
                           [275.03085327, 120.42667389],
                           [304.64593506, 120.67990112],
                           [333.73968506, 121.14579773],
                           [185.34428406, 148.93850708],
                           [215.58241272, 149.32409668],
                           [245.4022522, 149.86160278],
                           [275.17852783, 149.81486511],
                           [304.5428772, 150.28009033],
                           [333.79421997, 150.35099792],
                           [185.18565369, 178.83206177],
                           [215.445755, 179.09082031],
                           [245.40261841, 179.29736328],
                           [275.07699585, 179.61761475],
                           [304.49923706, 179.56680298],
                           [333.78756714, 179.81796265],
                           [185.00271606, 208.8531189],
                           [215.40895081, 208.91560364],
                           [245.23666382, 209.10614014],
                           [275.06970215, 209.07598877],
                           [304.59561157, 209.13166809],
                           [333.90438843, 209.09313965],
                           [184.86560059, 238.93251038],
                           [215.23825073, 238.76199341],
                           [245.25619507, 238.73907471],
                           [274.97564697, 238.73266602],
                           [304.59933472, 238.72055054],
                           [333.96078491, 238.68598938],
                           [184.71801758, 269.20956421],
                           [215.05563354, 268.91809082],
                           [245.1418457, 268.70123291],
                           [274.9392395, 268.59295654],
                           [304.65649414, 268.48577881],
                           [333.98513794, 268.29562378],
                           [184.43510437, 299.72283936],
                           [214.71133423, 299.52212524],
                           [244.97724915, 298.88729858],
                           [275.03652954, 298.61529541],
                           [304.74026489, 298.40454102],
                           [334.23959351, 298.21008301]])

objectPoints = numpy.array([[0., 0., 0.],
                           [0.083, 0., 0.],
                           [0.16599999, 0., 0.],
                           [0.24899998, 0., 0.],
                           [0.33199999, 0., 0.],
                           [0.41499999, 0., 0.],
                           [0., 0.083, 0.],
                           [0.083, 0.083, 0.],
                           [0.16599999, 0.083, 0.],
                           [0.24899998, 0.083, 0.],
                           [0.33199999, 0.083, 0.],
                           [0.41499999, 0.083, 0.],
                           [0., 0.16599999, 0.],
                           [0.083, 0.16599999, 0.],
                           [0.16599999, 0.16599999, 0.],
                           [0.24899998, 0.16599999, 0.],
                           [0.33199999, 0.16599999, 0.],
                           [0.41499999, 0.16599999, 0.],
                           [0., 0.24899998, 0.],
                           [0.083, 0.24899998, 0.],
                           [0.16599999, 0.24899998, 0.],
                           [0.24899998, 0.24899998, 0.],
                           [0.33199999, 0.24899998, 0.],
                           [0.41499999, 0.24899998, 0.],
                           [0., 0.33199999, 0.],
                           [0.083, 0.33199999, 0.],
                           [0.16599999, 0.33199999, 0.],
                           [0.24899998, 0.33199999, 0.],
                           [0.33199999, 0.33199999, 0.],
                           [0.41499999, 0.33199999, 0.],
                           [0., 0.41499999, 0.],
                           [0.083, 0.41499999, 0.],
                           [0.16599999, 0.41499999, 0.],
                           [0.24899998, 0.41499999, 0.],
                           [0.33199999, 0.41499999, 0.],
                           [0.41499999, 0.41499999, 0.],
                           [0., 0.49799997, 0.],
                           [0.083, 0.49799997, 0.],
                           [0.16599999, 0.49799997, 0.],
                           [0.24899998, 0.49799997, 0.],
                           [0.33199999, 0.49799997, 0.],
                           [0.41499999, 0.49799997, 0.],
                           [0., 0.58099997, 0.],
                           [0.083, 0.58099997, 0.],
                           [0.16599999, 0.58099997, 0.],
                           [0.24899998, 0.58099997, 0.],
                           [0.33199999, 0.58099997, 0.],
                           [0.41499999, 0.58099997, 0.]])

cameraMatrix = numpy.array([[364.21551784, 0., 253.82602981],
                            [0., 363.51882052, 206.4493302],
                            [0., 0., 1.]])

distortionCoefficient = numpy.array([[0.09360905, -0.37480762, 0.00147107, -0.00132464, 0.25238534]])

Z = numpy.zeros((48))
retval,R, T = cv2.solvePnP(objectPoints,imagePoints,cameraMatrix,distortionCoefficient) #from chessboard coordinate space to camera coordinate space
R,jacobian = cv2.Rodrigues(R) #from R-vector to R-matrix
for i in range(0,48):
    point = numpy.dot(objectPoints[i],R) + numpy.matrix.transpose(T)
    Z[i] = point[0,2] * 1000 #Z-value to mm

print(Z.max() - Z.min())
print(Z)

Image with the chessboard

1 个答案:

答案 0 :(得分:0)

好吧,这似乎是我的错。可能董事会并不是真的与相机平行,尽管我试图把它弄好。如果我们查看计算Z坐标,我们可以看到Z坐标从右下角到左上角线性减小。

print(numpy.round(Z.reshape(8,6)))

[[ 1013.  1009.  1005.  1001.   997.   993.]
 [ 1014.  1010.  1006.  1002.   998.   994.]
 [ 1015.  1011.  1007.  1003.  1000.   996.]
 [ 1016.  1012.  1009.  1005.  1001.   997.]
 [ 1018.  1014.  1010.  1006.  1002.   998.]
 [ 1019.  1015.  1011.  1007.  1003.   999.]
 [ 1020.  1016.  1012.  1008.  1004.  1001.]
 [ 1021.  1017.  1013.  1010.  1006.  1002.]]