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

Fixed cloudFromDepth() method when depth image is not the same size as the calibration file. That fixes projection map created from Tango databases (where depth size != rgb size)

Browse Source
This commit is contained in:
matlabbe
2016-07-08 12:47:23 -04:00
parent c8cd2545d3
commit 4cd8705710
8 changed files with 239 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
corelib/include/rtabmap/core/util3d.h
View File

@@ -74,16 +74,23 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
bool smoothing,
float maxZError = 0.02f);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0), "Use cloudFromDepth with CameraModel interface.");
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
const CameraModel & model,
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
float cx, float cy,
@@ -91,6 +98,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0), "Use cloudFromDepthRGB with CameraModel interface.");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
const CameraModel & model,
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(

12
corelib/include/rtabmap/core/util3d_registration.h
View File

@@ -92,18 +92,6 @@ Transform RTABMAP_EXP icpPointToPlane(
float epsilon = 0.0f,
bool icp2D = false);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
int decimation,
double maxDepth,
float voxel,
int samples,
const Transform & transform = Transform::getIdentity());
} // namespace util3d
} // namespace rtabmap

95
corelib/src/util3d.cpp
View File

@@ -248,9 +248,42 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
float minDepth,
std::vector<int> * validIndices)
{
CameraModel model(fx, fy, cx, cy);
return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepth,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices)
{
float rgbToDepthFactorX = 1.0f;
float rgbToDepthFactorY = 1.0f;
UASSERT(model.isValidForProjection());
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
int imageRows = imageDepth.rows;
int imageCols = imageDepth.cols;
if(model.imageHeight()>0 && model.imageWidth()>0)
{
UASSERT(model.imageHeight() % imageDepth.rows == 0 && model.imageWidth() % imageDepth.cols == 0);
UASSERT_MSG(model.imageHeight() % decimation == 0, uFormat("model.imageHeight()=%d decimation=%d", model.imageHeight(), 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));
rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows));
imageRows = model.imageHeight();
imageCols = model.imageWidth();
}
else
{
UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(decimation < 1)
@@ -259,8 +292,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
}
//cloud.header = cameraInfo.header;
cloud->height = imageDepth.rows/decimation;
cloud->width = imageDepth.cols/decimation;
cloud->height = imageRows/decimation;
cloud->width = imageCols/decimation;
cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width);
if(validIndices)
@@ -268,14 +301,27 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
validIndices->resize(cloud->size());
}
float depthFx = model.fx() * rgbToDepthFactorX;
float depthFy = model.fy() * rgbToDepthFactorY;
float depthCx = model.cx() * rgbToDepthFactorX;
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",
imageCols, imageRows,
imageDepth.cols, imageDepth.rows,
model.fx(), model.fy(), model.cx(), model.cy(),
rgbToDepthFactorX,
rgbToDepthFactorY,
decimation);
int oi = 0;
for(int h = 0; h < imageDepth.rows; h+=decimation)
for(int h = 0; h < imageRows && h/decimation < (int)cloud->height; h+=decimation)
{
for(int w = 0; w < imageDepth.cols; w+=decimation)
for(int w = 0; w < imageCols && w/decimation < (int)cloud->width; w+=decimation)
{
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
{
pt.x = ptXYZ.x;
@@ -310,8 +356,23 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
float maxDepth,
float minDepth,
std::vector<int> * validIndices)
{
CameraModel model(fx, fy, cx, cy);
return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices)
{
UDEBUG("");
UASSERT(model.isValidForProjection());
UASSERT((model.imageHeight() == 0 && model.imageWidth() == 0) || (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols));
UASSERT(imageRgb.rows % imageDepth.rows == 0 && imageRgb.cols % imageDepth.cols == 0);
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT_MSG(imageRgb.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageRgb.rows, decimation).c_str());
@@ -349,15 +410,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
float rgbToDepthFactorX = 1.0f/float((imageRgb.cols / imageDepth.cols));
float rgbToDepthFactorY = 1.0f/float((imageRgb.rows / imageDepth.rows));
float depthFx = fx * rgbToDepthFactorX;
float depthFy = fy * rgbToDepthFactorY;
float depthCx = cx * rgbToDepthFactorX;
float depthCy = cy * rgbToDepthFactorY;
float depthFx = model.fx() * rgbToDepthFactorX;
float depthFy = model.fy() * rgbToDepthFactorY;
float depthCx = model.cx() * rgbToDepthFactorX;
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",
imageRgb.cols, imageRgb.rows,
imageDepth.cols, imageDepth.rows,
fx, fy, cx, cy,
model.fx(), model.fy(), model.cx(), model.cy(),
rgbToDepthFactorX,
rgbToDepthFactorY,
decimation);
@@ -653,10 +714,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
{
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i].cx(),
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
sensorData.cameraModels()[i],
decimation,
maxDepth,
minDepth,
@@ -767,10 +825,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
cv::Mat(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i].cx(),
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
sensorData.cameraModels()[i],
decimation,
maxDepth,
minDepth,

54
corelib/src/util3d_registration.cpp
View File

@@ -380,60 +380,6 @@ Transform icpPointToPlane(
return Transform::fromEigen4f(icp.getFinalTransformation());
}
// If "voxel" > 0, "samples" is ignored
pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
int decimation,
double maxDepth,
float voxel,
int samples,
const Transform & transform)
{
UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = cloudFromDepth(
depth,
cx,
cy,
fx,
fy,
decimation);
if(cloud->size())
{
if(maxDepth>0.0)
{
cloud = passThrough(cloud, "z", 0, maxDepth);
}
if(cloud->size())
{
if(voxel>0)
{
cloud = voxelize(cloud, voxel);
}
else if(samples>0 && (int)cloud->size() > samples)
{
cloud = randomSampling(cloud, samples);
}
if(cloud->size())
{
if(!transform.isNull() && !transform.isIdentity())
{
cloud = transformPointCloud(cloud, transform);
}
}
}
}
return cloud;
}
}
}