Python numpy.matrix() Examples

The following are 30 code examples of numpy.matrix(). You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may also want to check out all available functions/classes of the module numpy , or try the search function .
Example #1
Source File: heston.py    From tensortrade with Apache License 2.0 6 votes vote down vote up
def heston_construct_correlated_path(params: ModelParameters,
                                     brownian_motion_one: np.array):
    """
    This method is a simplified version of the Cholesky decomposition method
    for just two assets. It does not make use of matrix algebra and is therefore
    quite easy to implement.

    Arguments:
        params : ModelParameters
            The parameters for the stochastic model.
        brownian_motion_one : np.array
            (Not filled)

    Returns:
        A correlated brownian motion path.
    """
    # We do not multiply by sigma here, we do that in the Heston model
    sqrt_delta = np.sqrt(params.all_delta)
    # Construct a path correlated to the first path
    brownian_motion_two = []
    for i in range(params.all_time - 1):
        term_one = params.cir_rho * brownian_motion_one[i]
        term_two = np.sqrt(1 - pow(params.cir_rho, 2)) * random.normalvariate(0, sqrt_delta)
        brownian_motion_two.append(term_one + term_two)
    return np.array(brownian_motion_one), np.array(brownian_motion_two) 
Example #2
Source File: losses_test.py    From object_detector_app with MIT License 6 votes vote down vote up
def testReturnsCorrectAnchorWiseLoss(self):
    prediction_tensor = tf.constant([[[-100, 100, -100],
                                      [100, -100, -100],
                                      [0, 0, -100],
                                      [-100, -100, 100]],
                                     [[-100, 0, 0],
                                      [-100, 100, -100],
                                      [-100, 100, -100],
                                      [100, -100, -100]]], tf.float32)
    target_tensor = tf.constant([[[0, 1, 0],
                                  [1, 0, 0],
                                  [1, 0, 0],
                                  [0, 0, 1]],
                                 [[0, 0, 1],
                                  [0, 1, 0],
                                  [0, 1, 0],
                                  [1, 0, 0]]], tf.float32)
    weights = tf.constant([[1, 1, .5, 1],
                           [1, 1, 1, 0]], tf.float32)
    loss_op = losses.WeightedSoftmaxClassificationLoss(True)
    loss = loss_op(prediction_tensor, target_tensor, weights=weights)

    exp_loss = np.matrix([[0, 0, - 0.5 * math.log(.5), 0],
                          [-math.log(.5), 0, 0, 0]])
    with self.test_session() as sess:
      loss_output = sess.run(loss)
      self.assertAllClose(loss_output, exp_loss) 
Example #3
Source File: 9_anomaly_and_rec.py    From deep-learning-note with MIT License 6 votes vote down vote up
def cost(params, Y, R, num_features):
    Y = np.matrix(Y)  # (1682, 943)
    R = np.matrix(R)  # (1682, 943)
    num_movies = Y.shape[0]
    num_users = Y.shape[1]
    
    # reshape the parameter array into parameter matrices
    X = np.matrix(np.reshape(params[:num_movies * num_features], (num_movies, num_features)))  # (1682, 10)
    Theta = np.matrix(np.reshape(params[num_movies * num_features:], (num_users, num_features)))  # (943, 10)
    
    # initializations
    J = 0
    
    # compute the cost
    error = np.multiply((X * Theta.T) - Y, R)  # (1682, 943)
    squared_error = np.power(error, 2)  # (1682, 943)
    J = (1. / 2) * np.sum(squared_error)
    
    return J 
Example #4
Source File: icp.py    From pointnet-registration-framework with MIT License 6 votes vote down vote up
def _icp_find_rigid_transform(p_from, p_target):
	######### flag = 1 for SVD 
	######### flag = 2 for Gaussian
	A, B = np.copy(p_from), np.copy(p_target)

	centroid_A = np.mean(A, axis=0)
	centroid_B = np.mean(B, axis=0)

	A -= centroid_A						# 500x3
	B -= centroid_B						# 500x3

	H = np.dot(A.T, B)					# 3x3
	# print(H)
	U, S, Vt = np.linalg.svd(H)
	# H_new = U*np.matrix(np.diag(S))*Vt
	# print(H_new)
	R = np.dot(Vt.T, U.T)

	# special reflection case
	if np.linalg.det(R) < 0:
		Vt[2,:] *= -1
		R = np.dot(Vt.T, U.T)
	t = np.dot(-R, centroid_A) + centroid_B
	return R, t 
Example #5
Source File: fisheye.py    From DualFisheye with MIT License 6 votes vote down vote up
def _get_equirectangular_raster(self, out_size):
        # Set image size (2x1 aspect ratio)
        rows = out_size
        cols = 2*out_size
        # Calculate longitude of each column.
        theta_x = np.linspace(-pi, pi, cols, endpoint=False, dtype='float32')
        cos_x = np.cos(theta_x).reshape(1,cols)
        sin_x = np.sin(theta_x).reshape(1,cols)
        # Calculate lattitude of each row.
        ystep = pi / rows
        theta_y = np.linspace(-pi/2 + ystep/2, pi/2 - ystep/2, rows, dtype='float32')
        cos_y = np.cos(theta_y).reshape(rows,1)
        sin_y = np.sin(theta_y).reshape(rows,1)
        # Calculate X, Y, and Z coordinates for each output pixel.
        x = cos_y * cos_x
        y = sin_y * np.ones((1,cols), dtype='float32')
        z = cos_y * sin_x
        # Vectorize the coordinates in raster order.
        xyz = np.matrix([x.ravel(), y.ravel(), z.ravel()])
        return [xyz, rows, cols]

    # Convert all lens parameters to a state vector. See also: optimize() 
Example #6
Source File: fisheye.py    From DualFisheye with MIT License 6 votes vote down vote up
def render_cubemap(self, out_size, mode='blend'):
        # Create coordinate arrays.
        cvec = np.arange(out_size, dtype='float32') - out_size/2        # Coordinate range [-S/2, S/2)
        vec0 = np.ones(out_size*out_size, dtype='float32') * out_size/2 # Constant vector +S/2
        vec1 = np.repeat(cvec, out_size)                                # Increment every N steps
        vec2 = np.tile(cvec, out_size)                                  # Sweep N times
        # Create XYZ coordinate vectors and render each cubemap face.
        render = lambda(xyz): self._render(xyz, out_size, out_size, mode)
        xm = render(np.matrix([-vec0, vec1, vec2]))     # -X face
        xp = render(np.matrix([vec0, vec1, -vec2]))     # +X face
        ym = render(np.matrix([-vec1, -vec0, vec2]))    # -Y face
        yp = render(np.matrix([vec1, vec0, vec2]))      # +Y face
        zm = render(np.matrix([-vec2, vec1, -vec0]))    # -Z face
        zp = render(np.matrix([vec2, vec1, vec0]))      # +Z face
        # Concatenate the individual faces in canonical order:
        # https://en.wikipedia.org/wiki/Cube_mapping#Memory_Addressing
        img_mat = np.concatenate([zp, zm, ym, yp, xm, xp], axis=0)
        return Image.fromarray(img_mat)

    # Get XYZ vectors for an equirectangular render, in raster order.
    # (Each row left to right, with rows concatenates from top to bottom.) 
Example #7
Source File: fashion_markov_model.py    From indras_net with GNU General Public License v3.0 6 votes vote down vote up
def set_state(self, new_state):
        '''
        Set Hipster's new type.
        This comes from evaling the env.
        Args:
            new_state: the agent's new state
        Returns:
            None
        '''
        old_type = self.ntype
        self.state = new_state
        self.ntype = STATE_MAP[new_state]
        self.env.change_agent_type(self, old_type, self.ntype)
        # Unlike the forest fire model, we don't here update environment's cell's
        # transition matrices. This is because the cell's transition matrix depends
        # on all the agents near it, not just one. 
Example #8
Source File: fisheye.py    From DualFisheye with MIT License 6 votes vote down vote up
def add_pixels(self, uv_px, img1d, weight=None):
        # Lookup row & column for each in-bounds coordinate.
        mask = self.get_mask(uv_px)
        xx = uv_px[0,mask]
        yy = uv_px[1,mask]
        # Update matrix according to assigned weight.
        if weight is None:
            img1d[mask] = self.img[yy,xx]
        elif np.isscalar(weight):
            img1d[mask] += self.img[yy,xx] * weight
        else:
            w1 = np.asmatrix(weight, dtype='float32')
            w3 = w1.transpose() * np.ones((1,3))
            img1d[mask] += np.multiply(self.img[yy,xx], w3[mask])


# A panorama image made from several FisheyeImage sources.
# TODO: Add support for supersampled anti-aliasing filters. 
Example #9
Source File: zombie.py    From indras_net with GNU General Public License v3.0 6 votes vote down vote up
def get_pre(self, agent, n_census):

        trans_str = ""

        d, total = self.dir_info(agent)

        if type(agent) == Zombie:
            trans_str += self.zombie_trans(d, total)
        else:
            trans_str += self.human_trans(d, total)

        trans_matrix = markov.from_matrix(np.matrix(trans_str))
        return trans_matrix
       
    # Finds out which direction (NORTH, SOUTH, EAST, WEST) as more of the 
    # opposite agent type depending on what agent we are dealing with
    # CAN'T GET RID OF OLD METHOD OF MOVEMENT DUE TO ERRORS
    # IN OTHER SCRIPTS 
Example #10
Source File: 5_nueral_network.py    From deep-learning-note with MIT License 6 votes vote down vote up
def cost0(params, input_size, hidden_size, num_labels, X, y, learning_rate):
    m = X.shape[0]
    X = np.matrix(X)
    y = np.matrix(y)
    
    # reshape the parameter array into parameter matrices for each layer
    theta1 = np.matrix(np.reshape(params[:hidden_size * (input_size + 1)], (hidden_size, (input_size + 1))))
    theta2 = np.matrix(np.reshape(params[hidden_size * (input_size + 1):], (num_labels, (hidden_size + 1))))
    
    # run the feed-forward pass
    a1, z2, a2, z3, h = forward_propagate(X, theta1, theta2)
    
    # compute the cost
    J = 0
    for i in range(m):
        first_term = np.multiply(-y[i,:], np.log(h[i,:]))
        second_term = np.multiply((1 - y[i,:]), np.log(1 - h[i,:]))
        J += np.sum(first_term - second_term)
    
    J = J / m
    
    return J 
Example #11
Source File: 3_logistic_regression.py    From deep-learning-note with MIT License 6 votes vote down vote up
def gradientReg(theta, X, y, learningRate):
    theta = np.matrix(theta)
    X = np.matrix(X)
    y = np.matrix(y)
    
    parameters = int(theta.ravel().shape[1])
    grad = np.zeros(parameters)
    
    error = sigmoid(X * theta.T) - y
    
    for i in range(parameters):
        term = np.multiply(error, X[:,i])
        
        if (i == 0):
            grad[i] = np.sum(term) / len(X)
        else:
            grad[i] = (np.sum(term) / len(X)) + ((learningRate / len(X)) * theta[:,i])
    
    return grad 
Example #12
Source File: 4_multi_classification.py    From deep-learning-note with MIT License 6 votes vote down vote up
def predict_all(X, all_theta):
    rows = X.shape[0]
    params = X.shape[1]
    num_labels = all_theta.shape[0]
    
    # same as before, insert ones to match the shape
    X = np.insert(X, 0, values=np.ones(rows), axis=1)
    
    # convert to matrices
    X = np.matrix(X)
    all_theta = np.matrix(all_theta)
    
    # compute the class probability for each class on each training instance
    h = sigmoid(X * all_theta.T)
    
    # create array of the index with the maximum probability
    h_argmax = np.argmax(h, axis=1)
    
    # because our array was zero-indexed we need to add one for the true label prediction
    h_argmax = h_argmax + 1
    
    return h_argmax 
Example #13
Source File: synthesis.py    From controlpy with GNU General Public License v3.0 6 votes vote down vote up
def controller_lqr_discrete_from_continuous_time(A, B, Q, R, dt):
    """Solve the discrete time LQR controller for a continuous time system.
    
    A and B are system matrices, describing the systems dynamics:
     dx/dt = A x + B u
    
    The controller minimizes the infinite horizon quadratic cost function:
     cost = integral (x.T*Q*x + u.T*R*u) dt
    where Q is a positive semidefinite matrix, and R is positive definite matrix.
    
    The controller is implemented to run at discrete times, at a rate given by
     onboard_dt, 
    i.e. u[k] = -K*x(k*t)
    Discretization is done by zero order hold.
    
    Returns K, X, eigVals:
    Returns gain the optimal gain K, the solution matrix X, and the closed loop system eigenvalues.
    The optimal input is then computed as:
     input: u = -K*x
    """
    #ref Bertsekas, p.151
    
    Ad, Bd = analysis.discretise_time(A, B, dt)

    return controller_lqr_discrete_time(Ad, Bd, Q, R) 
Example #14
Source File: fisheye.py    From DualFisheye with MIT License 5 votes vote down vote up
def _render(self, xyz, rows, cols, mode):
        # Allocate Nx3 or Nx1 "1D" pixel-list (raster-order).
        img1d = np.zeros((rows*cols, self.clrs), dtype='float32')
        # Determine rendering mode:
        if mode == 'overwrite':
            # Simplest mode: Draw first, then blindly overwrite second.
            for src in self.sources:
                uv = src.get_uv(xyz)
                src.add_pixels(uv, img1d)
        elif mode == 'align':
            # Alignment mode: Draw each one at 50% intensity.
            for src in self.sources:
                uv = src.get_uv(xyz)
                src.add_pixels(uv, img1d, 0.5)
        elif mode == 'blend':
            # Linear nearest-source blending.
            uv_list = []
            wt_list = []
            wt_total = np.zeros(rows*cols, dtype='float32')
            # Calculate per-image and total weight matrices.
            for src in self.sources:
                uv = src.get_uv(xyz)
                wt = src.get_weight(uv)
                uv_list.append(uv)
                wt_list.append(wt)
                wt_total += wt
            # Render overall image using calculated weights.
            for n in range(len(self.sources)):
                wt_norm = wt_list[n] / wt_total
                self.sources[n].add_pixels(uv_list[n], img1d, wt_norm)
        else:
            raise ValueError('Invalid render mode.')
        # Convert to fixed-point image matrix and return.
        img2d = np.reshape(img1d, (rows, cols, self.clrs))
        return np.asarray(img2d, dtype=self.dtype)

    # Compute a normalized alignment score, based on size of overlap and
    # the pixel-differences in that region.  Note: Lower = Better. 
Example #15
Source File: test_analysis.py    From controlpy with GNU General Public License v3.0 5 votes vote down vote up
def test_system_norm_Hinf(self):
        for matType in [np.matrix, np.array]:
            A = matType([[-1,2],[0,-3]])
            B = matType([[0,1]]).T
            C = matType([[1,0]])
            
            hinfnorm = controlpy.analysis.system_norm_Hinf(A, B, C)
            
            hinfnorm_lmi = sys_norm_hinf_LMI(A, B, C)

            tol = 1e-3
            self.assertLess(np.linalg.norm(hinfnorm-hinfnorm_lmi), tol)
           
    #TODO:
    # - observability tests, similar to controllability 
Example #16
Source File: synthesis.py    From controlpy with GNU General Public License v3.0 5 votes vote down vote up
def controller_lqr(A, B, Q, R):
    """Solve the continuous time LQR controller for a continuous time system.
    
    A and B are system matrices, describing the systems dynamics:
     dx/dt = A x + B u
    
    The controller minimizes the infinite horizon quadratic cost function:
     cost = integral (x.T*Q*x + u.T*R*u) dt
    
    where Q is a positive semidefinite matrix, and R is positive definite matrix.
    
    Returns K, X, eigVals:
    Returns gain the optimal gain K, the solution matrix X, and the closed loop system eigenvalues.
    The optimal input is then computed as:
     input: u = -K*x
    """
    #ref Bertsekas, p.151

    #first, try to solve the ricatti equation
    X = scipy.linalg.solve_continuous_are(A, B, Q, R)
    
    #compute the LQR gain
    K = np.dot(np.linalg.inv(R),(np.dot(B.T,X)))
    
    eigVals = np.linalg.eigvals(A-np.dot(B,K))
    
    return K, X, eigVals 
Example #17
Source File: synthesis.py    From controlpy with GNU General Public License v3.0 5 votes vote down vote up
def controller_lqr_discrete_time(A, B, Q, R):
    """Solve the discrete time LQR controller for a discrete time system.
    
    A and B are system matrices, describing the systems dynamics:
     x[k+1] = A x[k] + B u[k]
    
    The controller minimizes the infinite horizon quadratic cost function:
     cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
    
    where Q is a positive semidefinite matrix, and R is positive definite matrix.
    
    Returns K, X, eigVals:
    Returns gain the optimal gain K, the solution matrix X, and the closed loop system eigenvalues.
    The optimal input is then computed as:
     input: u = -K*x
    """

    #first, try to solve the ricatti equation
    X = scipy.linalg.solve_discrete_are(A, B, Q, R)
    
    #compute the LQR gain
    K = np.dot(np.linalg.inv(np.dot(np.dot(B.T,X),B)+R),(np.dot(np.dot(B.T,X),A)))  
    
    eigVals = np.linalg.eigvals(A-np.dot(B,K))
    
    return K, X, eigVals 
Example #18
Source File: fisheye.py    From DualFisheye with MIT License 5 votes vote down vote up
def __init__(self, src_file, lens=None):
        # Load the image file, and convert to a numpy matrix.
        self._update_img(Image.open(src_file))
        # Set lens parameters.
        if lens is None:
            self.lens = FisheyeLens(self.rows, self.cols)
        else:
            self.lens = lens

    # Update image matrix and corresponding size variables. 
Example #19
Source File: test_analysis.py    From controlpy with GNU General Public License v3.0 5 votes vote down vote up
def test_scalar_hurwitz(self):
        self.assertTrue(controlpy.analysis.is_hurwitz(np.matrix([[-1]])))
        self.assertTrue(controlpy.analysis.is_hurwitz(np.array([[-1]])))
        self.assertFalse(controlpy.analysis.is_hurwitz(np.matrix([[1]])))
        self.assertFalse(controlpy.analysis.is_hurwitz(np.array([[1]]))) 
Example #20
Source File: statesp.py    From python-control with BSD 3-Clause "New" or "Revised" License 5 votes vote down vote up
def _remove_useless_states(self):
        """Check for states that don't do anything, and remove them.

        Scan the A, B, and C matrices for rows or columns of zeros.  If the
        zeros are such that a particular state has no effect on the input-output
        dynamics, then remove that state from the A, B, and C matrices.

        """

        # Search for useless states and get indices of these states.
        #
        # Note: shape from np.where depends on whether we are storing state
        # space objects as np.matrix or np.array.  Code below will work
        # correctly in either case.
        ax1_A = np.where(~self.A.any(axis=1))[0]
        ax1_B = np.where(~self.B.any(axis=1))[0]
        ax0_A = np.where(~self.A.any(axis=0))[-1]
        ax0_C = np.where(~self.C.any(axis=0))[-1]
        useless_1 = np.intersect1d(ax1_A, ax1_B, assume_unique=True)
        useless_2 = np.intersect1d(ax0_A, ax0_C, assume_unique=True)
        useless = np.union1d(useless_1, useless_2)

        # Remove the useless states.
        self.A = delete(self.A, useless, 0)
        self.A = delete(self.A, useless, 1)
        self.B = delete(self.B, useless, 0)
        self.C = delete(self.C, useless, 1)

        self.states = self.A.shape[0]
        self.inputs = self.B.shape[1]
        self.outputs = self.C.shape[0] 
Example #21
Source File: fisheye.py    From DualFisheye with MIT License 5 votes vote down vote up
def __init__(self, rows=1024, cols=1024):
        # Fisheye lens parameters.
        self.fov_deg = 180
        self.radius_px = min(rows,cols) / 2
        # Pixel coordinates of the optical axis (X,Y).
        self.center_px = np.matrix([[cols/2], [rows/2]])
        # Quaternion mapping intended to actual optical axis.
        self.center_qq = [1, 0, 0, 0] 
Example #22
Source File: test_analysis.py    From controlpy with GNU General Public License v3.0 5 votes vote down vote up
def test_system_norm_H2(self):
        for matType in [np.matrix, np.array]:
            A = matType([[-1,2],[0,-3]])
            B = matType([[0,1]]).T
            C = matType([[1,0]])
            
            h2norm = controlpy.analysis.system_norm_H2(A, B, C)
            
            h2norm_lmi = sys_norm_h2_LMI(A, B, C)

            tol = 1e-3
            self.assertLess(np.linalg.norm(h2norm-h2norm_lmi), tol) 
Example #23
Source File: test_analysis.py    From controlpy with GNU General Public License v3.0 5 votes vote down vote up
def test_time_discretisation(self):
        for matType in [np.matrix, np.array]:
            Ac = matType([[0,1],[0,0]])
            Bc = matType([[0],[1]])
             
            dt = 0.1
             
            Ad = matType([[1,dt],[0,1]])
            Bd = matType([[dt**2/2],[dt]])
            for i in range(1000):
                T = matType(np.random.normal(size=[2,2]))
                Tinv = np.linalg.inv(T)
 
                AAc = np.dot(np.dot(T,Ac),Tinv)
                BBc = np.dot(T,Bc)
 
                AAd = np.dot(np.dot(T,Ad),Tinv)
                BBd = np.dot(T,Bd)
                 
                AAd2, BBd2 = controlpy.analysis.discretise_time(AAc, BBc, dt)
                 
                self.assertLess(np.linalg.norm(AAd-AAd2), 1e-6)
                self.assertLess(np.linalg.norm(BBd-BBd2), 1e-6)
                 
                 
            #test some random systems against Euler discretisation
            nx = 20
            nu = 20
            dt = 1e-6
            tol = 1e-3
            for i in range(1000):
                Ac = matType(np.random.normal(size=[nx,nx]))
                Bc = matType(np.random.normal(size=[nx,nu]))
                 
                Ad1 = np.identity(nx)+Ac*dt
                Bd1 = Bc*dt
                 
                Ad2, Bd2 = controlpy.analysis.discretise_time(Ac, Bc, dt)
                 
                self.assertLess(np.linalg.norm(Ad1-Ad2), tol)
                self.assertLess(np.linalg.norm(Bd1-Bd2), tol) 
Example #24
Source File: losses_test.py    From object_detector_app with MIT License 5 votes vote down vote up
def testReturnsCorrectLossWithClassIndices(self):
    prediction_tensor = tf.constant([[[-100, 100, -100, 100],
                                      [100, -100, -100, -100],
                                      [100, 0, -100, 100],
                                      [-100, -100, 100, -100]],
                                     [[-100, 0, 100, 100],
                                      [-100, 100, -100, 100],
                                      [100, 100, 100, 100],
                                      [0, 0, -1, 100]]], tf.float32)
    target_tensor = tf.constant([[[0, 1, 0, 0],
                                  [1, 0, 0, 1],
                                  [1, 0, 0, 0],
                                  [0, 0, 1, 1]],
                                 [[0, 0, 1, 0],
                                  [0, 1, 0, 0],
                                  [1, 1, 1, 0],
                                  [1, 0, 0, 0]]], tf.float32)
    weights = tf.constant([[1, 1, 1, 1],
                           [1, 1, 1, 0]], tf.float32)
    # Ignores the last class.
    class_indices = tf.constant([0, 1, 2], tf.int32)
    loss_op = losses.WeightedSigmoidClassificationLoss(True)
    loss = loss_op(prediction_tensor, target_tensor, weights=weights,
                   class_indices=class_indices)

    exp_loss = np.matrix([[0, 0, -math.log(.5), 0],
                          [-math.log(.5), 0, 0, 0]])
    with self.test_session() as sess:
      loss_output = sess.run(loss)
      self.assertAllClose(loss_output, exp_loss) 
Example #25
Source File: BlurDetection.py    From python-- with GNU General Public License v3.0 5 votes vote down vote up
def _imageToMatrix(self, image):
        """
        根据名称读取图片对象转化矩阵
        :param strName:
        :return: 返回矩阵
        """
        imgMat = np.matrix(image)
        return imgMat 
Example #26
Source File: icp.py    From pointnet-registration-framework with MIT License 5 votes vote down vote up
def icp_test(S, M, tree_M, M_sampled, tree_M_sampled, S_given, gt_pose, MAX_LOOPS, ftol):
	from math import sin, cos
	import matplotlib.pyplot as plt
	from mpl_toolkits.mplot3d import Axes3D
 
	icp = ICP(M_sampled, S, tree_M_sampled)
	start = time.time()

	matrix, points, itr, neighbor_idx = icp.compute(MAX_LOOPS, ftol)		# Perform the iterations.

	end = time.time()

	final_pose = helper.find_final_pose((np.linalg.inv(matrix)).reshape((1,4,4)))

	title = "Actual T (Red->Green): "
	for i in range(len(gt_pose[0])):
		if i>2:
			title += str(round(gt_pose[0][i]*(180/np.pi),2))
		else:
			title += str(gt_pose[0][i])
		title += ', '
	title += "\nPredicted T (Red->Blue): "

	for i in range(len(final_pose[0])):
		if i>2:
			title += str(round(final_pose[0,i]*(180/np.pi),3))
		else:
			title += str(round(final_pose[0,i],3))
		title += ', '

	title += '\nElapsed Time: '+str(np.round((end-start)*1000,3))+' ms'+' & Iterations: '+str(itr)
	title += ' & Method: ICP'

	rot = matrix[0:3,0:3]
	tra = np.reshape(np.transpose(matrix[0:3,3]), (3,1))
	transformed = np.matmul(rot, np.transpose(S_given)) + tra
	return final_pose, M, S_given, transformed.T, title, end-start, itr 
Example #27
Source File: icp.py    From pointnet-registration-framework with MIT License 5 votes vote down vote up
def check_convergence(M_prev, M, ftol):
	identityT = np.eye(4)
	errorT = np.dot(M, np.linalg.inv(M_prev))
	errorT = errorT - identityT
	errorT = errorT*errorT
	error = np.sum(errorT)

	converged = False
	if error < ftol:
		converged = True
	return converged

# def find_errors(final_rot, final_trans):
# 	import transforms3d
# 	import transforms3d.euler as t3d
# 	# Simple euler distand between translation part.
# 	gt_pose = [0.5, 0.2, 0.4, 40*(np.pi/180), 20*(np.pi/180), 30*(np.pi/180)]
# 	gt_position = gt_pose[0:3]				
# 	predicted_position = final_trans
# 	translation_error = np.sqrt(np.sum(np.square(gt_position - predicted_position)))
	

# 	# Convert euler angles rotation matrix.
# 	gt_euler = gt_pose[3:6]
# 	gt_mat = t3d.euler2mat(gt_euler[2],gt_euler[1],gt_euler[0],'szyx')

# 	# Multiply inverse of one rotation matrix with another rotation matrix.

# 	error_mat = np.dot(final_rot,np.linalg.inv(gt_mat))
# 	_,angle = transforms3d.axangles.mat2axangle(error_mat)			# Convert matrix to axis angle representation and that angle is error.
	
# 	return translation_error, abs(angle*(180/np.pi)) 
Example #28
Source File: icp.py    From pointnet-registration-framework with MIT License 5 votes vote down vote up
def _icp_Rt_to_matrix(R, t):
	# matrix M = [R, t; 0, 1]
	Rt = np.concatenate((R, np.expand_dims(t.T, axis=-1)), axis=1)
	a = np.concatenate((np.zeros_like(t), np.ones(1)))
	M = np.concatenate((Rt, np.expand_dims(a, axis=0)), axis=0)
	return M 
Example #29
Source File: losses_test.py    From object_detector_app with MIT License 5 votes vote down vote up
def testReturnsCorrectAnchorWiseLoss(self):
    prediction_tensor = tf.constant([[[-100, 100, -100],
                                      [100, -100, -100],
                                      [100, 0, -100],
                                      [-100, -100, 100]],
                                     [[-100, 0, 100],
                                      [-100, 100, -100],
                                      [100, 100, 100],
                                      [0, 0, -1]]], tf.float32)
    target_tensor = tf.constant([[[0, 1, 0],
                                  [1, 0, 0],
                                  [1, 0, 0],
                                  [0, 0, 1]],
                                 [[0, 0, 1],
                                  [0, 1, 0],
                                  [1, 1, 1],
                                  [1, 0, 0]]], tf.float32)
    weights = tf.constant([[1, 1, 1, 1],
                           [1, 1, 1, 0]], tf.float32)
    alpha = tf.constant(.5, tf.float32)
    loss_op = losses.BootstrappedSigmoidClassificationLoss(
        alpha, bootstrap_type='hard', anchorwise_output=True)
    loss = loss_op(prediction_tensor, target_tensor, weights=weights)

    exp_loss = np.matrix([[0, 0, -math.log(.5), 0],
                          [-math.log(.5), 0, 0, 0]])
    with self.test_session() as sess:
      loss_output = sess.run(loss)
      self.assertAllClose(loss_output, exp_loss) 
Example #30
Source File: losses_test.py    From object_detector_app with MIT License 5 votes vote down vote up
def testReturnsCorrectAnchorWiseLoss(self):
    prediction_tensor = tf.constant([[[-100, 100, -100],
                                      [100, -100, -100],
                                      [100, 0, -100],
                                      [-100, -100, 100]],
                                     [[-100, 0, 100],
                                      [-100, 100, -100],
                                      [100, 100, 100],
                                      [0, 0, -1]]], tf.float32)
    target_tensor = tf.constant([[[0, 1, 0],
                                  [1, 0, 0],
                                  [1, 0, 0],
                                  [0, 0, 1]],
                                 [[0, 0, 1],
                                  [0, 1, 0],
                                  [1, 1, 1],
                                  [1, 0, 0]]], tf.float32)
    weights = tf.constant([[1, 1, 1, 1],
                           [1, 1, 1, 0]], tf.float32)
    loss_op = losses.WeightedSigmoidClassificationLoss(True)
    loss = loss_op(prediction_tensor, target_tensor, weights=weights)

    exp_loss = np.matrix([[0, 0, -math.log(.5), 0],
                          [-math.log(.5), 0, 0, 0]])
    with self.test_session() as sess:
      loss_output = sess.run(loss)
      self.assertAllClose(loss_output, exp_loss)