From 4f4715fb829cf9ba42b61a203654042d3d18d8dc Mon Sep 17 00:00:00 2001
From: cyy <cyy@robot.local>
Date: Fri, 3 Jul 2026 07:17:10 +0000
Subject: [PATCH] Fix #9: point-to-line residual optimization for fence
 localization

---
 src/localizer.cpp | 48 +++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 46 insertions(+), 2 deletions(-)

diff --git a/src/localizer.cpp b/src/localizer.cpp
index d800100..fa54744 100644
--- a/src/localizer.cpp
+++ b/src/localizer.cpp
@@ -131,11 +131,55 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
     }
 
     // ── 5. Update offset ──
-    // Store metrics + confidence
     loc_lines_ = line_count;
     loc_conf_ = (line_count >= 3) ? 0.9 : (line_count >= 2 ? 0.6 : 0.3);
     if (std::abs(dyaw) > 0.15) loc_conf_ *= 0.5;
-    if (line_count >= 2) {
+
+    // Point-to-Line2D residual optimization (if fence model available)
+    if (use_fence_ && fence_.valid && yaw_count >= 1) {
+        constexpr double GATE = 0.20;
+        double H[9] = {0}, b2[3] = {0};
+        int n_pts = 0;
+        for (int i = 0; i < N; i += 2) {
+            double r = scan.ranges[i];
+            if (r < 0.1 || r > 4.5) continue;
+            double a = scan.angle_min + i * scan.angle_increment;
+            double wx = base.x + r * std::cos(a + base.yaw);
+            double wy = base.y + r * std::sin(a + base.yaw);
+            const Line2D* best = nullptr; double best_d = 1e9;
+            for (auto* L : {&fence_.left, &fence_.right, &fence_.bottom, &fence_.top}) {
+                double d = std::abs(L->nx * wx + L->ny * wy - L->d);
+                if (d < best_d) { best_d = d; best = L; }
+            }
+            if (best_d > GATE || !best) continue;
+            double res = best->nx * wx + best->ny * wy - best->d;
+            double qx = r * std::cos(a), qy = r * std::sin(a);
+            double dRx = -std::sin(base.yaw)*qx - std::cos(base.yaw)*qy;
+            double dRy =  std::cos(base.yaw)*qx - std::sin(base.yaw)*qy;
+            double Jyaw = best->nx * dRx + best->ny * dRy;
+            double w = (std::abs(res) < 0.05) ? 1.0 : 0.05 / std::abs(res);
+            double J[3] = {best->nx, best->ny, Jyaw};
+            for (int ri = 0; ri < 3; ++ri) {
+                b2[ri] -= w * J[ri] * res;
+                for (int ci = 0; ci < 3; ++ci) H[ri*3+ci] += w * J[ri] * J[ci];
+            }
+            ++n_pts;
+        }
+        if (n_pts > 10) {
+            double det = H[0]*(H[4]*H[8]-H[5]*H[7])-H[1]*(H[3]*H[8]-H[5]*H[6])+H[2]*(H[3]*H[7]-H[4]*H[6]);
+            if (std::abs(det) > 1e-12) {
+                double inv=1.0/det;
+                double dx=(H[4]*H[8]-H[5]*H[7])*b2[0]+(H[2]*H[7]-H[1]*H[8])*b2[1]+(H[1]*H[5]-H[2]*H[4])*b2[2];
+                double dy=(H[5]*H[6]-H[3]*H[8])*b2[0]+(H[0]*H[8]-H[2]*H[6])*b2[1]+(H[2]*H[3]-H[0]*H[5])*b2[2];
+                double da=(H[3]*H[7]-H[4]*H[6])*b2[0]+(H[1]*H[6]-H[0]*H[7])*b2[1]+(H[0]*H[4]-H[1]*H[3])*b2[2];
+                dx=limit(dx*inv,-0.10,0.10); dy=limit(dy*inv,-0.10,0.10); da=limit(da*inv,-0.10,0.10);
+                correction_.x=0.8*correction_.x+0.2*dx; correction_.y=0.8*correction_.y+0.2*dy;
+                correction_.yaw=0.8*correction_.yaw+0.2*da;
+                loc_dx_=correction_.x; loc_dy_=correction_.y; loc_dyaw_=correction_.yaw;
+                loc_conf_ = std::min(1.0, n_pts/80.0) * 0.9;
+            }
+        }
+    } else if (line_count >= 2) {
         double dx = sum_dx / line_count, dy = sum_dy / line_count;
         loc_dx_ = dx; loc_dy_ = dy; loc_dyaw_ = -dyaw;
         double corr_dist = std::sqrt(dx*dx + dy*dy);