mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-09-30 19:33:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.6.3 release

Browse Source 
old-commit-hash: d5f9caa82d
This commit is contained in:
Vehicle Researcher
2019-08-13 01:36:45 +00:00
parent eb89041a6a
commit 02cedeadd9
95 changed files with 1754 additions and 625 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/visiond/build_from_src.mk
+2 -3
View File
@@ -46,11 +46,10 @@ else
LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/include
LIBYUV_LIBS = $(PHONELIBS)/libyuv/x64/lib/libyuv.a
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lsodium
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/x64/lib/ -l:libczmq.a -l:libzmq.a
OPENCL_LIBS = -lOpenCL
UUID_LIBS = -luuid
TF_FLAGS = -I$(EXTERNAL)/tensorflow/include
TF_LIBS = -L$(EXTERNAL)/tensorflow/lib -ltensorflow \
selfdrive/visiond/models/driving.cc
+36 -29
View File
@@ -4,36 +4,32 @@
#include <unistd.h>
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#include <Eigen/Dense>
#endif
#include "common/timing.h"
#include "driving.h"
#ifdef MEDMODEL
#define MODEL_WIDTH 512
#define MODEL_HEIGHT 256
#define MODEL_NAME "driving_model_dlc"
#else
#define MODEL_WIDTH 320
#define MODEL_HEIGHT 160
#define MODEL_NAME "driving_model_dlc"
#endif
#define MODEL_WIDTH 512
#define MODEL_HEIGHT 256
#define MODEL_NAME "driving_model_dlc"
#define LEAD_MDN_N 5 // probs for 5 groups
#define MDN_VALS 4 // output xyva for each lead group
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
#define MDN_GROUP_SIZE 11
#define SPEED_BUCKETS 100
#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + SPEED_BUCKETS)
#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION)
#ifdef TEMPORAL
#define TEMPORAL_SIZE 512
#else
#define TEMPORAL_SIZE 0
#endif
// #define DUMP_YUV
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> vander;
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
@@ -70,6 +66,13 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform);
#ifdef DUMP_YUV
FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
fclose(dump_yuv_file);
assert(1==2);
#endif
//printf("readinggggg \n");
//FILE *f = fopen("goof_frame", "r");
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
@@ -84,7 +87,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
//net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
ModelData model = {0};
@@ -111,7 +114,11 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
const double max_dist = 140.0;
const double max_rel_vel = 10.0;
// current lead
// Every output distribution from the MDN includes the probabilties
// of it representing a current lead car, a lead car in 2s
// or a lead car in 4s
// Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
@@ -127,8 +134,8 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
// lead in 2s
// Find the distribution that corresponds to the lead in 2s
mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
@@ -150,20 +157,20 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
for (int i=0; i < SPEED_PERCENTILES; i++) {
model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
}
float sum = 0;
for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
sum += net_outputs.speed[idx];
int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
if (idx_percentile < SPEED_PERCENTILES ){
model.speed[idx_percentile] = ((float)idx)/2.0;
}
}
//float sum = 0;
//for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
// sum += net_outputs.speed[idx];
// int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
// if (idx_percentile < SPEED_PERCENTILES ){
// model.speed[idx_percentile] = ((float)idx)/2.0;
// }
//}
// make sure no percentiles are skipped
for (int i=SPEED_PERCENTILES-1; i > 0; i--){
if (model.speed[i-1] > model.speed[i]){
model.speed[i-1] = model.speed[i];
}
}
//for (int i=SPEED_PERCENTILES-1; i > 0; i--){
// if (model.speed[i-1] > model.speed[i]){
// model.speed[i-1] = model.speed[i];
// }
//}
return model;
}
selfdrive/visiond/models/driving.h
-1
View File
@@ -2,7 +2,6 @@
#define MODEL_H
// gate this here
#define MEDMODEL
#define TEMPORAL
#include "common/mat.h"
selfdrive/visiond/models/monitoring.cc
+2
View File
@@ -42,6 +42,8 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob);
memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob);
memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob);
memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob);
return ret;
}
selfdrive/visiond/models/monitoring.h
+3 -1
View File
@@ -9,7 +9,7 @@ extern "C" {
#endif
#define OUTPUT_SIZE_DEPRECATED 8
#define OUTPUT_SIZE 31
#define OUTPUT_SIZE 33
typedef struct MonitoringResult {
float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1];
@@ -20,6 +20,8 @@ typedef struct MonitoringResult {
float face_prob;
float left_eye_prob;
float right_eye_prob;
float left_blink_prob;
float right_blink_prob;
} MonitoringResult;
typedef struct MonitoringState {
selfdrive/visiond/runners/run.h
+3 -2
View File
@@ -7,8 +7,9 @@
#ifdef QCOM
#define DefaultRunModel SNPEModel
#else
#include "tfmodel.h"
#define DefaultRunModel TFModel
#define DefaultRunModel SNPEModel
/* #include "tfmodel.h" */
/* #define DefaultRunModel TFModel */
#endif
#endif
selfdrive/visiond/visiond.cc
+50 -9
View File
@@ -26,6 +26,12 @@
#include <capnp/serialize.h>
#include <jpeglib.h>
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif
#include "common/version.h"
#include "common/util.h"
#include "common/timing.h"
@@ -45,6 +51,7 @@
#include "cameras/camera_frame_stream.h"
#endif
// 3 models
#include "models/driving.h"
#include "models/monitoring.h"
@@ -58,7 +65,7 @@
#define UI_BUF_COUNT 4
//#define DUMP_RGB
// #define DUMP_RGB
//#define DEBUG_DRIVER_MONITOR
@@ -741,6 +748,8 @@ void* monitoring_thread(void *arg) {
framed.setFaceProb(res.face_prob);
framed.setLeftEyeProb(res.left_eye_prob);
framed.setRightEyeProb(res.right_eye_prob);
framed.setLeftBlinkProb(res.left_blink_prob);
framed.setRightBlinkProb(res.right_blink_prob);
auto words = capnp::messageToFlatArray(msg);
@@ -895,6 +904,8 @@ void* processing_thread(void *arg) {
#endif
#ifdef DUMP_RGB
s->rgb_width = s->frame_width;
s->rgb_height = s->frame_height;
FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb");
#endif
@@ -959,6 +970,8 @@ void* processing_thread(void *arg) {
#ifdef DUMP_RGB
if (cnt % 20 == 0) {
fwrite(bgr_ptr, s->rgb_buf_size, 1, dump_rgb_file);
LOG("%d x %d", s->rgb_width, s->rgb_height);
assert(1==2);
}
#endif
@@ -1202,6 +1215,24 @@ void* live_thread(void *arg) {
zpoller_t *poller = zpoller_new(liveCalibration_sock, terminate, NULL);
assert(poller);
/*
import numpy as np
from common.transformations.model import medmodel_frame_from_road_frame
medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
*/
Eigen::Matrix<float, 3, 3> ground_from_medmodel_frame;
ground_from_medmodel_frame <<
0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
-1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
-1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
Eigen::Matrix<float, 3, 3> eon_intrinsics;
eon_intrinsics <<
910.0, 0.0, 582.0,
0.0, 910.0, 437.0,
0.0, 0.0, 1.0;
while (!do_exit) {
zsock_t *which = (zsock_t*)zpoller_wait(poller, -1);
if (which == terminate || which == NULL) {
@@ -1226,15 +1257,25 @@ void* live_thread(void *arg) {
if (event.isLiveCalibration()) {
pthread_mutex_lock(&s->transform_lock);
#ifdef MEDMODEL
auto wm2 = event.getLiveCalibration().getWarpMatrixBig();
#else
auto wm2 = event.getLiveCalibration().getWarpMatrix2();
#endif
assert(wm2.size() == 3*3);
for (int i=0; i<3*3; i++) {
s->cur_transform.v[i] = wm2[i];
auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix();
Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
for (int i = 0; i < 4*3; i++){
extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
}
auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen;
Eigen::Matrix<float, 3, 3> camera_frame_from_ground;
camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3);
auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame;
for (int i=0; i<3*3; i++) {
s->cur_transform.v[i] = warp_matrix(i / 3, i % 3);
}
s->run_model = true;
pthread_mutex_unlock(&s->transform_lock);
}