Ejemplo n.º 1
0
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))
Ejemplo n.º 2
0
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, :]
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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
Ejemplo n.º 6
0
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
Ejemplo n.º 7
0
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)
Ejemplo n.º 8
0
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))
Ejemplo n.º 10
0
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)
Ejemplo n.º 11
0
			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)

Ejemplo n.º 12
0
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)))