def main():
args = parse_args()
image_path = os.path.join(args.path, 'image_02/', args.seq)
oxts_path = os.path.join(args.path, 'oxts/', args.seq + '.txt')
itms = [
os.path.join(image_path, it) for it in os.listdir(image_path)
if it.endswith('.png')
]
annotation = get_anno_tracking(oxts_path)
img_shape = cv2.imread(itms[0], 0).shape
cam = PinholeCamera(img_shape[1], img_shape[0], 718.8560, 718.8560,
607.1928, 185.2157)
vo = VisualOdometry(cam, annotation, args.use_abs_scale, args.skip_frame)
wb = Whiteboard()
for img_id in range(0, len(itms), args.skip_frame):
img = cv2.imread(
args.path + 'image_02/' + args.seq + '/' + str(img_id).zfill(6) +
'.png', 0)
vo.update(img, img_id)
if (img_id > 0):
cur_t = vo.cur_t.squeeze()
x, y, z = cur_t[0], cur_t[2], -cur_t[1]
else:
x, y, z = 0., 0., 0.
t_loc = np.array([vo.trueX, vo.trueY, vo.trueZ])
t_color = (0, 0, 255)
p_loc = np.array([x, y, z])
p_color = (img_id * 255.0 / len(itms),
255 - img_id * 255.0 / len(itms), 0)
wb.draw(img_id, 'True', t_loc, t_color)
wb.draw(img_id, 'Pred', p_loc, p_color)
wb.show(img)
if args.save_name:
wb.save(args.save_name)
def getVO(data_path, rgb_txt):
cam = PinholeCamera(640.0, 480.0, 517.3, 516.5, 318.6, 255.3, 0.2624,
-0.9531, -0.0054, 0.0026, 1.1633)
imgList = getImageLists()
vo = VisualOdometry(cam, imgList)
# xyz= np.array([[0],[0],[0]])
img = cv2.imread('./Data/' + imgList[0], 0)
f = open("teat2.txt", "w")
for img_id in range(len(imgList)):
# img = cv2.imread('./Data/'+imgList[img_id], 0)
vo.update(img, img_id)
if img_id > 1:
cur_t = vo.cur_t
cur_r = vo.cur_R
# xyz=cur_r.dot(xyz)+cur_t
name = imgList[img_id].split('/')[1].split('.')[0]
pose = transRo2Qu(cur_t, cur_r)
f.write(name + ' ' + pose + '\n')
print(img_id)
import numpy as np
import cv2
from matplotlib import pyplot as plt
from visual_odometry import PinholeCamera, VisualOdometry
sequence = 6
height, width = np.shape(
cv2.imread(
'../dataset/sequences/' + str(sequence).zfill(2) + '/image_0/' +
str(0).zfill(6) + '.png', 0))
cam = PinholeCamera(width, height, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam, '../dataset/poses/' + str(sequence).zfill(2) + '.txt')
window_width, window_height = 1000, 1000
traj = np.zeros((window_width, window_height, 3), dtype=np.uint8)
x, y, z = 0., 0., 0.
x_1, y_1, z_1 = 0., 0., 0.
true_x1, true_y1, true_z1 = 0., 0., 0.
trajectory = np.zeros((len(vo.annotations), 3))
gt = np.zeros((len(vo.annotations), 3))
for img_id in range(len(
vo.annotations)): # Terminates on end of image sequence
img = cv2.imread(
'../dataset/sequences/' + str(sequence).zfill(2) + '/image_0/' +
str(img_id).zfill(6) + '.png', 0)
vo.update(img, img_id)
cur_t = vo.cur_t
cur_t_u = vo.cur_t_unscaled
if (img_id > 2):
import numpy as np
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
# width, height, fx, fy, cx, cy,
cam = PinholeCamera(640, 480, 45, 45, 22.3, 14.9)
vo = VisualOdometry(cam, '/home/chris/Desktop/VO/dataset/poses/00.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
cap = cv2.VideoCapture(0)
for img_id in xrange(4541):
_, img = cap.read()
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
vo.update(img, img_id)
cur_t = vo.cur_t
if (img_id > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
x, y, z = 0., 0., 0.
draw_x, draw_y = int(x) + 290, int(z) + 90
true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90
cv2.circle(traj, (draw_x, draw_y), 1,
(img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1)
cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2)
cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1,
(255, 255, 255), 1, 8)
K = sim_data['K']
i_state = sim_data['i_state']
time = sim_data['time']
images = sim_data['landing']
RT_vector = sim_data['RT']
R_i2bcam_vector = sim_data['R_i2bcam']
num_images = images.shape[3]
max_images = 1000
step_size = (len(time) - 1) // num_images
# define pinhole camera model
# pixel height of NEAR MSI camera
width = 537
height = 244
cam = PinholeCamera(width, height, K[0, 0], K[1, 1], K[0, 2], K[1, 2])
vo = VisualOdometry(cam, time, i_state, RT_vector, R_i2bcam_vector)
# define asteroid and dumbbell like in the simulation
ast = asteroid.Asteroid('itokawa', 64)
dum = dumbbell.Dumbbell(m1=500, m2=500, l=0.003)
# initialize an estimated trajectory vector
est_time = np.zeros(max_images)
est_pos = np.zeros((max_images, 3))
est_R = np.zeros((max_images, 9))
# do some math based on how many images there are and skipping frames
# loop over the images
for ii in range(max_images):
time_index = (ii + 1) * step_size
def visual_odometry():
# ---------------- Initial definitions -----------------------
drone_states = DroneStates()
ic = image_converter()
#pose = Reference()
rospy.Subscriber('sensor_depth/depth', Float32, drone_states.set_depth)
rospy.Subscriber('sensor_imu_1/data', ImuData, drone_states.imu_data)
rospy.Subscriber('camera/image_raw', Image, ic.callback)
rospy.Subscriber('observer/state_estimate', StateEstimate,
drone_states.set_estimate)
rospy.init_node('visual_odometry', anonymous=True)
#pub = rospy.Publisher('reference_generator/reference', Reference, queue_size=1)
# Camera matrix is for image size 1920 x 1080
mtx = np.array([[1.35445761E+03, 0.00000000E+00, 8.91069717E+02],
[0.00000000E+00, 1.37997405E+03, 7.56192877E+02],
[0.00000000E+00, 0.00000000E+00, 1.00000000E+00]])
dist = np.array(
[-0.2708139, 0.20052465, 2.08302E-02, 0.0002806, -0.10134601])
cam = PinholeCamera(960, 540, mtx[0, 0] / 2, mtx[1, 1] / 2, mtx[0, 2] / 2,
mtx[1, 2] / 2)
# --------------------------------------------------------------
vo = VisualOdometry(cam)
kf = VOKalmanFilter()
traj = np.zeros((600, 650, 3), np.uint8)
init = True
row = [
'p_x', 'p_y', 'p_z', 'time', 'x_hat', 'y_hat', 'z_hat', 'a_x', 'a_y',
'a_z'
]
f = open('VOestimates.csv', 'w+')
writer = csv.writer(f)
writer.writerow(row)
x_hat = np.zeros((12, 1))
drone_t = np.zeros((3, 1))
reference = Reference()
# controller setup:
surge_controller = controller(0.1, 0., 0.)
sway_controller = controller(0.08, 0., 0.)
yaw_controller = controller(0.009, 0., 0.005)
depth_controller = controller(3.5, 0., 0.0)
depth_controller.init_ref = 0.5
rate = rospy.Rate(7.5)
while not rospy.is_shutdown():
# while i < len(depth_data.depth):
t = time.time()
#ret, frame = cap.read()
#if not ret:
# continue
frame = ic.cv_image
#print(frame)
frame = cv2.undistort(frame, mtx, dist)
frame = cv2.resize(frame, (0, 0), fx=0.5, fy=0.5)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# run visual odometry
vo.update(frame)
if init:
x, y, z, = 0., 0., 0.
dt = time.time() - t
init = False
else:
dt = time.time() - t
cur_t = drone_states.R_cd.dot(vo.cur_t)
x, y, z = cur_t[0, 0], cur_t[1, 0], cur_t[2, 0]
roll, pitch, yaw = rotationMatrixToEulerAngles(vo.cur_R)
keypoints = []
for m in vo.good:
keypoints.append(vo.kp_cur[m.trainIdx])
frame = cv2.drawKeypoints(frame,
keypoints,
np.array([]),
color=(0, 255, 0),
flags=0)
# Kalman filtering of the estimates
#if isinstance(vo.scale, np.float64):
# dt = time.time() - t
# drone_t = drone_states.R_cd.dot(vo.t)
# u = np.array([[x], [y], [drone_states.z], [drone_states.p], [drone_states.q], [drone_states.r]])
# kf.update(u, dt)
# x_hat = kf.x_hat * dt
# write estimates to file for plotting
row = [x, y, z, roll, pitch, yaw]
writer.writerow(row)
draw_x, draw_y = int(y) * (1) + 290, int(x) * (-1) + 290
cv2.circle(traj, (draw_x, draw_y), 1, (0, 255, 0), 1)
cv2.rectangle(traj, (10, 20), (650, 60), (0, 0, 0), -1)
text = "Coordinates: x=%2fm y=%2fm z=%2fm fps: %f" % (
x_filtered, y_filtered, z_filtered, 1 / dt)
cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1,
(255, 255, 255), 1, 8)
cv2.imshow('Trajectory', traj)
cv2.imshow('Video', frame)
ch = cv2.waitKey(1)
if ch & 0xFF == ord('q'):
f.close()
break
# -------------CONTROLLERS-------------------
if vo.scale == 1.0:
reference.heave = depth_controller.pid(drone_states.z, dt)
depth_scale(depth_controller, drone_states.z, z, vo)
reference.surge = 0.
reference.yaw = -0.05 * reference.heave
reference.sway = -0.3 * reference.heave
if isinstance(vo.scale, np.float64):
reference.surge = surge_controller.pid(x_filtered, dt)
reference.sway = sway_controller.pid(y_filtered, dt)
if -0.1 < reference.sway < 0:
reference.sway = -0.1
reference.yaw = yaw_controller.pid(drone_states.psi, dt)
reference.heave = depth_controller.pid(drone_states.z, dt)
reference.depth = 0.
reference.depth_rate = 0.
reference.heading = 0.
reference.heading_rate = 0.
rate.sleep()
reference.surge = 0.
reference.sway = 0.
reference.yaw = 0.
reference.depth = 0.
reference.depth_rate = 0.
reference.heading = 0.
reference.heading_rate = 0.
pub.publish(reference)
return x_rot, y_rot
meta_data_path = './interpolated.csv'
base_path = "C:/Users/asus/Desktop/Self Driving/Comma Scratch/data"
skip_factor = 1
start = 8800
df = pd.read_csv(meta_data_path)
df = df[df['frame_id'] == 'center_camera'][::skip_factor][start:]
df['timestamp'] = df['timestamp'] / 1000000000
ins = pd.read_csv('imu.csv')
ins['timestamp'] = ins['timestamp'] / 1000000000
cam = PinholeCamera(640, 480, 2152.445406, 2166.161453, 268.010970, 302.594211)
vo = VisualOdometry(cam, meta_data_path)
model = Bicycle()
traj = np.zeros((600, 600, 3), dtype=np.uint8)
#rel_x, rel_y = df[['lat', 'long']].values[0]
prev_lat, prev_lon = df[['lat', 'long']].values[0]
xg, yg = 0, 0
R_y = 0.4
Q_y = 0.025
INIT_y = 0
INIT_VAR_y = 0.5**2
kalman_y = KalmanFilter(INIT_y, INIT_VAR_y, R_y**2, Q_y**2)
R_x = 0.6
import numpy as np
from glob import glob
import os
import os.path
import cv2
import math
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(752.0, 480.0, 458.654, 457.296, 367.215, 248.375)
vo = VisualOdometry(cam)
traj = np.zeros((600, 600, 3), dtype=np.uint8)
def rename_images():
#import os
#import os.path
#renames images for a dataset for easy import while maintaining the order
img_types = ['.png']
for dirpath, dirnames, fnames in os.walk(path):
imgs = [f for f in fnames if os.path.splitext(f)[1] in img_types]
imgs.sort()
for j, im in enumerate(imgs):
name, ext = os.path.splitext(im)
os.rename(os.path.join(dirpath, im),
os.path.join(dirpath, '{}.{}'.format(j, ext)))
def read_image():
images = []
def main():
# Parse commands arguments
argument = argparse.ArgumentParser()
argument.add_argument("-pose_path", help="specify the path of ground truth poses")
argument.add_argument("-image_dir", help="specify the directory of input images (undistorted)")
argument.add_argument("-image_end", help="specify the filename extension of input images (e.g. png)")
argument.add_argument("--v", help="set verbose as true", action="store_true")
argument.add_argument("--a", help="use gps scale info", action="store_true")
args = argument.parse_args()
verbose = args.v
absolute = args.a
# Check validation of arguments
image_dir = os.path.realpath(IMAGE_DIR)
if args.image_dir is not None:
if os.path.isdir(args.image_dir):
image_dir = os.path.realpath(args.image_dir)
pose_path = os.path.realpath(POSE_PATH)
if args.pose_path is not None:
if os.path.isfile(args.pose_path):
pose_path = os.path.realpath(args.pose_path)
with open(pose_path) as f:
poses_context = f.readlines()
image_end = IMAGE_END
if args.pose_path is not None:
image_end = args.image_end
if absolute:
if not os.path.isfile(pose_path):
print(PRIFIX + '[Error] The ground truth poses not found, required by absolute scale info!')
sys.exit()
# Find all images under image_dir (with image_end)
images_files_list = []
for file in os.listdir(image_dir):
if file.endswith(image_end):
images_files_list.append(image_dir + '/' + file)
if len(images_files_list) == 0:
print(PRIFIX + '[Error] There is no image ends with ' + image_end + ' under image_dir!')
sys.exit()
images_files_list.sort()
# Show basic information
print(PRIFIX + "===== HyphaROS Mono-VO Example =====")
print(PRIFIX + "the pose_path: " + pose_path)
print(PRIFIX + "The image_dir: " + image_dir)
print(PRIFIX + "The image_end: " + image_end)
print(PRIFIX + "Use GPS scale: " + str(absolute))
print(PRIFIX + "Total images found: " + str(len(images_files_list)))
print('\n')
# Initial VisualOdometry Object
camera_model = PinholeCamera(1241.0, 376.0, 718.8560,
718.8560, 607.1928, 185.2157)
vo = VisualOdometry(camera_model)
trajectory_plot = np.zeros((1000,1000,3), dtype=np.uint8)
# =======================
# ==== Bootstrapping ====
# =======================
print(PRIFIX + "===== Start Bootstrapping =====")
first_index = 0
second_index = first_index + 3
first_frame = cv2.imread(images_files_list[first_index], 0)
second_frame = cv2.imread(images_files_list[second_index], 0)
second_keypoints, first_keypoints = vo.bootstrapping(first_frame, second_frame)
print(PRIFIX + "First frame id: ", str(first_index) )
print(PRIFIX + "Second frame id: ", str(second_index) )
print(PRIFIX + "Initial matches: ", second_keypoints.shape[0] )
# Draw features tracking
show_image = drawTrackedFeatures(second_frame, first_frame,
second_keypoints, first_keypoints)
print(PRIFIX + "Wait any input to continue ...")
cv2.imshow('Front Camera', show_image)
cv2.waitKey()
# =======================
# ==== Frame Process ====
# =======================
print('\n')
print(PRIFIX + "===== Start Frame Processing =====")
print(PRIFIX + "Press 'ESC' to stop the loop.")
prev_frame = second_frame
prev_keypoints = second_keypoints
# Main loop for frame processing
for index in range(second_index+1, len(images_files_list)):
curr_frame = cv2.imread(images_files_list[index], 0)
# Get GPS ground truth trajectory
true_pose, true_scale = getGroundTruthAndScale(poses_context, index)
true_x, true_y = int(true_pose[0])+290, int(true_pose[2])+90
# Process frame w/o true scale info
if not absolute:
curr_keypoints, prev_keypoints, reinitial = vo.processFrame(curr_frame,
prev_frame,
prev_keypoints)
# Process frame w/ true scale info
else:
curr_keypoints, prev_keypoints, reinitial = vo.processFrame(curr_frame,
prev_frame,
prev_keypoints,
absolute_scale = true_scale)
# Get VO translation
curr_t = vo.curr_t
if(index > 2):
x, y, z = curr_t[0], curr_t[1], curr_t[2]
else:
x, y, z = 0., 0., 0.
odom_x, odom_y = int(x)+290, int(z)+90
if verbose:
print('\n')
print(PRIFIX + "Image index: ", str(index))
print(PRIFIX + "Odometry T:\n", vo.curr_t)
print(PRIFIX + "Odometry R:\n", vo.curr_R)
# Draw trajectory
cv2.circle(trajectory_plot, (odom_x,odom_y), 1, (index*255/4540,255-index*255/4540,0), 1)
cv2.circle(trajectory_plot, (true_x,true_y), 1, (0,0,255), 2)
cv2.rectangle(trajectory_plot, (10, 20), (600, 60), (0,0,0), -1)
text = "Coordinates: x=%2fm y=%2fm z=%2fm"%(x,y,z)
cv2.putText(trajectory_plot, text, (20,40), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,255), 1, 8)
cv2.imshow('Trajectory', trajectory_plot)
# Draw features tracking
show_image = drawTrackedFeatures(curr_frame, prev_frame,
curr_keypoints, prev_keypoints,
reinitial=reinitial)
cv2.imshow('Front Camera', show_image)
if verbose and reinitial:
print(PRIFIX + "Re-initial, tracked features: ", str(prev_keypoints.shape[0]))
# Handle the exit key (ESC)
k = cv2.waitKey(10) & 0xff
if k == 27:
break
# Update the old datas
prev_frame = curr_frame
prev_keypoints = curr_keypoints
#ipdb.set_trace() # for debug
print('\n')
input(PRIFIX + "Press any key to exit.")
cv2.destroyAllWindows()
print(PRIFIX + "===== Finished ! =====")
import numpy as np
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(640.0, 480, 483, 45, 483.45, 300.98, 253.10)
vo = VisualOdometry(cam, '/home/hillcrest/project/data/kittk/poses/00.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
RIGHT = 2
cap = cv2.VideoCapture(RIGHT)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 720)
img_id = 0
while (1):
ret, img1 = cap.read()
img = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
vo.update(img, img_id)
img_id = img_id + 1
cur_t = vo.cur_t
if (img_id > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
# data is bullshit (starts to drive in any direction) -> we should start using VO
# as soon as we drive, not before
# TODO: I think this only works if openCV v3+ and not Duckiebots version v2.5
# Maybe we should try to use older functions, or upgrade, I am not sure..
########################
from visual_odometry import PinholeCamera, VisualOdometry
# Measure time
ms_start = int(round(time.time() * 1000))
# Sleep time for debugging
sleeptime = 0#0.3
plotting = True
# Cam: width, height, focal length x, focal length y, cam center x, cam center y
cam = PinholeCamera(640.0, 480.0, 1192, 1192, 320.0, 240.0)
# VO: cam, speed
vo = VisualOdometry(cam, 0.1)
# Some trajectory or so
traj = np.zeros((600,600,3), dtype=np.uint8)
# Loop through every image (gray scale) in our DB
for img_id in range(1,105):
# Load image
img = cv2.imread('DB_duckiebot/test_g_'+str(img_id).zfill(6)+'.png', 0)
# Update odometry
vo.update(img, img_id)
# Obtain positions
import numpy as np
import cv2
import camutils
from camera_sensors import CameraSensors
from imutils import rotate_img
from visual_odometry import PinholeCamera, VisualOdometry
from urllib2 import urlopen
import json
camera_name = "LG-K8"
cam = PinholeCamera("camera/" + camera_name + ".npz")
vo = VisualOdometry(cam)
traj = np.zeros((600, 600, 3), dtype=np.uint8)
ipcamera_url = "http://192.168.8.100:8080/"
cam = camutils.Cam(ipcamera_url)
cam.start()
sensor = CameraSensors(ipcamera_url)
prev_draw_x, prev_draw_y = 300, 300
then = cv2.getTickCount()
while True:
sensor.update()
if cam.is_opened():
frame = cam.get_frame()
if vo.optical_flow is None:
vo.optical_flow = np.zeros_like(frame)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
from filterpy.kalman import ExtendedKalmanFilter as EKF
import numpy as np
import time
import re
import sympy
import math
import cv2
import os
import time
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(640.0, 480.0, 1.112718502113158593e+03,
1.109070993342474367e+03, 3.261312488982279092e+02,
2.284030377231190130e+02)
vo = VisualOdometry(cam)
color = np.random.randint(0, 255, (100, 3))
lk_params = dict(winSize=(15, 15),
maxLevel=2,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10,
0.03))
traj = np.zeros((600, 600, 3), dtype=np.uint8)
img_id = 1
def getObs(idx):
curr = 0
with open("log.txt") as log:
for action in log:
if curr == idx:
import cv2
from camutils import Cam
from camera_sensors import CameraSensors
from visual_odometry import PinholeCamera
import numpy as np
import math
#ipcam_url = "http://192.168.8.100:8080/"
ipcam_url = "http://192.168.8.103:8080/"
cam = Cam(ipcam_url)
cam.start()
sensor = CameraSensors(ipcam_url)
camera_name = "LG-K8_scaled"
cam_data = PinholeCamera("camera/" + camera_name + ".npz")
# 2.6 cm
fx = cam_data.mtx[0, 0]
fy = cam_data.mtx[1, 1]
imgWidth = 864
imgHeight = 480
fovx = 2.0 * math.atan(imgWidth / (2.0 * fx)) * (180.0 / math.pi)
fovy = 2.0 * math.atan(imgHeight / (2.0 * fy)) * (180.0 / math.pi)
print fovx, fovy
chessboard_size = (9, 6)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((chessboard_size[1] * chessboard_size[0], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0],
0:chessboard_size[1]].T.reshape(-1, 2)
import numpy as np
import cv2
import sys
import glob
from visual_odometry import PinholeCamera, VisualOdometry
#cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
cam = PinholeCamera(1280.0, 720.0, 718.8560, 718.8560 * 0.5, 640, 360)
vo = VisualOdometry(cam, 'tmp')
mycam = cv2.VideoCapture(0)
def get_image():
retval, im = mycam.read()
return im
def main():
traj = np.zeros((600, 600, 3), dtype=np.uint8)
img_id = 0
while (1):
frame = get_image()
img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
vo.update(img, img_id)
cur_t = vo.cur_t
if (img_id > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
pose_file_len = len(pose_file.readlines())
print("pose_file_len = ", pose_file_len)
# find pinholeCamera parameters
first_image = cv2.imread(
'/database3/KITTI_odometry/dataset/sequences/' +
str(pose_id).zfill(2) + '/image_2/' + '000000.png', 0)
h, w = first_image.shape
h, w = float(h), float(w)
f = open(
'/database3/KITTI_odometry/dataset/sequences/' +
str(pose_id).zfill(2) + '/calib.txt', 'r')
calib_data = list(num for num in f.read().split())
fx, cx = float(calib_data[27]), float(calib_data[29])
fy, cy = float(calib_data[32]), float(calib_data[33])
cam = PinholeCamera(w, h, fx, fy, cx, cy)
vo = VisualOdometry(cam, pose_file_loc)
# init
traj = np.zeros((600, 600, 3), dtype=np.uint8)
ate_rmse_sum = 0.0
section_points = 10
adjustment_x = 0.0
adjustment_y = 0.0
rte_rmse_sum = 0.0
for img_id in range(pose_file_len):
img_file_loc = '/database3/KITTI_odometry/dataset/sequences/' + str(
pose_id).zfill(2) + '/image_2/' + str(img_id).zfill(6) + '.png'
img = cv2.imread(img_file_loc, 0)
vo.update(img, img_id)
def main():
parser = argparse.ArgumentParser(
prog='test_video.py', description='visual odometry script in python')
parser.add_argument('--type',
type=str,
default='KITTI',
choices=['KITTI', 'image', 'video'],
metavar='<type of input>')
parser.add_argument('--source', type=str, metavar='<path to source>')
parser.add_argument('--camera_width',
type=float,
metavar='resolution in x direction')
parser.add_argument('--camera_height',
type=float,
metavar='resolution in y direction')
parser.add_argument('--focal',
type=float,
metavar='focal length in pixels')
parser.add_argument('--pp_x',
type=float,
metavar='x coordinate of pricipal point')
parser.add_argument('--pp_y',
type=float,
metavar='y coordinate of pricipal point')
parser.add_argument('--skip', type=int, default=1)
parser.add_argument('--verbose',
type=bool,
default=True,
metavar='information of computing E')
parser.add_argument('--show',
type=bool,
default=False,
metavar='show the odometry result in real-time')
args = parser.parse_args()
print(args)
if args.type == 'KITTI':
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928,
185.2157)
vo = VisualOdometry(
cam, 'KITTI',
'/home/r05525060/sharedfolder/indoorirPano/Dataset/dataset/poses/00.txt'
)
for img_id in xrange(4541):
img = cv2.imread(
'/home/r05525060/sharedfolder/indoorirPano/Dataset/dataset/sequences/00/image_0/{:06d}.png'
.format(img_id), 0)
vo.update(img, img_id)
cur_t = vo.cur_t
if (img_id > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
x, y, z = 0., 0., 0.
draw_x, draw_y = int(x) + 290, int(z) + 90
true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90
cv2.circle(traj, (draw_x, draw_y), 1,
(img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0),
1) #TODO: 4540 to be changed
cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2)
cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1,
(255, 255, 255), 1, 8)
cv2.imshow('Road facing camera', img)
cv2.imshow('Trajectory', traj)
cv2.waitKey(1)
if args.type == 'video':
cap = cv2.VideoCapture(args.source)
cam = PinholeCamera(args.camera_width, args.camera_height, args.focal,
args.focal, args.pp_x, args.pp_y)
vo = VisualOdometry(cam, type='video', annotations=None)
frame_idx = 0
frame_count = 0
while (True):
ret, frame = cap.read()
frame_count += 1
if not ret:
print 'Video finished!'
break
if not frame_count % args.skip == 0:
continue
print 'processing... frame_count:', frame_count
print 'processing... frame_index:', frame_idx
gray_img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# print("shape of gray img:", gray_img.shape)
vo.update(gray_img, frame_idx)
cur_t = vo.cur_t
if (frame_idx > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
x, y, z = 0., 0., 0.
draw_x, draw_y = int(x) + 590, int(z) + 290
true_x, true_y = int(vo.trueX) + 590, int(vo.trueZ) + 290
# cv2.circle(traj, (draw_x,draw_y), 1, (frame_idx*255/4540,255-frame_idx*255/4540,0), 1)
cv2.circle(traj, (draw_x, draw_y), 1, (0, 255, 0), 2)
cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2)
cv2.rectangle(traj, (10, 20), (1200, 60), (0, 0, 0),
-1) # black backgroud for text
text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1,
(255, 255, 255), 1, 8)
if args.show:
cv2.imshow('Road facing camera', gray_img)
cv2.imshow('Trajectory', traj)
cv2.waitKey(1)
frame_idx += 1
cv2.putText(traj, "fps: {}.".format(30 / args.skip), (20, 100),
cv2.FONT_HERSHEY_PLAIN, 2, (255, 255, 255), 2, 8)
cv2.putText(traj, "focal length: {} pixels".format(args.focal), (20, 140),
cv2.FONT_HERSHEY_PLAIN, 2, (255, 255, 255), 2, 8)
if args.type == "KITTI":
cv2.imwrite('KITTI_map.jpg', traj)
elif args.type == "video":
cv2.imwrite(
'output_skip{}/{}_map_f{}_pp{}-{}.jpg'.format(
args.skip, os.path.basename(args.source), args.focal,
args.pp_x, args.pp_y), traj)
fx = P[0, 0] # focal length X
fy = P[1, 1] # focal length Y
cx = P[0, 2] # principal point X
cy = P[1, 2] # principal point Y
# get img width, height
img_path = img_dir + "000000.png"
img = cv2.imread(img_path, 0)
height = img.shape[0]
width = img.shape[1]
# create Camera and VisualOdometry objects
#cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
cam = PinholeCamera(width, height, fx, fy, cx, cy)
vo_learning = VisualOdometry(cam, poses_path, learning=True)
vo_fast = VisualOdometry(cam, poses_path, learning=False)
f2 = open(poses_path, 'r')
lines = f2.readlines()
max_x = max_y = max_z = -np.inf
min_x = min_y = min_z = np.inf
tx = ty = tz = 0
for line in lines:
T = np.array([float(x)
for x in line.replace('\n', '').split(' ')]).reshape(
(3, 4)) # 3x3 rotation matrix + 3x1 translation
tx = T[0, 3]
import numpy as np
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(1280.0, 720.0, 718.8560, 718.8560, 607.1928, 185.2157)
# cam = PinholeCamera(4032.0, 3024.0, 0, 0, 0, 0)
vo = VisualOdometry(cam)
traj = np.zeros((1200, 1200, 3), dtype=np.uint8)
cam = cv2.VideoCapture()
cam.open(
'/home/ron/documents/programovani/projekty/StarSight/PiCamera/camera/longNew.mp4'
)
results_dir = 'results'
print('Start')
mn = 0
mx = 10000
skip = 2
scale = 20
show_skip = 50
for img_id in range(mn, mx):
# img = cv2.imread(
# '/home/ron/documents/programovani/projekty/StarSight/PiCamera/zatacka/' + str(
# img_id) + '.jpg', 1)
# img = None
import numpy as np
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(640.0, 480.0, 360.0000, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam)
traj = np.zeros((600,600,3), dtype=np.uint8)
stream = cv2.VideoCapture(0)
img_id = 1
while(True):
ret_val, img = stream.read()
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# img = cv2.imread('/home/xxx/datasets/KITTI_odometry_gray/00/image_0/'+str(img_id).zfill(6)+'.png', 0)
vo.update(img)
cur_t = vo.cur_t
if(img_id > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
x, y, z = 0., 0., 0.
draw_x, draw_y = int(x)+290, int(z)+90
true_x, true_y = int(vo.trueX)+290, int(vo.trueZ)+90
cv2.circle(traj, (draw_x,draw_y), 1, (img_id*255/4540,255-img_id*255/4540,0), 1)
cv2.circle(traj, (true_x,true_y), 1, (0,0,255), 2)
cv2.rectangle(traj, (10, 20), (600, 60), (0,0,0), -1)
from visual_odometry import PinholeCamera, VisualOdometry
import numpy as np
import cv2
import time
#poses_dir = 'data/poses/00.txt' #for ground truth
img_dir = 'data/'
# Pinhole(width, height, fx, fy, cx, cy, k1, k2, p1, p2, p3)
cam = PinholeCamera(1824, 992, 375.88, 375.88, 909, 469)
#with open(poses_dir) as f:
# poses = f.readlines()#poses
print("Mobileye data loaded.")
############ Perform Visual Odometry ############
vo = VisualOdometry(cam)
traj = np.zeros((600, 600, 3), dtype=np.uint8)
predicted = []
# predicted = np.array(predicted)
# actual = np.array(actual)
frames_arr = []
import time
start = time.time()
frames = 565
#drawing trajectories for each frame starting form the 3rd
print("distortion coeff")
print(dist)
return mat, dist
#traj = np.zeros((600,600,3), dtype=np.uint8)
img = cv2.imread('../../res/cap0.png', 0)
mat, dist = get_cameramat_dist("cam_calib.pkl")
cam = PinholeCamera(img.shape[1],
img.shape[0],
fx=mat[0, 0],
fy=mat[1, 1],
cx=mat[0, 2],
cy=mat[1, 2],
k1=dist[0, 0],
k2=dist[0, 1],
p1=dist[0, 2],
p2=dist[0, 3],
k3=dist[0, 4])
vo = VisualOdometry(cam)
for img_id in range(0, 10):
img = cv2.imread('../../res/cap' + str(img_id) + '.png', 0)
vo.update(img, img_id)
cur_t = vo.cur_t
if (img_id > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
# -*- coding: utf-8 -*-
"""
Created on Tue Oct 16 19:42:46 2018
@author: Shelton
"""
import numpy as np
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
#cam=PinholeCamera(640,480,538.8636705068433,538.8445314918096,318.83256039016896,242.66774610227353,k1=0.24478296725809076, k2=-0.5104803048920522, p1=-0.006448619715457021, p2=-0.002142112640096728, k3=0.0)
index = input("input data number") ##input
print("data number:", index) ##print
index = str(index)
#vo = VisualOdometry(cam, '/home/turtlebot/Downloads/dataset/sequences/00/02.txt')
vo = VisualOdometry(
cam, '/home/turtlebot/Downloads/dataset/sequences/00/0' + index + '.txt')
traj = np.zeros((1000, 1000, 3), dtype=np.uint8)
for img_id in xrange(4541):
#img = cv2.imread('/home/turtlebot/Downloads/dataset/sequences/02/image_0/'+str(img_id).zfill(6)+'.png', 0)
img = cv2.imread(
'/home/turtlebot/Downloads/dataset/sequences/0' + index + '/image_0/' +
str(img_id).zfill(6) + '.png', 0)
#img=cv2.imread('/home/turtlebot/data/'+str(img_id)+'.png')
#img=img[240:480,:]
[0.00000000E+00, 1.37997405E+03, 7.56192877E+02], [0.00000000E+00, 0.00000000E+00, 1.00000000E+00]]) # Distortion coefficients dist = np.array([-0.2708139, 0.20052465, 2.08302E-02, 0.0002806, -0.10134601]) # -- Blueye in-air -- # Camera matrix: #mtx = np.array([[978.36617202, 0., 985.08473535], # [0., 975.69987506, 541.52130078], # [0., 0., 1.]]) # Distortion coefficients #dist = np.array([-0.37370139, 0.26899755, -0.00120655, -0.00185788, -0.1411856]) cam = PinholeCamera(1920, 1080, mtx[0, 0], mtx[1, 1], mtx[0, 2], mtx[1, 2]) # -------------------------------------------------------------- vo = VisualOdometry(cam) traj = np.zeros((600,650,3), np.uint8) init = True # controller setup: # surge_controller = controller(0.1, 0., 0.) # sway_controller = controller(0.08, 0., 0.) # yaw_controller = controller(0.009, 0., 0.005) # depth_controller = controller(3.5, 0., 0.0) # depth_controller.init_ref = 0.5
import numpy as np
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(320.0, 180.0, 963.93956241, 966.19092668, 644.61135581,
356.78176629)
vo = VisualOdometry(cam)
traj = np.zeros((600, 600, 3), dtype=np.uint8)
cap = cv2.VideoCapture(0)
i = 0
#cv2.namedWindow("x")
cv2.namedWindow("map")
ret, rgb_img = cap.read()
print(rgb_img.shape)
while True:
i = i + 1
ret, rgb_img = cap.read()
img = cv2.cvtColor(cv2.resize(rgb_img, (320, 180)), cv2.COLOR_BGR2GRAY)
vo.update(img)
if (i > 2):
x, y, z = vo.cur_t[0], vo.cur_t[1], vo.cur_t[2]
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
# 1) KITTI Dataset
# 2) College Library Indoors
# 3) Indoor Hallway
print("1. KITTI Dataset")
print("2. College Library Indoors")
print("3. Indoor Hallway")
selection = 1
selection = input("Enter your choice number: ")
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
if selection == "1":
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
elif selection == "2" or selection == "3":
cam = PinholeCamera(1080.0, 1920.0, 718.8560, 718.8560, 607.1928,
185.2157) #phone camera
vo = VisualOdometry(cam, 'ground_truth.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
range1 = 4541
range2 = 1363
range3 = 1115