Round 2 fixes: rotated fence corners, pt2line before Hough, incremental correction, loc_rms, dynamic decay

include/car_nav_lite/fence_model.hpp

@@ -35,6 +35,9 @@ struct FenceModel {
     // Pre-computed wall positions for localization
     double left_x = -2.5, right_x = 2.5;
     double bottom_y = -2.5, top_y = 2.5;
+    // Pre-computed corner points for drawing rotated fence
+    double p_lb_x=0, p_lb_y=0, p_lt_x=0, p_lt_y=0;
+    double p_rb_x=0, p_rb_y=0, p_rt_x=0, p_rt_y=0;
     double yaw_field = 0.0;
     double width = 0.0, height = 0.0;
     double cx = 0.0, cy = 0.0;

include/car_nav_lite/grid_map.hpp

@@ -51,6 +51,7 @@ public:
     // ── Layered map (fence + cone + dynamic) ──
     void freezeSessionMap();
     void composeLayers();
+    void decayDynamic(uint8_t amount = 10);
 
     // ── Pre-built obstacle marks ───────────────────────
     void markCircle(double wx, double wy, double radius, uint8_t value = 255);

src/grid_map.cpp

@@ -67,6 +67,10 @@ void GridMap::composeLayers() {
     }
 }
 
+void GridMap::decayDynamic(uint8_t amount) {
+    for (auto& v : dynamic_cells_) v = (v > amount) ? (v - amount) : 0;
+}
+
 void GridMap::freezeSessionMap() {
     frozen_ = true;
     // Merge cone layer into static for persistence
@@ -163,6 +167,7 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) {
     int ox, oy;
     if (!worldToGrid(pose.x, pose.y, ox, oy)) return;
 
+    if (frozen_) decayDynamic(10);
     auto& target = frozen_ ? dynamic_cells_ : cells_;
     for (int i = 0; i < scan.size(); ++i) {
         double r = scan.ranges[i];
@@ -241,12 +246,14 @@ void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) {
 
 void GridMap::markFenceModel(const FenceModel& fence, double thickness) {
     if (!fence.valid) return;
-    // Build four corners from boundary positions (already world-aligned)
+    // Draw four edges using pre-computed rotated corner points
-    double lx = fence.left_x, rx = fence.right_x;
-    double by = fence.bottom_y, ty = fence.top_y;
-    // Draw four edges
     struct Seg { double x1,y1,x2,y2; };
-    Seg segs[] = {{lx,by,lx,ty}, {rx,by,rx,ty}, {lx,by,rx,by}, {lx,ty,rx,ty}};
+    Seg segs[] = {
+        {fence.p_lb_x,fence.p_lb_y, fence.p_lt_x,fence.p_lt_y},  // left
+        {fence.p_rb_x,fence.p_rb_y, fence.p_rt_x,fence.p_rt_y},  // right
+        {fence.p_lb_x,fence.p_lb_y, fence.p_rb_x,fence.p_rb_y},  // bottom
+        {fence.p_lt_x,fence.p_lt_y, fence.p_rt_x,fence.p_rt_y},  // top
+    };
     int t = (int)(thickness / GRID_RES) + 1;
     for (auto& s : segs) {
         int gx1, gy1, gx2, gy2;

src/localizer.cpp

@@ -130,56 +130,48 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
         }
     }
 
-    // ── 5. Update offset ──
+    // ── 5a. Point-to-Line2D residual (primary, if fence model available) ──
-    loc_lines_ = line_count;
+    if (use_fence_ && fence_.valid) {
-    loc_conf_ = (line_count >= 3) ? 0.9 : (line_count >= 2 ? 0.6 : 0.3);
-    if (std::abs(dyaw) > 0.15) loc_conf_ *= 0.5;
-
-    // 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};
+        double H[9]={0}, b2[3]={0}; double sum_r2=0; int n_pts=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;
-        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 wx=base.x+r*cos(a+base.yaw),wy=base.y+r*sin(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);
+                double d=std::abs(L->nx*wx+L->ny*wy-L->d);if(d<best_d){best_d=d;best=L;}}
-                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 res=best->nx*wx+best->ny*wy-best->d;sum_r2+=res*res;
-            double qx = r * std::cos(a), qy = r * std::sin(a);
+            double qx=r*cos(a),qy=r*sin(a);
-            double dRx = -std::sin(base.yaw)*qx - std::cos(base.yaw)*qy;
+            double dRx=-sin(base.yaw)*qx-cos(base.yaw)*qy,dRy=cos(base.yaw)*qx-sin(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) {
+            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];}
-                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) {
+        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]);
-            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;
-            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;
+                // Incremental update (base is corrected pose)
-                correction_.yaw=0.8*correction_.yaw+0.2*da;
+                correction_.x+=0.2*dx;correction_.y+=0.2*dy;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;
+                double loc_rms=std::sqrt(sum_r2/n_pts);
+                loc_conf_=std::min(1.0,n_pts/80.0)*std::min(1.0,0.08/std::max(loc_rms,1e-3))*0.9;
+                loc_lines_=n_pts/10;
             }
         }
-    } else if (line_count >= 2) {
+        return;
+    }
+
+    // ── 5b. Hough fallback (no fence model) ──
+    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) {
         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);

src/session_mapper.cpp

@@ -115,6 +115,7 @@ bool SessionMapper::build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones
     Eigen::Vector2d tw = rotBack(fence.cx, y_max);
     fence.left_x = lw.x(); fence.right_x = rw.x();
     fence.bottom_y = bw.y(); fence.top_y = tw.y();
+    // Corner points for drawing rotated fence (lb/lt/rb/rt are already computed above)
 
     // Build fence lines from field-aligned extents (NOT from 0/w/h!)
     Eigen::Vector2d lb = rotBack(x_min, y_min), lt = rotBack(x_min, y_max);
@@ -126,6 +127,11 @@ bool SessionMapper::build(FenceModel& fence, std::vector<Eigen::Vector2d>& cones
     Eigen::Vector2d tb = rotBack(x_min, y_max), tr_ = rotBack(x_max, y_max);
     fence.top.setFromTwoPoints(tb.x(), tb.y(), tr_.x(), tr_.y());
 
+    fence.p_lb_x=lb.x();fence.p_lb_y=lb.y();
+    fence.p_lt_x=lt.x();fence.p_lt_y=lt.y();
+    fence.p_rb_x=rb.x();fence.p_rb_y=rb.y();
+    fence.p_rt_x=rt.x();fence.p_rt_y=rt.y();
+
     fence.valid = true;
 
     // ── 5. Extract cones: non-wall points → cluster ──