Python Transform.trans方法代码示例

# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import trans [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); 

# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import trans [as 别名]
def icp(data, model, thres=.001, maxIter=1000):
  '''
    compute icp on two point sets

    matrix: DxN

    return:
      qr - quaterion rotation
      qt - translation
  '''
  # augment if needed
  if data.shape[0] == 3:
    data = np.vstack((data, np.ones(data.shape[1])));
  if model.shape[0] == 3:
    model = np.vstack((model, np.ones(model.shape[1])));

  # initialize registration
  qr = np.array([1.0, 0.0, 0.0, 0.0]);
  qt = np.array([0.0, 0.0, 0.0]);
  dm = np.Inf;
  count = 0;

  # compute initial closest points
  di = np.arange(data.shape[1]);
  (imin, d) = closest_points(data, model);
  dk = np.sum(d[di, imin]);

  # loop until the change in error is below the threshold
  while (dm > thres and count < maxIter):
  
    # compute registration
    (qr, qt) = compute_registration(data, model[:,imin]);

    # transform data
    T = np.dot(tr.trans(qt), tr.quat2rot(qr));
    tdata = np.dot(T, data);

    # compute closest points
    (imin, d) = closest_points(tdata, model);
    dk1 = np.sum(d[di, imin]);

    dm = np.abs(dk1 - dk);
    dk = dk1;
    count += 1;

  return (qr, qt, dk);

# 需要导入模块: import Transform [as 别名]
# 或者: from Transform import trans [as 别名]
    return listener.transformPointCloud(target_frame, point_cloud);


def lookupTransform(dstFrame, srcFrame, stamp=rospy.Time(), waitTimeout=0.1):
  ''' returns the 4x4 transform for the given frames '''
  global listener
  try:
    listener.waitForTransform(dstFrame, srcFrame, stamp, rospy.Duration(waitTimeout))
    (t,q) = listener.lookupTransform(dstFrame, srcFrame, stamp);
  except (tf.Exception, tf.ExtrapolationException), ex:
    # transform failed, use most current transform
    rospy.logwarn(__name__ + ': unable to get transform : %s' % ex);
    rospy.logwarn(__name__ + ': proceeding with most recent transform');
    (t,q) = listener.lookupTransform(dstFrame, srcFrame, rospy.Time());

  return np.dot(tr.trans(t), tr.rosq2rot(q));


def get_frame_id(msg):
  ''' 
    returns the frame id for the input
    '' if no frame is specified
  '''
  if hasattr(msg, 'header'):
    return msg.header.frame_id;
  elif hasattr(msg, 'frame_id'):
    return obj.frame_id;
  else:
    return '';