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

Image Decimation can now be negative, meaning that decimation is done from RGB size instead of depth. If Depth size is smaller than RGB, it may be upsampled depending on the decimation value. Export Dialog: Fixed MLS not done even when checked.

Browse Source
This commit is contained in:
matlabbe
2016-10-30 16:03:23 -04:00
parent 20a5d80647
commit 768c864766
12 changed files with 230 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Memory.cpp
View File

@@ -419,8 +419,14 @@ void Memory::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
UASSERT(_imagePreDecimation >= 1);
UASSERT(_imagePostDecimation >= 1);
if(_imagePreDecimation == 0)
{
_imagePreDecimation = 1;
}
if(_imagePostDecimation == 0)
{
_imagePostDecimation = 1;
}
UASSERT(_rehearsalMaxDistance >= 0.0f);
UASSERT(_rehearsalMaxAngle >= 0.0f);

4
corelib/src/OccupancyGrid.cpp
View File

@@ -74,6 +74,10 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromCloud_);
Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
if(cloudDecimation_ == 0)
{
cloudDecimation_ = 1;
}
Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);

5
corelib/src/Odometry.cpp
View File

@@ -108,7 +108,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
UASSERT(_imageDecimation>=1);
if(_imageDecimation == 0)
{
_imageDecimation = 1;
}
if(_filteringStrategy == 2)
{

2
corelib/src/util2d.cpp
View File

@@ -1224,7 +1224,7 @@ cv::Mat decimate(const cv::Mat & image, int decimation)
cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
{
UASSERT(factor >= 1);
UASSERT_MSG(factor >= 1, uFormat("factor=%d", factor).c_str());
cv::Mat out;
if(!image.empty())
{

115
corelib/src/util3d.cpp
View File

@@ -254,38 +254,72 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepth,
const cv::Mat & imageDepthIn,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices)
{
if(decimation == 0)
{
decimation = 1;
}
float rgbToDepthFactorX = 1.0f;
float rgbToDepthFactorY = 1.0f;
UASSERT(model.isValidForProjection());
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
cv::Mat imageDepth = imageDepthIn;
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());
UASSERT(model.imageHeight() % imageDepthIn.rows == 0 && model.imageWidth() % imageDepthIn.cols == 0);
if(decimation < 0)
{
UDEBUG("Decimation from model (%d)", decimation);
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());
// decimate from RGB image size, upsample depth if needed
decimation = -1*decimation;
int targetSize = model.imageHeight() / decimation;
if(targetSize > imageDepthIn.rows)
{
UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
decimation = 1;
}
else if(targetSize == imageDepthIn.rows)
{
decimation = 1;
}
else
{
UASSERT(imageDepthIn.rows % targetSize == 0);
decimation = imageDepthIn.rows / targetSize;
}
}
else
{
UASSERT_MSG(imageDepthIn.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepthIn.rows, decimation).c_str());
UASSERT_MSG(imageDepthIn.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageDepthIn.cols, decimation).c_str());
}
rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols));
rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows));
}
else
{
decimation = abs(decimation);
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)
{
return cloud;
}
//cloud.header = cameraInfo.header;
cloud->height = imageDepth.rows/decimation;
@@ -358,29 +392,62 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
const cv::Mat & imageDepthIn,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices)
{
if(decimation == 0)
{
decimation = 1;
}
UDEBUG("");
UASSERT(model.isValidForProjection());
UASSERT_MSG((model.imageHeight() == 0 && model.imageWidth() == 0) ||
(model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols),
uFormat("model=%dx%d rgb=%dx%d", model.imageWidth(), model.imageHeight(), imageRgb.cols, imageRgb.rows).c_str());
UASSERT_MSG(imageRgb.rows % imageDepth.rows == 0 && imageRgb.cols % imageDepth.cols == 0,
uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepth.cols, imageDepth.rows).c_str());
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());
UASSERT_MSG(imageRgb.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageRgb.cols, decimation).c_str());
UASSERT_MSG(imageRgb.rows % imageDepthIn.rows == 0 && imageRgb.cols % imageDepthIn.cols == 0,
uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepthIn.cols, imageDepthIn.rows).c_str());
UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
if(decimation < 0)
{
UASSERT_MSG(imageRgb.rows % decimation == 0, uFormat("imageRgb.rows=%d decimation=%d", imageRgb.rows, decimation).c_str());
UASSERT_MSG(imageRgb.cols % decimation == 0, uFormat("imageRgb.cols=%d decimation=%d", imageRgb.cols, decimation).c_str());
}
else
{
UASSERT_MSG(imageDepthIn.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepthIn.rows, decimation).c_str());
UASSERT_MSG(imageDepthIn.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageDepthIn.cols, decimation).c_str());
}
cv::Mat imageDepth = imageDepthIn;
if(decimation < 0)
{
UDEBUG("Decimation from RGB image (%d)", decimation);
// decimate from RGB image size, upsample depth if needed
decimation = -1*decimation;
int targetSize = imageRgb.rows / decimation;
if(targetSize > imageDepthIn.rows)
{
UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
decimation = 1;
}
else if(targetSize == imageDepthIn.rows)
{
decimation = 1;
}
else
{
UASSERT(imageDepthIn.rows % targetSize == 0);
decimation = imageDepthIn.rows / targetSize;
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(decimation < 1)
{
return cloud;
}
bool mono;
if(imageRgb.channels() == 3) // BGR
@@ -714,6 +781,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
if(decimation == 0)
{
decimation = 1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
@@ -857,6 +929,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
{
if(decimation == 0)
{
decimation = 1;
}
UASSERT(!sensorData.imageRaw().empty());
UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection()));