From 8b45c5d71c6fc5f5b3d49dffc1b0d087e342d534 Mon Sep 17 00:00:00 2001 From: Raphael Memmesheimer Date: Sun, 19 Aug 2018 11:22:18 +0200 Subject: [PATCH 1/2] pose math --- tf_conversions/src/tf_conversions/posemath.py | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/tf_conversions/src/tf_conversions/posemath.py b/tf_conversions/src/tf_conversions/posemath.py index c244380f..bea060ea 100644 --- a/tf_conversions/src/tf_conversions/posemath.py +++ b/tf_conversions/src/tf_conversions/posemath.py @@ -57,7 +57,7 @@ def fromTf(tf): ((668.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) >>> import tf_conversions.posemath as pm >>> p = pm.fromTf(t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0))) - >>> print(pm.toMsg(p * p)) + >>> print pm.toMsg(p * p) position: x: 1337.0 y: 0.0 @@ -146,31 +146,30 @@ def toMatrix(f): # from camera parameters -def fromCameraParams(cv, rvec, tvec): +def fromCameraParams(cv2, rvec, tvec): """ :param cv: OpenCV module - :param rvec: A Rodrigues rotation vector - see :func:`Rodrigues2` - :type rvec: 3x1 :class:`CvMat` - :param tvec: A translation vector - :type tvec: 3x1 :class:`CvMat` + :param rvec: A Rodrigues rotation vector - see :func:`Rodrigues` + :type rvec: 3x1 :class:`numpy.array` + :param tvec: A translation vector + :type tvec: 3x1 :class:`numpy.array` :return: New :class:`PyKDL.Frame` object - For use with :func:`FindExtrinsicCameraParams2`:: + For use with :func:`solvePnP`:: - import cv + import cv2 import tf_conversions.posemath as pm ... - rvec = cv.CreateMat(3, 1, cv.CV_32FC1) - tvec = cv.CreateMat(3, 1, cv.CV_32FC1) - cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rvec, tvec) - pose = pm.fromCameraParams(cv, rvec, tvec) + _ret, rvec, tvec = cv2.solvePnP(model, corners, intrinsic_matrix, kc) + pose = pm.fromCameraParams(cv2, rvec, tvec) """ m = numpy.array([ [ 0, 0, 0, tvec[0,0] ], [ 0, 0, 0, tvec[1,0] ], [ 0, 0, 0, tvec[2,0] ], [ 0, 0, 0, 1.0 ] ], dtype = numpy.float32) - cv.Rodrigues2(rvec, m[:3,:3]) + m[:3,:3] = cv2.Rodrigues(rvec)[:3:3][0] + return fromMatrix(m) From 92c2dafb56bb46d89bee8fd3c0c4e7c2203a8c5a Mon Sep 17 00:00:00 2001 From: Raphael Memmesheimer Date: Mon, 20 Aug 2018 15:40:47 +0200 Subject: [PATCH 2/2] added brackets for py3 --- tf_conversions/src/tf_conversions/posemath.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf_conversions/src/tf_conversions/posemath.py b/tf_conversions/src/tf_conversions/posemath.py index bea060ea..72363101 100644 --- a/tf_conversions/src/tf_conversions/posemath.py +++ b/tf_conversions/src/tf_conversions/posemath.py @@ -57,7 +57,7 @@ def fromTf(tf): ((668.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) >>> import tf_conversions.posemath as pm >>> p = pm.fromTf(t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0))) - >>> print pm.toMsg(p * p) + >>> print(pm.toMsg(p * p)) position: x: 1337.0 y: 0.0