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

added util3d::cloudFromDisparity()

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1915 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-22 20:33:26 +00:00
parent d220301f50
commit 8e55d7847f
2 changed files with 50 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
corelib/src/util3d.cpp
View File

@@ -894,6 +894,50 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
return cloud;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
int decimation)
{
UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(decimation < 1)
{
return cloud;
}
//cloud.header = cameraInfo.header;
cloud->height = imageDisparity.rows/decimation;
cloud->width = imageDisparity.cols/decimation;
cloud->is_dense = false;
cloud->resize(cloud->height * cloud->width);
if(imageDisparity.type()==CV_16SC1)
{
for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
{
for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
{
float disp = float(imageDisparity.at<short>(h,w))/16.0f;
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
}
}
}
else
{
for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
{
for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
{
float disp = imageDisparity.at<float>(h,w);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
}
}
}
return cloud;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDisparity,