def getImage(self, idx): topic, data, stamp = self.bag._read_message(self.index[idx].position) if self.perform_synchronization: timestamp = acv.Time( self.timestamp_corrector.getLocalTime( data.header.stamp.to_sec())) else: timestamp = acv.Time(data.header.stamp.secs, data.header.stamp.nsecs) if data._type == 'mv_cameras/ImageSnappyMsg': if self.uncompress is None: from snappy import uncompress self.uncompress = uncompress img_data = np.reshape(self.uncompress( np.fromstring(data.data, dtype='uint8')), (data.height, data.width), order="C") elif data.encoding == "16UC1" or data.encoding == "mono16": image_16u = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = (image_16u / 256).astype("uint8") elif data.encoding == "8UC1" or data.encoding == "mono8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) elif data.encoding == "8UC3" or data.encoding == "rgb8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BGR2GRAY) elif data.encoding == "8UC4" or data.encoding == "bgra8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BGRA2GRAY) else: raise RuntimeError( "Unsupported Image format '{}' (Supported are: 16UC1 / mono16, 8UC1 / mono8, 8UC3 / rgb8, 8UC4 / bgra8 and ImageSnappyMsg)" .format(data.encoding)) return (timestamp, img_data)
def getMessage(self,idx): topic, data, stamp = self.bag._read_message(self.index[idx].position) if self.perform_synchronization: timestamp = acv.Time(self.timestamp_corrector.getLocalTime(data.header.stamp.to_sec())) else: timestamp = acv.Time( data.header.stamp.secs, data.header.stamp.nsecs ) omega = np.array( [data.angular_velocity.x, data.angular_velocity.y, data.angular_velocity.z]) alpha = np.array( [data.linear_acceleration.x, data.linear_acceleration.y, data.linear_acceleration.z] ) return (timestamp, omega, alpha)
def getData(self, idx): topic, data, stamp = self.bag._read_message(self.index[idx].position) if self.perform_synchronization: timestamp = acv.Time( self.timestamp_corrector.getLocalTime( data.header.stamp.to_sec())) else: timestamp = acv.Time(data.header.stamp.secs, data.header.stamp.nsecs) data = self.parser.parseData(data) return timestamp, data
def loadImuData(self): print "Reading IMU data ({0})".format(self.dataset.topic) # prepare progess bar iProgress = sm.Progress2(self.dataset.numMessages()) iProgress.sample() Rgyro = np.eye( 3) * self.gyroUncertaintyDiscrete * self.gyroUncertaintyDiscrete Raccel = np.eye( 3) * self.accelUncertaintyDiscrete * self.accelUncertaintyDiscrete # Now read the imu measurements. imu = [] for timestamp, omega, alpha in self.dataset: timestamp = acv.Time(timestamp.toSec()) imu.append( self.ImuMeasurement(timestamp, omega, alpha, Rgyro, Raccel)) iProgress.sample() self.imuData = imu if len(self.imuData) > 1: print "\r Read %d imu readings over %.1f seconds " % ( len(imu), imu[-1].stamp.toSec() - imu[0].stamp.toSec()) else: sm.logFatal( "Could not find any IMU messages. Please check the dataset.") sys.exit(-1)
def findCameraTimespan(self): tStart = acv.Time(0.0) tEnd = acv.Time(0.0) for cam in self.camList: if len(cam.targetObservations) > 0: tStartCam = cam.targetObservations[0].time() tEndCam = cam.targetObservations[-1].time() if tStart.toSec() > tStartCam.toSec(): tStart = tStartCam if tEndCam.toSec() > tEnd.toSec(): tEnd = tEndCam self.timeStart = tStart self.timeStart = tEnd
def getImage(self,idx): topic, data, stamp = self.bag._read_message(self.index[idx].position) ts = acv.Time( data.header.stamp.secs, data.header.stamp.nsecs ) if data._type == 'mv_cameras/ImageSnappyMsg': if self.uncompress is None: from snappy import uncompress self.uncompress = uncompress img_data = np.reshape(self.uncompress(np.fromstring(data.data, dtype='uint8')),(data.height,data.width), order="C") else: img_data = np.array(self.CVB.imgmsg_to_cv2(data)) return (ts, img_data)
def getImage(self, idx): topic, data, stamp = self.bag._read_message(self.index[idx].position) if self.perform_synchronization: timestamp = acv.Time( self.timestamp_corrector.getLocalTime( data.header.stamp.to_sec())) else: timestamp = acv.Time(data.header.stamp.secs, data.header.stamp.nsecs) if data._type == 'mv_cameras/ImageSnappyMsg': if self.uncompress is None: from snappy import uncompress self.uncompress = uncompress img_data = np.reshape(self.uncompress(np.fromstring(data.data, dtype='uint8')),\ (data.height,data.width), order="C") else: img_data = np.array(self.CVB.imgmsg_to_cv2(data)) if img_data.ndim == 3: img_data = cv2.cvtColor(img_data, cv2.COLOR_BGR2GRAY) return (timestamp, img_data)
def getMessage(self, idx): topic, data, stamp = self.bag._read_message(self.index[idx].position) timestamp = acv.Time(data.header.stamp.secs, data.header.stamp.nsecs) omega = np.array([ data.angular_velocity.x, data.angular_velocity.y, data.angular_velocity.z ]) alpha = np.array([ data.linear_acceleration.x, data.linear_acceleration.y, data.linear_acceleration.z ]) return (timestamp, omega, alpha)
def getImage(self, idx): topic, data, stamp = self.bag._read_message(self.index[idx].position) if self.perform_synchronization: timestamp = acv.Time( self.timestamp_corrector.getLocalTime( data.header.stamp.to_sec())) else: timestamp = acv.Time(data.header.stamp.secs, data.header.stamp.nsecs) if data._type == 'mv_cameras/ImageSnappyMsg': if self.uncompress is None: from snappy import uncompress self.uncompress = uncompress img_data = np.reshape(self.uncompress( np.fromstring(data.data, dtype='uint8')), (data.height, data.width), order="C") elif data._type == 'sensor_msgs/CompressedImage': # compressed images only have either mono or BGR normally (png and jpeg) # https://github.com/ros-perception/vision_opencv/blob/906d326c146bd1c6fbccc4cd1268253890ac6e1c/cv_bridge/src/cv_bridge.cpp#L480-L506 img_data = np.array(self.CVB.compressed_imgmsg_to_cv2(data)) if len(img_data.shape) > 2 and img_data.shape[2] == 3: img_data = cv2.cvtColor(img_data, cv2.COLOR_BGR2GRAY) elif data._type == 'sensor_msgs/Image': if data.encoding == "16UC1" or data.encoding == "mono16": image_16u = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = (image_16u / 256).astype("uint8") elif data.encoding == "8UC1" or data.encoding == "mono8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) elif data.encoding == "8UC3" or data.encoding == "bgr8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BGR2GRAY) elif data.encoding == "rgb8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_RGB2GRAY) elif data.encoding == "8UC4" or data.encoding == "bgra8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BGRA2GRAY) # bayes encodings conversions from # https://github.com/ros-perception/image_pipeline/blob/6caf51bd4484ae846cd8a199f7a6a4b060c6373a/image_proc/src/libimage_proc/processor.cpp#L70 elif data.encoding == "bayer_rggb8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_BG2GRAY) elif data.encoding == "bayer_bggr8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_RG2GRAY) elif data.encoding == "bayer_gbrg8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_GR2GRAY) elif data.encoding == "bayer_grbg8": img_data = np.array(self.CVB.imgmsg_to_cv2(data)) img_data = cv2.cvtColor(img_data, cv2.COLOR_BAYER_GB2GRAY) else: raise RuntimeError( "Unsupported Image Encoding: '{}'\nSupported are: " "16UC1 / mono16, 8UC1 / mono8, 8UC3 / rgb8 / bgr8, 8UC4 / bgra8, " "bayer_rggb8, bayer_bggr8, bayer_gbrg8, bayer_grbg8". format(data.encoding)) else: raise RuntimeError( "Unsupported Image Type: '{}'\nSupported are: " "mv_cameras/ImageSnappyMsg, sensor_msgs/CompressedImage, sensor_msgs/Image" .format(data._type)) return (timestamp, img_data)
def extract(self): # Load image paths print("Loading camera images from: " + self.cam_path) cam_files = [ f for f in os.listdir(self.cam_path) if os.path.isfile(os.path.join(self.cam_path, f)) ] cam_files = sorted(cam_files, key=natural_keys) file_count = 0 filename_num = file_count + 1 for f in cam_files: image_path = os.path.join(self.cam_path, f) image = cv2.imread(image_path, 0) np_image = np.array(image) # Detect target for camera split_file = f.rsplit('.', 1) timestamp = acv.Time(0, 0) success, observation = self.mono_validator.target.detector.findTarget( timestamp, np_image) observation.clearImage() if success: # Display image print("Processing image" + ' ' + str(filename_num)) # Get imu angle curr_imu_angle = self.imu_angles[file_count, 0:] [image_corners, reproj_corners, meanReprojectionError] = self.mono_validator.generateMonoview( np_image, observation, success) target_corners = observation.getCornersTargetFrame() T_t_c = observation.T_t_c() T_c_t = np.linalg.inv(T_t_c.T()) # Create output directory output_path = self.cam_path + "/points_data" create_directory(output_path) # Create output file file_name = str(filename_num) + ".txt" output_path = os.path.join(output_path, file_name) outfile = open(output_path, "w") # -- Output Gridpoints outfile.write("gridpoints:\n") for corner, pixel in zip(target_corners, image_corners): line = "{:.5f}".format(corner[0]) + " " line += "{:.5f}".format(corner[1]) + " " line += "{:.5f}".format(corner[2]) + " " line += "{:.5f}".format(pixel[0]) + " " line += "{:.5f}".format(pixel[1]) + "\n" outfile.write(line) # -- Ouput Transform outfile.write("tmatrix:\n") tmatrix = np.array(T_c_t) tmatrix_str = '\n'.join(" ".join('%0.5f' % (x) for x in y) for y in tmatrix) outfile.write(tmatrix_str) # -- Output Gimbal angles outfile.write("\ngimbalangles:\n") for i in range(len(curr_imu_angle)): outfile.write(str(curr_imu_angle[i]) + ' ') outfile.write("\n") # outfile.write("reprojectionerror:\n") # outfile.write(str(np.mean(meanReprojectionError))) # Finish output outfile.write("end:") outfile.close() file_count += 1 filename_num += 1