def find_error(this_model, consensus_set):
pointsL = consensus_set[:, :3]
pointsR = consensus_set[:, 3:6]
T = this_model
pointsR_maybe = utils.apply_transform(T, pointsL)
# our threshold increases with square of depth
return np.sum(mlab.vector_lengths(pointsR_maybe - pointsR))
def quiver(positions, directions, renderer=renderer):
''' lines is nx6 numpy array with each row containing position and
direction x, y, z, nx, ny, nz '''
positions = np.array(positions, dtype=np.float32) # cast all arrays to float
directions = np.array(directions, dtype=np.float32) # cast all arrays to float
# Create a vtkPoints object and store the points in it
pts = np.vstack((positions, positions + directions))
# Create a cell array to store the lines
pointids = np.arange(pts.shape[0], dtype=np.int)
# lines is nx2 array with each row containing 2 points to be connected
lines = pointids.reshape((2, -1)).T
# Create a polydata to store everything in
linesPolyData = tvtk.PolyData()
# Add the points to the dataset
linesPolyData.points = pts
# Add the lines to the dataset
linesPolyData.lines = lines
# Set line colors
# color from the vector direction
magnitudes = mlab.vector_lengths(directions, axis=1).reshape((-1, 1))
magnitudes[magnitudes == 0] = 1 # make magnitudes non zero
colors = np.abs((directions/magnitudes)) * 255
colors = colors.astype(np.uint8)
linesPolyData.cell_data.scalars = colors
linesPolyData.cell_data.scalars.name = 'colors'
return visualise(linesPolyData, renderer=renderer)
def quiver_args_from_transforms(*Targs, **kwargs):
origin = kwargs.get('origin')
xyzuvw0 = np.eye(3, 3)
xyz0 = np.zeros((3, 3))
colors = np.array([
0.8, # red
0.5, # green
0.2, # blue
])
xyzuvw = []
xyz = []
scalars = []
Tlist = [np.eye(4, 4)] if origin else []
Tlist.extend(Targs)
for T in Tlist:
xyzuvw_T = np.dot(T, np.vstack((xyzuvw0, [1, 1, 1])))[:3]
xyz_T = np.dot(T, np.vstack((xyz0, [1, 1, 1])))[:3]
xyzuvw.append(xyzuvw_T)
xyz.append(xyz_T)
scalars.append(colors)
xyzuvw = np.hstack(xyzuvw)
xyz = np.hstack(xyz)
scalars = np.hstack(scalars)
uvw = xyzuvw - xyz
u, v, w = uvw / matmlab.vector_lengths(uvw, axis=0)
x, y, z = xyz
return x, y, z, u, v, w, scalars
def vector_lengths( X, P=2., axis=None ):
"""
This function has been moved to matplotlib.mlab -- please import
it from there
"""
warnings.warn('vector_lengths has been moved to matplotlib.mlab -- please import it from there', DeprecationWarning)
import matplotlib.mlab as mlab
return mlab.vector_lengths( X, P=2., axis=axis )
def find_inliers(maybe_model, data):
pointsL = data[:, :3]
pointsR = data[:, 3:6]
T = maybe_model
pointsR_maybe = utils.apply_transform(T, pointsL)
# our threshold increases with square of depth
return (mlab.vector_lengths(pointsR_maybe - pointsR)
< 1.5*pointsR[:, 2]**2)
def vector_lengths( X, P=2., axis=None ):
"""
This function has been moved to matplotlib.mlab -- please import
it from there
"""
# deprecated from cbook in 0.98.4
warnings.warn('vector_lengths has been moved to matplotlib.mlab -- please import it from there', DeprecationWarning)
import matplotlib.mlab as mlab
return mlab.vector_lengths( X, P=2., axis=axis )
def vector_lengths(X, P=2.0, axis=None):
"""
This function has been moved to matplotlib.mlab -- please import
it from there
"""
# deprecated from cbook in 0.98.4
warnings.warn("vector_lengths has been moved to matplotlib.mlab -- please import it from there", DeprecationWarning)
import matplotlib.mlab as mlab
return mlab.vector_lengths(X, P=2.0, axis=axis)
def show_quads(positions, directions, size):
''' Represents points and normals as small plane patches (quads)
is positions is plane centre and directions is plane normals '''
D = size # size of plane patches
#pts = np.array( [ (0, 0, 0), (X, 0, 0), (X, X, 0), (0, X, 0) ] )
#quads = np.array( [ (0, 1, 2, 3), (0, 1, 2, 3)] )
# TODO convert loop to array operations
pts = []
for pos, direction in zip(positions, directions):
direction = (direction/ np.linalg.norm(direction)) # normalize direction vector
# calculate the vectors U, V which are in plane vectors perpendicular
# to the normal and projected into the xz, yz and xy planes
UVW = 0.5*D*np.array([(direction[2] , 0 , -direction[0]),
(0 , direction[2] , -direction[1]),
(direction[1] , -direction[0] , 0),
])
lens = mlab.vector_lengths(UVW, axis=1)
# one of them might be very short so use the other two
min_ind = np.argmin(lens)
UVW = np.delete(UVW, min_ind, axis=0)
U = UVW[0]
V = UVW[1]
# normalize
U /= np.linalg.norm(U)
V /= np.linalg.norm(V)
#C = np.cross(U, V) # should equal direction
# Create four points (must be in counter clockwise order)
pts.append(pos - U - V)
pts.append(pos + U - V)
pts.append(pos + U + V)
pts.append(pos - U + V)
pts = np.array(pts)
pointids = np.arange(pts.shape[0], dtype=np.int)
quads = pointids.reshape((-1, 4))
# Create a polydata to store everything in
polydata = tvtk.PolyData(points=pts, polys=quads)
# TODO this section has been copy pasted merge at some point
# Set colors from the vector direction
color_polydata_by_normals(polydata, directions, alpha=0)
return visualise(polydata, renderer=renderer)
def compute_errors_given_transforms(Tlistgroundtruth, Tlistartoolkit,
scaling=False):
ground_truth_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistgroundtruth])
artoolkit_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistartoolkit])
if scaling:
Tlistartoolkit = scale_and_translate(Tlistgroundtruth, Tlistartoolkit)
artoolkit_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistartoolkit])
trans_err = matmlab.vector_lengths(
ground_truth_pointcloud - artoolkit_pointcloud, axis=1)
rot_err = np.asarray(
[180 / np.pi * utils.angle_between_rotations(Tgt[:3, :3], Tml[:3,:3])
for Tgt, Tml in zip(Tlistgroundtruth, Tlistartoolkit)])
rot_err = np.abs(rot_err - np.mean(rot_err))
return ground_truth_pointcloud, artoolkit_pointcloud, trans_err, rot_err
def rgbs_by_normals(normals, alpha=0):
assert np.all(np.isreal(normals)), "normals not real"
magnitudes = mlab.vector_lengths(normals, axis=1).reshape((-1, 1))
magnitudes[magnitudes == 0] = 1
unit_normals = normals / magnitudes
# ## Coloring by hue space. Hue space is nice because it is cyclically
# # continuous, but my conversion from hsv to rgb is bad because it is not
# # continuous. FIX HSV to RGB with a continuous version. However, we need a
# # hemispherical colorspace here, not cylinderical.
# hue_normal = [0, 0, 1]
# # This clips normals to hemisphere in 3D around the hue_normal, but
# # creates symmetrical contours around the normal, which is not what we
# # want. However, to do any better we would need a spherical color space.
# # dot product is from [-1, 1], scale and shift it to [0, 1]
# h1 = np.dot(unit_normals, hue_normal) * 0.5 + 0.5
# assert np.all((h1 >= 0) & (h1 <= 1.0))
# # calling the above value hue is good because we have continuous circular
# # space for hue, which avoids discontinuities.
# #sat_normal = [0, 1, 0]
# #s = np.dot(unit_normals, sat_normal) * 0.5 + 0.5
# s = np.ones_like(h1) * 0.3
# v = np.ones_like(h1) * 0.7
# rgb = colorsys_hsv_to_rgb(h1, s, v)
# colors = (rgb * 255).astype(np.uint8)
# # To get transparency effects
# colors = np.empty((unit_normals.shape[0], 4), np.uint8)
# colors[3] = alpha
# # Simple absolute normal strategy
# colors = np.abs(unit_normals) * 255
# flip the normals. Choose the flipping plane carefully.
flipping_plane = [0, 0, 1]
unit_normals[np.dot(unit_normals, flipping_plane) < 0] *= -1
extrap_norms = extrapolate_hemisphere_to_sphere(unit_normals)
colors = extrap_norms * 127 + 128
assert np.all(np.isreal(colors)), "Colors is not real"
return np.asarray(colors, dtype=np.uint8)
if len(img_pos_target0) > max_lines:
img_pos_target0 = img_pos_target0[:max_lines]
img_pos_target1 = img_pos_target1[:max_lines]
target_loc0 = projectionto3d(K0, img_pos_target0)
target_loc0 = utils.apply_transform(T0, target_loc0)
origin0 = utils.apply_transform(T0, np.zeros(3))
_plot_lines(origin0, target_loc0, 10)
target_loc1 = projectionto3d(K1, img_pos_target1)
target_loc1 = utils.apply_transform(T1, target_loc1)
origin1 = utils.apply_transform(T1, np.zeros(3))
_plot_lines(origin1, target_loc1, 10)
if target_img_patch is not None and target_loc3d is not None:
pass
# add _plot_img for the patch at target_loc3d with normal along z-axis
elif target_loc3d is not None:
target_loc3d = np.asarray(target_loc3d).reshape(-1, 3)
if len(target_loc3d) > 4:
venlens = matmlab.vector_lengths(target_loc3d)
filtered = venlens < 10
else:
filtered = np.ones(len(target_loc3d), dtype=np.bool)
mlab.points3d(target_loc3d[filtered, 0],
target_loc3d[filtered, 1],
target_loc3d[filtered, 2], scale_factor=.01)
obj.scene.disable_render = False
if __name__ == '__main__':
pass
