def projectPixelTo3d(self, left_uv, disparity):
"""
:param left_uv: rectified pixel coordinates
:type left_uv: (u, v)
:param disparity: disparity, in pixels
:type disparity: float
Returns the 3D point (x, y, z) for the given pixel position,
using the cameras' :math:`P` matrices.
This is the inverse of :meth:`project3dToPixel`.
Note that a disparity of zero implies that the 3D point is at infinity.
"""
src = mkmat(4, 1, [left_uv[0], left_uv[1], disparity, 1.0])
dst = cv.CreateMat(4, 1, cv.CV_64FC1)
cv.SetZero(dst)
cv.MatMul(self.Q, src, dst)
x = dst[0, 0]
y = dst[1, 0]
z = dst[2, 0]
w = dst[3, 0]
if w != 0:
return (x / w, y / w, z / w)
else:
return (0.0, 0.0, 0.0)
def display_tracking(img_size, img1, img2, img_flow, homography):
for x in range(0, img_size[0], 100):
for y in range(0, img_size[1], 100):
cv.Circle(img1, (x, y), 3, (0, 255, 0, 0), -1, 8, 0)
point = cv.CreateMat(3, 1, cv.CV_64F)
point[0, 0] = x
point[1, 0] = y
point[2, 0] = 1
newpoint = cv.CreateMat(3, 1, cv.CV_64F)
cv.MatMul(homography, point, newpoint)
cv.Circle(img2, (int(newpoint[0, 0]), int(newpoint[1, 0])), 3,
(0, 255, 0, 0), -1, 8, 0)
cv.Line(img_flow, (x, y),
(int(newpoint[0, 0]), int(newpoint[1, 0])),
cv.CV_RGB(255, 0, 0), 2)
cv.Rectangle(img_flow, (0, 0), (150, 25), cv.CV_RGB(0, 0, 0), thickness=-1)
cv.PutText(
img_flow, "Good?: " + str(isHomographyGood(homography)), (0, 20),
cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.75, 0.75, thickness=2),
cv.CV_RGB(255, 255, 255))
cv.NamedWindow("Image1", 0)
cv.NamedWindow("Image2", 0)
cv.NamedWindow("Flow", 0)
cv.ResizeWindow("Image1", int(0.5 * img_size[0]), int(0.5 * img_size[1]))
cv.ResizeWindow("Image2", int(0.5 * img_size[0]), int(0.5 * img_size[1]))
cv.ResizeWindow("Flow", int(0.5 * img_size[0]), int(0.5 * img_size[1]))
cv.ShowImage("Image1", img1)
cv.ShowImage("Image2", img2)
cv.ShowImage("Flow", img_flow)
cv.WaitKey(0)
def project3dToPixel(self, point):
"""
:param point: 3D point
:type point: (x, y, z)
Returns the rectified pixel coordinates (u, v) of the 3D point,
using the camera :math:`P` matrix.
This is the inverse of :meth:`projectPixelTo3dRay`.
"""
src = mkmat(4, 1, [point[0], point[1], point[2], 1.0])
dst = cv.CreateMat(3, 1, cv.CV_64FC1)
cv.MatMul(self.P, src, dst)
x = dst[0, 0]
y = dst[1, 0]
w = dst[2, 0]
if w != 0:
return (x / w, y / w)
else:
return (float('nan'), float('nan'))