12
12
CameraInfo )
13
13
from cv_bridge import CvBridge , CvBridgeError
14
14
from ar_track_alvar_msgs .msg import AlvarMarkers
15
- import forward_kinematics_extrinsics
15
+ from forward_kinematics_extrinsics import forwardKinematics
16
16
17
17
ROSTOPIC_SET_ARM_JOINT = '/goal_dynamixel_position'
18
18
#flag = 0
@@ -48,7 +48,7 @@ def main(noiter=15):
48
48
rospy .sleep (2 )
49
49
Calib = rospy .wait_for_message ("/camera/color/camera_info" , CameraInfo )
50
50
K_matrix = np .array (Calib .K )
51
- fk = forward_kinematics_extrinsics . forwardKinematics ('bottom_plate' , 'ar_tag' )
51
+ fk = forwardKinematics ('bottom_plate' , 'ar_tag' )
52
52
53
53
positions = np .zeros ((noiter , 3 ))
54
54
orientations = np .zeros ((noiter , 4 ))
@@ -110,32 +110,45 @@ def artagcallback(i):
110
110
np .savez ("../data/calibration/calibration_info.npz" , position = positions , orientation = orientations , camerainfo = K_matrix , \
111
111
joint_angles = joint_angles , forward_kinematics = end_effector_positions )
112
112
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 )
115
116
116
117
117
118
def calculate_extrinsic (positions , K_matrix , end_effector_positions , noiter ):
118
- min_error = np . inf
119
+ max_inliers = 0
119
120
p = get_p (K_matrix , positions )
120
121
P = get_P (end_effector_positions )
121
- for i in range (0 ,5000 ):
122
+ for i in range (0 ,50000 ):
123
+ # Sample 6 points
122
124
indices = np .random .randint (0 , noiter , size = 6 )
123
125
M = get_projection_matrix (p [indices ,:], P [indices ,:])
126
+
127
+ # Calculate reprojection error
124
128
homogeneous_P = np .hstack ((P , np .ones ((P .shape [0 ], 1 ))))
125
129
estimated_p = np .matmul (M ,homogeneous_P .T )
126
130
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
132
143
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
+
139
152
140
153
def get_projection_matrix (p , P ):
141
154
num_points = p .shape [0 ]
@@ -167,7 +180,17 @@ def get_P(end_effector_position):
167
180
point_3D_world_frame = end_effector_position [:,0 :3 ,3 ]
168
181
return point_3D_world_frame
169
182
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
+
170
192
if __name__ == "__main__" :
171
193
# main(50)
172
194
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