def get_image_polygon(obj):
minps = PointStamped()
minps.header.stamp = rospy.Time(0)
minps.header.frame_id = 'base_footprint'
minps.point.x = obj.points[0].y
minps.point.y = -obj.points[0].x
minps.point.z = obj.points[0].z
minp = self.tf.transformPoint('kinect_rgb_optical_frame', minps).point
minps.point.x = obj.points[1].y
minps.point.y = -obj.points[1].x
minps.point.z = obj.points[1].z
maxp = self.tf.transformPoint('kinect_rgb_optical_frame', minps).point
ct = np.array( [(minp.x,minp.y,minp.z), (maxp.x,minp.y,minp.z), (maxp.x,maxp.y,maxp.z), (minp.x,maxp.y,maxp.z)] )
corners = cv.fromarray(ct)
out = cv.fromarray(np.zeros((4,2)))
cam_mat = np.array( [(525, 0, 319.5), (0, 525, 239.5), (0, 0, 1)] )
cv.ProjectPoints2(corners,
cv.fromarray(np.zeros((1,3))),
cv.fromarray(np.zeros((1,3))),
cv.fromarray(cam_mat),
cv.fromarray(np.zeros((1,4))),
out)
vs = []
for l in np.asarray(out):
vs.append( Vec2(*l) )
return Polygon(vs)
def errorfn(x):
xoff = 2
cvKK[0, 0] = x[0]
cvKK[1, 1] = x[1]
if not fixprincipalpoint:
cvKK[0, 2] = x[2]
cvKK[1, 2] = x[3]
xoff += 2
for i in range(5):
cvkc[i, 0] = x[xoff + i]
xoff += 5
e = zeros(len(all_object_points) * N * 2)
off = 0
for i in range(len(all_object_points)):
for j in range(3):
cvrvecs[0, j] = x[xoff + 6 * i + j]
cvtvecs[0, j] = x[xoff + 6 * i + 3 + j]
cv.ProjectPoints2(cv_object_points[i], cvrvecs[0],
cvtvecs[0], cvKK, cvkc, cv_image_points)
image_points = array(cv_image_points)
e[off:(off + len(image_points)
)] = all_corners[i][:, 0] - image_points[:, 0]
off += len(image_points)
e[off:(off + len(image_points)
)] = all_corners[i][:, 1] - image_points[:, 1]
off += len(image_points)
#print 'rms: ',sqrt(sum(e**2))
return e
def lmk_geo_to_img_geo(self, lmk):
parent = lmk.get_top_parent().parent_landmark
z = self.scene_lmk_to_bbox[parent].points[0].z
lmk_points = lmk.representation.get_points()
ct = np.zeros((len(lmk_points), 3))
for i, p in enumerate(lmk_points):
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = 'base_footprint'
ps.point.x = p.y
ps.point.y = -p.x
ps.point.z = z
pst = self.tf.transformPoint('kinect_rgb_optical_frame', ps).point
ct[i] = (pst.x, pst.y, pst.z)
corners = cv.fromarray(ct)
out = cv.fromarray(np.zeros((len(lmk_points), 2)))
cam_mat = np.array([(525, 0, 319.5), (0, 525, 239.5), (0, 0, 1)])
cv.ProjectPoints2(corners, cv.fromarray(np.zeros((1, 3))),
cv.fromarray(np.zeros(
(1, 3))), cv.fromarray(cam_mat),
cv.fromarray(np.zeros((1, 4))), out)
# print lmk, lmk_points, np.asarray(out)
return np.asarray(out)
def pointcloud_callback(msg):
if (running):
global cloudx,cloudy,cloud
cloud = pointcloud2_to_xyz_array(msg)
cloud_mat = cv.CreateMat(len(cloud),1,cv.CV_32FC3)
projection = cv.CreateMat(len(cloud),1,cv.CV_32FC2)
cloud_mat = cloud
cv.ProjectPoints2(cv.fromarray(cloud_mat),rotation_vector,translation_vector,intrinsic_mat,distortion_coeffs,projection)
(cloudx,cloudy) = cv2.split(np.asarray(projection))
def check_projection(cam_opts, distorted=True):
cam = _build_test_camera(**cam_opts)
R = cam.get_rect()
if not np.allclose(R, np.eye(3)):
# opencv's ProjectPoints2 does not take into account
# rectifciation matrix, thus we skip this test.
from nose.plugins.skip import SkipTest
raise SkipTest("Test %s is skipped: %s" %
(check_projection.__name__,
'cannot check if rectification matrix is not unity'))
pts3D = _build_points_3d()
n_pts = len(pts3D)
src = numpy2opencv_image(pts3D)
dst = numpy2opencv_pointmat(np.empty((n_pts, 2)))
t = np.array(cam.get_translation(), copy=True)
t.shape = 3, 1
R = cam.get_rotation()
rvec = numpy2opencv_image(np.empty((1, 3)))
cv.Rodrigues2(numpy2opencv_image(R), rvec)
if distorted:
K = cam.get_K()
cv_distortion = numpy2opencv_image(cam.get_D())
else:
K = cam.get_P()[:3, :3]
cv_distortion = numpy2opencv_image(np.zeros((5, 1)))
cv.ProjectPoints2(src, rvec, numpy2opencv_image(t), numpy2opencv_image(K),
cv_distortion, dst)
result_cv = opencv_pointmat2numpy(dst)
result_np = cam.project_3d_to_pixel(pts3D, distorted=distorted)
assert result_cv.shape == result_np.shape
if cam.is_opencv_compatible():
try:
assert np.allclose(result_cv, result_np)
except:
debug()
debug('result_cv')
debug(result_cv)
debug('result_np')
debug(result_np)
debug()
raise
else:
from nose.plugins.skip import SkipTest
raise SkipTest(
"Test %s is skipped: %s" %
(check_projection.__name__,
'camera model is not OpenCV compatible, skipping test'))
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 pointcloud_callback(msg):
if (running):
lock.acquire()
global master_cloud
cloud = pointcloud2xyz.pointcloud2_to_xyz_array(msg)
#print len(cloud)
if (len(cloud) > 0):
cloud_mat = cv.CreateMat(len(cloud), 1, cv.CV_32FC3)
projection = cv.CreateMat(len(cloud), 1, cv.CV_32FC2)
cloud_mat = cloud
cv.ProjectPoints2(cv.fromarray(cloud_mat), rotation_vector,
translation_vector, intrinsic_mat,
distortion_coeffs, projection)
(x, y) = cv2.split(np.asarray(projection))
index = 0
master_cloud = []
for i, j in zip(x, y):
master_cloud.append([i[0], j[0], cloud[index]])
index = index + 1
master_cloud = filter(in_frame, master_cloud)
lock.release()
def really_do_posit(locsar,
tags,
viewpoint,
FOCAL_LENGTH,
refpoints=[],
qimgsize=(0, 0)):
# change 3d coordinate systems
c = map(lambda entry: ({
'x': entry[0][0],
'y': entry[0][1]
}, entry[1]), locsar)
locpairs = em.ddImageLocstoLPT(viewpoint, c)
#translate to POSIT Specs
pts2d = map(lambda x: x[0], locpairs)
translation2d = (qimgsize[0] / 2.0, qimgsize[1] / 2.0)
pts2d = map(
lambda x: (x[0][0] - translation2d[0], x[0][1] - translation2d[1]),
locpairs)
translation3d = locpairs[0][1]
pts3d = map(lambda x: tuple(x[1] - translation3d), locpairs)
#convert tag coordinate system
c = map(
lambda x: ({
'x': 0,
'y': 0
}, {
'lat': x.lat,
'lon': x.lon,
'alt': x.alt
}), tags)
c2 = map(
lambda x: ({
'x': 0,
'y': 0
}, {
'lat': x['lat'],
'lon': x['lon'],
'alt': x['alt']
}), refpoints)
taglocpairs = em.ddImageLocstoLPT(viewpoint, c)
reflocpairs = em.ddImageLocstoLPT(viewpoint, c2)
tagpts3d = map(lambda x: tuple(x[1] - translation3d), taglocpairs)
refpts3d = map(lambda x: tuple(x[1] - translation3d), reflocpairs)
#compute POSIT
positobj = cv.CreatePOSITObject(pts3d)
rotMat, transVec = cv.POSIT(positobj, pts2d, FOCAL_LENGTH,
(cv.CV_TERMCRIT_EPS, 0, 0.000001))
# print "rotation matrix:\t{0}".format(rotMat)
# print "translation matrix:\t{0}".format(transVec)
#change rotMat to cvMat
rotMatCV = cv.CreateMat(3, 3, cv.CV_64F)
for i in range(0, 3):
for j in range(0, 3):
cv.Set2D(rotMatCV, i, j, rotMat[i][j])
#convert rotMatrix to rotVector
rotVec = cv.CreateMat(3, 1, cv.CV_64F)
cv.Rodrigues2(rotMatCV, rotVec)
#change transVec to cvMat
transVecCV = cv.CreateMat(1, 3, cv.CV_64F)
for i in range(0, 3):
transVecCV[0, i] = transVec[i]
#camera matrix
cameratrans = cv.CreateMat(3, 3, cv.CV_64F)
cv.SetIdentity(cameratrans)
cameratrans[0, 0] = FOCAL_LENGTH
cameratrans[1, 1] = FOCAL_LENGTH
#distCoefs
distCoef = cv.CreateMat(4, 1, cv.CV_64F)
cv.SetZero(distCoef)
#change 3d coordinate data format
pts3d_mat = cv.CreateMat(len(pts3d), 1, cv.CV_64FC3)
for i, m in enumerate(pts3d):
cv.Set2D(pts3d_mat, i, 0, cv.Scalar(*m))
#project points
d2 = cv.CreateMat(pts3d_mat.rows, 1, cv.CV_64FC2)
cv.ProjectPoints2(pts3d_mat, rotVec, transVecCV, cameratrans, distCoef, d2)
#compute self errors
xerrors = []
yerrors = []
for i in range(0, d2.rows):
xerror = abs(pts2d[i][0] - (d2[i, 0][0]))
yerror = abs(pts2d[i][1] - (d2[i, 0][1]))
xerrors.append(xerror)
yerrors.append(yerror)
print "avg xerror:\t {0}".format(sum(xerrors) / len(xerrors))
print "avg yerror:\t {0}".format(sum(yerrors) / len(yerrors))
#change tag 3d coordinate data format
tagpts3d_mat = cv.CreateMat(len(tagpts3d), 1, cv.CV_64FC3)
for i, m in enumerate(tagpts3d):
cv.Set2D(tagpts3d_mat, i, 0, cv.Scalar(*m))
refpts3d_mat = cv.CreateMat(len(refpts3d), 1, cv.CV_64FC3)
for i, m in enumerate(refpts3d):
cv.Set2D(refpts3d_mat, i, 0, cv.Scalar(*m))
#project points
d2 = cv.CreateMat(tagpts3d_mat.rows, 1, cv.CV_64FC2)
cv.ProjectPoints2(tagpts3d_mat, rotVec, transVecCV, cameratrans, distCoef,
d2)
d22 = cv.CreateMat(refpts3d_mat.rows, 1, cv.CV_64FC2)
cv.ProjectPoints2(refpts3d_mat, rotVec, transVecCV, cameratrans, distCoef,
d22)
ntags = []
nrefs = []
for i in range(0, d2.rows):
ntags.append((tags[i], (0, (d2[i, 0][0] + translation2d[0],
d2[i, 0][1] + translation2d[1]))))
for i in range(0, d22.rows):
nrefs.append((refpoints[i], (0, (d22[i, 0][0] + translation2d[0],
d22[i, 0][1] + translation2d[1]))))
return ntags, nrefs