def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.callback)
#self.image_sub = rospy.Subscriber("/ocam/image_raw",Image,self.callback)
#self.image_sub = rospy.Subscriber("/usb2_cam/image_rect_color",Image,self.callback)
self.CRT = CMT.CMT()
self.CRT.estimate_scale = 'estimate_scale'
self.CRT.estimate_rotation = 'estimate_rotation'
self.pause_time = 10
###########################Primer Region
im0 = cv2.imread('./frame_cap.jpg',
flags=cv2.IMREAD_COLOR) #flags=cv2.IMREAD_GRAYSCALE)
#im0 = cv2.imread('/home/sergio/catikin_ws_user/src/cmt/scripts/frame.jpg', flags=cv2.IMREAD_COLOR)
#im0 = cv2.imread('/home/sergio/catikin_ws_user/src/cmt/scripts/frame.jpg',flags=cv2.IMREAD_GRAYSCALE)
cv2.imshow('im0', im0)
#im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
im_gray0 = im0
im_draw = np.copy(im0)
print('Selecciona Primer Region')
tl = (84, 55)
br = (557, 307)
#tl=(35,35)
#bl=(605,325)
(tl, br) = util.get_rect(im_draw)
print('usando %i, %i como init bb', tl, br)
self.CRT.initialise(im_gray0, tl, br)
self.frame = 1
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image,
self.callback)
#self.image_sub = rospy.Subscriber("/usb2_cam/image_rect_color",Image,self.callback)
self.CRT = CMT.CMT()
self.CNT = CMT.CMT()
self.CST = CMT.CMT()
self.CRT = CMT.CMT()
self.CNT = CMT.CMT()
self.CST = CMT.CMT()
self.CRT.estimate_scale = 'estimate_scale'
self.CRT.estimate_rotation = 'estimate_rotation'
self.CNT.estimate_scale = 'estimate_scale'
self.CNT.estimate_rotation = 'estimate_rotation'
self.CST.estimate_scale = 'estimate_scale'
self.CST.estimate_rotation = 'estimate_rotation'
self.pause_time = 10
###########################Primer Region
im0 = cv2.imread('~/catikin_ws_c/src/cmt/scripts/imas.jpg',
flags=cv2.IMREAD_COLOR)
if im0:
print(im0.shape)
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im0)
print('Selecciona Primer Region')
(tl, br) = util.get_rect(im_draw)
print('using %i, %i as init bb', tl, br)
self.CRT.initialise(im_gray0, tl, br)
###########################Segunda Region
print('Selecciona Segunda Region')
(tl, br) = util.get_rect(im_draw)
print('using %i, %i as init bb', tl, br)
self.CNT.initialise(im_gray0, tl, br)
############################Tercer Region
print('Selecciona Tercer Region')
(tl, br) = util.get_rect(im_draw)
print('using %i, %i as init bb', tl, br)
self.CST.initialise(im_gray0, tl, br)
self.frame = 1
def _prompt_for_target(self):
"""Prompt user for target bounding box.
Launches an image viewer on which the user draws a bounding box.
Returns:
(int, int, int, int): bbox in (x, y, w, h) format.
"""
tl, br = util.get_rect(self.current_image)
return evaluation.BboxFormats.convert_bbox_format(
tl + br, evaluation.BboxFormats.TLBR, evaluation.BboxFormats.CCWH)
class Car(vehicle.Vehicle):
vertices = util.get_rect(6, 4)
height = 4.5
accel_force = -100.0
turn_force = 3
idle_turn_factor = .3
max_speed = 40
min_speed = -max_speed * .3
max_angular_velocity = 4
seat_offset = 1
wheel_offset = 2
side_friction = 3
friction_speed = 20
stop_speed = .1
a_damping = .4
timer = 0
def __init__(self, app):
vehicle.Vehicle.__init__(self, app)
wheel_tex = 'wheel.jpg'
self.wheel_rots = set()
with self.canvas:
self.rot = Rotate(0, 0, 1, 0)
Translate(0, 1.5, 0)
self.app.graphic_data.draw_mesh('models/car',
self.canvas,
texture='metal.jpg')
axel_width = 2.5
PushMatrix()
Translate(2, 0, 0)
PushMatrix()
Translate(0, 0, -axel_width)
self.fl_rot = Rotate(0, 0, 1, 0)
self.wheel_rots.add(Rotate(0, 0, 0, 1))
self.app.graphic_data.draw_mesh('models/wheel',
self.canvas,
wheel_tex)
PopMatrix()
PushMatrix()
Translate(0, 0, axel_width)
self.fr_rot = Rotate(0, 0, 1, 0)
self.wheel_rots.add(Rotate(0, 0, 0, 1))
self.app.graphic_data.draw_mesh('models/wheel',
self.canvas,
wheel_tex)
PopMatrix()
PopMatrix()
PushMatrix()
Translate(-2, 0, 0)
self.wheel_rots.add(Rotate(0, 0, 0, 1))
PushMatrix()
Translate(0, 0, -axel_width)
self.app.graphic_data.draw_mesh('models/wheel',
self.canvas,
wheel_tex)
PopMatrix()
PushMatrix()
Translate(0, 0, axel_width)
self.app.graphic_data.draw_mesh('models/wheel',
self.canvas,
wheel_tex)
PopMatrix()
PopMatrix()
def move(self, input_vector, dt):
self.input_vector = list(input_vector)
accel = self.throttle_down - self.throttle_up
self.local_vel = pymunk.Vec2d(self.collision.body.velocity).rotated(-self.collision.body.angle)
if self.local_vel.x:
mag = self.local_vel.x
if not accel:
mag *= self.idle_turn_factor
self.input_vector[0] *= mag
else:
self.input_vector[0] = 0
self.collision.body.apply_impulse_at_local_point((accel * self.accel_force * dt,
self.input_vector[0] * self.turn_force * dt),
(self.wheel_offset, 0))
def update(self, dt):
accel = (self.throttle_down - self.throttle_up)
target_angle = self.input_vector[0] * accel
da = accel * 1000 * dt
for r in self.wheel_rots:
r.angle += da
da = (target_angle - self.fr_rot.angle) * .2
self.fr_rot.angle += da
self.fl_rot.angle += da
self.rot.angle = self.collision.body.angle * -57.3
if not self.collision.standing:
self.collision.dy -= 10 * dt
self.collision.move_y(dt)
vehicle.Vehicle.update(self, dt)
raise Exception('Unable to parse bounding box')
if len(values) != 4:
raise Exception('Bounding box must have exactly 4 elements')
bbox = np.array(values)
# Convert to point representation, adding singleton dimension
bbox = util.bb2pts(bbox[None, :])
# Squeeze
bbox = bbox[0, :]
tl = bbox[:2]
br = bbox[2:4]
else:
# Get rectangle input from user
(tl, br) = util.get_rect(im_draw)
print 'using', tl, br, 'as init bb'
CMT.initialise(im_gray0, tl, br)
frame = 1
while True:
# Read image
status, im = cap.read()
if not status:
break
im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im)
tic = time.time()
def main(estimate_scale=True,
estimate_rotation=True,
input_box=None,
input_pic=None,
output=None):
CMT.estimate_scale = estimate_scale
CMT.estimate_rotation = estimate_rotation
pause_time = 10
skip = None
preview = None
quiet = False
if input_pic is not None:
print input_pic
input_dir = input_pic
elif args.inputpath is not None:
print args.inputpath
input_dir = args.inputpath
else:
input_dir = None
if output is not None:
output_dir = output
elif args.output is not None:
output_dir = args.output
else:
output_dir = None
if input_box is not None:
init_box = input_box
elif args.bbox is not None:
init_box = args.bbox
else:
init_box = None
if output_dir is not None:
quiet = True
if not os.path.exists(output_dir):
os.mkdir(output_dir)
elif not os.path.isdir(output_dir):
raise Exception(output_dir + ' exists, but is not a directory')
# Clean up
cv2.destroyAllWindows()
if input_dir is not None:
# If a path to a file was given, assume it is a single video file
if os.path.isfile(input_dir):
cap = cv2.VideoCapture(input_dir)
#Skip first frames if required
if skip is not None:
cap.set(cv2.cv.CV_CAP_PROP_POS_FRAMES, skip)
# Otherwise assume it is a format string for reading images
else:
cap = util.FileVideoCapture(input_dir)
#Skip first frames if required
if skip is not None:
cap.frame = 1 + skip
# By default do not show preview in both cases
if preview is None:
preview = False
else:
# If no input path was specified, open camera device
cap = cv2.VideoCapture(0)
if preview is None:
preview = True
# Check if videocapture is working
if not cap.isOpened():
print 'Unable to open video input.'
sys.exit(1)
while preview:
status, im = cap.read()
cv2.imshow('Preview', im)
k = cv2.waitKey(10)
if not k == -1:
break
# Read first frame
status, im0 = cap.read()
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im0)
if init_box is not None:
# Try to disassemble user specified bounding box
values = init_box.split(',')
try:
values = [int(v) for v in values]
except:
raise Exception('Unable to parse bounding box')
if len(values) != 4:
raise Exception('Bounding box must have exactly 4 elements')
bbox = np.array(values)
# Convert to point representation, adding singleton dimension
bbox = util.bb2pts(bbox[None, :]) #bbox[None, :]=array([[a, b, c,d]])
# Squeeze
bbox = bbox[0, :]
tl = bbox[:2]
br = bbox[2:4]
else:
# Get rectangle input from user
(tl, br) = util.get_rect(im_draw)
print 'using', tl, br, 'as init bb'
#CMT.initialise(im0, tl, br)
CMT.initialise(im_gray0, tl, br)
#util.draw_keypoints(selected_keypoints, im_gray0,(255, 0, 0))
frame = 1
while True:
# Read image
status, im = cap.read()
if not status:
break
im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im)
tic = time.time()
CMT.process_frame(im)
#CMT.process_frame(im_gray)
toc = time.time()
# Display results
# Draw updated estimate
if CMT.has_result:
cv2.line(im_draw, CMT.tl, CMT.tr, (255, 0, 0), 2)
cv2.line(im_draw, CMT.tr, CMT.br, (255, 0, 0), 2)
cv2.line(im_draw, CMT.br, CMT.bl, (255, 0, 0), 2)
cv2.line(im_draw, CMT.bl, CMT.tl, (255, 0, 0), 2)
# cv2.imshow("im_draw1",im_draw)
# cv2.waitKey()
#util.draw_keypoints(CMT.tracked_keypoints, im_draw, (255, 255, 255))
# this is from simplescale
#util.draw_keypoints(CMT.votes[:, :2], im_draw) # blue
#util.draw_keypoints(CMT.outliers[:, :2], im_draw, (0, 0, 255))
if output_dir is not None:
# Original image
# cv2.imwrite('{0}/input_{1:08d}.png'.format(output_dir, frame), im)
# Output image
cv2.imwrite('{0}/output_{1:08d}.png'.format(output_dir, frame),
im_draw)
'''
# Keypoints
with open('{0}/keypoints_{1:08d}.csv'.format(output_dir, frame), 'w') as f:
f.write('x y\n')
np.savetxt(f, CMT.tracked_keypoints[:, :2], fmt='%.2f')
# Outlier
with open('{0}/outliers_{1:08d}.csv'.format(output_dir, frame), 'w') as f:
f.write('x y\n')
np.savetxt(f, CMT.outliers, fmt='%.2f')
# Votes
with open('{0}/votes_{1:08d}.csv'.format(output_dir, frame), 'w') as f:
f.write('x y\n')
np.savetxt(f, CMT.votes, fmt='%.2f')
'''
# Bounding box
# with open('{0}/bbox_{1:08d}.csv'.format(output_dir, frame), 'w') as f:
with open('{0}/bbox.txt'.format(output_dir, frame), 'a') as f:
# f.write('x y\n')
# Duplicate entry tl is not a mistake, as it is used as a drawing instruction
# np.savetxt(f, np.array((CMT.tl, CMT.tr, CMT.br, CMT.bl, CMT.tl)), fmt='%.2f')
L = []
for i in [CMT.tl, CMT.tr, CMT.br, CMT.bl]:
#st = '(' + str(int(i[0])) + ' ' + str(int(i[1])) + ')'
st = '(' + str(float(i[0])) + ' ' + str(float(i[1])) + ')'
L.append(st)
bb = ','.join(L)
f.write(bb + '\n')
if not quiet:
cv2.imshow('main', im_draw)
# Check key input
k = cv2.waitKey(pause_time)
key = chr(k & 255)
if key == 'q':
break
if key == 'd':
import ipdb
ipdb.set_trace()
# Remember image
im_prev = im_gray
# Advance frame number
frame += 1
raise Exception('Unable to parse bounding box')
if len(values) != 4:
raise Exception('Bounding box must have exactly 4 elements')
bbox = np.array(values)
# Convert to point representation, adding singleton dimension
bbox = util.bb2pts(bbox[None, :])
# Squeeze
bbox = bbox[0, :]
tl = bbox[:2]
br = bbox[2:4]
else:
# Get rectangle input from user
(tl, br) = util.get_rect(im_draw)
print 'using', tl, br, 'as init bb'
CMT.initialise(im_gray0, tl, br)
frame = 1
while True:
# Read image
status, im = cap.read()
if not status:
break
im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im)
def callback(self, data):
global frame
global started
#global cv_image
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
print(self.bridge.imgmsg_to_cv2(data, "bgr8"))
except CvBridgeError as e:
print(e)
# Clean up
preview = 'preview'
# If no input path was specified, open camera device
#cap = cv2.VideoCapture(0)
if preview is None:
preview = True
# Check if videocapture is working
#if not cap.isOpened():
# print("Unable to open video input.")
# sys.exit(1)
#while preview:
#status, im = cap.read()
status = True
im = cv_image
if started == 0:
cv2.imshow('Preview', im)
#cv2.imshow('Preview', cv_image)
k = cv2.waitKey(10)
if not k == -1:
#break
im0 = cv_image
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
#im_gray0 = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(cv_image)
(tl, br) = util.get_rect(im_draw)
print("using", tl, br, "as init bb")
CMT.initialise(im_gray0, tl, br)
frame = 1
started = 1
cv2.destroyAllWindows()
# Read first frame
#status, im0 = cap.read()
if started == 1:
# Read image
#status, im = cap.read()
#im=cv_image
if not status:
cv2.destroyAllWindows()
sys.exit(1)
im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
#im_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im)
#im_draw = np.copy(cv_image)
tic = time.time()
CMT.process_frame(im_gray)
toc = time.time()
# Display results
# Draw updated estimate
if CMT.has_result:
cv2.line(im_draw, CMT.tl, CMT.tr, (255, 0, 0), 4)
cv2.line(im_draw, CMT.tr, CMT.br, (255, 0, 0), 4)
cv2.line(im_draw, CMT.br, CMT.bl, (255, 0, 0), 4)
cv2.line(im_draw, CMT.bl, CMT.tl, (255, 0, 0), 4)
util.draw_keypoints(CMT.tracked_keypoints, im_draw,
(255, 255, 255))
# this is from simplescale
util.draw_keypoints(CMT.votes[:, :2], im_draw) # blue
util.draw_keypoints(CMT.outliers[:, :2], im_draw, (0, 0, 255))
cv2.imshow('main', im_draw)
#cv2.imshow('main', im_draw)
# Check key input
k = cv2.waitKey(pause_time)
key = chr(k & 255)
if key == 'q':
#cap.release()
cv2.destroyAllWindows()
rospy.signal_shutdown("ROSPy Shutdown")
#break
sys.exit(1)
if key == 'd':
import ipdb
ipdb.set_trace()
# Remember image
im_prev = im_gray
# Advance frame number
frame += 1
print(
"{5:04d}: center: {0:.2f},{1:.2f} scale: {2:.2f}, active: {3:03d}, {4:04.0f}ms"
.format(CMT.center[0], CMT.center[1], CMT.scale_estimate,
CMT.active_keypoints.shape[0], 1000 * (toc - tic),
frame))
#(rows,cols,channels) = cv_image.shape
#if cols > 60 and rows > 60 :
# cv2.circle(cv_image, (50,50), 10, 255)
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
class Helicopter(vehicle.Vehicle):
vertices = util.get_rect(6, 4)
height = 5.5
accel_force = -100.0
turn_force = 100
idle_turn_factor = .3
max_speed = 20
min_speed = -max_speed * .5
max_angular_velocity = 2
max_y_vel = 10
seat_offset = -.1
side_friction = 3
friction_speed = 15
stop_speed = .1
a_damping = .4
prop_vel = 0
def __init__(self, app):
vehicle.Vehicle.__init__(self, app)
with self.canvas:
self.rot = Rotate(0, 0, 1, 0)
self.pitch = Rotate(0, 0, 0, 1)
self.roll = Rotate(0, 1, 0, 0)
self.app.graphic_data.draw_mesh('models/helicopter',
self.canvas,
texture='metal.jpg')
PushMatrix()
self.prop_rot = Rotate(0, 0, 1, 0)
Translate(0, 4, 0)
self.app.graphic_data.draw_mesh('models/propellers',
self.canvas,
texture=None)
PopMatrix()
def velocity_func(self, body, gravity, damping, dt):
if self.collision.standing:
body.velocity *= .5
body.angular_velocity *= 0
else:
if self.input_vector:
if abs(body.angular_velocity) > self.max_angular_velocity:
body.angular_velocity = self.max_angular_velocity * cmp(
body.angular_velocity, 0)
self.local_vel = pymunk.Vec2d(
body.velocity).rotated(-body.angle)
self.local_vel.y *= (1 - self.side_friction * dt)
self.local_vel.x = min(self.max_speed,
max(self.min_speed, self.local_vel.x))
body.velocity = self.local_vel.rotated(body.angle)
else:
friction = self.friction_speed * dt
if self.local_vel.x < friction:
body.velocity *= .8
body.angular_velocity *= 0
else:
body.velocity -= body.velocity.normalized() * friction
body.angular_velocity *= 1 - (.9 * dt)
def move(self, input_vector, dt):
if not self.collision.standing:
self.local_vel = pymunk.Vec2d(
self.collision.body.velocity).rotated(
-self.collision.body.angle)
self.input_vector = input_vector
self.collision.body.apply_impulse_at_local_point(
(self.input_vector[1] * self.accel_force * dt,
self.input_vector[0] * self.turn_force * dt),
(self.wheel_offset, 0))
def update(self, dt):
if self.turned_on:
accel = (self.throttle_up - self.throttle_down)
target_vel = accel * 200 + 700
self.prop_vel += cmp(target_vel, self.prop_vel) * 500 * dt
self.prop_rot.angle += self.prop_vel * dt
elif self.prop_vel > 0:
self.prop_vel -= 200 * dt
if self.prop_vel < 0:
self.prop_vel = 0
else:
self.prop_rot.angle += self.prop_vel * dt
self.rot.angle = self.collision.body.angle * -57.3
if self.collision.standing:
if self.collision.dy > 0:
self.collision.standing = None
self.pitch.angle = 0
self.roll.angle = 0
else:
self.pitch.angle += (self.input_vector[1] * 20 -
self.pitch.angle) * .05
self.roll.angle += (self.input_vector[0] * 20 -
self.roll.angle) * .05
self.collision.dy += (self.throttle_up - self.throttle_down -
.2) * 30 * dt
if abs(self.collision.dy) > self.max_y_vel:
self.collision.dy = cmp(self.collision.dy, 0) * self.max_y_vel
vehicle.Vehicle.update(self, dt)
status, banana = cap.read()
banana=cv2.imread(args.banana, cv2.IMREAD_GRAYSCALE )
tl=[0,0]
br=[banana.shape[1],banana.shape[0]]
status, im0 = cap.read()
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
# Trying to overlay banana on top of the captured frame.
x_offset=y_offset=0
im_gray0[y_offset:y_offset+banana.shape[0], x_offset:x_offset+banana.shape[1]] = banana
# Temp drawing of the images. Rectangle is ignored here.
im_draw = np.copy(im_gray0)
(tl2, br2) = util.get_rect(im_draw)
print 'using', tl, br, 'as init bb'
CMT.initialise(im_gray0, tl, br)
else:
status, im0 = cap.read()
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im0)
(tl, br) = util.get_rect(im_draw)
print 'using', tl, br, 'as init bb'
CMT.initialise(im_gray0, tl, br)
status, im0 = cap.read()
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im0)
