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

Added floor filtering options for features extraction (#1408)

Browse Source 
* Added floor filtering options for features extraction

* cleanup
This commit is contained in:
matlabbe
2024-12-10 12:36:21 -08:00
committed by GitHub
parent a613998652
commit 38cacb7978
11 changed files with 579 additions and 317 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
corelib/src/Features2d.cpp
View File

@@ -1315,6 +1315,11 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
cv::Mat imgRoi(image, roi);
cv::Mat maskRoi;
if(!mask.empty())
{
maskRoi = cv::Mat(mask, roi);
}
#ifdef RTABMAP_CUDASIFT
if(gpu_)
{
@@ -1383,6 +1388,12 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
//std::cout << cv::Mat(1, 128*4, CV_8UC1, desc) << std::endl;
continue;
}
// Ignore keypoints not in the mask
if(!maskRoi.empty() && maskRoi.at<unsigned char>(cudaSiftData_->h_data[i].ypos, cudaSiftData_->h_data[i].xpos) == 0)
{
continue;
}
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(cudaSiftData_->h_data[i].sharpness, i));
}
@@ -1412,12 +1423,6 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
else
#endif
{
cv::Mat maskRoi;
if(!mask.empty())
{
maskRoi = cv::Mat(mask, roi);
}
#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
#ifdef RTABMAP_NONFREE
sift_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints

23
corelib/src/Memory.cpp
View File

@@ -91,6 +91,7 @@ Memory::Memory(const ParametersMap & parameters) :
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
_depthAsMask(Parameters::defaultMemDepthAsMask()),
_maskFloorThreshold(Parameters::defaultMemDepthMaskFloorThr()),
_stereoFromMotion(Parameters::defaultMemStereoFromMotion()),
_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
@@ -576,6 +577,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
Parameters::parse(params, Parameters::kMemDepthMaskFloorThr(), _maskFloorThreshold);
Parameters::parse(params, Parameters::kMemStereoFromMotion(), _stereoFromMotion);
Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
@@ -4884,7 +4886,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
imageMono.cols % decimatedData.depthRaw().cols == 0 &&
imageMono.rows/decimatedData.depthRaw().rows == imageMono.cols/decimatedData.depthRaw().cols)
{
depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
depthMask = decimatedData.depthRaw();
if(_maskFloorThreshold != 0.0f)
{
UASSERT(!decimatedData.cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
}
UDEBUG("Masking floor done.");
}
depthMask = util2d::interpolate(depthMask, imageMono.rows/depthMask.rows, 0.1f);
}
else
{

48
corelib/src/RegistrationVis.cpp
View File

@@ -94,6 +94,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
_depthAsMask(Parameters::defaultVisDepthAsMask()),
_maskFloorThreshold(Parameters::defaultVisDepthMaskFloorThr()),
_minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
_detectorFrom(0),
@@ -155,6 +156,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
Parameters::parse(parameters, Parameters::kVisDepthMaskFloorThr(), _maskFloorThreshold);
Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr);
Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance);
uInsert(_bundleParameters, parameters);
@@ -423,13 +425,32 @@ Transform RegistrationVis::computeTransformationImpl(
imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
{
depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
depthMask = fromSignature.sensorData().depthRaw();
if(_maskFloorThreshold != 0.0f)
{
UASSERT(!fromSignature.sensorData().cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
}
UDEBUG("Masking floor done.");
}
depthMask = util2d::interpolate(depthMask, imageFrom.rows/depthMask.rows, 0.1f);
}
else
{
UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
Parameters::kVisDepthAsMask().c_str(),
fromSignature.sensorData().imageRaw().rows, fromSignature.sensorData().imageRaw().cols,
imageFrom.rows, imageFrom.cols,
fromSignature.sensorData().depthRaw().rows, fromSignature.sensorData().depthRaw().cols);
}
}
@@ -770,13 +791,32 @@ Transform RegistrationVis::computeTransformationImpl(
imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
{
depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
depthMask = toSignature.sensorData().depthRaw();
if(_maskFloorThreshold != 0.0f)
{
UASSERT(!toSignature.sensorData().cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
}
UDEBUG("Masking floor done.");
}
depthMask = util2d::interpolate(depthMask, imageTo.rows/depthMask.rows, 0.1f);
}
else
{
UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
Parameters::kVisDepthAsMask().c_str(),
toSignature.sensorData().imageRaw().rows, toSignature.sensorData().imageRaw().cols,
imageTo.rows, imageTo.cols,
toSignature.sensorData().depthRaw().rows, toSignature.sensorData().depthRaw().cols);
}
}

109
corelib/src/util3d.cpp
View File

@@ -3007,6 +3007,115 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
}
}
cv::Mat filterFloor(const cv::Mat & depth, const std::vector<CameraModel> & cameraModels, float threshold, cv::Mat * depthBelow)
{
cv::Mat output = depth.clone();
if(depth.empty())
{
return output;
}
if(depthBelow)
{
*depthBelow = cv::Mat::zeros(output.size(), output.type());
}
UASSERT(!cameraModels.empty());
UASSERT(cameraModels[0].isValidForReprojection());
// Support camera model with different resolution than depth image
float rgbToDepthFactorX = float(cameraModels[0].imageWidth()) / float(output.cols/cameraModels.size());
float rgbToDepthFactorY = float(cameraModels[0].imageHeight()) / float(output.rows);
int depthWidth = output.cols/cameraModels.size();
UASSERT(depthWidth*(int)cameraModels.size() == output.cols);
// for each camera
for(size_t i=0; i<cameraModels.size(); ++i)
{
const CameraModel & cam = cameraModels[i];
UASSERT(cam.isValidForReprojection());
const Transform & localTransform = cam.localTransform();
UASSERT(!localTransform.isNull());
if(i>0)
{
// Make sure all models are the same resolution
UASSERT(cam.imageWidth() == cameraModels[i-1].imageWidth());
UASSERT(cam.imageHeight() == cameraModels[i-1].imageHeight());
}
float depthFx = cam.fx() / rgbToDepthFactorX;
float depthFy = cam.fy() / rgbToDepthFactorY;
float depthCx = cam.cx() / rgbToDepthFactorX;
float depthCy = cam.cy() / rgbToDepthFactorY;
cv::Mat subImage = output.colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));
cv::Mat subImageBelow;
if(depthBelow)
subImageBelow = depthBelow->colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));
for(int y=0; y<subImage.rows; ++y)
{
if(subImage.type() == CV_16UC1)
{
unsigned short * ptr = (unsigned short *)subImage.row(y).ptr();
unsigned short * ptrBelow = 0;
if(depthBelow)
{
ptrBelow = (unsigned short *)subImageBelow.row(y).ptr();
}
for(int x=0; x<subImage.cols; ++x)
{
if(ptr[x] > 0)
{
float d = float(ptr[x])/1000.0f;
cv::Point3f pt;
pt.x = (x - depthCx) * d / depthFx;
pt.y = (y - depthCy) * d / depthFy;
pt.z = d;
pt = util3d::transformPoint(pt, localTransform);
if(pt.z < threshold)
{
if(ptrBelow)
{
ptrBelow[x] = ptr[x];
}
ptr[x] = 0;
}
}
}
}
else // CV_32FC1
{
float * ptr = (float *)subImage.row(y).ptr();
float * ptrBelow = 0;
if(depthBelow)
{
ptrBelow = (float *)subImageBelow.row(y).ptr();
}
for(int x=0; x<subImage.cols; ++x)
{
if(ptr[x] > 0.0f)
{
float & d = ptr[x];
cv::Point3f pt;
pt.x = (x - depthCx) * d / depthFx;
pt.y = (y - depthCy) * d / depthFy;
pt.z = d;
pt = util3d::transformPoint(pt, localTransform);
if(pt.z < threshold)
{
if(ptrBelow)
{
ptrBelow[x] = ptr[x];
}
d = 0;
}
}
}
}
}
}
return output;
}
class ProjectionInfo {
public:
ProjectionInfo():