def __init__(self):
self.node_name = "Multi Vehicle Tracker"
self.active = True
# Tracker Configuration
self.configuration = rospy.get_param(
'~multivehicle_tracker') # TODO: On-the-fly reconfiguration
self.bot_detection_filter = lambda detection: detection.class_id == self.configuration[
'detection_info']['id']
self.multivehicle_tracker = NaiveMultitargetTracker()
self.groundplane_homography = load_homography(
rospy.get_param('~vehicle_name'))
# Subscribers
self.sub_detections = rospy.Subscriber("~object_detections",
ObjectDetectionList,
self.on_detections_received,
queue_size=1)
# Publishers
self.pub_tracklet = rospy.Publisher("~tracking",
TrackletList,
queue_size=1)
# timer for updating the params
self.continuous_prediction = rospy.Timer(rospy.Duration.from_sec(0.1),
self.on_delta_t)
self.garbage_collection = rospy.Timer(rospy.Duration.from_sec(0.2),
self.garbage_collection)
def __init__(self, robot_name='', map_file=''):
# Robot name
self.robot_name = robot_name
# Load camera calibration parameters
self.intrinsics = load_camera_intrinsics(robot_name)
self.H = inv(load_homography(self.robot_name))
def __init__(self, robot_name='', map_file=''):
# Set robot name
self.robot_name = robot_name
# Load map
self.map_data = load_map(map_file)
# Load homography
self.H = load_homography(self.robot_name)
def __init__(self, robot_name=''):
# Robot name
self.robot_name = robot_name
# Array which saves known obstacles to track them better
self.track_array = np.int_([])
# Load camera calibration parameters
self.intrinsics = load_camera_intrinsics(robot_name)
self.H = load_homography(self.robot_name)
self.inv_H = inv(self.H)
# define where to cut the image, color range,...
self.crop = 150 # default value but is overwritten in init_homo
# self.lower_yellow = np.array([20,100,150]) #more restrictive -> if you can be sure for "no reddish yellow"
self.lower_yellow = np.array([15, 100, 200])
self.upper_yellow = np.array([35, 255, 255])
self.lower_orange = np.array([0, 100, 100])
self.upper_orange = np.array([15, 255, 255])
self.lower_white = np.array([0, 0, 150])
self.upper_white = np.array([255, 25, 255])
self.ref_world_point_x = 1.7 # this is the reference world point where we crop the img
self.major_intertia_thres = 10 # if you want to detect very little ducks might lower it to 10, not smaller,..
self.img_width = 0 # to be set in init_inv_homography
self.img_height = 0 # to be set in init_inv_homography
self.maximum_height = 0 # to be set in ground2bird_view_pixel_init
self.maximum_left = 0
self.factor = 1.0 # to be set in ground2bird_view_pixel_init
self.obst_thres = 20 # to be set in init_inv_homography, this is default was 35
self.minimum_tracking_distance = 60
self.min_consec_tracking = 2 #number if times you must have seen critical object before adding as obstacle
self.new_track_array = np.array([])
self.M = self.init_inv_homography()
self.inv_M = inv(self.M)
center_world_coords = np.float32([[0.0], [0.0], [1.0]])
center_pixels = self.ground2bird_view_pixel(center_world_coords)
self.center_x = int(center_pixels[0])
self.center_y = int(center_pixels[1])
def __init__(self, robot_name=''):
self.robot_name = robot_name
# max curvature parameters
self.maximum_feasible_curvature = 1e6 # TODO: adjust this number!
self.num_intervals = 32
self.alpha_min = 0.1
self.alpha_max = 2.0
self.num_alphas = 20
self.alphas = np.linspace(self.alpha_min, self.alpha_max, self.num_alphas)
# lane error parameters
self.max_iterations = 20
self.init_grad_step_size = 1.0
self.step_size_increase = 1.2
self.step_size_decrease = 0.5
# drawing parameters
homography = dt.load_homography(self.robot_name)
self.H = np.linalg.inv(homography)
def __init__(self, robot_name=''):
self.robot_name = robot_name
# camera parameters
self.intrinsics = dt.load_camera_intrinsics(self.robot_name)
homography = dt.load_homography(self.robot_name)
self.H = np.linalg.inv(homography)
# edge detection parameters
self.canny_lower_threshold = 100 # self.SetupParameter("~canny_lower_threshold", 100)
self.canny_upper_threshold = 200 # self.SetupParameter("~canny_upper_threshold", 200)
self.canny_aperture_size = 3 # self.SetupParameter("~canny_aperture_size", 5)
# visibility parameters
self.border_cutoff = 10.0 # remove 10pixels around border
self.x_dist_cutoff = 0.5 # remove things that are beyond 0.5m from Duckiebot
pt_img_h = np.dot(self.H, np.array([self.x_dist_cutoff, 0.0, 1.0]))
pt_img = pt_img_h[0:2]/pt_img_h[2]
n_par_img = np.dot(self.H[0:2, 0:2],np.array([0.0, 1.0]))
n_par_img = n_par_img/np.linalg.norm(n_par_img)
n_perp_img = np.array([n_par_img[1],-n_par_img[0]])
if n_perp_img[1] < 0.0:
n_perp_img = -n_perp_img
self.A_visible = np.array([[-1.0, 0.0], [1.0, 0.0], [0.0, -1.0], [0.0, 1.0], -n_perp_img])
b_visible = np.array([- (0.0 + self.border_cutoff), 640.0 - self.border_cutoff, -(0.0 + self.border_cutoff),
480.0 - self.border_cutoff, -np.dot(pt_img, n_perp_img)])
self.b_visible = b_visible[:, np.newaxis]
self.mask_visible = np.zeros(shape=(480,640), dtype=np.uint8)
pts_grid = np.meshgrid(np.arange(0,640),np.arange(0,480))
pts = np.reshape(pts_grid,(2, 640*480))
visible = np.all(np.dot(self.A_visible, pts) - self.b_visible < 0.0, 0)
self.mask_visible[pts[1,visible],pts[0,visible]] = 255
self.A_visible_img = np.array([[-1.0, 0.0], [1.0, 0.0], [0.0, -1.0], [0.0, 1.0]])
self.b_visible_img = np.array([0.0, 640.0, 0.0, 480.0])
# weighting parameters
self.weight_c = 5.0
# TODO:
# tune weight
# multiple lines
# distance from duckiebot
# localization algorithm parameters
self.line_search_length = 20
self.max_num_iter = 2
self.ctrl_pts_density = 80 # number of control points per edge length (in meters)
self.min_num_ctrl_pts = 10
self.max_num_ctrl_pts = 100
# edge templates
self.edge_models = {'THREE_WAY_INTERSECTION': em.EdgeModel('THREE_WAY_INTERSECTION', self.ctrl_pts_density),
'FOUR_WAY_INTERSECTION': em.EdgeModel('FOUR_WAY_INTERSECTION', self.ctrl_pts_density)}
# auxiliary variables
self.A_par_to_perp = np.array([[0.0, 1.0], [-1.0, 0.0]], dtype=float)
px_offset = np.array([self.line_search_length, self.line_search_length])
self.px_offset = px_offset[:, np.newaxis]
self.G1 = np.zeros(shape=(3, 3), dtype=float)
self.G1[0, 2] = 1
self.G2 = np.zeros(shape=(3, 3), dtype=float)
self.G2[1, 2] = 1
self.G3 = np.zeros(shape=(3, 3), dtype=float)
self.G3[0, 1] = 1
self.G3[1, 0] = -1
type=str)
parser.add_argument("output_directory",
help="specify output directory",
type=str)
args = parser.parse_args()
dir_path = args.output_directory
im_path = args.input_bagpath
rospy.init_node('obstacle_detection_node', disable_signals=True)
detector = Detector(robot_name=robot_name)
crop = detector.crop
print crop
intrinsics = load_camera_intrinsics(robot_name)
visualizer = Visualizer(robot_name=robot_name)
H = load_homography(robot_name)
obst_list = PoseArray()
marker_list = MarkerArray()
#CREATE NEW DIRECTORY
if os.path.exists(dir_path):
shutil.rmtree(dir_path)
os.makedirs(dir_path)
#cv2.imwrite(dir+ '/' + str(i) + '.jpg',im1)
nummer = 1
while (True):
filename = im_path + '/' + str(nummer) + '.jpg'
im1 = cv2.imread(filename) #reads BGR