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;
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
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);