From 0dd099925cf630c1ba97fc01c75844b5dfc82e44 Mon Sep 17 00:00:00 2001 From: Matia Pizzoli Date: Sun, 15 Nov 2015 15:39:58 +0100 Subject: [PATCH] added lock guard on reference image --- include/rmd/depthmap.h | 35 ++++++++++++++++--- include/rmd/depthmap_node.h | 2 +- include/rmd/publisher.h | 23 +++++-------- src/depthmap.cpp | 51 ++++++++++++++++++---------- src/depthmap_node.cpp | 23 ++++++------- src/publisher.cpp | 68 +++++++++++++++++++++---------------- test/dataset_main.cpp | 6 ++-- 7 files changed, 126 insertions(+), 82 deletions(-) diff --git a/include/rmd/depthmap.h b/include/rmd/depthmap.h index ea51a6c..53131ff 100644 --- a/include/rmd/depthmap.h +++ b/include/rmd/depthmap.h @@ -25,6 +25,8 @@ #include #include +#include + namespace rmd { @@ -54,10 +56,16 @@ class Depthmap void update(const cv::Mat &img_curr, const SE3 &T_curr_world); - const cv::Mat_ outputDepthmap(); - const cv::Mat_ outputDenoisedDepthmap(float lambda, int iterations); - const cv::Mat_ outpuConvergenceMap(); - const cv::Mat outputReferenceImage(); + void downloadDepthmap(); + void downloadDenoisedDepthmap(float lambda, int iterations); + + const cv::Mat_ getDepthmap() const; + + void downloadConvergenceMap(); + const cv::Mat_ getConvergenceMap() const; + + + const cv::Mat getReferenceImage() const; size_t getConvergedCount() const; float getConvergedPercentage() const; @@ -66,6 +74,21 @@ class Depthmap // only for test and debug static cv::Mat scaleMat(const cv::Mat &depthmap); + float getFx() const + { return fx_; } + + float getFy() const + { return fy_; } + + float getCx() const + { return cx_; } + + float getCy() const + { return cy_; } + + std::mutex & getRefImgMutex() + { return ref_img_mutex_; } + private: void inputImage(const cv::Mat &img_8uc1); @@ -78,7 +101,9 @@ class Depthmap cv::Mat undist_map1_, undist_map2_; cv::Mat img_undistorted_32fc1_; cv::Mat img_undistorted_8uc1_; + cv::Mat ref_img_undistorted_8uc1_; + std::mutex ref_img_mutex_; bool is_distorted_; @@ -86,6 +111,8 @@ class Depthmap cv::Mat output_depth_32fc1_; cv::Mat output_convergence_int_; + + const float fx_, fy_, cx_, cy_; }; } diff --git a/include/rmd/depthmap_node.h b/include/rmd/depthmap_node.h index 4cffdba..e1d3c91 100644 --- a/include/rmd/depthmap_node.h +++ b/include/rmd/depthmap_node.h @@ -48,7 +48,7 @@ class DepthmapNode private: void denoiseAndPublishResults(); - std::unique_ptr depthmap_; + std::shared_ptr depthmap_; State state_; float ref_compl_perc_; diff --git a/include/rmd/publisher.h b/include/rmd/publisher.h index 441e85c..1ae4d5c 100644 --- a/include/rmd/publisher.h +++ b/include/rmd/publisher.h @@ -1,6 +1,8 @@ #ifndef RMD_PUBLISHER_H #define RMD_PUBLISHER_H +#include + #include #include #include @@ -16,26 +18,19 @@ class Publisher typedef pcl::PointCloud PointCloud; public: - Publisher(float fx, - float cx, - float fy, - float cy, - ros::NodeHandle &nh); + Publisher(ros::NodeHandle &nh, + std::shared_ptr depthmap); - void publishDepthmap(const cv::Mat &depthmap) const; + void publishDepthmap() const; - void publishPointCloud(const cv::Mat &depthmap, - const cv::Mat &ref_img, - const cv::Mat &convergence) const; + void publishPointCloud() const; - void publishDepthmapAndPointCloud(const cv::Mat &depthmap, - const cv::Mat &ref_img, - const cv::Mat &convergence) const; + void publishDepthmapAndPointCloud() const; private: - float fx_, cx_, fy_, cy_; - ros::NodeHandle &nh_; + std::shared_ptr depthmap_; + image_transport::Publisher depthmap_publisher_; PointCloud::Ptr pc_; diff --git a/src/depthmap.cpp b/src/depthmap.cpp index 6678e63..a189482 100644 --- a/src/depthmap.cpp +++ b/src/depthmap.cpp @@ -17,17 +17,20 @@ #include -rmd::Depthmap::Depthmap( - size_t width, - size_t height, - float fx, - float cx, - float fy, - float cy) +rmd::Depthmap::Depthmap(size_t width, + size_t height, + float fx, + float cx, + float fy, + float cy) : width_(width) , height_(height) , is_distorted_(false) , seeds_(width, height, rmd::PinholeCamera(fx, fy, cx, cy)) + , fx_(fx) + , fy_(fy) + , cx_(cx) + , cy_(cy) { cv_K_ = (cv::Mat_(3, 3) << fx, 0.0f, cx, 0.0f, fy, cy, 0.0f, 0.0f, 1.0f); denoiser_.reset(new rmd::DepthmapDenoiser(width_, height_)); @@ -65,12 +68,17 @@ bool rmd::Depthmap::setReferenceImage( { denoiser_->setLargeSigmaSq(max_depth-min_depth); inputImage(img_curr); - img_undistorted_8uc1_.copyTo(ref_img_undistorted_8uc1_); - return seeds_.setReferenceImage( - reinterpret_cast(img_undistorted_32fc1_.data), - T_curr_world, - min_depth, - max_depth); + const bool ret = seeds_.setReferenceImage(reinterpret_cast(img_undistorted_32fc1_.data), + T_curr_world, + min_depth, + max_depth); + + { + std::lock_guard lock(ref_img_mutex_); + img_undistorted_8uc1_.copyTo(ref_img_undistorted_8uc1_); + } + + return ret; } void rmd::Depthmap::update( @@ -96,13 +104,12 @@ void rmd::Depthmap::inputImage(const cv::Mat &img_8uc1) img_undistorted_8uc1_.convertTo(img_undistorted_32fc1_, CV_32F, 1.0f/255.0f); } -const cv::Mat_ rmd::Depthmap::outputDepthmap() +void rmd::Depthmap::downloadDepthmap() { seeds_.downloadDepthmap(reinterpret_cast(output_depth_32fc1_.data)); - return output_depth_32fc1_; } -const cv::Mat_ rmd::Depthmap::outputDenoisedDepthmap(float lambda, int iterations) +void rmd::Depthmap::downloadDenoisedDepthmap(float lambda, int iterations) { denoiser_->denoise( seeds_.getMu(), @@ -112,16 +119,24 @@ const cv::Mat_ rmd::Depthmap::outputDenoisedDepthmap(float lambda, int it reinterpret_cast(output_depth_32fc1_.data), lambda, iterations); +} + +const cv::Mat_ rmd::Depthmap::getDepthmap() const +{ return output_depth_32fc1_; } -const cv::Mat_ rmd::Depthmap::outpuConvergenceMap() +void rmd::Depthmap::downloadConvergenceMap() { seeds_.downloadConvergence(reinterpret_cast(output_convergence_int_.data)); +} + +const cv::Mat_ rmd::Depthmap::getConvergenceMap() const +{ return output_convergence_int_; } -const cv::Mat rmd::Depthmap::outputReferenceImage() +const cv::Mat rmd::Depthmap::getReferenceImage() const { return ref_img_undistorted_8uc1_; } diff --git a/src/depthmap_node.cpp b/src/depthmap_node.cpp index a972294..1a71bf6 100644 --- a/src/depthmap_node.cpp +++ b/src/depthmap_node.cpp @@ -56,12 +56,12 @@ bool rmd::DepthmapNode::init() const float cam_cx = vk::getParam("remode/cam_cx"); const float cam_cy = vk::getParam("remode/cam_cy"); - depthmap_.reset(new rmd::Depthmap(cam_width, - cam_height, - cam_fx, - cam_cx, - cam_fy, - cam_cy)); + depthmap_ = std::make_shared(cam_width, + cam_height, + cam_fx, + cam_cx, + cam_fy, + cam_cy); if(vk::hasParam("remode/cam_k1") && vk::hasParam("remode/cam_k2") && @@ -77,7 +77,7 @@ bool rmd::DepthmapNode::init() ref_compl_perc_ = vk::getParam("remode/ref_compl_perc", 10.0f); - publisher_.reset(new rmd::Publisher(cam_fx, cam_cx, cam_fy, cam_cy, nh_)); + publisher_.reset(new rmd::Publisher(nh_, depthmap_)); return true; } @@ -155,13 +155,10 @@ void rmd::DepthmapNode::denseInputCallback( void rmd::DepthmapNode::denoiseAndPublishResults() { - cv::Mat curr_depth = depthmap_->outputDenoisedDepthmap(0.5f, 200); - cv::Mat curr_convergence = depthmap_->outpuConvergenceMap(); - cv::Mat curr_ref_img = depthmap_->outputReferenceImage().clone(); + depthmap_->downloadDenoisedDepthmap(0.5f, 200); + depthmap_->downloadConvergenceMap(); std::async(std::launch::async, &rmd::Publisher::publishDepthmapAndPointCloud, - publisher_.get(), - curr_depth, curr_ref_img, curr_convergence); - + publisher_.get()); } diff --git a/src/publisher.cpp b/src/publisher.cpp index 23a721d..60a0b22 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -1,4 +1,5 @@ #include + #include #include @@ -6,51 +7,60 @@ #include -rmd::Publisher::Publisher(float fx, float cx, float fy, float cy, ros::NodeHandle &nh) - : fx_(fx) - , cx_(cx) - , fy_(fy) - , cy_(cy) - , nh_(nh) +rmd::Publisher::Publisher(ros::NodeHandle &nh, + std::shared_ptr depthmap) + : nh_(nh) , pc_(new PointCloud) { + depthmap_ = depthmap; image_transport::ImageTransport it(nh_); depthmap_publisher_= it.advertise("remode/depth", 10); pub_pc_ = nh_.advertise("remode/pointcloud", 1); } -void rmd::Publisher::publishDepthmap(const cv::Mat &depthmap) const +void rmd::Publisher::publishDepthmap() const { cv_bridge::CvImage cv_image; cv_image.header.stamp = ros::Time::now(); cv_image.header.frame_id = "depthmap"; cv_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - cv_image.image = depthmap; + cv_image.image = depthmap_->getDepthmap(); depthmap_publisher_.publish(cv_image.toImageMsg()); std::cout << "INFO: publishing depth map" << std::endl; } -void rmd::Publisher::publishPointCloud(const cv::Mat &depthmap, - const cv::Mat &ref_img, - const cv::Mat &convergence) const +void rmd::Publisher::publishPointCloud() const { - for(int y=0; y lock(depthmap_->getRefImgMutex()); + + const cv::Mat depth = depthmap_->getDepthmap(); + const cv::Mat convergence = depthmap_->getConvergenceMap(); + const cv::Mat ref_img = depthmap_->getReferenceImage(); + + const float fx = depthmap_->getFx(); + const float fy = depthmap_->getFy(); + const float cx = depthmap_->getCx(); + const float cy = depthmap_->getCy(); + + for(int y=0; y(y,x); - if(convergence.at(y, x) != rmd::ConvergenceState::DIVERGED && - convergence.at(y, x) != rmd::ConvergenceState::BORDER) + for(int x=0; x(y,x); - p.intensity = intensity; - pc_->push_back(p); + Eigen::Vector3f f((x-cx)/fx, (y-cy)/fy, 1.0); + f.normalize(); + Eigen::Vector3f xyz = f*depth.at(y,x); + if(convergence.at(y, x) != rmd::ConvergenceState::DIVERGED && + convergence.at(y, x) != rmd::ConvergenceState::BORDER) + { + PointType p; + p.x = xyz.x(); + p.y = xyz.y(); + p.z = xyz.z(); + uint8_t intensity = ref_img.at(y,x); + p.intensity = intensity; + pc_->push_back(p); + } } } } @@ -70,10 +80,8 @@ void rmd::Publisher::publishPointCloud(const cv::Mat &depthmap, } } -void rmd::Publisher::publishDepthmapAndPointCloud(const cv::Mat &depthmap, - const cv::Mat &ref_img, - const cv::Mat &convergence) const +void rmd::Publisher::publishDepthmapAndPointCloud() const { - publishDepthmap(depthmap); - publishPointCloud(depthmap, ref_img, convergence); + publishDepthmap(); + publishPointCloud(); } diff --git a/test/dataset_main.cpp b/test/dataset_main.cpp index 3329972..25b84d1 100644 --- a/test/dataset_main.cpp +++ b/test/dataset_main.cpp @@ -107,12 +107,14 @@ int main(int argc, char **argv) } // show depthmap - cv::Mat result = depthmap.outputDepthmap(); + depthmap.downloadDepthmap(); + cv::Mat result = depthmap.getDepthmap(); cv::Mat colored = rmd::Depthmap::scaleMat(result); cv::imshow("result", colored); // denoise - cv::Mat denoised_result = depthmap.outputDenoisedDepthmap(0.4f, 200); + depthmap.downloadDenoisedDepthmap(0.5f, 200); + cv::Mat denoised_result = depthmap.getDepthmap(); cv::Mat colored_denoised = rmd::Depthmap::scaleMat(denoised_result); cv::imshow("denoised_result", colored_denoised);