def convert_image(msg, flag = False): ''' Converts ROS image to OpenCV image, then blocks out errant pixels and rectifies. ''' bridge = CvBridge() try: # cv_bridge automatically scales, we need to remove that behavior if bridge.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']: mono16 = bridge.imgmsg_to_cv2(msg, '16UC1') mono8 = numpy.array(numpy.clip(mono16, 0, 255), dtype=numpy.uint8) img = mono8 elif 'FC1' in msg.encoding: # floating point image handling img = bridge.imgmsg_to_cv2(msg, "passthrough") _, max_val, _, _ = cv2.minMaxLoc(img) if max_val > 0: scale = 255.0 / max_val img = (img * scale).astype(numpy.uint8) else: img = img.astype(numpy.uint8) else: img = bridge.imgmsg_to_cv2(msg, "mono8") except CvBridgeError as e: print text_colors.WARNING + 'Warning: Image converted unsuccessfully before processing.' + text_colors.ENDCOLOR raise e # Black out a rectangle in the top left of the image since there are often erroneous pixels there (image conversion error perhaps?) cv2.rectangle(img, (0,0), (30,1), 0, cv2.cv.CV_FILLED) show_image('blacked out', img, flag) return img
class camera: def __init__(self): self.br = CvBridge() self.ir_image_sub = rospy.Subscriber("/kinect2/sd/image_ir",Image,self.ir_callback) # self.rgb_image_sub = rospy.Subscriber("/kinect2/hd/image_color",Image,self.rgb_callback) self.ir_img = None self.rgb_img = None self.rgb_rmat = None self.rgb_tvec = None self.ir_rmat = None self.ir_tvec = None def ir_callback(self,data): try: ir_img = self.br.imgmsg_to_cv2(data, "passthrough") ir_min = np.amin(ir_img) ir_max = np.amax(ir_img) factor = 255.0 / float(ir_max - ir_min) self.ir_img = np.minimum(np.maximum(ir_img - ir_min, 0) * factor, 255.0) self.ir_img = self.ir_img.astype(np.uint8) clahe = cv2.createCLAHE() self.ir_img = clahe.apply(self.ir_img) except CvBridgeError as e: print(e) # print "ir image size: ",self.ir_img.shape ir_ret, ir_corners = cv2.findChessboardCorners(self.ir_img, (x_num,y_num)) cv2.namedWindow('ir_img', cv2.WINDOW_NORMAL) cv2.imshow('ir_img',self.ir_img) cv2.waitKey(5) if ir_ret == True: ir_tempimg = self.ir_img.copy() cv2.cornerSubPix(ir_tempimg,ir_corners,(11,11),(-1,-1),criteria) cv2.drawChessboardCorners(ir_tempimg, (x_num,y_num), ir_corners,ir_ret) # ret, rvec, tvec = cv2.solvePnP(objpoints, corners, mtx, dist, flags = cv2.CV_EPNP) ir_rvec, self.ir_tvec, ir_inliers = cv2.solvePnPRansac(objpoints, ir_corners, depth_mtx, depth_dist) self.ir_rmat, _ = cv2.Rodrigues(ir_rvec) print("The world coordinate system's origin in camera's coordinate system:") print("===ir_camera rvec:") print(ir_rvec) print("===ir_camera rmat:") print(self.ir_rmat) print("===ir_camera tvec:") print(self.ir_tvec) print("ir_camera_shape: ") print(self.ir_img.shape) print("The camera origin in world coordinate system:") print("===camera rmat:") print(self.ir_rmat.T) print("===camera tvec:") print(-np.dot(self.ir_rmat.T, self.ir_tvec)) directory = '/home/chentao/kinect2_calibration' if not os.path.exists(directory): os.makedirs(directory) depth_stream = open("/home/chentao/kinect2_calibration/ir_camera_pose.yaml", "w") data = {'rmat':self.ir_rmat.tolist(), 'tvec':self.ir_tvec.tolist()} yaml.dump(data, depth_stream) cv2.imshow('ir_img',ir_tempimg) cv2.waitKey(5) def rgb_callback(self,data): try: self.rgb_img = self.br.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) gray = cv2.cvtColor(self.rgb_img,cv2.COLOR_BGR2GRAY) rgb_ret, rgb_corners = cv2.findChessboardCorners(gray, (x_num,y_num),None) cv2.namedWindow('rgb_img', cv2.WINDOW_NORMAL) cv2.imshow('rgb_img',self.rgb_img) cv2.waitKey(5) if rgb_ret == True: rgb_tempimg = self.rgb_img.copy() cv2.cornerSubPix(gray,rgb_corners,(5,5),(-1,-1),criteria) cv2.drawChessboardCorners(rgb_tempimg, (x_num,y_num), rgb_corners,rgb_ret) rgb_rvec, self.rgb_tvec, rgb_inliers = cv2.solvePnPRansac(objpoints, rgb_corners, rgb_mtx, rgb_dist) self.rgb_rmat, _ = cv2.Rodrigues(rgb_rvec) print("The world coordinate system's origin in camera's coordinate system:") print("===rgb_camera rvec:") print(rgb_rvec) print("===rgb_camera rmat:") print(self.rgb_rmat) print("===rgb_camera tvec:") print(self.rgb_tvec) print("rgb_camera_shape: ") print(self.rgb_img.shape) print("The camera origin in world coordinate system:") print("===camera rmat:") print(self.rgb_rmat.T) print("===camera tvec:") print(-np.dot(self.rgb_rmat.T, self.rgb_tvec)) directory = '/home/chentao/kinect2_calibration' if not os.path.exists(directory): os.makedirs(directory) rgb_stream = open("/home/chentao/kinect2_calibration/rgb_camera_pose.yaml", "w") data = {'rmat':self.rgb_rmat.tolist(), 'tvec':self.rgb_tvec.tolist()} yaml.dump(data, rgb_stream) cv2.imshow('rgb_img',rgb_tempimg) cv2.waitKey(5) def mkgray(self, msg): """ Convert a message into a 8-bit 1 channel monochrome OpenCV image """ # as cv_bridge automatically scales, we need to remove that behavior # TODO: get a Python API in cv_bridge to check for the image depth. if self.br.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']: mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') mono8 = np.array(np.clip(mono16, 0, 255), dtype=np.uint8) return mono8 elif 'FC1' in msg.encoding: # floating point image handling img = self.br.imgmsg_to_cv2(msg, "passthrough") _, max_val, _, _ = cv2.minMaxLoc(img) if max_val > 0: scale = 255.0 / max_val mono_img = (img * scale).astype(np.uint8) else: mono_img = img.astype(np.uint8) return mono_img else: return self.br.imgmsg_to_cv2(msg, "mono8")
from velodyne_msgs.msg import VelodyneScan rclpy.init(args=None) node = rclpy.create_node('blackfly') # Modify the QoS profile of the publisher '''qos_profile = QoSProfile(depth=10) qos_profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE''' publisher = node.create_publisher(Image, 'blackfly', 1) #qos_profile) br = CvBridge() dtype, n_channels = br.encoding_to_dtype_with_channels('8UC3') class TriggerType: SOFTWARE = 1 HARDWARE = 2 CHOSEN_TRIGGER = TriggerType.SOFTWARE class TriggerSubscriber(Node): def __init__(self): super().__init__('Trigger_Subscriber') self.subscription = self.create_subscription(VelodyneScan, '/velodyne_packets',
class camera: def __init__(self): self.br = CvBridge() # self.ir_image_sub = rospy.Subscriber("/camera/ir/image_raw",Image,self.ir_callback) self.rgb_image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.rgb_callback) self.ir_img = None self.rgb_img = None self.rgb_rmat = None self.rgb_tvec = None self.ir_rmat = None self.ir_tvec = None def isclose(x, y, rtol=1.e-5, atol=1.e-8): return bool(abs(x - y) <= atol + rtol * abs(y)) def euler_angles_from_rotation_matrix(self, R): ''' From a paper by Gregory G. Slabaugh (undated), "Computing Euler angles from a rotation matrix ''' phi = 0.0 #if self.isclose(R[2,0],-1.0): #theta = math.pi/2.0 #psi = math.atan2(R[0,1],R[0,2]) #elif self.isclose(R[2,0],1.0): #theta = -math.pi/2.0 #psi = math.atan2(-R[0,1],-R[0,2]) #else: theta = -math.asin(R[2, 0]) cos_theta = math.cos(theta) psi = math.atan2(R[2, 1] / cos_theta, R[2, 2] / cos_theta) phi = math.atan2(R[1, 0] / cos_theta, R[0, 0] / cos_theta) return psi, theta, phi def ir_callback(self, data): try: self.ir_img = self.mkgray(data) except CvBridgeError as e: print(e) ir_ret, ir_corners = cv2.findChessboardCorners(self.ir_img, (x_num, y_num)) cv2.namedWindow('ir_img', cv2.WINDOW_NORMAL) cv2.imshow('ir_img', self.ir_img) cv2.waitKey(5) if ir_ret == True: ir_tempimg = self.ir_img.copy() cv2.cornerSubPix(ir_tempimg, ir_corners, (11, 11), (-1, -1), criteria) cv2.drawChessboardCorners(ir_tempimg, (x_num, y_num), ir_corners, ir_ret) # ret, rvec, tvec = cv2.solvePnP(objpoints, corners, mtx, dist, flags = cv2.CV_EPNP) ir_rvec, self.ir_tvec, ir_inliers = cv2.solvePnPRansac( objpoints, ir_corners, depth_mtx, depth_dist) self.ir_rmat, _ = cv2.Rodrigues(ir_rvec) print( "The world coordinate system's origin in camera's coordinate system:" ) print("===ir_camera rvec:") print(ir_rvec) print("===ir_camera rmat:") print(self.ir_rmat) print("===ir_camera tvec:") print(self.ir_tvec) print("ir_camera_shape: ") print(self.ir_img.shape) print("The camera origin in world coordinate system:") print("===camera rmat:") print(self.ir_rmat.T) print("===camera tvec:") print(-np.dot(self.ir_rmat.T, self.ir_tvec)) depth_stream = open( "/home/qiang/SWLib/xtion_calib_tutorial/kinect_calibration/ir_camera_pose.yaml", "w") data = { 'rmat': self.ir_rmat.tolist(), 'tvec': self.ir_tvec.tolist() } yaml.dump(data, depth_stream) cv2.imshow('ir_img', ir_tempimg) cv2.waitKey(5) def rgb_callback(self, data): try: self.rgb_img = self.br.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) gray = cv2.cvtColor(self.rgb_img, cv2.COLOR_BGR2GRAY) rgb_ret, rgb_corners = cv2.findChessboardCorners( gray, (x_num, y_num), None) cv2.namedWindow('rgb_img', cv2.WINDOW_NORMAL) cv2.imshow('rgb_img', self.rgb_img) cv2.waitKey(5) if rgb_ret == True: rgb_tempimg = self.rgb_img.copy() cv2.cornerSubPix(gray, rgb_corners, (5, 5), (-1, -1), criteria) cv2.drawChessboardCorners(rgb_tempimg, (x_num, y_num), rgb_corners, rgb_ret) _, rgb_rvec, self.rgb_tvec, rgb_inliers = cv2.solvePnPRansac( objpoints, rgb_corners, rgb_mtx, rgb_dist) self.rgb_rmat, _ = cv2.Rodrigues(rgb_rvec) print( "The world coordinate system's origin in camera's coordinate system:" ) print("===rgb_camera rvec:") print(rgb_rvec) print("===rgb_camera rmat:") print(self.rgb_rmat) type(self.rgb_rmat) psi, theta, phi = self.euler_angles_from_rotation_matrix( self.rgb_rmat) print("===rgb_camera rpy: %f,%f,%f", psi, theta, phi) print("===rgb_camera tvec:") print(self.rgb_tvec) print("rgb_camera_shape: ") print(self.rgb_img.shape) print("The camera origin in world coordinate system:") print("===camera rmat:") print(self.rgb_rmat.T) print("===camera tvec:") print(-np.dot(self.rgb_rmat.T, self.rgb_tvec)) rgb_stream = open( "/home/qiang/SWLib/xtion_calib_tutorial/kinect_calibration/rgb_camera_pose.yaml", "w") data = { 'rmat': self.rgb_rmat.tolist(), 'tvec': self.rgb_tvec.tolist() } yaml.dump(data, rgb_stream) cv2.imshow('rgb_img', rgb_tempimg) cv2.waitKey(5) def mkgray(self, msg): """ Convert a message into a 8-bit 1 channel monochrome OpenCV image """ # as cv_bridge automatically scales, we need to remove that behavior # TODO: get a Python API in cv_bridge to check for the image depth. if self.br.encoding_to_dtype_with_channels( msg.encoding)[0] in ['uint16', 'int16']: mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') mono8 = np.array(np.clip(mono16, 0, 255), dtype=np.uint8) return mono8 elif 'FC1' in msg.encoding: # floating point image handling img = self.br.imgmsg_to_cv2(msg, "passthrough") _, max_val, _, _ = cv2.minMaxLoc(img) if max_val > 0: scale = 255.0 / max_val mono_img = (img * scale).astype(np.uint8) else: mono_img = img.astype(np.uint8) return mono_img else: return self.br.imgmsg_to_cv2(msg, "mono8")
class registration: def __init__(self): self.br = CvBridge() #self.depth_image_sub = rospy.Subscriber("/camera/depth/image_raw",Image,self.depth_callback) # As you cannot get rgb and ir images simultaneously from kinect, you will need to get the coordinates # of the corners separately on rgb image and ir image # To do this, uncomment the following two lines one at a time #self.rgb_image_sub = rospy.Subscriber("/camera/rgb/image_raw",Image,self.rgb_calib_callback) #self.ir_image_sub = rospy.Subscriber("/camera/ir/image_raw",Image,self.ir_calib_callback) self.ir_img = None self.rgb_img = None self.rgb_rmat = None self.rgb_tvec = None self.ir_rmat = None self.ir_tvec = None self.ir_to_rgb_rmat = None self.ir_to_rgb_tvec = None self.depth_image = None self.rgb_image = None self.rgb_corners = None self.ir_corners = None self.load_intrinsics() self.load_corners() def depth_callback(self,data): try: self.depth_image= self.br.imgmsg_to_cv2(data) except CvBridgeError as e: print(e) def ir_calib_callback(self,data): try: self.ir_img = self.mkgray(data) except CvBridgeError as e: print(e) ir_ret, ir_corners = cv2.findChessboardCorners(self.ir_img, (y_num,x_num)) cv2.imshow('ir_img',self.ir_img) cv2.waitKey(5) if ir_ret == True: ir_tempimg = self.ir_img.copy() cv2.cornerSubPix(ir_tempimg,ir_corners,(11,11),(-1,-1),criteria) cv2.drawChessboardCorners(ir_tempimg, (y_num,x_num), ir_corners,ir_ret) # ret, rvec, tvec = cv2.solvePnP(objpoints, corners, mtx, dist, flags = cv2.CV_EPNP) depth_stream = open("/home/chentao/kinect_calibration/ir_camera_corners.yaml", "w") data = {'corners':ir_corners.tolist()} yaml.dump(data, depth_stream) cv2.imshow('ir_img',ir_tempimg) cv2.waitKey(5) def rgb_calib_callback(self,data): try: self.rgb_img = self.br.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) gray = cv2.cvtColor(self.rgb_img,cv2.COLOR_BGR2GRAY) rgb_ret, rgb_corners = cv2.findChessboardCorners(gray, (y_num,x_num),None) cv2.imshow('rgb_img',self.rgb_img) cv2.waitKey(5) if rgb_ret == True: rgb_tempimg = self.rgb_img.copy() cv2.cornerSubPix(gray,rgb_corners,(11,11),(-1,-1),criteria) cv2.drawChessboardCorners(rgb_tempimg, (y_num,x_num), rgb_corners,rgb_ret) rgb_stream = open("/home/chentao/kinect_calibration/rgb_camera_corners.yaml", "w") data = {'corners':rgb_corners.tolist()} yaml.dump(data, rgb_stream) cv2.imshow('rgb_img',rgb_tempimg) cv2.waitKey(5) def load_intrinsics(self): ir_stream = open("/home/chentao/kinect_calibration/depth_1504270110.yaml", "r") ir_doc = yaml.load(ir_stream) self.ir_mtx = np.array(ir_doc['camera_matrix']['data']).reshape(3,3) self.ir_dist = np.array(ir_doc['distortion_coefficients']['data']) ir_stream.close() rgb_stream = open("/home/chentao/kinect_calibration/rgb_1504270110.yaml", "r") rgb_doc = yaml.load(rgb_stream) self.rgb_mtx = np.array(rgb_doc['camera_matrix']['data']).reshape(3,3) self.rgb_dist = np.array(rgb_doc['distortion_coefficients']['data']) rgb_stream.close() def load_corners(self): ir_stream = open("/home/chentao/kinect_calibration/ir_camera_corners.yaml", "r") ir_doc = yaml.load(ir_stream) self.ir_corners = np.array(ir_doc['corners']).reshape(-1,2).astype('float32') ir_stream.close() rgb_stream = open("/home/chentao/kinect_calibration/rgb_camera_corners.yaml", "r") rgb_doc = yaml.load(rgb_stream) self.rgb_corners = np.array(rgb_doc['corners']).reshape(-1,2) self.rgb_corners = self.rgb_corners.astype('float32') rgb_stream.close() def ir_to_rgb(self): if self.rgb_corners != None and self.ir_corners != None: _,_,_,_,_,R,T,E,F = cv2.stereoCalibrate([objpoints], [self.ir_corners], [self.rgb_corners],(480, 640), self.ir_mtx, self.ir_dist, self.rgb_mtx, self.rgb_dist, flags = cv2.cv.CV_CALIB_FIX_INTRINSIC) print "R:" print R print "T:" print T def register_depth_to_rgb(self): if self.depth_image == None or self.rgb_image == None: return self.registered_depth = np.zeros(self.depth_image.shape) self.registered_rgb = np.zeros(self.rgb_image.shape) for row_i in range(self.depth_image.shape[0]): for column_j in range(self.depth_image.shape[1]): depth_pix_point = np.array([column_j, row_i, self.depth_image[row_i, column_j]]) #(x,y,z) depth_coord_point = np.dot(np.linalg.inv(self.depth_mtx), depth_pix_point) rgb_coord_point = np.dot(self.ir_to_rgb_rmat, depth_coord_point).reshape(3,1) + self.ir_to_rgb_tvec rgb_pix_point = np.dot(self.rgb_mtx, rgb_coord_point) rgb_x = int(rgb_pix_point[0] / float(rgb_pix_point[2])) rgb_y = int(rgb_pix_point[1] / float(rgb_pix_point[2])) rgb_x = np.clip(rgb_x, 0, self.rgb_image.shape[1] - 1) rgb_y = np.clip(rgb_y, 0, self.rgb_image.shape[0] - 1) self.registered_rgb[row_i, column_j, :] = self.rgb_image[rgb_y, rgb_x, :] cv2.imshow("Registered_rgb", self.registered_rgb) cv2.imshow("RGB", self.rgb_image) cv2.imshow("Depth",self.depth_image) cv2.waitKey(10) def get_ir_to_rgb_rmat(self): if self.ir_to_rgb_rmat != None: return self.ir_to_rgb_rmat def get_ir_to_rgb_tvec(self): if self.ir_to_rgb_tvec != None: return self.ir_to_rgb_tvec def mkgray(self, msg): """ Convert a message into a 8-bit 1 channel monochrome OpenCV image """ # as cv_bridge automatically scales, we need to remove that behavior # TODO: get a Python API in cv_bridge to check for the image depth. if self.br.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']: mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') mono8 = np.array(np.clip(mono16, 0, 255), dtype=np.uint8) return mono8 elif 'FC1' in msg.encoding: # floating point image handling img = self.br.imgmsg_to_cv2(msg, "passthrough") _, max_val, _, _ = cv2.minMaxLoc(img) if max_val > 0: scale = 255.0 / max_val mono_img = (img * scale).astype(np.uint8) else: mono_img = img.astype(np.uint8) return mono_img else: return self.br.imgmsg_to_cv2(msg, "mono8")