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)
def main():
flag_train = False
image_list_file = open(sys.argv[1])
image_list_file.readline() # skip the first line
image_list = image_list_file.read().split('\n')
scale_gt = np.loadtxt(sys.argv[2])
vo_params={'fx':716,'fy':716,'cx':607,'cy':189,'bucket_size':30,'dense':2,'feature_threshold':0.000007}
vo = VisualOdometry(vo_params)
model_params = {'image_shape':[1241,376],'grid_size':5}
mc = ModelConstructor(model_params)
if flag_train:
environment_model = model_construction(image_list,scale_gt,vo,mc)
mc.write('environment_model')
else:
mc.read('environment_model')
environment_model = mc.model
#mc.visualization()
scale_params={'threshold':20}
sc = ScalePredictor(mc,scale_params)
vo.reset()
path = vo_with_scale(image_list,vo,sc)
np.savetxt('path.txt',path)
draw.draw_path(path)
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]
@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,:]
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]
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)
vo = VisualOdometry(cam,
'/home/surya/monoVO-python/dataset/sequences/00/00.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
for img_id in xrange(4541):
img = cv2.imread(
'/home/surya/monoVO-python/dataset/sequences/00/image_0/' +
str(img_id).zfill(6) + '.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:
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)
height = 480
fx = 554.254691191187
fy = 554.254691191187
cx = 320.5
cy = 240.5
D = [0.0, 0.0, 0.0, 0.0, 0.0]
cam = KinectCamera(width, height, fx, fy, cx, cy, D)
feature_tracker = FeatureTracker(num_features=MinNumFeatureDefault,
num_levels=8,
scale_factor=1.2,
match_ratio_test=RatioTest,
tracker_type=FeatureTrackerTypes.DES_BF)
vo = VisualOdometry(cam, feature_tracker, True, True)
"""
Live plot of the Data using Matplotlib animation
"""
plt.style.use('seaborn')
x = []
y = []
x_t = []
y_t = []
errors = []
frames = []
fig, axs = plt.subplots(ncols=2,
import numpy as np
import cv2
import os
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam, "dataset/poses/01.txt")
traj = np.zeros((600, 600, 3), dtype=np.uint8)
images_path = 'data_odometry_gray/dataset/sequences/00/image_0'
files = [os.path.join(images_path, p) for p in sorted(os.listdir(images_path))]
for img_id in range(len(files)):
if img_id != 173:
img = cv2.imread(files[img_id], 0)
vo.update(img, img_id)
# cv2.circle(img, (st1,st2), 1, (0,0,255), 1)
cur_t = vo.cur_t
if (img_id > 0):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
x, y, z = 0., 0., 0.
#print('xframe_id',img_id,':',x)
#print('yframe_id',img_id,':',z)
#print('trueX frame_id',img_id,':',vo.trueX)
#print('trueY frame_id',img_id,':',vo.trueZ)
draw_x, draw_y = int(x) + 290, int(z) + 90
true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90
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)
#vo = VisualOdometry(cam, '/home/xxx/datasets/KITTI_odometry_poses/00.txt')
vo = VisualOdometry(cam, 'C:/Users/KJoshi/Downloads/Kitti/00.txt')
traj = np.zeros((600,600,3), dtype=np.uint8)
for img_id in range(4000):
img = cv2.imread('C:/Users/KJoshi/Downloads/Kitti/image_0/'+str(img_id).zfill(6)+'.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:
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)
import numpy as np
import sys
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
import cv2
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam, 'dataset/dataset/poses/00.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
for img_id in range(4541):
img = cv2.imread(
'dataset/sequences/00/image_0/' + str(img_id).zfill(6) + '.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:
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)
noise_std = 0
for noise_std in [5, 15, 25, 35, 45, 55]:
for dynamic in [0, 1]:
name = "dynamic"
#print("upper limit of noise std:", upper)
print("noise std:", noise_std)
if (dynamic == 0):
print('fixed threshold')
name = "fixed"
else:
print("dynamic_threshold")
mse_lst = []
for k in range(num_trials):
cam = PinholeCamera(1226.0, 370.0, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam, '/Users/farhantoddywala/desktop/dataset/sequences/06/calib2.txt')
traj = np.zeros((600,600,3), dtype=np.uint8)
scale = 1.0
predicted = []
true = []
#noise_std = 0
for img_id in range(0, len(X)):
img = cv2.imread('/Users/farhantoddywala/desktop/dataset/sequences/06/image_0/'+str(img_id).zfill(6)+'.png', 0)
#img = cv2.resize(img, (620,188))
#print(img[0][0])
#print(img.shape)
#rand = np.random.randint(low = -1, high = 2)
#noise_std = min(max(noise_std + rand, 0), upper)
img = img + np.random.normal(loc = 0, scale = noise_std, size = img.shape)
img[img > 255] = 255
img[img < 0] = 0
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)
cv2.imshow('Road facing camera', img)
# cv2.moveWindow('Trajectory', 800, 800)
cv2.imshow('Trajectory', traj)
cv2.waitKey(1)
cv2.imwrite('map.png', traj)
if __name__ == "__main__":
args = get_parser().parse_args()
print("Command Line Args:", args)
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam, args.pose_file)
run(vo, args.image_dir)
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]
ty = T[1, 3]
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)
vo = VisualOdometry(cam,
"D:\\file\\data_odometry_gray\\dataset\\poses\\00.txt")
traj = np.zeros((600, 600, 3), dtype=np.uint8)
for img_id in range(4541):
img = cv2.imread(
"D:\\file\\data_odometry_gray\\dataset\\sequences\\00\\image_0\\" +
str(img_id).zfill(6) + ".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:
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)
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)
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)
vo = VisualOdometry(cam, 'E:/Dataset/KITTI/odometry/dataset/poses/00.txt')
traj = np.zeros((600,600,3), dtype=np.uint8)
for img_id in range(4541):
img = cv2.imread('E:/Dataset/KITTI/odometry/dataset/sequences/00/image_2/%06d.png'%(img_id), 0)
vo.update(img, img_id)
if vo.state == 2:
x, y, z = vo.get_T()
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)
cv2.imshow('Road facing camera', img)
cv2.imshow('Trajectory', traj)
if 27 == cv2.waitKey(1):
break
# -- 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
cap = cv2.VideoCapture("pool.MP4")
row = ['x', 'y', 'z', 'roll', 'pitch', 'yaw']
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(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(
cam,
'/home/r05525060/sharedfolder/indoorirPano/Dataset/dataset/poses/00.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
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)
cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2)
cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
config.cam_settings['Camera.cx'], config.cam_settings['Camera.cy'],
config.DistCoef, config.cam_settings['Camera.fps'])
num_features=2000 # how many features do you want to detect and track?
# select your tracker configuration (see the file feature_tracker_configs.py)
# LK_SHI_TOMASI, LK_FAST
# SHI_TOMASI_ORB, FAST_ORB, ORB, BRISK, AKAZE, FAST_FREAK, SIFT, ROOT_SIFT, SURF, SUPERPOINT, FAST_TFEAT
tracker_config = FeatureTrackerConfigs.LK_SHI_TOMASI
tracker_config['num_features'] = num_features
feature_tracker = feature_tracker_factory(**tracker_config)
# create visual odometry object
vo = VisualOdometry(cam, groundtruth, feature_tracker)
is_draw_traj_img = True
traj_img_size = 800
traj_img = np.zeros((traj_img_size, traj_img_size, 3), dtype=np.uint8)
half_traj_img_size = int(0.5*traj_img_size)
draw_scale = 1
is_draw_3d = True
if kUsePangolin:
viewer3D = Viewer3D()
else:
plt3d = Mplot3d(title='3D trajectory')
is_draw_err = True
err_plt = Mplot2d(xlabel='img id', ylabel='m',title='error')
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)
vo = VisualOdometry(cam, '/home/hillcrest/project/data/kittk/poses/00.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
for img_id in xrange(4541):
imgPath = '/home/hillcrest/project/data/kittk/sequences/00/image_0/' + str(
img_id).zfill(6) + '.png'
img = cv2.imread(imgPath, 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)
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)
from visual_odometry import VisualOdometry
from camera import PinholeCamera
import cv2
import glob
import numpy as np
camera = PinholeCamera(1241.0, 376.0, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(camera, "00.txt")
img_paths = sorted(glob.glob("data/*"))
traj = np.zeros((600,600,3), dtype=np.uint8)
frame_id = 0
for path in img_paths:
img = cv2.imread(path, 0)
vo.process_image(img, frame_id)
t = vo.t
if frame_id > 1:
x, y, z = t[0], t[1], 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, (true_x,true_y), 1, (255,0,0), 2)
cv2.circle(traj, (draw_x, draw_y), 1, (0,0,255), 2)
cv2.imshow('Road facing camera', img)
cv2.imshow('Trajectory', traj)
cv2.waitKey(1)
frame_id += 1
cv2.imwrite('map2.png', traj)
import numpy as np
import cv2
import matplotlib.pyplot as plt
from visual_odometry import PinholeCamera, VisualOdometry
import time
import pdb
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam, './dataset/00.txt')
traj = np.zeros((600, 600, 3), dtype=np.uint8)
fig, ax = plt.subplots(nrows=2)
plt.show(block=False)
for img_id in range(4541):
img = cv2.imread('./dataset/00/image_0/' + str(img_id).zfill(6) + '.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:
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)
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
for img_id in range(frames):
img = cv2.imread(img_dir + str(img_id).zfill(6) + '.png', 0)
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)
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
Q_x = 0.02
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):
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)
import numpy as np
import cv2
import sys
from matplotlib import pyplot as plt
gt_path = sys.argv[1]
image_path = sys.argv[2]
from visual_odometry import PinholeCamera, VisualOdometry
cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
vo = VisualOdometry(cam, gt_path)
traj = np.zeros((600,600,3), dtype=np.uint8)
for img_id in range(4541):
print(img_id)
img = cv2.imread(sys.argv[2]+str(img_id).zfill(6)+'.png', 0)
#print(img)
vo.update(img, img_id)
img_vis = img.copy()
if img_id>0:
print(vo.feature3d.shape)
feature2d = vo.feature3d[:,0:2]
feature2d[:,0] = feature2d[:,0]*cam.fx/vo.feature3d[:,2]+cam.cx
feature2d[:,1] = feature2d[:,1]*cam.fx/vo.feature3d[:,2]+cam.cy
print(feature2d)
for point2d in feature2d:
cv2.circle(img_vis,(int(point2d[0]),int(point2d[1])),3,(0,255,0),-1)
import numpy as np
import cv2
from visual_odometry import FisheyeCamera, VisualOdometry
cam = FisheyeCamera(640.0, 480.0, 320, 240, -179.47183, 0, 0.002316744,
-3.6359684 * 10**(-6), 2.0546507 * 10**(-8), 256)
vo = VisualOdometry(
cam, '/home/dongk/다운로드/rpg_urban_fisheye_info/info/groundtruth.txt'
) # CHANGE THIS DIRECTORY PATH
traj = np.zeros((600, 600, 3), dtype=np.uint8)
for img_id in range(2500):
img = cv2.imread('/home/dongk/다운로드/rpg_urban_fisheye_data/data/img/' +
'img' + str(img_id + 390 + 1).zfill(4) + '_0.png',
0) # CHANGE THIS DIRECTORY PATH
vo.update(img, img_id)
cur_t = vo.cur_t
if (img_id > 2):
x, y, z = cur_t[0, 0], cur_t[1, 0], cur_t[2, 0]
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.trueY) + 90
cv2.circle(traj, (draw_x, draw_y), 1, (255.0, 0), 2)
cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 1)
cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
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)
vo = VisualOdometry(cam, '/home/xxx/datasets/KITTI_odometry_poses/00.txt')
traj = np.zeros((600,600,3), dtype=np.uint8)
for img_id in xrange(4541):
img = cv2.imread('/home/xxx/datasets/KITTI_odometry_gray/00/image_0/'+str(img_id).zfill(6)+'.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:
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)
cv2.imshow('Road facing camera', img)