def key_press(e):
k = e.key()
if k == ord('+'):
constant[0] *= 2
elif k == ord('-'):
constant[0] /= 2
elif k == QtCore.Qt.Key_Space:
im_gray0, (tl, br) = detect()
CMT.initialise(im_gray0, tl, br)
print constant[0]
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.CMT = CMT.CMT()
self.bridge = CvBridge()
self.first = True
self.rect = None
self.init_img = None
self.center = None
self.prev_center = None
self.h = None
self.w = None
self.linear_speed_x = 0.1
self.k_yaw = 0.0005
self.k_alt = 0.0005
self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image",
Image, self.callback)
self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel",
numpy_msg(geometry_msgs.Twist),
queue_size=1)
def __init__(self, input_queue):
multiprocessing.Process.__init__(self)
self.CMT = CMT.CMT()
self.templet = cv2.resize(cv2.imread(config['Video']['original']), (FRAME_WIDTH, FRAME_HEIGHT))
self.cap = cv2.VideoCapture(config['Video']['cap'])
self.message_queue = input_queue
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
self.cap.set(cv2.CAP_PROP_FPS, 10)
def __init__(self):
# self.image_pub = rospy.Publisher("image_topic_2",Image)
self.CMT = CMT.CMT()
self.bridge = CvBridge()
self.tracker = cv2.TrackerTLD_create()
self.count = 0
self.first = True
self.first_ttc = True
self.rect = None
self.init_img = None
self.center = None
self.prev_center = None
self.hit_buoy = False
self.track_window = None
self.h = None
self.w = None
self.dt = 1.0 / 19.0
self.bbox_h = None
self.bbox_h_init = None
self.bbox_h_prev = None
self.linear_speed_x = 0.35
self.k_yaw = 0.0005
self.k_alt = 0.0005
# self.k_yaw = 0.0008
# self.k_alt = 0.0008
self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image",
Image, self.callback)
self.jerk_sub = rospy.Subscriber("/jerk", Float32, self.jerk_callback)
self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel",
numpy_msg(geometry_msgs.Twist),
queue_size=1)
smach.State.__init__(self, outcomes=['hit', 'in_pursuit'])
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 __init__(self):
"""Initialize CMT detector"""
self._cmt = CMT.CMT()
self.location = None
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
import CMT
import util
parser = argparse.ArgumentParser(
description=
'Self-correction Object Tracking Algorithm Based on Static-Adaptive Appearance Model'
)
parser.add_argument('inputpath', nargs='?', help='The input path.')
parser.add_argument('--bbox',
dest='bbox',
help='Specify initial bounding box.')
parser.add_argument('--output-dir',
dest='output',
help='Specify a directory for output data.')
args = parser.parse_args()
CMT = CMT.CMT()
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:
results = empty((num_frames, 4))
results[:] = nan
results[0, :] = init_region
frame = 0
im0 = cv2.imread(images[frame])
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im0)
tl, br = (util.array_to_int_tuple(init_region[:2]), util.array_to_int_tuple(init_region[:2] + init_region[2:4] - 1))
try:
CMT.initialise(im_gray0, tl, br)
while frame < num_frames:
im = cv2.imread(images[frame])
im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
CMT.process_frame(im_gray)
results[frame, :] = CMT.bb
# Advance frame number
frame += 1
except:
pass # Swallow errors
np.savetxt('output.txt', results, delimiter=',')
else:
# Clean up
region = None
tracker = None
with trax.server.Server(options, verbose=True) as server:
while True:
request = server.wait()
if request.type in ["quit", "error"]:
break
if request.type == "initialize":
region = request.region
im0 = read_trax_image(request.image)
im_gray0 = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
tl = (region.x, region.y)
br = (region.x + region.width - 1, region.y + region.height - 1)
tracker = None
try:
tracker = CMT.CMT()
tracker.initialise(im_gray0, tl, br)
except Exception:
tracker = None
else:
im = read_trax_image(request.image)
im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
if not tracker == None:
tracker.process_frame(im_gray)
if not math.isnan(tracker.bb[0]):
region = trax.region.Rectangle(tracker.bb[0],
tracker.bb[1],
tracker.bb[2],
tracker.bb[3])
else:
region = trax.region.Special(0)
minNeighbors=5,
minSize=(30, 30),
flags=cv2.cv.CV_HAAR_SCALE_IMAGE
)
if len(faces) == 0:
sys.stdout.write('.')
sys.stdout.flush()
print 'done'
(x, y, w, h) = faces[0]
(tl, br) = ((x, y), (x + w, y + h))
return im_gray0, (tl, br)
im_gray0, (tl, br) = detect()
CMT.initialise(im_gray0, tl, br)
face_pos = []
import bmo
constant = [float(5e7)]
from PySide import QtGui, QtCore
def key_press(e):
k = e.key()
if k == ord('+'):
constant[0] *= 2
elif k == ord('-'):
constant[0] /= 2
elif k == QtCore.Qt.Key_Space:
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)
def run_CMT(input_path, skip_frames, bbox, SHOW_IMAGES=False, clip_name='main'):
from numpy import empty, nan
import CMT
import util
CMT = CMT.CMT()
CMT.estimate_scale = True
CMT.estimate_rotation = False
# read video and set delay accordingly
cap = cv2.VideoCapture(input_path)
cap.set(cv2.cv.CV_CAP_PROP_POS_FRAMES, skip_frames)
# something wrong with reading the shot it seems
if not cap.isOpened():
print '#Unable to open video input.'
sys.exit(1)
status, im0 = cap.read()
im_gray0_ = cv2.cvtColor(im0, cv2.COLOR_BGR2GRAY)
im_gray0 = cv2.equalizeHist(im_gray0_)
im_draw = np.copy(im0)
# prepare initial bbox values
bbox_values = [int(v) for v in bbox]
bbox = array(bbox_values)
# Convert to point representation, adding singleton dimension
bbox = util.bb2pts(bbox[None, :])
# Squeeze
bbox = bbox[0, :]
tl = bbox[:2]
br = bbox[2:4]
print '#using', tl, br, 'as init bb'
CMT_TRACKS=[]
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_gray = cv2.equalizeHist(im_gray_)
im_draw = np.copy(im)
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)
CMT_rect = [CMT.tl, CMT.tr,CMT.bl,CMT.br]
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 SHOW_IMAGES:
cv2.imshow(clip_name, im_draw)
# Check key input
k = cv2.cv.WaitKey(10) & 0xff
# if args.quiet:
# if CMT.has_result: print '1'#, print_str
# else: print '0'#, print_str
# Remember image
im_prev = im_gray
# Advance frame number
frame += 1
print_str = '{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)
if CMT.has_result: CMT_TRACKS.append([1,CMT_rect])
else: CMT_TRACKS.append([0,nan])#, print_str
return CMT_TRACKS
#!/usr/bin/env python
import argparse
import cv2
from numpy import empty, nan
import os
import sys
import time
import CMT
import numpy as np
import util
CMTobj = CMT.CMT()
parser = argparse.ArgumentParser(description='Track an object.')
parser.add_argument('inputpath', nargs='?', help='The input path.')
parser.add_argument('--challenge',
dest='challenge',
action='store_true',
help='Enter challenge mode.')
parser.add_argument('--preview',
dest='preview',
action='store_const',
const=True,
default=None,
help='Force preview')
parser.add_argument('--no-preview',
dest='preview',
action='store_const',
import cv2
from numpy import empty, nan
import os
import threading
import multiprocessing
import sys
import time
from multiprocessing.dummy import Pool as Threadpool
import CMT
import numpy as np
#import CMT2
import util
from functools import partial
CMT = [CMT.CMT(), CMT.CMT()]
#CMT2 = CMT2.CMT2()
parser = argparse.ArgumentParser(description='Track an object.')
parser.add_argument('inputpath', nargs='?', help='The input path.')
parser.add_argument('--challenge',
dest='challenge',
action='store_true',
help='Enter challenge mode.')
parser.add_argument('--preview',
dest='preview',
action='store_const',
const=True,
default=None,
help='Force preview')
#!/usr/bin/env python
import argparse
import cv2
from numpy import empty, nan
import os
import sys
import time
import CMT
import numpy as np
import util
CMT = CMT.CMT()
parser = argparse.ArgumentParser(description='Track an object.')
parser.add_argument('inputpath', nargs='?', help='The input path.')
parser.add_argument('--challenge',
dest='challenge',
action='store_true',
help='Enter challenge mode.')
parser.add_argument('--preview',
dest='preview',
action='store_const',
const=True,
default=None,
help='Force preview')
parser.add_argument('--no-preview',
dest='preview',
action='store_const',
def loop():
# Read image
status, im = read()
if not status:
pass
im_gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
im_draw = np.copy(im)
tic = time.time()
CMT.process_frame(im_gray)
toc = time.time()
# Display results
coords = None
# 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)
shape = im_draw.shape
coords = tuple(CMT.center[i] / shape[i] for i in xrange(2))
diag = [ np.array(CMT.tl) - np.array(CMT.br),
np.array(CMT.tr) - np.array(CMT.bl) ]
length = np.array( [ math.sqrt(x.dot(x)) for x in diag ] )
size = np.amax( length )
depth = (constant[0] / float( shape[0])) / size
# the webcam is at (0, -0.5)
coords = (coords[0], -0.5 + coords[1], depth)
# 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 not args.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
return coords
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_cmt",
Image,
queue_size=1)
self.vis_odo_pub = rospy.Publisher("visual_odometry",
Odometry,
queue_size=1)
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,
queue_size=1)
self.yaw_sub = rospy.Subscriber("model_car/yaw2",
Float32,
self.headingcallback,
queue_size=1)
#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_cuadri_1500.jpg', flags=cv2.IMREAD_GRAYSCALE)
im0 = cv2.imread('./frame_cap13.jpg', flags=cv2.IMREAD_GRAYSCALE)
#im0 = cv2.imread('./frame_cap_13.jpg', flags=cv2.IMREAD_GRAYSCALE)
#im0 = cv2.imread('./frame_fisica3.jpg', flags=cv2.IMREAD_GRAYSCALE)
#im0 = cv2.imread('./frame_cap_16.jpg', flags=cv2.IMREAD_GRAYSCALE)
#im0 = cv2.imread('./frame_cap_noche.jpg', flags=cv2.IMREAD_GRAYSCALE)
#im0 = cv2.imread('./frame_cap3.jpg', flags=cv2.IMREAD_GRAYSCALE)#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
#print(np.size(im_gray0))
#im_gray0=cv2.blur(im0,(3,3))
#cv2.line(im_gray0, (70,0), (70,300), (0, 0, 0), 150)
#cv2.line(im_gray0, (490,0), (490,300), (0, 0, 0), 150)
#im_gray0=cv2.resize(im0, (360, 240))
#im_draw = np.copy(im0)
print('Selecciona Primer Region')
#tl=(84, 55) #talvez
#br=(557, 307#tal vez
#tl=(68, 68)
#br=(544, 316)
#tl=(35,35)
#bl=(605,325)
#tl=(78, 59) #ultimos con156kp
#br=(553, 297)#uktimos con 156kp
tl = (73, 56) #labo umi
br = (449, 244) #labo umi
#tl=(88, 58) #camaras labo
#br=(429, 240)#camaras labo
#tl=(93, 61) #camaras labo nuevo
#br=(436, 241)#labo camaras nuevo
tl = (85, 59) #last camrasa
br = (428, 239) #lascamaras
#tl=(166, 64)
#br=(348, 237)
#(84, 63) (429, 237)
#tl=(87, 57)#camaras
#br= (426, 234)#camaras
tl = (87, 57) #camaras
br = (326, 154) #camaras
tl = (59, 44) #camaras peque
br = (300, 195) #camaras peque
tl = (128, 25)
br = (394, 278)
##cuadrilatero
#tl=(157,6)
#br=(408,257)
tl = (154, 21) #55)
br = (392, 259)
#tl=(40+20,27+20)
#br=(269-20, 256-20)
tl = (40 + 10, 27 + 10)
br = (269 - 10, 256 - 10)
tl = (320 - 146, 180 - 146)
br = (320 + 146, 180 + 146)
#(158, 14) (392, 261)
##fisica
#tl=(118, 54)
#br=(348, 250)
#(tl, br) = util.get_rect(im_gray0)
print('usando %i, %i como init bb', tl, br)
self.CRT.initialise(im_gray0, tl, br)
print('Num keypoint init', self.CRT.num_initial_keypoints)
self.frame = 1
self.conta = 0
self.frame_id = 'visual_odometry'
self.child_frame_id = 'base_link2'
self.msg = Odometry()
tl = bbox[:2]
br = bbox[2:4]
else:
# print("bbox arg is required")
# exit()
(tl, br) = util.get_rect(im_draw)
if not quiet:
print('using', tl, br, 'as init bb')
socket.send(b"" + str(tl[0]) + ":" + str(tl[1]) + "|" + str(br[0]) + ":" + str(tl[1]) + "|" + str(br[0]) + ":" + str(br[1]) + "|" + str(tl[0]) + ":" + str(br[1]))
message_reply = socket.recv()
tic = time.time()
if args.tracker == 'CMT':
CMT.initialise(im_gray0, tl, br)
else:
CMT.init(frame, (tl[0], tl[1], br[0] - tl[0], br[1] - tl[1]))
toc = time.time()
with open("log.txt", "a") as logfile:
s = "Tracker Init Time: " + str(args.tracker) + ": " + str(toc - tic)
if not quiet:
print(s)
logfile.write(s + "\n")
time_start = time.time()
elif (frame_counter > frame_init_at):
im_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
