i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 12:09:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

pointcloud_to_depthimage: added upscale option

Browse Source
This commit is contained in:
matlabbe
2022-11-26 18:20:25 -08:00
parent ccd7b62721
commit 671c80dc3f
1 changed files with 12 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/pointcloud_to_depthimage.cpp
+12
View File
@@ -65,6 +65,8 @@ public:
fillHolesError_(0.1), fillHolesError_(0.1),
fillIterations_(1), fillIterations_(1),
decimation_(1), decimation_(1),
upscale_(false),
upscaleDepthErrorRatio_(0.02),
approxSync_(0), approxSync_(0),
exactSync_(0) exactSync_(0)
{} {}
@@ -100,6 +102,8 @@ private:
pnh.param("fill_iterations", fillIterations_, fillIterations_); pnh.param("fill_iterations", fillIterations_, fillIterations_);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
pnh.param("approx", approx, approx); pnh.param("approx", approx, approx);
pnh.param("upscale", upscale_, upscale_);
pnh.param("upscale_depth_error_ratio", upscaleDepthErrorRatio_, upscaleDepthErrorRatio_);
if(fixedFrameId_.empty() && approx) if(fixedFrameId_.empty() && approx)
{ {
@@ -118,6 +122,7 @@ private:
ROS_INFO(" fill_holes_error=%f", fillHolesError_); ROS_INFO(" fill_holes_error=%f", fillHolesError_);
ROS_INFO(" fill_iterations=%d", fillIterations_); ROS_INFO(" fill_iterations=%d", fillIterations_);
ROS_INFO(" decimation=%d", decimation_); ROS_INFO(" decimation=%d", decimation_);
ROS_INFO(" upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
image_transport::ImageTransport it(nh); image_transport::ImageTransport it(nh);
depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
@@ -239,6 +244,11 @@ private:
depthImage.header = cameraInfoMsg->header; depthImage.header = cameraInfoMsg->header;
if(decimation_>1 && upscale_)
{
depthImage.image = rtabmap::util2d::interpolate(depthImage.image, decimation_, upscaleDepthErrorRatio_);
}
if(depthImage32Pub_.getNumSubscribers()) if(depthImage32Pub_.getNumSubscribers())
{ {
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
@@ -288,6 +298,8 @@ private:
double fillHolesError_; double fillHolesError_;
int fillIterations_; int fillIterations_;
int decimation_; int decimation_;
bool upscale_;
double upscaleDepthErrorRatio_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyApproxSyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_; message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;