163 lines
7.3 KiB
Python
163 lines
7.3 KiB
Python
#!/usr/bin/env python3
|
|
"""Build the virtual-centerline construction figure (fig_center.pdf).
|
|
|
|
Three panels, following subsec:centerline of the paper:
|
|
|
|
(a) clearance field of the free space (distance transform), the EVG-thin
|
|
skeleton ridge extracted from it, and the coarse A* path on the skeleton
|
|
graph from the entrance to the loading bay.
|
|
(b) the local tangent--normal rectangular corridors of
|
|
eq:local_corridor_tangent / eq:local_corridor_normal built around each
|
|
resampled reference point, and the centerline returned by the FEM-pos QP
|
|
smoother of eq:qp_problem, overlaid on the coarse path.
|
|
(c) the discrete curvature profile before and after smoothing, obtained from
|
|
the second difference ||d_i|| = ell^2 kappa_i + O(ell^4), against the
|
|
bound kappa_max = 1 / R_min imposed by eq:qp_curvature.
|
|
|
|
Construction illustration on a synthetic site -- not an experimental result.
|
|
"""
|
|
import numpy as np
|
|
import matplotlib
|
|
matplotlib.use("Agg")
|
|
import matplotlib.pyplot as plt
|
|
from matplotlib.patches import Polygon as MplPolygon, Rectangle
|
|
|
|
import sitegeom as sg
|
|
|
|
R_MIN = 12.0
|
|
KAP_MAX = 1.0 / R_MIN
|
|
HALF_W = 1.6 # lateral corridor half-width used by qp_smooth
|
|
S_HALF = 0.5 # tangential corridor half-length, for illustration
|
|
|
|
|
|
def curvature(P):
|
|
"""Discrete curvature via second differences, on P's own spacing."""
|
|
seg = np.hypot(*np.diff(P, axis=0).T)
|
|
ell = 0.5 * (seg[:-1] + seg[1:])
|
|
d2 = P[:-2] - 2.0 * P[1:-1] + P[2:]
|
|
kap = np.hypot(d2[:, 0], d2[:, 1]) / ell ** 2
|
|
s = np.concatenate([[0.0], np.cumsum(seg)])
|
|
return s[1:-1], kap
|
|
|
|
|
|
def main():
|
|
g = sg.build_centerline()
|
|
xs, ys, inside, dist = g["xs"], g["ys"], g["inside"], g["dist"]
|
|
skel, raw, center = g["skel"], g["raw"], g["center"]
|
|
|
|
coarse = sg.resample(raw, 1.0)
|
|
t_c, n_c = sg.tangent_normal(coarse)
|
|
|
|
s_raw, k_raw = curvature(coarse)
|
|
s_sm, k_sm = curvature(center)
|
|
print("coarse |kappa| max %.4f -> R %.2f m" % (k_raw.max(), 1 / k_raw.max()))
|
|
print("smooth |kappa| max %.4f -> R %.2f m" % (k_sm.max(), 1 / k_sm.max()))
|
|
print("bound kappa_max %.4f -> R %.2f m" % (KAP_MAX, R_MIN))
|
|
print("smoothed violates bound: %s" % bool((k_sm > KAP_MAX + 1e-9).any()))
|
|
print("coarse length %.2f m / smoothed length %.2f m"
|
|
% (np.hypot(*np.diff(coarse, axis=0).T).sum(),
|
|
np.hypot(*np.diff(center, axis=0).T).sum()))
|
|
print("max lateral deviation from coarse %.2f m"
|
|
% np.abs(((center - coarse) * n_c).sum(axis=1)).max())
|
|
|
|
plt.rcParams.update({
|
|
"font.family": "serif",
|
|
"font.serif": ["Nimbus Roman", "Times New Roman", "DejaVu Serif"],
|
|
"font.size": 7,
|
|
"mathtext.fontset": "stix",
|
|
"axes.linewidth": 0.5,
|
|
"pdf.fonttype": 42,
|
|
})
|
|
fig = plt.figure(figsize=(7.16, 1.95), constrained_layout=True)
|
|
gs = fig.add_gridspec(1, 3, width_ratios=[1.0, 1.0, 0.72])
|
|
ax0, ax1, ax2 = (fig.add_subplot(gs[0]), fig.add_subplot(gs[1]),
|
|
fig.add_subplot(gs[2]))
|
|
|
|
# ---- (a) clearance field, skeleton, coarse A* --------------------------
|
|
dm = np.ma.array(dist, mask=~inside)
|
|
im = ax0.pcolormesh(xs, ys, dm, cmap="Blues", shading="auto",
|
|
rasterized=True, vmin=0.0, vmax=dist.max())
|
|
rr, cc = np.nonzero(skel)
|
|
ax0.plot(xs[cc], ys[rr], ls="none", marker="s", ms=0.55,
|
|
color="#4A4A4A", label="EVG skeleton", zorder=3)
|
|
ax0.plot(raw[:, 0], raw[:, 1], color="#C44E0A", lw=0.9,
|
|
label=r"coarse A$^\ast$", zorder=4)
|
|
ax0.plot(*sg.ENTRANCE, marker=">", ms=3.2, color="#111111", zorder=5)
|
|
ax0.plot(*sg.LOAD_POSE, marker="o", ms=3.0, color="#111111", zorder=5)
|
|
ax0.annotate("entrance", sg.ENTRANCE, textcoords="offset points",
|
|
xytext=(3, -6), fontsize=5.6)
|
|
ax0.annotate("loading bay", sg.LOAD_POSE, textcoords="offset points",
|
|
xytext=(-13, 4), fontsize=5.6)
|
|
ax0.legend(loc="lower right", fontsize=5.2, frameon=False,
|
|
handlelength=1.4, borderaxespad=0.1, labelspacing=0.25,
|
|
markerscale=3.0)
|
|
ax0.set_title("(a) clearance field and skeleton", fontsize=6.6, pad=2)
|
|
cb = fig.colorbar(im, ax=ax0, fraction=0.040, pad=0.015)
|
|
cb.set_ticks([0.0, dist.max()])
|
|
cb.set_ticklabels(["0", "%.0f m" % dist.max()])
|
|
cb.ax.tick_params(labelsize=5.2, width=0.4, length=1.8)
|
|
cb.outline.set_linewidth(0.4)
|
|
|
|
# ---- (b) local corridors and the smoothed centerline -------------------
|
|
ax1.add_patch(MplPolygon(sg.SITE, closed=True, facecolor="#F4F4F4",
|
|
edgecolor="#585858", lw=0.7, zorder=1))
|
|
for i in range(0, len(coarse), 3):
|
|
ang = np.degrees(np.arctan2(t_c[i, 1], t_c[i, 0]))
|
|
r = Rectangle((-S_HALF, -HALF_W), 2 * S_HALF, 2 * HALF_W,
|
|
facecolor="#BBD7EE", edgecolor="#4A87BE",
|
|
lw=0.25, alpha=0.75, zorder=2)
|
|
tr = (matplotlib.transforms.Affine2D()
|
|
.rotate_deg(ang).translate(*coarse[i]) + ax1.transData)
|
|
r.set_transform(tr)
|
|
ax1.add_patch(r)
|
|
ax1.plot(coarse[:, 0], coarse[:, 1], color="#C44E0A", lw=0.7,
|
|
ls=(0, (2.0, 1.3)), label=r"coarse A$^\ast$", zorder=4)
|
|
ax1.plot(center[:, 0], center[:, 1], color="#703A94", lw=1.2,
|
|
label=r"$\Lambda_{\mathrm{center}}$ (QP)", zorder=5)
|
|
# annotate one corridor with its four bounds
|
|
i0 = len(coarse) // 2
|
|
p0 = coarse[i0]
|
|
ax1.annotate(r"$s_i^\pm,\ l_i^\pm$", p0, textcoords="offset points",
|
|
xytext=(6, 7), fontsize=5.8,
|
|
arrowprops=dict(arrowstyle="-", lw=0.4, color="#333333",
|
|
shrinkA=0, shrinkB=1))
|
|
ax1.legend(loc="lower right", fontsize=5.2, frameon=False,
|
|
handlelength=1.5, borderaxespad=0.1, labelspacing=0.25)
|
|
ax1.set_title("(b) local corridors and QP smoothing", fontsize=6.6, pad=2)
|
|
|
|
for ax in (ax0, ax1):
|
|
ax.set_aspect("equal")
|
|
ax.set_xlim(sg.SITE[:, 0].min() - 1, sg.SITE[:, 0].max() + 1)
|
|
ax.set_ylim(sg.SITE[:, 1].min() - 1, sg.SITE[:, 1].max() + 1)
|
|
ax.set_xticks([])
|
|
ax.set_yticks([])
|
|
for sp in ax.spines.values():
|
|
sp.set_visible(False)
|
|
|
|
# ---- (c) curvature profile --------------------------------------------
|
|
ax2.plot(s_raw, k_raw, color="#C44E0A", lw=0.7, ls=(0, (2.0, 1.3)),
|
|
label=r"coarse A$^\ast$")
|
|
ax2.plot(s_sm, k_sm, color="#703A94", lw=1.0,
|
|
label=r"$\Lambda_{\mathrm{center}}$")
|
|
ax2.axhline(KAP_MAX, color="#111111", lw=0.6, ls=(0, (4.0, 1.6)))
|
|
ax2.annotate(r"$\kappa_{\max}=1/R_{\min}$", (s_sm[-1], KAP_MAX),
|
|
textcoords="offset points", xytext=(-2, 3), fontsize=5.6,
|
|
ha="right")
|
|
ax2.set_xlabel("arc length $s$ (m)", fontsize=6.2, labelpad=1.5)
|
|
ax2.set_ylabel(r"$\kappa$ (m$^{-1}$)", fontsize=6.2, labelpad=1.5)
|
|
ax2.set_title("(c) curvature profile", fontsize=6.6, pad=2)
|
|
ax2.tick_params(labelsize=5.6, width=0.4, length=2.0, pad=1.5)
|
|
ax2.set_xlim(0, max(s_raw[-1], s_sm[-1]))
|
|
ax2.set_ylim(0, max(k_raw.max(), KAP_MAX) * 1.18)
|
|
ax2.legend(loc="upper left", fontsize=5.2, frameon=False,
|
|
handlelength=1.5, borderaxespad=0.2, labelspacing=0.25)
|
|
for sp in ("top", "right"):
|
|
ax2.spines[sp].set_visible(False)
|
|
|
|
fig.savefig("fig_center.pdf", dpi=600)
|
|
print("wrote fig_center.pdf")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|