From 547f2854489a0378e44bc79ac45e5ab9edb066d8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 7 Mar 2022 17:51:41 -0500 Subject: [PATCH] point_cloud_xyz[rgb]: Fixed roi ratios when camera_info is not the same resolution than depth image. --- src/nodelets/point_cloud_xyz.cpp | 71 +++++++++++++++++++++++------ src/nodelets/point_cloud_xyzrgb.cpp | 59 ++++++++++++++++++------ 2 files changed, 100 insertions(+), 30 deletions(-) diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 3948f4ec..77febb3e 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -205,12 +205,12 @@ private: } void callback( - const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfo) { - if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && - depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && - depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) + if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && + depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 && + depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0) { NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16"); return; @@ -220,28 +220,68 @@ private: { ros::WallTime time = ros::WallTime::now(); - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg); cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_); - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); + rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo); pcl::PointCloud::Ptr pclCloud; - rtabmap::CameraModel m( - model.fx(), - model.fy(), - model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), - model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); + + cv::Mat depth = imageDepthPtr->image; + if( roiRatios_.size() == 4 && + ((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) || + (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) || + (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) || + (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f))) + { + cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_); + cv::Rect roiRgb; + if(model.imageWidth() && model.imageHeight()) + { + roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_); + } + if( roiDepth.width%decimation_==0 && + roiDepth.height%decimation_==0 && + (roiRgb.width != 0 || + (roiRgb.width%decimation_==0 && + roiRgb.height%decimation_==0))) + { + depth = cv::Mat(depth, roiDepth); + if(model.imageWidth() != 0 && model.imageHeight() != 0) + { + model = model.roi(roiRgb); + } + else + { + model = model.roi(roiDepth); + } + } + else + { + NODELET_ERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting " + "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly " + "by decimation parameter (%d). Ignoring ROI ratios...", + roiRatios_[0], + roiRatios_[1], + roiRatios_[2], + roiRatios_[3], + roiDepth.width, + roiDepth.height, + roiRgb.width, + roiRgb.height, + decimation_); + } + } pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromDepth( - cv::Mat(imageDepthPtr->image, roi), - m, + depth, + model, decimation_, maxDepth_, minDepth_, indices.get()); - processAndPublish(pclCloud, indices, depth->header); + processAndPublish(pclCloud, indices, depthMsg->header); NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec()); } @@ -276,6 +316,7 @@ private: pcl::PointCloud::Ptr pclCloud; rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); + UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight()); rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T); pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromDisparity( diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index 95b9a310..3a31d5f2 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -296,25 +296,51 @@ private: cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(*cameraInfo); - - ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols); - ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows); + rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo); pcl::PointCloud::Ptr pclCloud; - cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_); - rtabmap::CameraModel m( - model.fx(), - model.fy(), - model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), - model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); + cv::Mat rgb = imagePtr->image; + cv::Mat depth = imageDepthPtr->image; + if( roiRatios_.size() == 4 && + ((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) || + (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) || + (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) || + (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f))) + { + cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_); + cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_); + if( roiDepth.width%decimation_==0 && + roiDepth.height%decimation_==0 && + roiRgb.width%decimation_==0 && + roiRgb.height%decimation_==0) + { + depth = cv::Mat(depth, roiDepth); + rgb = cv::Mat(rgb, roiRgb); + model = model.roi(roiRgb); + } + else + { + NODELET_ERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting " + "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly " + "by decimation parameter (%d). Ignoring ROI ratios...", + roiRatios_[0], + roiRatios_[1], + roiRatios_[2], + roiRatios_[3], + roiDepth.width, + roiDepth.height, + roiRgb.width, + roiRgb.height, + decimation_); + } + } + pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromDepthRGB( - cv::Mat(imagePtr->image, roi), - cv::Mat(imageDepthPtr->image, roi), - m, + rgb, + depth, + model, decimation_, maxDepth_, minDepth_, @@ -372,6 +398,8 @@ private: pcl::PointCloud::Ptr pclCloud; rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); + UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight()); + UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight()); rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T); pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromDisparityRGB( @@ -467,7 +495,8 @@ private: maxDepth_, minDepth_, indices.get(), - stereoBMParameters_); + stereoBMParameters_, + roiRatios_); processAndPublish(pclCloud, indices, image->header); }