00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "camera_caffe_main.h"
00029
00030 using namespace Argus;
00031 using namespace EGLStream;
00032
00033 static camera_caffe_context ctx;
00034
00035
00036 #define MAX_PENDING_FRAMES (3)
00037 #define DEFAULT_FPS (30)
00038
00039
00040 static int CAPTURE_TIME = 30;
00041 static bool DO_STAT = false;
00042
00043
00044 #define PRODUCER_PRINT(...) printf("PRODUCER: " __VA_ARGS__)
00045 #define CONSUMER_PRINT(...) printf("CONSUMER: " __VA_ARGS__)
00046 #define CHECK_ERROR(expr) \
00047 do { \
00048 if ((expr) < 0) { \
00049 abort(); \
00050 ORIGINATE_ERROR(#expr " failed"); \
00051 } \
00052 } while (0);
00053
00054 namespace ArgusSamples
00055 {
00056
00057
00058
00059
00060
00061
00062
00063
00064 class ConsumerThread : public Thread
00065 {
00066 public:
00067 explicit ConsumerThread(OutputStream* stream);
00068 ~ConsumerThread();
00069
00070 bool isInError()
00071 {
00072 return m_gotError;
00073 }
00074
00075 private:
00076
00077
00078 virtual bool threadInitialize();
00079 virtual bool threadExecute();
00080 virtual bool threadShutdown();
00081
00082
00083 bool createImageConverter();
00084 void abort();
00085
00086 static bool converterCapturePlaneDqCallback(
00087 struct v4l2_buffer *v4l2_buf,
00088 NvBuffer *buffer,
00089 NvBuffer *shared_buffer,
00090 void *arg);
00091 static bool converterOutputPlaneDqCallback(
00092 struct v4l2_buffer *v4l2_buf,
00093 NvBuffer *buffer,
00094 NvBuffer *shared_buffer,
00095 void *arg);
00096
00097 void writeFrameToOpencvConsumer(
00098 camera_caffe_context *p_ctx,
00099 NvBuffer *buffer);
00100
00101 OutputStream* m_stream;
00102 UniqueObj<FrameConsumer> m_consumer;
00103 NvVideoConverter *m_ImageConverter;
00104 std::queue < NvBuffer * > *m_ConvOutputPlaneBufQueue;
00105 pthread_mutex_t m_queueLock;
00106 pthread_cond_t m_queueCond;
00107 int conv_buf_num;
00108 int m_numPendingFrames;
00109
00110 camera_caffe_context *m_pContext;
00111
00112 bool m_gotError;
00113 };
00114
00115 ConsumerThread::ConsumerThread(OutputStream* stream) :
00116 m_stream(stream),
00117 m_ImageConverter(NULL),
00118 m_gotError(false)
00119 {
00120 conv_buf_num = FRAME_CONVERTER_BUF_NUMBER;
00121 m_ConvOutputPlaneBufQueue = new std::queue < NvBuffer * >;
00122 pthread_mutex_init(&m_queueLock, NULL);
00123 pthread_cond_init(&m_queueCond, NULL);
00124 m_pContext = &ctx;
00125 m_numPendingFrames = 0;
00126 }
00127
00128 ConsumerThread::~ConsumerThread()
00129 {
00130 delete m_ConvOutputPlaneBufQueue;
00131 if (m_ImageConverter)
00132 {
00133 if (DO_STAT)
00134 m_ImageConverter->printProfilingStats(std::cout);
00135 delete m_ImageConverter;
00136 }
00137 }
00138
00139 bool ConsumerThread::threadInitialize()
00140 {
00141
00142 m_consumer = UniqueObj<FrameConsumer>(FrameConsumer::create(m_stream));
00143 if (!m_consumer)
00144 ORIGINATE_ERROR("Failed to create FrameConsumer");
00145
00146
00147 if (!createImageConverter())
00148 ORIGINATE_ERROR("Failed to create video m_ImageConverteroder");
00149
00150 return true;
00151 }
00152
00153 bool ConsumerThread::threadExecute()
00154 {
00155 IStream *iStream = interface_cast<IStream>(m_stream);
00156 IFrameConsumer *iFrameConsumer = interface_cast<IFrameConsumer>(m_consumer);
00157
00158
00159 CONSUMER_PRINT("Waiting until producer is connected...\n");
00160 if (iStream->waitUntilConnected() != STATUS_OK)
00161 ORIGINATE_ERROR("Stream failed to connect.");
00162 CONSUMER_PRINT("Producer has connected; continuing.\n");
00163
00164
00165 while (!m_gotError)
00166 {
00167 struct v4l2_buffer v4l2_buf;
00168 struct v4l2_plane planes[MAX_PLANES];
00169
00170 memset(&v4l2_buf, 0, sizeof(v4l2_buf));
00171 memset(planes, 0, MAX_PLANES * sizeof(struct v4l2_plane));
00172
00173 v4l2_buf.m.planes = planes;
00174
00175 pthread_mutex_lock(&m_queueLock);
00176 while (!m_gotError &&
00177 ((m_ConvOutputPlaneBufQueue->empty()) || (m_numPendingFrames >= MAX_PENDING_FRAMES)))
00178 {
00179 pthread_cond_wait(&m_queueCond, &m_queueLock);
00180 }
00181
00182 if (m_gotError)
00183 {
00184 pthread_mutex_unlock(&m_queueLock);
00185 break;
00186 }
00187
00188 NvBuffer *buffer = NULL;
00189 int fd = -1;
00190
00191 buffer = m_ConvOutputPlaneBufQueue->front();
00192 m_ConvOutputPlaneBufQueue->pop();
00193 pthread_mutex_unlock(&m_queueLock);
00194
00195
00196 UniqueObj<Frame> frame(iFrameConsumer->acquireFrame());
00197 IFrame *iFrame = interface_cast<IFrame>(frame);
00198 if (!iFrame)
00199 break;
00200
00201
00202 NV::IImageNativeBuffer *iNativeBuffer =
00203 interface_cast<NV::IImageNativeBuffer>(iFrame->getImage());
00204 if (!iNativeBuffer)
00205 ORIGINATE_ERROR("IImageNativeBuffer not supported by Image.");
00206 fd = iNativeBuffer->createNvBuffer(Size(ctx.width, ctx.height),
00207 NvBufferColorFormat_YUV420,
00208 NvBufferLayout_BlockLinear);
00209
00210
00211 v4l2_buf.index = buffer->index;
00212
00213
00214 buffer->planes[0].bytesused = 1;
00215 buffer->planes[0].fd = fd;
00216 buffer->planes[1].fd = fd;
00217 buffer->planes[2].fd = fd;
00218 pthread_mutex_lock(&m_queueLock);
00219 m_numPendingFrames++;
00220 pthread_mutex_unlock(&m_queueLock);
00221
00222 CONSUMER_PRINT("acquireFd %d (%d frames)\n", fd, m_numPendingFrames);
00223
00224 int ret = m_ImageConverter->output_plane.qBuffer(v4l2_buf, buffer);
00225 if (ret < 0) {
00226 abort();
00227 ORIGINATE_ERROR("Fail to qbuffer for conv output plane");
00228 }
00229 }
00230
00231
00232
00233 m_ImageConverter->capture_plane.waitForDQThread(2000);
00234
00235 CONSUMER_PRINT("Done.\n");
00236
00237 requestShutdown();
00238
00239 return true;
00240 }
00241
00242 bool ConsumerThread::threadShutdown()
00243 {
00244 return true;
00245 }
00246
00247 void ConsumerThread::writeFrameToOpencvConsumer(
00248 camera_caffe_context *p_ctx, NvBuffer *buffer)
00249 {
00250 NvBuffer::NvBufferPlane *plane = &buffer->planes[0];
00251 uint8_t *pdata = (uint8_t *) plane->data;
00252
00253
00254 if (p_ctx->lib_handler)
00255 {
00256 p_ctx->opencv_img_processing(p_ctx->opencv_handler
00257 , pdata, plane->fmt.width, plane->fmt.height);
00258 }
00259 }
00260
00261 bool ConsumerThread::converterCapturePlaneDqCallback(
00262 struct v4l2_buffer *v4l2_buf,
00263 NvBuffer * buffer,
00264 NvBuffer * shared_buffer,
00265 void *arg)
00266 {
00267 ConsumerThread *thiz = (ConsumerThread*)arg;
00268 camera_caffe_context *p_ctx = thiz->m_pContext;
00269 int e;
00270
00271 if (!v4l2_buf)
00272 {
00273 REPORT_ERROR("Failed to dequeue buffer from conv capture plane");
00274 thiz->abort();
00275 return false;
00276 }
00277
00278 if (v4l2_buf->m.planes[0].bytesused == 0)
00279 {
00280 return false;
00281 }
00282
00283 thiz->writeFrameToOpencvConsumer(p_ctx, buffer);
00284
00285 e = thiz->m_ImageConverter->capture_plane.qBuffer(*v4l2_buf, NULL);
00286 if (e < 0)
00287 ORIGINATE_ERROR("qBuffer failed");
00288
00289 return true;
00290 }
00291
00292 bool ConsumerThread::converterOutputPlaneDqCallback(
00293 struct v4l2_buffer *v4l2_buf,
00294 NvBuffer * buffer,
00295 NvBuffer * shared_buffer,
00296 void *arg)
00297 {
00298 ConsumerThread *thiz = (ConsumerThread*)arg;
00299
00300 if (!v4l2_buf)
00301 {
00302 REPORT_ERROR("Failed to dequeue buffer from conv capture plane");
00303 thiz->abort();
00304 return false;
00305 }
00306
00307 if (v4l2_buf->m.planes[0].bytesused == 0)
00308 {
00309 return false;
00310 }
00311 NvBufferDestroy(shared_buffer->planes[0].fd);
00312
00313 CONSUMER_PRINT("releaseFd %d (%d frames)\n", shared_buffer->planes[0].fd, thiz->m_numPendingFrames);
00314 pthread_mutex_lock(&thiz->m_queueLock);
00315 thiz->m_numPendingFrames--;
00316 thiz->m_ConvOutputPlaneBufQueue->push(buffer);
00317 pthread_cond_broadcast(&thiz->m_queueCond);
00318 pthread_mutex_unlock(&thiz->m_queueLock);
00319
00320 return true;
00321 }
00322
00323 bool ConsumerThread::createImageConverter()
00324 {
00325 int ret = 0;
00326
00327
00328 m_ImageConverter = NvVideoConverter::createVideoConverter("conv");
00329 if (!m_ImageConverter)
00330 ORIGINATE_ERROR("Could not create m_ImageConverteroder");
00331
00332 if (DO_STAT)
00333 m_ImageConverter->enableProfiling();
00334
00335 m_ImageConverter->capture_plane.
00336 setDQThreadCallback(converterCapturePlaneDqCallback);
00337 m_ImageConverter->output_plane.
00338 setDQThreadCallback(converterOutputPlaneDqCallback);
00339
00340 ret = m_ImageConverter->setOutputPlaneFormat(V4L2_PIX_FMT_YUV420M, m_pContext->width,
00341 m_pContext->height, V4L2_NV_BUFFER_LAYOUT_BLOCKLINEAR);
00342 if (ret < 0)
00343 ORIGINATE_ERROR("Could not set output plane format");
00344
00345 ret = m_ImageConverter->setCapturePlaneFormat(V4L2_PIX_FMT_ABGR32, m_pContext->width,
00346 m_pContext->height, V4L2_NV_BUFFER_LAYOUT_PITCH);
00347 if (ret < 0)
00348 ORIGINATE_ERROR("Could not set capture plane format");
00349
00350
00351
00352 ret = m_ImageConverter->output_plane.setupPlane(V4L2_MEMORY_DMABUF, conv_buf_num, false, false);
00353 if (ret < 0)
00354 ORIGINATE_ERROR("Could not setup output plane");
00355
00356
00357
00358 ret = m_ImageConverter->capture_plane.setupPlane(V4L2_MEMORY_MMAP, conv_buf_num, true, false);
00359 if (ret < 0)
00360 ORIGINATE_ERROR("Could not setup capture plane");
00361
00362
00363 for (uint32_t i = 0; i < m_ImageConverter->output_plane.getNumBuffers(); i++)
00364 {
00365 m_ConvOutputPlaneBufQueue->push(
00366 m_ImageConverter->output_plane.getNthBuffer(i));
00367 }
00368
00369
00370 ret = m_ImageConverter->output_plane.setStreamStatus(true);
00371 if (ret < 0)
00372 ORIGINATE_ERROR("fail to set conv output stream on");
00373
00374
00375 ret = m_ImageConverter->capture_plane.setStreamStatus(true);
00376 if (ret < 0)
00377 ORIGINATE_ERROR("fail to set conv capture stream on");
00378
00379
00380
00381 m_ImageConverter->capture_plane.startDQThread(this);
00382 m_ImageConverter->output_plane.startDQThread(this);
00383
00384
00385 for (uint32_t i = 0; i < m_ImageConverter->capture_plane.getNumBuffers(); i++)
00386 {
00387 struct v4l2_buffer v4l2_buf;
00388 struct v4l2_plane planes[MAX_PLANES];
00389
00390 memset(&v4l2_buf, 0, sizeof(v4l2_buf));
00391 memset(planes, 0, MAX_PLANES * sizeof(struct v4l2_plane));
00392
00393 v4l2_buf.index = i;
00394 v4l2_buf.m.planes = planes;
00395
00396 ret = m_ImageConverter->capture_plane.qBuffer(v4l2_buf, NULL);
00397 if (ret < 0) {
00398 abort();
00399 ORIGINATE_ERROR("Error queueing buffer at conv capture plane");
00400 }
00401 }
00402
00403 printf("create vidoe converter return true\n");
00404 return true;
00405 }
00406
00407 void ConsumerThread::abort()
00408 {
00409 m_ImageConverter->abort();
00410 m_gotError = true;
00411 }
00412
00413
00414
00415
00416
00417
00418
00419 static bool execute(camera_caffe_context *p_ctx)
00420 {
00421
00422 UniqueObj<CameraProvider> cameraProvider = UniqueObj<CameraProvider>(CameraProvider::create());
00423 ICameraProvider *iCameraProvider = interface_cast<ICameraProvider>(cameraProvider);
00424 if (!iCameraProvider)
00425 ORIGINATE_ERROR("Failed to create CameraProvider");
00426
00427
00428 std::vector<CameraDevice*> cameraDevices;
00429 iCameraProvider->getCameraDevices(&cameraDevices);
00430 if (cameraDevices.size() == 0)
00431 ORIGINATE_ERROR("No cameras available");
00432
00433
00434 UniqueObj<CaptureSession> captureSession(
00435 iCameraProvider->createCaptureSession(cameraDevices[0]));
00436 ICaptureSession *iCaptureSession = interface_cast<ICaptureSession>(captureSession);
00437 if (!iCaptureSession)
00438 ORIGINATE_ERROR("Failed to get ICaptureSession interface");
00439
00440
00441 PRODUCER_PRINT("Creating output stream\n");
00442 UniqueObj<OutputStreamSettings> streamSettings(iCaptureSession->createOutputStreamSettings());
00443 IOutputStreamSettings *iStreamSettings = interface_cast<IOutputStreamSettings>(streamSettings);
00444 if (iStreamSettings)
00445 {
00446 iStreamSettings->setPixelFormat(PIXEL_FMT_YCbCr_420_888);
00447 iStreamSettings->setResolution(Size(p_ctx->width, p_ctx->height));
00448 }
00449 else
00450 {
00451 ORIGINATE_ERROR("NULL for output stream settings!");
00452 }
00453 UniqueObj<OutputStream> outputStream(iCaptureSession->createOutputStream(streamSettings.get()));
00454
00455
00456 PRODUCER_PRINT("Launching consumer thread\n");
00457 ConsumerThread frameConsumerThread(outputStream.get());
00458 PROPAGATE_ERROR(frameConsumerThread.initialize());
00459
00460
00461 PROPAGATE_ERROR(frameConsumerThread.waitRunning());
00462
00463
00464 UniqueObj<Request> request(iCaptureSession->createRequest());
00465 IRequest *iRequest = interface_cast<IRequest>(request);
00466 if (!iRequest)
00467 ORIGINATE_ERROR("Failed to create Request");
00468 iRequest->enableOutputStream(outputStream.get());
00469
00470 ISourceSettings *iSourceSettings = interface_cast<ISourceSettings>(iRequest->getSourceSettings());
00471 if (!iSourceSettings)
00472 ORIGINATE_ERROR("Failed to get ISourceSettings interface");
00473 iSourceSettings->setFrameDurationRange(Range<uint64_t>(1e9/DEFAULT_FPS));
00474
00475
00476 PRODUCER_PRINT("Starting repeat capture requests.\n");
00477 if (iCaptureSession->repeat(request.get()) != STATUS_OK)
00478 ORIGINATE_ERROR("Failed to start repeat capture request");
00479
00480
00481 for (int i = 0; i < CAPTURE_TIME && !frameConsumerThread.isInError(); i++)
00482 sleep(1);
00483
00484
00485 iCaptureSession->stopRepeat();
00486 iCaptureSession->waitForIdle();
00487
00488
00489 outputStream.reset();
00490
00491
00492 PROPAGATE_ERROR(frameConsumerThread.shutdown());
00493
00494 PRODUCER_PRINT("Done -- exiting.\n");
00495
00496 return true;
00497 }
00498
00499 };
00500
00501 static int init_ctx(camera_caffe_context *p_ctx)
00502 {
00503
00504 p_ctx->lib_handler = dlopen(p_ctx->lib_file, RTLD_LAZY);
00505 if (!p_ctx->lib_handler)
00506 {
00507 printf("Fail to open OPENCV LIB, continue to run without processing\n");
00508 }
00509 else
00510 {
00511 p_ctx->opencv_handler_open = (opencv_handler_open_t)
00512 dlsym(p_ctx->lib_handler, OPENCV_HANDLER_OPEN_ENTRY);
00513 if (!p_ctx->opencv_handler_open)
00514 ORIGINATE_ERROR("Fail to get sym opencv_handler_open");
00515 p_ctx->opencv_handler_close = (opencv_handler_close_t)
00516 dlsym(p_ctx->lib_handler, OPENCV_HANDLER_CLOSE_ENTRY);
00517 if (!p_ctx->opencv_handler_close)
00518 ORIGINATE_ERROR("Fail to get sym opencv_handler_close");
00519 p_ctx->opencv_img_processing = (opencv_img_processing_t)
00520 dlsym(p_ctx->lib_handler, OPENCV_IMG_PROCESSING_ENTRY);
00521 if (!p_ctx->opencv_img_processing)
00522 ORIGINATE_ERROR("Fail to get sym opencv_img_processing");
00523 p_ctx->opencv_set_config = (opencv_set_config_t)
00524 dlsym(p_ctx->lib_handler, OPENCV_SET_CONFIG_ENTRY);
00525 if (!p_ctx->opencv_set_config)
00526 ORIGINATE_ERROR("Fail to get sym opencv_set_config");
00527
00528 p_ctx->opencv_handler = p_ctx->opencv_handler_open();
00529 if (!p_ctx->opencv_handler)
00530 ORIGINATE_ERROR("Fail to opencv_handler_open");
00531
00532
00533 p_ctx->opencv_set_config(p_ctx->opencv_handler
00534 , OPENCV_CONSUMER_CONFIG_IMGWIDTH, &p_ctx->width);
00535 p_ctx->opencv_set_config(p_ctx->opencv_handler
00536 , OPENCV_CONSUMER_CONFIG_IMGHEIGHT, &p_ctx->height);
00537 p_ctx->opencv_set_config(p_ctx->opencv_handler
00538 , OPENCV_CONSUMER_CONFIG_CAFFE_MODELFILE, p_ctx->model_file);
00539 p_ctx->opencv_set_config(p_ctx->opencv_handler
00540 , OPENCV_CONSUMER_CONFIG_CAFFE_TRAINEDFILE, p_ctx->trained_file);
00541 p_ctx->opencv_set_config(p_ctx->opencv_handler
00542 , OPENCV_CONSUMER_CONFIG_CAFFE_MEANFILE, p_ctx->mean_file);
00543 p_ctx->opencv_set_config(p_ctx->opencv_handler
00544 , OPENCV_CONSUMER_CONFIG_CAFFE_LABELFILE, p_ctx->label_file);
00545 p_ctx->opencv_set_config(p_ctx->opencv_handler
00546 , OPENCV_CONSUMER_CONFIG_START, NULL);
00547 }
00548
00549 return 0;
00550 }
00551
00552
00553 static void deinit_ctx(camera_caffe_context *p_ctx)
00554 {
00555 if (p_ctx)
00556 {
00557 if (p_ctx->lib_handler)
00558 {
00559 p_ctx->opencv_handler_close(p_ctx->opencv_handler);
00560 dlclose(p_ctx->lib_handler);
00561 }
00562 if (p_ctx->lib_file)
00563 free(p_ctx->lib_file);
00564 if (p_ctx->model_file)
00565 free(p_ctx->model_file);
00566 if (p_ctx->mean_file)
00567 free(p_ctx->mean_file);
00568 if (p_ctx->label_file)
00569 free(p_ctx->label_file);
00570 }
00571 return;
00572 }
00573
00574
00575 static void set_default(camera_caffe_context *p_ctx)
00576 {
00577 memset(p_ctx, 0, sizeof(camera_caffe_context));
00578 return;
00579 }
00580
00581
00582 int main(int argc, const char *argv[])
00583 {
00584 int ret = EXIT_SUCCESS;
00585 camera_caffe_context *p_ctx = &ctx;
00586
00587 set_default(p_ctx);
00588 ret = parse_csv_args(p_ctx, argc, argv);
00589 if (ret < 0)
00590 ORIGINATE_ERROR("Fail to parse arguments.");
00591 ret = init_ctx(p_ctx);
00592 if (ret < 0)
00593 ORIGINATE_ERROR("Fail to initialize context.");
00594
00595 if (!ArgusSamples::execute(p_ctx))
00596 return EXIT_FAILURE;
00597
00598 deinit_ctx(p_ctx);
00599
00600 return ret;
00601 }