add webcam to cameras (#1201)
ZwX1616
5 years ago
committed by
GitHub
parent
4e4bea8525
commit
a95e61edf4
19 changed files with 605 additions and 86 deletions
@ -0,0 +1,269 @@ |
||||
#include "camera_webcam.h" |
||||
|
||||
#include <unistd.h> |
||||
#include <string.h> |
||||
#include <signal.h> |
||||
#include <pthread.h> |
||||
|
||||
#include "common/util.h" |
||||
#include "common/timing.h" |
||||
#include "common/swaglog.h" |
||||
#include "buffering.h" |
||||
|
||||
#include <opencv2/opencv.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/core.hpp> |
||||
#include <opencv2/videoio.hpp> |
||||
|
||||
extern volatile sig_atomic_t do_exit; |
||||
|
||||
#define FRAME_WIDTH 1164 |
||||
#define FRAME_HEIGHT 874 |
||||
#define FRAME_WIDTH_FRONT 1152 |
||||
#define FRAME_HEIGHT_FRONT 864 |
||||
|
||||
namespace { |
||||
void camera_open(CameraState *s, VisionBuf *camera_bufs, bool rear) { |
||||
assert(camera_bufs); |
||||
s->camera_bufs = camera_bufs; |
||||
} |
||||
|
||||
void camera_close(CameraState *s) { |
||||
tbuffer_stop(&s->camera_tb); |
||||
} |
||||
|
||||
void camera_release_buffer(void *cookie, int buf_idx) { |
||||
CameraState *s = static_cast<CameraState *>(cookie); |
||||
} |
||||
|
||||
void camera_init(CameraState *s, int camera_id, unsigned int fps) { |
||||
assert(camera_id < ARRAYSIZE(cameras_supported)); |
||||
s->ci = cameras_supported[camera_id]; |
||||
assert(s->ci.frame_width != 0); |
||||
|
||||
s->frame_size = s->ci.frame_height * s->ci.frame_stride; |
||||
s->fps = fps; |
||||
|
||||
tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s); |
||||
} |
||||
|
||||
static void* rear_thread(void *arg) { |
||||
int err; |
||||
|
||||
set_thread_name("webcam_rear_thread"); |
||||
CameraState* s = (CameraState*)arg; |
||||
|
||||
cv::VideoCapture cap_rear(1); // road
|
||||
cap_rear.set(cv::CAP_PROP_FRAME_WIDTH, 1280); |
||||
cap_rear.set(cv::CAP_PROP_FRAME_HEIGHT, 720); |
||||
cap_rear.set(cv::CAP_PROP_FPS, s->fps); |
||||
cap_rear.set(cv::CAP_PROP_AUTOFOCUS, 0); // off
|
||||
cap_rear.set(cv::CAP_PROP_FOCUS, 0); // 0 - 255?
|
||||
// cv::Rect roi_rear(160, 0, 960, 720);
|
||||
|
||||
cv::Size size; |
||||
size.height = s->ci.frame_height; |
||||
size.width = s->ci.frame_width; |
||||
|
||||
// transforms calculation see tools/webcam/warp_vis.py
|
||||
float ts[9] = {1.00220264, 0.0, -59.40969163,
|
||||
0.0, 1.00220264, 76.20704846,
|
||||
0.0, 0.0, 1.0}; |
||||
const cv::Mat transform = cv::Mat(3, 3, CV_32F, ts); |
||||
|
||||
if (!cap_rear.isOpened()) { |
||||
err = 1; |
||||
} |
||||
|
||||
uint32_t frame_id = 0; |
||||
TBuffer* tb = &s->camera_tb; |
||||
|
||||
while (!do_exit) { |
||||
cv::Mat frame_mat; |
||||
cv::Mat transformed_mat; |
||||
|
||||
cap_rear >> frame_mat; |
||||
|
||||
// int rows = frame_mat.rows;
|
||||
// int cols = frame_mat.cols;
|
||||
// printf("Raw Rear, R=%d, C=%d\n", rows, cols);
|
||||
|
||||
cv::warpPerspective(frame_mat, transformed_mat, transform, size, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); |
||||
|
||||
int transformed_size = transformed_mat.total() * transformed_mat.elemSize(); |
||||
|
||||
const int buf_idx = tbuffer_select(tb); |
||||
s->camera_bufs_metadata[buf_idx] = { |
||||
.frame_id = frame_id, |
||||
}; |
||||
|
||||
cl_command_queue q = s->camera_bufs[buf_idx].copy_q; |
||||
cl_mem yuv_cl = s->camera_bufs[buf_idx].buf_cl; |
||||
cl_event map_event; |
||||
void *yuv_buf = (void *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, |
||||
CL_MAP_WRITE, 0, transformed_size, |
||||
0, NULL, &map_event, &err); |
||||
assert(err == 0); |
||||
clWaitForEvents(1, &map_event); |
||||
clReleaseEvent(map_event); |
||||
memcpy(yuv_buf, transformed_mat.data, transformed_size); |
||||
|
||||
clEnqueueUnmapMemObject(q, yuv_cl, yuv_buf, 0, NULL, &map_event); |
||||
clWaitForEvents(1, &map_event); |
||||
clReleaseEvent(map_event); |
||||
tbuffer_dispatch(tb, buf_idx); |
||||
|
||||
frame_id += 1; |
||||
frame_mat.release(); |
||||
transformed_mat.release(); |
||||
} |
||||
|
||||
cap_rear.release(); |
||||
return NULL; |
||||
} |
||||
|
||||
void front_thread(CameraState *s) { |
||||
int err; |
||||
|
||||
cv::VideoCapture cap_front(2); // driver
|
||||
cap_front.set(cv::CAP_PROP_FRAME_WIDTH, 1280); |
||||
cap_front.set(cv::CAP_PROP_FRAME_HEIGHT, 720); |
||||
cap_front.set(cv::CAP_PROP_FPS, s->fps); |
||||
// cv::Rect roi_front(320, 0, 960, 720);
|
||||
|
||||
cv::Size size; |
||||
size.height = s->ci.frame_height; |
||||
size.width = s->ci.frame_width; |
||||
|
||||
// transforms calculation see tools/webcam/warp_vis.py
|
||||
float ts[9] = {0.94713656, 0.0, -30.16740088,
|
||||
0.0, 0.94713656, 91.030837,
|
||||
0.0, 0.0, 1.0}; |
||||
const cv::Mat transform = cv::Mat(3, 3, CV_32F, ts); |
||||
|
||||
if (!cap_front.isOpened()) { |
||||
err = 1; |
||||
} |
||||
|
||||
uint32_t frame_id = 0; |
||||
TBuffer* tb = &s->camera_tb; |
||||
|
||||
while (!do_exit) { |
||||
cv::Mat frame_mat; |
||||
cv::Mat transformed_mat; |
||||
|
||||
cap_front >> frame_mat; |
||||
|
||||
// int rows = frame_mat.rows;
|
||||
// int cols = frame_mat.cols;
|
||||
// printf("Raw Front, R=%d, C=%d\n", rows, cols);
|
||||
|
||||
cv::warpPerspective(frame_mat, transformed_mat, transform, size, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); |
||||
|
||||
int transformed_size = transformed_mat.total() * transformed_mat.elemSize(); |
||||
|
||||
const int buf_idx = tbuffer_select(tb); |
||||
s->camera_bufs_metadata[buf_idx] = { |
||||
.frame_id = frame_id, |
||||
}; |
||||
|
||||
cl_command_queue q = s->camera_bufs[buf_idx].copy_q; |
||||
cl_mem yuv_cl = s->camera_bufs[buf_idx].buf_cl; |
||||
cl_event map_event; |
||||
void *yuv_buf = (void *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, |
||||
CL_MAP_WRITE, 0, transformed_size, |
||||
0, NULL, &map_event, &err); |
||||
assert(err == 0); |
||||
clWaitForEvents(1, &map_event); |
||||
clReleaseEvent(map_event); |
||||
memcpy(yuv_buf, transformed_mat.data, transformed_size); |
||||
|
||||
clEnqueueUnmapMemObject(q, yuv_cl, yuv_buf, 0, NULL, &map_event); |
||||
clWaitForEvents(1, &map_event); |
||||
clReleaseEvent(map_event); |
||||
tbuffer_dispatch(tb, buf_idx); |
||||
|
||||
frame_id += 1; |
||||
frame_mat.release(); |
||||
transformed_mat.release(); |
||||
} |
||||
|
||||
cap_front.release(); |
||||
return; |
||||
} |
||||
|
||||
} // namespace
|
||||
|
||||
CameraInfo cameras_supported[CAMERA_ID_MAX] = { |
||||
// road facing
|
||||
[CAMERA_ID_LGC920] = { |
||||
.frame_width = FRAME_WIDTH, |
||||
.frame_height = FRAME_HEIGHT, |
||||
.frame_stride = FRAME_WIDTH*3, |
||||
.bayer = false, |
||||
.bayer_flip = false, |
||||
}, |
||||
// driver facing
|
||||
[CAMERA_ID_LGC615] = { |
||||
.frame_width = FRAME_WIDTH_FRONT, |
||||
.frame_height = FRAME_HEIGHT_FRONT, |
||||
.frame_stride = FRAME_WIDTH_FRONT*3, |
||||
.bayer = false, |
||||
.bayer_flip = false, |
||||
}, |
||||
}; |
||||
|
||||
void cameras_init(DualCameraState *s) { |
||||
memset(s, 0, sizeof(*s)); |
||||
|
||||
camera_init(&s->rear, CAMERA_ID_LGC920, 20); |
||||
s->rear.transform = (mat3){{ |
||||
1.0, 0.0, 0.0, |
||||
0.0, 1.0, 0.0, |
||||
0.0, 0.0, 1.0, |
||||
}}; |
||||
|
||||
camera_init(&s->front, CAMERA_ID_LGC615, 10); |
||||
s->front.transform = (mat3){{ |
||||
1.0, 0.0, 0.0, |
||||
0.0, 1.0, 0.0, |
||||
0.0, 0.0, 1.0, |
||||
}}; |
||||
} |
||||
|
||||
void camera_autoexposure(CameraState *s, float grey_frac) {} |
||||
|
||||
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, |
||||
VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, |
||||
VisionBuf *camera_bufs_front) { |
||||
assert(camera_bufs_rear); |
||||
assert(camera_bufs_front); |
||||
int err; |
||||
|
||||
// LOG("*** open front ***");
|
||||
camera_open(&s->front, camera_bufs_front, false); |
||||
|
||||
// LOG("*** open rear ***");
|
||||
camera_open(&s->rear, camera_bufs_rear, true); |
||||
} |
||||
|
||||
void cameras_close(DualCameraState *s) { |
||||
camera_close(&s->rear); |
||||
camera_close(&s->front); |
||||
} |
||||
|
||||
void cameras_run(DualCameraState *s) { |
||||
set_thread_name("webcam_thread"); |
||||
|
||||
int err; |
||||
pthread_t rear_thread_handle; |
||||
err = pthread_create(&rear_thread_handle, NULL, |
||||
rear_thread, &s->rear); |
||||
assert(err == 0); |
||||
|
||||
front_thread(&s->front); |
||||
|
||||
err = pthread_join(rear_thread_handle, NULL); |
||||
assert(err == 0); |
||||
cameras_close(s); |
||||
} |
||||
@ -0,0 +1,58 @@ |
||||
#ifndef CAMERA_WEBCAM |
||||
#define CAMERA_WEBCAM |
||||
|
||||
#include <stdbool.h> |
||||
|
||||
#ifdef __APPLE__ |
||||
#include <OpenCL/cl.h> |
||||
#else |
||||
#include <CL/cl.h> |
||||
#endif |
||||
|
||||
#include "common/mat.h" |
||||
|
||||
#include "buffering.h" |
||||
#include "common/visionbuf.h" |
||||
#include "camera_common.h" |
||||
|
||||
#define FRAME_BUF_COUNT 16 |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
typedef struct CameraState { |
||||
int camera_id; |
||||
CameraInfo ci; |
||||
int frame_size; |
||||
|
||||
VisionBuf *camera_bufs; |
||||
FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; |
||||
TBuffer camera_tb; |
||||
|
||||
int fps; |
||||
float digital_gain; |
||||
|
||||
float cur_gain_frac; |
||||
|
||||
mat3 transform; |
||||
} CameraState; |
||||
|
||||
|
||||
typedef struct DualCameraState { |
||||
int ispif_fd; |
||||
|
||||
CameraState rear; |
||||
CameraState front; |
||||
} DualCameraState; |
||||
|
||||
void cameras_init(DualCameraState *s); |
||||
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); |
||||
void cameras_run(DualCameraState *s); |
||||
void cameras_close(DualCameraState *s); |
||||
void camera_autoexposure(CameraState *s, float grey_frac); |
||||
#ifdef __cplusplus |
||||
} // extern "C"
|
||||
#endif |
||||
|
||||
#endif |
||||
@ -0,0 +1,43 @@ |
||||
Run openpilot with webcam on PC/laptop |
||||
===================== |
||||
What's needed: |
||||
- Ubuntu 16.04 |
||||
- Python 3.7.3 |
||||
- GPU (recommended) |
||||
- Two USB webcams, at least 720p and 78 degrees FOV (e.g. Logitech C920/C615) |
||||
- [Car harness](https://comma.ai/shop/products/comma-car-harness) w/ black panda (or the outdated grey panda/giraffe combo) to connect to your car |
||||
- [Panda paw](https://comma.ai/shop/products/panda-paw) (or USB-A to USB-A cable) to connect panda to your computer |
||||
- Tape, Charger, ... |
||||
That's it! |
||||
|
||||
## Clone openpilot and install the requirements |
||||
``` |
||||
cd ~ |
||||
git clone https://github.com/commaai/openpilot.git |
||||
# Follow [this readme](https://github.com/commaai/openpilot/tree/master/tools) to install the requirements |
||||
# Add line "export PYTHONPATH=$HOME/openpilot" to your ~/.bashrc |
||||
# You may also need to install tensorflow-gpu 2.0.0 |
||||
# Install [OpenCV4](https://www.pyimagesearch.com/2018/08/15/how-to-install-opencv-4-on-ubuntu/) |
||||
``` |
||||
## Build openpilot for webcam |
||||
``` |
||||
cd ~/openpilot |
||||
scons use_webcam=1 |
||||
touch prebuilt |
||||
``` |
||||
## Connect the hardwares |
||||
``` |
||||
# Connect the road facing camera first, then the driver facing camera |
||||
# (default indexes are 1 and 2; can be modified in selfdrive/camerad/cameras/camera_webcam.cc) |
||||
# Connect your computer to panda |
||||
``` |
||||
## GO |
||||
``` |
||||
cd ~/openpilot/tools/webcam |
||||
./accept_terms.py # accept the user terms so that thermald can detect the car started |
||||
cd ~/openpilot/selfdrive |
||||
PASSIVE=0 NOSENSOR=1 ./manager.py |
||||
# Start the car, then the UI should show the road webcam's view |
||||
# Adjust and secure the webcams (you can run tools/webcam/front_mount_helper.py to help mount the driver camera) |
||||
# Finish calibration and engage! |
||||
``` |
||||
@ -0,0 +1,9 @@ |
||||
#!/usr/bin/env python |
||||
from common.params import Params |
||||
from selfdrive.version import terms_version, training_version |
||||
|
||||
if __name__ == '__main__': |
||||
params = Params() |
||||
params.put("HasAcceptedTerms", str(terms_version, 'utf-8')) |
||||
params.put("CompletedTrainingVersion", str(training_version, 'utf-8')) |
||||
print("Terms Accepted!") |
||||
@ -0,0 +1,35 @@ |
||||
#!/usr/bin/env python |
||||
import numpy as np |
||||
|
||||
# copied from common.transformations/camera.py |
||||
eon_dcam_focal_length = 860.0 # pixels |
||||
webcam_focal_length = 908.0 # pixels |
||||
|
||||
eon_dcam_intrinsics = np.array([ |
||||
[eon_dcam_focal_length, 0, 1152/2.], |
||||
[ 0, eon_dcam_focal_length, 864/2.], |
||||
[ 0, 0, 1]]) |
||||
|
||||
webcam_intrinsics = np.array([ |
||||
[webcam_focal_length, 0., 1280/2.], |
||||
[ 0., webcam_focal_length, 720/2.], |
||||
[ 0., 0., 1.]]) |
||||
|
||||
cam_id = 2 |
||||
|
||||
if __name__ == "__main__": |
||||
import cv2 |
||||
|
||||
trans_webcam_to_eon_front = np.dot(eon_dcam_intrinsics,np.linalg.inv(webcam_intrinsics)) |
||||
|
||||
cap = cv2.VideoCapture(cam_id) |
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) |
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) |
||||
|
||||
while (True): |
||||
ret, img = cap.read() |
||||
if ret: |
||||
img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1152,864), borderMode=cv2.BORDER_CONSTANT, borderValue=0) |
||||
img = img[:,-864//2:,:] |
||||
cv2.imshow('preview', img) |
||||
cv2.waitKey(10) |
||||
@ -0,0 +1,45 @@ |
||||
#!/usr/bin/env python |
||||
import numpy as np |
||||
|
||||
# copied from common.transformations/camera.py |
||||
eon_focal_length = 910.0 # pixels |
||||
eon_dcam_focal_length = 860.0 # pixels |
||||
|
||||
webcam_focal_length = 908.0 # pixels |
||||
|
||||
eon_intrinsics = np.array([ |
||||
[eon_focal_length, 0., 1164/2.], |
||||
[ 0., eon_focal_length, 874/2.], |
||||
[ 0., 0., 1.]]) |
||||
|
||||
eon_dcam_intrinsics = np.array([ |
||||
[eon_dcam_focal_length, 0, 1152/2.], |
||||
[ 0, eon_dcam_focal_length, 864/2.], |
||||
[ 0, 0, 1]]) |
||||
|
||||
webcam_intrinsics = np.array([ |
||||
[webcam_focal_length, 0., 1280/2.], |
||||
[ 0., webcam_focal_length, 720/2.], |
||||
[ 0., 0., 1.]]) |
||||
|
||||
if __name__ == "__main__": |
||||
import cv2 |
||||
trans_webcam_to_eon_rear = np.dot(eon_intrinsics,np.linalg.inv(webcam_intrinsics)) |
||||
trans_webcam_to_eon_front = np.dot(eon_dcam_intrinsics,np.linalg.inv(webcam_intrinsics)) |
||||
print("trans_webcam_to_eon_rear:\n", trans_webcam_to_eon_rear) |
||||
print("trans_webcam_to_eon_front:\n", trans_webcam_to_eon_front) |
||||
|
||||
cap = cv2.VideoCapture(1) |
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) |
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) |
||||
|
||||
while (True): |
||||
ret, img = cap.read() |
||||
if ret: |
||||
# img = cv2.warpPerspective(img, trans_webcam_to_eon_rear, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0) |
||||
img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0) |
||||
print(img.shape, end='\r') |
||||
cv2.imshow('preview', img) |
||||
cv2.waitKey(10) |
||||
|
||||
|
||||
Loading…
Reference in new issue