-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpose_est_theta_ros.py
156 lines (125 loc) · 5.63 KB
/
pose_est_theta_ros.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
#!/usr/bin/env python
# import sys
# sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
import cv2 as cv
import numpy as np
import glob
from matplotlib import pyplot as plt
from math import *
import time
#global parameters
success_pose = 0
# winName = "Pose estimation"
# cv.namedWindow(winName, cv.WINDOW_NORMAL)
#loading intrinsics
#mtx = np.array([[700.4, 0. , 628.787], [ 0. , 700.4, 372.022],[ 0. , 0. , 1. ]])
#dist = np.array([[-0.175725, 0.0290343, 0., 0., 1.]])
mtx = np.array([[690.08699358 , 0., 265.02791994],[ 0., 686.77087112, 243.98903059],[ 0., 0. , 1. ]]) #For logitech c615
dist = np.array([[-0.02285312, -0.14753576 , 0.00196857, -0.01649874 , 0.31742216]])
class pose_estimation:
def __init__(self):
self.tl = 0
self.tr = 0
self.br = 0
self.bl = 0
self.theta_ls = []
self.depth_ls = []
self.transx_ls = []
self.transy_ls = []
self.box_pose = []
def clear_all(self):
self.tl = 0
self.tr = 0
self.br = 0
self.bl = 0
self.theta_ls = []
self.depth_ls = []
self.transx_ls = []
self.transy_ls = []
def draw(img,corners,imgpts):
#print("imgpts[1].ravel()",(imgpts[1].ravel()))
corner = tuple(corners[0].ravel())
img = cv.line(img,corner,tuple(imgpts[0].ravel()),(255,0,0),5)
img = cv.line(img,corner,tuple(imgpts[1].ravel()),(0,255,0),5)
img = cv.line(img,corner,tuple(imgpts[2].ravel()),(0,0,255),5)
return img
#Loading images in RGB and grayscale
def process_theta(self,ori_img,tl,tr,br,bl):
#Finding the slope between points
avg_theta_long = 0
avg_theta_short = 0
slope_1 = (tr[1]-tl[1])/(tr[0]-tl[0])
theta_1 = degrees(atan(slope_1))
if(abs(theta_1) >=0 and abs(theta_1)<=90):
#print("theta_1 condition satisfied")
avg_theta_long = avg_theta_long + theta_1
#print("theta_1:",theta_1)
slope_2 = (br[1]-tr[1])/(br[0]-tr[0])
theta_2 = degrees(atan(slope_2))
if(abs(theta_2) >=0 and abs(theta_2)<=90):
#print("theta_2 condition satisfied")
avg_theta_short = avg_theta_short + theta_2
slope_3 = (bl[1] - br[1])/(bl[0] - br[0])
theta_3 = degrees(atan(slope_3))
#print("theta_3:",theta_3)
if(abs(theta_3) >=0 and abs(theta_3)<=90):
#print("theta_3 condition satisfied")
avg_theta_long = avg_theta_long + theta_3
slope_4 = (tl[1] - bl[1])/(tl[0] - bl[0])
theta_4 = degrees(atan(slope_4))
#print("theta_4:",theta_4)
if(abs(theta_4) >=0 and abs(theta_4)<=90):
#print("condition 4 theta satisfied")
avg_theta_short = avg_theta_short + theta_4
avg_theta_long = avg_theta_long /2
avg_theta_short = avg_theta_short /2
#print("avg_theta_long:",avg_theta_long)
#print("avg_theta_short:",avg_theta_short)
return [avg_theta_long,avg_theta_short]
def process_depth(self,ori_img,tl,tr,br,bl):
fx = 690.0869
fy = 686.7708
cx = 265.0279
cy = 243.9890
worldlength = 30 #in cms
pixellength_1 = sqrt((tl[0] - tr[0])**2 + (tl[1] - tr[1])**2)
pixellength_2 = sqrt((bl[0] - br[0])**2 + (bl[1] - br[1])**2)
#print("pixellength_1:",pixellength_1)
#print("pixellength_2:",pixellength_2)
pixellength = (pixellength_1 + pixellength_2)/2
depth = (fy * worldlength)/(pixellength)
trans_x = ((tl[0] - cx) * depth)/(fx)
trans_y = ((tl[1] - cy) * depth)/(fy)
#print("Depth in cms:",depth)
#print("trans_x:",trans_x)
#print("trans_y:",trans_y)
#inp = input("waiting for input...")
return depth,trans_x,trans_y
def plot_on_img(self,ori_img,tl,tr,br,bl):
cv.drawMarker(ori_img,(int(tl[0]),int(tl[1])), color=(0,255,0),markerType=cv.MARKER_DIAMOND,markerSize=3,thickness=3)
cv.drawMarker(ori_img,(int(tr[0]),int(tr[1])), color=(0,255,0),markerType=cv.MARKER_DIAMOND,markerSize=3,thickness=3)
cv.drawMarker(ori_img,(int(br[0]),int(br[1])), color=(0,255,0),markerType=cv.MARKER_DIAMOND,markerSize=3,thickness=3)
cv.drawMarker(ori_img,(int(bl[0]),int(bl[1])), color=(0,255,0),markerType=cv.MARKER_DIAMOND,markerSize=3,thickness=3)
def process_pose_1(self,ori_img,box_all):
#print("len(box_all):",len(box_all))
for i in range(0,len(box_all)):
if(box_all[i].shape == (4,2)):
self.tl = box_all[i][0]
self.tr = box_all[i][1]
self.br = box_all[i][2]
self.bl = box_all[i][3]
theta = self.process_theta(ori_img,self.tl,self.tr,self.br,self.bl) #Returns the theta values of the longer edge and the shorter edge in that order
self.theta_ls.append(theta)
depth_est,trans_x,trans_y = self.process_depth(ori_img,self.tl,self.tr,self.br,self.bl)
self.box_pose.append([theta , depth_est , trans_x , trans_y])
self.plot_on_img(ori_img,self.tl,self.tr,self.br,self.bl)
#Displaying corners on the image
winName = "Visualise corners"
cv.namedWindow(winName,cv.WINDOW_NORMAL)
cv.imshow(winName,ori_img)
cv.waitKey(1)
return self.box_pose , 1
#For debugging purposes - if the bounding boxes aren't accurate
if __name__ == '__main__':
ori_img = cv.imread("/home/varghese/Work/rbccps/MBZIRC/python_scripts/pose_estimation/ground_truth/3.png")
process_pose(ori_img , 0,0,0,0,0,0)