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'))