#include <mv_common.h>
#include "mv_3d.h"
#include <mv_testsuite_common.h>
+#include <vision_source.h>
#include <glib-2.0/glib.h>
#include <gst/gst.h>
#define MAX_STRING_LENGTH 1024
#define ARRAY_SIZE(x) (sizeof((x)) / sizeof((x)[0]))
-#define MAX_FRAMES 1800 // 30 fps * 60s
-#define INTRINSIC_FILE_PATH "/usr/share/capi-media-vision/stereoCalibZedVGA.json"
#ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
using namespace open3d;
#endif
+#define VISION_SOURCE_DEVICE_ID 3
+
class StopWatch
{
public:
- StopWatch()
- {
- start = std::chrono::steady_clock::now();
- }
+ StopWatch() = default;
~StopWatch() = default;
+ void resetTime() {
+ start = std::chrono::steady_clock::now();
+ }
std::chrono::milliseconds elapsedTime()
{
return std::chrono::duration_cast<std::chrono::milliseconds>(
typedef struct _appdata
{
- std::chrono::milliseconds diffMs;
+ StopWatch stopWatch;
std::string dataPath;
std::string intrinsicName;
std::string rgbName;
std::string datasetName;
+ std::string dispResultFile;
float minDisp;
float maxDisp;
int fmt;
-} appdata;
-
-static float *vertex3d = NULL;
-mv_source_h mv_source3d;
-
-gulong handler;
-
-// Gstreamer
-GstElement *pipeline;
-
-// Gstreamer - camera src
-GstElement *pipecam, *source, *filter, *queue1;
-GstElement *vrate, *vrfilter;
-GstElement *vconv, *vcfilter, *fsink;
-// Gstreamer - depth handling
-GstElement *pipedepth, *appsource;
-GstElement *asfilter;
-GstElement *queue2;
-GstElement *asvconv, *mjpegenc;
-GstElement *flsink;
-GstElement *fsink2;
-GstElement *sink;
+ int imgWidth;
+ int imgHeight;
+ unsigned char *buffer;
+ mv_source_h mv_source;
+ mv_3d_h mv3d_handle;
+} appdata;
-typedef struct _SrcData
-{
- GstElement *src;
- int numFrame;
-} SrcData;
+static appdata app;
// MediaVision
mv_engine_config_h engine_config;
-mv_source_h mv_source;
-mv_3d_h mv3d_handle;
-
-/* internal functions for testsuite */
-GCond g_thread_cond;
-GMutex g_thread_mutex;
-
-void __wait()
-{
- g_mutex_lock(&g_thread_mutex);
- g_print("\t[__wait] waiting... until finishing\n");
- g_cond_wait(&g_thread_cond, &g_thread_mutex);
- g_print("\t[__wait] get signal from callback\n");
- g_mutex_unlock(&g_thread_mutex);
-}
-void __signal()
-{
- g_mutex_lock(&g_thread_mutex);
- g_cond_signal(&g_thread_cond);
- g_print("\t[__signal] send signal to test proc\n");
- g_mutex_unlock(&g_thread_mutex);
-}
+static GMainLoop *loop;
static bool isLive;
static bool isAsync;
-static double depth_scale;
-static double depth_trunc;
static int camWidth;
static int camHeight;
static int minDisp;
static int maxDisp;
static int display_xpos;
static int display_ypos;
+
void int_handler(int sig)
{
char c;
c = getchar();
if (c == 'y' || c == 'Y') {
- g_signal_handler_disconnect(fsink, handler);
- printf("delete buffer, used by visualize 3d\n");
-
- gst_element_send_event(pipecam, gst_event_new_eos());
- gst_element_send_event(pipedepth, gst_event_new_eos());
-
- if (vertex3d != NULL) {
- delete [] vertex3d;
- vertex3d = NULL;
- }
-
- if (mv_source3d != NULL) {
- mv_destroy_source(mv_source3d);
- mv_source3d = NULL;
- }
-
sleep(1);
+ g_main_loop_quit(loop);
} else {
printf("no");
signal(SIGINT, int_handler);
getchar(); // Get new line character
}
-static gboolean bus_call(GstBus *bus, GstMessage *msg, gpointer data)
-{
- GMainLoop *loop = (GMainLoop *) data;
-
- switch (GST_MESSAGE_TYPE(msg)) {
- case GST_MESSAGE_EOS:
- printf("End of stream\n");
- g_main_loop_quit(loop);
- break;
-
- case GST_MESSAGE_ERROR: {
- gchar *debug;
- GError *error;
-
- gst_message_parse_error(msg, &error, &debug);
- g_free(debug);
-
- printf("Error: %s\n", error->message);
- g_error_free(error);
-
- g_main_loop_quit(loop);
- break;
- }
- default:
- break;
- }
-
- return TRUE;
-}
-
-#ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
-void WritePointCloud(const char* data_path,
- const char* color_filename,
- const char* depth_filename,
- unsigned int width,
- unsigned int height,
- const camera::PinholeCameraIntrinsic& intrinsic)
-{
- geometry::Image img_color, img_depth;
-
- io::ReadImage(color_filename, img_color);
- io::ReadImage(depth_filename, img_depth);
-
- utility::LogInfo("depth filename : {}", depth_filename);
- utility::LogInfo("Reading RGBD image : ");
- utility::LogInfo(" Color : {:d} x {:d} x {:d} ({:d} bits per channel)",
- img_color.width_, img_color.height_, img_color.num_of_channels_,
- img_color.bytes_per_channel_ * 8);
- utility::LogInfo(" Depth : {:d} x {:d} x {:d} ({:d} bits per channel)",
- img_depth.width_, img_depth.height_, img_depth.num_of_channels_,
- img_depth.bytes_per_channel_ * 8);
- double depth_scale = 1000.0, depth_trunc = 200.0;
- bool convert_rgb_to_intensity = true;
- std::shared_ptr<geometry::RGBDImage> rgbd_image =
- geometry::RGBDImage::CreateFromColorAndDepth(
- img_color, img_depth, depth_scale, depth_trunc,
- convert_rgb_to_intensity);
-
- auto pcd = geometry::PointCloud::CreateFromRGBDImage(*rgbd_image, intrinsic);
- std::string dataPath = std::string(data_path);
- const std::string filename_ply(std::string(data_path) + std::string("/") +
- dataPath.substr(dataPath.find_last_of("/\\") + 1) +
- std::string(".ply"));
-
- if (io::WritePointCloud(filename_ply, *pcd)) {
- utility::LogInfo("Successfully wrote {}", filename_ply);
- } else {
- utility::LogError("Failed to write {}", filename_ply);
- }
-}
-#endif
-
void _depth_stereo_cb(mv_source_h source, unsigned short *depth, unsigned int width,
unsigned int height, void *user_data)
{
-#ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
- double maxDepth = 2000.0;
- double minDepth = 175.0;
- double depthDenom = static_cast<double>(maxDepth - minDepth);
-
- camera::PinholeCameraIntrinsic intrinsic;
- io::ReadIJsonConvertible(INTRINSIC_FILE_PATH, intrinsic);
-
- vertex3d = new float[width * height * 4];
- memset(vertex3d, 0, width * height * 4);
- utility::LogInfo("focal_x = {}", intrinsic.GetFocalLength().first);
- utility::LogInfo("focal_y = {}", intrinsic.GetFocalLength().second);
- utility::LogInfo("cx = {}", intrinsic.GetPrincipalPoint().first);
- utility::LogInfo("cy = {}", intrinsic.GetPrincipalPoint().second);
-
- // depth (16bit)
- geometry::Image img_depth;
- img_depth.Prepare(width, height, 1, sizeof(unsigned short));
- uint16_t *pImg_depth_data = (unsigned short*)img_depth.data_.data();
- memcpy(pImg_depth_data, depth, width*height*sizeof(unsigned short));
- auto pcd = geometry::PointCloud::CreateFromDepthImage(
- img_depth, intrinsic,
- Eigen::Matrix4d::Identity(),
- 1000.0, 2.0, 1,
- false);
-
- cv::Mat colorMap = cv::Mat(cv::Size(width,height), CV_8UC1);
- for (unsigned int j = 0; j < height; ++j) {
- for (unsigned int i = 0; i < width; ++i) {
- int idx = j * width + i;
- int idxInv = width * height - idx - 1;
- auto depth_raw = pcd->points_.data()[idx][2] * 1000.0;
- auto depthCut = std::min(std::max(depth_raw, minDepth), maxDepth);
- colorMap.at<unsigned char>(height -j - 1, width -i - 1) =
- static_cast<unsigned char>(
- (depthCut - minDepth) * 255.0 / depthDenom);
-
- // 175 ~ 1900
- auto inverseDepthCut = maxDepth - depthCut;
- if (inverseDepthCut <= (maxDepth - 1900)) {
- vertex3d[idxInv] = 0.0f;
- } else if (inverseDepthCut > (maxDepth - 1900) && inverseDepthCut < 1825) {
- vertex3d[idxInv] = inverseDepthCut * 0.001f;
- } else {
- vertex3d[idxInv] = 0.0f;
- }
- }
- }
+ auto mv3d = static_cast<appdata *>(user_data);
- cv::Mat colorized_depth;
- cv::applyColorMap(colorMap, colorized_depth, cv::COLORMAP_JET);
+ cv::Mat dump(cv::Size(width, height), CV_16U);
- int err;
- if (mv_source3d == NULL) {
- err = mv_create_source(&mv_source3d);
- if (MEDIA_VISION_ERROR_NONE != err) {
- LOGE("Errors were occurred during creating the source!!! code : %i", err);
+ for (int y = 0; y < height; y++) {
+ unsigned short *ptr = depth + y * width;
+ for (int x = 0; x < width; x++) {
+ dump.at<unsigned short>(y,x) = static_cast<unsigned short>( 440.92750f * 21.87095f / static_cast<float>(ptr[x]));
}
}
- mv_source_fill_by_buffer(mv_source3d,
- colorized_depth.ptr<unsigned char>(),
- colorized_depth.elemSize() * colorized_depth.size().width
- * colorized_depth.size().height,
- colorized_depth.size().width,
- colorized_depth.size().height,
- MEDIA_VISION_COLORSPACE_RGB888);
-#ifdef BUILD_VISUALIZER
- mv_util_visualizer_3d(mv_source3d, vertex3d, display_xpos, display_ypos);
-#endif
- mv_destroy_source(mv_source3d);
- mv_source3d = NULL;
- delete [] vertex3d;
- vertex3d = NULL;
- __signal();
-#endif
+ cv::imwrite(mv3d->dispResultFile, dump);
}
-static void stereo_handoff(GstElement *object, GstBuffer *buffer, GstPad *pad,
- gpointer userData)
+static int _vision_source_cb(vision_source_buffer_s *buffer, void *user_data)
{
- GstMapInfo map;
- SrcData *srcData = static_cast<SrcData *>(userData);
- int ret = MEDIA_VISION_ERROR_NONE;
- if (srcData->numFrame >= MAX_FRAMES) {
- GstFlowReturn ret;
- g_signal_emit_by_name(srcData->src, "end-of-stream", &ret);
- return;
+ auto app = static_cast<appdata *>(user_data);
+ if (app->mv_source == nullptr || app->mv3d_handle == nullptr) {
+ printf("mv_source or mv3d_handle is null\n");
+ return 0;
}
- if (buffer != NULL) {
- gst_buffer_map(buffer, &map, GST_MAP_READ);
-
- mv_source_fill_by_buffer(mv_source, map.data, 1344 * 376 * 1, 1344, 376,
- MEDIA_VISION_COLORSPACE_Y800);
-
- gst_buffer_unmap(buffer, &map);
-
- if (isAsync) {
- ret = mv_3d_run_async(mv_source, NULL, NULL, mv3d_handle);
- __wait();
- } else {
- ret = mv_3d_run(mv_source, NULL, NULL, mv3d_handle);
- }
-
- if (ret != MEDIA_VISION_ERROR_NONE) {
- LOGE("Fail to mv_3d_run_async()");
- }
+ printf("_vision_source_cb: %lld ms\n", static_cast<long long int>(app->stopWatch.elapsedTime().count()));
- mv_source_clear(mv_source);
+ for (unsigned int h = 0; h < buffer->resolution.height; h++) {
+ memcpy(app->buffer + buffer->resolution.width * h,
+ buffer->planes[0].data + (buffer->planes[0].align_width * h),
+ buffer->resolution.width);
}
-}
+ mv_source_fill_by_buffer(app->mv_source, app->buffer, app->imgWidth * app->imgHeight,
+ app->imgWidth, app->imgHeight, MEDIA_VISION_COLORSPACE_Y800);
-int createPipelineCam(SrcData &srcData)
-{
- /*
- v4l2src device=/dev/video0 !
- video/x-raw, width=1344, height=376, framerate=30/1, format=YUY2 !
- queue ! videorate ! video/x-raw, framerate=30/1 !
- videoconvert ! video/x-raw, format=GRAY8 ! fakesink -v -e
- */
- printf("enter - createPipelineCam\n");
- source = gst_element_factory_make("v4l2src", "src");
- filter = gst_element_factory_make("capsfilter", "filter");
-
- vconv = gst_element_factory_make("videoconvert", "convert");
- vcfilter = gst_element_factory_make("capsfilter", "vcfilter");
- fsink = gst_element_factory_make("fakesink", "fsink");
-
- if (!pipecam || !source || !filter || !vconv || !vcfilter || !fsink) {
- printf("pipecam[%p]\n", pipecam);
- printf("source [%p], filter [%p]\n", source, filter);
- printf("vconv [%p], vcfilter [%p] \n", vconv, vcfilter);
- printf("fsink [%p] \n ", fsink);
- return -1;
- }
-
- /* set up the pipeline */
- g_object_set(G_OBJECT(source), "device", "/dev/video0", NULL);
- g_object_set(
- G_OBJECT(filter), "caps",
- gst_caps_from_string(
- "video/x-raw, width=1344, height=376, framerate=30/1, format=YUY2"),
- NULL);
- g_object_set(G_OBJECT(vcfilter), "caps",
- gst_caps_from_string("video/x-raw, format=GRAY8"), NULL);
- g_object_set(G_OBJECT(fsink), "sync", FALSE, NULL);
-
- g_object_set(G_OBJECT(fsink), "signal-handoffs", TRUE, NULL);
- handler = g_signal_connect(fsink, "handoff", G_CALLBACK(stereo_handoff),
- &srcData);
-
- /* add elements into the pipeline */
- gst_bin_add_many(GST_BIN(pipecam), source, filter, vconv, vcfilter, fsink,
- NULL);
-
- /* link elements */
- gst_element_link_many(source, filter, /*queue1, vrate, vrfilter,*/
- vconv, vcfilter, fsink, NULL);
- return 0;
-}
-
-int createPipelineDepth()
-{
- /*
- appsrc ! video/x-raw, width=672, height=376, framerate=30/1, format=GRAY8 !
- videoconvert ! fakesink -v -e
- */
- appsource = gst_element_factory_make("appsrc", "appsrc");
- asfilter = gst_element_factory_make("capsfilter", "asfilter");
- asvconv = gst_element_factory_make("videoconvert", "asvconv");
- //sink = gst_element_factory_make("tizenwlsink", "sink");
- sink = gst_element_factory_make("fakesink", "sink");
-
- if (!pipedepth || !appsource || !asfilter || !asvconv || !sink) {
- printf("pipeline[%p]\n", pipedepth);
- printf("appsrc [%p], asfilter[%p] \n", appsource, asfilter);
- printf("asvconv [%p], sink [%p] \n", asvconv, sink);
- return -1;
- }
-
- /* set up the pipeline */
- GstCaps *caps = gst_caps_from_string(
- "video/x-raw, width=672, height=376, framerate=30/1, format=GRAY16_LE");
- gst_app_src_set_caps((GstAppSrc *) appsource, caps);
- g_object_set(
- G_OBJECT(asfilter), "caps",
- gst_caps_from_string(
- "video/x-raw, width=672, height=376, framerate=30/1, format=GRAY16_LE"),
- NULL);
- g_object_set(G_OBJECT(sink), "use-tbm", FALSE, NULL);
- g_object_set(G_OBJECT(sink), "sync", FALSE, NULL);
-
- /* add elements into the pipeline */
- gst_bin_add_many(GST_BIN(pipedepth), appsource, asfilter, asvconv, sink,
- NULL);
-
- /* link elements */
- gst_element_link_many(appsource, asfilter, asvconv, sink, NULL);
+ mv_3d_run_async(app->mv3d_handle, app->mv_source, nullptr, nullptr);
+ mv_source_clear(app->mv_source);
return 0;
}
{
signal(SIGINT, int_handler);
- gst_init(&argc, &argv);
- GMainLoop *loop;
- GstBus *bus;
- guint busWatchIdCam;
- guint busWatchIdDepth;
- camWidth = 672;
- camHeight = 376;
-
+ camWidth = 640;
+ camHeight = 400;
isLive = true;
isAsync = true;
- depth_scale = 1000.0;
- depth_trunc = 200.0;
+ std::string stereoConfigFile;
+ std::string dispResultFile;
+ // mv_depthstream_test_suite 1 8 96 0 0 640 400 /usr/share/dfs-qcmv/stereoCalibDA.yaml /tmp/test.png
if (argc > 1) {
- isLive = atoi(argv[1]) !=0 ? true : false;
- depth_scale = static_cast<double>(atof(argv[2]));
- depth_trunc = static_cast<double>(atof(argv[3]));
-
- isAsync = atoi(argv[4]) !=0 ? true : false;
- minDisp = atoi(argv[5]);
- maxDisp = atoi(argv[6]);
- display_xpos = atoi(argv[7]);
- display_ypos = atoi(argv[8]);
+ isAsync = (atoi(argv[1])!=0);
+ minDisp = atoi(argv[2]);
+ maxDisp = atoi(argv[3]);
+ display_xpos = atoi(argv[4]);
+ display_ypos = atoi(argv[5]);
+ camWidth = atoi(argv[6]);
+ camHeight = atoi(argv[7]);
+ stereoConfigFile = argv[8];
+ dispResultFile = argv[9];
}
#ifdef MV_3D_POINTCLOUD_IS_AVAILABLE
utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);
#endif
- SrcData srcData;
int ret = MEDIA_VISION_ERROR_NONE;
-
printf("enter main\n");
// mediavision
ret = mv_engine_config_set_string_attribute(
engine_config, MV_3D_DEPTH_STEREO_CONFIG_FILE_PATH,
- "/usr/share/capi-media-vision/stereoCalibZedVGA.yaml");
-
+ stereoConfigFile.c_str());
if (ret != MEDIA_VISION_ERROR_NONE) {
return -1;
}
+ app.dispResultFile = dispResultFile;
// mv3d handle
- ret = mv_3d_create(&mv3d_handle);
+ ret = mv_3d_create(&app.mv3d_handle);
if (ret != MEDIA_VISION_ERROR_NONE) {
return -1;
}
- ret = mv_3d_configure(mv3d_handle, engine_config);
+ ret = mv_3d_configure(app.mv3d_handle, engine_config);
if (ret != MEDIA_VISION_ERROR_NONE) {
return -1;
}
- ret = mv_3d_set_depth_cb(mv3d_handle, _depth_stereo_cb, nullptr);
+ ret = mv_3d_set_depth_cb(app.mv3d_handle, _depth_stereo_cb, &app);
if (ret != MEDIA_VISION_ERROR_NONE) {
return -1;
}
- ret = mv_3d_prepare(mv3d_handle);
+ ret = mv_3d_prepare(app.mv3d_handle);
if (ret != MEDIA_VISION_ERROR_NONE) {
return -1;
}
// source
- ret = mv_create_source(&mv_source);
+ ret = mv_create_source(&app.mv_source);
if (ret != MEDIA_VISION_ERROR_NONE) {
return -1;
}
- // gstreamer
- loop = g_main_loop_new(NULL, FALSE);
- pipedepth = gst_pipeline_new("pipedepth");
- pipecam = gst_pipeline_new("pipecam");
-
- // create depth and prepare
- ret = createPipelineDepth();
- if (ret != 0)
+ vision_source_format_s format {
+ .pixel_format = VISION_SOURCE_PIXEL_FORMAT_NV12,
+ .resolution = { camWidth << 1, camHeight },
+ .fps = 10,
+ .quality = 0,
+ .bitrate = 0
+ };
+
+ app.imgWidth = camWidth << 1;
+ app.imgHeight = camHeight;
+ app.buffer = new unsigned char [app.imgWidth * app.imgHeight];
+ app.stopWatch.resetTime();
+
+ // vision-source
+ vision_source_h vision_source_handle = nullptr;
+ ret = vision_source_init(&vision_source_handle);
+ if (ret != VISION_SOURCE_ERROR_NONE) {
return -1;
+ }
- srcData.src = appsource;
- srcData.numFrame = 0;
- // create camera
- ret = createPipelineCam(srcData);
- if (ret != 0)
+ ret = vision_source_open_device(vision_source_handle, VISION_SOURCE_DEVICE_ID);
+ if (ret != VISION_SOURCE_ERROR_NONE) {
return -1;
+ }
- bus = gst_pipeline_get_bus(GST_PIPELINE(pipecam));
- busWatchIdCam = gst_bus_add_watch(bus, bus_call, loop);
- gst_object_unref(bus);
-
- bus = gst_pipeline_get_bus(GST_PIPELINE(pipedepth));
- busWatchIdDepth = gst_bus_add_watch(bus, bus_call, loop);
- gst_object_unref(bus);
-
- gst_element_set_state(pipecam, GST_STATE_PLAYING);
- gst_element_set_state(pipedepth, GST_STATE_PLAYING);
+ loop = g_main_loop_new(NULL,false);
+ ret = vision_source_set_stream_format(vision_source_handle, &format);
+ if (ret != VISION_SOURCE_ERROR_NONE) {
+ return -1;
+ }
+ vision_source_start_stream(vision_source_handle, _vision_source_cb, &app);
+ printf("starting...\n");
g_main_loop_run(loop);
+ printf("stop\n");
- gst_element_set_state(pipecam, GST_STATE_NULL);
- gst_element_set_state(pipedepth, GST_STATE_NULL);
+ delete [] app.buffer;
- gst_object_unref(GST_OBJECT(pipecam));
- gst_object_unref(GST_OBJECT(pipedepth));
- g_source_remove(busWatchIdCam);
- g_source_remove(busWatchIdDepth);
- g_main_loop_unref(loop);
+ vision_source_stop_stream(vision_source_handle);
+ vision_source_close_device(vision_source_handle);
+ vision_source_exit(vision_source_handle);
- mv_3d_destroy(mv3d_handle);
+ mv_destroy_source(app.mv_source);
+ mv_3d_destroy(app.mv3d_handle);
mv_destroy_engine_config(engine_config);
- mv_destroy_source(mv_source);
return 0;
}
projects / platform / core / api / mediavision.git / commitdiff