diff --git a/components/rgbd-sources/src/local.cpp b/components/rgbd-sources/src/local.cpp index 28ba2b4102db31450d93f0a95e6b7da4122d2c0e..410cb9f96c0a1c1ca48cadd52300e25b260c3aad 100644 --- a/components/rgbd-sources/src/local.cpp +++ b/components/rgbd-sources/src/local.cpp @@ -232,14 +232,14 @@ bool LocalSource::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda high_resolution_clock::now().time_since_epoch()).count(); // Limit max framerate - if (timestamp - timestamp_ < tps_) { - sleep_for(milliseconds((int)std::round((tps_ - (timestamp - timestamp_))*1000))); - } + //if (timestamp - timestamp_ < tps_) { + // sleep_for(milliseconds((int)std::round((tps_ - (timestamp - timestamp_))*1000))); + //} timestamp_ = timestamp; if (camera_b_ || !stereo_) { - if (!camera_a_->retrieve(l)) { + if (!camera_a_->retrieve(left_)) { LOG(ERROR) << "Unable to read frame from camera A"; return false; } @@ -257,9 +257,9 @@ bool LocalSource::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda int resx = frame.cols / 2; if (flip_) { r = Mat(frame, Rect(0, 0, resx, frame.rows)); - l = Mat(frame, Rect(resx, 0, frame.cols-resx, frame.rows)); + left_ = Mat(frame, Rect(resx, 0, frame.cols-resx, frame.rows)); } else { - l = Mat(frame, Rect(0, 0, resx, frame.rows)); + left_ = Mat(frame, Rect(0, 0, resx, frame.rows)); r = Mat(frame, Rect(resx, 0, frame.cols-resx, frame.rows)); } } @@ -267,7 +267,7 @@ bool LocalSource::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda if (downsize_ != 1.0f) { // cv::cuda::resize() - cv::resize(l, l, cv::Size((int)(l.cols * downsize_), (int)(l.rows * downsize_)), + cv::resize(left_, left_, cv::Size((int)(left_.cols * downsize_), (int)(left_.rows * downsize_)), 0, 0, cv::INTER_LINEAR); cv::resize(r, r, cv::Size((int)(r.cols * downsize_), (int)(r.rows * downsize_)), 0, 0, cv::INTER_LINEAR); @@ -281,13 +281,13 @@ bool LocalSource::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda if (flip_v_) { Mat tl, tr; - cv::flip(l, tl, 0); + cv::flip(left_, tl, 0); cv::flip(r, tr, 0); - l = tl; + left_ = tl; r = tr; } - l_out.upload(l, stream); + l_out.upload(left_, stream); r_out.upload(r, stream); return true; diff --git a/components/rgbd-sources/src/local.hpp b/components/rgbd-sources/src/local.hpp index 9526f0d2ddcbfc5b82eb941fd17996a888f6c46a..e3fcb91bd585d8090cfb50a8ee83bf49dd78f98d 100644 --- a/components/rgbd-sources/src/local.hpp +++ b/components/rgbd-sources/src/local.hpp @@ -26,6 +26,8 @@ class LocalSource : public Configurable { unsigned int width() const { return width_; } unsigned int height() const { return height_; } + + cv::Mat &cachedLeft() { return left_; } //void setFramerate(float fps); //float getFramerate() const; @@ -47,6 +49,7 @@ class LocalSource : public Configurable { cv::VideoCapture *camera_b_; unsigned int width_; unsigned int height_; + cv::Mat left_; }; } diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp index 41716ab2b565156b3347463ee575e7bf1506bcc0..b75380746942b3af2af1c5fca11b43afa848cf6e 100644 --- a/components/rgbd-sources/src/stereovideo.cpp +++ b/components/rgbd-sources/src/stereovideo.cpp @@ -105,12 +105,11 @@ bool StereoVideoSource::grab() { calib_->rectifyStereo(left_, right_, stream_); disp_->compute(left_, right_, disp_tmp_, stream_); disparityToDepth(disp_tmp_, depth_tmp_, calib_->getQ(), stream_); - left_.download(rgb_, stream_); // TODO original left RGB image could be stored in host memory + //left_.download(rgb_, stream_); + rgb_ = lsrc_->cachedLeft(); depth_tmp_.download(depth_, stream_); - // TODO(Nick) Could potentially leave this until getFrames? - stream_.waitForCompletion(); - + stream_.waitForCompletion(); return true; }