示例#1
0
def get_bbox(pc, pose=None):
    '''
    get the bounding box for the point cloud
    if a pose is provided then it will find the bbox
      for the data aligned to that pose

    return: bbox array
        |xmin, xmax|
        |ymin, ymax|
        |zmin, zmax|
  '''
    pts = ru.to_array(pc)

    if pose != None:
        ps = ru.to_PoseStamped(pose)
        ps_wrtPC = ru.transformPose(ru.get_frame_id(pc), ps)

        T = tr.rospose2tr(ps_wrtPC.pose)
        pts = np.dot(np.linalg.inv(T), np.vstack(
            (pts.T, np.ones(pts.shape[0])))).T

    pmin = np.min(pts[:, :3], axis=0)
    pmax = np.max(pts[:, :3], axis=0)
    bbox = np.array([pmin, pmax]).T

    return bbox
示例#2
0
def fit_sphere(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(30.0)):
  ''' 
    fit a sphere to the two point/normal pairs 
    eps: distance thres for determining if the fit is a valid sphere
          both points must be < eps from the sphere
    alpha: angle thres for determining if the fit is a valid sphere
          both normals must be < alpha from the sphere normals

    return (valid, center, radius);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
  p1 = np.asarray(p1);
  n1 = np.asarray(n1);
  p2 = np.asarray(p2);
  n2 = np.asarray(n2);

  # find closes points on the lines
  (pc0, pc1) = mu.line_line_closest_point(p1, n1, p2, n2);
  # center of the sphere
  c = (pc0 + pc1) / 2.0;
  # compute radius
  r = (np.linalg.norm(p1-c) + np.linalg.norm(p2-c))/2.0;

  # check if the fit is valid
  if ((np.abs(r - np.linalg.norm(p1 - c)) > eps)
      or (np.abs(r - np.linalg.norm(p2 - c)) > eps)):
    return (False, c, r);

  sa = np.sin(alpha);
  if ((np.sin(np.abs(ru.angle_between(n1, p1-c))) > sa)
      or (np.sin(np.abs(ru.angle_between(n2, p2-c))) > sa)):
    return (False, c, r);
  
  return (True, c, r);
示例#3
0
def fit_sphere(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(30.0)):
    ''' 
    fit a sphere to the two point/normal pairs 
    eps: distance thres for determining if the fit is a valid sphere
          both points must be < eps from the sphere
    alpha: angle thres for determining if the fit is a valid sphere
          both normals must be < alpha from the sphere normals

    return (valid, center, radius);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
    p1 = np.asarray(p1)
    n1 = np.asarray(n1)
    p2 = np.asarray(p2)
    n2 = np.asarray(n2)

    # find closes points on the lines
    (pc0, pc1) = mu.line_line_closest_point(p1, n1, p2, n2)
    # center of the sphere
    c = (pc0 + pc1) / 2.0
    # compute radius
    r = (np.linalg.norm(p1 - c) + np.linalg.norm(p2 - c)) / 2.0

    # check if the fit is valid
    if ((np.abs(r - np.linalg.norm(p1 - c)) > eps)
            or (np.abs(r - np.linalg.norm(p2 - c)) > eps)):
        return (False, c, r)

    sa = np.sin(alpha)
    if ((np.sin(np.abs(ru.angle_between(n1, p1 - c))) > sa)
            or (np.sin(np.abs(ru.angle_between(n2, p2 - c))) > sa)):
        return (False, c, r)

    return (True, c, r)
示例#4
0
def get_bbox(pc, pose=None):
  '''
    get the bounding box for the point cloud
    if a pose is provided then it will find the bbox
      for the data aligned to that pose

    return: bbox array
        |xmin, xmax|
        |ymin, ymax|
        |zmin, zmax|
  '''
  pts = ru.to_array(pc);

  if pose != None:
    ps = ru.to_PoseStamped(pose);
    ps_wrtPC = ru.transformPose(ru.get_frame_id(pc), ps);

    T = tr.rospose2tr(ps_wrtPC.pose);
    pts = np.dot(np.linalg.inv(T), np.vstack((pts.T, np.ones(pts.shape[0])))).T; 

  pmin = np.min(pts[:,:3], axis=0);
  pmax = np.max(pts[:,:3], axis=0);
  bbox = np.array([pmin, pmax]).T;

  return bbox;
示例#5
0
文件: Transform.py 项目: brindza/IUCS
def order_transforms(Tlist, Tbase, transWeight=1.0, rotWeight=1.0, method=2):
  ''' orders the Transforms/Frames '''
  Tlist = [ru.to_array(T) for T in Tlist]; 
  Tbase = ru.to_array(Tbase);

  d = [Tdist(T, Tbase, transWeight, rotWeight, method) for T in Tlist];

  return (np.argsort(d), d); 
示例#6
0
def order_transforms(Tlist, Tbase, transWeight=1.0, rotWeight=1.0, method=2):
    ''' orders the Transforms/Frames '''
    Tlist = [ru.to_array(T) for T in Tlist]
    Tbase = ru.to_array(Tbase)

    d = [Tdist(T, Tbase, transWeight, rotWeight, method) for T in Tlist]

    return (np.argsort(d), d)
示例#7
0
def ev2rot(*args, **kwargs):
    ''' 
    converts eigen vectors to a 4x4 transform representing the rotation

    if eigen values are provided it will sort the vectors
    if no values are provided it is assumed the vectors are sorted

    accepted args:
    - iterable of 3 'vectors'
    - 3 vector arguments

    kwargs:
    - 'evals': iterable of 3 values

    returns the Quaternion as an array

    NOTE: this is not the ros pose convention [x, y, z, w]. use ev2rosq(r) to get a ros quaternion message
  '''
    if (len(args) == 1):
        evec = np.array(map(ru.to_array, args[0]))
    if (len(args) > 1):
        evec = np.array(map(ru.to_array, args[:3]))

    if (kwargs.has_key('evals')):
        # sort based on eigen values (high->low)
        isortEvals = np.argsort(ru.to_array(kwargs['evals']))[::-1]
        evec = evec[isortEvals]

    # normalize
    evec = np.array([(ev / np.linalg.norm(ev)) for ev in evec])

    R = np.eye(4)
    R[:3, :3] = evec.T

    return R
示例#8
0
def get_centroid(pts):
    '''
    computes the centroid of the point cloud
  '''
    pts = ru.to_array(pts)

    return np.mean(pts, axis=0)
示例#9
0
def fit_plane(p1, n1, p2, n2, p3, n3, alpha=np.deg2rad(30.0)):
  ''' 
    fit a plane to the three point/normal pairs 
    alpha: angle thres for determining if the fit is a valid sphere
          both normals must be < alpha from the sphere normals

    return (valid, p, normal);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
  p1 = np.asarray(p1);
  n1 = np.asarray(n1);
  p2 = np.asarray(p2);
  n2 = np.asarray(n2);
  p3 = np.asarray(p3);
  n3 = np.asarray(n3);

  v1 = p2 - p1;
  v2 = p3 - p1;
  n = np.cross(v1, v2);
  n /= np.linalg.norm(n);

  sa = np.sin(alpha);
  sang = map(lambda nx: np.sin(np.abs(ru.angle_between(n, nx))), [n1, n2, n3]);
  if (np.any(sang > sa)):
    return (False, p1, n);
  return (True, p1, n);
示例#10
0
def estimate_normals(pts, k=5, weightNeighbors=True, viewpoint=None):
    '''
    estimates the point cloud normals from an unorder point cloud
    pts: NxD point array
    k: number of nearest neighbors to consider
    weightNeighbors: flag indicating that weights on the neighbors should be used
    viewpoint: vector indicating the viewpoint direction to correct direction
  '''
    def compute_normal(pts):
        cent = np.mean(pts)
        cov = np.cov(np.transpose(pts - cent))
        (u, s, vh) = np.linalg.svd(cov)
        return vh[2, :]

    pts = ru.to_array(pts)

    # compute distance matrix
    d = distance_matrix(pts, pts)
    # find nearest neighbors
    nn = np.argsort(d, axis=1)

    normals = np.array(map(lambda row: compute_normal(pts[row[:k + 1], :]),
                           nn))

    if (viewpoint != None):
        vcheck = map(lambda i: np.dot(normals[i, :], viewpoint - pts[i, :]),
                     np.arange(pts.shape[0]))
        normals[(vcheck < 0), :] *= -1.0

    return normals
示例#11
0
def estimate_normals(pts, k=5, weightNeighbors=True, viewpoint=None):
  '''
    estimates the point cloud normals from an unorder point cloud
    pts: NxD point array
    k: number of nearest neighbors to consider
    weightNeighbors: flag indicating that weights on the neighbors should be used
    viewpoint: vector indicating the viewpoint direction to correct direction
  '''
  def compute_normal(pts):
    cent = np.mean(pts);
    cov = np.cov(np.transpose(pts - cent));
    (u, s, vh) = np.linalg.svd(cov);
    return vh[2,:];

  pts = ru.to_array(pts);
  
  # compute distance matrix
  d = distance_matrix(pts, pts);
  # find nearest neighbors
  nn = np.argsort(d, axis=1);

  normals = np.array(map(lambda row: compute_normal(pts[row[:k+1],:]), nn));

  if (viewpoint != None):
    vcheck = map(lambda i: np.dot(normals[i,:], viewpoint - pts[i,:]), np.arange(pts.shape[0]));
    normals[(vcheck < 0),:] *= -1.0;

  return normals;
示例#12
0
def fit_plane(p1, n1, p2, n2, p3, n3, alpha=np.deg2rad(30.0)):
    ''' 
    fit a plane to the three point/normal pairs 
    alpha: angle thres for determining if the fit is a valid sphere
          both normals must be < alpha from the sphere normals

    return (valid, p, normal);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
    p1 = np.asarray(p1)
    n1 = np.asarray(n1)
    p2 = np.asarray(p2)
    n2 = np.asarray(n2)
    p3 = np.asarray(p3)
    n3 = np.asarray(n3)

    v1 = p2 - p1
    v2 = p3 - p1
    n = np.cross(v1, v2)
    n /= np.linalg.norm(n)

    sa = np.sin(alpha)
    sang = map(lambda nx: np.sin(np.abs(ru.angle_between(n, nx))),
               [n1, n2, n3])
    if (np.any(sang > sa)):
        return (False, p1, n)
    return (True, p1, n)
示例#13
0
def get_centroid(pts):
  '''
    computes the centroid of the point cloud
  '''
  pts = ru.to_array(pts);

  return np.mean(pts, axis=0);
示例#14
0
文件: Transform.py 项目: brindza/IUCS
def ev2rot(*args, **kwargs):
  ''' 
    converts eigen vectors to a 4x4 transform representing the rotation

    if eigen values are provided it will sort the vectors
    if no values are provided it is assumed the vectors are sorted

    accepted args:
    - iterable of 3 'vectors'
    - 3 vector arguments

    kwargs:
    - 'evals': iterable of 3 values

    returns the Quaternion as an array

    NOTE: this is not the ros pose convention [x, y, z, w]. use ev2rosq(r) to get a ros quaternion message
  ''' 
  if (len(args) == 1):
    evec = np.array(map(ru.to_array, args[0]));
  if (len(args) > 1):
    evec = np.array(map(ru.to_array, args[:3]));

  if (kwargs.has_key('evals')):
    # sort based on eigen values (high->low)
    isortEvals = np.argsort(ru.to_array(kwargs['evals']))[::-1];
    evec = evec[isortEvals];

  # normalize
  evec = np.array([(ev / np.linalg.norm(ev)) for ev in evec]);
  
  R = np.eye(4);
  R[:3,:3] = evec.T;

  return R;
示例#15
0
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); 
示例#16
0
def get_blob_pose(pts):
  '''
    computes the pose of the point cloud
    arr is expected to be Nx3

    return Pose
  '''
  pts = ru.to_array(pts);
  
  cent = get_centroid(pts);
  (x, y, z) = get_ev(pts);
  
  pose = geometry_msgs.Pose();
  pose.position = geometry_msgs.Point(*cent);
  pose.orientation = tr.ev2rosq((x,y,z));

  return pose;
示例#17
0
def project_points(points, caminfo):
  ''' 
    projects the 3D point into the desired image coordinates 

    points: array of points to project (default is Nx3) in the camera frame

    return: Nx2 array of projected points
  '''
  # format input correctly
  points = ru.to_array(points);
  pshape = points.shape;
  if (len(pshape) < 2):
    points = np.array([points]);
  elif (max(pshape) > 3):
    if (pshape[1] > pshape[0]):
      points = points.T; 
  elif (max(pshape) == 3):
    if (pshape[0] > pshape[1]):
      points = points.T;

  # get camera matrix
  C = np.reshape(np.array(caminfo.K), (3,3));
  # get distorition coef
  (k1, k2, p1, p2, k3) = caminfo.D;  
    
  # normalize with z
  points = points.T/points[:,2];

  # undistort
  r2 = np.sum(points[:2,:]**2, axis=0);
  xy = np.prod(points[:2,:], axis=0);
  d1 = (1.0 + k1*r2 + k2*r2**2 + k3*r2**3);
  c1 = points[:2,:]*np.array([d1,d1]);
  c2 = np.array([2.0*p1*xy, 2.0*p2*xy]);
  c3 = np.array([p2*(r2 + 2*points[0,:]**2), p1*(r2 + 2*points[1,:]**2)]);
  points[:2,:] = c1 + c2 + c3;

  return C;

  # project
  prj =  np.dot(C, points);

  if (prj.shape[1] == 1):
    return prj[:2,0].T;
  else:
    return prj[:2,:].T;
示例#18
0
def get_blob_pose(pts):
    '''
    computes the pose of the point cloud
    arr is expected to be Nx3

    return Pose
  '''
    pts = ru.to_array(pts)

    cent = get_centroid(pts)
    (x, y, z) = get_ev(pts)

    pose = geometry_msgs.Pose()
    pose.position = geometry_msgs.Point(*cent)
    pose.orientation = tr.ev2rosq((x, y, z))

    return pose
示例#19
0
def get_ev(pts, includeEigenValues=False):
  '''
    computes the eigen vectors for the point cloud
    arr is expected to be Nx3

    return:
      array of row vectors (e1, e2, e3)
  '''
  pts = ru.to_array(pts);

  trans = -1 * np.mean(pts, axis=0);
  pts += trans;

  cov = np.cov(pts.T);
  (u, s, vh) = np.linalg.svd(cov);

  if includeEigenValues:
    return (vh, s);
  else:
    return vh;
示例#20
0
def get_ev(pts, includeEigenValues=False):
    '''
    computes the eigen vectors for the point cloud
    arr is expected to be Nx3

    return:
      array of row vectors (e1, e2, e3)
  '''
    pts = ru.to_array(pts)

    trans = -1 * np.mean(pts, axis=0)
    pts += trans

    cov = np.cov(pts.T)
    (u, s, vh) = np.linalg.svd(cov)

    if includeEigenValues:
        return (vh, s)
    else:
        return vh
示例#21
0
文件: Transform.py 项目: brindza/IUCS
def Tdist(T0, T1, transWeight=1.0, rotWeight=1.0, method=2):
  if (method == 1):
    # 1 norm
    td = np.linalg.norm(T0[:3,3] - T1[:3,3])
    rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1), ord=1);
    return (transWeight * td + rotWeight * rd);
  
  if (method == 2):
    # 2 norm
    td = np.linalg.norm(T0[:3,3] - T1[:3,3])
    rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1));
    return (transWeight * td + rotWeight * rd);

  if (method == 'ax'):
    # squared angle between each axis
    td = np.linalg.norm(T0[:3,3] - T1[:3,3])
    (x0, y0, z0) = rot2ev(T0);
    (x1, y1, z1) = rot2ev(T1);
    rd = np.linalg.norm([ru.angle_between(v0, v1) for (v0, v1) in ((x0, x1), (y0, y1), (z0, z1))]);
    return (transWeight * td + rotWeight * rd);
示例#22
0
文件: Transform.py 项目: brindza/IUCS
def translate_rospose(pose, t):
  ''' 
    translates a ros pose with respect to the poses orientation

    return will be a Pose or PoseStamped based on the input (header will be copied)
  '''
  t = ru.to_array(t); 

  if (t.ndim == 2):
    # 4x4 transform (extract the translation from it) 
    t = trans(t[:3,3]);
  else:
    t = trans(t[:3]);

  T = rospose2tr(pose);
  nT = np.dot(T, t);

  if (type(pose) == geometry_msgs.PoseStamped):
    return geometry_msgs.PoseStamped(pose=tr2rospose(nT), header=copy.copy(pose.header));
  else:
    return tr2rospose(nT);
示例#23
0
def translate_rospose(pose, t):
    ''' 
    translates a ros pose with respect to the poses orientation

    return will be a Pose or PoseStamped based on the input (header will be copied)
  '''
    t = ru.to_array(t)

    if (t.ndim == 2):
        # 4x4 transform (extract the translation from it)
        t = trans(t[:3, 3])
    else:
        t = trans(t[:3])

    T = rospose2tr(pose)
    nT = np.dot(T, t)

    if (type(pose) == geometry_msgs.PoseStamped):
        return geometry_msgs.PoseStamped(pose=tr2rospose(nT),
                                         header=copy.copy(pose.header))
    else:
        return tr2rospose(nT)
示例#24
0
文件: Transform.py 项目: brindza/IUCS
def rotate_rospose(pose, r):
  ''' 
    rotates a ros pose orientation with respect to the poses current orientation
    rot: either list of EulerZYX angles (z, y, x) or 4x4 transform matrix

    return will be a Pose or PoseStamped based on the input (header will be copied)
  '''
  R = ru.to_array(r); 

  if (R.ndim == 2):
    # 4x4 transform (extract the rotation from it) 
    R[:3,3] = 0.0;
  else:
    R = ezyx(r);

  T = rospose2tr(pose);
  nT = np.dot(T, R);

  if (type(pose) == geometry_msgs.PoseStamped):
    return geometry_msgs.PoseStamped(pose=tr2rospose(nT), header=copy.copy(pose.header));
  else:
    return tr2rospose(nT);
示例#25
0
def rotate_rospose(pose, r):
    ''' 
    rotates a ros pose orientation with respect to the poses current orientation
    rot: either list of EulerZYX angles (z, y, x) or 4x4 transform matrix

    return will be a Pose or PoseStamped based on the input (header will be copied)
  '''
    R = ru.to_array(r)

    if (R.ndim == 2):
        # 4x4 transform (extract the rotation from it)
        R[:3, 3] = 0.0
    else:
        R = ezyx(r)

    T = rospose2tr(pose)
    nT = np.dot(T, R)

    if (type(pose) == geometry_msgs.PoseStamped):
        return geometry_msgs.PoseStamped(pose=tr2rospose(nT),
                                         header=copy.copy(pose.header))
    else:
        return tr2rospose(nT)
示例#26
0
def Tdist(T0, T1, transWeight=1.0, rotWeight=1.0, method=2):
    if (method == 1):
        # 1 norm
        td = np.linalg.norm(T0[:3, 3] - T1[:3, 3])
        rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1), ord=1)
        return (transWeight * td + rotWeight * rd)

    if (method == 2):
        # 2 norm
        td = np.linalg.norm(T0[:3, 3] - T1[:3, 3])
        rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1))
        return (transWeight * td + rotWeight * rd)

    if (method == 'ax'):
        # squared angle between each axis
        td = np.linalg.norm(T0[:3, 3] - T1[:3, 3])
        (x0, y0, z0) = rot2ev(T0)
        (x1, y1, z1) = rot2ev(T1)
        rd = np.linalg.norm([
            ru.angle_between(v0, v1)
            for (v0, v1) in ((x0, x1), (y0, y1), (z0, z1))
        ])
        return (transWeight * td + rotWeight * rd)
示例#27
0
 def compute_pose(c, n):
     v = np.cross([1, 0, 0], n)
     v /= np.linalg.norm(v)
     th = ru.angle_between([1, 0, 0], n)
     return geometry_msgs.Pose(geometry_msgs.Point(*c),
                               tr.axis_angle2rosq(v, th))
示例#28
0
 def compute_pose(c, n):
   v = np.cross([1, 0, 0], n);
   v /= np.linalg.norm(v);
   th = ru.angle_between([1, 0, 0], n);
   return geometry_msgs.Pose(geometry_msgs.Point(*c), tr.axis_angle2rosq(v, th));
示例#29
0
def fit_cone(p1, n1, p2, n2, p3, n3, eps=0.005, alpha=np.deg2rad(10.0)):
  ''' 
    fit a cone to the three point/normal pairs 
    eps: distance thres for determining if the fit is a valid cylinder
          both points must be < eps from the cylinder
    alpha: angle thres for determining if the fit is a valid cylinder
          both normals must be < alpha from the cylinder normals

    return (valid, apex, axis, opening angle);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
  p1 = np.asarray(p1);
  n1 = np.asarray(n1);
  p2 = np.asarray(p2);
  n2 = np.asarray(n2);
  p3 = np.asarray(p3);
  n3 = np.asarray(n3);

  # find cone apex (intersection of the 3 planes formed by the point/normal pairs)
  # line normal from plane 1/2 intersection
  (lp, ln) = mu.plane_plane_intersection(p1, n1, p2, n2);
  c = mu.line_plane_intersection(lp, ln, p3, n3);

  # find the axis from the normal of the plane formed by the three points
  pc1 = c + (p1 - c)/np.linalg.norm(p1 - c);
  pc2 = c + (p2 - c)/np.linalg.norm(p2 - c);
  pc3 = c + (p3 - c)/np.linalg.norm(p3 - c);
  a = np.cross(pc2 - pc1, pc3 - pc1);
  a /= np.linalg.norm(a);

  # find opening anlge
  ac = np.array(map(lambda p: ru.angle_between(p - c, a), [p1, p2, p3]));
  w = np.sum(ac)/3.0;

  # check validity

  # project each point onto the axis
  p1_proj_a = np.dot(p1 - c, a) * a;
  p2_proj_a = np.dot(p2 - c, a) * a;
  p3_proj_a = np.dot(p3 - c, a) * a;
  # and rejections
  p1_rej_a = (p1 - c) - p1_proj_a;
  p2_rej_a = (p2 - c) - p2_proj_a;
  p3_rej_a = (p3 - c) - p3_proj_a;

  # projection mag
  d1 = np.linalg.norm(p1_proj_a);
  d2 = np.linalg.norm(p2_proj_a);
  d3 = np.linalg.norm(p3_proj_a);
  # mag of vector from axis to cone edge
  r1 = d1 * np.tan(w);
  r2 = d2 * np.tan(w);
  r3 = d3 * np.tan(w);

  # scale rejections to find the cone point 
  c1 = c + p1_proj_a + (r1/np.linalg.norm(p1_rej_a))*p1_rej_a;
  c2 = c + p2_proj_a + (r2/np.linalg.norm(p2_rej_a))*p2_rej_a;
  c3 = c + p3_proj_a + (r3/np.linalg.norm(p3_rej_a))*p3_rej_a;
  

  # is the point within distance thres?
  distToCone = np.array([np.linalg.norm(p1 - c1), np.linalg.norm(p2 - c2), np.linalg.norm(p3 - c3)]);
  if np.any(distToCone > eps):
    return (False, c, a, w);

  # compute cone normals
  cn1 = np.cross(np.cross(a, (c1 - c)), (c1 - c));
  cn2 = np.cross(np.cross(a, (c2 - c)), (c2 - c));
  cn3 = np.cross(np.cross(a, (c3 - c)), (c3 - c));
  cn1 /= np.linalg.norm(cn1);
  cn2 /= np.linalg.norm(cn2);
  cn3 /= np.linalg.norm(cn3);

  # are the normals close?
  sa = np.sin(alpha);
  if ((np.sin(np.abs(ru.angle_between(cn1, n1))) > sa)
      or (np.sin(np.abs(ru.angle_between(cn2, n2))) > sa)
      or (np.sin(np.abs(ru.angle_between(cn3, n3))) > sa)):
    return (False, c, a, w);

  return (True, c, a, w);
示例#30
0
def fit_cylinder(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(10.0)):
    ''' 
    fit a cylinder to the two point/normal pairs 
    eps: distance thres for determining if the fit is a valid cylinder
          both points must be < eps from the cylinder
    alpha: angle thres for determining if the fit is a valid cylinder
          both normals must be < alpha from the cylinder normals

    return (valid, center, axis, radius);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
    p1 = np.asarray(p1)
    n1 = np.asarray(n1)
    p2 = np.asarray(p2)
    n2 = np.asarray(n2)

    # find cylinder axis
    a = np.cross(n1, n2)
    a /= np.linalg.norm(a)

    # project lines (defined by the point/normal) onto the plane defined by a.x = 0
    pp1 = p1 - a * np.dot(a, p1)
    pn1 = n1 - a * np.dot(a, n1)
    pp2 = p2 - a * np.dot(a, p2)
    pn2 = n2 - a * np.dot(a, n2)

    # find intersection
    (c, c1) = mu.line_line_closest_point(pp1, pn1, pp2, pn2)

    # cylinder radius
    r = np.linalg.norm(pp1 - c)

    # check if the fit is valid

    # find rejections of the points from the cylinder axis
    cp1 = pp1 - c
    cp2 = pp2 - c
    ca = c + a
    rej1 = cp1 - np.dot(cp1, ca) * ca
    rej2 = cp2 - np.dot(cp2, ca) * ca

    pa = create_normals_pose_array(np.array([p1, p2]), np.array([rej1, rej2]))
    #Debug.pa1Pub.publish(pa);
    print rej1
    print rej2
    print np.sin(np.abs(ru.angle_between(rej1, n1)))
    print np.sin(np.abs(ru.angle_between(rej2, n2)))
    print np.abs(r - np.linalg.norm(rej1))
    print np.abs(r - np.linalg.norm(rej2))

    sa = np.sin(alpha)
    if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
            or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
        return (False, c, a, r)

    if ((np.abs(r - np.linalg.norm(rej1)) > eps)
            or (np.abs(r - np.linalg.norm(rej2)) > eps)):
        return (False, c, a, r)

    sa = np.sin(alpha)
    if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
            or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
        return (False, c, a, r)

    return (True, c, a, r)
示例#31
0
def fit_cone(p1, n1, p2, n2, p3, n3, eps=0.005, alpha=np.deg2rad(10.0)):
    ''' 
    fit a cone to the three point/normal pairs 
    eps: distance thres for determining if the fit is a valid cylinder
          both points must be < eps from the cylinder
    alpha: angle thres for determining if the fit is a valid cylinder
          both normals must be < alpha from the cylinder normals

    return (valid, apex, axis, opening angle);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
    p1 = np.asarray(p1)
    n1 = np.asarray(n1)
    p2 = np.asarray(p2)
    n2 = np.asarray(n2)
    p3 = np.asarray(p3)
    n3 = np.asarray(n3)

    # find cone apex (intersection of the 3 planes formed by the point/normal pairs)
    # line normal from plane 1/2 intersection
    (lp, ln) = mu.plane_plane_intersection(p1, n1, p2, n2)
    c = mu.line_plane_intersection(lp, ln, p3, n3)

    # find the axis from the normal of the plane formed by the three points
    pc1 = c + (p1 - c) / np.linalg.norm(p1 - c)
    pc2 = c + (p2 - c) / np.linalg.norm(p2 - c)
    pc3 = c + (p3 - c) / np.linalg.norm(p3 - c)
    a = np.cross(pc2 - pc1, pc3 - pc1)
    a /= np.linalg.norm(a)

    # find opening anlge
    ac = np.array(map(lambda p: ru.angle_between(p - c, a), [p1, p2, p3]))
    w = np.sum(ac) / 3.0

    # check validity

    # project each point onto the axis
    p1_proj_a = np.dot(p1 - c, a) * a
    p2_proj_a = np.dot(p2 - c, a) * a
    p3_proj_a = np.dot(p3 - c, a) * a
    # and rejections
    p1_rej_a = (p1 - c) - p1_proj_a
    p2_rej_a = (p2 - c) - p2_proj_a
    p3_rej_a = (p3 - c) - p3_proj_a

    # projection mag
    d1 = np.linalg.norm(p1_proj_a)
    d2 = np.linalg.norm(p2_proj_a)
    d3 = np.linalg.norm(p3_proj_a)
    # mag of vector from axis to cone edge
    r1 = d1 * np.tan(w)
    r2 = d2 * np.tan(w)
    r3 = d3 * np.tan(w)

    # scale rejections to find the cone point
    c1 = c + p1_proj_a + (r1 / np.linalg.norm(p1_rej_a)) * p1_rej_a
    c2 = c + p2_proj_a + (r2 / np.linalg.norm(p2_rej_a)) * p2_rej_a
    c3 = c + p3_proj_a + (r3 / np.linalg.norm(p3_rej_a)) * p3_rej_a

    # is the point within distance thres?
    distToCone = np.array([
        np.linalg.norm(p1 - c1),
        np.linalg.norm(p2 - c2),
        np.linalg.norm(p3 - c3)
    ])
    if np.any(distToCone > eps):
        return (False, c, a, w)

    # compute cone normals
    cn1 = np.cross(np.cross(a, (c1 - c)), (c1 - c))
    cn2 = np.cross(np.cross(a, (c2 - c)), (c2 - c))
    cn3 = np.cross(np.cross(a, (c3 - c)), (c3 - c))
    cn1 /= np.linalg.norm(cn1)
    cn2 /= np.linalg.norm(cn2)
    cn3 /= np.linalg.norm(cn3)

    # are the normals close?
    sa = np.sin(alpha)
    if ((np.sin(np.abs(ru.angle_between(cn1, n1))) > sa)
            or (np.sin(np.abs(ru.angle_between(cn2, n2))) > sa)
            or (np.sin(np.abs(ru.angle_between(cn3, n3))) > sa)):
        return (False, c, a, w)

    return (True, c, a, w)
示例#32
0
def fit_cylinder(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(10.0)):
  ''' 
    fit a cylinder to the two point/normal pairs 
    eps: distance thres for determining if the fit is a valid cylinder
          both points must be < eps from the cylinder
    alpha: angle thres for determining if the fit is a valid cylinder
          both normals must be < alpha from the cylinder normals

    return (valid, center, axis, radius);
      valid is a bool indicating if the fit was valid based on the points and normals
  '''
  p1 = np.asarray(p1);
  n1 = np.asarray(n1);
  p2 = np.asarray(p2);
  n2 = np.asarray(n2);

  # find cylinder axis
  a = np.cross(n1, n2);
  a /= np.linalg.norm(a);

  # project lines (defined by the point/normal) onto the plane defined by a.x = 0
  pp1 = p1 - a * np.dot(a, p1);
  pn1 = n1 - a * np.dot(a, n1);
  pp2 = p2 - a * np.dot(a, p2);
  pn2 = n2 - a * np.dot(a, n2);
  
  # find intersection
  (c, c1) = mu.line_line_closest_point(pp1, pn1, pp2, pn2);

  # cylinder radius
  r = np.linalg.norm(pp1 - c);

  # check if the fit is valid
  
  # find rejections of the points from the cylinder axis
  cp1 = pp1 - c;
  cp2 = pp2 - c;
  ca = c + a;
  rej1 = cp1 - np.dot(cp1, ca)*ca; 
  rej2 = cp2 - np.dot(cp2, ca)*ca; 

  pa = create_normals_pose_array(np.array([p1,p2]), np.array([rej1,rej2]));
  #Debug.pa1Pub.publish(pa);
  print rej1
  print rej2
  print np.sin(np.abs(ru.angle_between(rej1, n1)))
  print np.sin(np.abs(ru.angle_between(rej2, n2)))
  print np.abs(r - np.linalg.norm(rej1))
  print np.abs(r - np.linalg.norm(rej2))


  sa = np.sin(alpha);
  if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
      or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
    return (False, c, a, r);

  if ((np.abs(r - np.linalg.norm(rej1)) > eps)
      or (np.abs(r - np.linalg.norm(rej2)) > eps)):
    return (False, c, a, r);

  sa = np.sin(alpha);
  if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
      or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
    return (False, c, a, r);
  
  return (True, c, a, r);