From 0802d430a52bdcbcd5be23735ae5a87974eec1b9 Mon Sep 17 00:00:00 2001 From: cyy Date: Fri, 3 Jul 2026 07:42:13 +0000 Subject: [PATCH] Round 3: pt2line truly before Hough, total correction limit, loc_rms in metrics --- include/car_nav_lite/localizer.hpp | 2 +- src/localizer.cpp | 81 ++++++++++++++++-------------- src/main.cpp | 4 +- 3 files changed, 45 insertions(+), 42 deletions(-) diff --git a/include/car_nav_lite/localizer.hpp b/include/car_nav_lite/localizer.hpp index 40bb703..e844586 100644 --- a/include/car_nav_lite/localizer.hpp +++ b/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_; diff --git a/src/localizer.cpp b/src/localizer.cpp index 4a831c2..490dc1e 100644 --- a/src/localizer.cpp +++ b/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;i4.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(dGATE||!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) ── - 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;i4.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(dGATE||!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) ── + // ── 5. Hough update ── 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; diff --git a/src/main.cpp b/src/main.cpp index 59e3a6e..d4954dc 100644 --- a/src/main.cpp +++ b/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);