diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp index 84d1e574b8e7aff91c774a1035c75280cd8ac03c..b467099f59e766a0fadb9d8330e276f952f10d04 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp @@ -8,7 +8,6 @@ #include "ftl/operators/opticalflow.hpp" #endif - #include "ftl/operators/smoothing.hpp" #include "ftl/operators/colours.hpp" #include "ftl/operators/normals.hpp" @@ -69,10 +68,10 @@ void StereoVideoSource::init(const string &file) { lsrc_ = ftl::create<LocalSource>(host_, "feed"); } - cv::Size size = cv::Size(lsrc_->width(), lsrc_->height()); + color_size_ = cv::Size(lsrc_->width(), lsrc_->height()); frames_ = std::vector<Frame>(2); - calib_ = ftl::create<Calibrate>(host_, "calibration", size, stream_); + calib_ = ftl::create<Calibrate>(host_, "calibration", color_size_, stream_); if (!calib_->isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!"; // Generate camera parameters from camera matrix @@ -131,6 +130,9 @@ void StereoVideoSource::init(const string &file) { pipeline_input_->append<ftl::operators::NVOpticalFlow>("optflow"); #endif + depth_size_ = cv::Size( host_->value("width", color_size_.width), + host_->value("height", color_size_.height)); + pipeline_depth_ = ftl::config::create<ftl::operators::Graph>(host_, "disparity"); pipeline_depth_->append<ftl::operators::FixstarsSGM>("algorithm"); @@ -205,8 +207,28 @@ bool StereoVideoSource::compute(int n, int b) { } if (chan == Channel::Depth) { - pipeline_depth_->apply(frame, frame, host_, cv::cuda::StreamAccessor::getStream(stream_)); + // stereo algorithms assume input same size as output, resize if + // necessary + bool resize = (depth_size_ != color_size_); + if (resize) { + cv::cuda::GpuMat &left = frame.get<cv::cuda::GpuMat>(Channel::Left); + cv::cuda::GpuMat &right = frame.get<cv::cuda::GpuMat>(Channel::Right); + std::swap(fullres_left_, left); + std::swap(fullres_right_, right); + cv::cuda::resize(fullres_left_, left, depth_size_, 0.0, 0.0, cv::INTER_CUBIC, stream_); + cv::cuda::resize(fullres_right_, right, depth_size_, 0.0, 0.0, cv::INTER_CUBIC, stream_); + } + + pipeline_depth_->apply(frame, frame, host_, cv::cuda::StreamAccessor::getStream(stream_)); stream_.waitForCompletion(); + + if (resize) { + cv::cuda::GpuMat &left = frame.get<cv::cuda::GpuMat>(Channel::Left); + cv::cuda::GpuMat &right = frame.get<cv::cuda::GpuMat>(Channel::Right); + std::swap(fullres_left_, left); + std::swap(fullres_right_, right); + } + host_->notify(timestamp_, frame.get<cv::cuda::GpuMat>(Channel::Left), frame.get<cv::cuda::GpuMat>(Channel::Depth)); diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp index 78fcdbcf809eeae0d5de6da30b99ce3dc07f9214..6cd5a09b67bbbf0e4e6e66ccfa8056f293194e40 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp @@ -38,9 +38,15 @@ class StereoVideoSource : public detail::Source { LocalSource *lsrc_; Calibrate *calib_; + cv::Size color_size_; + cv::Size depth_size_; + ftl::operators::Graph *pipeline_input_; ftl::operators::Graph *pipeline_depth_; + cv::cuda::GpuMat fullres_left_; + cv::cuda::GpuMat fullres_right_; + bool ready_; cv::cuda::Stream stream_;