@@ -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 ('arm_base_link ' , 'ar_tag' )
51
+ fk = forward_kinematics_extrinsics .forwardKinematics ('bottom_plate ' , 'ar_tag' )
52
52
53
53
positions = np .zeros ((noiter , 3 ))
54
54
orientations = np .zeros ((noiter , 4 ))
@@ -81,18 +81,17 @@ def artagcallback(i):
81
81
# # Send nodes of the path as commands to controller
82
82
count = 0
83
83
while count < noiter :
84
-
85
84
joint_angle = generatesample ()
86
85
joint_angles [count , :] = joint_angle
87
86
print (joint_angles )
88
87
joint_angle = (joint_angle * rad_from_deg ).tolist ()
89
88
set_arm_joint (pub , np .array (joint_angle ))
90
89
91
- joint_angle = np .append (joint_angle , 0 )
92
-
93
- M = fk .getJointForwardKinematics (joint_angle .reshape ((1 ,6 )))
94
- end_effector_positions [count , :, :] = np .matmul (np .linalg .inv (M [0 ,:,:]),M [5 ,:,:])
90
+ joint_angle = np .append (np .append (0 , joint_angle ), 0 )
95
91
92
+ M = fk .getJointForwardKinematics (joint_angle .reshape ((1 ,7 )))
93
+ end_effector_positions [count , :, :] = np .matmul (np .linalg .inv (M [0 ,:,:]),M [6 ,:,:])
94
+ print (end_effector_positions [count , :, :])
96
95
rospy .sleep (4 )
97
96
98
97
try :
@@ -111,16 +110,16 @@ def artagcallback(i):
111
110
np .savez ("../data/calibration/calibration_info.npz" , position = positions , orientation = orientations , camerainfo = K_matrix , \
112
111
joint_angles = joint_angles , forward_kinematics = end_effector_positions )
113
112
114
- R = calculate_extrinsic (positions , K_matrix , end_effector_positions )
113
+ R = calculate_extrinsic (positions , K_matrix , end_effector_positions , noiter )
115
114
print (R )
116
115
117
116
118
- def calculate_extrinsic (positions , K_matrix , end_effector_positions ):
119
- min_error = 10000000000000000
117
+ def calculate_extrinsic (positions , K_matrix , end_effector_positions , noiter ):
118
+ min_error = np . inf
120
119
p = get_p (K_matrix , positions )
121
120
P = get_P (end_effector_positions )
122
- for i in range (0 ,1000 ):
123
- indices = np .random .randint (0 , 100 , size = 6 )
121
+ for i in range (0 ,5000 ):
122
+ indices = np .random .randint (0 , noiter , size = 6 )
124
123
M = get_projection_matrix (p [indices ,:], P [indices ,:])
125
124
homogeneous_P = np .hstack ((P , np .ones ((P .shape [0 ], 1 ))))
126
125
estimated_p = np .matmul (M ,homogeneous_P .T )
@@ -132,9 +131,10 @@ def calculate_extrinsic(positions, K_matrix, end_effector_positions):
132
131
final_M = M
133
132
K ,R = linalg .rq (final_M [:,0 :3 ])
134
133
print (K / K [2 ,2 ])
135
- T = np .matmul (np .linalg .inv (K ),M [:,3 ])
134
+ T = np .matmul (np .linalg .inv (K ),final_M [:,3 ])
136
135
H = np .vstack ((np .hstack ((R ,T .reshape ((3 ,1 )))),np .array ([[0 ,0 ,0 ,1 ]])))
137
- print (H )
136
+ print (np .linalg .inv (H ))
137
+ print (min_error )
138
138
return H
139
139
140
140
def get_projection_matrix (p , P ):
@@ -168,10 +168,6 @@ def get_P(end_effector_position):
168
168
return point_3D_world_frame
169
169
170
170
if __name__ == "__main__" :
171
- < << << << HEAD
172
- main (30 )
173
- == == == =
174
- main (100 )
175
- # calib_info = np.load("calibration_info.npz")
176
- # calculate_extrinsic(calib_info['position'], calib_info['camerainfo'], calib_info['forward_kinematics'])
177
- >> >> >> > 00 f3b8d0a97eda761ca978e46306ced5397a027d
171
+ # main(50)
172
+ calib_info = np .load ("../data/calibration/calibration_info.npz" )
173
+ calculate_extrinsic (calib_info ['position' ], calib_info ['camerainfo' ], calib_info ['forward_kinematics' ], 20 )
0 commit comments