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

util3d::cloud[RGB]FromSensorData(): supporting roi ratios for stereo images

Browse Source
This commit is contained in:
matlabbe
2017-08-02 15:14:03 -04:00
parent dd3c7d8fa4
commit 82fd5b36a8
2 changed files with 69 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
-4
View File
@@ -892,12 +892,8 @@ IF(ORB_SLAM2_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = YES (License: GPLv3)") MESSAGE(STATUS " With ORB_SLAM2 = YES (License: GPLv3)")
ELSEIF(NOT WITH_ORB_SLAM2) ELSEIF(NOT WITH_ORB_SLAM2)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_ORB_SLAM2=OFF)") MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_ORB_SLAM2=OFF)")
ELSEIF(G2O_FOUND AND RealSense_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version, WITH_REALSENSE should also be off to avoid eigen errors)")
ELSEIF(G2O_FOUND) ELSEIF(G2O_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)") MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
ELSEIF(RealSense_FOUND)
MESSAGE(STATUS " With ORB_SLAM2 = NO (WITH_REALSENSE should be off to avoid eigen errors)")
ELSE() ELSE()
MESSAGE(STATUS " With ORB_SLAM2 = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)") MESSAGE(STATUS " With ORB_SLAM2 = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
ENDIF() ENDIF()
corelib/src/util3d.cpp
+69 -5
View File
@@ -957,9 +957,40 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
leftMono = sensorData.imageRaw(); leftMono = sensorData.imageRaw();
} }
cv::Mat right(sensorData.rightRaw());
StereoCameraModel model = sensorData.stereoCameraModel();
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 roi = util2d::computeRoi(leftMono, roiRatios);
if( roi.width%decimation==0 &&
roi.height%decimation==0)
{
leftMono = cv::Mat(leftMono, roi);
right = cv::Mat(right, roi);
model.roi(roi);
}
else
{
UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (left=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios[0],
roiRatios[1],
roiRatios[2],
roiRatios[3],
roi.width,
roi.height,
decimation);
}
}
cloud = cloudFromDisparity( cloud = cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), stereoParameters), util2d::disparityFromStereoImages(leftMono, right, stereoParameters),
sensorData.stereoCameraModel(), model,
decimation, decimation,
maxDepth, maxDepth,
minDepth, minDepth,
@@ -1091,10 +1122,43 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
{ {
//stereo //stereo
UDEBUG(""); UDEBUG("");
cv::Mat left(sensorData.imageRaw());
cv::Mat right(sensorData.rightRaw());
StereoCameraModel model = sensorData.stereoCameraModel();
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 roi = util2d::computeRoi(left, roiRatios);
if( roi.width%decimation==0 &&
roi.height%decimation==0)
{
left = cv::Mat(left, roi);
right = cv::Mat(right, roi);
model.roi(roi);
}
else
{
UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (left=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios[0],
roiRatios[1],
roiRatios[2],
roiRatios[3],
roi.width,
roi.height,
decimation);
}
}
cloud = cloudFromStereoImages( cloud = cloudFromStereoImages(
sensorData.imageRaw(), left,
sensorData.rightRaw(), right,
sensorData.stereoCameraModel(), model,
decimation, decimation,
maxDepth, maxDepth,
minDepth, minDepth,