def add_coordinate_transform(ax,R,t,size=1.0,*args,**kwargs):
"""Draws a coordinate transform on the plot ax"""
axes = so3.matrix(so3.transpose(R))
colors = ['r','g','b']
for (v,c) in zip(axes,colors):
a = Arrow3D(t, vectorops.madd(t,v,size), lw=1, color=c, *args, **kwargs)
ax.add_artist(a)
def save_pcd_new(dev, name, number, transformCameraInWorld):
st0 = time.time()
dev.wait_for_frames()
color_ori = deepcopy(dev.cad)
position_ori = deepcopy(dev.points)
color = np.reshape(np.array(color_ori), (640 * 480, 3)) / 255.0
pos_ori = np.reshape(np.array(position_ori), (640 * 480, 3))
xyzrgb_ori = np.hstack((pos_ori, color))
# core!
#xyzrgb_ori = xyzrgb_ori[np.all(xyzrgb_ori != 0, axis = 1)]
#xyzrgb_ori = xyzrgb_ori[xyzrgb_ori[:,0:3]!=np.array([0,0,0])]
#xyzrgb_ori = xyzrgb_ori[xyzrgb_ori[:,3]!=0 or xyzrgb_ori[:,4]!=0 or xyzrgb_ori[:,5]!=0]
num_points = xyzrgb_ori.shape[0]
pos_ori = xyzrgb_ori[:, :3].T
color = xyzrgb_ori[:, 3:]
print('[*]get xyzrgb of shape %s' % str(xyzrgb_ori.shape))
pos_4item = np.vstack((pos_ori, np.ones((1, num_points))))
R_array = np.array(so3.matrix(transformCameraInWorld[0]))
T_array = np.reshape(np.array(transformCameraInWorld[1]), (3, 1))
Trans_array = np.vstack((np.hstack(
(R_array, T_array)), np.array([0, 0, 0, 1])))
pos_trans = np.dot(Trans_array, pos_4item)[:3, :]
xyzrgb = np.hstack((pos_trans.T, color))
new_xyzrgb_path = name + str(number) + '_new.npy'
np.save(new_xyzrgb_path, xyzrgb)
st3 = time.time()
print('[*]saving pcd with time: %f s' % (st3 - st0))
def axis_rotation_magnitude(R, a):
"""Given a rotation matrix R and a unit axis a, determines how far R rotates
about a. Specifically, if R = from_axis_angle((a,v)) this should return v (modulo pi).
"""
cterm = so3.trace(R) - vectorops.dot(a, so3.apply(R, a))
mR = so3.matrix(R)
sterm = -(a[0] * (mR[1][2] - mR[2][1]) + a[1] *
(mR[2][0] - mR[0][2]) + a[2] * (mR[0][1] - mR[1][0]))
return math.atan2(sterm, cterm)
def getSlice(self,p):
"""Gets the wrench space slice at the given point of application. Return value is a 3D Volume
whose coordinates are the force dimensions f of the wrench
[f ]
[p x f].
"""
v = Volume()
W = np.vstack((np.eye(3),so3.matrix(so3.cross_product(p))))
for c in self.convex_decomposition:
if isinstance(c,(VPolytope,HPolytope)):
cslice = c.slice(W)
else:
cslice = slice_hull(c,W)
v.convex_decomposition.append(cslice)
return v
mu = interpolate_linear(m1, m2, u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu, angle)
return so3.rotation(axis, angle)
def interpolate_transform(T1, T2, u):
"""Interpolate linearly between the two transformations T1 and T2."""
return (interpolate_rotation(T1[0], T2[0],
u), interpolate_linear(T1[1], T2[1], u))
Ra = so3.rotation((0, 1, 0), math.pi * -0.9)
Rb = so3.rotation((0, 1, 0), math.pi * 0.9)
print("Angle between")
print(so3.matrix(Ra))
print("and")
print(so3.matrix(Rb))
print("is", so3.distance(Ra, Rb))
Ta = [Ra, [-1, 0, 1]]
Tb = [Rb, [1, 0, 1]]
if __name__ == "__main__":
vis.add("world", se3.identity())
vis.add("start", Ta)
vis.add("end", Tb)
vis.add("interpolated", Ta)
vis.edit("start")
vis.edit("end")
vis.setAttribute("world", "length", 0.25)
vis.setAttribute("interpolated", "fancy", True)
def to_numpy(obj, type='auto'):
"""Converts a Klamp't object to a numpy array or multiple numpy arrays.
Supports:
* lists and tuples
* RigidTransform: returned as 4x4 homogeneous coordinate transform
* Matrix3, Rotation: returned as 3x3 matrix. Can't be determined
with 'auto', need to specify type='Matrix3' or 'Rotation'.
* Configs
* Trajectory: returns a pair (times,milestones)
* TriangleMesh: returns a pair (verts,indices)
* PointCloud: returns a n x (3+k) array, where k is the # of properties
* VolumeGrid: returns a triple (bmin,bmax,array)
* Geometry3D: returns a pair (T,geomdata)
If you want to get a transformed point cloud or mesh, you can pass in a
Geometry3D as the obj, and its geometry data type as the type.
"""
global supportedTypes
if type == 'auto':
otype = types.objectToTypes(obj)
if isinstance(otype, (list, tuple)):
for t in otype:
if t in supportedTypes:
type = t
break
if type == 'auto':
raise ValueError('obj is not a supported type: ' +
', '.join(otype))
else:
type = otype
if type not in supportedTypes:
raise ValueError(type + ' is not a supported type')
if type == 'RigidTransform':
return np.array(se3.homogeneous(obj))
elif type == 'Rotation' or type == 'Matrix3':
return np.array(so3.matrix(obj))
elif type == 'Trajectory':
return np.array(obj.times), np.array(obj.milestones)
elif type == 'TriangleMesh':
from klampt import Geometry3D
if isinstance(obj, Geometry3D):
res = to_numpy(obj.getTriangleMesh(), type)
R = to_numpy(obj.getCurrentTransform()[0], 'Matrix3')
t = to_numpy(obj.getCurrentTransform()[1], 'Vector3')
return (np.dot(R, res[0]) + t, res[1])
return (np.array(obj.vertices).reshape((len(obj.vertices) // 3, 3)),
np.array(obj.indices, dtype=np.int32).reshape(
(len(obj.indices) // 3, 3)))
elif type == 'PointCloud':
from klampt import Geometry3D
if isinstance(obj, Geometry3D):
res = to_numpy(obj.getPointCloud(), type)
R = to_numpy(obj.getCurrentTransform()[0], 'Matrix3')
t = to_numpy(obj.getCurrentTransform()[1], 'Vector3')
res[:, :3] = np.dot(R, res[:, :3]) + t
return res
points = np.array(obj.vertices).reshape((obj.numPoints(), 3))
if obj.numProperties() == 0:
return points
properties = np.array(obj.properties).reshape(
(obj.numPoints(), obj.numProperties()))
return np.hstack((points, properties))
elif type == 'VolumeGrid':
bmin = np.array(obj.bbox)[:3]
bmax = np.array(obj.bbox)[3:]
values = np.array(obj.values).reshape(
(obj.dims[0], obj.dims[1], obj.dims[2]))
return (bmin, bmax, values)
elif type == 'Geometry3D':
if obj.type() == 'PointCloud':
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), to_numpy(
obj.getPointCloud(), obj.type())
elif obj.type() == 'TriangleMesh':
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), to_numpy(
obj.getTriangleMesh(), obj.type())
elif obj.type() == 'VolumeGrid':
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), to_numpy(
obj.getVolumeGrid(), obj.type())
elif obj.type() == 'Group':
arrays = []
for i in range(obj.numElements()):
arrays.append(to_numpy(obj.getElement(i), 'Geometry3D'))
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), arrays
else:
return np.array(obj)
mu = interpolate_linear(m1, m2, u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu, angle)
return so3.rotation(axis, angle)
def interpolate_transform(T1, T2, u):
"""Interpolate linearly between the two transformations T1 and T2."""
return (interpolate_rotation(T1[0], T2[0],
u), interpolate_linear(T1[1], T2[1], u))
Ra = so3.rotation((0, 1, 0), math.pi * -0.9)
Rb = so3.rotation((0, 1, 0), math.pi * 0.9)
print "Angle between"
print so3.matrix(Ra)
print "and"
print so3.matrix(Rb)
print "is", so3.distance(Ra, Rb)
Ta = [Ra, [-1, 0, 1]]
Tb = [Rb, [1, 0, 1]]
if __name__ == "__main__":
vis.add("world", se3.identity())
vis.add("start", Ta)
vis.add("end", Tb)
vis.add("interpolated", Ta)
vis.edit("start")
vis.edit("end")
vis.setAttribute("world", "length", 0.25)
vis.setAttribute("interpolated", "fancy", True)
def save_pcd(dev, name, number, transformCameraInWorld):
st0 = time.time()
dev.wait_for_frames()
color_ori = deepcopy(dev.cad)
position_ori = deepcopy(dev.points)
color = np.reshape(np.array(color_ori), (640 * 480, 3)) / 255.0
pos_ori = np.reshape(np.array(position_ori), (640 * 480, 3))
xyzrgb_ori = np.hstack((pos_ori, color))
xyzrgb_ori = xyzrgb_ori[np.all(xyzrgb_ori != 0, axis=1)]
num_points = xyzrgb_ori.shape[0]
pos_ori = xyzrgb_ori[:, :3].T
color = xyzrgb_ori[:, 3:]
#print(pos_ori.shape)
pos_4item = np.vstack((pos_ori, np.ones((1, num_points))))
R_array = np.array(so3.matrix(transformCameraInWorld[0]))
T_array = np.reshape(np.array(transformCameraInWorld[1]), (3, 1))
Trans_array = np.vstack((np.hstack(
(R_array, T_array)), np.array([0, 0, 0, 1])))
pos_trans = np.dot(Trans_array, pos_4item)[:3, :]
xyzrgb = np.hstack((pos_trans.T, color))
new_xyzrgb_path = name + str(number) + '_new.xyzrgb'
st1 = time.time()
np.savetxt(new_xyzrgb_path, xyzrgb, delimiter=' ')
st2 = time.time()
np.save('test.npy', xyzrgb)
st3 = time.time()
print(
'saving pcd with process time: %f s, save new pcd with time: %f s, npy with time: %f s'
% (st1 - st0, st2 - st1, st3 - st2))
cad = color_ori #size is 480 x 640 x 3
p = position_ori
print '----------------------------saving data--------------------------------------------'
data = open(name + str(number) + '.xyzrgb', 'w')
count = 0
for i in range(480):
for j in range(640):
ptC = cad[i][j]
pt = p[i][j]
if vectorops.norm(pt) > 0 and vectorops.norm_L1(ptC) > 0:
ptW = se3.apply(transformCameraInWorld, pt)
data.write(
str(ptW[0]) + ' ' + str(ptW[1]) + ' ' + str(ptW[2]) + ' ')
data.write(
str(float(ptC[0]) / 255.0) + ' ' +
str(float(ptC[1]) / 255.0) + ' ' +
str(float(ptC[2]) / 255.0) + '\n')
count = count + 1
data.close()
et = time.time()
print(count)
print('saving pcd with time: %f s' % (et - st3))
T = np.eye(4)
T[:3, :3] = C
T[:3, 3] = t
out_data.append(list(T[0]) + list(T[1]) + list(T[2]) + list(T[3]))
"""
TT = [0.492127,0.003738762,0.08270439,-0.02360799,0.7022632,-0.3517123,-0.5801101,-0.2158636] # from advio-04
TT = [0.514813,-0.005759389,-0.09049783,-0.001374606,0.64807,-0.264358,0.6264483,0.343049] # a complete line from advio-23
A = TT[4: 8]
B = so3.from_quaternion(A)
print(B)
C = so3.matrix(B)
print(C)
t = TT[1: 4]
T = np.eye(4)
T[:3, :3] = C
T[:3, 3] = t
print(T)
print([list(T[0]), list(T[1]), list(T[2]), list(T[3])])
print(np.array([list(T[0]), list(T[1]), list(T[2]), list(T[3])]))
print(list(T[0]) + list(T[1]) + list(T[2]) + list(T[3]))
print(list(T[0]) + list(T[1]) + list(T[2]))
#"""
