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

point_cloud_xyz[rgb]: Fixed roi ratios when camera_info is not the same resolution than depth image.

Browse Source
This commit is contained in:
matlabbe
2022-03-07 17:51:41 -05:00
parent 0325a87bca
commit 547f285448
2 changed files with 100 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_xyzrgb.cpp
+44 -15
View File
@@ -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<pcl::PointXYZRGB>::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<int>);
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<pcl::PointXYZRGB>::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<int>);
pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
@@ -467,7 +495,8 @@ private:
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
stereoBMParameters_,
roiRatios_);
processAndPublish(pclCloud, indices, image->header);
}