├── map.png
├── README.md
├── test.py
└── visual_odometry.py


/map.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/uoip/monoVO-python/HEAD/map.png


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | **Keywords:** FAST Feature Detection, LK Feature Tracking, Five-Point Motion Estimation
 2 | 
 3 | port from https://github.com/yueying/LearningVO
 4 | 
 5 | ![map](map.png)
 6 | 
 7 | ### Requirements
 8 | * Python 2.7
 9 | * Numpy
10 | * OpenCV
11 | 
12 | ### Dataset
13 |  [KITTI odometry data set (grayscale, 22 GB)](http://www.cvlibs.net/datasets/kitti/eval_odometry.php)
14 |  
15 | ### Usage
16 | Modify the path in test.py to your image sequences and ground truth trajectories, then run
17 | ```
18 | python test.py
19 | ```
20 | 
21 | ### References
22 | 1. [一个简单的视觉里程计实现 | 冯兵的博客](http://fengbing.net/2015/07/26/%E4%B8%80%E4%B8%AA%E7%AE%80%E5%8D%95%E7%9A%84%E8%A7%86%E8%A7%89%E9%87%8C%E7%A8%8B%E8%AE%A1%E5%AE%9E%E7%8E%B01/ )<br>
23 | 2. [Monocular Visual Odometry using OpenCV](http://avisingh599.github.io/vision/monocular-vo/) and its related project report [_Monocular Visual Odometry_](http://avisingh599.github.io/assets/ugp2-report.pdf) | Avi Singh
24 |  
25 | Search "cv2.findEssentialMat", "cv2.recoverPose" etc. in github, you'll find more python projects on slam / visual odometry / 3d reconstruction
26 | 


--------------------------------------------------------------------------------
/test.py:
--------------------------------------------------------------------------------
 1 | import numpy as np 
 2 | import cv2
 3 | 
 4 | from visual_odometry import PinholeCamera, VisualOdometry
 5 | 
 6 | 
 7 | cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
 8 | vo = VisualOdometry(cam, '/home/xxx/datasets/KITTI_odometry_poses/00.txt')
 9 | 
10 | traj = np.zeros((600,600,3), dtype=np.uint8)
11 | 
12 | for img_id in xrange(4541):
13 | 	img = cv2.imread('/home/xxx/datasets/KITTI_odometry_gray/00/image_0/'+str(img_id).zfill(6)+'.png', 0)
14 | 
15 | 	vo.update(img, img_id)
16 | 
17 | 	cur_t = vo.cur_t
18 | 	if(img_id > 2):
19 | 		x, y, z = cur_t[0], cur_t[1], cur_t[2]
20 | 	else:
21 | 		x, y, z = 0., 0., 0.
22 | 	draw_x, draw_y = int(x)+290, int(z)+90
23 | 	true_x, true_y = int(vo.trueX)+290, int(vo.trueZ)+90
24 | 
25 | 	cv2.circle(traj, (draw_x,draw_y), 1, (img_id*255/4540,255-img_id*255/4540,0), 1)
26 | 	cv2.circle(traj, (true_x,true_y), 1, (0,0,255), 2)
27 | 	cv2.rectangle(traj, (10, 20), (600, 60), (0,0,0), -1)
28 | 	text = "Coordinates: x=%2fm y=%2fm z=%2fm"%(x,y,z)
29 | 	cv2.putText(traj, text, (20,40), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,255), 1, 8)
30 | 
31 | 	cv2.imshow('Road facing camera', img)
32 | 	cv2.imshow('Trajectory', traj)
33 | 	cv2.waitKey(1)
34 | 
35 | cv2.imwrite('map.png', traj)
36 | 


--------------------------------------------------------------------------------
/visual_odometry.py:
--------------------------------------------------------------------------------
  1 | import numpy as np 
  2 | import cv2
  3 | 
  4 | STAGE_FIRST_FRAME = 0
  5 | STAGE_SECOND_FRAME = 1
  6 | STAGE_DEFAULT_FRAME = 2
  7 | kMinNumFeature = 1500
  8 | 
  9 | lk_params = dict(winSize  = (21, 21), 
 10 | 				#maxLevel = 3,
 11 |              	criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01))
 12 | 
 13 | def featureTracking(image_ref, image_cur, px_ref):
 14 | 	kp2, st, err = cv2.calcOpticalFlowPyrLK(image_ref, image_cur, px_ref, None, **lk_params)  #shape: [k,2] [k,1] [k,1]
 15 | 
 16 | 	st = st.reshape(st.shape[0])
 17 | 	kp1 = px_ref[st == 1]
 18 | 	kp2 = kp2[st == 1]
 19 | 
 20 | 	return kp1, kp2
 21 | 
 22 | 
 23 | class PinholeCamera:
 24 | 	def __init__(self, width, height, fx, fy, cx, cy, 
 25 | 				k1=0.0, k2=0.0, p1=0.0, p2=0.0, k3=0.0):
 26 | 		self.width = width
 27 | 		self.height = height
 28 | 		self.fx = fx
 29 | 		self.fy = fy
 30 | 		self.cx = cx
 31 | 		self.cy = cy
 32 | 		self.distortion = (abs(k1) > 0.0000001)
 33 | 		self.d = [k1, k2, p1, p2, k3]
 34 | 
 35 | 
 36 | class VisualOdometry:
 37 | 	def __init__(self, cam, annotations):
 38 | 		self.frame_stage = 0
 39 | 		self.cam = cam
 40 | 		self.new_frame = None
 41 | 		self.last_frame = None
 42 | 		self.cur_R = None
 43 | 		self.cur_t = None
 44 | 		self.px_ref = None
 45 | 		self.px_cur = None
 46 | 		self.focal = cam.fx
 47 | 		self.pp = (cam.cx, cam.cy)
 48 | 		self.trueX, self.trueY, self.trueZ = 0, 0, 0
 49 | 		self.detector = cv2.FastFeatureDetector_create(threshold=25, nonmaxSuppression=True)
 50 | 		with open(annotations) as f:
 51 | 			self.annotations = f.readlines()
 52 | 
 53 | 	def getAbsoluteScale(self, frame_id):  #specialized for KITTI odometry dataset
 54 | 		ss = self.annotations[frame_id-1].strip().split()
 55 | 		x_prev = float(ss[3])
 56 | 		y_prev = float(ss[7])
 57 | 		z_prev = float(ss[11])
 58 | 		ss = self.annotations[frame_id].strip().split()
 59 | 		x = float(ss[3])
 60 | 		y = float(ss[7])
 61 | 		z = float(ss[11])
 62 | 		self.trueX, self.trueY, self.trueZ = x, y, z
 63 | 		return np.sqrt((x - x_prev)*(x - x_prev) + (y - y_prev)*(y - y_prev) + (z - z_prev)*(z - z_prev))
 64 | 
 65 | 	def processFirstFrame(self):
 66 | 		self.px_ref = self.detector.detect(self.new_frame)
 67 | 		self.px_ref = np.array([x.pt for x in self.px_ref], dtype=np.float32)
 68 | 		self.frame_stage = STAGE_SECOND_FRAME
 69 | 
 70 | 	def processSecondFrame(self):
 71 | 		self.px_ref, self.px_cur = featureTracking(self.last_frame, self.new_frame, self.px_ref)
 72 | 		E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0)
 73 | 		_, self.cur_R, self.cur_t, mask = cv2.recoverPose(E, self.px_cur, self.px_ref, focal=self.focal, pp = self.pp)
 74 | 		self.frame_stage = STAGE_DEFAULT_FRAME 
 75 | 		self.px_ref = self.px_cur
 76 | 
 77 | 	def processFrame(self, frame_id):
 78 | 		self.px_ref, self.px_cur = featureTracking(self.last_frame, self.new_frame, self.px_ref)
 79 | 		E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0)
 80 | 		_, R, t, mask = cv2.recoverPose(E, self.px_cur, self.px_ref, focal=self.focal, pp = self.pp)
 81 | 		absolute_scale = self.getAbsoluteScale(frame_id)
 82 | 		if(absolute_scale > 0.1):
 83 | 			self.cur_t = self.cur_t + absolute_scale*self.cur_R.dot(t) 
 84 | 			self.cur_R = R.dot(self.cur_R)
 85 | 		if(self.px_ref.shape[0] < kMinNumFeature):
 86 | 			self.px_cur = self.detector.detect(self.new_frame)
 87 | 			self.px_cur = np.array([x.pt for x in self.px_cur], dtype=np.float32)
 88 | 		self.px_ref = self.px_cur
 89 | 
 90 | 	def update(self, img, frame_id):
 91 | 		assert(img.ndim==2 and img.shape[0]==self.cam.height and img.shape[1]==self.cam.width), "Frame: provided image has not the same size as the camera model or image is not grayscale"
 92 | 		self.new_frame = img
 93 | 		if(self.frame_stage == STAGE_DEFAULT_FRAME):
 94 | 			self.processFrame(frame_id)
 95 | 		elif(self.frame_stage == STAGE_SECOND_FRAME):
 96 | 			self.processSecondFrame()
 97 | 		elif(self.frame_stage == STAGE_FIRST_FRAME):
 98 | 			self.processFirstFrame()
 99 | 		self.last_frame = self.new_frame
100 | 


--------------------------------------------------------------------------------