def imagePubCB(event):
camPos = pose[0:3]
camOrient = pose[3:7]
targetPos = pose[7:10]
targetOrient = pose[10:]
# Transform
br.sendTransform(targetPos,targetOrient,rospy.Time.now(),"ugv0","world")
# Inverse camera pose
coiInv = qInv(camOrient)
cpiInv = rotateVec(-camPos,coiInv)
(rvec,jac) = cv2.Rodrigues(q2m(coiInv)[0:3,0:3])
# Image Projections
(imagePoint,jac) = cv2.projectPoints(np.array([[targetPos]]),rvec,cpiInv,camMat,distCoeffs)
imagePoint = np.squeeze(imagePoint)
(bearing,jac) = cv2.projectPoints(np.array([[targetPos]]),rvec,cpiInv,np.eye(3),np.zeros(5))
bearing = np.squeeze(bearing)
# Publish image point
centMsg = Center()
centMsg.header.stamp = rospy.Time.now()
centMsg.header.frame_id = str(markerID)
centMsg.x = imagePoint[0]
centMsg.y = imagePoint[1]
centMsg.x1 = bearing[0]
centMsg.x2 = bearing[1]
centerPub.publish(centMsg)
def imagePubCB(event):
camPos = pose[0:3]
camOrient = pose[3:7]
targetPos = pose[7:10]
targetOrient = pose[10:]
# Inverse camera pose
coiInv = qInv(camOrient)
cpiInv = rotateVec(-camPos,coiInv)
(rvec,jac) = cv2.Rodrigues(q2m(coiInv)[0:3,0:3])
# Image Projections
(imagePoint,jac) = cv2.projectPoints(np.array([[targetPos]]),rvec,cpiInv,camMat,distCoeffs)
imagePoint = np.squeeze(imagePoint)
(bearing,jac) = cv2.projectPoints(np.array([[targetPos]]),rvec,cpiInv,np.eye(3),np.zeros(5))
bearing = np.squeeze(bearing)
# Publish image point
centMsg = Center()
centMsg.header.stamp = rospy.Time.now()
centMsg.header.frame_id = str(markerID)
centMsg.x = imagePoint[0]
centMsg.y = imagePoint[1]
centerPub.publish(centMsg)
# Publish pose
relPos = rotateVec(targetPos - camPos,qInv(camOrient))
relOrient = qMult(qInv(camOrient),targetOrient)
poseMsg = PoseStamped()
poseMsg.header.stamp = centMsg.header.stamp
poseMsg.header.frame_id = str(markerID)
poseMsg.pose.position.x = relPos[0]
poseMsg.pose.position.y = relPos[1]
poseMsg.pose.position.z = relPos[2]
poseMsg.pose.orientation.x = relOrient[0]
poseMsg.pose.orientation.y = relOrient[1]
poseMsg.pose.orientation.z = relOrient[2]
poseMsg.pose.orientation.w = relOrient[3]
posePub.publish(poseMsg)
