def __init__(self,
id: str,
port: int,
texture: str,
width: float,
height: float,
depth: float,
repeat: int,
nx: float,
ny: float,
nz: float,
px: float,
py: float,
pz: float,
collidable: bool,
parent: 'InnerModelNode' = None):
super(InnerModelDisplay, self).__init__(id, parent)
self.normal = InnerModelVector.vec3d(nx, ny, nz)
self.point = InnerModelVector.vec3d(px, py, pz)
self.port = port
self.repeat = repeat # redundant
self.texture = texture
self.width = width
self.height = height
self.depth = depth
self.collidable = collidable # redundant
def transform(self,
destId: str,
origId: str,
origVec: 'InnerModelVector' = None) -> 'InnerModelVector':
'''Get the transformation between two nodes with given ids'''
if origVec is None:
origVec = InnerModelVector.vec3d(0, 0, 0)
if origVec.size() == 3:
m = self.getTransformationMatrix(destId, origId)
v = origVec.toHomogeneousCoordinates()
vt_ = m.dot(v)
vt = InnerModelVector((4, ))
np.copyto(vt, vt_)
return vt.fromHomogeneousCoordinates()
else:
M = self.getTransformationMatrix(destId, origId)
a = M.dot(origVec.subVector(
0, 2).toHomogeneousCoordinates()).fromHomogeneousCoordinates()
R = Rot3D(origVec[3], origVec[4], origVec[5])
b = (M.getSubmatrix(0, 2, 0, 2).dot(R)).extractAnglesR_min()
ret = InnerModelVector.vec6d(a[0], a[1], a[2], b[3], b[4], b[5])
return ret
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 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 testGetRTMat(self):
mat = InnerModelRTMat.getInnerModelRTMat(tx=10,
ty=2,
tz=1,
rx=np.pi / 4,
ry=np.pi / 6,
rz=np.pi / 3)
self.assertTrue(mat is not None)
r1 = R.from_euler('x', 45, degrees=True)
m = r1.as_matrix()
self.assertTrue(np.allclose(m, mat.Rx))
r2 = R.from_euler('y', 30, degrees=True)
m = r2.as_matrix()
self.assertTrue(np.allclose(m, mat.Ry))
r3 = R.from_euler('z', 60, degrees=True)
m = r3.as_matrix()
self.assertTrue(np.allclose(m, mat.Rz))
r = r1 * r2 * r3
m = r.as_matrix()
self.assertTrue(np.allclose(m, mat.R))
v = InnerModelVector.vec3d(10, 2, 1)
self.assertTrue(np.allclose(mat.Tr, v))
def project(self,
origVec: 'InnerModelVector',
reference: str = None) -> 'InnerModelVetor':
if reference is not None:
origVec = self.innerModel.transform(self.id, reference, origVec)
pc = self.camera.project(origVec)
v = InnerModelVector.vec3d(pc[0], pc[1], origVec.norm2())
return v
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 horizonLine(self,
planeId: str,
cameraId: str,
heightOffset: float = 0) -> 'InnerModelVector':
plane = self.innerModel.getNode(planeId)
camera = self.innerModel.getNode(cameraId)
rtm = self.innerModel.getRotationMatrixTo(cameraId, planeId)
vec = InnerModelVector.vec3d(plane.normal[0], plane.normal[1],
plane.normal[2])
normal = rtm.dot(vec)
if normal[1] <= 0.0000002:
raise Exception("InnerModelCamera: something wrong")
p1 = InnerModelVector.vec3d(0, 0, 0)
p2 = InnerModelVector.vec3d(0, 0, 0)
p1[2] = p2[2] = 1000
if normal[1] > 0.0000001:
p1[1] = p2[1] = p1[2] * normal[2] / normal[1]
if normal[1] > 0.0000001:
p1[1] -= 200 * normal[0] / normal[1]
p1[0] = 200
if normal[1] > 0.0000001:
p2[1] -= 200 * normal[0] / normal[1]
p2[0] = -200
p1 = self.project(cameraId, p1)
p2 = self.project(cameraId, p2)
dx = p2[0] - p1[0]
dy = p2[1] - p1[1]
if abs(dx) <= 1:
if abs(dy) <= 1:
raise Exception("Degenerated camera")
return InnerModelVector.vec3d(-1, 0, p1[0])
else:
h = camera.camera.getHeight()
return InnerModelVector.vec3d(
dy / dx, -1, h - (p1[1] - (dy * p1[0] / dx)) + heightOffset)
def getInnerModelRTCMat(ox: float, oy: float, oz: float, x: float,
y: float, z: float) -> 'InnerModelRTCMat':
mat = InnerModelRTCMat((4, 4))
mat.Rx = Rot3DCOX.getRot3DCOX(alpha=ox)
mat.Ry = Rot3DCOY.getRot3DCOY(alpha=oy)
mat.Rz = Rot3DCOZ.getRot3DCOZ(alpha=oz)
R = mat.Rx.dot(mat.Ry.dot(mat.Rz))
np.copyto(mat.R, R)
mat.Tr = InnerModelVector.vec3d(x, y, z)
mat.do_inject()
return mat
def extractAnglesR_min(self) -> 'InnerModelVector':
'''Returns euler angles with lower magnitude'''
r = self.extractAnglesR()
if (r.shape[0] == 1):
return InnerModelVector.vec3d(r[0], r[1], r[2])
v1 = InnerModelVector((3, ))
np.copyto(v1, r[0])
v2 = InnerModelVector((3, ))
np.copyto(v2, r[1])
if (np.linalg.norm(v1) < np.linalg.norm(v2)):
return v1
return v2
def getInnerModelRTMat(tx: float, ty: float, tz: float, rx: float,
ry: float, rz: float) -> 'InnerModelRTMat':
'''Get an instance of this class with translation as well as the rotation angle given'''
mat = InnerModelRTMat((4, 4))
mat.Rx = Rot3DOX.getRot3DOX(alpha=rx)
mat.Ry = Rot3DOY.getRot3DOY(alpha=ry)
mat.Rz = Rot3DOZ.getRot3DOZ(alpha=rz)
R = mat.Rx.dot(mat.Ry.dot(mat.Rz))
np.copyto(mat.R, R)
mat.Tr = InnerModelVector.vec3d(tx, ty, tz)
mat.do_inject()
return mat
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 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
def projectFromCameraToPlane(self, to: str, coord: 'InnerModelVector',
cameraId: str, vPlane: 'InnerModelVector',
dist: float) -> 'InnerModelVector':
pCam = InnerModelVector((3, ))
res = InnerModelVector((3, ))
pCam[0] = -coord[0] + self.getWidth() / 2
pCam[1] = -1 * (coord[1] - self.getHeight() / 2)
pCam[2] = self.getFocal()
pDest = self.innerModel.transform(to, cameraId, pCam)
pCent = self.innerModel.transform(to, cameraId,
InnerModelVector.vec3d(0, 0, 0))
direc = pDest - pCent
dxz = direc[0] / direc[2]
dyz = direc[1] / direc[2]
res[2] = dist + vPlane[0] * (dxz * pCent[2] - pCent[0]) + vPlane[1] * (
dyz * pCent[2] - pCent[1])
res[2] = res[2] / (vPlane[0] * dxz + vPlane[1] * dyz + vPlane[2])
res[0] = dxz * (res[2] - pCent[2]) + pCent[0]
res[1] = dyz * (res[2] - pCent[2]) + pCent[1]
return res
def test_3d(self):
a = InnerModelVector.vec3d(1., 2., 3.)
b = InnerModelVector.vec3d(3., 2., 1.)
assert (a[0] + b[0] == 1. + 3.)
assert (a[1] + b[1] == 2. + 2.)
assert (a[2] + b[2] == 3. + 1.)
