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