def getPlaneProjectionMatrix(self, destCamera: str, planeId: str, sourceCamera: str): planeN = self.innerModel.getNode(planeId).normal planeN = self.innerModel.getRotationMatrixTo(sourceCamera, planeId).dot(planeN) planePoint = self.innerModel.transform( sourceCamera, planeId, self.innerModel.getNode(planeId).point) R = self.innerModel.getRotationMatrixTo(destCamera, sourceCamera) t = self.innerModel.transform(destCamera, sourceCamera, InnerModelVector.vec3d(0, 0, 0)) n = InnerModelMatrix.makeDiagonal(planeN) K1 = self.innerModel.getNode(sourceCamera).camera.qmat d = -1 * (planePoint.dot(planeN)) H = (R - ((t.dot(n.getTranspose())) / d)).dot(K1.invert()) HFinal = InnerModelMatrix((4, 3)) HFinal.inject(H, 0, 0) HFinal = HFinal * 1000 * 1000 HFinal[3][0] = 1000 * H[2][0] HFinal[3][1] = 1000 * H[2][1] HFinal[3][2] = 1000 * H[2][2] return HFinal
def testEuler(self): m1 = InnerModelMatrix((3, 3)) r = R.from_euler('xyz', [0, 0, 90], degrees=True) m2 = r.as_matrix() np.copyto(m1, m2) v = m1.extractAnglesR() self.assertTrue(np.allclose(v, np.array([0, 0, np.pi / 2])))
def testEigen(self): m = InnerModelMatrix((3, 3)) m1 = np.array([[2, 3, 0], [0, 3, 8], [0, 0, 1]]) np.copyto(m, m1) w, V = np.linalg.eig(m1) w_, V_ = m.eigenValsVectors() self.assertTrue(np.allclose(w, w_)) self.assertTrue(np.allclose(V, V_))
def testSVD(self): m1 = InnerModelMatrix((3, 3)) self.assertTrue(m1 is not None) m = np.array([[10, 3, 4], [0, 1, 0], [0, 10, 90]]) np.copyto(m1, m) U, S, V = m1.SVD() U_T = U.getTranspose() V_T = V.getTranspose() I = np.identity(3) self.assertTrue(np.allclose(U.dot(U_T), I)) self.assertTrue(np.allclose(V.dot(V_T), I)) self.assertTrue(isDiag(S))
def __init__(self, *args, **kwargs): self.Rx = Rot3DCOX.getRot3DCOX(alpha=0) self.Ry = Rot3DCOY.getRot3DCOY(alpha=0) self.Rz = Rot3DCOZ.getRot3DCOZ(alpha=0) self.R = InnerModelMatrix.identity(3) self.Tr = InnerModelVector.vec3d(0, 0, 0) self.do_inject()
def polar3DToCamera(self, p: 'InnerModelMatrix') -> 'InnerModelMatrix': r = InnerModelMatrix((3, 1)) r[2] = p[2] / (p[0] * p[0] / (self.focusX * self.focusY) + (p[1] * p[1] / (self.focusX * self.focusY) + 1)) r[0] = r[2] * p[0] / self.focusX r[1] = r[2] * p[1] / self.focusY return r
def compute3DPointFromImageAngles(self, firstCam: str, left: 'InnerModelVector', secondCam: str, right: 'InnerModelVector', refSystem: str) -> 'InnerModelVector': ray = InnerModelVector.vec3d(math.tan(left[0]), math.tan(left[1]), 1) pI = copy.deepcopy(ray) pI[0] = pI[0] / pI[2] pI[1] = pI[1] / pI[2] pI[2] = 1 ray[0] = math.tan(right[0]) ray[1] = math.tan(right[1]) ray[2] = 1 pD = copy.deepcopy(ray) pD[0] = pD[0] / pD[2] pD[1] = pD[1] / pD[2] pD[2] = 1 n = np.bitwise_xor(pI, pD) A = InnerModelMatrix((3, 3)) A[0][0] = pI[0] A[0][1] = -1 * pD[0] A[0][2] = n[0] A[1][0] = pI[1] A[1][1] = -1 * pD[1] A[1][2] = n[1] A[2][0] = pI[2] A[2][1] = -1 * pD[2] A[2][2] = n[2] TI = self.innerModel.getRotationMatrixTo( refSystem, firstCam).fromHomogeneousCoordinates() TD = self.innerModel.getRotationMatrixTo( refSystem, secondCam).fromHomogeneousCoordinates() T = TD - TI abc = A.invert().dot(T) pR = pI.dot(abc[0]) pR = pR + TI pR = (n.dot(abc[2] / 2)) + pR return pR
def __init__(self, *args, **kwargs): '''Initialise all the rotation matrices with angle of rotation 0 and translation 0 vector''' self.Rx = Rot3DOX.getRot3DOX(alpha=0) self.Ry = Rot3DOY.getRot3DOY(alpha=0) self.Rz = Rot3DOZ.getRot3DOZ(alpha=0) self.R = InnerModelMatrix.identity(3) self.Tr = InnerModelVector.vec3d(0, 0, 0) self.do_inject()
def compute3DPointFromImageCoords(self, firstCam: str, left: 'InnerModelVector', secondCam: str, right: 'InnerModelVector', refSystem: str) -> 'InnerModelVector': ray = self.backProject(firstCam, left) pI = self.innerModel.getRotationMatrixTo(refSystem, firstCam).dot(ray) pI[0] = pI[0] / pI[2] pI[1] = pI[1] / pI[2] pI[2] = 1 ray = self.backProject(secondCam, right) pD = self.innerModel.getRotationMatrixTo(refSystem, secondCam).dot(ray) pD[0] = pD[0] / pD[2] pD[1] = pD[1] / pD[2] pD[2] = 1 n = np.bitwise_xor(pI, pD) A = InnerModelMatrix((3, 3)) A[0][0] = pI[0] A[0][1] = -1 * pD[0] A[0][2] = n[0] A[1][0] = pI[1] A[1][1] = -1 * pD[1] A[1][2] = n[1] A[2][0] = pI[2] A[2][1] = -1 * pD[2] A[2][2] = n[2] TI = self.innerModel.getRotationMatrixTo( refSystem, firstCam).fromHomogeneousCoordinates() TD = self.innerModel.getRotationMatrixTo( refSystem, secondCam).fromHomogeneousCoordinates() T = TD - TI abc = A.invert().dot(T) pR = pI.dot(abc[0]) pR = pR + TI pR = (n.dot(abc[2] / 2)) + pR return pR
def getRotationMatrixTo(self, to: str, fr: str) -> 'Rot3D': '''Get rotation matrix betwn two nodes with given ids''' rret = InnerModelMatrix.identity(3) self.setLists(fr, to) for node in self.listA: rret = node.rmat.getR().dot(rret) for node in self.listB: rret = node.rmat.getR().getTranspose().dot(rret) return rret
def testInvert(self): m1 = InnerModelMatrix((3, 3)) m = np.array([[10, 0, 0], [5, 10, 0], [0, 0, 10]]) np.copyto(m1, m) m1_inv = m1.invert() I = np.identity(3) self.assertTrue(np.allclose(m1.dot(m1_inv), I)) m = np.array([[4, 0, 0, 0], [0, 4, 0, 0], [0, 0, 4, 0]]) m1 = InnerModelMatrix((3, 4)) np.copyto(m1, m) with self.assertRaises(AssertionError): m1_inv = m1.invert()
def __init__(self, Fx: float = None, Fy: float = None, Ox: float = None, Oy: float = None, c: 'InnerModelCam' = None): self.qmat = InnerModelMatrix((3, 3)) if (c is not None): self = copy.deepcopy(c) elif Fx is not None: self.focalX = Fx self.focalY = Fy self.centerX = Ox self.centerY = Oy self.width = Ox * 2 self.height = Oy * 2 self.size = self.width * self.height self.qmat[0][0] = Fx self.qmat[0][1] = 0 self.qmat[0][1] = Ox self.qmat[1][0] = 0 self.qmat[1][1] = -1 * Fy self.qmat[1][2] = Oy self.qmat[2][0] = 0 self.qmat[2][1] = 0 self.qmat[2][2] = 1 else: self.focalX = 200 self.focalY = 200 self.centerX = 160 self.centerY = 120 self.width = self.centerX * 2 self.height = self.centerY * 2 self.size = self.width * self.height self.focusX = None self.focusY = None
def getHomographyMatrix(self, destCamera: str, planeId: str, sourceCamera: str): planeN = self.innerModel.getNode(planeId).normal planeN = self.innerModel.getRotationMatrixTo(sourceCamera, planeId).dot(planeN) planePoint = self.innerModel.transform( sourceCamera, planeId, self.innerModel.getNode(planeId).point) R = self.innerModel.getRotationMatrixTo(destCamera, sourceCamera) t = self.innerModel.transform(destCamera, sourceCamera, InnerModelVector.vec3d(0, 0, 0)) n = InnerModelMatrix.makeDiagonal(planeN) K1 = self.innerModel.getNode(sourceCamera).camera.qmat K2 = self.innerModel.getNode(destCamera).camera.qmat d = -1 * (planePoint.dot(planeN)) H = K2.dot(R - ((t.dot(n.getTranspose())) / d)).dot(K1.invert()) return H
class InnerModelCam(object): def __init__(self, Fx: float = None, Fy: float = None, Ox: float = None, Oy: float = None, c: 'InnerModelCam' = None): self.qmat = InnerModelMatrix((3, 3)) if (c is not None): self = copy.deepcopy(c) elif Fx is not None: self.focalX = Fx self.focalY = Fy self.centerX = Ox self.centerY = Oy self.width = Ox * 2 self.height = Oy * 2 self.size = self.width * self.height self.qmat[0][0] = Fx self.qmat[0][1] = 0 self.qmat[0][1] = Ox self.qmat[1][0] = 0 self.qmat[1][1] = -1 * Fy self.qmat[1][2] = Oy self.qmat[2][0] = 0 self.qmat[2][1] = 0 self.qmat[2][2] = 1 else: self.focalX = 200 self.focalY = 200 self.centerX = 160 self.centerY = 120 self.width = self.centerX * 2 self.height = self.centerY * 2 self.size = self.width * self.height self.focusX = None self.focusY = None def getAngles(self, p: 'InnerModelVector') -> 'InnerModelVector': assert (p.size() == 2) r = self.toZeroCenter(p) r[0] = math.atan2(r[0], self.focusX) r[1] = math.atan2(r[1], self.focusY) return r def getAnglesHomogeneous(self, p: 'InnerModelVector') -> 'InnerModelVector': r = self.toZeroCenterHomogeneous(p) r[0] = r[0] / self.focusX r[1] = r[1] / self.focusY return r def getFocal(self) -> float: return self.focusX def getFocalX(self) -> float: return self.focalX def getFocalY(self) -> float: return self.focalY def getHeight(self) -> int: return self.height def getWidth(self) -> int: return self.width def getSize(self) -> int: return self.size def getRayHomogeneous(self, p: 'InnerModelVector') -> 'InnerModelVector': r = self.toZeroCenterHomogeneous(p) r[0] = r[0] / self.focusX r[1] = r[1] / self.focusY return r def getRay(self, p: 'InnerModelVector') -> 'InnerModelVector': r = self.toZeroCenter(p) r[0] = r[0] / self.focusX r[1] = r[1] / self.focusY return r def polar3DToCamera(self, p: 'InnerModelMatrix') -> 'InnerModelMatrix': r = InnerModelMatrix((3, 1)) r[2] = p[2] / (p[0] * p[0] / (self.focusX * self.focusY) + (p[1] * p[1] / (self.focusX * self.focusY) + 1)) r[0] = r[2] * p[0] / self.focusX r[1] = r[2] * p[1] / self.focusY return r def project(self, p: 'InnerModelVector') -> 'InnerModelVector': loc = self.qmat.dot(p) res = InnerModelVector((3, )) if (abs(loc[2]) != 0): res[0] = loc[0] / loc[2] res[1] = loc[1] / loc[2] res[2] = loc[2] return res else: print("InnerModelCamera: warning projected point at infinite") return res def set(self, Fx: float, Fy: float, Ox: float, Oy: float): self.focalX = Fx self.focalY = Fy self.centerX = Ox self.centerY = Oy self.width = Ox * 2 self.height = Oy * 2 self.size = self.width * self.height self.qmat[0][0] = Fx self.qmat[0][1] = 0 self.qmat[0][1] = Ox self.qmat[1][0] = 0 self.qmat[1][1] = -1 * Fy self.qmat[1][2] = Oy self.qmat[2][0] = 0 self.qmat[2][1] = 0 self.qmat[2][2] = 1 def setFocal(self, f: float): self.focusX = f self.focusY = f def setFocalX(self, fx: float): self.focalX = fx def setFocalY(self, fy: float): self.focalY = fy def setSize(self, w: int, h: int): self.width = w self.height = h self.size = w * h def toZeroCenter(self, p: 'InnerModelVector') -> 'InnerModelVector': assert (p.size() >= 2) r = InnerModelVector((2, )) r[0] = p[0] - self.centerX r[1] = p[1] - self.centerY return r def toZeroCenterHomogeneous(self, p: 'InnerModelVector') -> 'InnerModelVector': assert (p.size() >= 2) r = InnerModelVector((3, )) r[0] = p[0] - self.centerX r[1] = r[1] - self.centerY r[2] = 1 return r