From 881f1e313a744d6df8ea3de247ff920a764ee1d5 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Sun, 7 Jun 2020 13:05:38 +0300
Subject: [PATCH] Refactor pylon as local source
---
components/rgbd-sources/CMakeLists.txt | 11 +-
components/rgbd-sources/src/source.cpp | 8 +-
.../src/sources/stereovideo/calibrate.hpp | 1 -
.../stereovideo/{local.cpp => opencv.cpp} | 2 +-
.../stereovideo/{local.hpp => opencv.hpp} | 0
.../src/sources/stereovideo/pylon.cpp | 223 ++++++++++++++++++
.../src/sources/stereovideo/pylon.hpp | 60 +++++
.../src/sources/stereovideo/stereovideo.cpp | 63 +++--
8 files changed, 328 insertions(+), 40 deletions(-)
rename components/rgbd-sources/src/sources/stereovideo/{local.cpp => opencv.cpp} (99%)
rename components/rgbd-sources/src/sources/stereovideo/{local.hpp => opencv.hpp} (100%)
create mode 100644 components/rgbd-sources/src/sources/stereovideo/pylon.cpp
create mode 100644 components/rgbd-sources/src/sources/stereovideo/pylon.hpp
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index d15fd8d61..7e47d77e4 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -1,6 +1,6 @@
set(RGBDSRC
src/sources/stereovideo/calibrate.cpp
- src/sources/stereovideo/local.cpp
+ src/sources/stereovideo/opencv.cpp
src/source.cpp
src/frame.cpp
src/frameset.cpp
@@ -18,16 +18,9 @@ if (HAVE_REALSENSE)
endif()
if (HAVE_PYLON)
- list(APPEND RGBDSRC "src/sources/pylon/pylon.cpp")
+ list(APPEND RGBDSRC "src/sources/stereovideo/pylon.cpp")
endif()
-if (LibArchive_FOUND)
- list(APPEND RGBDSRC
- src/sources/snapshot/snapshot.cpp
- src/sources/snapshot/snapshot_source.cpp
- )
-endif (LibArchive_FOUND)
-
add_library(ftlrgbd ${RGBDSRC})
# target_compile_options(ftlrgbd PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-fPIC>)
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index b7ff97b2f..4ced4ba02 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -133,14 +133,8 @@ static ftl::rgbd::BaseSourceImpl *createFileImpl(const ftl::URI &uri, Source *ho
}
static ftl::rgbd::BaseSourceImpl *createDeviceImpl(const ftl::URI &uri, Source *host) {
- if (uri.getPathSegment(0) == "video" || uri.getPathSegment(0) == "camera") {
+ if (uri.getPathSegment(0) == "video" || uri.getPathSegment(0) == "camera" || uri.getPathSegment(0) == "pylon") {
return new StereoVideoSource(host);
- } else if (uri.getPathSegment(0) == "pylon") {
-#ifdef HAVE_PYLON
- return new PylonSource(host);
-#else
- LOG(ERROR) << "You did not build with 'pylon'";
-#endif
} else if (uri.getPathSegment(0) == "realsense") {
#ifdef HAVE_REALSENSE
return new RealsenseSource(host);
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
index 523608a04..3eaa1bc2d 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
@@ -7,7 +7,6 @@
#include <opencv2/core.hpp>
#include <opencv2/core/cuda.hpp>
-#include "local.hpp"
#include <string>
#include <vector>
#include <ftl/rgbd/camera.hpp>
diff --git a/components/rgbd-sources/src/sources/stereovideo/local.cpp b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
similarity index 99%
rename from components/rgbd-sources/src/sources/stereovideo/local.cpp
rename to components/rgbd-sources/src/sources/stereovideo/opencv.cpp
index 7b01d33f2..23c5165a8 100644
--- a/components/rgbd-sources/src/sources/stereovideo/local.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
@@ -9,7 +9,7 @@
#include <ftl/threads.hpp>
#include <ftl/profiler.hpp>
-#include "local.hpp"
+#include "opencv.hpp"
#include "calibrate.hpp"
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
diff --git a/components/rgbd-sources/src/sources/stereovideo/local.hpp b/components/rgbd-sources/src/sources/stereovideo/opencv.hpp
similarity index 100%
rename from components/rgbd-sources/src/sources/stereovideo/local.hpp
rename to components/rgbd-sources/src/sources/stereovideo/opencv.hpp
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
new file mode 100644
index 000000000..80abb1463
--- /dev/null
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
@@ -0,0 +1,223 @@
+#include "pylon.hpp"
+
+#include "calibrate.hpp"
+#include <loguru.hpp>
+#include <ftl/threads.hpp>
+#include <ftl/rgbd/source.hpp>
+
+#include <pylon/PylonIncludes.h>
+#include <pylon/BaslerUniversalInstantCamera.h>
+
+#include <opencv2/imgproc.hpp>
+
+using ftl::rgbd::detail::PylonDevice;
+using std::string;
+using ftl::codecs::Channel;
+using cv::cuda::GpuMat;
+using cv::Mat;
+using namespace Pylon;
+
+PylonDevice::PylonDevice(nlohmann::json &config)
+ : ftl::rgbd::detail::Device(config), ready_(false), lcam_(nullptr), rcam_(nullptr) {
+
+ auto &inst = CTlFactory::GetInstance();
+
+ Pylon::DeviceInfoList_t devices;
+ inst.EnumerateDevices(devices);
+
+ if (devices.size() == 0) {
+ LOG(ERROR) << "No Pylon devices attached";
+ return;
+ } else {
+ for (auto d : devices) {
+ LOG(INFO) << " - found Pylon device - " << d.GetFullName() << "(" << d.GetModelName() << ")";
+ }
+ }
+
+ try {
+ lcam_ = new CBaslerUniversalInstantCamera( CTlFactory::GetInstance().CreateDevice(devices[0]));
+ lcam_->RegisterConfiguration( new Pylon::CSoftwareTriggerConfiguration, Pylon::RegistrationMode_ReplaceAll, Pylon::Cleanup_Delete);
+ lcam_->Open();
+
+ if (devices.size() >= 2) {
+ rcam_ = new CBaslerUniversalInstantCamera( CTlFactory::GetInstance().CreateDevice(devices[1]));
+ rcam_->RegisterConfiguration( new Pylon::CSoftwareTriggerConfiguration, Pylon::RegistrationMode_ReplaceAll, Pylon::Cleanup_Delete);
+ rcam_->Open();
+ }
+
+ _configureCamera(lcam_);
+ if (rcam_) _configureCamera(rcam_);
+
+ lcam_->StartGrabbing( Pylon::GrabStrategy_OneByOne);
+
+ ready_ = true;
+ } catch (const Pylon::GenericException &e) {
+ // Error handling.
+ LOG(ERROR) << "Pylon: An exception occurred - " << e.GetDescription();
+ }
+
+ width_ = value("depth_width", fullwidth_);
+ height_ = value("depth_height", fullheight_);
+
+ // Allocate page locked host memory for fast GPU transfer
+ left_hm_ = cv::cuda::HostMem(height_, width_, CV_8UC4);
+ right_hm_ = cv::cuda::HostMem(height_, width_, CV_8UC4);
+ hres_hm_ = cv::cuda::HostMem(fullheight_, fullwidth_, CV_8UC4);
+}
+
+PylonDevice::~PylonDevice() {
+
+}
+
+std::vector<ftl::rgbd::detail::DeviceDetails> PylonDevice::listDevices() {
+ auto &inst = CTlFactory::GetInstance();
+
+ Pylon::DeviceInfoList_t devices;
+ inst.EnumerateDevices(devices);
+
+ std::vector<ftl::rgbd::detail::DeviceDetails> results;
+
+ int count=0;
+ for (auto d : devices) {
+ //LOG(INFO) << " - found Pylon device - " << d.GetFullName() << "(" << d.GetModelName() << ")";
+ auto &r = results.emplace_back();
+ r.id = count++;
+ r.name = d.GetModelName();
+ r.maxheight = 0;
+ r.maxwidth = 0;
+ }
+
+ return results;
+}
+
+void PylonDevice::_configureCamera(CBaslerUniversalInstantCamera *cam) {
+ // Get the camera control object.
+ GenApi::INodeMap& nodemap = cam->GetNodeMap();
+ // Get the parameters for setting the image area of interest (Image AOI).
+ CIntegerParameter width(nodemap, "Width");
+ CIntegerParameter height(nodemap, "Height");
+ CIntegerParameter offsetX(nodemap, "OffsetX");
+ CIntegerParameter offsetY(nodemap, "OffsetY");
+
+ fullwidth_ = width.GetValue();
+ fullheight_ = height.GetValue();
+
+ LOG(INFO) << "Camera resolution = " << fullwidth_ << "x" << fullheight_;
+
+ // Set the pixel data format.
+ CEnumParameter format(nodemap, "PixelFormat");
+ LOG(INFO) << "Camera format: " << format.GetValue();
+
+ if (format.CanSetValue("BayerBG8")) { // YCbCr422_8
+ format.SetValue("BayerBG8");
+ } else {
+ LOG(WARNING) << "Could not change pixel format";
+ }
+}
+
+bool PylonDevice::grab() {
+ if (!isReady()) return false;
+
+ try {
+ lcam_->WaitForFrameTriggerReady( 30, Pylon::TimeoutHandling_ThrowException);
+ if (rcam_) rcam_->WaitForFrameTriggerReady( 30, Pylon::TimeoutHandling_ThrowException);
+
+ lcam_->ExecuteSoftwareTrigger();
+ if (rcam_) rcam_->ExecuteSoftwareTrigger();
+ } catch (const GenericException &e) {
+ LOG(ERROR) << "Pylon: Trigger exception - " << e.GetDescription();
+ return false;
+ }
+
+ return true;
+}
+
+bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) {
+ if (!isReady()) return false;
+
+ Mat l, r ,hres;
+
+ // Use page locked memory
+ l = left_hm_.createMatHeader();
+ r = right_hm_.createMatHeader();
+ hres = hres_hm_.createMatHeader();
+
+ Mat &lfull = (!hasHigherRes()) ? l : hres;
+ Mat &rfull = (!hasHigherRes()) ? r : rtmp_;
+
+ try {
+ Pylon::CGrabResultPtr result_left;
+ Pylon::CGrabResultPtr result_right;
+
+ int lcount = 0;
+ if (lcam_->RetrieveResult(0, result_left, Pylon::TimeoutHandling_Return)) ++lcount;
+
+ int rcount = 0;
+ if (rcam_ && rcam_->RetrieveResult(0, result_right, Pylon::TimeoutHandling_Return)) ++rcount;
+
+ if (lcount == 0 || !result_left->GrabSucceeded()) {
+ LOG(ERROR) << "Retrieve failed";
+ return false;
+ }
+
+ cv::Mat wrap_left(
+ result_left->GetHeight(),
+ result_left->GetWidth(),
+ CV_8UC1,
+ (uint8_t*)result_left->GetBuffer());
+
+ cv::cvtColor(wrap_left, lfull, cv::COLOR_BayerBG2BGRA);
+
+ if (isStereo()) {
+ c->rectifyLeft(lfull);
+ }
+
+ if (hasHigherRes()) {
+ //FTL_Profile("Frame Resize", 0.01);
+ cv::resize(lfull, l, l.size(), 0.0, 0.0, cv::INTER_CUBIC);
+ h_l.upload(hres, stream);
+ } else {
+ h_l = cv::cuda::GpuMat();
+ }
+
+ l_out.upload(l, stream);
+
+ // TODO Perhaps multithread this as for OpenCV device
+ if (rcount > 0 && result_right->GrabSucceeded()) {
+ cv::Mat wrap_right(
+ result_right->GetHeight(),
+ result_right->GetWidth(),
+ CV_8UC1,
+ (uint8_t*)result_right->GetBuffer());
+
+ cv::cvtColor(wrap_right, rfull, cv::COLOR_BayerBG2BGRA);
+
+ if (isStereo()) {
+ c->rectifyRight(rfull);
+
+ if (hasHigherRes()) {
+ // TODO: Use threads?
+ cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC);
+ h_r = rfull;
+ }
+ else {
+ h_r = Mat();
+ }
+ }
+
+ r_out.upload(r, stream);
+
+ //frame.create<cv::cuda::GpuMat>(ftl::codecs::Channel::Colour2).upload(tmp_);
+ }
+
+ } catch (const GenericException &e) {
+ LOG(ERROR) << "Pylon: An exception occurred - " << e.GetDescription();
+ }
+
+ return true;
+}
+
+bool PylonDevice::isReady() const {
+ return lcam_ && lcam_->IsOpen();
+}
+
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
new file mode 100644
index 000000000..84b0742fb
--- /dev/null
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
@@ -0,0 +1,60 @@
+#pragma once
+#ifndef _FTL_RGBD_PYLONDEVICE_HPP_
+#define _FTL_RGBD_PYLONDEVICE_HPP_
+
+#include "device.hpp"
+#include <string>
+
+namespace Pylon {
+class CBaslerUniversalInstantCamera;
+}
+
+namespace ftl {
+namespace rgbd {
+namespace detail {
+
+class PylonDevice : public ftl::rgbd::detail::Device {
+ public:
+ explicit PylonDevice(nlohmann::json &config);
+ ~PylonDevice();
+
+ static std::vector<DeviceDetails> listDevices();
+
+ bool grab() override;
+ bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) override;
+
+ unsigned int width() const override { return width_; }
+ unsigned int height() const override { return height_; };
+
+ unsigned int fullWidth() const override { return fullwidth_; }
+ unsigned int fullHeight() const override { return fullheight_; }
+
+ double getTimestamp() const override { return 0.0; }
+
+ bool isStereo() const override { return lcam_ && rcam_; }
+
+ bool isReady() const;
+
+ private:
+ bool ready_;
+ Pylon::CBaslerUniversalInstantCamera *lcam_;
+ Pylon::CBaslerUniversalInstantCamera *rcam_;
+ cv::Mat tmp_;
+ uint32_t fullwidth_;
+ uint32_t fullheight_;
+ uint32_t width_;
+ uint32_t height_;
+
+ cv::cuda::HostMem left_hm_;
+ cv::cuda::HostMem right_hm_;
+ cv::cuda::HostMem hres_hm_;
+ cv::Mat rtmp_;
+
+ void _configureCamera(Pylon::CBaslerUniversalInstantCamera *cam);
+};
+
+}
+}
+}
+
+#endif // _FTL_RGBD_PYLON_HPP_
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index 00805313c..bb33c6240 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -24,10 +24,13 @@
#include "ftl/threads.hpp"
#include "calibrate.hpp"
-#include "local.hpp"
+#include "opencv.hpp"
+
+#ifdef HAVE_PYLON
+#include "pylon.hpp"
+#endif
using ftl::rgbd::detail::Calibrate;
-using ftl::rgbd::detail::OpenCVDevice;
using ftl::rgbd::detail::StereoVideoSource;
using ftl::codecs::Channel;
using std::string;
@@ -42,7 +45,13 @@ ftl::rgbd::detail::Device::~Device() {
StereoVideoSource::StereoVideoSource(ftl::rgbd::Source *host)
: ftl::rgbd::BaseSourceImpl(host), ready_(false) {
- init("");
+ auto uri = host->get<std::string>("uri");
+ if (uri) {
+ init(*uri);
+ } else {
+ init("");
+ }
+
}
StereoVideoSource::StereoVideoSource(ftl::rgbd::Source *host, const string &file)
@@ -59,27 +68,37 @@ StereoVideoSource::~StereoVideoSource() {
void StereoVideoSource::init(const string &file) {
capabilities_ = kCapVideo | kCapStereo;
- /*if (ftl::is_video(file)) {
- // Load video file
- LOG(INFO) << "Using video file...";
- //lsrc_ = ftl::create<LocalSource>(host_, "feed", file);
- } else if (ftl::is_directory(file)) {
- // FIXME: This is not an ideal solution...
- ftl::config::addPath(file);
-
- auto vid = ftl::locateFile("video.mp4");
- if (!vid) {
- LOG(FATAL) << "No video.mp4 file found in provided paths (" << file << ")";
- } else {
- LOG(INFO) << "Using test directory...";
- //lsrc_ = ftl::create<LocalSource>(host_, "feed", *vid);
+ ftl::URI uri(file);
+
+ if (uri.getScheme() == ftl::URI::SCHEME_DEVICE) {
+ if (uri.getPathSegment(0) == "pylon") {
+ #ifdef HAVE_PYLON
+ LOG(INFO) << "Using Pylon...";
+ lsrc_ = ftl::create<ftl::rgbd::detail::PylonDevice>(host_, "feed");
+ #else
+ throw FTL_Error("Not built with pylon support");
+ #endif
+ } else if (uri.getPathSegment(0) == "video" || uri.getPathSegment(0) == "video") {
+ // Now detect automatically which device to use
+ #ifdef HAVE_PYLON
+ auto pylon_devices = ftl::rgbd::detail::PylonDevice::listDevices();
+ if (pylon_devices.size() > 0) {
+ LOG(INFO) << "Using Pylon...";
+ lsrc_ = ftl::create<ftl::rgbd::detail::PylonDevice>(host_, "feed");
+ } else {
+ // Use cameras
+ LOG(INFO) << "Using OpenCV cameras...";
+ lsrc_ = ftl::create<ftl::rgbd::detail::OpenCVDevice>(host_, "feed");
+ }
+ #else
+ // Use cameras
+ LOG(INFO) << "Using OpenCV cameras...";
+ lsrc_ = ftl::create<ftl::rgbd::detail::OpenCVDevice>(host_, "feed");
+ #endif
}
}
- else {*/
- // Use cameras
- LOG(INFO) << "Using cameras...";
- lsrc_ = ftl::create<OpenCVDevice>(host_, "feed");
- //}
+
+ if (!lsrc_) return; // throw?
color_size_ = cv::Size(lsrc_->width(), lsrc_->height());
--
GitLab