@ -10,8 +10,6 @@
# include "driving.h"
# include "clutil.h"
constexpr int MODEL_PATH_DISTANCE = 192 ;
constexpr int POLYFIT_DEGREE = 4 ;
constexpr int DESIRE_PRED_SIZE = 32 ;
constexpr int OTHER_META_SIZE = 4 ;
@ -32,7 +30,6 @@ constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
constexpr int POSE_SIZE = 12 ;
constexpr int MIN_VALID_LEN = 10 ;
constexpr int PLAN_IDX = 0 ;
constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N * PLAN_MHP_GROUP_SIZE ;
constexpr int LL_PROB_IDX = LL_IDX + 4 * 2 * 2 * 33 ;
@ -51,8 +48,6 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
// #define DUMP_YUV
Eigen : : Matrix < float , MODEL_PATH_DISTANCE , POLYFIT_DEGREE - 1 > vander ;
void model_init ( ModelState * s , cl_device_id device_id , cl_context context ) {
frame_init ( & s - > frame , MODEL_WIDTH , MODEL_HEIGHT , device_id , context ) ;
s - > input_frames = std : : make_unique < float [ ] > ( MODEL_FRAME_SIZE * 2 ) ;
@ -75,13 +70,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
s - > m - > addTrafficConvention ( s - > traffic_convention , TRAFFIC_CONVENTION_LEN ) ;
# endif
// Build Vandermonde matrix
for ( int i = 0 ; i < TRAJECTORY_SIZE ; i + + ) {
for ( int j = 0 ; j < POLYFIT_DEGREE - 1 ; j + + ) {
vander ( i , j ) = pow ( X_IDXS [ i ] , POLYFIT_DEGREE - j - 1 ) ;
}
}
s - > q = CL_CHECK_ERR ( clCreateCommandQueue ( context , device_id , 0 , & err ) ) ;
}
@ -136,31 +124,6 @@ void model_free(ModelState* s) {
CL_CHECK ( clReleaseCommandQueue ( s - > q ) ) ;
}
void poly_fit ( float * in_pts , float * in_stds , float * out , int valid_len ) {
// References to inputs
Eigen : : Map < Eigen : : Matrix < float , Eigen : : Dynamic , 1 > > pts ( in_pts , valid_len ) ;
Eigen : : Map < Eigen : : Matrix < float , Eigen : : Dynamic , 1 > > std ( in_stds , valid_len ) ;
Eigen : : Map < Eigen : : Matrix < float , POLYFIT_DEGREE - 1 , 1 > > p ( out , POLYFIT_DEGREE - 1 ) ;
float y0 = pts [ 0 ] ;
pts = pts . array ( ) - y0 ;
// Build Least Squares equations
Eigen : : Matrix < float , Eigen : : Dynamic , POLYFIT_DEGREE - 1 > lhs = vander . topRows ( valid_len ) . array ( ) . colwise ( ) / std . array ( ) ;
Eigen : : Matrix < float , Eigen : : Dynamic , 1 > rhs = pts . array ( ) / std . array ( ) ;
// Improve numerical stability
Eigen : : Matrix < float , POLYFIT_DEGREE - 1 , 1 > scale = 1. / ( lhs . array ( ) * lhs . array ( ) ) . sqrt ( ) . colwise ( ) . sum ( ) ;
lhs = lhs * scale . asDiagonal ( ) ;
// Solve inplace
p = lhs . colPivHouseholderQr ( ) . solve ( rhs ) ;
// Apply scale to output
p = p . transpose ( ) * scale . asDiagonal ( ) ;
out [ 3 ] = y0 ;
}
static const float * get_best_data ( const float * data , int size , int group_size , int offset ) {
int max_idx = 0 ;
for ( int i = 1 ; i < size ; i + + ) {
@ -180,30 +143,6 @@ static const float *get_lead_data(const float *lead, int t_offset) {
return get_best_data ( lead , LEAD_MHP_N , LEAD_MHP_GROUP_SIZE , t_offset - LEAD_MHP_SELECTION ) ;
}
void fill_path ( cereal : : ModelData : : PathData : : Builder path , const float * data , const float prob ,
float valid_len , int valid_len_idx , int ll_idx ) {
float points [ TRAJECTORY_SIZE ] = { } ;
float stds [ TRAJECTORY_SIZE ] = { } ;
float poly [ POLYFIT_DEGREE ] = { } ;
for ( int i = 0 ; i < TRAJECTORY_SIZE ; i + + ) {
// negative sign because mpc has left positive
if ( ll_idx = = 0 ) {
points [ i ] = - data [ 30 * i + 16 ] ;
stds [ i ] = exp ( data [ 30 * ( 33 + i ) + 16 ] ) ;
} else {
points [ i ] = - data [ 2 * 33 * ll_idx + 2 * i ] ;
stds [ i ] = exp ( data [ 2 * 33 * ( 4 + ll_idx ) + 2 * i ] ) ;
}
}
const float std = stds [ 0 ] ;
poly_fit ( points , stds , poly , valid_len_idx ) ;
path . setPoly ( poly ) ;
path . setProb ( prob ) ;
path . setStd ( std ) ;
path . setValidLen ( valid_len ) ;
}
void fill_lead_v2 ( cereal : : ModelDataV2 : : LeadDataV2 : : Builder lead , const float * lead_data , const float * prob , int t_offset , float t ) {
const float * data = get_lead_data ( lead_data , t_offset ) ;
@ -212,27 +151,13 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *le
float xyva_arr [ LEAD_MHP_VALS ] ;
float xyva_stds_arr [ LEAD_MHP_VALS ] ;
for ( int i = 0 ; i < LEAD_MHP_VALS ; i + + ) {
xyva_arr [ i ] = data [ LEAD_MHP_VALS + i ] ;
xyva_arr [ i ] = data [ i ] ;
xyva_stds_arr [ i ] = exp ( data [ LEAD_MHP_VALS + i ] ) ;
}
lead . setXyva ( xyva_arr ) ;
lead . setXyvaStd ( xyva_stds_arr ) ;
}
void fill_lead ( cereal : : ModelData : : LeadData : : Builder lead , const float * lead_data , const float * prob , int t_offset ) {
const float * data = get_lead_data ( lead_data , t_offset ) ;
lead . setProb ( sigmoid ( prob [ t_offset ] ) ) ;
lead . setDist ( data [ 0 ] ) ;
lead . setStd ( exp ( data [ LEAD_MHP_VALS ] ) ) ;
// TODO make all msgs same format
lead . setRelY ( - data [ 1 ] ) ;
lead . setRelYStd ( exp ( data [ LEAD_MHP_VALS + 1 ] ) ) ;
lead . setRelVel ( data [ 2 ] ) ;
lead . setRelVelStd ( exp ( data [ LEAD_MHP_VALS + 2 ] ) ) ;
lead . setRelA ( data [ 3 ] ) ;
lead . setRelAStd ( exp ( data [ LEAD_MHP_VALS + 3 ] ) ) ;
}
template < class MetaBuilder >
void fill_meta ( MetaBuilder meta , const float * meta_data ) {
float desire_state_softmax [ DESIRE_LEN ] ;
@ -332,54 +257,22 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
}
}
void fill_model ( cereal : : ModelData : : Builder & framed , const ModelDataRaw & net_outputs ) {
// Find the distribution that corresponds to the most probable plan
const float * best_plan = get_plan_data ( net_outputs . plan ) ;
// x pos at 10s is a good valid_len
float valid_len = 0 ;
for ( int i = 1 ; i < TRAJECTORY_SIZE ; i + + ) {
if ( const float len = best_plan [ 30 * i ] ; len > = valid_len ) {
valid_len = len ;
}
}
// clamp to 10 and MODEL_PATH_DISTANCE
valid_len = fmin ( MODEL_PATH_DISTANCE , fmax ( MIN_VALID_LEN , valid_len ) ) ;
int valid_len_idx = 0 ;
for ( int i = 1 ; i < TRAJECTORY_SIZE ; i + + ) {
if ( valid_len > = X_IDXS [ valid_len_idx ] ) {
valid_len_idx = i ;
}
}
fill_path ( framed . initPath ( ) , best_plan , 1.0 , valid_len , valid_len_idx , 0 ) ;
fill_path ( framed . initLeftLane ( ) , net_outputs . lane_lines , sigmoid ( net_outputs . lane_lines_prob [ 1 ] ) , valid_len , valid_len_idx , 1 ) ;
fill_path ( framed . initRightLane ( ) , net_outputs . lane_lines , sigmoid ( net_outputs . lane_lines_prob [ 2 ] ) , valid_len , valid_len_idx , 2 ) ;
fill_lead ( framed . initLead ( ) , net_outputs . lead , net_outputs . lead_prob , 0 ) ;
fill_lead ( framed . initLeadFuture ( ) , net_outputs . lead , net_outputs . lead_prob , 1 ) ;
fill_meta ( framed . initMeta ( ) , net_outputs . meta ) ;
}
void model_publish ( PubMaster & pm , uint32_t vipc_frame_id , uint32_t frame_id , float frame_drop ,
const ModelDataRaw & net_outputs , const float * raw_pred , uint64_t timestamp_eof ,
float model_execution_time ) {
const uint32_t frame_age = ( frame_id > vipc_frame_id ) ? ( frame_id - vipc_frame_id ) : 0 ;
auto do_publish = [ & ] ( auto init_model_func , const char * pub_name ) {
MessageBuilder msg ;
auto framed = ( msg . initEvent ( ) . * ( init_model_func ) ) ( ) ;
framed . setFrameId ( vipc_frame_id ) ;
framed . setFrameAge ( frame_age ) ;
framed . setFrameDropPerc ( frame_drop * 100 ) ;
framed . setTimestampEof ( timestamp_eof ) ;
framed . setModelExecutionTime ( model_execution_time ) ;
if ( send_raw_pred ) {
framed . setRawPred ( kj : : arrayPtr ( ( const uint8_t * ) raw_pred , ( OUTPUT_SIZE + TEMPORAL_SIZE ) * sizeof ( float ) ) ) ;
}
fill_model ( framed , net_outputs ) ;
pm . send ( pub_name , msg ) ;
} ;
do_publish ( & cereal : : Event : : Builder : : initModel , " model " ) ;
do_publish ( & cereal : : Event : : Builder : : initModelV2 , " modelV2 " ) ;
MessageBuilder msg ;
auto framed = msg . initEvent ( ) . initModelV2 ( ) ;
framed . setFrameId ( vipc_frame_id ) ;
framed . setFrameAge ( frame_age ) ;
framed . setFrameDropPerc ( frame_drop * 100 ) ;
framed . setTimestampEof ( timestamp_eof ) ;
framed . setModelExecutionTime ( model_execution_time ) ;
if ( send_raw_pred ) {
framed . setRawPred ( kj : : arrayPtr ( ( const uint8_t * ) raw_pred , ( OUTPUT_SIZE + TEMPORAL_SIZE ) * sizeof ( float ) ) ) ;
}
fill_model ( framed , net_outputs ) ;
pm . send ( " modelV2 " , msg ) ;
}
void posenet_publish ( PubMaster & pm , uint32_t vipc_frame_id , uint32_t vipc_dropped_frames ,
HaraldSchafer
4 years ago
committed by
GitHub