From 37e7d185dc0c90b700642963993f10092a0d9fa9 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 3 Mar 2023 15:37:26 -0800 Subject: [PATCH 01/12] refactor radard --- selfdrive/controls/lib/radar_helpers.py | 166 +++++++++++++++++++++++ selfdrive/controls/radard.py | 171 +----------------------- 2 files changed, 169 insertions(+), 168 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 4bb01792677caa..311da57766ea6f 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,3 +1,8 @@ +import math +import cereal.messaging as messaging +from collections import defaultdict, deque +from third_party.cluster.fastcluster_py import cluster_points_centroid +from common.numpy_fast import interp from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D @@ -14,6 +19,81 @@ 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 KalmanParams(): + def __init__(self, dt): + # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. + # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s + assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s" + self.A = [[1.0, dt], [0.0, 1.0]] + self.C = [1.0, 0.0] + #Q = np.matrix([[10., 0.0], [0.0, 100.]]) + #R = 1e3 + #K = np.matrix([[ 0.05705578], [ 0.03073241]]) + dts = [dt * 0.01 for dt in range(1, 21)] + K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394, + 0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801, + 0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814, + 0.35353899, 0.36200124] + K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219, + 0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714, + 0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557, + 0.26393339, 0.26278425] + self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] + + +def laplacian_pdf(x, mu, b): + b = max(b, 1e-4) + return math.exp(-abs(x-mu)/b) + + +def match_vision_to_cluster(v_ego, lead, clusters): + # match vision point to best statistical cluster match + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA + + def prob(c): + prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) + + # This is isn't exactly right, but good heuristic + return prob_d * prob_y * prob_v + + cluster = max(clusters, key=prob) + + # if no 'sane' match is found return -1 + # stationary radar points can be false positives + dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) + if dist_sane and vel_sane: + return cluster + else: + return None + + +def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): + # Determine leads, this is where the essential logic happens + if len(clusters) > 0 and ready and lead_msg.prob > .5: + cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) + else: + cluster = None + + lead_dict = {'status': False} + if cluster is not None: + lead_dict = cluster.get_RadarState(lead_msg.prob) + elif (cluster is None) and ready and (lead_msg.prob > .5): + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + + if low_speed_override: + low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] + if len(low_speed_clusters) > 0: + closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) + + # Only choose new cluster if it is actually closer than the previous one + if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): + lead_dict = closest_cluster.get_RadarState() + + return lead_dict + class Track(): def __init__(self, v_lead, kalman_params): self.cnt = 0 @@ -156,3 +236,89 @@ def potential_low_speed_lead(self, v_ego): def is_potential_fcw(self, model_prob): return model_prob > .9 + + +class RadarD(): + def __init__(self, radar_ts, delay=0): + self.current_time = 0 + + self.tracks = defaultdict(dict) + self.kalman_params = KalmanParams(radar_ts) + + # v_ego + self.v_ego = 0. + self.v_ego_hist = deque([0], maxlen=delay+1) + + self.ready = False + + def update(self, sm, rr): + self.current_time = 1e-9*max(sm.logMonoTime.values()) + + if sm.updated['carState']: + self.v_ego = sm['carState'].vEgo + self.v_ego_hist.append(self.v_ego) + if sm.updated['modelV2']: + self.ready = True + + ar_pts = {} + for pt in rr.points: + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] + + # *** remove missing points from meta data *** + for ids in list(self.tracks.keys()): + if ids not in ar_pts: + self.tracks.pop(ids, None) + + # *** compute the tracks *** + for ids in ar_pts: + rpt = ar_pts[ids] + + # align v_ego by a fixed time to align it with the radar measurement + v_lead = rpt[2] + self.v_ego_hist[0] + + # create the track if it doesn't exist or it's a new track + if ids not in self.tracks: + self.tracks[ids] = Track(v_lead, self.kalman_params) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) + + idens = list(sorted(self.tracks.keys())) + track_pts = [self.tracks[iden].get_key_for_cluster() for iden in idens] + + # If we have multiple points, cluster them + if len(track_pts) > 1: + cluster_idxs = cluster_points_centroid(track_pts, 2.5) + clusters = [None] * (max(cluster_idxs) + 1) + + for idx in range(len(track_pts)): + cluster_i = cluster_idxs[idx] + if clusters[cluster_i] is None: + clusters[cluster_i] = Cluster() + clusters[cluster_i].add(self.tracks[idens[idx]]) + elif len(track_pts) == 1: + # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 + cluster_idxs = [0] + clusters = [Cluster()] + clusters[0].add(self.tracks[idens[0]]) + else: + clusters = [] + + # if a new point, reset accel to the rest of the cluster + for idx in range(len(track_pts)): + if self.tracks[idens[idx]].cnt <= 1: + aLeadK = clusters[cluster_idxs[idx]].aLeadK + aLeadTau = clusters[cluster_idxs[idx]].aLeadTau + self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) + + # *** publish radarState *** + dat = messaging.new_message('radarState') + dat.valid = sm.all_checks() and len(rr.errors) == 0 + radarState = dat.radarState + radarState.mdMonoTime = sm.logMonoTime['modelV2'] + radarState.radarErrors = list(rr.errors) + radarState.carStateMonoTime = sm.logMonoTime['carState'] + + leads_v3 = sm['modelV2'].leadsV3 + if len(leads_v3) > 1: + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + return dat diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 34f0f274fe7b6b..359cb4ab2ade4e 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -1,178 +1,13 @@ #!/usr/bin/env python3 import importlib -import math -from collections import defaultdict, deque - import cereal.messaging as messaging + from cereal import car -from common.numpy_fast import interp from common.params import Params from common.realtime import Ratekeeper, Priority, config_realtime_process -from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA +from selfdrive.controls.lib.radar_helpers import RadarD from system.swaglog import cloudlog -from third_party.cluster.fastcluster_py import cluster_points_centroid - - -class KalmanParams(): - def __init__(self, dt): - # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. - # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s - assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s" - self.A = [[1.0, dt], [0.0, 1.0]] - self.C = [1.0, 0.0] - #Q = np.matrix([[10., 0.0], [0.0, 100.]]) - #R = 1e3 - #K = np.matrix([[ 0.05705578], [ 0.03073241]]) - dts = [dt * 0.01 for dt in range(1, 21)] - K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394, - 0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801, - 0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814, - 0.35353899, 0.36200124] - K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219, - 0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714, - 0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557, - 0.26393339, 0.26278425] - self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] - - -def laplacian_pdf(x, mu, b): - b = max(b, 1e-4) - return math.exp(-abs(x-mu)/b) - - -def match_vision_to_cluster(v_ego, lead, clusters): - # match vision point to best statistical cluster match - offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA - - def prob(c): - prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) - prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) - prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) - - # This is isn't exactly right, but good heuristic - return prob_d * prob_y * prob_v - - cluster = max(clusters, key=prob) - - # if no 'sane' match is found return -1 - # stationary radar points can be false positives - dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) - if dist_sane and vel_sane: - return cluster - else: - return None - - -def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): - # Determine leads, this is where the essential logic happens - if len(clusters) > 0 and ready and lead_msg.prob > .5: - cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) - else: - cluster = None - - lead_dict = {'status': False} - if cluster is not None: - lead_dict = cluster.get_RadarState(lead_msg.prob) - elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) - - if low_speed_override: - low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] - if len(low_speed_clusters) > 0: - closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) - - # Only choose new cluster if it is actually closer than the previous one - if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): - lead_dict = closest_cluster.get_RadarState() - - return lead_dict - - -class RadarD(): - def __init__(self, radar_ts, delay=0): - self.current_time = 0 - - self.tracks = defaultdict(dict) - self.kalman_params = KalmanParams(radar_ts) - - # v_ego - self.v_ego = 0. - self.v_ego_hist = deque([0], maxlen=delay+1) - - self.ready = False - - def update(self, sm, rr): - self.current_time = 1e-9*max(sm.logMonoTime.values()) - - if sm.updated['carState']: - self.v_ego = sm['carState'].vEgo - self.v_ego_hist.append(self.v_ego) - if sm.updated['modelV2']: - self.ready = True - - ar_pts = {} - for pt in rr.points: - ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] - - # *** remove missing points from meta data *** - for ids in list(self.tracks.keys()): - if ids not in ar_pts: - self.tracks.pop(ids, None) - - # *** compute the tracks *** - for ids in ar_pts: - rpt = ar_pts[ids] - - # align v_ego by a fixed time to align it with the radar measurement - v_lead = rpt[2] + self.v_ego_hist[0] - - # create the track if it doesn't exist or it's a new track - if ids not in self.tracks: - self.tracks[ids] = Track(v_lead, self.kalman_params) - self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) - - idens = list(sorted(self.tracks.keys())) - track_pts = [self.tracks[iden].get_key_for_cluster() for iden in idens] - - # If we have multiple points, cluster them - if len(track_pts) > 1: - cluster_idxs = cluster_points_centroid(track_pts, 2.5) - clusters = [None] * (max(cluster_idxs) + 1) - - for idx in range(len(track_pts)): - cluster_i = cluster_idxs[idx] - if clusters[cluster_i] is None: - clusters[cluster_i] = Cluster() - clusters[cluster_i].add(self.tracks[idens[idx]]) - elif len(track_pts) == 1: - # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 - cluster_idxs = [0] - clusters = [Cluster()] - clusters[0].add(self.tracks[idens[0]]) - else: - clusters = [] - - # if a new point, reset accel to the rest of the cluster - for idx in range(len(track_pts)): - if self.tracks[idens[idx]].cnt <= 1: - aLeadK = clusters[cluster_idxs[idx]].aLeadK - aLeadTau = clusters[cluster_idxs[idx]].aLeadTau - self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) - - # *** publish radarState *** - dat = messaging.new_message('radarState') - dat.valid = sm.all_checks() and len(rr.errors) == 0 - radarState = dat.radarState - radarState.mdMonoTime = sm.logMonoTime['modelV2'] - radarState.radarErrors = list(rr.errors) - radarState.carStateMonoTime = sm.logMonoTime['carState'] - - leads_v3 = sm['modelV2'].leadsV3 - if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) - return dat + # fuses camera and radar data for best lead detection From 77b975167af316c96655a2b830f386719738a912 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 3 Mar 2023 15:49:07 -0800 Subject: [PATCH 02/12] Revert "refactor radard" This reverts commit 4b3507ff58e753334969b6ba2c2f6b1b35a200cc. --- selfdrive/controls/lib/radar_helpers.py | 166 ----------------------- selfdrive/controls/radard.py | 171 +++++++++++++++++++++++- 2 files changed, 168 insertions(+), 169 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 311da57766ea6f..4bb01792677caa 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,8 +1,3 @@ -import math -import cereal.messaging as messaging -from collections import defaultdict, deque -from third_party.cluster.fastcluster_py import cluster_points_centroid -from common.numpy_fast import interp from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D @@ -19,81 +14,6 @@ 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 KalmanParams(): - def __init__(self, dt): - # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. - # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s - assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s" - self.A = [[1.0, dt], [0.0, 1.0]] - self.C = [1.0, 0.0] - #Q = np.matrix([[10., 0.0], [0.0, 100.]]) - #R = 1e3 - #K = np.matrix([[ 0.05705578], [ 0.03073241]]) - dts = [dt * 0.01 for dt in range(1, 21)] - K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394, - 0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801, - 0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814, - 0.35353899, 0.36200124] - K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219, - 0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714, - 0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557, - 0.26393339, 0.26278425] - self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] - - -def laplacian_pdf(x, mu, b): - b = max(b, 1e-4) - return math.exp(-abs(x-mu)/b) - - -def match_vision_to_cluster(v_ego, lead, clusters): - # match vision point to best statistical cluster match - offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA - - def prob(c): - prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) - prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) - prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) - - # This is isn't exactly right, but good heuristic - return prob_d * prob_y * prob_v - - cluster = max(clusters, key=prob) - - # if no 'sane' match is found return -1 - # stationary radar points can be false positives - dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) - if dist_sane and vel_sane: - return cluster - else: - return None - - -def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): - # Determine leads, this is where the essential logic happens - if len(clusters) > 0 and ready and lead_msg.prob > .5: - cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) - else: - cluster = None - - lead_dict = {'status': False} - if cluster is not None: - lead_dict = cluster.get_RadarState(lead_msg.prob) - elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) - - if low_speed_override: - low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] - if len(low_speed_clusters) > 0: - closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) - - # Only choose new cluster if it is actually closer than the previous one - if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): - lead_dict = closest_cluster.get_RadarState() - - return lead_dict - class Track(): def __init__(self, v_lead, kalman_params): self.cnt = 0 @@ -236,89 +156,3 @@ def potential_low_speed_lead(self, v_ego): def is_potential_fcw(self, model_prob): return model_prob > .9 - - -class RadarD(): - def __init__(self, radar_ts, delay=0): - self.current_time = 0 - - self.tracks = defaultdict(dict) - self.kalman_params = KalmanParams(radar_ts) - - # v_ego - self.v_ego = 0. - self.v_ego_hist = deque([0], maxlen=delay+1) - - self.ready = False - - def update(self, sm, rr): - self.current_time = 1e-9*max(sm.logMonoTime.values()) - - if sm.updated['carState']: - self.v_ego = sm['carState'].vEgo - self.v_ego_hist.append(self.v_ego) - if sm.updated['modelV2']: - self.ready = True - - ar_pts = {} - for pt in rr.points: - ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] - - # *** remove missing points from meta data *** - for ids in list(self.tracks.keys()): - if ids not in ar_pts: - self.tracks.pop(ids, None) - - # *** compute the tracks *** - for ids in ar_pts: - rpt = ar_pts[ids] - - # align v_ego by a fixed time to align it with the radar measurement - v_lead = rpt[2] + self.v_ego_hist[0] - - # create the track if it doesn't exist or it's a new track - if ids not in self.tracks: - self.tracks[ids] = Track(v_lead, self.kalman_params) - self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) - - idens = list(sorted(self.tracks.keys())) - track_pts = [self.tracks[iden].get_key_for_cluster() for iden in idens] - - # If we have multiple points, cluster them - if len(track_pts) > 1: - cluster_idxs = cluster_points_centroid(track_pts, 2.5) - clusters = [None] * (max(cluster_idxs) + 1) - - for idx in range(len(track_pts)): - cluster_i = cluster_idxs[idx] - if clusters[cluster_i] is None: - clusters[cluster_i] = Cluster() - clusters[cluster_i].add(self.tracks[idens[idx]]) - elif len(track_pts) == 1: - # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 - cluster_idxs = [0] - clusters = [Cluster()] - clusters[0].add(self.tracks[idens[0]]) - else: - clusters = [] - - # if a new point, reset accel to the rest of the cluster - for idx in range(len(track_pts)): - if self.tracks[idens[idx]].cnt <= 1: - aLeadK = clusters[cluster_idxs[idx]].aLeadK - aLeadTau = clusters[cluster_idxs[idx]].aLeadTau - self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) - - # *** publish radarState *** - dat = messaging.new_message('radarState') - dat.valid = sm.all_checks() and len(rr.errors) == 0 - radarState = dat.radarState - radarState.mdMonoTime = sm.logMonoTime['modelV2'] - radarState.radarErrors = list(rr.errors) - radarState.carStateMonoTime = sm.logMonoTime['carState'] - - leads_v3 = sm['modelV2'].leadsV3 - if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) - return dat diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 359cb4ab2ade4e..34f0f274fe7b6b 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -1,13 +1,178 @@ #!/usr/bin/env python3 import importlib -import cereal.messaging as messaging +import math +from collections import defaultdict, deque +import cereal.messaging as messaging from cereal import car +from common.numpy_fast import interp from common.params import Params from common.realtime import Ratekeeper, Priority, config_realtime_process -from selfdrive.controls.lib.radar_helpers import RadarD +from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA from system.swaglog import cloudlog - +from third_party.cluster.fastcluster_py import cluster_points_centroid + + +class KalmanParams(): + def __init__(self, dt): + # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. + # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s + assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s" + self.A = [[1.0, dt], [0.0, 1.0]] + self.C = [1.0, 0.0] + #Q = np.matrix([[10., 0.0], [0.0, 100.]]) + #R = 1e3 + #K = np.matrix([[ 0.05705578], [ 0.03073241]]) + dts = [dt * 0.01 for dt in range(1, 21)] + K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394, + 0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801, + 0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814, + 0.35353899, 0.36200124] + K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219, + 0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714, + 0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557, + 0.26393339, 0.26278425] + self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] + + +def laplacian_pdf(x, mu, b): + b = max(b, 1e-4) + return math.exp(-abs(x-mu)/b) + + +def match_vision_to_cluster(v_ego, lead, clusters): + # match vision point to best statistical cluster match + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA + + def prob(c): + prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) + + # This is isn't exactly right, but good heuristic + return prob_d * prob_y * prob_v + + cluster = max(clusters, key=prob) + + # if no 'sane' match is found return -1 + # stationary radar points can be false positives + dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) + if dist_sane and vel_sane: + return cluster + else: + return None + + +def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): + # Determine leads, this is where the essential logic happens + if len(clusters) > 0 and ready and lead_msg.prob > .5: + cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) + else: + cluster = None + + lead_dict = {'status': False} + if cluster is not None: + lead_dict = cluster.get_RadarState(lead_msg.prob) + elif (cluster is None) and ready and (lead_msg.prob > .5): + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + + if low_speed_override: + low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] + if len(low_speed_clusters) > 0: + closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) + + # Only choose new cluster if it is actually closer than the previous one + if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): + lead_dict = closest_cluster.get_RadarState() + + return lead_dict + + +class RadarD(): + def __init__(self, radar_ts, delay=0): + self.current_time = 0 + + self.tracks = defaultdict(dict) + self.kalman_params = KalmanParams(radar_ts) + + # v_ego + self.v_ego = 0. + self.v_ego_hist = deque([0], maxlen=delay+1) + + self.ready = False + + def update(self, sm, rr): + self.current_time = 1e-9*max(sm.logMonoTime.values()) + + if sm.updated['carState']: + self.v_ego = sm['carState'].vEgo + self.v_ego_hist.append(self.v_ego) + if sm.updated['modelV2']: + self.ready = True + + ar_pts = {} + for pt in rr.points: + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] + + # *** remove missing points from meta data *** + for ids in list(self.tracks.keys()): + if ids not in ar_pts: + self.tracks.pop(ids, None) + + # *** compute the tracks *** + for ids in ar_pts: + rpt = ar_pts[ids] + + # align v_ego by a fixed time to align it with the radar measurement + v_lead = rpt[2] + self.v_ego_hist[0] + + # create the track if it doesn't exist or it's a new track + if ids not in self.tracks: + self.tracks[ids] = Track(v_lead, self.kalman_params) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) + + idens = list(sorted(self.tracks.keys())) + track_pts = [self.tracks[iden].get_key_for_cluster() for iden in idens] + + # If we have multiple points, cluster them + if len(track_pts) > 1: + cluster_idxs = cluster_points_centroid(track_pts, 2.5) + clusters = [None] * (max(cluster_idxs) + 1) + + for idx in range(len(track_pts)): + cluster_i = cluster_idxs[idx] + if clusters[cluster_i] is None: + clusters[cluster_i] = Cluster() + clusters[cluster_i].add(self.tracks[idens[idx]]) + elif len(track_pts) == 1: + # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 + cluster_idxs = [0] + clusters = [Cluster()] + clusters[0].add(self.tracks[idens[0]]) + else: + clusters = [] + + # if a new point, reset accel to the rest of the cluster + for idx in range(len(track_pts)): + if self.tracks[idens[idx]].cnt <= 1: + aLeadK = clusters[cluster_idxs[idx]].aLeadK + aLeadTau = clusters[cluster_idxs[idx]].aLeadTau + self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) + + # *** publish radarState *** + dat = messaging.new_message('radarState') + dat.valid = sm.all_checks() and len(rr.errors) == 0 + radarState = dat.radarState + radarState.mdMonoTime = sm.logMonoTime['modelV2'] + radarState.radarErrors = list(rr.errors) + radarState.carStateMonoTime = sm.logMonoTime['carState'] + + leads_v3 = sm['modelV2'].leadsV3 + if len(leads_v3) > 1: + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + return dat # fuses camera and radar data for best lead detection From f9a20ff7fa1f2e5ee986406cf1908af2d9f01810 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 3 Mar 2023 16:16:00 -0800 Subject: [PATCH 03/12] May work --- selfdrive/controls/lib/radar_helpers.py | 16 +++++++++------- selfdrive/controls/radard.py | 9 +++++---- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 4bb01792677caa..05ae25648c3eb0 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,4 +1,4 @@ -from common.numpy_fast import mean +from common.numpy_fast import mean, clip from common.kalman.simple_kalman import KF1D @@ -130,15 +130,17 @@ def get_RadarState(self, model_prob=0.0): "aLeadTau": float(self.aLeadTau) } - def get_RadarState_from_vision(self, lead_msg, v_ego): + def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): + v_ego_error_factor = clip(v_ego / (model_v_ego + 1e-3), 0.7, 1.3) + lead_v_pred = lead_msg.v[0] * v_ego_error_factor return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_msg.v[0] - v_ego), - "vLead": float(lead_msg.v[0]), - "vLeadK": float(lead_msg.v[0]), - "aLeadK": float(0), - "aLeadTau": _LEAD_ACCEL_TAU, + "vRel": float(lead_v_pred - v_ego), + "vLead": float(lead_v_pred), + "vLeadK": float(lead_v_pred), + "aLeadK": float(lead_msg.a), + "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), "radar": False, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 34f0f274fe7b6b..8f2356d157577d 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -64,7 +64,7 @@ def prob(c): return None -def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): +def get_lead(v_ego, ready, clusters, lead_msg, model_v_ego, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) @@ -75,7 +75,7 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): if cluster is not None: lead_dict = cluster.get_RadarState(lead_msg.prob) elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] @@ -169,9 +169,10 @@ def update(self, sm, rr): radarState.carStateMonoTime = sm.logMonoTime['carState'] leads_v3 = sm['modelV2'].leadsV3 + model_v_ego = sm['modelV2'].temporalPose.trans[0] if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], model_v_ego, low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], model_v_ego, low_speed_override=False) return dat From 956b5f0c0d68e3acd8c49707c1b41f8fc0f5ff08 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 3 Mar 2023 17:50:11 -0800 Subject: [PATCH 04/12] No radar for test --- selfdrive/controls/radard.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 8f2356d157577d..8985aa09315a17 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -67,7 +67,7 @@ def prob(c): def get_lead(v_ego, ready, clusters, lead_msg, model_v_ego, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: - cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) + cluster = None #match_vision_to_cluster(v_ego, lead_msg, clusters) else: cluster = None From c8b8a726bd9fad438789e452c17f9c0772662ae8 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Mon, 6 Mar 2023 09:58:02 -0800 Subject: [PATCH 05/12] check length --- selfdrive/controls/radard.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 8985aa09315a17..8ae9dc6b29d5e0 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -168,8 +168,11 @@ def update(self, sm, rr): radarState.radarErrors = list(rr.errors) radarState.carStateMonoTime = sm.logMonoTime['carState'] + if len(sm['modelV2'].temporalPose.trans): + model_v_ego = sm['modelV2'].temporalPose.trans[0] + else: + model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 - model_v_ego = sm['modelV2'].temporalPose.trans[0] if len(leads_v3) > 1: radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], model_v_ego, low_speed_override=True) radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], model_v_ego, low_speed_override=False) From cec461cb5a19643feeb6e1f4955df4dee6bc9219 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Mon, 6 Mar 2023 10:17:47 -0800 Subject: [PATCH 06/12] no accel for now --- selfdrive/controls/lib/radar_helpers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 05ae25648c3eb0..4aafabe9ce2d2d 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -139,7 +139,7 @@ def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): "vRel": float(lead_v_pred - v_ego), "vLead": float(lead_v_pred), "vLeadK": float(lead_v_pred), - "aLeadK": float(lead_msg.a), + "aLeadK": float(0), "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), From 25ac3dd7833c573e91f089ac34b21873877fb08d Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Mon, 6 Mar 2023 10:18:49 -0800 Subject: [PATCH 07/12] First accel --- selfdrive/controls/lib/radar_helpers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 4aafabe9ce2d2d..6da66b43195684 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -139,7 +139,7 @@ def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): "vRel": float(lead_v_pred - v_ego), "vLead": float(lead_v_pred), "vLeadK": float(lead_v_pred), - "aLeadK": float(0), + "aLeadK": float(lead_msg.a[0]), "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), From 244726faa30aa361e215c3f1e82544ae652dd620 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 1 Jun 2023 17:29:12 -0700 Subject: [PATCH 08/12] Cleaner way --- selfdrive/controls/lib/radar_helpers.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 6da66b43195684..2d5866c3914e82 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,4 +1,4 @@ -from common.numpy_fast import mean, clip +from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D @@ -131,14 +131,13 @@ def get_RadarState(self, model_prob=0.0): } def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): - v_ego_error_factor = clip(v_ego / (model_v_ego + 1e-3), 0.7, 1.3) - lead_v_pred = lead_msg.v[0] * v_ego_error_factor + 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_pred - v_ego), - "vLead": float(lead_v_pred), - "vLeadK": float(lead_v_pred), + "vRel": float(lead_v_rel_pred), + "vLead": float(v_ego + lead_v_rel_pred), + "vLeadK": float(v_ego + lead_v_rel_pred), "aLeadK": float(lead_msg.a[0]), "aLeadTau": 0.3, "fcw": False, From 92bbb819745164d6a01bba7f105610ba33abad0f Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 1 Jun 2023 17:31:04 -0700 Subject: [PATCH 09/12] Re-enable radar --- selfdrive/controls/radard.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 8ae9dc6b29d5e0..521cea816f09ac 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -67,7 +67,7 @@ def prob(c): def get_lead(v_ego, ready, clusters, lead_msg, model_v_ego, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: - cluster = None #match_vision_to_cluster(v_ego, lead_msg, clusters) + cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) else: cluster = None From e79cf665982108831d9dc9148696871dad49454d Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 1 Jun 2023 21:13:50 -0700 Subject: [PATCH 10/12] update proc replay --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 52735b686bb2c1..669e06577e0dbd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -29f846f525a4e14f380afd20ae8fa0804011ab6e \ No newline at end of file +1ea15cae51525b739232a2b80efff896c578617d From aa7e161ef57867f049b858bf500b3c21a3c9091c Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 2 Jun 2023 11:35:50 -0700 Subject: [PATCH 11/12] This might cause oscillation --- selfdrive/controls/lib/radar_helpers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 2d5866c3914e82..4184340dc5437f 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -138,7 +138,7 @@ def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): "vRel": float(lead_v_rel_pred), "vLead": float(v_ego + lead_v_rel_pred), "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": float(lead_msg.a[0]), + "aLeadK": 0.0, "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), From 9001a9e8707676bb1513a8df5b645a1f526cccdb Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 2 Jun 2023 12:51:50 -0700 Subject: [PATCH 12/12] Update ref commit --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 669e06577e0dbd..7fa8dd953076e0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1ea15cae51525b739232a2b80efff896c578617d +73549898edd3d9a428fa6f58e25c2c0300290ef2