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

Round 3: pt2line truly before Hough, total correction limit, loc_rms in metrics

Browse Source
This commit is contained in:
cyy
2026-07-03 07:42:13 +00:00
parent caaa18d30d
commit 0802d430a5
3 changed files with 45 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/car_nav_lite/localizer.hpp
View File

@@ -24,7 +24,7 @@ public:
// Metrics
int loc_lines_ = 0;
double loc_dx_ = 0, loc_dy_ = 0, loc_dyaw_ = 0;
double loc_conf_ = 1.0;
double loc_conf_ = 1.0, loc_rms_ = 999.0;
private:
Pose2D odom_pose_;

81
src/localizer.cpp
View File

@@ -40,7 +40,47 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
const int N = scan.size();
Pose2D base = base_override ? *base_override : odom_pose_;
// ── 1. Render scan to local image ──
// ── 1. Point-to-Line2D residual (primary, no Hough needed) ──
if (use_fence_ && fence_.valid) {
constexpr double GATE = 0.20;
double H[9]={0}, b2[3]={0}; double sum_r2=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*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(d<best_d){best_d=d;best=L;}}
if(best_d>GATE||!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;
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
}
}
// Fence residual failed → fall through to Hough
}
// ── 2. Hough fallback: render scan to image + line detection ──
constexpr int IMG_SZ = 600, HALF = IMG_SZ / 2;
int cx_g, cy_g;
map.worldToGrid(base.x, base.y, cx_g, cy_g);
@@ -130,44 +170,7 @@ void Localizer::correctWithScanCV(const LaserScan& scan, const GridMap& map,
}
}
// ── 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}; double sum_r2=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*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(d<best_d){best_d=d;best=L;}}
if(best_d>GATE||!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;
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);
// 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;
}
}
return;
}
// ── 5b. Hough fallback (no fence model) ──
// ── 5. Hough update ──
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;

4
src/main.cpp
View File

@@ -314,10 +314,10 @@ private:
char buf[512];
snprintf(buf, sizeof(buf),
"{\"state\":\"%s\",\"x\":%.2f,\"y\":%.2f,\"yaw\":%.1f,"
"\"loc_conf\":%.2f,\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f,"
"\"loc_conf\":%.2f,\"loc_rms\":%.3f,\"loc_lines\":%d,\"loc_dx\":%.3f,\"loc_dy\":%.3f,\"loc_dyaw\":%.2f,"
"\"mppi_alive\":%d,\"mppi_total\":%d,\"mppi_minC\":%.1f}",
m.state, m.pose_x, m.pose_y, m.pose_yaw*57.3,
localizer_.loc_conf_, m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3,
localizer_.loc_conf_, localizer_.loc_rms_, m.loc_lines, m.loc_dx, m.loc_dy, m.loc_dyaw*57.3,
m.mppi_alive, m.mppi_total, m.mppi_min_cost);
std_msgs::msg::String msg; msg.data = buf;
pub_metrics_->publish(msg);