예제 #1
0
파일: icp.py 프로젝트: chielk/robot_eyes
def compute_rms(source, target, nn=None):
    '''
    Make a single call to FLANN rms.

    If a flannobject with prebuilt index is given, use that,
    otherwise, do a full search.
    '''
    if nn:
        results, dists = nn.match(target)
    else:
        nn = NN()
        nn.add(source)
        results, dists = nn.match(target)
    return math.sqrt(sum(dists) / float(len(dists)))
예제 #2
0
def compute_rms(source, target, nn=None):
    '''
    Make a single call to FLANN rms.

    If a flannobject with prebuilt index is given, use that,
    otherwise, do a full search.
    '''
    if nn:
        results, dists = nn.match(target)
    else:
        nn = NN()
        nn.add(source)
        results, dists = nn.match(target)
    return math.sqrt(sum(dists) / float(len(dists)))
예제 #3
0
import numpy as np
from nn import NN, Layer

X = np.array([[0, 0], [0, 1], [1, 0], [1, 1]])

Y = np.array([[0], [1], [1], [0]])

nn = NN(2)
nn.add(Layer(3))
nn.add(Layer(1))

print("Predictions before training")
print(nn.feed_forward(X))
print()

nn.train(X, Y)

print()
print("Predictions after training")
print(nn.feed_forward(X))
예제 #4
0
파일: icp.py 프로젝트: chielk/robot_eyes
def icp(source, target, D=3, verbosity=0, epsilon=0.000001):
    '''
    Perform ICP for two arrays containing points. Note that these
    arrays must be row-major!

    NOTE:
    This function returns the rotation matrix, translation for a
    transition FROM target TO source. This approach was chosen because
    of computational efficiency: it is now possible to index the source
    points beforehand, and query the index for matches from the target.

    In other words: Each new point gets matched to an old point. This is
    quite intuitive, as the set of source points may be (much) larger.
    '''
    nn = NN()

    # init R as identity, t as zero
    R = np.eye(D, dtype='float64')
    t = np.zeros((1, D), dtype='float64')
    T = homogenize_transformation(R, t)
    transformed_target = target

    # Build index beforehand for faster querying
    nn.add(source)

    # Initialize rms to bs values
    rms = 2
    rms_new = 1

    while True:
        # Update root mean squared error
        rms = rms_new

        # Rotate and translate the target using homogeneous coordinates
        # unused: transformed_target = np.dot(T, target_h).T[:, :D]
        transformed_target = np.dot(R, transformed_target.T).T + t
        centroid_transformed_target = np.mean(transformed_target, axis=0)

        # Use flann to find nearest neighbours. Note that because of index it
        # means 'for each transformed_target find the corresponding source'
        results, dists = nn.match(transformed_target)

        # Compute new RMS
        rms_new = math.sqrt(sum(dists) / float(len(dists)))

        # Give feedback if necessary
        if verbosity > 0:
            sys.stdout.write("\rRMS: {}".format(rms_new))
            sys.stdout.flush()

        # We find this case some times, but are not sure if it should be
        # possible. Is it possible for the RMS of a (sub)set of points to
        # increase?
        # assert rms > rms_new, "RMS was not minimized?"

        # Check threshold
        if rms - rms_new < epsilon:
            break

        # Use array slicing to get the correct targets
        selected_source = nn.get(results)
        centroid_selected_source = np.mean(selected_source, axis=0)

        # Compute covariance, perform SVD using Kabsch algorithm
        correlation = np.dot(
            (transformed_target - centroid_transformed_target).T,
            (selected_source - centroid_selected_source))
        u, s, v = np.linalg.svd(correlation)

        # u . S . v = correlation =
        # V . S . W.T

        # Ensure righthandedness coordinate system and calculate R
        d = np.linalg.det(np.dot(v, u.T))
        sign_matrix = np.eye(D)
        sign_matrix[D-1, D-1] = d
        R = np.dot(np.dot(v.T, sign_matrix), u.T)
        t[0, :] = np.dot(R, -centroid_transformed_target) + \
            centroid_selected_source

        # Combine transformations so far with new found R and t
        # Note: Latest transformation should be on inside (r) of the equation
        T = np.dot(T, homogenize_transformation(R, t))

        if verbosity > 2:
            try:
                if raw_input("Enter 'q' to quit, or anything else to" +
                             "continue") == "q":
                    sys.exit(0)
            except EOFError:
                print("")
                sys.exit(0)
            except KeyboardInterrupt:
                print("")
                sys.exit(0)

    # Unpack the built transformation matrix
    R, t = dehomogenize_transformation(T)
    return R, t, T, rms_new, nn
예제 #5
0
def icp(source, target, D=3, verbosity=0, epsilon=0.000001):
    '''
    Perform ICP for two arrays containing points. Note that these
    arrays must be row-major!

    NOTE:
    This function returns the rotation matrix, translation for a
    transition FROM target TO source. This approach was chosen because
    of computational efficiency: it is now possible to index the source
    points beforehand, and query the index for matches from the target.

    In other words: Each new point gets matched to an old point. This is
    quite intuitive, as the set of source points may be (much) larger.
    '''
    nn = NN()

    # init R as identity, t as zero
    R = np.eye(D, dtype='float64')
    t = np.zeros((1, D), dtype='float64')
    T = homogenize_transformation(R, t)
    transformed_target = target

    # Build index beforehand for faster querying
    nn.add(source)

    # Initialize rms to bs values
    rms = 2
    rms_new = 1

    while True:
        # Update root mean squared error
        rms = rms_new

        # Rotate and translate the target using homogeneous coordinates
        # unused: transformed_target = np.dot(T, target_h).T[:, :D]
        transformed_target = np.dot(R, transformed_target.T).T + t
        centroid_transformed_target = np.mean(transformed_target, axis=0)

        # Use flann to find nearest neighbours. Note that because of index it
        # means 'for each transformed_target find the corresponding source'
        results, dists = nn.match(transformed_target)

        # Compute new RMS
        rms_new = math.sqrt(sum(dists) / float(len(dists)))

        # Give feedback if necessary
        if verbosity > 0:
            sys.stdout.write("\rRMS: {}".format(rms_new))
            sys.stdout.flush()

        # We find this case some times, but are not sure if it should be
        # possible. Is it possible for the RMS of a (sub)set of points to
        # increase?
        # assert rms > rms_new, "RMS was not minimized?"

        # Check threshold
        if rms - rms_new < epsilon:
            break

        # Use array slicing to get the correct targets
        selected_source = nn.get(results)
        centroid_selected_source = np.mean(selected_source, axis=0)

        # Compute covariance, perform SVD using Kabsch algorithm
        correlation = np.dot(
            (transformed_target - centroid_transformed_target).T,
            (selected_source - centroid_selected_source))
        u, s, v = np.linalg.svd(correlation)

        # u . S . v = correlation =
        # V . S . W.T

        # Ensure righthandedness coordinate system and calculate R
        d = np.linalg.det(np.dot(v, u.T))
        sign_matrix = np.eye(D)
        sign_matrix[D - 1, D - 1] = d
        R = np.dot(np.dot(v.T, sign_matrix), u.T)
        t[0, :] = np.dot(R, -centroid_transformed_target) + \
            centroid_selected_source

        # Combine transformations so far with new found R and t
        # Note: Latest transformation should be on inside (r) of the equation
        T = np.dot(T, homogenize_transformation(R, t))

        if verbosity > 2:
            try:
                if raw_input("Enter 'q' to quit, or anything else to" +
                             "continue") == "q":
                    sys.exit(0)
            except EOFError:
                print("")
                sys.exit(0)
            except KeyboardInterrupt:
                print("")
                sys.exit(0)

    # Unpack the built transformation matrix
    R, t = dehomogenize_transformation(T)
    return R, t, T, rms_new, nn