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