def __init__(self, camera_calibration_file):
self.first_image = True
self.camera_calibration_file = camera_calibration_file
self.M = None
self.M_inv = None
self.undistorter = None # will be set with first processed image because it needs image size
self.lane_det = ld.LaneDetector(n_windows=9,
margin=80,
min_pix=40,
poly_margin=50)
def __init__(self,
objectDetector=None,
laneDetector=None,
grassDetector=None):
# Subscribe to PointCloud2
self.subImage = rospy.Subscriber("/left/image_rect_color",\
Image, self.cbImage, queue_size=1)
self.subPoints = rospy.Subscriber("/points2",\
PointCloud2, self.cbPoints, queue_size=1)
self.images = []
self.mask = None
self.imageStamps = []
self.imageInd = 0
self.bridge = CvBridge()
self.showDetected = False
self.obstacleCloud = np.array([])
if objectDetector:
self.objectDetector = objectDetector
else:
self.objectDetector = ObstacleDetector((240, 320))
# We assume that the normal of the plane is pointing straight up
# in the vision (-Y direction)
self.objectDetector.setNormalConstraint(np.array([0., -1., -0.2]))
# And allow the search to deviate only 30 degrees from that
self.objectDetector.angleConstraint = 30.
if laneDetector:
self.laneDetector = laneDetector
else:
self.laneDetector = LaneDetector((240, 320))
if grassDetector:
self.grassDetector = grassDetector
else:
# Init with H,S,V thresholds
self.grassDetector = GrassDetector(45, 35, 35)
if VERBOSE:
print "Subscribed to /left/image_rect_color"
print "Subscribed to /points2"
K = np.asarray(
[[617.2716, 0, 327.2818], [0, 617.1263, 245.0939], [0, 0, 1]],
np.float32)
D = np.asarray([0, 0, 0, 0, 0], np.float32)
FOV_h = np.radians(91.2)
FOV_v = np.radians(65.5)
params = LaneDetector.CameraParams()
params.K = K
params.D = D
params.FOV_h = FOV_h
params.FOV_v = FOV_v
params.height = height
params.width = width
det = LaneDetector.LaneDetector(R, t, params)
mask_img = det.filter(img)
blur_img = det.blur_mask(mask_img)
warped_img = det.perspective_warp(blur_img)
(left, center, right) = det.sliding_window(warped_img)
waypoints = det.generate_waypoints(img, center)
plt.figure(1)
plt.subplot(221)
plt.imshow(img)
plt.subplot(222)
plt.imshow(blur_img, cmap='gray')
plt.subplot(223)
plt.imshow(warped_img, cmap='gray')
