Example #1
0
 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
Example #2
0
    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
Example #3
0
    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
Example #4
0
    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)
Example #5
0
    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)
Example #6
0
    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
Example #7
0
    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)
Example #8
0
    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
Example #9
0
 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()
Example #10
0
    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))
Example #11
0
    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)
Example #12
0
    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
Example #13
0
 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
Example #14
0
 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
Example #15
0
    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()
Example #16
0
    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)
Example #17
0
 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
Example #18
0
 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
Example #19
0
    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)
Example #20
0
    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
Example #21
0
    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
Example #22
0
    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)
Example #23
0
    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
Example #24
0
    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
Example #25
0
 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
Example #26
0
 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)
Example #27
0
    def getRow(self, row: int) -> 'InnerModelVector':
        '''Get a particular row'''

        y = InnerModelVector((self.shape[1], ))
        np.copyto(y, self[row, :])
        return y
Example #28
0
    def getCol(self, col: int) -> 'InnerModelVector':
        '''Returns a give column'''

        y = InnerModelVector((self.shape[0], ))
        np.copyto(y, self[:, col])
        return y
Example #29
0
 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.)