Exemplo n.º 1
0
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)
Exemplo n.º 2
0
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)