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

Added warning on invalid decimation value when creating a RGBD cloud from SensorData

Browse Source
This commit is contained in:
matlabbe
2015-08-25 16:29:27 -04:00
parent 537a473482
commit dbc03024ef
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d.cpp
+10 -2
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util2d.h> #include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h> #include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h> #include <pcl/common/transforms.h>
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/imgproc.hpp>
@@ -287,8 +288,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
{ {
UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols); UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1)); UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT(imageDepth.rows % decimation == 0); UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
UASSERT(imageDepth.cols % decimation == 0); UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageDepth.rows, decimation).c_str());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(decimation < 1) if(decimation < 1)
@@ -641,6 +642,13 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
{ {
if(sensorData.cameraModels()[i].isValid()) if(sensorData.cameraModels()[i].isValid())
{ {
if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
{
UWARN("Image size (%d,%d) modulus decimation (%d) is not null "
"for the cloud creation! Setting decimation to 1...",
subImageWidth, sensorData.depthRaw().rows, decimation);
decimation = 1;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB( pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)), cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),