i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

cloudFromDepthRGB() iterating over depth image size instead of RGB image size. DbViewer: fixed scale of image view when depth is smaller than rgb.

Browse Source
This commit is contained in:
matlabbe
2016-09-01 16:42:34 -04:00
parent 87b9f30dce
commit dd2f89628e
2 changed files with 27 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
corelib/src/util3d.cpp
View File

@@ -267,9 +267,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
UASSERT(model.isValidForProjection()); UASSERT(model.isValidForProjection());
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1)); UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
int imageRows = imageDepth.rows;
int imageCols = imageDepth.cols;
if(model.imageHeight()>0 && model.imageWidth()>0) if(model.imageHeight()>0 && model.imageWidth()>0)
{ {
UASSERT(model.imageHeight() % imageDepth.rows == 0 && model.imageWidth() % imageDepth.cols == 0); UASSERT(model.imageHeight() % imageDepth.rows == 0 && model.imageWidth() % imageDepth.cols == 0);
@@ -277,8 +274,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
UASSERT_MSG(model.imageWidth() % decimation == 0, uFormat("model.imageWidth()=%d decimation=%d", model.imageWidth(), decimation).c_str()); UASSERT_MSG(model.imageWidth() % decimation == 0, uFormat("model.imageWidth()=%d decimation=%d", model.imageWidth(), decimation).c_str());
rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols)); rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols));
rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows)); rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows));
imageRows = model.imageHeight();
imageCols = model.imageWidth();
} }
else else
{ {
@@ -293,8 +288,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
} }
//cloud.header = cameraInfo.header; //cloud.header = cameraInfo.header;
cloud->height = imageRows/decimation; cloud->height = imageDepth.rows/decimation;
cloud->width = imageCols/decimation; cloud->width = imageDepth.cols/decimation;
cloud->is_dense = false; cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width); cloud->resize(cloud->height * cloud->width);
if(validIndices) if(validIndices)
@@ -307,8 +302,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
float depthCx = model.cx() * rgbToDepthFactorX; float depthCx = model.cx() * rgbToDepthFactorX;
float depthCy = model.cy() * rgbToDepthFactorY; float depthCy = model.cy() * rgbToDepthFactorY;
UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
imageCols, imageRows,
imageDepth.cols, imageDepth.rows, imageDepth.cols, imageDepth.rows,
model.fx(), model.fy(), model.cx(), model.cy(), model.fx(), model.fy(), model.cx(), model.cy(),
rgbToDepthFactorX, rgbToDepthFactorX,
@@ -316,13 +310,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
decimation); decimation);
int oi = 0; int oi = 0;
for(int h = 0; h < imageRows && h/decimation < (int)cloud->height; h+=decimation) for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
{ {
for(int w = 0; w < imageCols && w/decimation < (int)cloud->width; w+=decimation) for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
{ {
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false); pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth)) if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
{ {
pt.x = ptXYZ.x; pt.x = ptXYZ.x;
@@ -400,8 +394,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
} }
//cloud.header = cameraInfo.header; //cloud.header = cameraInfo.header;
cloud->height = imageRgb.rows/decimation; cloud->height = imageDepth.rows/decimation;
cloud->width = imageRgb.cols/decimation; cloud->width = imageDepth.cols/decimation;
cloud->is_dense = false; cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width); cloud->resize(cloud->height * cloud->width);
if(validIndices) if(validIndices)
@@ -409,12 +403,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
validIndices->resize(cloud->size()); validIndices->resize(cloud->size());
} }
float rgbToDepthFactorX = 1.0f/float((imageRgb.cols / imageDepth.cols)); float rgbToDepthFactorX = float(imageRgb.cols) / float(imageDepth.cols);
float rgbToDepthFactorY = 1.0f/float((imageRgb.rows / imageDepth.rows)); float rgbToDepthFactorY = float(imageRgb.rows) / float(imageDepth.rows);
float depthFx = model.fx() * rgbToDepthFactorX; float depthFx = model.fx() / rgbToDepthFactorX;
float depthFy = model.fy() * rgbToDepthFactorY; float depthFy = model.fy() / rgbToDepthFactorY;
float depthCx = model.cx() * rgbToDepthFactorX; float depthCx = model.cx() / rgbToDepthFactorX;
float depthCy = model.cy() * rgbToDepthFactorY; float depthCy = model.cy() / rgbToDepthFactorY;
UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d", UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
imageRgb.cols, imageRgb.rows, imageRgb.cols, imageRgb.rows,
@@ -425,26 +419,31 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
decimation); decimation);
int oi = 0; int oi = 0;
for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation) for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
{ {
for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation) for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
{ {
pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation)); pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
int x = int(w*rgbToDepthFactorX);
int y = int(h*rgbToDepthFactorY);
UASSERT(x >=0 && x<imageRgb.cols && y >=0 && y<imageRgb.rows);
if(!mono) if(!mono)
{ {
pt.b = imageRgb.at<cv::Vec3b>(h,w)[0]; const unsigned char * bgr = imageRgb.ptr<unsigned char>(y,x);
pt.g = imageRgb.at<cv::Vec3b>(h,w)[1]; pt.b = bgr[0];
pt.r = imageRgb.at<cv::Vec3b>(h,w)[2]; pt.g = bgr[1];
pt.r = bgr[2];
} }
else else
{ {
unsigned char v = imageRgb.at<unsigned char>(h,w); unsigned char v = imageRgb.at<unsigned char>(y,x);
pt.b = v; pt.b = v;
pt.g = v; pt.g = v;
pt.r = v; pt.r = v;
} }
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false); pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth)) if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
{ {
pt.x = ptXYZ.x; pt.x = ptXYZ.x;

2
guilib/src/DatabaseViewer.cpp
View File

@@ -2629,7 +2629,7 @@ void DatabaseViewer::update(int value,
if(!imgDepth.isNull()) if(!imgDepth.isNull())
{ {
view->setImageDepth(imgDepth); view->setImageDepth(imgDepth);
if(!img.isNull()) if(img.isNull())
{ {
rect = imgDepth.rect(); rect = imgDepth.rect();
} }