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)))
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)))
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
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
