def dualquat_from_twist(tw):
ang = np.linalg.norm(tw[:3])
if ang < np.finfo(np.float32).eps:
return dualquat(quat.identity(), tw[3:])
return dualquat(quat(ang, tw[:3] / ang), tw[3:])
def transform_to_screw(translation, quat_in_wxyz, tol=1e-6):
dq = dq3d.dualquat(dq3d.quat(quat_as_xyzw(quat_in_wxyz)), translation)
screw = dual_quaternion_to_screw(dq, tol)
return screw
import copy
import numpy as np
import open3d as o3
from probreg import filterreg
from probreg import callbacks
from probreg import transformation
from dq3d import dualquat, quat
n_points = 30
points = np.array([[i * 0.05, 0.0, 0.0] for i in range(n_points)])
tfs = [(quat(np.deg2rad(0.0), np.array([0.0, 0.0, 1.0])), np.array([0.0, 0.0, 0.0])),
(quat(np.deg2rad(30.0), np.array([0.0, 0.0, 1.0])), np.array([0.0, 0.0, 0.3]))]
dqs = [dualquat(t[0], t[1]) for t in tfs]
ws = transformation.DeformableKinematicModel.SkinningWeight(n_points)
for i in range(n_points):
ws['pair'][i][0] = 0
ws['pair'][i][1] = 1
for i in range(n_points):
ws['val'][i][0] = float(i) / n_points
ws['val'][i][1] = 1.0 - float(i) / n_points
dtf = transformation.DeformableKinematicModel(dqs, ws)
tf_points = dtf.transform(points)
source = o3.geometry.PointCloud()
source.points = o3.utility.Vector3dVector(points)
target = o3.geometry.PointCloud()
target.points = o3.utility.Vector3dVector(tf_points)
cbs = [callbacks.Open3dVisualizerCallback(source, target)]
cv = lambda x: np.asarray(x.points if isinstance(x, o3.geometry.PointCloud) else x)
reg = filterreg.DeformableKinematicFilterReg(cv(source), ws, 0.01)
import numpy as np
import transformations as tf
from dq3d import quat, dualquat
from dq3d import op
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d.axes3d as p3
import matplotlib.animation as animation
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
mids = []
euler1 = np.random.rand(3) * 2.0 * np.pi - np.pi
euler2 = np.random.rand(3) * 2.0 * np.pi - np.pi
dq1 = dualquat(quat(*tf.quaternion_from_euler(*euler1)), np.random.rand(3))
dq2 = dualquat(quat(*tf.quaternion_from_euler(*euler2)), np.random.rand(3))
def update_anim(i):
plt.cla()
t = i * 0.01
mid = op.sclerp(dq1, dq2, t)
pts = np.array([dq1.translation(), dq2.translation()])
mids.append(mid.translation())
mids_arr = np.array(mids)
ax.scatter(pts[:, 0], pts[:, 1], pts[:, 2])
ax.scatter(mids_arr[:, 0], mids_arr[:, 1], mids_arr[:, 2])
ax.set_xlim(0, 1)
ax.set_ylim(0, 1)
ax.set_zlim(0, 1)