Skip to content

Commit 3e6b569

Browse files
committed
Added opencv pnpransac support and refactored code
1 parent a23f233 commit 3e6b569

File tree

1 file changed

+41
-18
lines changed

1 file changed

+41
-18
lines changed

scripts/Assignment3_Camera_Calibration.py

Lines changed: 41 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
CameraInfo)
1313
from cv_bridge import CvBridge, CvBridgeError
1414
from ar_track_alvar_msgs.msg import AlvarMarkers
15-
import forward_kinematics_extrinsics
15+
from forward_kinematics_extrinsics import forwardKinematics
1616

1717
ROSTOPIC_SET_ARM_JOINT = '/goal_dynamixel_position'
1818
#flag = 0
@@ -48,7 +48,7 @@ def main(noiter=15):
4848
rospy.sleep(2)
4949
Calib = rospy.wait_for_message("/camera/color/camera_info", CameraInfo)
5050
K_matrix = np.array(Calib.K)
51-
fk = forward_kinematics_extrinsics.forwardKinematics('bottom_plate', 'ar_tag')
51+
fk = forwardKinematics('bottom_plate', 'ar_tag')
5252

5353
positions = np.zeros((noiter, 3))
5454
orientations = np.zeros((noiter, 4))
@@ -110,32 +110,45 @@ def artagcallback(i):
110110
np.savez("../data/calibration/calibration_info.npz", position=positions, orientation=orientations, camerainfo=K_matrix, \
111111
joint_angles=joint_angles, forward_kinematics=end_effector_positions)
112112

113-
R = calculate_extrinsic(positions, K_matrix, end_effector_positions, noiter)
114-
print(R)
113+
calculate_extrinsic(positions, K_matrix, end_effector_positions, noiter)
114+
print("\n================================\n")
115+
calculate_extrinsic_opencv(positions, K_matrix, end_effector_positions)
115116

116117

117118
def calculate_extrinsic(positions, K_matrix, end_effector_positions, noiter):
118-
min_error = np.inf
119+
max_inliers = 0
119120
p = get_p(K_matrix, positions)
120121
P = get_P(end_effector_positions)
121-
for i in range(0,5000):
122+
for i in range(0,50000):
123+
# Sample 6 points
122124
indices = np.random.randint(0, noiter, size=6)
123125
M = get_projection_matrix(p[indices,:], P[indices,:])
126+
127+
# Calculate reprojection error
124128
homogeneous_P = np.hstack((P, np.ones((P.shape[0], 1))))
125129
estimated_p = np.matmul(M,homogeneous_P.T)
126130
estimated_p = estimated_p/estimated_p[2,:]
127-
error = np.linalg.norm(p-estimated_p[0:2,:].T)
128-
print(error)
129-
if error < min_error:
130-
min_error = error
131-
final_M = M
131+
error = p - estimated_p[0:2,:].T
132+
error = error[:,0]*error[:,0] + error[:,1]*error[:,1]
133+
134+
# Find inliers
135+
inlier_indices = np.unique(np.where(np.abs(error)<3))
136+
noinliers = np.sum(np.abs(error)<2)
137+
if noinliers > max_inliers:
138+
max_inliers = noinliers
139+
final_inliers = inlier_indices
140+
141+
final_M = get_projection_matrix(p[final_inliers,:], P[final_inliers,:])
142+
# RQ decomposition
132143
K,R = linalg.rq(final_M[:,0:3])
133-
print(K/K[2,2])
134-
T = np.matmul(np.linalg.inv(K),final_M[:,3])
135-
H = np.vstack((np.hstack((R,T.reshape((3,1)))),np.array([[0,0,0,1]])))
136-
print(np.linalg.inv(H))
137-
print(min_error)
138-
return H
144+
rvec = forwardKinematics.getEulerAngles(np.linalg.inv(R))
145+
tvec = np.matmul(np.linalg.inv(K),final_M[:,3])
146+
H = np.vstack((np.hstack((R,tvec.reshape((3,1)))),np.array([[0,0,0,1]])))
147+
print("Using PnP Algorithm without non-linear optimization ")
148+
print("Orientation : {0}".format(rvec))
149+
print("Position : {0}".format(tvec))
150+
return np.linalg.inv(H)
151+
139152

140153
def get_projection_matrix(p, P):
141154
num_points = p.shape[0]
@@ -167,7 +180,17 @@ def get_P(end_effector_position):
167180
point_3D_world_frame = end_effector_position[:,0:3,3]
168181
return point_3D_world_frame
169182

183+
184+
def calculate_extrinsic_opencv(positions, K_matrix, end_effector_positions):
185+
p = get_p(K_matrix, positions)
186+
P = get_P(end_effector_positions)
187+
success, rvec, tvec, inliers = cv2.solvePnPRansac(P, p, K_matrix.reshape((3,3)), distCoeffs=None)
188+
print("Using OpenCV solvePnPRansac")
189+
print("Orientation : {0}".format(rvec.T[0]))
190+
print("Position : {0}".format(tvec.T[0]))
191+
170192
if __name__ == "__main__":
171193
# main(50)
172194
calib_info = np.load("../data/calibration/calibration_info.npz")
173-
calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'], 20)
195+
calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'], 20)
196+
calculate_extrinsic_opencv(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'])

0 commit comments

Comments
 (0)