def trueParallelTransport(x, v, w, t):
v3D = rs.chartVelocityTo3D(x, v)
x3D = rs.localChartTo3D(x)
w3D = rs.chartVelocityTo3D(x, w)
n = np.linalg.norm(v3D)
if n < 1e-10:
return w3D
squaredNorm = np.dot(v3D, v3D)
x3DFinal = computeGeodesic(x, v, t)
v3DFinal = computeGeodesicVelocity(x, v, t)
print(np.dot(w3D, w3D) - np.dot(w3D, v3D) / squaredNorm)
sign = np.sign(np.dot(w3D, np.cross(x3D, v3D)))
truepw3D = v3DFinal * np.dot(w3D, v3D) / (squaredNorm) + sign * np.sqrt(
np.dot(w3D, w3D) - np.dot(w3D, v3D)**2 / squaredNorm) * np.cross(
x3DFinal, v3DFinal / n)
return truepw3D
#Update the estimate
Jacobi = Jacobi + Weights[i] * xPerturbed
xtraj[k + 1] = xcurr
alphatraj[k + 1] = Jacobi / (epsilon * delta)
return xtraj, alphatraj
x = np.array([1., 0.8])
x3D = rs.localChartTo3D(x)
direction = np.array([0., 1.])
v = np.pi * direction / np.sqrt(
np.dot(direction, co_vector_from_vector(x, direction))) / 10.
alpha = co_vector_from_vector(x, v)
#true endpoint and parallel vector
v3D = rs.chartVelocityTo3D(x, v)
n = np.linalg.norm(v3D)
x3DFinal = np.cos(n) * x3D + np.sin(n) * v3D / n
v3DFinal = -np.sin(n) * n * x3DFinal + np.cos(n) * v3D
errors = []
steps = [elt * 50 for elt in range(30, 200)]
nb = [1. / elt for elt in steps]
for step in steps:
xtraj, alphatraj = compute_geodesic(x, alpha, step)
errors.append(np.linalg.norm(rs.localChartTo3D(xtraj[-1]) - x3DFinal))
print rs.localChartTo3D(xtraj[-1]), x3DFinal
print "Error when self-transporting :", errors[-1]
plt.scatter(nb,
errors,
def computeGeodesic(x, v, t):
x3D = rs.localChartTo3D(x)
v3D = rs.chartVelocityTo3D(x, v)
norm = np.linalg.norm(v3D)
return np.cos(t * norm) * x3D + np.sin(t * norm) * v3D / norm
x = [math.pi / 4, 0.]
v = np.array([2.9616, 1.4810]) / 2.
alpha = [1.4808, 0.37025]
vortho = np.array([-v[1], v[0] / np.sin(x[0])**2])
w = [alpha[1], -alpha[0]]
w = vortho + v
# x = [math.pi/4,0.]
# v = np.array([2.1, 1.4810])/2.
# alpha = co_vector_from_vector(x,v)
# print("alpha",alpha)
# vortho = np.array([-v[1], v[0]/np.sin(x[0])**2])
# w=[alpha[1], -alpha[0]]
v3D = rs.chartVelocityTo3D(x, v)
x3D = rs.localChartTo3D(x)
w3D = rs.chartVelocityTo3D(x, w)
#true end point, end velocity and parallel transport
x3DFinal = computeGeodesic(x, v, 1.)
v3DFinal = computeGeodesicVelocity(x, v, 1.)
pw3D = trueParallelTransport(x, v, w, 1.)
print(x3D)
print("w3D", w3D)
print("v3DInitial", v3D, "v3DFinal", v3DFinal)
print("x3DInitial", x3D, "x3DFinal", x3DFinal)
print("initial w :", w3D, "pw :", pw3D)
steps = [i for i in range(10, 200, 3)]
# steps = [int(i) for i in np.linspace(10,100)]