def run_test(id_test, test, id_estimator, estimator):
    global W, H

    logger.info('     id_test: %s' % id_test)
    logger.info('id_estimator: %s' % id_estimator)
    from led_detection.unit_tests import LEDDetectionUnitTest
    assert isinstance(test, LEDDetectionUnitTest)
    query = test.get_query()
    print( query['images']['rgb'][0].shape)
    H, W, _ = query['images']['rgb'][0].shape
    print('shape is %s, %s'%(W, H))
    result = estimator.detect_led(**query)

    # We are testing whether the expected detections are a subset of
    # the returned ones, we will accept duplicate detections of the
    # same LED
    match_count = [0]*len(test.expected)
    for r in result.detections:
        m = find_match(r, test.expected)
        if(m != -1):
            match_count[m]+=1

    missedLEDs = [test.expected[i] for i in range(0,  len(match_count)) if match_count[i]==0]
    if(missedLEDs):
        logger.error('missed LED detections (%s): \n %s' % (len(missedLEDs),missedLEDs))

    # Valerio: check this - sometimes the error above is thrown, but the test
    # does not return False
    return not 0 in match_count
Exemple #2
0
def run_test(id_test, test, id_estimator, estimator):
    global W, H

    logger.info('     id_test: %s' % id_test)
    logger.info('id_estimator: %s' % id_estimator)
    from led_detection.unit_tests import LEDDetectionUnitTest
    assert isinstance(test, LEDDetectionUnitTest)
    query = test.get_query()
    print( query['images']['rgb'][0].shape)
    H, W, _ = query['images']['rgb'][0].shape
    print('shape is %s, %s'%(W, H))
    result = estimator.detect_led(**query)

    # We are testing whether the expected detections are a subset of
    # the returned ones, we will accept duplicate detections of the
    # same LED
    match_count = [0]*len(test.expected)
    for r in result.detections:
        m = find_match(r, test.expected)
        if(m != -1):
            match_count[m]+=1

    missedLEDs = [test.expected[i] for i in range(0,  len(match_count)) if match_count[i]==0]
    if(missedLEDs):
        logger.error('missed LED detections (%s): \n %s' % (len(missedLEDs),missedLEDs))

    return not 0 in match_count
Exemple #3
0
def run_test(id_test, test, id_estimator, estimator):
    logger.info('     id_test: %s' % id_test)
    logger.info('id_estimator: %s' % id_estimator)
    from led_detection.unit_tests import LEDDetectionUnitTest
    assert isinstance(test, LEDDetectionUnitTest)
    query = test.get_query()
    result = estimator.detect_led(**query)
    raise NotImplementedError('To finish: Compare result to expected result.')
Exemple #4
0
def run_test(id_test, test, id_estimator, estimator):
    logger.info('     id_test: %s' % id_test)
    logger.info('id_estimator: %s' % id_estimator)
    from led_detection.unit_tests import LEDDetectionUnitTest
    assert isinstance(test, LEDDetectionUnitTest)
    query = test.get_query()
    result = estimator.detect_led(**query)
    raise NotImplementedError('To finish: Compare result to expected result.')
Exemple #5
0
def main():
    script_name = os.path.basename(sys.argv[0])
    args = sys.argv[1:]
    if len(args) != 2:
        msg = """
Usage:

    rosrun led_detection <script> <tests> <algorithms>
    
where:

    <tests> = comma separated list of algorithms. May use "*".
    <algorithms> = comma separated list of algorithms. May use  "*".
    
For example, this runs all tests on all algorithms:


    rosrun led_detection <script> '*' '*'
    
"""

        msg = msg.replace('<script>', script_name)
        logger.error(msg)
        sys.exit(2)

    which_tests0 = sys.argv[1]
    which_estimators0 = sys.argv[2]

    root = os.environ['DUCKIETOWN_ROOT']
    dirname = 'catkin_ws/src/led_detection/scripts/'
    filename = '20160312-allblinking_test1-argo.led_detection_test.yaml'
    filename = os.path.join(root, dirname, filename)

    alltests = load_tests(filename)
    estimators = {'dummy': DummyLEDDetector()}

    which_tests = expand_string(which_tests0, list(alltests))
    which_estimators = expand_string(which_estimators0, list(estimators))

    logger.info('     tests: %r |-> %s' % (which_tests0, which_tests))
    logger.info('estimators: %r |-> %s' %
                (which_estimators0, which_estimators))

    # which tests to execute
    for id_test in which_tests:
        for id_estimator in which_estimators:
            test_results = run_test(id_test, alltests[id_test], id_estimator,
                                    estimators[id_estimator])
Exemple #6
0
def main():
    script_name = os.path.basename(sys.argv[0])
    args = sys.argv[1:]
    if len(args) != 2:
        msg = """
Usage:

    rosrun led_detection <script> <tests> <algorithms>
    
where:

    <tests> = comma separated list of algorithms. May use "*".
    <algorithms> = comma separated list of algorithms. May use  "*".
    
For example, this runs all tests on all algorithms:


    rosrun led_detection <script> '*' '*'
    
"""

        msg = msg.replace('<script>', script_name)
        logger.error(msg)
        sys.exit(2)

    which_tests0 = sys.argv[1]
    which_estimators0 = sys.argv[2]

    root = os.environ['DUCKIETOWN_ROOT']
    dirname = 'catkin_ws/src/led_detection/scripts/'
    filename = '20160312-allblinking_test1-argo.led_detection_test.yaml'
    filename = os.path.join(root, dirname, filename)

    alltests = load_tests(filename)
    estimators = {'dummy': DummyLEDDetector()}
    
    which_tests = expand_string(which_tests0, list(alltests))
    which_estimators = expand_string(which_estimators0, list(estimators))

    logger.info('     tests: %r |-> %s' % (which_tests0, which_tests))
    logger.info('estimators: %r |-> %s' % (which_estimators0, which_estimators))

    # which tests to execute
    for id_test in which_tests:
        for id_estimator in which_estimators:
            test_results = run_test(id_test, alltests[id_test],
                                    id_estimator, estimators[id_estimator])
Exemple #7
0
def main():
    script_name = os.path.basename(sys.argv[0])
    args = sys.argv[1:]
    if len(args) != 2:
        msg = 'I need two arguments. Please see README.md for documentation.'
        logger.error(msg)
        sys.exit(2)

    which_tests0 = sys.argv[1]
    which_estimators0 = sys.argv[2]

    package_dir = get_ros_package_path('led_detection')
    logger.debug('Package dir: %r' % package_dir)

    # dirname = 'catkin_ws/src/f23-LED/led_detection/scripts/'
    #filename = 'all_tests.yaml'
    filename = os.path.join(package_dir, 'scripts', 'dp45_tests.yaml')

    alltests = load_tests(filename)
    estimators = {
        'baseline':
        LEDDetector(ploteverything=False, verbose=True, plotfinal=False),
        'LEDDetector_plots':
        LEDDetector(True, True, True)
    }
    #,'LEDDetector_forloops' : LEDDetector_forloops(True, True, True)}

    which_tests = expand_string(which_tests0, list(alltests))
    which_estimators = expand_string(which_estimators0, list(estimators))

    logger.info('     tests: %r |-> %s' % (which_tests0, which_tests))
    logger.info('estimators: %r |-> %s' %
                (which_estimators0, which_estimators))

    # which tests to execute
    test_results = {}
    for id_test in which_tests:
        for id_estimator in which_estimators:
            result = run_test(id_test, alltests[id_test], id_estimator,
                              estimators[id_estimator])
            test_results[(id_test, id_estimator)] = result

    nfailed = list(test_results.values()).count(False)
    if not nfailed:
        logger.info('All tests passed')
    else:
        which = [k for k, v in test_results.items() if not v]
        logger.error('These tests failed: %s ' % which)
        sys.exit(3)
Exemple #8
0
def main():
    script_name = os.path.basename(sys.argv[0])
    args = sys.argv[1:]
    if len(args) != 2:
        msg = """
Usage:

    rosrun led_detection <script> <tests> <algorithms>

where:

    <tests> = comma separated list of algorithms. May use "*".
    <algorithms> = comma separated list of algorithms. May use  "*".

For example, this runs all tests on all algorithms:


    rosrun led_detection <script> '*' '*'

The default algorithm is called "baseline", and the tests are invoked using:

    rosrun led_detection <script> '*' 'baseline'

"""

        msg = msg.replace('<script>', script_name)
        logger.error(msg)
        sys.exit(2)

    which_tests0 = sys.argv[1]
    which_estimators0 = sys.argv[2]

    root = os.environ['DUCKIETOWN_ROOT']
    dirname = 'catkin_ws/src/f23-LED/led_detection/scripts/'
    #filename = 'all_tests.yaml'
    filename = 'dp45_tests.yaml'
    filename = os.path.join(root, dirname, filename)

    alltests = load_tests(filename)
    estimators = {
        'baseline':
        LEDDetector(ploteverything=False, verbose=True, plotfinal=False),
        'LEDDetector_plots':
        LEDDetector(True, True, True)
    }
    #,'LEDDetector_forloops' : LEDDetector_forloops(True, True, True)}

    which_tests = expand_string(which_tests0, list(alltests))
    which_estimators = expand_string(which_estimators0, list(estimators))

    logger.info('     tests: %r |-> %s' % (which_tests0, which_tests))
    logger.info('estimators: %r |-> %s' %
                (which_estimators0, which_estimators))

    # which tests to execute
    test_results = {}
    for id_test in which_tests:
        for id_estimator in which_estimators:
            result = run_test(id_test, alltests[id_test], id_estimator,
                              estimators[id_estimator])
            test_results[(id_test, id_estimator)] = result

    nfailed = list(test_results.values()).count(False)
    if not nfailed:
        logger.info('All tests passed')
    else:
        which = [k for k, v in test_results.items() if not v]
        logger.error('These tests failed: %s ' % which)
        sys.exit(3)
Exemple #9
0
    def detect_led(self, images, mask, frequencies_to_detect,
                   min_distance_between_LEDs_pixels):

        assert len(images.shape) == 1
        n = images.shape[0]
        if n == 0:
            raise ValueError('No images provided')

        timestamps = images['timestamp']
        rgb = images['rgb']

        rgb0 = rgb[0]
        if not mask.shape == rgb0.shape:
            raise ValueError('Invalid mask')

        if not isinstance(frequencies_to_detect, list):
            raise ValueError(frequencies_to_detect)

        if not min_distance_between_LEDs_pixels > 0:
            raise ValueError(min_distance_between_LEDs_pixels)

        channel = images['rgb'][:, :, :, 0]  # just using first channel

        cell_width = 15
        cell_height = 15

        (cell_vals, crop_offset) = self.downsample(channel, cell_width,
                                                   cell_height)

        candidates_mask = self.get_candidate_cells(cell_vals, 100)
        candidate_cells = [(i, j)
                           for (i, j) in np.ndindex(candidates_mask.shape)
                           if candidates_mask[i, j]]

        # Create result object
        result = LEDDetectionArray()

        # Detect frequencies and discard non-periodic signals
        # ts_tolerance = 0.2 # unnecessary
        f_tolerance = 0.25
        min_num_periods = 3

        for (i, j) in candidate_cells:
            signal = cell_vals[:, i, j]
            signal = signal - np.mean(signal)

            zero_crossings = np.where(np.diff(np.sign(signal)))[0]
            zero_crossings_t = timestamps[zero_crossings]
            led_img_coords = Vector2D((0.5 + j) * cell_width + crop_offset[1],
                                      (0.5 + i) * cell_height + crop_offset[0])
            diffs = [
                b - a for a, b in zip(zero_crossings_t, zero_crossings_t[1:])
            ]

            if (self.verbose):
                logger.info('Coords: %s, %s' %
                            (led_img_coords.x, led_img_coords.y))
                logger.info('Zero crossings: %s' % zero_crossings_t)
                logger.info('Diffs: %s' % diffs)
                logger.info('Zero-crossing measured freq %s' %
                            (0.5 / np.mean(diffs)))

            if (len(zero_crossings) < min_num_periods):
                if (self.verbose):
                    logger.info('Not an LED, discarded\n')
                continue

            # Frequency estimation based on zero crossings - quite bad
            #for f in frequencies_to_detect:
            #    if(all(d-ts_tolerance <= 0.5/f <= d+ts_tolerance for d in diffs)):
            #        if(self.verbose):
            #            logger.info('Confirmed LED with frequency %s\n'%f)
            # recover coordinates of centroid
            #        result.detections.append(LEDDetection(timestamps[0], timestamps[-1],
            #        led_img_coords, f, '', -1)) # -1...confidence not implemented
            #        break

            # Frequency estimation based on FFT
            T = 1.0 / 30  # TODO expecting 30 fps, but RESAMPLE to be sure
            f = np.linspace(0.0, 1.0 / (2.0 * T), n / 2)
            signal_f = scipy.fftpack.fft(signal)
            y_f = 2.0 / n * np.abs(signal_f[:n / 2])
            fft_peak_freq = 1.0 * np.argmax(y_f) / T / n
            if (self.verbose):
                logger.info('FFT peak frequency: %s' % fft_peak_freq)

            # Bin frequency into the ones to detect
            freq = [
                x for x in frequencies_to_detect
                if abs(x - fft_peak_freq) < f_tolerance
            ]
            if (freq):
                result.detections.append(
                    LEDDetection(rospy.Time.from_sec(timestamps[0]),
                                 rospy.Time.from_sec(timestamps[-1]),
                                 led_img_coords, freq[0], '',
                                 -1))  # -1...confidence not implemented
                if (self.verbose):
                    logger.info('LED confirmed, frequency: %s' % freq)
            else:
                logger.info('Could not associate frequency, discarding')

            print(signal.shape)
            print(timestamps[:15].shape)
            # Plot all signals and FFTs
            if (self.ploteverything):
                fig, ax1 = plt.subplots()
                ax1.plot(timestamps[:15], signal)
                fig, ax2 = plt.subplots()
                ax2.plot(f[:15], y_f)
                plt.show()

        plt.imshow(rgb0)
        ax = plt.gca()

        font = {
            'family': 'serif',
            'color': 'red',
            'weight': 'bold',
            'size': 16,
        }

        # Plot all results
        if (self.plotfinal):
            for r in result.detections:
                pos = r.pixels_normalized
                ax.add_patch(
                    Rectangle(
                        (pos.x - 0.5 * cell_width, pos.y - 0.5 * cell_height),
                        cell_width,
                        cell_height,
                        edgecolor="red",
                        linewidth=3,
                        facecolor="none"))
                plt.text(pos.x - 0.5 * cell_width,
                         pos.y - cell_height,
                         str(r.frequency),
                         fontdict=font)

            plt.show()

        return result
Exemple #10
0
    def detect_led(self,
                   images,
                   mask,
                   frequencies_to_detect,
                   min_distance_between_LEDs_pixels):

        assert len(images.shape) == 1
        n = images.shape[0]
        if n == 0:
            raise ValueError('No images provided')

        timestamps = images['timestamp']
        # print('timestamps: {0}'.format(timestamps))
        rgb = images['rgb']

        rgb0 = rgb[0]
        if not mask.shape == rgb0.shape:
            raise ValueError('Invalid mask')

        if not isinstance(frequencies_to_detect, list):
            raise ValueError(frequencies_to_detect)

        if not min_distance_between_LEDs_pixels > 0:
            raise ValueError(min_distance_between_LEDs_pixels)

        #channel = images['rgb'][:,:,:,0] # just using first channel

        # Go for the following lines if you want to use a grayscale image
        # as an input instead of preferring one specific channel
        channel = np.zeros(images['rgb'].shape[0:-1])
        for i in range(n):
            channel[i,:,:] = cv2.cvtColor(images['rgb'][i,:,:,:], cv2.COLOR_BGR2GRAY)

        print('channel.shape {0}'.format(channel.shape))
        W = channel.shape[2]
        H = channel.shape[1]

        cell_width = 15
        cell_height = 15
        var_threshold = 100

        (cell_vals, crop_offset) = self.downsample(channel, cell_width, cell_height)
        candidates_mask = self.get_candidate_cells(cell_vals, var_threshold)

        candidate_cells = [(i,j) for (i,j) in np.ndindex(candidates_mask.shape) if candidates_mask[i,j]]

        if(self.publisher is not None):
            for idx in candidate_cells:
                self.debug_msg.candidates.append(Vector2D(idx[0], idx[1]))
            #self.republish()

        # Create result object
        result = LEDDetectionArray()
        unfiltered = LEDDetectionArray()

        # Detect frequencies and discard non-periodic signals
        # ts_tolerance = 0.2 # unnecessary
        f_tolerance = 0.3

        for (i,j) in candidate_cells:
            signal = cell_vals[:,i,j]
            signal = signal-np.mean(signal)

            led_img_coords = Vector2D((0.5+j)*cell_width+crop_offset[1], (0.5+i)*cell_height+crop_offset[0])
            led_img_coords_norm = Vector2D(1.0*led_img_coords.x/W, 1.0*led_img_coords.y/H)

            if(self.verbose):
                logger.info('Coords: %s, %s'% (led_img_coords.x,led_img_coords.y))

            # Frequency estimation based on FFT
            T = 1.0/30 # TODO expecting 30 fps, but RESAMPLE to be sure
            f = np.linspace(0.0, 1.0/(2.0*T), n/2)
            signal_f = scipy.fftpack.fft(signal)
            y_f =  2.0/n * np.abs(signal_f[:n/2])
            fft_peak_freq = 1.0*np.argmax(y_f)/T/n

            unfiltered.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
                rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, fft_peak_freq, '', -1, timestamps, signal, f, y_f)) # -1...confidence not implemented

            if(self.verbose):
                logger.info('FFT peak frequency: %s'% fft_peak_freq)

            # Bin frequency into the ones to detect
            freq = [x for x in frequencies_to_detect if abs(x-fft_peak_freq)<f_tolerance]
            if(freq):
                result.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
                rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, freq[0], '', -1, [], [], [], [])) # -1...confidence not implemented
                if(self.verbose):
                    logger.info('LED confirmed, frequency: %s'% freq)
            else:
                logger.info('Could not associate frequency, discarding')

            # Plot all signals and FFTs
            if(self.ploteverything):
                fig, ax1 = plt.subplots()
                ax1.plot(timestamps, signal)
                fig, ax2 = plt.subplots()
                ax2.plot(f,y_f)
                plt.show()

        plt.imshow(rgb0)
        ax = plt.gca()

        font = {'family': 'serif',
                'color':  'red',
                'weight': 'bold',
                'size': 16,
                }

        # Plot all results
        if(self.plotfinal):
            for r in result.detections:
                pos_n = r.pixels_normalized
                pos = Vector2D(1.0*pos_n.x*W, 1.0*pos_n.y*H)
                ax.add_patch(Rectangle((pos.x-0.5*cell_width, pos.y-0.5*cell_height), cell_width, cell_height, edgecolor="red", linewidth=3, facecolor="none"))
                plt.text(pos.x-0.5*cell_width, pos.y-cell_height, str(r.frequency), fontdict=font)

            plt.show()
        if(self.publisher is not None):
            self.debug_msg.led_all_unfiltered = unfiltered
            self.debug_msg.state = 0
            self.republish()

        return result
    def detect_led(self,
                   images,
                   mask,
                   frequencies_to_detect,
                   min_distance_between_LEDs_pixels):

        assert len(images.shape) == 1
        n = images.shape[0]
        if n == 0:
            raise ValueError('No images provided')

        timestamps = images['timestamp']
        rgb = images['rgb']

        rgb0 = rgb[0]
        if not mask.shape == rgb0.shape:
            raise ValueError('Invalid mask')

        if not isinstance(frequencies_to_detect, list):
            raise ValueError(frequencies_to_detect)

        if not min_distance_between_LEDs_pixels > 0:
            raise ValueError(min_distance_between_LEDs_pixels)

        channel = images['rgb'][:,:,:,0] # just using first channel

        cell_width = 15
        cell_height = 15

        (cell_vals, crop_offset) = self.downsample(channel, cell_width, cell_height)

        candidates_mask = self.get_candidate_cells(cell_vals, 100)
        candidate_cells = [(i,j) for (i,j) in np.ndindex(candidates_mask.shape) if candidates_mask[i,j]]

        # Create result object
        result = LEDDetectionArray()

        # Detect frequencies and discard non-periodic signals
        # ts_tolerance = 0.2 # unnecessary
        f_tolerance = 0.25
        min_num_periods = 3

        for (i,j) in candidate_cells:
            signal = cell_vals[:,i,j]
            signal = signal-np.mean(signal)

            zero_crossings = np.where(np.diff(np.sign(signal)))[0]
            zero_crossings_t = timestamps[zero_crossings]
            led_img_coords = Vector2D((0.5+j)*cell_width+crop_offset[1], (0.5+i)*cell_height+crop_offset[0])       
            diffs = [b-a for a, b in zip(zero_crossings_t, zero_crossings_t[1:])]
            
            if(self.verbose):
                logger.info('Coords: %s, %s'% (led_img_coords.x,led_img_coords.y))
                logger.info('Zero crossings: %s'%zero_crossings_t)
                logger.info('Diffs: %s'%diffs)
                logger.info('Zero-crossing measured freq %s'% (0.5/np.mean(diffs)))

            if(len(zero_crossings)<min_num_periods):
                if(self.verbose):
                    logger.info('Not an LED, discarded\n')
                continue

            # Frequency estimation based on zero crossings - quite bad
            #for f in frequencies_to_detect:
            #    if(all(d-ts_tolerance <= 0.5/f <= d+ts_tolerance for d in diffs)):
            #        if(self.verbose):
            #            logger.info('Confirmed LED with frequency %s\n'%f)
                   # recover coordinates of centroid
            #        result.detections.append(LEDDetection(timestamps[0], timestamps[-1],
            #        led_img_coords, f, '', -1)) # -1...confidence not implemented
            #        break

            # Frequency estimation based on FFT
            T = 1.0/30 # TODO expecting 30 fps, but RESAMPLE to be sure
            f = np.linspace(0.0, 1.0/(2.0*T), n/2)
            signal_f = scipy.fftpack.fft(signal)
            y_f =  2.0/n * np.abs(signal_f[:n/2])
            fft_peak_freq = 1.0*np.argmax(y_f)/T/n
            if(self.verbose):
                logger.info('FFT peak frequency: %s'% fft_peak_freq)

            # Bin frequency into the ones to detect
            freq = [x for x in frequencies_to_detect if abs(x-fft_peak_freq)<f_tolerance]
            if(freq):
                result.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
                rospy.Time.from_sec(timestamps[-1]), led_img_coords, freq[0], '', -1)) # -1...confidence not implemented
                if(self.verbose):
                    logger.info('LED confirmed, frequency: %s'% freq)
            else:
                logger.info('Could not associate frequency, discarding')

            print(signal.shape)
            print(timestamps[:15].shape)
            # Plot all signals and FFTs
            if(self.ploteverything):
                fig, ax1 = plt.subplots()
                ax1.plot(timestamps[:15], signal)
                fig, ax2 = plt.subplots()
                ax2.plot(f[:15],y_f)
                plt.show()

        plt.imshow(rgb0)
        ax = plt.gca()

        font = {'family': 'serif',
                'color':  'red',
                'weight': 'bold',
                'size': 16,
                }

        # Plot all results
        if(self.plotfinal):
            for r in result.detections:
                pos = r.pixels_normalized
                ax.add_patch(Rectangle((pos.x-0.5*cell_width, pos.y-0.5*cell_height), cell_width, cell_height, edgecolor="red", linewidth=3, facecolor="none"))
                plt.text(pos.x-0.5*cell_width, pos.y-cell_height, str(r.frequency), fontdict=font)

            plt.show()

        return result
Exemple #12
0
    def detect_led(self, images, mask, frequencies_to_detect,
                   min_distance_between_LEDs_pixels):

        assert len(images.shape) == 1
        n = images.shape[0]
        if n == 0:
            raise ValueError('No images provided')

        timestamps = images['timestamp']
        rgb = images['rgb']

        rgb0 = rgb[0]
        if not mask.shape == rgb0.shape:
            raise ValueError('Invalid mask')

        if not isinstance(frequencies_to_detect, list):
            raise ValueError(frequencies_to_detect)

        if not min_distance_between_LEDs_pixels > 0:
            raise ValueError(min_distance_between_LEDs_pixels)

        # tuneable parameters
        partitions_x = 20
        partitions_y = 20
        intensity_variance_threshold = 500  #TODO: this is arbitrary

        # should be 480x640?
        W = rgb0.shape[0]
        H = rgb0.shape[1]
        partition_width = W / partitions_x
        partition_height = H / partitions_y

        # create result object
        result = LEDDetectionArray()

        partition_intensity_data = []

        with open('raw_data_argo1.json', 'w') as f:
            numpy.savez(f,
                        images=images,
                        mask=mask,
                        frequencies_to_detect=frequencies_to_detect,
                        min_distance_between_LEDs_pixels=
                        min_distance_between_LEDs_pixels)

        return 0
        # jump by partition
        for x in range(0, W, partition_width):
            for y in range(0, H, partition_height):
                logger.info('Looking at partition %s %s' % (x, y))
                # look at same partition across multiple images
                average_intensities = []
                for image in images['rgb']:
                    intensities = []
                    # loop through pixels in each partition
                    for x_coord in range(x, x + partition_width):
                        for y_coord in range(y, y + partition_height):
                            if x_coord > W or y_coord > H:
                                continue
                            pixel = image[x_coord][y_coord]
                            # a simple approach from http://stackoverflow.com/questions/596216/formula-to-determine-brightness-of-rgb-color
                            # could also use matplotlib's rgb_to_hsv?
                            # TODO: check if pixel order is RGB or BGR (assuming RGB)
                            #intensity = 0.299*pixel[0] + 0.587*pixel[1] + 0.114*pixel[2]
                            #intensity = pixel[0] # red channel
                            #intensity = pixel[1] # green channel
                            intensity = pixel[2]  # blue channel
                            intensities.append(intensity)

                    average_intensities.append(numpy.amax(intensities))
                partition_intensity_data.append(average_intensities)
                variance = numpy.var(average_intensities)
                if variance > intensity_variance_threshold:
                    #threshold into "on" and "off" states
                    pass
Exemple #13
0
def main():
    script_name = os.path.basename(sys.argv[0])
    args = sys.argv[1:]
    if len(args) != 2:
        msg = """
Usage:

    rosrun led_detection <script> <tests> <algorithms>

where:

    <tests> = comma separated list of algorithms. May use "*".
    <algorithms> = comma separated list of algorithms. May use  "*".

For example, this runs all tests on all algorithms:


    rosrun led_detection <script> '*' '*'

The default algorithm is called "baseline", and the tests are invoked using:

    rosrun led_detection <script> '*' 'baseline'


"""

        msg = msg.replace('<script>', script_name)
        logger.error(msg)
        sys.exit(2)

    which_tests0 = sys.argv[1]
    which_estimators0 = sys.argv[2]

    root = os.environ['DUCKIETOWN_ROOT']
    dirname = 'catkin_ws/src/f23-LED/led_detection/scripts/'
    filename = 'all_tests.yaml'
    filename = os.path.join(root, dirname, filename)

    alltests = load_tests(filename)
    estimators = {  'baseline' :
                    LEDDetector(ploteverything=False, verbose=True, plotfinal=False),
                    'LEDDetector_plots' : LEDDetector(True, True, True)}
                 #,'LEDDetector_forloops' : LEDDetector_forloops(True, True, True)}

    which_tests = expand_string(which_tests0, list(alltests))
    which_estimators = expand_string(which_estimators0, list(estimators))

    logger.info('     tests: %r |-> %s' % (which_tests0, which_tests))
    logger.info('estimators: %r |-> %s' % (which_estimators0, which_estimators))

    # which tests to execute
    test_results = {}
    for id_test in which_tests:
        for id_estimator in which_estimators:
            result = run_test(id_test, alltests[id_test], id_estimator, estimators[id_estimator])
            test_results[(id_test, id_estimator)] = result

    nfailed = list(test_results.values()).count(False)
    if not nfailed:
        logger.info('All tests passed')
    else:
        which = [k for k, v in test_results.items() if not v]
        logger.error('These tests failed: %s ' % which)
        sys.exit(3)
Exemple #14
0
    def detect_led(self,
                   images,
                   frequencies_to_detect,
                   cell_size,
                   crop_rect_norm=[0,0,1.0,1.0]):

        assert len(images.shape) == 1
        n = images.shape[0]
        if n == 0:
            raise ValueError('No images provided')

        timestamps = images['timestamp']
        rgb = images['rgb']
        H, W, _ = rgb[0].shape

        if not isinstance(frequencies_to_detect, list):
            raise ValueError(frequencies_to_detect)

        if(self.publisher is not None):
            self.debug_msg.cell_size = cell_size
            self.debug_msg.crop_rect_norm = crop_rect_norm
        self.republish()

        # Crop + Greyscale
        tlx = floor(1.0*W*crop_rect_norm[0])
        tly = floor(1.0*H*crop_rect_norm[1])
        brx = ceil(1.0*W*crop_rect_norm[2])
        bry = ceil(1.0*H*crop_rect_norm[3])
        croppedshape = [images['rgb'].shape[0], bry-tly, brx-tlx] 
        channel = np.zeros(croppedshape)
        for i in range(n):
            channel[i,:,:] = cv2.cvtColor(images['rgb'][i,tly:bry,tlx:brx,:], cv2.COLOR_BGR2GRAY)

        print('expected shape {0}'.format(croppedshape))
        print('channel.shape {0}'.format(channel.shape))

        cell_width = cell_size[0]
        cell_height = cell_size[1]
        var_threshold = 100

        (cell_vals, crop_offset) = self.downsample(channel, cell_width, cell_height)
        candidates_mask = self.get_candidate_cells(cell_vals, var_threshold)

        candidate_cells = [(i,j) for (i,j) in np.ndindex(candidates_mask.shape) if candidates_mask[i,j]]

        if(self.publisher is not None):
            for idx in candidate_cells:
                self.debug_msg.candidates.append(Vector2D(idx[0]+crop_offset[1]+tly, idx[1]+crop_offset[0]+tlx))

        # Create result object
        result = LEDDetectionArray()
        unfiltered = LEDDetectionArray()

        # Detect frequencies and discard non-periodic signals
        f_tolerance = 0.3

        for (i,j) in candidate_cells:
            signal = cell_vals[:,i,j]
            signal = signal-np.mean(signal)

            led_img_coords = Vector2D((0.5+j)*cell_width+crop_offset[1]+tlx, (0.5+i)*cell_height+crop_offset[0]+tly)
            led_img_coords_norm = Vector2D(1.0*led_img_coords.x/W, 1.0*led_img_coords.y/H)

            if(self.verbose):
                logger.info('Coords: %s, %s'% (led_img_coords.x,led_img_coords.y))

            # Frequency estimation based on FFT
            T = 1.0/30 # TODO expecting 30 fps, but RESAMPLE to be sure
            f = np.linspace(0.0, 1.0/(2.0*T), n/2)
            signal_f = scipy.fftpack.fft(signal)
            y_f =  2.0/n * np.abs(signal_f[:n/2])
            fft_peak_freq = 1.0*np.argmax(y_f)/T/n

            unfiltered.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
                rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, fft_peak_freq, '', -1, timestamps, signal, f, y_f)) # -1...confidence not implemented

            if(self.verbose):
                logger.info('FFT peak frequency: %s'% fft_peak_freq)

            # Bin frequency into the ones to detect
            freq = [x for x in frequencies_to_detect if abs(x-fft_peak_freq)<f_tolerance]
            if(freq):
                result.detections.append(LEDDetection(rospy.Time.from_sec(timestamps[0]),
                rospy.Time.from_sec(timestamps[-1]), led_img_coords_norm, freq[0], '', -1, [], [], [], [])) # -1...confidence not implemented
                if(self.verbose):
                    logger.info('LED confirmed, frequency: %s'% freq)
            else:
                logger.info('Could not associate frequency, discarding')

            # Plot all signals and FFTs
            if(self.ploteverything):
                fig, ax1 = plt.subplots()
                ax1.plot(timestamps, signal)
                fig, ax2 = plt.subplots()
                ax2.plot(f,y_f)
                plt.show()
                
        if(self.ploteverything):
            plt.imshow(rgb[0])
            ax = plt.gca()


        font = {'family': 'serif',
                'color':  'red',
                'weight': 'bold',
                'size': 16,
                }

        # Plot all results
        if(self.plotfinal):
            for r in result.detections:
                pos_n = r.pixels_normalized
                pos = Vector2D(1.0*pos_n.x*W, 1.0*pos_n.y*H)
                ax.add_patch(Rectangle((pos.x-0.5*cell_width, pos.y-0.5*cell_height), cell_width, cell_height, edgecolor="red", linewidth=3, facecolor="none"))
                plt.text(pos.x-0.5*cell_width, pos.y-cell_height, str(r.frequency), fontdict=font)

            plt.show()

        if(self.publisher is not None):
            self.debug_msg.led_all_unfiltered = unfiltered
            self.debug_msg.state = 0
            self.republish()

        return result
Exemple #15
0
    def detect_led(self,
                   images,
                   mask,
                   frequencies_to_detect,
                   min_distance_between_LEDs_pixels):

        assert len(images.shape) == 1
        n = images.shape[0]
        if n == 0:
            raise ValueError('No images provided')

        timestamps = images['timestamp']
        rgb = images['rgb']

        rgb0 = rgb[0]
        if not mask.shape == rgb0.shape:
            raise ValueError('Invalid mask')

        if not isinstance(frequencies_to_detect, list):
            raise ValueError(frequencies_to_detect)

        if not min_distance_between_LEDs_pixels > 0:
            raise ValueError(min_distance_between_LEDs_pixels)

        # tuneable parameters
        partitions_x = 20
        partitions_y = 20
        intensity_variance_threshold = 500 #TODO: this is arbitrary

        # should be 480x640?
        W = rgb0.shape[0]
        H = rgb0.shape[1]
        partition_width = W / partitions_x
        partition_height = H / partitions_y

        # create result object
        result = LEDDetectionArray()

        partition_intensity_data = []

        with open('raw_data_argo1.json','w') as f: 
            numpy.savez(f, images=images, mask=mask,
                frequencies_to_detect=frequencies_to_detect, min_distance_between_LEDs_pixels=min_distance_between_LEDs_pixels)

        return 0
        # jump by partition
        for x in range(0, W, partition_width):
            for y in range(0, H, partition_height):
                logger.info('Looking at partition %s %s' % (x, y))
                # look at same partition across multiple images
                average_intensities = []
                for image in images['rgb']:
                    intensities = []
                    # loop through pixels in each partition
                    for x_coord in range(x, x+partition_width):
                        for y_coord in range(y,y+partition_height):
                            if x_coord > W or y_coord > H:
                                continue
                            pixel = image[x_coord][y_coord]
                            # a simple approach from http://stackoverflow.com/questions/596216/formula-to-determine-brightness-of-rgb-color
                            # could also use matplotlib's rgb_to_hsv?
                            # TODO: check if pixel order is RGB or BGR (assuming RGB)
                            #intensity = 0.299*pixel[0] + 0.587*pixel[1] + 0.114*pixel[2]
                            #intensity = pixel[0] # red channel
                            #intensity = pixel[1] # green channel
                            intensity = pixel[2] # blue channel
                            intensities.append(intensity)


                    average_intensities.append(numpy.amax(intensities))
                partition_intensity_data.append(average_intensities)
                variance = numpy.var(average_intensities)
                if variance > intensity_variance_threshold:
                    #threshold into "on" and "off" states
                    pass