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 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 test_6d(self):
a = InnerModelVector.vec6d(1., 2., 3., 4., 5., 6.)
assert (a[0] == 1)
assert (a[5] == 6)
assert (a.shape[0] == 6)
assert (a.min() == [1, 0])
assert (a.max() == [6, 5])
b = InnerModelVector.vec6d(2, 0, 0, 0, 0, 0)
assert (a.dot_product(b) == 2)
assert (b.norm2() == 2)
def anglesToImageCoord(self, cameraId: str, pan: float, tilt: float,
anglesRefs: float) -> 'InnerModelVector':
p = InnerModelVector((3, ))
ray = InnerModelVector((3, ))
p[0] = math.tan(pan)
p[1] = math.tan(tilt)
p[2] = 1
ray = self.innerModel.getRotationMatrixTo(cameraId, anglesRefs).dot(p)
ray[0] = ray[0] / ray[2]
ray[1] = ray[1] / ray[2]
ray[2] = 1
return self.project(cameraId, ray)
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 recursiveConstructor(self, node, tr, parentId):
'''Recursively construct all the parts in a body'''
if isinstance(node, InnerModelMesh):
self.makeMesh(node, tr, parentId)
elif isinstance(node, InnerModelPlane):
self.makePlane(node, tr, parentId)
elif isinstance(node, (InnerModelRGBD, InnerModelCamera)):
self.makeCamera(node, tr, parentId)
elif isinstance(node, InnerModelLaser):
self.makeLaser(node, tr, parentId)
elif isinstance(node, InnerModelIMU):
self.makeIMU(node, tr, parentId)
elif isinstance(node, (InnerModelJoint, InnerModelPrismaticJoint)):
self.makeJoint(node, tr, parentId)
elif isinstance(node, InnerModelTouchSensor):
self.hasTouchSensor = True
parentId = node.parent.id
self.touchSensorLink.append(parentId)
else:
self.baseMass += node.mass
tr_ = InnerModelVector.vec6d(node.tx / 100.0 + tr.x(),
node.ty / 100.0 + tr.y(),
node.tz / 100.0 + tr.z(),
node.rx + tr.rx(), node.ry + tr.ry(),
node.rz + tr.rz())
for child in node.children:
self.recursiveConstructor(child, tr_, parentId)
def getDiagonal(self) -> 'InnerModelVector':
'''Returns diagonal elements in form of a vector'''
x = self.diagonal()
y = InnerModelVector((x.shape[0]))
np.copyto(y, x)
return y
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 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 eigenValsVectors(self) -> ('InnerModelVector', 'InnerModelMatrix'):
'''Returns eigen values and eigen vectors'''
assert (self.isSquare())
w, V = np.linalg.eig(self)
y = InnerModelVector(w.shape)
np.copyto(y, w)
return (y, V)
def toVector(self) -> 'InnerModelVector':
'''Converts the 1 column matrix to a vector'''
assert (self.shape[0] > 0 and self.shape[1] == 1)
rows = self.shape[0]
y = InnerModelVector((rows, ))
np.copyto(y, self[:, 0])
return y
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
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 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 transform6D(self,
destId: str,
orgId: str,
origVec: 'InnerModelVector' = None) -> 'InnerModelVector':
'''Get the transformation between two nodes with given ids'''
if origVec is not None:
assert (origVec.size() == 6)
return self.transform(destId, orgId, origVec)
else:
v = InnerModelVector.vec6d(0, 0, 0, 0, 0, 0)
return self.transform(destId, orgId, v)
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 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 createBody(self, node):
'''Create a separate body from the node'''
assert (self.getClassType(node) == 'Transform')
self.imNode = node
self.id = node.id
# self.baseMass = node.mass
self.baseMass = 10
self.basePosition = [node.tx / 100.0, node.ty / 100.0, node.tz / 100.0]
self.baseOrientation = p.getQuaternionFromEuler(
[node.rx, node.ry, node.rz])
tr = InnerModelVector.vec6d(0, 0, 0, 0, 0, 0)
parentId = 0
for child in node.children:
self.recursiveConstructor(child, tr, parentId)
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 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 laserTo (self, dest: str, r: float, alpha: float) -> 'InnerModelVector':
p = InnerModelVector((3,))
p[0] = r*math.sin (alpha)
p[1] = 0
p[2] = r*math.cos (alpha)
return self.innerModel.transform (dest, self.id, p)
def getRow(self, row: int) -> 'InnerModelVector':
'''Get a particular row'''
y = InnerModelVector((self.shape[1], ))
np.copyto(y, self[row, :])
return y
def getCol(self, col: int) -> 'InnerModelVector':
'''Returns a give column'''
y = InnerModelVector((self.shape[0], ))
np.copyto(y, self[:, col])
return y
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.)