| 485 | |
| 486 | |
| 487 | def sample_traj_by_length(points, num_samples): |
| 488 | # Sample points evenly from traj based on the euclidean distance |
| 489 | |
| 490 | pts = np.array(points, dtype=float) # shape (M, 2) |
| 491 | |
| 492 | # 1) 每段长度 |
| 493 | seg = pts[1:] - pts[:-1] |
| 494 | seg_len = np.sqrt((seg**2).sum(axis=1)) # shape (M-1,) |
| 495 | |
| 496 | # 2) 累积长度 |
| 497 | cum = np.cumsum(seg_len) |
| 498 | total_length = cum[-1] |
| 499 | |
| 500 | # 3) 目标等距长度位置 |
| 501 | target = np.linspace(0, total_length, num_samples) |
| 502 | |
| 503 | res = [] |
| 504 | for t in target: |
| 505 | # 4) 找到它落在哪一段 |
| 506 | idx = np.searchsorted(cum, t) |
| 507 | if idx == 0: |
| 508 | prev = 0. |
| 509 | else: |
| 510 | prev = cum[idx-1] |
| 511 | |
| 512 | # 5) 在该段内插值 |
| 513 | ratio = (t - prev) / seg_len[idx] |
| 514 | p = pts[idx] * ratio + pts[idx+1] * (1-ratio) # careful: direction reversed? |
| 515 | # Actually want: start*(1-ratio) + end*ratio |
| 516 | p = pts[idx] * (1 - ratio) + pts[idx+1] * ratio |
| 517 | res.append(p) |
| 518 | return np.array(res) |
| 519 | |
| 520 | |
| 521 | |