|
|
|
|
@ -1,9 +1,8 @@ |
|
|
|
|
#include <stdio.h> |
|
|
|
|
#include <stdlib.h> |
|
|
|
|
#include <unistd.h> |
|
|
|
|
#include <mutex> |
|
|
|
|
#include <eigen3/Eigen/Dense> |
|
|
|
|
|
|
|
|
|
#include "visionbuf.h" |
|
|
|
|
#include "visionipc_client.h" |
|
|
|
|
#include "common/swaglog.h" |
|
|
|
|
#include "common/clutil.h" |
|
|
|
|
@ -14,12 +13,12 @@ |
|
|
|
|
|
|
|
|
|
ExitHandler do_exit; |
|
|
|
|
// globals
|
|
|
|
|
bool run_model; |
|
|
|
|
bool live_calib_seen; |
|
|
|
|
mat3 cur_transform; |
|
|
|
|
pthread_mutex_t transform_lock; |
|
|
|
|
std::mutex transform_lock; |
|
|
|
|
|
|
|
|
|
void* live_thread(void *arg) { |
|
|
|
|
set_thread_name("live"); |
|
|
|
|
void calibration_thread() { |
|
|
|
|
set_thread_name("calibration"); |
|
|
|
|
set_realtime_priority(50); |
|
|
|
|
|
|
|
|
|
SubMaster sm({"liveCalibration"}); |
|
|
|
|
@ -37,17 +36,7 @@ void* live_thread(void *arg) { |
|
|
|
|
-1.84808520e-20, 9.00738606e-04,-4.28751576e-02; |
|
|
|
|
|
|
|
|
|
Eigen::Matrix<float, 3, 3> fcam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(fcam_intrinsic_matrix.v); |
|
|
|
|
#ifndef QCOM2 |
|
|
|
|
float db_s = 0.5; // debayering does a 2x downscale
|
|
|
|
|
#else |
|
|
|
|
float db_s = 1.0; |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
mat3 yuv_transform = transform_scale_buffer((mat3){{ |
|
|
|
|
1.0, 0.0, 0.0, |
|
|
|
|
0.0, 1.0, 0.0, |
|
|
|
|
0.0, 0.0, 1.0, |
|
|
|
|
}}, db_s); |
|
|
|
|
const mat3 yuv_transform = get_model_yuv_transform(); |
|
|
|
|
|
|
|
|
|
while (!do_exit) { |
|
|
|
|
if (sm.update(100) > 0){ |
|
|
|
|
@ -70,17 +59,77 @@ void* live_thread(void *arg) { |
|
|
|
|
transform.v[i] = warp_matrix(i / 3, i % 3); |
|
|
|
|
} |
|
|
|
|
mat3 model_transform = matmul3(yuv_transform, transform); |
|
|
|
|
pthread_mutex_lock(&transform_lock); |
|
|
|
|
std::lock_guard lk(transform_lock); |
|
|
|
|
cur_transform = model_transform; |
|
|
|
|
run_model = true; |
|
|
|
|
pthread_mutex_unlock(&transform_lock); |
|
|
|
|
live_calib_seen = true; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void run_model(ModelState &model, VisionIpcClient &vipc_client) { |
|
|
|
|
// messaging
|
|
|
|
|
PubMaster pm({"modelV2", "cameraOdometry"}); |
|
|
|
|
SubMaster sm({"lateralPlan", "roadCameraState"}); |
|
|
|
|
|
|
|
|
|
// setup filter to track dropped frames
|
|
|
|
|
const float dt = 1. / MODEL_FREQ; |
|
|
|
|
const float ts = 10.0; // filter time constant (s)
|
|
|
|
|
const float frame_filter_k = (dt / ts) / (1. + dt / ts); |
|
|
|
|
float frames_dropped = 0; |
|
|
|
|
|
|
|
|
|
uint32_t frame_id = 0, last_vipc_frame_id = 0; |
|
|
|
|
double last = 0; |
|
|
|
|
int desire = -1; |
|
|
|
|
uint32_t run_count = 0; |
|
|
|
|
|
|
|
|
|
while (!do_exit) { |
|
|
|
|
VisionIpcBufExtra extra = {}; |
|
|
|
|
VisionBuf *buf = vipc_client.recv(&extra); |
|
|
|
|
if (buf == nullptr) continue; |
|
|
|
|
|
|
|
|
|
transform_lock.lock(); |
|
|
|
|
mat3 model_transform = cur_transform; |
|
|
|
|
const bool run_model_this_iter = live_calib_seen; |
|
|
|
|
transform_lock.unlock(); |
|
|
|
|
|
|
|
|
|
if (sm.update(0) > 0) { |
|
|
|
|
// TODO: path planner timeout?
|
|
|
|
|
desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); |
|
|
|
|
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (run_model_this_iter) { |
|
|
|
|
run_count++; |
|
|
|
|
|
|
|
|
|
float vec_desire[DESIRE_LEN] = {0}; |
|
|
|
|
if (desire >= 0 && desire < DESIRE_LEN) { |
|
|
|
|
vec_desire[desire] = 1.0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
double mt1 = millis_since_boot(); |
|
|
|
|
ModelDataRaw model_buf = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height, |
|
|
|
|
model_transform, vec_desire); |
|
|
|
|
double mt2 = millis_since_boot(); |
|
|
|
|
float model_execution_time = (mt2 - mt1) / 1000.0; |
|
|
|
|
|
|
|
|
|
// tracked dropped frames
|
|
|
|
|
uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1; |
|
|
|
|
frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U); |
|
|
|
|
if (run_count < 10) frames_dropped = 0; // let frame drops warm up
|
|
|
|
|
float frame_drop_ratio = frames_dropped / (1 + frames_dropped); |
|
|
|
|
|
|
|
|
|
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time, |
|
|
|
|
kj::ArrayPtr<const float>(model.output.data(), model.output.size())); |
|
|
|
|
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof); |
|
|
|
|
|
|
|
|
|
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio); |
|
|
|
|
last = mt1; |
|
|
|
|
last_vipc_frame_id = extra.frame_id; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
return NULL; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
int main(int argc, char **argv) { |
|
|
|
|
int err; |
|
|
|
|
set_realtime_priority(54); |
|
|
|
|
|
|
|
|
|
#ifdef QCOM |
|
|
|
|
@ -90,16 +139,8 @@ int main(int argc, char **argv) { |
|
|
|
|
set_core_affinity(4); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
pthread_mutex_init(&transform_lock, NULL); |
|
|
|
|
|
|
|
|
|
// start calibration thread
|
|
|
|
|
pthread_t live_thread_handle; |
|
|
|
|
err = pthread_create(&live_thread_handle, NULL, live_thread, NULL); |
|
|
|
|
assert(err == 0); |
|
|
|
|
|
|
|
|
|
// messaging
|
|
|
|
|
PubMaster pm({"modelV2", "cameraOdometry"}); |
|
|
|
|
SubMaster sm({"lateralPlan", "roadCameraState"}); |
|
|
|
|
std::thread thread = std::thread(calibration_thread); |
|
|
|
|
|
|
|
|
|
// cl init
|
|
|
|
|
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); |
|
|
|
|
@ -111,90 +152,21 @@ int main(int argc, char **argv) { |
|
|
|
|
LOGW("models loaded, modeld starting"); |
|
|
|
|
|
|
|
|
|
VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context); |
|
|
|
|
|
|
|
|
|
while (!do_exit){ |
|
|
|
|
if (!vipc_client.connect(false)){ |
|
|
|
|
util::sleep_for(100); |
|
|
|
|
continue; |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
while (!do_exit && !vipc_client.connect(false)) { |
|
|
|
|
util::sleep_for(100); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// loop
|
|
|
|
|
while (!do_exit) { |
|
|
|
|
VisionBuf *b = &vipc_client.buffers[0]; |
|
|
|
|
// run the models
|
|
|
|
|
// vipc_client.connected is false only when do_exit is true
|
|
|
|
|
if (vipc_client.connected) { |
|
|
|
|
const VisionBuf *b = &vipc_client.buffers[0]; |
|
|
|
|
LOGW("connected with buffer size: %d (%d x %d)", b->len, b->width, b->height); |
|
|
|
|
|
|
|
|
|
// setup filter to track dropped frames
|
|
|
|
|
const float dt = 1. / MODEL_FREQ; |
|
|
|
|
const float ts = 10.0; // filter time constant (s)
|
|
|
|
|
const float frame_filter_k = (dt / ts) / (1. + dt / ts); |
|
|
|
|
float frames_dropped = 0; |
|
|
|
|
|
|
|
|
|
uint32_t frame_id = 0, last_vipc_frame_id = 0; |
|
|
|
|
double last = 0; |
|
|
|
|
int desire = -1; |
|
|
|
|
uint32_t run_count = 0; |
|
|
|
|
|
|
|
|
|
while (!do_exit) { |
|
|
|
|
VisionIpcBufExtra extra; |
|
|
|
|
VisionBuf *buf = vipc_client.recv(&extra); |
|
|
|
|
if (buf == nullptr){ |
|
|
|
|
continue; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
pthread_mutex_lock(&transform_lock); |
|
|
|
|
mat3 model_transform = cur_transform; |
|
|
|
|
const bool run_model_this_iter = run_model; |
|
|
|
|
pthread_mutex_unlock(&transform_lock); |
|
|
|
|
|
|
|
|
|
if (sm.update(0) > 0){ |
|
|
|
|
// TODO: path planner timeout?
|
|
|
|
|
desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); |
|
|
|
|
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
double mt1 = 0, mt2 = 0; |
|
|
|
|
if (run_model_this_iter) { |
|
|
|
|
run_count++; |
|
|
|
|
|
|
|
|
|
float vec_desire[DESIRE_LEN] = {0}; |
|
|
|
|
if (desire >= 0 && desire < DESIRE_LEN) { |
|
|
|
|
vec_desire[desire] = 1.0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mt1 = millis_since_boot(); |
|
|
|
|
|
|
|
|
|
ModelDataRaw model_buf = |
|
|
|
|
model_eval_frame(&model, buf->buf_cl, buf->width, buf->height, |
|
|
|
|
model_transform, vec_desire); |
|
|
|
|
mt2 = millis_since_boot(); |
|
|
|
|
float model_execution_time = (mt2 - mt1) / 1000.0; |
|
|
|
|
|
|
|
|
|
// tracked dropped frames
|
|
|
|
|
uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1; |
|
|
|
|
frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U); |
|
|
|
|
if (run_count < 10) frames_dropped = 0; // let frame drops warm up
|
|
|
|
|
float frame_drop_ratio = frames_dropped / (1 + frames_dropped); |
|
|
|
|
|
|
|
|
|
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time, |
|
|
|
|
kj::ArrayPtr<const float>(model.output.data(), model.output.size())); |
|
|
|
|
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof); |
|
|
|
|
|
|
|
|
|
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); |
|
|
|
|
last = mt1; |
|
|
|
|
last_vipc_frame_id = extra.frame_id; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
run_model(model, vipc_client); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
model_free(&model); |
|
|
|
|
|
|
|
|
|
LOG("joining live_thread"); |
|
|
|
|
err = pthread_join(live_thread_handle, NULL); |
|
|
|
|
assert(err == 0); |
|
|
|
|
LOG("joining calibration thread"); |
|
|
|
|
thread.join(); |
|
|
|
|
CL_CHECK(clReleaseContext(context)); |
|
|
|
|
pthread_mutex_destroy(&transform_lock); |
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
|
Jason Young
5 years ago
committed by
GitHub