本文整理汇总了Python中Transform.rot2tr方法的典型用法代码示例。如果您正苦于以下问题：Python Transform.rot2tr方法的具体用法？Python Transform.rot2tr怎么用？Python Transform.rot2tr使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Transform的用法示例。
在下文中一共展示了Transform.rot2tr方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: get_chessboard_pose
# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import rot2tr [as 别名]
def get_chessboard_pose(img, info, ncol=5, nrow=4, squareSize=0.045, useSubPix=True, publishDetection=True, isRect=True):
'''
compute the pose of the chessboard in the camera frame
'''
(success, chessboard_wrtImage) = find_chessboard(img, ncol=ncol, nrow=nrow, useSubPix=useSubPix);
if (len(chessboard_wrtImage) == 0):
rospy.logwarn(__name__ + ': no chessboard found');
return None;
if (publishDetection):
chessboardCV = draw_chessboard(img, chessboard_wrtImage, ncol=ncol, nrow=nrow);
chessboardMsg = cvmat2msg(chessboardCV, img.encoding, frame_id=img.header.frame_id, stamp=rospy.Time().now());
chessboardDetectionPub.publish(chessboardMsg);
# compute pose
chessboard_wrtBoard = cv.CreateMat(ncol*nrow, 1, cv.CV_32FC3);
for i in range(ncol*nrow):
chessboard_wrtBoard[i,0] = ((i % ncol) * squareSize, (i / ncol) * squareSize, 0);
# get camera intrinsics
(cameraMatrix, distCoeffs, projectionMatrix, rotationMatrix) = info2cvmat(info);
# extrinsic outputs
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
# find chessboard pose
if isRect:
distCoeffs = cv.CreateMat(1, 4, cv.CV_32FC1);
distCoeffs[0,0] = 0; distCoeffs[0,1] = 0; distCoeffs[0,2] = 0; distCoeffs[0,3] = 0;
cv.FindExtrinsicCameraParams2(chessboard_wrtBoard, chessboard_wrtImage, projectionMatrix[:,:3], distCoeffs, rot, trans);
else:
cv.FindExtrinsicCameraParams2(chessboard_wrtBoard, chessboard_wrtImage, cameraMatrix, distCoeffs, rot, trans);
# get transform from rot, trans
rot = np.asarray(rot);
trans = np.asarray(trans);
th = np.linalg.norm(rot);
r = rot / th;
R = np.cos(th)*np.eye(3) + (1 - np.cos(th))*np.dot(r, r.T) + np.sin(th)*np.array([[0, -r[2], r[1]], [r[2], 0, -r[0]], [-r[1], r[0], 0]]);
if not isRect:
Trect = tr.trans(-projectionMatrix[0,3]/cameraMatrix[0,0],-projectionMatrix[1,3]/cameraMatrix[1,1], 0)
Rcam = tr.rot2tr(np.asarray(rotationMatrix));
Tmodel2cam = np.dot(Trect, np.dot(Rcam, np.dot(tr.trans(trans), tr.rot2tr(R))));
else:
Tmodel2cam = np.dot(tr.trans(trans), tr.rot2tr(R));
return ru.to_PoseStamped(Tmodel2cam, frame_id=info.header.frame_id, stamp=info.header.stamp);