cyy/car_nav_lite
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Round 4: quality gate for fence residual, skip Hough when fence exists

Browse Source
This commit is contained in:
cyy
2026-07-03 07:50:54 +00:00
parent 0802d430a5
commit ed23cf5482
1 changed files with 21 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
src/localizer.cpp
View File

@@ -60,24 +60,28 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
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];} 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; ++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]); double loc_rms = (n_pts>0) ? std::sqrt(sum_r2/n_pts) : 999.0;
if(std::abs(det)>1e-12){double inv=1.0/det; double conf_pts = std::min(1.0, n_pts/80.0);
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 conf_rms = std::min(1.0, 0.08/std::max(loc_rms,1e-3));
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 quality = conf_pts * conf_rms;
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]; loc_rms_=loc_rms; loc_conf_=quality*0.9; loc_lines_=n_pts/10;
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; // Only apply correction if quality is adequate
correction_.x=limit(correction_.x,-0.30,0.30);correction_.y=limit(correction_.y,-0.30,0.30); 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]);
correction_.yaw=limit(correction_.yaw,-0.25,0.25); if (n_pts>20 && quality>0.25 && std::abs(det)>1e-12) {
loc_dx_=correction_.x;loc_dy_=correction_.y;loc_dyaw_=correction_.yaw; double inv=1.0/det;
double loc_rms=std::sqrt(sum_r2/n_pts); 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];
loc_rms_=loc_rms; 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];
loc_conf_=std::min(1.0,n_pts/80.0)*std::min(1.0,0.08/std::max(loc_rms,1e-3))*0.9; 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];
loc_lines_=n_pts/10; dx=limit(dx*inv,-0.10,0.10);dy=limit(dy*inv,-0.10,0.10);da=limit(da*inv,-0.10,0.10);
return; // success, skip Hough fallback 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;
return;
} }
// Fence residual failed → fall through to Hough // Quality insufficient — don't update correction, fall through to Hough
if (use_fence_) return; // fence exists: trust quality gate, skip Hough entirely
} }
// ── 2. Hough fallback: render scan to image + line detection ── // ── 2. Hough fallback: render scan to image + line detection ──