def getRealPinPtsInRob(self):
left_pin_lead_pose_mm_deg, right_pin_lead_pose_mm_deg = self.getPinLeadPoses_mm_deg(
)
t_ref_to_robot_base = self.getTref2r()
RealPinPtsInRob_left = projectPts(
left_pin_lead_pose_mm_deg[:3].reshape(3, -1), t_ref_to_robot_base)
RealPinPtsInRob_right = projectPts(
right_pin_lead_pose_mm_deg[:3].reshape(3, -1), t_ref_to_robot_base)
RealPinPtsInRob = np.hstack((RealPinPtsInRob_left.reshape(3, -1),
RealPinPtsInRob_right.reshape(3, -1)))
return RealPinPtsInRob
def setUp(self):
ParameterYamlPath = 'data/CameraCalibrationData.yaml'
CameraCalibrationData = FIT.loadYaml(fileName=ParameterYamlPath)
TcAcB = np.array(CameraCalibrationData['Tc14c13'])
CameraMatrix_14 = np.array(CameraCalibrationData['CameraMatrix_1414'])
CameraMatrix_13 = np.array(CameraCalibrationData['CameraMatrix_1313'])
DistCoeffs_1414 = np.array(CameraCalibrationData['DistCoeffs_1414'])
DistCoeffs_1313 = np.array(CameraCalibrationData['DistCoeffs_1313'])
E = np.array(CameraCalibrationData['E'])
F = np.array(CameraCalibrationData['F'])
TctA = np.array(CameraCalibrationData['Tct_1414'])
TctB = np.array(CameraCalibrationData['Tct_1313'])
ToolPose = [454.19, -202.32, 292.58, 126.09, 3.69, 177.50]
self.Ttr = VGL.Pose2T(pose=ToolPose)
self.PtsImgA_2xn = np.array(
[[6.366322275682084637e+02, 7.899394910272621928e+02],
[7.416458478168997317e+02, 6.542366444992233028e+02]],
dtype=np.float32)
self.PtsImgB_2xn = np.array(
[[8.149478495193284289e+02, 9.375415089453317705e+02],
[3.091887147386404422e+02, 3.632621606795080424e+02]],
dtype=np.float32)
MySteV = \
SteV(cameraMatrixA=CameraMatrix_14, cameraMatrixB=CameraMatrix_13,
distCoeffsA=DistCoeffs_1414, distCoeffsB=DistCoeffs_1313, TcAcB=TcAcB, E=E, F=F)
self.EIHSteV = \
EIHSteV(cameraMatrixA=CameraMatrix_14, cameraMatrixB=CameraMatrix_13,
distCoeffsA=DistCoeffs_1414, distCoeffsB=DistCoeffs_1313,
TcAcB=TcAcB, E=E, F=F, TctA=TctA, TctB=TctB)
self.PtsInCamA_3xn, self.PtsInCamB_3xn, _, _ = \
MySteV.get3dPts(imgPtsA_2xn=self.PtsImgA_2xn, imgPtsB_2xn=self.PtsImgB_2xn)
self.PtsInToolA_3xn = VGL.projectPts(pts_dxn=self.PtsInCamA_3xn,
projectMatrix=TctA)
self.PtsInToolB_3xn = VGL.projectPts(pts_dxn=self.PtsInCamB_3xn,
projectMatrix=TctB)
self.PtsInRobA_3xn = VGL.projectPts(pts_dxn=self.PtsInToolA_3xn,
projectMatrix=self.Ttr)
self.PtsInRobB_3xn = VGL.projectPts(pts_dxn=self.PtsInToolB_3xn,
projectMatrix=self.Ttr)
self.ProjImgPtsA = MySteV.projectPts(pts_3xn=self.PtsInCamA_3xn,
flag=MySteV.CAM_A)
self.ProjImgPtsB = MySteV.projectPts(pts_3xn=self.PtsInCamB_3xn,
flag=MySteV.CAM_B)
def testDepthZ(self):
"""
To verify if calculating depth z is right or not
"""
PinPtsInCamA, PinPtsInCamB = self.MyEIHSteVision.get3dPts(
imgPtsA_2xn=self.RealImgPtsA_pin,
imgPtsB_2xn=self.RealImgPtsB_pin,
flag=EIHSteV.COOR_IMG2CAM)
TcA2ref = self.vs_gazebo_env.getTcA2ref()
TcB2ref = self.vs_gazebo_env.getTcB2ref()
PinPtsInRefA = VGL.projectPts(PinPtsInCamA, TcA2ref)
PinPtsInRefB = VGL.projectPts(PinPtsInCamB, TcB2ref)
print 'PinPtsInRefA', PinPtsInRefA
print 'PinPtsInRefB', PinPtsInRefB
left_pin_lead_pose_mm_deg, right_pin_lead_pose_mm_deg = self.vs_gazebo_env.getPinLeadPoses_mm_deg(
)
print 'left_pin_lead_pose_mm_deg', left_pin_lead_pose_mm_deg
print 'right_pin_lead_pose_mm_deg', right_pin_lead_pose_mm_deg
# distCoeffsA=(), distCoeffsB=(),
TcAcB=Tc14c13, E=E_1413, F=F_1413, TctA=Tct14, TctB=None)
Pose = np.array(
[301.97729492, 204.28045654, 71.00037384, -153.69342041, 0., 0.])
Ttr = VGL.Pose2T(pose=Pose)
RealPinPtsInRob = np.array([[299, 302, 300, 303], [194, 191, 190, 190],
[128, 128, 130, 130]])[:, :1].reshape((3, -1))
# DeltaPose = np.array([10,10,10,50,40,30]).reshape((6,1)).astype('float32')
DeltaPose = np.array([10, 10, 10, 10, 0, 0]).reshape(
(6, 1)).astype('float32')
T = VGL.Pose2T(pose=DeltaPose)
RealHolePtsInRob = VGL.projectPts(pts_dxn=RealPinPtsInRob, projectMatrix=T)
DeltaPinToHoleInRob = RealHolePtsInRob - RealPinPtsInRob
print "DeltaPinToHoleInRob:\n", DeltaPinToHoleInRob
RealImgPtsA_hole = MyEIHSteVision.projectPts(pts_3xn=RealHolePtsInRob,
flag=EIHSteV.PROJ_ROB2IMGA,
Ttr=Ttr)
RealImgPtsB_hole = MyEIHSteVision.projectPts(pts_3xn=RealHolePtsInRob,
flag=EIHSteV.PROJ_ROB2IMGB,
Ttr=Ttr)
RealImgPtsA_pin = MyEIHSteVision.projectPts(pts_3xn=RealPinPtsInRob,
flag=EIHSteV.PROJ_ROB2IMGB,
Ttr=Ttr)
RealImgPtsA_hole = MyEIHSteVision.projectPts(pts_3xn=RealHolePtsInRob,