From caaa18d30d91e3adf68b19b12f8e5011111c26b2 Mon Sep 17 00:00:00 2001 From: cyy Date: Fri, 3 Jul 2026 07:25:24 +0000 Subject: [PATCH] Round 2 fixes: rotated fence corners, pt2line before Hough, incremental correction, loc_rms, dynamic decay --- include/car_nav_lite/fence_model.hpp | 3 ++ include/car_nav_lite/grid_map.hpp | 1 + src/grid_map.cpp | 17 +++++-- src/localizer.cpp | 76 +++++++++++++--------------- src/session_mapper.cpp | 6 +++ 5 files changed, 56 insertions(+), 47 deletions(-) diff --git a/include/car_nav_lite/fence_model.hpp b/include/car_nav_lite/fence_model.hpp index a3a3a72..0fa8676 100644 --- a/include/car_nav_lite/fence_model.hpp +++ b/include/car_nav_lite/fence_model.hpp @@ -35,6 +35,9 @@ struct FenceModel { // Pre-computed wall positions for localization double left_x = -2.5, right_x = 2.5; double bottom_y = -2.5, top_y = 2.5; + // Pre-computed corner points for drawing rotated fence + double p_lb_x=0, p_lb_y=0, p_lt_x=0, p_lt_y=0; + double p_rb_x=0, p_rb_y=0, p_rt_x=0, p_rt_y=0; double yaw_field = 0.0; double width = 0.0, height = 0.0; double cx = 0.0, cy = 0.0; diff --git a/include/car_nav_lite/grid_map.hpp b/include/car_nav_lite/grid_map.hpp index 59d3b90..f3351df 100644 --- a/include/car_nav_lite/grid_map.hpp +++ b/include/car_nav_lite/grid_map.hpp @@ -51,6 +51,7 @@ public: // ── Layered map (fence + cone + dynamic) ── void freezeSessionMap(); void composeLayers(); + void decayDynamic(uint8_t amount = 10); // ── Pre-built obstacle marks ─────────────────────── void markCircle(double wx, double wy, double radius, uint8_t value = 255); diff --git a/src/grid_map.cpp b/src/grid_map.cpp index e69c5af..27da6fa 100644 --- a/src/grid_map.cpp +++ b/src/grid_map.cpp @@ -67,6 +67,10 @@ void GridMap::composeLayers() { } } +void GridMap::decayDynamic(uint8_t amount) { + for (auto& v : dynamic_cells_) v = (v > amount) ? (v - amount) : 0; +} + void GridMap::freezeSessionMap() { frozen_ = true; // Merge cone layer into static for persistence @@ -163,6 +167,7 @@ void GridMap::updateScan(const LaserScan& scan, const Pose2D& pose) { int ox, oy; if (!worldToGrid(pose.x, pose.y, ox, oy)) return; + if (frozen_) decayDynamic(10); auto& target = frozen_ ? dynamic_cells_ : cells_; for (int i = 0; i < scan.size(); ++i) { double r = scan.ranges[i]; @@ -241,12 +246,14 @@ void GridMap::markCircle(double wx, double wy, double radius, uint8_t value) { void GridMap::markFenceModel(const FenceModel& fence, double thickness) { if (!fence.valid) return; - // Build four corners from boundary positions (already world-aligned) - double lx = fence.left_x, rx = fence.right_x; - double by = fence.bottom_y, ty = fence.top_y; - // Draw four edges + // Draw four edges using pre-computed rotated corner points struct Seg { double x1,y1,x2,y2; }; - Seg segs[] = {{lx,by,lx,ty}, {rx,by,rx,ty}, {lx,by,rx,by}, {lx,ty,rx,ty}}; + Seg segs[] = { + {fence.p_lb_x,fence.p_lb_y, fence.p_lt_x,fence.p_lt_y}, // left + {fence.p_rb_x,fence.p_rb_y, fence.p_rt_x,fence.p_rt_y}, // right + {fence.p_lb_x,fence.p_lb_y, fence.p_rb_x,fence.p_rb_y}, // bottom + {fence.p_lt_x,fence.p_lt_y, fence.p_rt_x,fence.p_rt_y}, // top + }; int t = (int)(thickness / GRID_RES) + 1; for (auto& s : segs) { int gx1, gy1, gx2, gy2; diff --git a/src/localizer.cpp b/src/localizer.cpp index fa54744..4a831c2 100644 --- a/src/localizer.cpp +++ b/src/localizer.cpp @@ -130,56 +130,48 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map, } } - // ── 5. Update offset ── - 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; - - // Point-to-Line2D residual optimization (if fence model available) - if (use_fence_ && fence_.valid && yaw_count >= 1) { + // ── 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}; - 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 * std::cos(a + base.yaw); - double wy = base.y + r * std::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; - double qx = r * std::cos(a), qy = r * std::sin(a); - double dRx = -std::sin(base.yaw)*qx - std::cos(base.yaw)*qy; - double dRy = std::cos(base.yaw)*qx - std::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]; - } + 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; + 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.8*correction_.x+0.2*dx; correction_.y=0.8*correction_.y+0.2*dy; - correction_.yaw=0.8*correction_.yaw+0.2*da; - loc_dx_=correction_.x; loc_dy_=correction_.y; loc_dyaw_=correction_.yaw; - loc_conf_ = std::min(1.0, n_pts/80.0) * 0.9; + 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; } } - } else if (line_count >= 2) { + 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; + if (line_count >= 2) { double dx = sum_dx / line_count, dy = sum_dy / line_count; loc_dx_ = dx; loc_dy_ = dy; loc_dyaw_ = -dyaw; double corr_dist = std::sqrt(dx*dx + dy*dy); diff --git a/src/session_mapper.cpp b/src/session_mapper.cpp index 30e3304..00c45f1 100644 --- a/src/session_mapper.cpp +++ b/src/session_mapper.cpp @@ -115,6 +115,7 @@ bool SessionMapper::build(FenceModel& fence, std::vector& cones Eigen::Vector2d tw = rotBack(fence.cx, y_max); fence.left_x = lw.x(); fence.right_x = rw.x(); fence.bottom_y = bw.y(); fence.top_y = tw.y(); + // Corner points for drawing rotated fence (lb/lt/rb/rt are already computed above) // Build fence lines from field-aligned extents (NOT from 0/w/h!) Eigen::Vector2d lb = rotBack(x_min, y_min), lt = rotBack(x_min, y_max); @@ -126,6 +127,11 @@ bool SessionMapper::build(FenceModel& fence, std::vector& cones Eigen::Vector2d tb = rotBack(x_min, y_max), tr_ = rotBack(x_max, y_max); fence.top.setFromTwoPoints(tb.x(), tb.y(), tr_.x(), tr_.y()); + fence.p_lb_x=lb.x();fence.p_lb_y=lb.y(); + fence.p_lt_x=lt.x();fence.p_lt_y=lt.y(); + fence.p_rb_x=rb.x();fence.p_rb_y=rb.y(); + fence.p_rt_x=rt.x();fence.p_rt_y=rt.y(); + fence.valid = true; // ── 5. Extract cones: non-wall points → cluster ──