def __init__(self):
"""!
@brief Constructs a new instance.
"""
self.VideoFrame = np.array([])
self.DepthFrameRaw = np.array([]).astype(np.uint16)
""" Extra arrays for colormaping the depth image"""
self.DepthFrameHSV = np.zeros((480, 640, 3)).astype(np.uint8)
self.DepthFrameRGB = np.array([])
"""initialize kinect & turn off auto gain and whitebalance"""
freenect.sync_get_video_with_res(resolution=freenect.RESOLUTION_HIGH)
# print(freenect.sync_set_autoexposure(False))
freenect.sync_set_autoexposure(False)
# print(freenect.sync_set_whitebalance(False))
freenect.sync_set_whitebalance(False)
"""check depth returns a frame, and flag kinectConnected"""
if (freenect.sync_get_depth_with_res(
format=freenect.DEPTH_11BIT) == None):
self.kinectConnected = False
else:
self.kinectConnected = True
# mouse clicks & calibration variables
self.depth2rgb_affine = np.float32([[1, 0, 0], [0, 1, 0]])
self.kinectCalibrated = False
self.last_click = np.array([0, 0])
self.new_click = False
self.rgb_click_points = np.zeros((5, 2), int)
self.depth_click_points = np.zeros((5, 2), int)
""" block info """
self.block_contours = np.array([])
self.block_detections = np.array([])
def __init__(self):
self.currentVideoFrame = np.array([])
self.currentDepthFrame = np.array([])
self.previousDepthFrame = np.array([])
self.currentHiResFrame = np.array([])
self.workspacemask = np.array([])
freenect.sync_set_autoexposure(False)
freenect.sync_set_whitebalance(False)
if (freenect.sync_get_depth() == None):
self.kinectConnected = False
else:
self.kinectConnected = True
# mouse clicks & calibration variables
self.depth2rgb_affine = np.float32([[1, 0, 0], [0, 1, 0]])
self.workcamera_affine = np.float32([[1, 0, 0], [0, 1, 0]])
self.work_camera_extrinsic_inv = np.array([[0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0,
1]]).astype(np.float32)
self.kinectCalibrated = False
self.last_click = np.array([0, 0])
self.new_click = False
self.rgb_click_points = np.zeros((5, 2), int)
self.depth_click_points = np.zeros((5, 2), int)
self.rotation_matrix = np.zeros((3, 3), float)
self.translation_matrix = np.zeros((3, 1), float)
self.intrinsic_matrix = np.zeros((3, 3), float)
self.intrinsic_matrix_inverse = np.zeros((3, 3), float)
self.corners_rgb = []
self.corners_depth = []
self.Z_c = 0.0
""" Extra arrays for colormaping the depth image"""
self.DepthHSV = np.zeros((480, 640, 3)).astype(np.uint8)
self.DepthH = np.zeros((480, 640, 3)).astype(np.uint8)
self.DepthS = np.zeros((480, 640, 3)).astype(np.uint8)
self.DepthV = np.zeros((480, 640, 3)).astype(np.uint8)
self.DepthCM = np.array([])
# Block Depth Isolation
self.DepthHSVThreshold = np.zeros((480, 640, 3)).astype(np.uint8)
self.DepthCMThreshold = np.array([])
self.BlockMask = np.zeros((480, 640, 1)).astype(np.uint8)
self.roi = np.zeros((480, 640, 3)).astype(np.uint8)
""" block info """
self.block_contours = []
self.block_coordinates = []
self.block_coordinates_raw = []
self.block_orientations = []
self.box_corners = []
self.blocks = {}
"""Time variables"""
self.currentTime = 0.0
self.previousTime = 0.0
def __init__(self):
"""!
@brief Constructs a new instance.
"""
self.VideoFrame = np.array([])
self.VideoFrameHSV = np.array([])
self.DepthFrameRaw = np.array([]).astype(np.uint16)
""" Extra arrays for colormapping the depth image"""
self.DepthFrameHSV = np.zeros((480, 640, 3)).astype(np.uint8)
self.DepthFrameRGB = np.array([])
self.DepthFrameFiltered = np.array([])
"""initialize kinect & turn off auto gain and whitebalance"""
freenect.sync_get_video_with_res(resolution=freenect.RESOLUTION_MEDIUM)
# print(freenect.sync_set_autoexposure(False))
freenect.sync_set_autoexposure(False)
# print(freenect.sync_set_whitebalance(False))
freenect.sync_set_whitebalance(False)
"""check depth returns a frame, and flag kinectConnected"""
if (freenect.sync_get_depth_with_res(
format=freenect.DEPTH_11BIT) == None):
self.kinectConnected = False
else:
self.kinectConnected = True
self.kinectCalibrated = False
# mouse clicks & calibration variables
# self.depth2rgb_affine = np.float32([[1,0,0],[0,1,0]]) #no transform
# self.depth2rgb_affine = np.float32([[ 9.28726252E-1, -1.14277108E-2, -2.06562788],
# [ 6.51754476E-3, 9.21278359E-1, 4.02982221E+1]]) # older version
self.depth2rgb_affine = np.float32(
[[9.21074557E-1, -9.91213238E-3, -2.15895387E-1],
[3.72252283E-3, 9.19210560E-1,
4.14502181E+1]]) # determined from test_kinect
""" inverse extrinsic matrix """
self.loadCameraCalibration(
"/home/student/armlab-w20/util/calibration.cfg")
self.getWorkspaceBoundary()
self.last_click = np.array([0, 0])
self.last_rclick = np.array([0, 0])
self.new_click = False
self.new_rclick = False
self.rgb_click_points = np.zeros((5, 2), np.float32)
self.depth_click_points = np.zeros((5, 2), int)
""" block info """
self.block_contours = np.array([])
self.block_detections = np.array([])
self.blocks = []
color.initColors()
def toggleExposure(self, state):
"""!
@brief Toggle auto exposure
@param state False turns off auto exposure True turns it on
"""
if state == False:
freenect.sync_get_video_with_res(
resolution=freenect.RESOLUTION_MEDIUM)
# print(freenect.sync_set_autoexposure(False))
freenect.sync_set_autoexposure(False)
# print(freenect.sync_set_whitebalance(False))
freenect.sync_set_whitebalance(False)
else:
freenect.sync_get_video_with_res(
resolution=freenect.RESOLUTION_MEDIUM)
# print(freenect.sync_set_autoexposure(True))
freenect.sync_set_autoexposure(True)
# print(freenect.sync_set_whitebalance(True))
freenect.sync_set_whitebalance(True)
from apriltag import apriltag
cv2.namedWindow('Video')
print('Press ESC in window to stop')
def get_video():
return frame_convert2.video_cv(
freenect.sync_get_video_with_res(
resolution=freenect.RESOLUTION_HIGH)[0])
freenect.init()
freenect.sync_get_video_with_res(resolution=freenect.RESOLUTION_HIGH)
freenect.sync_set_autoexposure(False)
freenect.sync_set_whitebalance(False)
detector = apriltag("tag36h11", threads=4, decimate=2.0)
object_points = np.array(
[[-0.1, -0.1, 0.0], [0.1, -0.1, 0.0], [0.1, 0.1, 0.0], [-0.1, 0.1, 0.0]],
dtype="double")
camera_matrix = np.array([[1.09194704e+03, 0.00000000e+00, 6.79986322e+02],
[0.00000000e+00, 1.09300427e+03, 5.09471427e+02],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]],
dtype="double")
dist_coeffs = np.array(
[0.17099743, -0.24604911, 0.00678919, 0.01108217, 0.02124964])
#dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
while 1: