def test_speed():
#a = np.array([[4,3,2,1],[4,3,2,1],[4,3,2,1]])
#b = np.array([[1,2,3,4],[1,2,3,4],[1,2,3,4]])
print(np.array([[1,0,0], [0,1,0], [0,0,1], [1,1,1]]))
a = [[0,0,1,1,0.5],
[0,1,1,0, -1],
[0,0,0,0, 0]]
a_mod = [[0,0,1,1,0.5],
[0,1,1,0, -1],
[0,0,0,0, 0],
[1,1,1,1, 1]]
b_bad = [[0,1,0,1,0.5],
[0,0,0,0, 0],
[0,1,1,0, -1]]
b_bad_mod = [[0,1,0,1,0.5],
[0,0,0,0, 0],
[0,1,1,0, -1],
[1,1,1,1, 1]]
b_good = [[0,0,1,1,0.5],
[0,0,0,0,0 ],
[0,1,1,0,-1 ]]
b_good_mod = [[0,0,0.5,1,1],
[0,0, 0,0,0],
[0,1, -1,1,0],
[1,1, 1,1,1]]
#for i in range(20000):
M_bad = trans.superimposition_matrix(a, b_bad)
M_good = trans.superimposition_matrix(a, b_good)
print(np.dot(M_bad, a_mod))
print(np.dot(M_good, a_mod))
def transformations_transform(pts1, pts2):
print pts1.T.shape
print pts2.T.shape
#trans = transformations.affine_matrix_from_points(pts1.T, pts2.T)
trans = transformations.superimposition_matrix(pts1.T, pts2.T)
print trans
return trans[0:3, :]
def recenter(cam_dict):
src = [[], [], [], []] # current camera locations
dst = [[], [], [], []] # original camera locations
for image in proj.image_list:
if image.name in cam_dict:
newned = cam_dict[image.name]['ned']
else:
newned, ypr, quat = image.get_camera_pose()
src[0].append(newned[0])
src[1].append(newned[1])
src[2].append(newned[2])
src[3].append(1.0)
origned, ypr, quat = image.get_camera_pose()
dst[0].append(origned[0])
dst[1].append(origned[1])
dst[2].append(origned[2])
dst[3].append(1.0)
print "%s %s" % (origned, newned)
Aff3D = transformations.superimposition_matrix(src, dst, scale=True)
print "Aff3D:\n", Aff3D
scale, shear, angles, trans, persp = transformations.decompose_matrix(Aff3D)
R = transformations.euler_matrix(*angles)
print "R:\n", R
# rotate, translate, scale the group of camera positions to best
# align with original locations
update_cams = Aff3D.dot( np.array(src) )
print update_cams[:3]
for i, p in enumerate(update_cams.T):
key = proj.image_list[i].name
if not key in cam_dict:
cam_dict[key] = {}
ned = [ p[0], p[1], p[2] ]
print "ned:", ned
cam_dict[key]['ned'] = ned
if False:
# adjust the camera projection matrix (rvec) to rotate by the
# amount of the affine transformation as well (this should now
# be better accounted for in solvePnP2()
rvec = cam_dict[key]['rvec']
tvec = cam_dict[key]['tvec']
Rcam, jac = cv2.Rodrigues(rvec)
print "Rcam:\n", Rcam
Rcam_new = R[:3,:3].dot(Rcam)
print "Rcam_new:\n", Rcam_new
rvec, jac = cv2.Rodrigues(Rcam_new)
cam_dict[key]['rvec'] = rvec
tvec = -np.matrix(Rcam_new) * np.matrix(ned).T
cam_dict[key]['tvec'] = tvec
def get_recenter_affine(src_list, dst_list):
src = [[], [], [], []] # current camera locations
dst = [[], [], [], []] # original camera locations
for i in range(len(src_list)):
src_ned = src_list[i]
src[0].append(src_ned[0])
src[1].append(src_ned[1])
src[2].append(src_ned[2])
src[3].append(1.0)
dst_ned = dst_list[i]
dst[0].append(dst_ned[0])
dst[1].append(dst_ned[1])
dst[2].append(dst_ned[2])
dst[3].append(1.0)
print "%s <-- %s" % (dst_ned, src_ned)
A = transformations.superimposition_matrix(src, dst, scale=True)
print "A:\n", A
return A
def get_recenter_affine(cam_dict):
src = [[], [], [], []] # current camera locations
dst = [[], [], [], []] # original camera locations
for image in proj.image_list:
if image.feature_count > 0:
newned = cam_dict[image.name]['ned']
src[0].append(newned[0])
src[1].append(newned[1])
src[2].append(newned[2])
src[3].append(1.0)
origned, ypr, quat = image.get_camera_pose()
dst[0].append(origned[0])
dst[1].append(origned[1])
dst[2].append(origned[2])
dst[3].append(1.0)
print image.name, '%s -> %s' % (origned, newned)
A = transformations.superimposition_matrix(src, dst, scale=True)
print "Affine 3D:\n", A
return A
def get_recenter_affine(cam_dict):
src = [[], [], [], []] # current camera locations
dst = [[], [], [], []] # original camera locations
for image in proj.image_list:
if image.name in cam_dict:
newned = cam_dict[image.name]['ned']
else:
newned, ypr, quat = image.get_camera_pose()
src[0].append(newned[0])
src[1].append(newned[1])
src[2].append(newned[2])
src[3].append(1.0)
origned, ypr, quat = image.get_camera_pose()
dst[0].append(origned[0])
dst[1].append(origned[1])
dst[2].append(origned[2])
dst[3].append(1.0)
print "%s %s" % (origned, newned)
A = transformations.superimposition_matrix(src, dst, scale=True)
return A
def georef(ply,camCoords,output):
print "Inside georef",os.getcwd()
header=""
camImgCoords,head1 = readply(camCoords,0)
print "camImgCoords", camImgCoords
grCord = np.genfromtxt("GroundCoordinates.txt", usecols=(0,1,2))
camGrCoords = np.matrix(grCord,dtype=np.float64, copy=False)
print "camGrCoords", camGrCoords
mdlCoords,header = readply(ply,13)
print "mdlCoords", mdlCoords
Tr = trans.superimposition_matrix(camImgCoords.T,camGrCoords.T,True)
print "Transformation", Tr
print trans.decompose_matrix(Tr)
grCoords = applyTransform(Tr,mdlCoords.T)
print "grCoords", grCoords
colorCoords,head3 = readcolorply(ply,13)
print "color", colorCoords
tempclr=np.hstack((grCoords[:,:-1],colorCoords))
print"tempclr",tempclr
print "header",head3
writeply("",tempclr,output)
def solvePnP2( pts_dict ):
# build a new cam_dict that is a copy of the current one
cam_dict = {}
for i1 in proj.image_list:
cam_dict[i1.name] = {}
rvec, tvec = i1.get_proj()
ned, ypr, quat = i1.get_camera_pose()
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = ned
camw, camh = proj.cam.get_image_params()
for i, i1 in enumerate(proj.image_list):
print i1.name
scale = float(i1.width) / float(camw)
K = proj.cam.get_K(scale)
rvec1, tvec1 = i1.get_proj()
cam2body = i1.get_cam2body()
body2cam = i1.get_body2cam()
# include our own position in the average
quat_start_weight = 100
ned_start_weight = 2
count = 0
sum_quat = np.array(i1.camera_pose['quat']) * quat_start_weight
sum_ned = np.array(i1.camera_pose['ned']) * ned_start_weight
for j, i2 in enumerate(proj.image_list):
matches = i1.match_list[j]
if len(matches) < 8:
continue
count += 1
rvec2, tvec2 = i2.get_proj()
# build obj_pts and img_pts to position i1 relative to the
# matches in i2.
img1_pts = []
img2_pts = []
obj_pts = []
for pair in matches:
kp1 = i1.kp_list[ pair[0] ]
img1_pts.append( kp1.pt )
kp2 = i2.kp_list[ pair[1] ]
img2_pts.append( kp2.pt )
key = "%d-%d" % (i, pair[0])
if not key in pts_dict:
key = "%d-%d" % (j, pair[1])
# print key, pts_dict[key]
obj_pts.append(pts_dict[key])
# compute the centroid of obj_pts
sum = np.zeros(3)
for p in obj_pts:
sum += p
obj_center = sum / len(obj_pts)
print "obj_pts center =", obj_center
# given the previously computed triangulations (and
# averages of point 3d locations if they are involved in
# multiple triangulations), then compute an estimate for
# both matching camera poses. The relative positioning of
# these camera poses should be pretty accurate.
(result, rvec1, tvec1) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img1_pts),
K, None, rvec1, tvec1,
useExtrinsicGuess=True)
(result, rvec2, tvec2) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img2_pts),
K, None, rvec2, tvec2,
useExtrinsicGuess=True)
Rned2cam1, jac = cv2.Rodrigues(rvec1)
Rned2cam2, jac = cv2.Rodrigues(rvec2)
ned1 = -np.matrix(Rned2cam1[:3,:3]).T * np.matrix(tvec1)
ned2 = -np.matrix(Rned2cam2[:3,:3]).T * np.matrix(tvec2)
print "cam1 orig=%s new=%s" % (i1.camera_pose['ned'], ned1)
print "cam2 orig=%s new=%s" % (i2.camera_pose['ned'], ned2)
# compute a rigid transform (rotation + translation) to
# align the estimated camera locations (projected from the
# triangulation) back with the original camera points, and
# roughly rotated around the centroid of the object
# points.
src = np.zeros( (3,3) ) # current camera locations
dst = np.zeros( (3,3) ) # original camera locations
src[0,:] = np.squeeze(ned1)
src[1,:] = np.squeeze(ned2)
src[2,:] = obj_center
dst[0,:] = i1.camera_pose['ned']
dst[1,:] = i2.camera_pose['ned']
dst[2,:] = obj_center
print "src:\n", src
print "dst:\n", dst
M = transformations.superimposition_matrix(src, dst)
print "M:\n", M
# Our (i1) Rned2cam matrix is body2cam * Rned, so solvePnP
# returns this combination. We can extract Rbody2ned by
# premultiplying by cam2body aka inv(body2cam).
Rned2body = cam2body.dot(Rned2cam1)
# print "Rned2body:\n", Rned2body
Rbody2ned = np.matrix(Rned2body).T # IR
# print "Rbody2ned:\n", Rbody2ned
# print "R (after M * R):\n", R
# now transform by the earlier "M" affine transform to
# rotate the work space closer to the actual camera points
Rot = M[:3,:3].dot(Rbody2ned)
# make 3x3 rotation matrix into 4x4 homogeneous matrix
hRot = np.concatenate( (np.concatenate( (Rot, np.zeros((3,1))),1),
np.mat([0,0,0,1])) )
# print "hRbody2ned:\n", hRbody2ned
quat = transformations.quaternion_from_matrix(hRot)
sum_quat += quat
sum_ned += np.squeeze(np.asarray(ned1))
print "count = ", count
newned = sum_ned / (ned_start_weight + count)
print "orig ned =", i1.camera_pose['ned']
print "new ned =", newned
newquat = sum_quat / (quat_start_weight + count)
newIR = transformations.quaternion_matrix(newquat)
print "orig ypr = ", i1.camera_pose['ypr']
(yaw, pitch, roll) = transformations.euler_from_quaternion(newquat, 'rzyx')
print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
newR = np.transpose(newIR[:3,:3]) # equivalent to inverting
newRned2cam = body2cam.dot(newR[:3,:3])
rvec, jac = cv2.Rodrigues(newRned2cam)
tvec = -np.matrix(newRned2cam) * np.matrix(newned).T
#print "orig pose:\n", cam_dict[i1.name]
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = newned
#print "new pose:\n", cam_dict[i1.name]
return cam_dict
cv2.circle(img=bgr_plot, center=tuple(corners[0].flatten()), radius=5, color=(0,0,255), thickness=2)
cv2.imshow(window_name,bgr_plot)
cv2.waitKey(20)
print "chessboard found"
except ValueError:
cam_trans, cam_rot, corners = None,None, None
cv2.imshow(window_name,bgr.copy())
cv2.waitKey(20)
print "chessboard not found"
return cam_trans, cam_rot, corners
cv2.namedWindow('kinect corners', cv2.cv.CV_WINDOW_NORMAL)
cv2.namedWindow('left corners', cv2.cv.CV_WINDOW_NORMAL)
kin_trans_list = []
cam_trans_list = []
with open('/home/joschu/Data/calib/info_left.pkl','r') as fh: cam_info=cPickle.load(fh)
left_cam_matrix = np.array(cam_info.P).reshape(3,4)[:3,:3]
kin_cam_matrix = pcl_utils.CAMERA_MATRIX
for (bgr_kin, bgr_left) in zip(bgr_kinects, bgr_lefts):
kin_trans, kin_rot, kin_corn = get_trans_rot_corners(bgr_kin, 'kinect corners', kin_cam_matrix)
cam_trans, cam_rot, cam_corn = get_trans_rot_corners(bgr_left, 'left corners', left_cam_matrix)
if kin_trans is not None and cam_trans is not None:
kin_trans_list.append(kin_trans.flatten())
cam_trans_list.append(cam_trans.flatten())
M = transformations.superimposition_matrix(np.array(kin_trans_list),np.array(cam_trans_list))
import numpy as np import math import transformations from points import * NOM = NOM.T NOM_A = NOM_A.T MEAS = MEAS.T MEAS_A = MEAS_A.T print "\nNOMINAL\n\n",NOM,"\n\nMEASURED\n\n",MEAS,"\n" print "\nNOMINAL_A\n\n",NOM_A,"\n\nMEASURED_A\n\n",MEAS_A,"\n" #Rotation matrix may be pre- or post- multiplied (changing between a right-handed system and a left-handed system). R = transformations.superimposition_matrix(MEAS,NOM,usesvd=True) scale, shear, angles, translate, perspective = transformations.decompose_matrix(R) #R = transformations.inverse_matrix(R) print "Rotation matrix\n\n",R,"\n" #rot=R.T p1 = transformations.euler_matrix(MEAS_A[0,0]/180*np.pi,MEAS_A[1,0]/180*np.pi,MEAS_A[2,0]/180*np.pi, axes='sxyz') p2 = transformations.euler_matrix(MEAS_A[0,1]/180*np.pi,MEAS_A[1,1]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p3 = transformations.euler_matrix(MEAS_A[0,2]/180*np.pi,MEAS_A[1,2]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p4 = transformations.euler_matrix(MEAS_A[0,3]/180*np.pi,MEAS_A[1,3]/180*np.pi,MEAS_A[2,3]/180*np.pi, axes='sxyz') t1 = transformations.translation_matrix(MEAS[0:,0]) t2 = transformations.translation_matrix(MEAS[0:,1]) t3 = transformations.translation_matrix(MEAS[0:,2]) t4 = transformations.translation_matrix(MEAS[0:,3]) #print "p1\n",p1,"\n","p2\n",p2,"\n","p3\n",p3,"\n","p4\n",p4,"\n" #print "t1\n",t1,"\n","t2\n",t2,"\n","t3\n",t3,"\n","t4\n",t4,"\n" p1n = np.dot(t1,p1) p2n = np.dot(t2,p2)
if averageSample is not None: averageSample = np.add(averageSample , npBox) else: averageSample = npBox; averageSample = (averageSample/len(args.samples)).transpose() with open(args.target) as targetFile: jsonData = json.load(targetFile); targetBox = np.array(jsonData['box']).transpose() print "Sample" print averageSample.transpose() print averageSample.shape print "\nTarget" print targetBox.transpose() print targetBox.shape offset = transformations.superimposition_matrix(averageSample, targetBox, scale=True); print "\nOffset matrix" print offset print "\nWith transform" averageSample = np.vstack((averageSample,[1,1,1,1,1,1])) print np.dot(offset, averageSample).transpose() with open(args.output, 'w') as matrixFile: json.dump(offset.flatten().tolist(), matrixFile)
import numpy import math from transformations import affine_matrix_from_points, translation_matrix, random_rotation_matrix, scale_matrix, concatenate_matrices, superimposition_matrix # def transform(matrix1, matrix2): # v0 = [[0, 0, 0], [1, 1, 1], [2, 2, 2]] # v1 = [[0, 0, 0], [-1, -1, -1], [-2, -2, -2]] # mat = superimposition_matrix(v0, v1) # print(mat) # v3 = numpy.dot(numpy.array(mat).T, numpy.array(v0)) # print(v3) # newV = numpy.dot(mat, numpy.array(v1)) # print(newV) # v0 = numpy.random.rand(3, 3) # print(v0) # M = superimposition_matrix(v0, v1) # print(numpy.allclose(numpy.dot(v0, M), v1)) R = random_rotation_matrix(numpy.random.random(3)) # print(R) v0 = [[1,0,0], [0,1,0], [0,0,1]] v1 = numpy.dot(R, v0) M = superimposition_matrix(v0, v1) print(numpy.allclose(v1, numpy.dot(M, v0)))
