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

22
src/localizer.cpp
View File

@@ -60,8 +60,16 @@ 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 conf_rms = std::min(1.0, 0.08/std::max(loc_rms,1e-3));
double quality = conf_pts * conf_rms;
loc_rms_=loc_rms; loc_conf_=quality*0.9; loc_lines_=n_pts/10;
// Only apply correction if quality is adequate
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 (n_pts>20 && quality>0.25 && 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 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 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]; 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];
@@ -70,14 +78,10 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
correction_.x=limit(correction_.x,-0.30,0.30);correction_.y=limit(correction_.y,-0.30,0.30); 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); correction_.yaw=limit(correction_.yaw,-0.25,0.25);
loc_dx_=correction_.x;loc_dy_=correction_.y;loc_dyaw_=correction_.yaw; loc_dx_=correction_.x;loc_dy_=correction_.y;loc_dyaw_=correction_.yaw;
double loc_rms=std::sqrt(sum_r2/n_pts); return;
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
} }
} // Quality insufficient — don't update correction, fall through to Hough
// Fence residual failed → 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 ──