Round 3: pt2line truly before Hough, total correction limit, loc_rms in metrics

include/car_nav_lite/localizer.hpp

@@ -24,7 +24,7 @@ public:
     // Metrics
     int loc_lines_ = 0;
     double loc_dx_ = 0, loc_dy_ = 0, loc_dyaw_ = 0;
-    double loc_conf_ = 1.0;
+    double loc_conf_ = 1.0, loc_rms_ = 999.0;
 
 private:
     Pose2D odom_pose_;

src/localizer.cpp

@@ -40,7 +40,47 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
     const int N = scan.size();
     Pose2D base = base_override ? *base_override : odom_pose_;
 
-    // ── 1. Render scan to local image ──
+    // ── 1. Point-to-Line2D residual (primary, no Hough needed) ──
+    if (use_fence_ && fence_.valid) {
+        constexpr double GATE = 0.20;
+        double H[9]={0}, b2[3]={0}; double sum_r2=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*cos(a+base.yaw),wy=base.y+r*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;sum_r2+=res*res;
+            double qx=r*cos(a),qy=r*sin(a);
+            double dRx=-sin(base.yaw)*qx-cos(base.yaw)*qy,dRy=cos(base.yaw)*qx-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.2*dx;correction_.y+=0.2*dy;correction_.yaw+=0.2*da;
+                correction_.x=limit(correction_.x,-0.30,0.30);correction_.y=limit(correction_.y,-0.30,0.30);
+                correction_.yaw=limit(correction_.yaw,-0.25,0.25);
+                loc_dx_=correction_.x;loc_dy_=correction_.y;loc_dyaw_=correction_.yaw;
+                double loc_rms=std::sqrt(sum_r2/n_pts);
+                loc_rms_=loc_rms;
+                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;
+                return;  // success, skip Hough fallback
+            }
+        }
+        // Fence residual failed → fall through to Hough
+    }
+
+    // ── 2. Hough fallback: render scan to image + line detection ──
     constexpr int IMG_SZ = 600, HALF = IMG_SZ / 2;
     int cx_g, cy_g;
     map.worldToGrid(base.x, base.y, cx_g, cy_g);
@@ -130,44 +170,7 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
         }
     }
 
-    // ── 5a. Point-to-Line2D residual (primary, if fence model available) ──
+    // ── 5. Hough update ──
-    if (use_fence_ && fence_.valid) {
-        constexpr double GATE = 0.20;
-        double H[9]={0}, b2[3]={0}; double sum_r2=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*cos(a+base.yaw),wy=base.y+r*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;sum_r2+=res*res;
-            double qx=r*cos(a),qy=r*sin(a);
-            double dRx=-sin(base.yaw)*qx-cos(base.yaw)*qy,dRy=cos(base.yaw)*qx-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);
-                // Incremental update (base is corrected pose)
-                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;
-                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;
-            }
-        }
-        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;

src/main.cpp

@@ -314,10 +314,10 @@ private:
         char buf[512];
         snprintf(buf, sizeof(buf),
             "{\"state\":\"%s\",\"x\":%.2f,\"y\":%.2f,\"yaw\":%.1f,"
-            "\"loc_conf\":%.2f,\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f,"
+            "\"loc_conf\":%.2f,\"loc_rms\":%.3f,\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f,"
             "\"mppi_alive\":%d,\"mppi_total\":%d,\"mppi_minC\":%.1f}",
             m.state, m.pose_x, m.pose_y, m.pose_yaw*57.3,
-            localizer_.loc_conf_, m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3,
+            localizer_.loc_conf_, localizer_.loc_rms_, m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3,
             m.mppi_alive, m.mppi_total, m.mppi_min_cost);
         std_msgs::msg::String msg; msg.data = buf;
         pub_metrics_->publish(msg);