steering_wheel.py 文件源码

def estimate_angle(self, image):
        #from scipy import misc
        subimg = cnn_extract_steering_wheel_image(image)
        #misc.imshow(subimg)
        subimg = cnn_downscale_image(subimg)
        #misc.imshow(subimg)
        angle_raw_bins = self.model.forward_image(subimg, volatile=True, requires_grad=False, gpu=Config.GPU, softmax=True)
        angle_raw_bins = angle_raw_bins.data[0].cpu().numpy()
        angle_raw_bin = np.argmax(angle_raw_bins)
        #print(angle_raw_bins.data.cpu().numpy())

        """
        angle_raw_center = angle_raw_bin * CNN_ANGLE_BIN_SIZE + CNN_ANGLE_BIN_SIZE * 0.5 - 180
        angle_raw_left = angle_raw_center - CNN_ANGLE_BIN_SIZE
        angle_raw_right = angle_raw_center + CNN_ANGLE_BIN_SIZE
        angle_raw_center_p = angle_raw_bins[angle_raw_bin]
        angle_raw_left_p = angle_raw_bins[angle_raw_bin-1] if angle_raw_bin-1 > 0 else 0
        angle_raw_right_p = angle_raw_bins[angle_raw_bin+1] if angle_raw_bin+1 < angle_raw_bins.shape[0] else 0

        angle_raw = angle_raw_left_p * angle_raw_left + angle_raw_center_p * angle_raw_center + angle_raw_right_p * angle_raw_right
        """
        angle_raw = angle_raw_bin * CNN_ANGLE_BIN_SIZE + CNN_ANGLE_BIN_SIZE * 0.5 - 180

        #print(angle_raw)
        possible_angles = [angle_raw]
        if angle_raw < 0:
            possible_angles.append(180+(180-abs(angle_raw)))
            possible_angles.append(-360-abs(angle_raw))
        if angle_raw > 0:
            possible_angles.append(-180-(180-abs(angle_raw)))
            possible_angles.append(360+abs(angle_raw))
        possible_angles_dist = [(a, abs(self.last_angle - a)) for a in possible_angles]
        possible_angles_dist_sort = sorted(possible_angles_dist, key=lambda t: t[1])
        angle = possible_angles_dist_sort[0][0]

        if angle > Config.STEERING_WHEEL_MAX:
            angle = angle - 360
        elif angle < Config.STEERING_WHEEL_MIN:
            angle = angle + 360

        if abs(angle - self.last_angle) >= self.overflow_degrees:
            if self.overflow_counter >= self.overflow_max_count:
                self.last_angle = angle
                self.last_angle_raw = angle_raw
                self.overflow_counter = 0
            else:
                angle = self.last_angle
                angle_raw = self.last_angle_raw
            self.overflow_counter += 1
        else:
            self.last_angle = angle
            self.last_angle_raw = angle_raw
            self.overflow_counter = 0

        return angle, angle_raw