def __init__(self, defaultQuat, service_name='update_pose_target'):
self.cameraCalib = CameraCalibration()
rospy.wait_for_service(service_name, timeout=None)
self._update_target = rospy.ServiceProxy(service_name, TrackPose)
self.pose_target_listener = None
self.mouth_target_listener = None
self.defaultQuat = defaultQuat
def test_car_length(self):
cc = CameraCalibration()
X1, Y1 = cc.image2world(661.498, 465.096)
X2, Y2 = cc.image2world(799.19, 617.622)
length = np.sqrt((X2 - X1)**2 + (Y2 - Y1)**2)
real_length = 11
assert np.abs(length - real_length) < 4
def test_car_width(self):
cc = CameraCalibration()
X1, Y1 = cc.image2world(914.768, 603.941)
X2, Y2 = cc.image2world(799.19, 617.622)
width = np.sqrt((X2 - X1)**2 + (Y2 - Y1)**2)
real_width = 2.55
assert np.abs(width - real_width) < 0.2
def __init__(self, filename=join(dirname(dirname(realpath(__file__))), "data/cctv_ftg1_SPEED_000000_carLoc.csv"),
top_obs_rec=906, bottom_obs_rec=439,
window_size=10, start_time_stamp='2018-01-01T08:00'):
self.start_time_stamp = start_time_stamp
self.start_time = []
self.car_type = []
self.track_coord = []
with open(filename, 'r') as f:
for line in f:
dt = line.strip().split("; ")
self.start_time.append(float(dt[0]) * 0.1 / 3)
self.car_type.append(dt[1])
self.track_coord.append([eval(x) for x in dt[2:]])
self.start_time = np.array(self.start_time)
self.car_type = np.array(self.car_type)
self.top_obs_rec = top_obs_rec
self.bottom_obs_rec = bottom_obs_rec
self.window_size = window_size
if pd.to_datetime(self.start_time_stamp) < pd.to_datetime('2018-09-11T16:00'):
self.cc = CameraCalibration(default=False)
self.scale = 1
self.top_obs_rec = 931.761
self.bottom_obs_rec = 439.723
else:
self.cc = CameraCalibration(
p1=(787.919, 640.099), p2=(594.782, 404.630),
p3=(675.065, 399.513), p4=(913.843, 619.450),
u2=703.475, u4=823.241, default=False
)
self.scale = 31.05945702896952 / 51.07394876776243
self.top_obs_rec = 793.968
self.bottom_obs_rec = 337.152
def calibrate(self):
"""
Compute the camera matrix
"""
self.calib = CameraCalibration(self.nColumns, self.nRows)
self.calib.calibrate(self.srcFolder)
self.params.calib = self.calib
self.nFrames = len(self.calib.srcImgs)
self.stream = ImageListVideoStream(self.calib.srcImgs)
self._setDisplay()
self._setDisplayMax()
self._updateImgProvider()
grad_color_stack, img = color_gradient_threshold_pipeline(image, img_processor)
birds_eye, M, Minv = calibration.perspective_transform(img, src, dst)
undist = calibration.undistort(image)
search, left_fitx, right_fitx, ploty, image_final = find_lane_lines.find(birds_eye, undist, Minv, left_line, right_line)
return image_final
#__main__
# Camera Calibration
img = mpimg.imread('camera_cal/calibration1.jpg')
img_size = (img.shape[1], img.shape[0])
calibration = CameraCalibration(chessboard_images_dir='camera_cal', pattern_size=(9,6))
calibration.calibrate_camera(img_size)
# Image Processor
img_processor = ImageProcessor()
# src and dst for perspective transform
test_img = mpimg.imread('test_images/straight_lines2.jpg')
test_img_size = (test_img.shape[1], test_img.shape[0])
src = np.float32([[580,460], [710,460], [1150,720], [150,720]])
offset = 200
dst = np.float32([ [offset, 0],
[test_img_size[0]-offset, 0],
[test_img_size[0]-offset, test_img_size[1]-0],
[offset, test_img_size[1]-0]
def scaled_sobel(abs_sobel):
return np.uint8(255 * abs_sobel / np.max(abs_sobel))
@staticmethod
def create_binary(image):
return np.zeros_like(image)
@staticmethod
def region_of_interest(img, vertices):
mask = np.zeros_like(img)
if len(img.shape) > 2:
channel_count = img.shape[2]
ignore_mask_color = (255, ) * channel_count
else:
ignore_mask_color = 255
cv2.fillPoly(mask, vertices, ignore_mask_color)
masked_image = cv2.bitwise_and(img, mask)
return masked_image
run = False
if run:
camcal = CameraCalibration()
camcal.calibrate_camera()
det = LaneLineDetector(camcal)
output = 'harder_challenge_video_submit.mp4'
clip1 = VideoFileClip('harder_challenge_video.mp4')
clip = clip1.fl_image(det.pipeline)
clip.write_videofile(output, audio=False)
import cv2
from camera_calibration import CameraCalibration
import image_processing as imgproc
import numpy as np
from line import Line
scaled_size = 1
image_size = (int(1280 * scaled_size), int(720 * scaled_size))
offset = image_size[1] * 0.3
file_name = './output_images/undist_straight_lines1.jpg'
calibration = CameraCalibration('camera_cal/')
calibration.calibrate()
intrinsic_mat = calibration.get_intrinsic()
dist_paras = calibration.get_distortion_paras()
perspective_src_points = scaled_size * np.float32(
[[233, 694], [595, 450], [686, 450], [1073, 694]]) # These points are manually selected
perspective_dst_points = np.float32([[offset, image_size[1]], [offset, 0],
[image_size[0] - offset, 0], [image_size[0] - offset, image_size[1]]])
#######################
img = cv2.imread(file_name)
undist = calibration.distort_correction(img)
undist = cv2.resize(undist, (0, 0), fx=scaled_size, fy=scaled_size)
hls = cv2.cvtColor(undist, cv2.COLOR_BGR2HSV)
h = hls[:, :, 0]
s = hls[:, :, 1]
v = hls[:, :, 2]
continue
gtDistances += cv_dists
tofDistances += tof_dists
stereoDistances += stereo_dists
# print("-----image ", fname, "-----")
# for i in range(len(cv_dists)):
# print('corner {:02d} has gt = {:.1f}, tof = {:.1f}, stereo = {:.1f}'.format(
# i, cv_dists[i], tof_dists[i], stereo_dists[i]))
return [gtDistances, tofDistances, stereoDistances]
if __name__ == '__main__':
square_size = 26.3571
width = 6
height = 5
cal_obj = CameraCalibration(square_size, width, height)
rgbModel = 'rgbCamera.yml'
tofModel = 'irCamera.yml'
stereoModel = 'stereo.yml'
cal_obj.load_cameraModel(rgbModel, tofModel, stereoModel)
dirpath = '../data/bias_calibration/corner*'
gtDistances, tofs, stereo_distances = calibrateDepth(
cal_obj,
dirpath,
irPrefix='Gray_',
depthPrefix='DepthImage_',
rgbPrefix='RBG_',
ifVisualization=False,
ifDivide16=True,
ifUseRGB=True)
