Offline PGO: target submap can include current scan and accept unsupported loop closures

[RaymondLmx]

Summary

The offline Python PGO can use the current scan as part of its own loop-closure target submap. Consequently, ICP can accept a loop even when the current scan has no geometric overlap with any historical scan.

This is not a graph self-edge: the reported edge connects the current keyframe to a historical keyframe, but its geometric evidence includes the current scan itself.

Version and scope

• Reproduced on v0.0.14b1, commit 92a853a400648d5a0b3a4fa36eff51584ff41790.
• Python 3.12/aarch64, NumPy 2.5.2, open3d-unofficial-arm==0.19.0.post9, lockfile-pinned gtsam-extended==4.3a1.post1.
• No PGO/ICP parameter overrides, optimizer mocks, source patches, robot hardware or recording required.
• Code inspection of main at aa8a1584698b6b932142f051801acefd7317856a shows the same insertion order, submap bounds and defaults. I have not run this reproducer against a complete main environment.

Root cause

_PGOState.process() inserts the current keyframe before _search_for_loops().
Candidate-center selection excludes the current keyframe and applies loop_time_thresh, but the subsequent _get_submap(loop_idx, loop_submap_half_range) includes all indices in:

lo = max(0, idx - half_range)
hi = min(len(self._key_poses) - 1, idx + half_range)

With current index 9, candidate index 0, default half-range 10, the target contains indices 0..9, including the source scan. A 27-second separation of the center frames passes the default 20-second time gate.

Current source at inspected commit

Minimal synthetic reproducer

Run in a v0.0.14b1 environment with mapping dependencies. This uses correctly world-registered clouds and matching attached poses, so it does not depend on the separate CLI sensor/world input-routing issue.

import numpy as np
from dimos.mapping.loop_closure import pgo
from dimos.memory2.type.observation import Observation
from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2

u, v = np.meshgrid(np.arange(-3., 3.01, .15),
                   np.arange(-3., 3.01, .15))
u, v = u.ravel(), v.ravel()
room = np.concatenate([
    np.column_stack([u, v, np.full_like(u, -3.)]),
    np.column_stack([np.full_like(u, 3.), u, v]),
    np.column_stack([u, np.full_like(u, 3.), v]),
])

def cloud(points, ts=100.):
    return PointCloud2.from_numpy(points, frame_id="odom", timestamp=ts)

# Current scene is deliberately disjoint from ALL historical point clouds.
# This is an adversarial fixture, not a physically realistic capture.
_, historical_score = pgo._icp(cloud(room), cloud(room + 100.))
assert np.isinf(historical_score)

rows = []
for i in range(10):
    ts = 100. + 3 * i
    x = float(i + 1) if i < 9 else 1.
    rows.append(Observation(
        ts=ts,
        _data=cloud(room + 100. if i < 9 else room, ts),
        pose=(x, 0., 0., 0., 0., 0., 1.),
    ))

graph = list(pgo.PGO()(iter(rows)))[-1].data
print([(e.source.ts, e.target.ts, e.score) for e in graph.loops])
assert len(graph.loops) == 0, "No historical geometric support for a loop"

Observed versus expected

Observed: [(127.0, 100.0, 0.0)]; assertion fails. The log reports source=9 target=0 score=0.0.

Independent in-memory control using the same stock state and ICP:

• Target _get_submap(0, 10): 5,582 points, score 0; every source point has a zero-distance correspondence.
• Historical-only control _get_submap(0, 8): 2,791 points, score infinity; no correspondences within the default ICP distance.

Expected: no loop accepted without historical geometric support. The target must not contain the current scan. Candidate-center temporal filtering alone does not guarantee temporal independence of all target members.

Impact / proposed discussion

Self-inclusion makes the matching score unreliable as independent loop evidence and can add unsupported constraints. It does not prove every affected real-world loop is wrong or that map distortion necessarily occurs.

Would you prefer explicitly excluding the source keyframe from target construction, or applying an appropriate temporal eligibility rule to all target submap members as well? I can contribute a focused fix and positive/negative regressions. This report does not propose changing ICP thresholds or replacing the optimizer.