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);
def detcb(imagemsg):
if imagemsg.header.stamp > timestart:
cvimage = self.bridge.imgmsg_to_cv(imagemsg, 'mono8')
corners = self.detect(cvimage)
if corners is not None:
with donecond:
# get the pose with respect to the attached sensor tf frame
if self.cvKK is None or self.cvkc is None:
KK, kc, Ts, error = self.calibrateIntrinsicCamera(
[self.getObjectPoints()], [corners],
(cvimage.width, cvimage.height),
usenonlinoptim=False)
T = Ts[0]
else:
cvrvec = cv.CreateMat(1, 3, cv.CV_64F)
cvtvec = cv.CreateMat(1, 3, cv.CV_64F)
cv.FindExtrinsicCameraParams2(
cv.fromarray(self.getObjectPoints()),
cv.fromarray(corners), self.cvKK, self.cvkc,
cvrvec, cvtvec)
T = matrixFromAxisAngle(array(cvrvec)[0])
T[0:3, 3] = array(cvtvec)[0]
data[0] = {
'T': T,
'corners_raw': corners,
'image_raw': array(cvimage)
}
if 'type' in self.pattern:
data[0]['type'] = self.pattern['type']
donecond.notifyAll()
def FindExtrinsicCameraParams(imagepoints, objectpoints, KK):
""" Use OpenCV to solve for the affine transformation that matches imagepoints to object points
imagepoints - 2xN array
objectpoints - 3xN array
KK - 3x3 array or 4 element array
"""
imagepoints = array(imagepoints,float)
objectpoints = array(objectpoints,float)
if len(KK.shape) == 1:
cvKK = cv.CreateMat(3,3,cv.CV_32FC1)
cvKK[0,0] = KK[0]; cvKK[0,1] = 0; cvKK[0,2] = KK[2];
cvKK[1,0] = 0; cvKK[1,1] = KK[1]; cvKK[1,2] = KK[3];
cvKK[2,0] = 0; cvKK[2,1] = 0; cvKK[2,2] = 1;
else:
cvKK = cv.fromarray(KK)
cvDist = cv.CreateMat(4,1,cv.CV_32FC1)
cvDist[0,0] = 0; cvDist[1,0] = 0; cvDist[2,0] = 0; cvDist[3,0] = 0;
rvec = cv.CreateMat(3,1,cv.CV_32FC1)
tvec = cv.CreateMat(3,1,cv.CV_32FC1)
object_points = cv.CreateMatHeader(3,objectpoints.shape[0],cv.CV_32FC1)
cv.SetData(object_points,struct.pack('f'*(objectpoints.shape[0]*3),*transpose(objectpoints).flat),4*objectpoints.shape[0])
image_points = cv.CreateMatHeader(2,imagepoints.shape[0],cv.CV_32FC1)
cv.SetData(image_points,struct.pack('f'*(imagepoints.shape[0]*2),*transpose(imagepoints).flat),4*imagepoints.shape[0])
cv.FindExtrinsicCameraParams2(object_points,image_points,cvKK,cvDist,rvec,tvec)
T = matrixFromAxisAngle((rvec[0,0],rvec[1,0],rvec[2,0]))
T[0:3,3] = [tvec[0,0],tvec[1,0],tvec[2,0]]
return T
def get_target_pose(cam):
# Populate object_points
object_points = cv.fromarray(
numpy.array([[p.x, p.y, p.z] for p in cam.features.object_points]))
image_points = cv.fromarray(
numpy.array([[p.x, p.y] for p in cam.features.image_points]))
dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]]))
camera_matrix = numpy.array(
[[cam.cam_info.P[0], cam.cam_info.P[1], cam.cam_info.P[2]],
[cam.cam_info.P[4], cam.cam_info.P[5], cam.cam_info.P[6]],
[cam.cam_info.P[8], cam.cam_info.P[9], cam.cam_info.P[10]]])
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(object_points, image_points,
cv.fromarray(camera_matrix), dist_coeffs,
rot, trans)
rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot3x3)
frame = PyKDL.Frame()
for i in range(3):
frame.p[i] = trans[i, 0]
for i in range(3):
for j in range(3):
frame.M[i, j] = rot3x3[i, j]
return frame
def gotimage(self, imgmsg):
if imgmsg.encoding == "bayer_bggr8":
imgmsg.encoding = "8UC1"
img = CvBridge().imgmsg_to_cv(imgmsg)
# cv.ShowImage("DataMatrix", img)
# cv.WaitKey(6)
self.track(img)
# monocular case
print self.cams
if 'l' in self.cams:
for (code, corners) in self.tracking.items():
model = cv.fromarray(numpy.array([[0,0,0], [.1, 0, 0], [.1, .1, 0], [0, .1, 0]], numpy.float32))
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(model,
cv.fromarray(numpy.array(corners, numpy.float32)),
self.cams['l'].intrinsicMatrix(),
self.cams['l'].distortionCoeffs(),
rot,
trans)
ts = geometry_msgs.msg.TransformStamped()
ts.header.frame_id = imgmsg.header.frame_id
ts.header.stamp = imgmsg.header.stamp
ts.child_frame_id = code
posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
ts.transform.translation = posemsg.position
ts.transform.rotation = posemsg.orientation
tfm = tf.msg.tfMessage([ts])
self.pub_tf.publish(tfm)
def handle_monocular(self, msg):
def pt2line(x0, y0, x1, y1, x2, y2):
""" point is (x0, y0), line is (x1, y1, x2, y2) """
return abs((x2 - x1) * (y1 - y0) - (x1 - x0) *
(y2 - y1)) / math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
(image, camera) = msg
rgb = self.mkgray(image)
C = self.image_corners(rgb)
if C:
cc = self.board.n_cols
cr = self.board.n_rows
errors = []
for r in range(cr):
(x1, y1) = C[(cc * r) + 0]
(x2, y2) = C[(cc * r) + cc - 1]
for i in range(1, cc - 1):
(x0, y0) = C[(cc * r) + i]
errors.append(pt2line(x0, y0, x1, y1, x2, y2))
linearity_rms = math.sqrt(
sum([e**2 for e in errors]) / len(errors))
# Add in reprojection check
A = cv.CreateMat(3, 3, 0)
image_points = cv.fromarray(numpy.array(C))
object_points = cv.fromarray(numpy.zeros([cc * cr, 3]))
for i in range(cr):
for j in range(cc):
object_points[i * cc + j, 0] = j * self.board.dim
object_points[i * cc + j, 1] = i * self.board.dim
object_points[i * cc + j, 2] = 0.0
dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]]))
camera_matrix = numpy.array(
[[camera.P[0], camera.P[1], camera.P[2]],
[camera.P[4], camera.P[5], camera.P[6]],
[camera.P[8], camera.P[9], camera.P[10]]])
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(object_points, image_points,
cv.fromarray(camera_matrix),
dist_coeffs, rot, trans)
# Convert rotation into a 3x3 Rotation Matrix
rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot3x3)
# Reproject model points into image
object_points_world = numpy.asmatrix(rot3x3) * (
numpy.asmatrix(object_points).T) + numpy.asmatrix(trans)
reprojected_h = camera_matrix * object_points_world
reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
reprojection_errors = image_points - reprojected.T
reprojection_rms = numpy.sqrt(
numpy.sum(numpy.array(reprojection_errors)**2) /
numpy.product(reprojection_errors.shape))
# Print the results
print "Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (
linearity_rms, reprojection_rms)
else:
print 'no chessboard'
def solve(C, Q, matches, dbsiftpath, dbimgpath):
# open EarthMine info
info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info')
em = render_tags.EarthmineImageInfo(dbimgpath, info)
map3d = C.pixelmap.open(dbsiftpath)
# find non-None features
vector = []
for i, m in enumerate(matches):
d = m['db']
# get 3d pt of feature
feature = map3d[int(d[0]), int(d[1])]
if not feature:
continue
# convert from latlon to meters relative to earthmine camera
pz, px = geom.lltom(em.lat, em.lon, feature['lat'], feature['lon'])
py = feature['alt'] - em.alt
vector.append((m['query'][:2], (px, py, -pz)))
print vector[0]
# reference camera matrix
# f 0 0
# 0 f 0
# 0 0 1
A = cv.CreateMat(3, 3, cv.CV_64F)
cv.SetZero(A)
f = 662 # focal len?
cv.Set2D(A, 0, 0, cv.Scalar(f))
cv.Set2D(A, 1, 1, cv.Scalar(f))
cv.Set2D(A, 2, 2, cv.Scalar(1))
# convert vector to to cvMats
objectPoints3d = cv.CreateMat(len(vector), 1, cv.CV_64FC3)
imagePoints2d = cv.CreateMat(len(vector), 1, cv.CV_64FC2)
for i, (p2d, p3d) in enumerate(vector):
cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*p2d))
cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*p3d))
coeff = cv.CreateMat(4, 1, cv.CV_64F)
rvec = cv.CreateMat(3, 1, cv.CV_64F)
tvec = cv.CreateMat(3, 1, cv.CV_64F)
cv.SetZero(coeff)
cv.SetZero(rvec)
cv.SetZero(tvec)
# since rvec, tvec are zero the initial guess is the earthmine camera
ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A,
coeff, rvec, tvec, useExtrinsicGuess=False)
np_rvec = np.matrix(rvec)
np_tvec = np.matrix(tvec)
print np_rvec
print np_tvec
return np_rvec, np_tvec
def backproject_depth(im, depth, corners):
# Use 'extrinsic rectification' to find plane equation
global obj, cam, distcc, rvec, tvec, bp
obj = np.mgrid[:6, :8, :1].astype('f').reshape(3, -1) * 0.0254
f = 583.0
cx = 321
cy = 249
distcc = np.zeros((4, 1), 'f')
rvec = np.zeros((3, 1), 'f')
tvec = np.zeros((3, 1), 'f')
cam = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]])
cv.FindExtrinsicCameraParams2(obj, corners, cam, distcc, rvec, tvec)
# Back project to see how it went
bp = np.array(corners)
cv.ProjectPoints2(obj, rvec, tvec, cam, distcc, bp)
global pts1
# Show the points in a point cloud, using rvec and tvec
RT = np.eye(4).astype('f')
RT[:3, :3] = expmap.axis2rot(rvec.squeeze())
RT[:3, 3] = tvec.squeeze()
#RT = np.linalg.inv(RT)
pts1 = np.dot(RT[:3, :3], obj) + RT[:3, 3].reshape(3, 1)
pts1[1, :] *= -1
pts1[2, :] *= -1
rgb1 = np.zeros((pts1.shape[1], 4), 'f')
rgb1[:, :] = [0, 0, 0, 1]
# Also show the points in the region, using the calib image
global mask, pts2
bounds = corners[[0, 7, 5 * 8 + 7, 5 * 8], :]
polys = (tuple([tuple(x) for x in tuple(bounds)]), )
mask = np.zeros((480, 640), 'f')
cv.FillPoly(mask, polys, [1, 0, 0])
mask = (mask > 0) & (depth < 2047)
v, u = np.mgrid[:480, :640].astype('f')
pts2 = np.vstack(project(depth[mask].astype('f'), u[mask], v[mask]))
rgb2 = np.zeros((pts2.shape[1], 4), 'f')
rgb2[:, :] = [0, 1, 0, 1]
if np.any(np.isnan(pts2.mean(1))): return
window.update_points(
np.hstack((pts1, pts2)).transpose(), np.vstack((rgb1, rgb2)))
window.lookat = pts1.mean(1)
window.Refresh()
return
def handle_mono(self, qq):
(imgmsg, caminfo) = qq
img = CvBridge().imgmsg_to_cv(imgmsg, "mono8")
pcm = image_geometry.PinholeCameraModel()
pcm.fromCameraInfo(caminfo)
self.track(img)
for (code, corners) in self.tracking.items():
# detector numbers vertices clockwise like this:
# 1 2
# 0 3
if isinstance(self.dim, (float, int)):
d = self.dim
else:
d = self.dim[code]
# User specifies black bar length, but detector uses
# full module, including the 2-unit quiet zone,
# so scale by 14/10
d = d * 14 / 10
model = cv.fromarray(numpy.array([[0,0,0], [0, d, 0], [d, d, 0], [d, 0, 0]], numpy.float32))
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(model,
cv.fromarray(numpy.array(corners, numpy.float32)),
pcm.intrinsicMatrix(),
pcm.distortionCoeffs(),
rot,
trans)
ts = geometry_msgs.msg.TransformStamped()
ts.header.frame_id = imgmsg.header.frame_id
ts.header.stamp = imgmsg.header.stamp
ts.child_frame_id = code
posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
ts.transform.translation = posemsg.position
ts.transform.rotation = posemsg.orientation
tfm = tf.msg.tfMessage([ts])
self.pub_tf.publish(tfm)
def fit(self, data):
m = mean(data, 0)
diff = data - tile(m, (data.shape[0], 1))
area0 = abs(linalg.det(dot(transpose(diff[:, 0:2]), diff[:, 0:2])))
if area0 < 0.00001: # check if point area is large enough
#print 'degenerate 2d data %f'%area0
return None
# have to compute if the 3d points are collinear or not
eigvalues = linalg.eigvalsh(dot(transpose(diff[:, 2:5]), diff[:, 2:5]))
if sum(abs(eigvalues) <= 1e-9
) >= 2: # check if point area is large enough
#print 'degenerate 3d points',eigvalues
return None
if data.shape[0] == self.ninitial:
object_points = self.object_points
image_points = self.image_points
else:
object_points = cv.CreateMatHeader(3, data.shape[0], cv.CV_32FC1)
image_points = cv.CreateMatHeader(2, data.shape[0], cv.CV_32FC1)
cv.SetData(
object_points,
struct.pack('f' * (data.shape[0] * 3),
*transpose(data[:, 2:5]).flat), 4 * data.shape[0])
cv.SetData(
image_points,
struct.pack('f' * (data.shape[0] * 2),
*transpose(data[:, 0:2]).flat), 4 * data.shape[0])
cv.FindExtrinsicCameraParams2(object_points, image_points, self.cvKK,
self.cvDist, self.rvec, self.tvec)
#cv.FindExtrinsicCameraParams2(cv.fromarray(data[:,2:5]),cv.fromarray(data[:,0:2]),self.cvKK,self.cvDist,self.rvec,self.tvec)
T = matrixFromAxisAngle(
(self.rvec[0, 0], self.rvec[1, 0], self.rvec[2, 0]))
T[0:3, 3] = [self.tvec[0, 0], self.tvec[1, 0], self.tvec[2, 0]]
# make sure that texture faces towards the image (ie, z axis has negative z component)
if T[2, 2] < 0:
return None
return T
class CrCalibration:
def __init__(self, size, dim):
self.chess_size = size
self.dim = dim
left_ns = rospy.remap_name('left')
range_ns = rospy.remap_name('range')
limg_sub = message_filters.Subscriber(left_ns + '/image_rect',
sensor_msgs.msg.Image)
rimg_sub = message_filters.Subscriber(range_ns + '/image_rect',
sensor_msgs.msg.Image)
linfo_sub = message_filters.Subscriber(left_ns + '/camera_info',
sensor_msgs.msg.CameraInfo)
rinfo_sub = message_filters.Subscriber(range_ns + '/camera_info',
sensor_msgs.msg.CameraInfo)
ts = message_filters.TimeSynchronizer(
[limg_sub, linfo_sub, rimg_sub, rinfo_sub], 16)
ts.registerCallback(self.queue_cr)
self.bridge = cv_bridge.CvBridge()
self.frame_list = []
self.static_pose = None
self.result_list = []
self.last_err = 0
def calc_frame(self, lst):
pos = PyKDL.Vector()
ex = 0
ey = 0
ez = 0
for f in lst:
pos += f.p
z, y, x = f.M.GetEulerZYX()
ex += x
ey += y
ez += z
size = len(lst)
return PyKDL.Frame(
PyKDL.Rotation.EulerZYX(ez / size, ey / size, ex / size),
pos / size)
def calc_distance(self, f1, f2):
test = f1 * f2.Inverse()
angle, axis = test.M.GetRotAngle()
norm = test.p.Norm()
return (norm + (angle / 10.0))
def queue_cr(self, limg, linfo, rimg, rinfo):
#
lpose = self.find_checkerboard_pose(limg, linfo)
rpose = self.find_checkerboard_pose(rimg, rinfo)
if lpose == None or rpose == None:
if lpose == None:
rospy.loginfo("can't find CB on left camera image")
if rpose == None:
rospy.loginfo("can't find CB on range image")
return False
# calcurate pose
lframe = pm.fromMsg(lpose.pose)
rframe = pm.fromMsg(rpose.pose)
frame = lframe * rframe.Inverse()
# check pose euniqness
for r in self.result_list:
if self.calc_distance(r[0], lframe) < 0.075:
return False
# check pose_movement
if len(self.frame_list) == 0:
self.static_pose = lframe
self.frame_list.append(frame)
return False
dist = self.calc_distance(self.static_pose, lframe)
print dist
if dist > 0.012:
self.frame_list = []
self.static_pose = None
return False
self.frame_list.append(frame)
if len(self.frame_list) > 5:
self.result_list.append(
[self.static_pose,
self.calc_frame(self.frame_list)])
self.frame_list = []
self.static_pose = None
# check resut list num
ret = self.calc_frame([r[1] for r in self.result_list])
err = 0.0
for r in self.result_list:
err += self.calc_distance(r[1], ret)
qx, qy, qz, qw = ret.M.GetQuaternion()
rospy.loginfo("%f %f %f %f %f %f %f / err = %f" %
(ret.p.x(), ret.p.y(), ret.p.z(), qx, qy, qz, qw,
err / len(self.result_list)))
self.last_err = err / len(self.result_list)
# finish check
if len(self.result_list) > 7 and self.last_err < 0.1:
rospy.loginfo("Finished size = %d, err = %f" %
(len(self.result_list), self.last_err))
print "translation: [%f, %f, %f]\nrotation: [%f, %f, %f, %f]" % (
ret.p.x(), ret.p.y(), ret.p.z(), qx, qy, qz, qw)
print "(make-coords :pos #f(%f %f %f) :rot (quaternion2matrix #f(%f %f %f %f)))" % (
1000 * ret.p.x(), 1000 * ret.p.y(), 1000 * ret.p.z(), qw,
qx, qy, qz)
#print "<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"cam_link_broadcaster\" args=\"%f %f %f %f %f %f %f link1 link2 30\" />" % (ret.p.x(), ret.p.y(), ret.p.z(), qw, qx, qy, qz)
exit(-1)
return True
return True
def detect(self, image):
corners_x = self.chess_size[0]
corners_y = self.chess_size[1]
#Here, we'll actually call the openCV detector
found, corners = cv.FindChessboardCorners(
image, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH)
if found:
board_corners = (corners[0], corners[corners_x - 1],
corners[(corners_y - 1) * corners_x],
corners[len(corners) - 1])
#find the perimeter of the checkerboard
perimeter = 0.0
for i in range(len(board_corners)):
next = (i + 1) % 4
xdiff = board_corners[i][0] - board_corners[next][0]
ydiff = board_corners[i][1] - board_corners[next][1]
perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff)
#estimate the square size in pixels
square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2)
radius = int(square_size * 0.5 + 0.5)
corners = cv.FindCornerSubPix(
image, corners, (radius, radius), (-1, -1),
(cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1))
#uncomment to debug chessboard detection
#cv.DrawChessboardCorners(image, (corners_x, corners_y), corners, 1)
#cv.NamedWindow("image")
#cv.ShowImage("image", image)
#cv.WaitKey(600)
#we'll also generate the object points if the user has specified spacing
object_points = cv.CreateMat(3, corners_x * corners_y, cv.CV_32FC1)
for y in range(corners_y):
for x in range(corners_x):
cv.SetReal2D(object_points, 0, y * corners_x + x,
x * self.dim)
cv.SetReal2D(object_points, 1, y * corners_x + x,
y * self.dim)
cv.SetReal2D(object_points, 2, y * corners_x + x, 0.0)
#not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert
corners_cv = cv.CreateMat(2, corners_x * corners_y, cv.CV_32FC1)
for i in range(corners_x * corners_y):
cv.SetReal2D(corners_cv, 0, i, corners[i][0])
cv.SetReal2D(corners_cv, 1, i, corners[i][1])
return (corners_cv, object_points)
else:
#cv.NamedWindow("image_scaled")
#cv.ShowImage("image_scaled", image_scaled)
#cv.WaitKey(600)
rospy.logwarn("Didn't find checkerboard")
return (None, None)
return
def intrinsic_matrix_from_info(self, cam_info):
intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
#Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix
for i in range(0, 3):
for j in range(0, 3):
intrinsic_matrix[i, j] = cam_info.P[4 * i + j]
return intrinsic_matrix
def find_checkerboard_pose(self, ros_image, cam_info):
#we need to convert the ros image to an opencv image
try:
image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
except CvBridgeError, e:
rospy.logerror("Error importing image %s" % e)
return
corners, model = self.detect(image)
if corners == None or model == None:
return None
else:
#find the pose of the checkerboard
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
kc = cv.CreateMat(1, 4, cv.CV_32FC1)
cv.Set(kc, 0.0)
intrinsic_matrix = self.intrinsic_matrix_from_info(cam_info)
cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc,
rot, trans)
#We want to build a transform now, but first we have to convert the
#rotation vector we get back from OpenCV into a rotation matrix
rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot_mat)
#Now we need to convert this rotation matrix into a quaternion
#This can be done better in never versions of opencv, but we need to be boxturtle compatible
numpy_mat = numpy.fromstring(rot_mat.tostring(),
dtype=numpy.float32).reshape((3, 3))
#of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here
full_pose = numpy.zeros((4, 4))
#first, copy in the rotation matrix
full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3]
#next, we'll copy in the translation
full_pose[0:3, 3] = [trans[i, 0] for i in range(3)]
#and make sure to add a 1 in the lower right corner
full_pose[3][3] = 1.0
rospy.logdebug("%s" % numpy_mat)
rospy.logdebug("%s" % full_pose)
tf_trans = tf.transformations.translation_from_matrix(full_pose)
tf_rot = tf.transformations.quaternion_from_matrix(full_pose)
#and now we'll actually build our pose stamped
board_pose = geometry_msgs.msg.PoseStamped()
board_pose.header = ros_image.header
board_pose.pose.position.x = tf_trans[0]
board_pose.pose.position.y = tf_trans[1]
board_pose.pose.position.z = tf_trans[2]
board_pose.pose.orientation.x = tf_rot[0]
board_pose.pose.orientation.y = tf_rot[1]
board_pose.pose.orientation.z = tf_rot[2]
board_pose.pose.orientation.w = tf_rot[3]
rospy.logdebug("%s" % board_pose)
#we'll publish the pose so we can display it in rviz
# self.pose_pub.publish(board_pose)
return board_pose
class ImageCbDetectorNode:
def __init__(self):
self.corners_x = rospy.get_param('~corners_x', 6)
self.corners_y = rospy.get_param('~corners_y', 6)
self.spacing_x = rospy.get_param('~spacing_x', None)
self.spacing_y = rospy.get_param('~spacing_y', None)
self.width_scaling = rospy.get_param('~width_scaling', 1)
self.height_scaling = rospy.get_param('~height_scaling', 1)
self.im_cb_detector = ImageCbDetector()
self.image_sub = rospy.Subscriber("image_stream", Image, self.callback)
self.caminfo_sub = rospy.Subscriber("camera_info", CameraInfo,
self.cam_info_cb)
self.pose_pub = rospy.Publisher("board_pose", PoseStamped, latch=True)
self.pose_srv = None
self.bridge = CvBridge()
self.mutex = threading.RLock()
self.cam_info = None
self.ros_image = None
def intrinsic_matrix_from_info(self, cam_info):
intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
#Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix
for i in range(0, 3):
for j in range(0, 3):
intrinsic_matrix[i, j] = cam_info.P[4 * i + j]
return intrinsic_matrix
def attempt_to_advertise(self):
#we'll only advertise the service after we've received a cam_info/image pair
if self.ros_image != None and self.cam_info != None:
self.pose_srv = rospy.Service("get_checkerboard_pose",
GetCheckerboardPose,
self.find_checkerboard_service)
def cam_info_cb(self, cam_info):
with self.mutex:
self.cam_info = cam_info
if self.pose_srv == None:
self.attempt_to_advertise()
def callback(self, ros_image):
#copy over the latest image to be used when a service call is made
with self.mutex:
self.ros_image = ros_image
if self.pose_srv == None:
self.attempt_to_advertise()
def find_checkerboard_service(self, req):
poses = []
min_x = self.ros_image.width
min_y = self.ros_image.height
max_x = 0
max_y = 0
for i in range(10):
rospy.sleep(0.5)
#copy the image over
with self.mutex:
image = self.ros_image
result = self.find_checkerboard_pose(image, req.corners_x,
req.corners_y, req.spacing_x,
req.spacing_y,
self.width_scaling,
self.height_scaling)
if result is None:
rospy.logerr("Couldn't find a checkerboard")
continue
p, corners = result
poses.append(p)
for j in range(corners.cols):
x = cv.GetReal2D(corners, 0, j)
y = cv.GetReal2D(corners, 1, j)
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
# convert all poses to twists
twists = []
for p in poses:
twists.append(
PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose)))
# get average twist
twist_res = PyKDL.Twist()
PyKDL.SetToZero(twist_res)
for t in twists:
for i in range(3):
twist_res.vel[i] += t.vel[i] / len(poses)
twist_res.rot[i] += t.rot[i] / len(poses)
# get noise
noise_rot = 0
noise_vel = 0
for t in twists:
n_vel = (t.vel - twist_res.vel).Norm()
n_rot = (t.rot - twist_res.rot).Norm()
if n_vel > noise_vel:
noise_vel = n_vel
if n_rot > noise_rot:
noise_rot = n_rot
# set service resul
pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0)
pose_msg = PoseStamped()
pose_msg.header = poses[0].header
pose_msg.pose = posemath.toMsg(pose_res)
return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y,
max_y, noise_vel, noise_rot)
def find_checkerboard_pose(self, ros_image, corners_x, corners_y,
spacing_x, spacing_y, width_scaling,
height_scaling):
#we need to convert the ros image to an opencv image
try:
image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
except CvBridgeError, e:
rospy.logerror("Error importing image %s" % e)
return
corners, model = self.im_cb_detector.detect(image, corners_x,
corners_y, spacing_x,
spacing_y, width_scaling,
height_scaling)
if corners != None and model != None and self.cam_info != None:
#find the pose of the checkerboard
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
with self.mutex:
intrinsic_matrix = self.intrinsic_matrix_from_info(
self.cam_info)
kc = cv.CreateMat(1, 4, cv.CV_32FC1)
cv.Set(kc, 0.0)
cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix,
kc, rot, trans)
#We want to build a transform now, but first we have to convert the
#rotation vector we get back from OpenCV into a rotation matrix
rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot_mat)
#Now we need to convert this rotation matrix into a quaternion
#This can be done better in never versions of opencv, but we need to be boxturtle compatible
numpy_mat = numpy.fromstring(rot_mat.tostring(),
dtype=numpy.float32).reshape((3, 3))
#of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here
full_pose = numpy.zeros((4, 4))
#first, copy in the rotation matrix
full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3]
#next, we'll copy in the translation
full_pose[0:3, 3] = [trans[i, 0] for i in range(3)]
#and make sure to add a 1 in the lower right corner
full_pose[3][3] = 1.0
rospy.logdebug("%s" % numpy_mat)
rospy.logdebug("%s" % full_pose)
tf_trans = tf.transformations.translation_from_matrix(full_pose)
tf_rot = tf.transformations.quaternion_from_matrix(full_pose)
#and now we'll actually build our pose stamped
board_pose = PoseStamped()
board_pose.header = ros_image.header
board_pose.pose.position.x = tf_trans[0]
board_pose.pose.position.y = tf_trans[1]
board_pose.pose.position.z = tf_trans[2]
board_pose.pose.orientation.x = tf_rot[0]
board_pose.pose.orientation.y = tf_rot[1]
board_pose.pose.orientation.z = tf_rot[2]
board_pose.pose.orientation.w = tf_rot[3]
rospy.logdebug("%s" % board_pose)
#we'll publish the pose so we can display it in rviz
self.pose_pub.publish(board_pose)
return (board_pose, corners)
def calibrate(self):
self.projector_to_camera_translation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
self.projector_to_camera_rotation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
cv.SetZero(self.projector_to_camera_translation_vector)
cv.SetZero(self.projector_to_camera_rotation_vector)
for i in range(self.number_of_scenes):
camera_tvec = cv.CreateMat(3, 1, cv.CV_32FC1)
camera_rvec = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(self.object_points[i],
self.camera_image_points[i],
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs(),
camera_rvec, camera_tvec)
print "Camera To Board Vector:"
for row in range(camera_tvec.height):
for col in range(camera_tvec.width):
print camera_tvec[row, col],
print
print
projector_tvec = cv.CreateMat(3, 1, cv.CV_32FC1)
projector_rvec = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(
self.object_points[i], self.projector_image_points[i],
self.projector_model.intrinsicMatrix(),
self.projector_model.distortionCoeffs(), projector_rvec,
projector_tvec)
print "Projector To Board Vector:"
for row in range(projector_tvec.height):
for col in range(projector_tvec.width):
print projector_tvec[row, col],
print
print
camera_rmat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(camera_rvec, camera_rmat)
projector_rmat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(projector_rvec, projector_rmat)
scene_projector_to_camera_rotation_matrix = cv.CreateMat(
3, 3, cv.CV_32FC1)
cv.GEMM(camera_rmat, projector_rmat, 1, None, 0,
scene_projector_to_camera_rotation_matrix, cv.CV_GEMM_B_T)
scene_projector_to_camera_rotation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
for i in range(3):
for j in range(3):
print scene_projector_to_camera_rotation_matrix[i, j],
print
print
cv.Rodrigues2(scene_projector_to_camera_rotation_matrix,
scene_projector_to_camera_rotation_vector)
print "Scene Rotation Vector:"
for row in range(scene_projector_to_camera_rotation_vector.height):
for col in range(
scene_projector_to_camera_rotation_vector.width):
print scene_projector_to_camera_rotation_vector[row, col],
print
print
scene_projector_to_camera_translation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
cv.GEMM(projector_rmat, projector_tvec, -1, None, 0,
scene_projector_to_camera_translation_vector,
cv.CV_GEMM_A_T)
cv.GEMM(camera_rmat, scene_projector_to_camera_translation_vector,
1, camera_tvec, 1,
scene_projector_to_camera_translation_vector, 0)
print "Scene Translation Vector:"
for row in range(
scene_projector_to_camera_translation_vector.height):
for col in range(
scene_projector_to_camera_translation_vector.width):
print scene_projector_to_camera_translation_vector[row,
col],
print
print
cv.Add(scene_projector_to_camera_translation_vector,
self.projector_to_camera_translation_vector,
self.projector_to_camera_translation_vector)
cv.Add(scene_projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_vector)
cv.ConvertScale(self.projector_to_camera_translation_vector,
self.projector_to_camera_translation_vector,
scale=1.0 / self.number_of_scenes)
cv.ConvertScale(self.projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_vector,
scale=1.0 / self.number_of_scenes)
print "Final Translation Vector:"
for row in range(self.projector_to_camera_translation_vector.height):
for col in range(
self.projector_to_camera_translation_vector.width):
print self.projector_to_camera_translation_vector[row, col],
print
print
print "Final Rotation Vector:"
for row in range(self.projector_to_camera_rotation_vector.height):
for col in range(self.projector_to_camera_rotation_vector.width):
print self.projector_to_camera_rotation_vector[row, col],
print
def qsolve(C, Q, matches, dbsiftpath, dbimgpath):
# Get image information.
info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info')
qsource = render_tags.QueryImageInfo(Q.datasource)
dbsource = render_tags.EarthmineImageInfo(dbimgpath, info)
map3d = C.pixelmap.open(dbsiftpath)
# Get 2d pixels and 3d locations of feature inliers
matches = [(m['query'][0],m['query'][1],m['db'][0],m['db'][1]) for m in matches]
matches = list(set(matches))
q2d = [arr([m[0],m[1]]) for m in matches]
db2d = [arr([m[2],m[3]]) for m in matches]
db3d = [map3d[int(d[0]),int(d[1])] for d in db2d]
i = 0
while i < len(db3d):
if db3d[i] is None:
q2d.pop(i)
db2d.pop(i)
db3d.pop(i)
else:
i = i+1
olat,olon,oalt = dbsource.lat,dbsource.lon,dbsource.alt
qlat,qlon = qsource.lat,qsource.lon
qzx = geom.lltom(olat,olon,qlat,qlon)
zx = [geom.lltom(olat,olon,d['lat'],d['lon']) for d in db3d]
y = [dbsource.alt-d['alt'] for d in db3d]
xyz = [[zx[i][1],y[i],zx[i][0]] for i in range(len(y))]
print len(xyz)
# Set K, Rhat
wx,wy = qsource.pgmsize[0], qsource.pgmsize[1]
tx,ty = qsource.view_angle[0], qsource.view_angle[1]
f1, f2 = (wx-1)/(2*np.tan(tx/2)), (wy-1)/(2*np.tan(ty/2))
f = (f1+f2) / 2
K = arr([[f,0,(wx-1)/2.0],
[0,f,(wy-1)/2.0],
[0,0,1]])
y,p,r = qsource.yaw, qsource.pitch, qsource.roll
print [180*y/np.pi,180*p/np.pi,180*r/np.pi]
Ry = arr([[np.cos(y),0,np.sin(y)],
[0,1,0],
[-np.sin(y),0,np.cos(y)]])
Rx = arr([[1,0,0],
[0,np.cos(p),-np.sin(p)],
[0,np.sin(p),np.cos(p)]])
Rz = arr([[np.cos(r),-np.sin(r),0],
[np.sin(r),np.cos(r),0],
[0,0,1]])
Rhat = dot(Ry,dot(Rx,Rz)) # camera orientation (camera to world)
tRhat = tp(Rhat)
# reference camera matrix
# f 0 cx
# 0 f cy
# 0 0 1
A = cv.CreateMat(3, 3, cv.CV_64F)
cv.SetZero(A)
cv.Set2D(A, 0, 0, cv.Scalar(f))
cv.Set2D(A, 1, 1, cv.Scalar(f))
cv.Set2D(A, 2, 2, cv.Scalar(1))
cv.Set2D(A, 0, 2, cv.Scalar((wx-1)/2.0))
cv.Set2D(A, 1, 2, cv.Scalar((wy-1)/2.0))
# convert 2d, 3d points to cvMats
objectPoints3d = cv.CreateMat(len(xyz), 1, cv.CV_64FC3)
imagePoints2d = cv.CreateMat(len(xyz), 1, cv.CV_64FC2)
for i in range(len(xyz)):
cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*q2d[i]))
cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*xyz[i]))
# set initial rotation and translation vectors, distortion coefficients
coeff = cv.CreateMat(4, 1, cv.CV_64F)
cv.SetZero(coeff)
rmat = cv.CreateMat(3, 3, cv.CV_64F)
cv.Set2D(rmat, 0, 0, cv.Scalar(tRhat[0,0]))
cv.Set2D(rmat, 0, 1, cv.Scalar(tRhat[0,1]))
cv.Set2D(rmat, 0, 2, cv.Scalar(tRhat[0,2]))
cv.Set2D(rmat, 1, 0, cv.Scalar(tRhat[1,0]))
cv.Set2D(rmat, 1, 1, cv.Scalar(tRhat[1,1]))
cv.Set2D(rmat, 1, 2, cv.Scalar(tRhat[1,2]))
cv.Set2D(rmat, 2, 0, cv.Scalar(tRhat[2,0]))
cv.Set2D(rmat, 2, 1, cv.Scalar(tRhat[2,1]))
cv.Set2D(rmat, 2, 2, cv.Scalar(tRhat[2,2]))
print 'YPR init for PnP'
print 180*np.array(geom.ypr_fromR(Rhat))/np.pi
rvec = cv.CreateMat(3, 1, cv.CV_64F)
cv.SetZero(rvec)
cv.Rodrigues2(rmat,rvec) # convert from rotation matrix to Rodrigues vector
tvec = cv.CreateMat(3, 1, cv.CV_64F)
cv.SetZero(tvec)
# solvepnp
ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A,
coeff, rvec, tvec, useExtrinsicGuess=False)
np_rvec = np.matrix(rvec).A
cv.Rodrigues2(rvec,rmat)
np_rmat = np.transpose(np.matrix(rmat)).A
np_tvec = np.dot(np_rmat,np.squeeze(np.matrix(tvec).A))
print 'Rq from PnP'
print np_rmat
print 'YPR from PnP'
print 180*np.array(geom.ypr_fromR(np_rmat))/np.pi
return np_rmat, np_tvec
def find_checkerboard_pose(self, ros_image, cam_info):
#we need to convert the ros image to an opencv image
try:
image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
except CvBridgeError as e:
rospy.logerror("Error importing image %s" % e)
return
corners, model = self.detect(image)
if corners == None or model == None:
return None
else:
#find the pose of the checkerboard
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
kc = cv.CreateMat(1, 4, cv.CV_32FC1)
cv.Set(kc, 0.0)
intrinsic_matrix = self.intrinsic_matrix_from_info(cam_info)
cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc,
rot, trans)
#We want to build a transform now, but first we have to convert the
#rotation vector we get back from OpenCV into a rotation matrix
rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot_mat)
#Now we need to convert this rotation matrix into a quaternion
#This can be done better in never versions of opencv, but we need to be boxturtle compatible
numpy_mat = numpy.fromstring(rot_mat.tostring(),
dtype=numpy.float32).reshape((3, 3))
#of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here
full_pose = numpy.zeros((4, 4))
#first, copy in the rotation matrix
full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3]
#next, we'll copy in the translation
full_pose[0:3, 3] = [trans[i, 0] for i in range(3)]
#and make sure to add a 1 in the lower right corner
full_pose[3][3] = 1.0
rospy.logdebug("%s" % numpy_mat)
rospy.logdebug("%s" % full_pose)
tf_trans = tf.transformations.translation_from_matrix(full_pose)
tf_rot = tf.transformations.quaternion_from_matrix(full_pose)
#and now we'll actually build our pose stamped
board_pose = geometry_msgs.msg.PoseStamped()
board_pose.header = ros_image.header
board_pose.pose.position.x = tf_trans[0]
board_pose.pose.position.y = tf_trans[1]
board_pose.pose.position.z = tf_trans[2]
board_pose.pose.orientation.x = tf_rot[0]
board_pose.pose.orientation.y = tf_rot[1]
board_pose.pose.orientation.z = tf_rot[2]
board_pose.pose.orientation.w = tf_rot[3]
rospy.logdebug("%s" % board_pose)
#we'll publish the pose so we can display it in rviz
# self.pose_pub.publish(board_pose)
return board_pose
def dbsolve(C, Rhat, matches, dbsiftpath, dbimgpath):
# Get image information.
info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info')
dbsource = render_tags.EarthmineImageInfo(dbimgpath, info)
map3d = C.pixelmap.open(dbsiftpath)
# Get 2d pixels and 3d locations of feature inliers
matches = [(m['query'][0],m['query'][1],m['db'][0],m['db'][1]) for m in matches]
matches = list(set(matches))
db2d = [arr([m[2],m[3]]) for m in matches]
db3d = [map3d[int(d[0]),int(d[1])] for d in db2d]
i = 0
while i < len(db3d):
if db3d[i] is None:
db2d.pop(i)
db3d.pop(i)
else:
i = i+1
olat,olon,oalt = dbsource.lat,dbsource.lon,dbsource.alt
zx = [geom.lltom(olat,olon,d['lat'],d['lon']) for d in db3d]
y = [dbsource.alt-d['alt'] for d in db3d]
xyz = [[zx[i][1],y[i],zx[i][0]] for i in range(len(y))]
# reference camera matrix Kd
wx,wy = dbsource.image.size
fov = dbsource.fov
f = (wx-1)/(2*np.tan(fov/2))
# f 0 cx
# 0 f cy
# 0 0 1
A = cv.CreateMat(3, 3, cv.CV_64F)
cv.SetZero(A)
cv.Set2D(A, 0, 0, cv.Scalar(f))
cv.Set2D(A, 1, 1, cv.Scalar(f))
cv.Set2D(A, 2, 2, cv.Scalar(1))
cv.Set2D(A, 0, 2, cv.Scalar((wx-1)/2.0))
cv.Set2D(A, 1, 2, cv.Scalar((wy-1)/2.0))
# convert 2d, 3d points to cvMats
objectPoints3d = cv.CreateMat(len(xyz), 1, cv.CV_64FC3)
imagePoints2d = cv.CreateMat(len(xyz), 1, cv.CV_64FC2)
for i in range(len(xyz)):
cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*db2d[i]))
cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*xyz[i]))
# set initial rotation and translation vectors, distortion coefficients
coeff = cv.CreateMat(4, 1, cv.CV_64F)
cv.SetZero(coeff)
rmat = cv.CreateMat(3, 3, cv.CV_64F)
tRhat = tp(Rhat)
cv.Set2D(rmat, 0, 0, cv.Scalar(tRhat[0,0]))
cv.Set2D(rmat, 0, 1, cv.Scalar(tRhat[0,1]))
cv.Set2D(rmat, 0, 2, cv.Scalar(tRhat[0,2]))
cv.Set2D(rmat, 1, 0, cv.Scalar(tRhat[1,0]))
cv.Set2D(rmat, 1, 1, cv.Scalar(tRhat[1,1]))
cv.Set2D(rmat, 1, 2, cv.Scalar(tRhat[1,2]))
cv.Set2D(rmat, 2, 0, cv.Scalar(tRhat[2,0]))
cv.Set2D(rmat, 2, 1, cv.Scalar(tRhat[2,1]))
cv.Set2D(rmat, 2, 2, cv.Scalar(tRhat[2,2]))
rvec = cv.CreateMat(3, 1, cv.CV_64F)
cv.SetZero(rvec)
cv.Rodrigues2(rmat,rvec) # convert from rotation matrix to Rodrigues vector
tvec = cv.CreateMat(3, 1, cv.CV_64F)
cv.SetZero(tvec)
#print Rhat
# solvepnp
ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A,
coeff, rvec, tvec, useExtrinsicGuess=False)
np_rvec = np.matrix(rvec).A
cv.Rodrigues2(rvec,rmat)
np_rmat = np.transpose(np.matrix(rmat)).A
np_tvec = np.dot(np_rmat,np.squeeze(np.matrix(tvec).A))
return np_rmat, np_tvec
def calibrateFromObservations(self,
observations,
calibextrinsic=True,
pruneoutliers=True,
full_output=True,
fixprincipalpoint=False,
Tcameraestimate=None):
while True:
if len(observations) < 3:
raise self.CalibrationError('very little observations: %d!' %
len(observations),
action=1)
object_points = self.getObjectPoints()
all_object_points = []
all_corners = []
imagesize = None
for o in observations:
all_object_points.append(object_points)
all_corners.append(o['corners_raw'])
if imagesize:
assert imagesize[0] == o['image_raw'].shape[
1] and imagesize[1] == o['image_raw'].shape[0]
else:
imagesize = (o['image_raw'].shape[1],
o['image_raw'].shape[0])
intrinsiccondition = self.validateCalibrationData(
all_object_points, all_corners)
if intrinsiccondition is None:
raise self.CalibrationError(
'bad condition number, %d observations' %
(len(observations)),
action=1)
KKorig, kcorig, Traws, error_intrinsic, error_grad = self.calibrateIntrinsicCamera(
all_object_points,
all_corners,
imagesize,
fixprincipalpoint=fixprincipalpoint,
computegradients=True)
cvKKorig = cv.fromarray(KKorig)
cvkcorig = cv.fromarray(kcorig)
thresh = median(error_intrinsic) + 0.5
if any(error_intrinsic > thresh):
# compute again using a pruned set of observations
print 'pruning observations (thresh=%f) because intrinsic error is: ' % thresh, error_intrinsic
observations = [
o for i, o in enumerate(observations)
if error_intrinsic[i] <= thresh
]
else:
if mean(error_intrinsic) > 0.6:
raise self.CalibrationError(
'intrinsic error is huge (%s)! giving up, KK=(%s)' %
(str(error_intrinsic), str(KKorig)))
break
if not calibextrinsic:
return KKorig, kcorig, None, {
'error_intrinsic': error_intrinsic,
'intrinsiccondition': intrinsiccondition,
'error_grad': error_grad
}
if Tcameraestimate is None:
raise TypeError(
'Tcameraestimate needs to be initialized to a 4x4 matrix')
# unwarp all images and re-detect the checkerboard patterns
cvKK = cv.CreateMat(3, 3, cv.CV_64F)
cv.GetOptimalNewCameraMatrix(cvKKorig, cvkcorig, imagesize, 0, cvKK)
cvUndistortionMapX = cv.CreateMat(imagesize[1], imagesize[0],
cv.CV_32F)
cvUndistortionMapY = cv.CreateMat(imagesize[1], imagesize[0],
cv.CV_32F)
cv.InitUndistortRectifyMap(cvKKorig, cvkcorig, cv.fromarray(eye(3)),
cvKK, cvUndistortionMapX,
cvUndistortionMapY)
KK = array(cvKK)
KKinv = linalg.inv(KK)
if full_output:
print 'KKorig: ', KKorig, ' KK:', KK
cvimage = None
cvkczero = cv.fromarray(zeros(kcorig.shape))
cvrvec = cv.CreateMat(1, 3, cv.CV_64F)
cvtvec = cv.CreateMat(1, 3, cv.CV_64F)
all_optimization_data = []
Tworldpatternestimates = []
for i, o in enumerate(observations):
cvimage_dist = cv.fromarray(o['image_raw'])
if not cvimage:
cvimage = cv.CloneMat(cvimage_dist)
cv.Remap(cvimage_dist, cvimage, cvUndistortionMapX,
cvUndistortionMapY,
cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS)
corners = self.detect(cvimage)
if corners is not None:
cv.FindExtrinsicCameraParams2(cv.fromarray(object_points),
cv.fromarray(corners), cvKK,
cvkczero, cvrvec, cvtvec)
T = matrixFromAxisAngle(array(cvrvec)[0])
T[0:3, 3] = array(cvtvec)[0]
Tworldpatternestimates.append(
dot(dot(o['Tlink'], Tcameraestimate), T))
all_optimization_data.append(
(transformPoints(KKinv, corners), linalg.inv(o['Tlink'])))
else:
print 'could not detect original pattern ', i
# have the following equation: Tlink * Tcamera * Tdetectedpattern * corners3d = Tworldpattern * corners3d
# need to solve for Tcamera and Tworldpattern
# instead of using Tdetectedpattern, use projected difference:
# corners - proj( inv(Tcamera) * inv(Tlink) * Tworldpattern *corners3d)
corners3d = self.getObjectPoints()
quatWorldPatternEstimates = array([
quatFromRotationMatrix(T[0:3, 0:3]) for T in Tworldpatternestimates
])
quatWorldPatternInitial, success = leastsq(
lambda q: quatArrayTDist(q / sqrt(sum(q**2)),
quatWorldPatternEstimates),
quatWorldPatternEstimates[0],
maxfev=100000,
epsfcn=1e-6)
rWorldPatternInitial = zeros(6, float64)
rWorldPatternInitial[0:3] = axisAngleFromQuat(quatWorldPatternInitial)
rWorldPatternInitial[3:6] = mean(
array([T[0:3, 3] for T in Tworldpatternestimates]), 0)
Tcameraestimateinv = linalg.inv(Tcameraestimate)
rCameraInitial = zeros(6, float64)
rCameraInitial[0:3] = axisAngleFromRotationMatrix(
Tcameraestimateinv[0:3, 0:3])
rCameraInitial[3:6] = Tcameraestimateinv[0:3, 3]
def errorfn(x, optimization_data):
Tworldpattern = matrixFromAxisAngle(x[0:3])
Tworldpattern[0:3, 3] = x[3:6]
Tcamerainv = matrixFromAxisAngle(x[6:9])
Tcamerainv[0:3, 3] = x[9:12]
err = zeros(len(optimization_data) * len(corners3d) * 2)
off = 0
for measuredcameracorners, Tlinkinv in optimization_data:
cameracorners3d = transformPoints(
dot(dot(Tcamerainv, Tlinkinv), Tworldpattern), corners3d)
iz = 1.0 / cameracorners3d[:, 2]
err[off:(
off + len(corners3d)
)] = measuredcameracorners[:, 0] - cameracorners3d[:, 0] * iz
off += len(corners3d)
err[off:(
off + len(corners3d)
)] = measuredcameracorners[:, 1] - cameracorners3d[:, 1] * iz
off += len(corners3d)
if full_output:
print 'rms: ', sqrt(sum(err**2))
return err
optimization_data = all_optimization_data
xorig, cov_x, infodict, mesg, iter = leastsq(
lambda x: errorfn(x, all_optimization_data),
r_[rWorldPatternInitial, rCameraInitial],
maxfev=100000,
epsfcn=1e-6,
full_output=1)
if pruneoutliers:
# prune the images with the most error
errors = reshape(
errorfn(xorig, all_optimization_data)**2,
(len(all_optimization_data), len(corners3d) * 2))
errorreprojection = mean(
sqrt(KK[0, 0]**2 * errors[:, 0:len(corners3d)] +
KK[1, 1]**2 * errors[:, len(corners3d):]), 1)
#thresh=mean(errorreprojection)+std(errorreprojection)
thresh = median(
errorreprojection) + 20 #0.5*std(errorreprojection)
print 'thresh:', thresh, 'errors:', errorreprojection
optimization_data = [
all_optimization_data[i]
for i in flatnonzero(errorreprojection <= thresh)
]
x, cov_x, infodict, mesg, iter = leastsq(
lambda x: errorfn(x, optimization_data),
xorig,
maxfev=100000,
epsfcn=1e-6,
full_output=1)
else:
x = xorig
Tcamerainv = matrixFromAxisAngle(x[6:9])
Tcamerainv[0:3, 3] = x[9:12]
Tcamera = linalg.inv(Tcamerainv)
Tworldpattern = matrixFromAxisAngle(x[0:3])
Tworldpattern[0:3, 3] = x[3:6]
points3d = self.Compute3DObservationPoints(Tcamerainv,
optimization_data)
if full_output:
errors = reshape(
errorfn(x, optimization_data)**2,
(len(optimization_data), len(corners3d) * 2))
error_reprojection = sqrt(
KK[0, 0]**2 * errors[:, 0:len(corners3d)] +
KK[1, 1]**2 * errors[:, len(corners3d):]).flatten()
print 'final reprojection error (pixels): ', mean(
error_reprojection), std(error_reprojection)
error_3d = sqrt(
sum((transformPoints(Tworldpattern, corners3d) - points3d)**2,
1))
print '3d error: ', mean(error_3d), std(error_3d)
return KKorig, kcorig, Tcamera, {
'error_reprojection': error_reprojection,
'error_3d': error_3d,
'error_intrinsic': error_intrinsic,
'intrinsiccondition': intrinsiccondition,
'error_grad': error_grad,
'KK': array(cvKK)
}
return KKorig, kcorig, Tcamera, None
class ImageCbDetectorNode:
def __init__(self):
self.mutex = threading.RLock()
self.corners_x = rospy.get_param('~corners_x', 6)
self.corners_y = rospy.get_param('~corners_y', 6)
self.spacing_x = rospy.get_param('~spacing_x', 0.022)
self.spacing_y = rospy.get_param('~spacing_y', 0.022)
self.publish_period = rospy.Duration(
rospy.get_param('~publish_period', 0))
self.last_publish_time = rospy.Time()
self.width_scaling = rospy.get_param('~width_scaling', 1)
self.height_scaling = rospy.get_param('~height_scaling', 1)
self.im_cb_detector = ImageCbDetector()
self.pose_srv = None
self.bridge = CvBridge()
self.cam_info = None
self.ros_image = None
self.image_sub = rospy.Subscriber("image_stream", Image, self.callback)
self.caminfo_sub = rospy.Subscriber("camera_info", CameraInfo,
self.cam_info_cb)
self.pose_pub = rospy.Publisher("board_pose", PoseStamped)
def intrinsic_matrix_from_info(self, cam_info):
intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
#Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix
for i in range(0, 3):
for j in range(0, 3):
intrinsic_matrix[i, j] = cam_info.P[4 * i + j]
return intrinsic_matrix
def attempt_to_advertise(self):
#we'll only advertise the service after we've received a cam_info/image pair
if self.ros_image != None and self.cam_info != None:
self.pose_srv = rospy.Service("get_checkerboard_pose",
GetCheckerboardPose,
self.find_checkerboard_service)
def cam_info_cb(self, cam_info):
with self.mutex:
self.cam_info = cam_info
if self.pose_srv == None:
self.attempt_to_advertise()
def callback(self, ros_image):
#copy over the latest image to be used when a service call is made
with self.mutex:
self.ros_image = ros_image
if self.pose_srv == None:
self.attempt_to_advertise()
if rospy.Duration(
) != self.publish_period and rospy.rostime.get_rostime(
) - self.last_publish_time > self.publish_period:
pose = self.find_checkerboard_pose(
ros_image, self.corners_x, self.corners_y, self.spacing_x,
self.spacing_y, self.width_scaling, self.height_scaling)
if pose:
self.pose_pub.publish(pose)
self.last_publish_time = rospy.rostime.get_rostime()
def find_checkerboard_service(self, req):
#copy the image over
with self.mutex:
image = self.ros_image
pose = self.find_checkerboard_pose(image, req.corners_x, req.corners_y,
req.spacing_x, req.spacing_y,
self.width_scaling,
self.height_scaling)
return GetCheckerboardPoseResponse(pose)
def find_checkerboard_pose(self, ros_image, corners_x, corners_y,
spacing_x, spacing_y, width_scaling,
height_scaling):
#we need to convert the ros image to an opencv image
try:
image = self.bridge.imgmsg_to_cv(ros_image, "mono8")
except CvBridgeError, e:
rospy.logerror("Error importing image %s" % e)
return
corners, model = self.im_cb_detector.detect(image, corners_x,
corners_y, spacing_x,
spacing_y, width_scaling,
height_scaling)
if corners != None and model != None and self.cam_info != None:
#find the pose of the checkerboard
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
with self.mutex:
kc = cv.CreateMat(1, 4, cv.CV_32FC1)
cv.Set(kc, 0.0)
intrinsic_matrix = self.intrinsic_matrix_from_info(
self.cam_info)
cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix,
kc, rot, trans)
#We want to build a transform now, but first we have to convert the
#rotation vector we get back from OpenCV into a rotation matrix
rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(rot, rot_mat)
#Now we need to convert this rotation matrix into a quaternion
#This can be done better in never versions of opencv, but we need to be boxturtle compatible
numpy_mat = numpy.fromstring(rot_mat.tostring(),
dtype=numpy.float32).reshape((3, 3))
#of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here
full_pose = numpy.zeros((4, 4))
#first, copy in the rotation matrix
full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3]
#next, we'll copy in the translation
full_pose[0:3, 3] = [trans[i, 0] for i in range(3)]
#and make sure to add a 1 in the lower right corner
full_pose[3][3] = 1.0
rospy.logdebug("%s" % numpy_mat)
rospy.logdebug("%s" % full_pose)
tf_trans = tf.transformations.translation_from_matrix(full_pose)
tf_rot = tf.transformations.quaternion_from_matrix(full_pose)
#and now we'll actually build our pose stamped
board_pose = PoseStamped()
board_pose.header = ros_image.header
board_pose.pose.position.x = tf_trans[0]
board_pose.pose.position.y = tf_trans[1]
board_pose.pose.position.z = tf_trans[2]
board_pose.pose.orientation.x = tf_rot[0]
board_pose.pose.orientation.y = tf_rot[1]
board_pose.pose.orientation.z = tf_rot[2]
board_pose.pose.orientation.w = tf_rot[3]
rospy.logdebug("%s" % board_pose)
#we'll publish the pose so we can display it in rviz
self.pose_pub.publish(board_pose)
return board_pose