mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
1012 lines
27 KiB
C++
1012 lines
27 KiB
C++
/*
|
|
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
* Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
* Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
* Neither the name of the Universite de Sherbrooke nor the
|
|
names of its contributors may be used to endorse or promote products
|
|
derived from this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <rtabmap/core/util3d.h>
|
|
#include <rtabmap/core/util3d_transforms.h>
|
|
#include <rtabmap/core/util3d_filtering.h>
|
|
#include <rtabmap/core/util2d.h>
|
|
#include <rtabmap/utilite/ULogger.h>
|
|
#include <rtabmap/utilite/UMath.h>
|
|
#include <pcl/io/pcd_io.h>
|
|
#include <pcl/common/transforms.h>
|
|
#include <opencv2/imgproc/imgproc.hpp>
|
|
|
|
namespace rtabmap
|
|
{
|
|
|
|
namespace util3d
|
|
{
|
|
|
|
cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
|
|
{
|
|
cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
|
|
|
|
for(unsigned int h = 0; h < cloud.height; h++)
|
|
{
|
|
for(unsigned int w = 0; w < cloud.width; w++)
|
|
{
|
|
if(bgrOrder)
|
|
{
|
|
frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
|
|
frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
|
|
frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
|
|
}
|
|
else
|
|
{
|
|
frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
|
|
frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
|
|
frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
|
|
}
|
|
}
|
|
}
|
|
return frameBGR;
|
|
}
|
|
|
|
// return float image in meter
|
|
cv::Mat depthFromCloud(
|
|
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
|
|
float & fx,
|
|
float & fy,
|
|
bool depth16U)
|
|
{
|
|
cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
|
|
fx = 0.0f; // needed to reconstruct the cloud
|
|
fy = 0.0f; // needed to reconstruct the cloud
|
|
for(unsigned int h = 0; h < cloud.height; h++)
|
|
{
|
|
for(unsigned int w = 0; w < cloud.width; w++)
|
|
{
|
|
float depth = cloud.at(h*cloud.width + w).z;
|
|
if(depth16U)
|
|
{
|
|
depth *= 1000.0f;
|
|
unsigned short depthMM = 0;
|
|
if(depth <= (float)USHRT_MAX)
|
|
{
|
|
depthMM = (unsigned short)depth;
|
|
}
|
|
frameDepth.at<unsigned short>(h,w) = depthMM;
|
|
}
|
|
else
|
|
{
|
|
frameDepth.at<float>(h,w) = depth;
|
|
}
|
|
|
|
// update constants
|
|
if(fx == 0.0f &&
|
|
uIsFinite(cloud.at(h*cloud.width + w).x) &&
|
|
uIsFinite(depth) &&
|
|
w != cloud.width/2 &&
|
|
depth > 0)
|
|
{
|
|
fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth);
|
|
if(depth16U)
|
|
{
|
|
fx*=1000.0f;
|
|
}
|
|
}
|
|
if(fy == 0.0f &&
|
|
uIsFinite(cloud.at(h*cloud.width + w).y) &&
|
|
uIsFinite(depth) &&
|
|
h != cloud.height/2 &&
|
|
depth > 0)
|
|
{
|
|
fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth);
|
|
if(depth16U)
|
|
{
|
|
fy*=1000.0f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return frameDepth;
|
|
}
|
|
|
|
// return (unsigned short 16bits image in mm) (float 32bits image in m)
|
|
void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
|
|
cv::Mat & frameBGR,
|
|
cv::Mat & frameDepth,
|
|
float & fx,
|
|
float & fy,
|
|
bool bgrOrder,
|
|
bool depth16U)
|
|
{
|
|
frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
|
|
frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
|
|
|
|
fx = 0.0f; // needed to reconstruct the cloud
|
|
fy = 0.0f; // needed to reconstruct the cloud
|
|
for(unsigned int h = 0; h < cloud.height; h++)
|
|
{
|
|
for(unsigned int w = 0; w < cloud.width; w++)
|
|
{
|
|
//rgb
|
|
if(bgrOrder)
|
|
{
|
|
frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
|
|
frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
|
|
frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
|
|
}
|
|
else
|
|
{
|
|
frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
|
|
frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
|
|
frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
|
|
}
|
|
|
|
//depth
|
|
float depth = cloud.at(h*cloud.width + w).z;
|
|
if(depth16U)
|
|
{
|
|
depth *= 1000.0f;
|
|
unsigned short depthMM = 0;
|
|
if(depth <= (float)USHRT_MAX)
|
|
{
|
|
depthMM = (unsigned short)depth;
|
|
}
|
|
frameDepth.at<unsigned short>(h,w) = depthMM;
|
|
}
|
|
else
|
|
{
|
|
frameDepth.at<float>(h,w) = depth;
|
|
}
|
|
|
|
// update constants
|
|
if(fx == 0.0f &&
|
|
uIsFinite(cloud.at(h*cloud.width + w).x) &&
|
|
uIsFinite(depth) &&
|
|
w != cloud.width/2 &&
|
|
depth > 0)
|
|
{
|
|
fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth));
|
|
if(depth16U)
|
|
{
|
|
fx/=1000.0f;
|
|
}
|
|
}
|
|
if(fy == 0.0f &&
|
|
uIsFinite(cloud.at(h*cloud.width + w).y) &&
|
|
uIsFinite(depth) &&
|
|
h != cloud.height/2 &&
|
|
depth > 0)
|
|
{
|
|
fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth));
|
|
if(depth16U)
|
|
{
|
|
fy/=1000.0f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pcl::PointXYZ projectDepthTo3D(
|
|
const cv::Mat & depthImage,
|
|
float x, float y,
|
|
float cx, float cy,
|
|
float fx, float fy,
|
|
bool smoothing,
|
|
float maxZError)
|
|
{
|
|
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
|
|
|
|
pcl::PointXYZ pt;
|
|
float bad_point = std::numeric_limits<float>::quiet_NaN ();
|
|
|
|
float depth = util2d::getDepth(depthImage, x, y, smoothing, maxZError);
|
|
if(depth)
|
|
{
|
|
// Use correct principal point from calibration
|
|
cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
|
|
cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
|
|
|
|
// Fill in XYZ
|
|
pt.x = (x - cx) * depth / fx;
|
|
pt.y = (y - cy) * depth / fy;
|
|
pt.z = depth;
|
|
}
|
|
else
|
|
{
|
|
pt.x = pt.y = pt.z = bad_point;
|
|
}
|
|
return pt;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
|
|
const cv::Mat & imageDepth,
|
|
float cx, float cy,
|
|
float fx, float fy,
|
|
int decimation)
|
|
{
|
|
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
|
|
UASSERT(imageDepth.rows % decimation == 0);
|
|
UASSERT(imageDepth.cols % decimation == 0);
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
if(decimation < 1)
|
|
{
|
|
return cloud;
|
|
}
|
|
|
|
//cloud.header = cameraInfo.header;
|
|
cloud->height = imageDepth.rows/decimation;
|
|
cloud->width = imageDepth.cols/decimation;
|
|
cloud->is_dense = false;
|
|
|
|
cloud->resize(cloud->height * cloud->width);
|
|
|
|
int count = 0 ;
|
|
|
|
for(int h = 0; h < imageDepth.rows; h+=decimation)
|
|
{
|
|
for(int w = 0; w < imageDepth.cols; 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);
|
|
pt.x = ptXYZ.x;
|
|
pt.y = ptXYZ.y;
|
|
pt.z = ptXYZ.z;
|
|
++count;
|
|
}
|
|
}
|
|
|
|
return cloud;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
|
|
const cv::Mat & imageRgb,
|
|
const cv::Mat & imageDepth,
|
|
float cx, float cy,
|
|
float fx, float fy,
|
|
int decimation)
|
|
{
|
|
UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
|
|
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
|
|
UASSERT(imageDepth.rows % decimation == 0);
|
|
UASSERT(imageDepth.cols % decimation == 0);
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
|
|
if(decimation < 1)
|
|
{
|
|
return cloud;
|
|
}
|
|
|
|
bool mono;
|
|
if(imageRgb.channels() == 3) // BGR
|
|
{
|
|
mono = false;
|
|
}
|
|
else if(imageRgb.channels() == 1) // Mono
|
|
{
|
|
mono = true;
|
|
}
|
|
else
|
|
{
|
|
return cloud;
|
|
}
|
|
|
|
//cloud.header = cameraInfo.header;
|
|
cloud->height = imageDepth.rows/decimation;
|
|
cloud->width = imageDepth.cols/decimation;
|
|
cloud->is_dense = false;
|
|
cloud->resize(cloud->height * cloud->width);
|
|
|
|
for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=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));
|
|
if(!mono)
|
|
{
|
|
pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
|
|
pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
|
|
pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
|
|
}
|
|
else
|
|
{
|
|
unsigned char v = imageRgb.at<unsigned char>(h,w);
|
|
pt.b = v;
|
|
pt.g = v;
|
|
pt.r = v;
|
|
}
|
|
|
|
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
|
|
pt.x = ptXYZ.x;
|
|
pt.y = ptXYZ.y;
|
|
pt.z = ptXYZ.z;
|
|
}
|
|
}
|
|
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);
|
|
UASSERT(imageDisparity.rows % decimation == 0);
|
|
UASSERT(imageDisparity.cols % decimation == 0);
|
|
UASSERT(decimation >= 1);
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
|
//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,
|
|
float cx, float cy,
|
|
float fx, float baseline,
|
|
int decimation)
|
|
{
|
|
UASSERT(!imageRgb.empty() && !imageDisparity.empty());
|
|
UASSERT(imageRgb.rows == imageDisparity.rows &&
|
|
imageRgb.cols == imageDisparity.cols &&
|
|
(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
|
|
UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1);
|
|
UASSERT(decimation >= 1);
|
|
UASSERT(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0);
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
|
|
|
|
bool mono;
|
|
if(imageRgb.channels() == 3) // BGR
|
|
{
|
|
mono = false;
|
|
}
|
|
else // Mono
|
|
{
|
|
mono = true;
|
|
}
|
|
|
|
//cloud.header = cameraInfo.header;
|
|
cloud->height = imageRgb.rows/decimation;
|
|
cloud->width = imageRgb.cols/decimation;
|
|
cloud->is_dense = false;
|
|
cloud->resize(cloud->height * cloud->width);
|
|
|
|
for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
|
|
{
|
|
for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
|
|
{
|
|
pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
|
|
if(!mono)
|
|
{
|
|
pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
|
|
pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
|
|
pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
|
|
}
|
|
else
|
|
{
|
|
unsigned char v = imageRgb.at<unsigned char>(h,w);
|
|
pt.b = v;
|
|
pt.g = v;
|
|
pt.r = v;
|
|
}
|
|
|
|
float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
|
|
pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
|
|
pt.x = ptXYZ.x;
|
|
pt.y = ptXYZ.y;
|
|
pt.z = ptXYZ.z;
|
|
}
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
|
|
const cv::Mat & imageLeft,
|
|
const cv::Mat & imageRight,
|
|
float cx, float cy,
|
|
float fx, float baseline,
|
|
int decimation)
|
|
{
|
|
UASSERT(!imageLeft.empty() && !imageRight.empty());
|
|
UASSERT(imageRight.type() == CV_8UC1);
|
|
UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
|
|
UASSERT(imageLeft.rows == imageRight.rows &&
|
|
imageLeft.cols == imageRight.cols);
|
|
UASSERT(decimation >= 1);
|
|
|
|
cv::Mat leftColor = imageLeft;
|
|
cv::Mat rightMono = imageRight;
|
|
|
|
if(leftColor.rows % decimation != 0 ||
|
|
leftColor.cols % decimation != 0)
|
|
{
|
|
leftColor = util2d::decimate(leftColor, decimation);
|
|
rightMono = util2d::decimate(rightMono, decimation);
|
|
fx /= float(decimation);
|
|
cx /= float(decimation);
|
|
cy /= float(decimation);
|
|
decimation = 1;
|
|
}
|
|
|
|
cv::Mat leftMono;
|
|
if(leftColor.channels() == 3)
|
|
{
|
|
cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY);
|
|
}
|
|
else
|
|
{
|
|
leftMono = leftColor;
|
|
}
|
|
|
|
return cloudFromDisparityRGB(
|
|
leftColor,
|
|
util2d::disparityFromStereoImages(leftMono, rightMono),
|
|
cx, cy,
|
|
fx, baseline,
|
|
decimation);
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
|
|
const SensorData & sensorData,
|
|
int decimation,
|
|
float maxDepth,
|
|
float voxelSize,
|
|
int samples)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
|
|
|
|
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
|
|
{
|
|
//depth
|
|
UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
|
|
int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
|
|
cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
|
|
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
|
|
{
|
|
if(sensorData.cameraModels()[i].isValid())
|
|
{
|
|
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(),
|
|
decimation);
|
|
|
|
if(tmp->size())
|
|
{
|
|
bool filtered = false;
|
|
if(tmp->size() && maxDepth)
|
|
{
|
|
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
|
|
filtered = true;
|
|
}
|
|
|
|
if(tmp->size() && voxelSize)
|
|
{
|
|
tmp = util3d::voxelize(tmp, voxelSize);
|
|
filtered = true;
|
|
}
|
|
|
|
if(tmp->size() && samples)
|
|
{
|
|
tmp = util3d::sampling(tmp, samples);
|
|
filtered = true;
|
|
}
|
|
|
|
if(tmp->size() && !filtered)
|
|
{
|
|
tmp = util3d::removeNaNFromPointCloud(tmp);
|
|
}
|
|
|
|
if(tmp->size())
|
|
{
|
|
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
|
|
}
|
|
|
|
*cloud += *tmp;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
UERROR("Camera model %d is invalid", i);
|
|
}
|
|
}
|
|
|
|
if(cloud->size() && voxelSize)
|
|
{
|
|
cloud = util3d::voxelize(cloud, voxelSize);
|
|
}
|
|
}
|
|
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
|
|
{
|
|
//stereo
|
|
UASSERT(sensorData.rightRaw().type() == CV_8UC1);
|
|
|
|
cv::Mat leftMono;
|
|
if(sensorData.imageRaw().channels() == 3)
|
|
{
|
|
cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY);
|
|
}
|
|
else
|
|
{
|
|
leftMono = sensorData.imageRaw();
|
|
}
|
|
cloud = cloudFromDisparity(
|
|
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
|
|
sensorData.stereoCameraModel().left().cx(),
|
|
sensorData.stereoCameraModel().left().cy(),
|
|
sensorData.stereoCameraModel().left().fx(),
|
|
sensorData.stereoCameraModel().baseline(),
|
|
decimation);
|
|
|
|
if(cloud->size())
|
|
{
|
|
bool filtered = false;
|
|
if(cloud->size() && maxDepth)
|
|
{
|
|
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
|
|
filtered = true;
|
|
}
|
|
|
|
if(cloud->size() && voxelSize)
|
|
{
|
|
cloud = util3d::voxelize(cloud, voxelSize);
|
|
filtered = true;
|
|
}
|
|
|
|
if(cloud->size() && !filtered)
|
|
{
|
|
cloud = util3d::removeNaNFromPointCloud(cloud);
|
|
}
|
|
|
|
if(cloud->size())
|
|
{
|
|
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
|
|
}
|
|
}
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
|
|
const SensorData & sensorData,
|
|
int decimation,
|
|
float maxDepth,
|
|
float voxelSize,
|
|
int samples)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
|
|
|
|
if(!sensorData.imageRaw().empty())
|
|
{
|
|
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
|
|
{
|
|
//depth
|
|
UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
|
|
UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
|
|
int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
|
|
cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
|
|
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
|
|
{
|
|
if(sensorData.cameraModels()[i].isValid())
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
|
|
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)),
|
|
sensorData.cameraModels()[i].cx(),
|
|
sensorData.cameraModels()[i].cy(),
|
|
sensorData.cameraModels()[i].fx(),
|
|
sensorData.cameraModels()[i].fy(),
|
|
decimation);
|
|
|
|
if(tmp->size())
|
|
{
|
|
bool filtered = false;
|
|
if(tmp->size() && maxDepth)
|
|
{
|
|
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
|
|
filtered = true;
|
|
}
|
|
|
|
if(tmp->size() && voxelSize)
|
|
{
|
|
tmp = util3d::voxelize(tmp, voxelSize);
|
|
filtered = true;
|
|
}
|
|
|
|
if(tmp->size() && samples)
|
|
{
|
|
tmp = util3d::sampling(tmp, samples);
|
|
filtered = true;
|
|
}
|
|
|
|
if(tmp->size() && !filtered)
|
|
{
|
|
tmp = util3d::removeNaNFromPointCloud(tmp);
|
|
}
|
|
|
|
if(tmp->size())
|
|
{
|
|
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
|
|
}
|
|
|
|
*cloud += *tmp;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
UERROR("Camera model %d is invalid", i);
|
|
}
|
|
}
|
|
|
|
if(cloud->size() && voxelSize)
|
|
{
|
|
cloud = util3d::voxelize(cloud, voxelSize);
|
|
}
|
|
}
|
|
else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
|
|
{
|
|
//stereo
|
|
cloud = cloudFromStereoImages(sensorData.imageRaw(),
|
|
sensorData.rightRaw(),
|
|
sensorData.stereoCameraModel().left().cx(),
|
|
sensorData.stereoCameraModel().left().cy(),
|
|
sensorData.stereoCameraModel().left().fx(),
|
|
sensorData.stereoCameraModel().baseline(),
|
|
decimation);
|
|
|
|
if(cloud->size())
|
|
{
|
|
bool filtered = false;
|
|
if(cloud->size() && maxDepth)
|
|
{
|
|
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
|
|
filtered = true;
|
|
}
|
|
|
|
if(cloud->size() && voxelSize)
|
|
{
|
|
cloud = util3d::voxelize(cloud, voxelSize);
|
|
filtered = true;
|
|
}
|
|
|
|
if(cloud->size() && !filtered)
|
|
{
|
|
cloud = util3d::removeNaNFromPointCloud(cloud);
|
|
}
|
|
|
|
if(cloud->size())
|
|
{
|
|
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
|
|
const cv::Mat & depthImage,
|
|
float fx,
|
|
float fy,
|
|
float cx,
|
|
float cy,
|
|
float maxDepth,
|
|
const Transform & localTransform)
|
|
{
|
|
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
|
|
UASSERT(!localTransform.isNull());
|
|
|
|
pcl::PointCloud<pcl::PointXYZ> scan;
|
|
int middle = depthImage.rows/2;
|
|
if(middle)
|
|
{
|
|
scan.resize(depthImage.cols);
|
|
int oi = 0;
|
|
for(int i=0; i<depthImage.cols; ++i)
|
|
{
|
|
pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
|
|
if(pcl::isFinite(pt) && (maxDepth == 0 || pt.z < maxDepth))
|
|
{
|
|
if(!localTransform.isIdentity())
|
|
{
|
|
pt = util3d::transformPoint(pt, localTransform);
|
|
}
|
|
scan[oi++] = pt;
|
|
}
|
|
}
|
|
scan.resize(oi);
|
|
}
|
|
return scan;
|
|
}
|
|
|
|
|
|
cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
|
|
{
|
|
UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
|
|
cv::Mat depth16U;
|
|
if(!depth32F.empty())
|
|
{
|
|
depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
|
|
for(int i=0; i<depth32F.rows; ++i)
|
|
{
|
|
for(int j=0; j<depth32F.cols; ++j)
|
|
{
|
|
float depth = (depth32F.at<float>(i,j)*1000.0f);
|
|
unsigned short depthMM = 0;
|
|
if(depth <= (float)USHRT_MAX)
|
|
{
|
|
depthMM = (unsigned short)depth;
|
|
}
|
|
depth16U.at<unsigned short>(i, j) = depthMM;
|
|
}
|
|
}
|
|
}
|
|
return depth16U;
|
|
}
|
|
|
|
cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
|
|
{
|
|
UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
|
|
cv::Mat depth32F;
|
|
if(!depth16U.empty())
|
|
{
|
|
depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
|
|
for(int i=0; i<depth16U.rows; ++i)
|
|
{
|
|
for(int j=0; j<depth16U.cols; ++j)
|
|
{
|
|
float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
|
|
depth32F.at<float>(i, j) = depth;
|
|
}
|
|
}
|
|
}
|
|
return depth32F;
|
|
}
|
|
|
|
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
|
|
{
|
|
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
|
|
for(unsigned int i=0; i<cloud.size(); ++i)
|
|
{
|
|
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
|
|
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
|
|
}
|
|
return laserScan;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
|
|
{
|
|
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
|
|
output->resize(laserScan.cols);
|
|
for(int i=0; i<laserScan.cols; ++i)
|
|
{
|
|
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
|
|
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
|
|
}
|
|
return output;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
|
|
const cv::Mat & matrix,
|
|
const Transform & tranform)
|
|
{
|
|
UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
|
|
UASSERT(matrix.rows == 1);
|
|
|
|
Eigen::Affine3f t = tranform.toEigen3f();
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
cloud->resize(matrix.cols);
|
|
if(matrix.channels() == 2)
|
|
{
|
|
for(int i=0; i<matrix.cols; ++i)
|
|
{
|
|
cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
|
|
cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
|
|
cloud->at(i).z = 0.0f;
|
|
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
|
|
}
|
|
}
|
|
else // channels=3
|
|
{
|
|
for(int i=0; i<matrix.cols; ++i)
|
|
{
|
|
cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
|
|
cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
|
|
cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
|
|
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
|
|
}
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
|
|
pcl::PointXYZ projectDisparityTo3D(
|
|
const cv::Point2f & pt,
|
|
float disparity,
|
|
float cx, float cy, float fx, float baseline)
|
|
{
|
|
if(disparity > 0.0f && baseline > 0.0f && fx > 0.0f)
|
|
{
|
|
float W = disparity/baseline;// + (right_.cx() - left_.cx()) / Tx;
|
|
return pcl::PointXYZ((pt.x - cx)/W, (pt.y - cy)/W, fx/W);
|
|
}
|
|
float bad_point = std::numeric_limits<float>::quiet_NaN ();
|
|
return pcl::PointXYZ(bad_point, bad_point, bad_point);
|
|
}
|
|
|
|
pcl::PointXYZ projectDisparityTo3D(
|
|
const cv::Point2f & pt,
|
|
const cv::Mat & disparity,
|
|
float cx, float cy, float fx, float baseline)
|
|
{
|
|
UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
|
|
int u = int(pt.x+0.5f);
|
|
int v = int(pt.y+0.5f);
|
|
float bad_point = std::numeric_limits<float>::quiet_NaN ();
|
|
if(uIsInBounds(u, 0, disparity.cols) &&
|
|
uIsInBounds(v, 0, disparity.rows))
|
|
{
|
|
float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
|
|
return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
|
|
}
|
|
return pcl::PointXYZ(bad_point, bad_point, bad_point);
|
|
}
|
|
|
|
cv::Mat depthFromDisparity(const cv::Mat & disparity,
|
|
float fx, float baseline,
|
|
int type)
|
|
{
|
|
UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
|
|
UASSERT(type == CV_32FC1 || type == CV_16U);
|
|
cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
|
|
for (int i = 0; i < disparity.rows; i++)
|
|
{
|
|
for (int j = 0; j < disparity.cols; j++)
|
|
{
|
|
float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
|
|
if (disparity_value > 0.0f)
|
|
{
|
|
// baseline * focal / disparity
|
|
float d = baseline * fx / disparity_value;
|
|
if(depth.type() == CV_32FC1)
|
|
{
|
|
depth.at<float>(i,j) = d;
|
|
}
|
|
else
|
|
{
|
|
depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return depth;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
|
|
{
|
|
*cloud += *(*iter);
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
|
|
for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
|
|
{
|
|
*cloud+=*(*iter);
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
|
|
{
|
|
//compute total size
|
|
unsigned int totalSize = 0;
|
|
for(unsigned int i=0; i<indices.size(); ++i)
|
|
{
|
|
totalSize += (unsigned int)indices[i]->size();
|
|
}
|
|
pcl::IndicesPtr ind(new std::vector<int>(totalSize));
|
|
unsigned int io = 0;
|
|
for(unsigned int i=0; i<indices.size(); ++i)
|
|
{
|
|
for(unsigned int j=0; j<indices[i]->size(); ++j)
|
|
{
|
|
ind->at(io++) = indices[i]->at(j);
|
|
}
|
|
}
|
|
return ind;
|
|
}
|
|
|
|
pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
|
|
{
|
|
pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
|
|
ind->resize(ind->size()+indicesB->size());
|
|
unsigned int oi = (unsigned int)indicesA->size();
|
|
for(unsigned int i=0; i<indicesB->size(); ++i)
|
|
{
|
|
ind->at(oi++) = indicesB->at(i);
|
|
}
|
|
return ind;
|
|
}
|
|
|
|
void savePCDWords(
|
|
const std::string & fileName,
|
|
const std::multimap<int, pcl::PointXYZ> & words,
|
|
const Transform & transform)
|
|
{
|
|
if(words.size())
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZ> cloud;
|
|
cloud.resize(words.size());
|
|
int i=0;
|
|
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
|
|
{
|
|
cloud[i++] = transformPoint(iter->second, transform);
|
|
}
|
|
pcl::io::savePCDFile(fileName, cloud);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|