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

Fix #9: point-to-line residual optimization for fence localization

Browse Source
This commit is contained in:
cyy
2026-07-03 07:17:10 +00:00
parent 85598eaa51
commit 4f4715fb82
1 changed files with 46 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
src/localizer.cpp
View File

@@ -131,11 +131,55 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
}
// ── 5. Update offset ──
// Store metrics + confidence
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) {
// Point-to-Line2D residual optimization (if fence model available)
if (use_fence_ && fence_.valid && yaw_count >= 1) {
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];
}
++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.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;
}
}
} else 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);