diff --git a/src/localizer.cpp b/src/localizer.cpp index 490dc1e..6e6449e 100644 --- a/src/localizer.cpp +++ b/src/localizer.cpp @@ -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];} ++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 - } + double loc_rms = (n_pts>0) ? std::sqrt(sum_r2/n_pts) : 999.0; + 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 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; + 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 ──