radard: no clustering (#29010)
* First commit
* cleanup
* Update ref
* Doesnt deserve two files
* cleanup radard
old-commit-hash: ca699e3989
beeps
Harald Schäfer
2 years ago
committed by
GitHub
parent
05b2a0b884
commit
8a48732784
7 changed files with 126 additions and 216 deletions
@ -1,159 +0,0 @@ |
||||
from common.numpy_fast import mean |
||||
from common.kalman.simple_kalman import KF1D |
||||
|
||||
|
||||
# Default lead acceleration decay set to 50% at 1s |
||||
_LEAD_ACCEL_TAU = 1.5 |
||||
|
||||
# radar tracks |
||||
SPEED, ACCEL = 0, 1 # Kalman filter states enum |
||||
|
||||
# stationary qualification parameters |
||||
v_ego_stationary = 4. # no stationary object flag below this speed |
||||
|
||||
RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car |
||||
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame |
||||
|
||||
class Track(): |
||||
def __init__(self, v_lead, kalman_params): |
||||
self.cnt = 0 |
||||
self.aLeadTau = _LEAD_ACCEL_TAU |
||||
self.K_A = kalman_params.A |
||||
self.K_C = kalman_params.C |
||||
self.K_K = kalman_params.K |
||||
self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) |
||||
|
||||
def update(self, d_rel, y_rel, v_rel, v_lead, measured): |
||||
# relative values, copy |
||||
self.dRel = d_rel # LONG_DIST |
||||
self.yRel = y_rel # -LAT_DIST |
||||
self.vRel = v_rel # REL_SPEED |
||||
self.vLead = v_lead |
||||
self.measured = measured # measured or estimate |
||||
|
||||
# computed velocity and accelerations |
||||
if self.cnt > 0: |
||||
self.kf.update(self.vLead) |
||||
|
||||
self.vLeadK = float(self.kf.x[SPEED][0]) |
||||
self.aLeadK = float(self.kf.x[ACCEL][0]) |
||||
|
||||
# Learn if constant acceleration |
||||
if abs(self.aLeadK) < 0.5: |
||||
self.aLeadTau = _LEAD_ACCEL_TAU |
||||
else: |
||||
self.aLeadTau *= 0.9 |
||||
|
||||
self.cnt += 1 |
||||
|
||||
def get_key_for_cluster(self): |
||||
# Weigh y higher since radar is inaccurate in this dimension |
||||
return [self.dRel, self.yRel*2, self.vRel] |
||||
|
||||
def reset_a_lead(self, aLeadK, aLeadTau): |
||||
self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) |
||||
self.aLeadK = aLeadK |
||||
self.aLeadTau = aLeadTau |
||||
|
||||
|
||||
class Cluster(): |
||||
def __init__(self): |
||||
self.tracks = set() |
||||
|
||||
def add(self, t): |
||||
# add the first track |
||||
self.tracks.add(t) |
||||
|
||||
# TODO: make generic |
||||
@property |
||||
def dRel(self): |
||||
return mean([t.dRel for t in self.tracks]) |
||||
|
||||
@property |
||||
def yRel(self): |
||||
return mean([t.yRel for t in self.tracks]) |
||||
|
||||
@property |
||||
def vRel(self): |
||||
return mean([t.vRel for t in self.tracks]) |
||||
|
||||
@property |
||||
def aRel(self): |
||||
return mean([t.aRel for t in self.tracks]) |
||||
|
||||
@property |
||||
def vLead(self): |
||||
return mean([t.vLead for t in self.tracks]) |
||||
|
||||
@property |
||||
def dPath(self): |
||||
return mean([t.dPath for t in self.tracks]) |
||||
|
||||
@property |
||||
def vLat(self): |
||||
return mean([t.vLat for t in self.tracks]) |
||||
|
||||
@property |
||||
def vLeadK(self): |
||||
return mean([t.vLeadK for t in self.tracks]) |
||||
|
||||
@property |
||||
def aLeadK(self): |
||||
if all(t.cnt <= 1 for t in self.tracks): |
||||
return 0. |
||||
else: |
||||
return mean([t.aLeadK for t in self.tracks if t.cnt > 1]) |
||||
|
||||
@property |
||||
def aLeadTau(self): |
||||
if all(t.cnt <= 1 for t in self.tracks): |
||||
return _LEAD_ACCEL_TAU |
||||
else: |
||||
return mean([t.aLeadTau for t in self.tracks if t.cnt > 1]) |
||||
|
||||
@property |
||||
def measured(self): |
||||
return any(t.measured for t in self.tracks) |
||||
|
||||
def get_RadarState(self, model_prob=0.0): |
||||
return { |
||||
"dRel": float(self.dRel), |
||||
"yRel": float(self.yRel), |
||||
"vRel": float(self.vRel), |
||||
"vLead": float(self.vLead), |
||||
"vLeadK": float(self.vLeadK), |
||||
"aLeadK": float(self.aLeadK), |
||||
"status": True, |
||||
"fcw": self.is_potential_fcw(model_prob), |
||||
"modelProb": model_prob, |
||||
"radar": True, |
||||
"aLeadTau": float(self.aLeadTau) |
||||
} |
||||
|
||||
def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): |
||||
lead_v_rel_pred = lead_msg.v[0] - model_v_ego |
||||
return { |
||||
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), |
||||
"yRel": float(-lead_msg.y[0]), |
||||
"vRel": float(lead_v_rel_pred), |
||||
"vLead": float(v_ego + lead_v_rel_pred), |
||||
"vLeadK": float(v_ego + lead_v_rel_pred), |
||||
"aLeadK": 0.0, |
||||
"aLeadTau": 0.3, |
||||
"fcw": False, |
||||
"modelProb": float(lead_msg.prob), |
||||
"radar": False, |
||||
"status": True |
||||
} |
||||
|
||||
def __str__(self): |
||||
ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" |
||||
return ret |
||||
|
||||
def potential_low_speed_lead(self, v_ego): |
||||
# stop for stuff in front of you and low speed, even without model confirmation |
||||
# Radar points closer than 0.75, are almost always glitches on toyota radars |
||||
return abs(self.yRel) < 1.0 and (v_ego < v_ego_stationary) and (0.75 < self.dRel < 25) |
||||
|
||||
def is_potential_fcw(self, model_prob): |
||||
return model_prob > .9 |
||||
@ -1 +1 @@ |
||||
219a815856d8984cb4933d83db9a15bf7cd09f16 |
||||
af03f2ddbc5244f9a885445c0452987a4bb81302 |
||||
|
||||
Loading…
Reference in new issue