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

Decimation parameters for feature detection cannot be negative anymore (always done based to RGB image and depth image is decimated only if it is bigger than decimated rgb image). MainWindow: fixed light blue screen-only on full rehearsal merge.

Browse Source
This commit is contained in:
matlabbe
2021-06-25 12:54:52 -04:00
parent be24203840
commit eabf3e3d57
9 changed files with 112 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
corelib/src/Memory.cpp
View File

@@ -585,11 +585,11 @@ 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());
if(_imagePreDecimation <= 0)
if(_imagePreDecimation == 0)
{
_imagePreDecimation = 1;
}
if(_imagePostDecimation <= 0)
if(_imagePostDecimation == 0)
{
_imagePostDecimation = 1;
}
@@ -4291,6 +4291,24 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(_imagePreDecimation > 1)
{
preDecimation = _imagePreDecimation;
int decimationDepth = _imagePreDecimation;
if( !data.cameraModels().empty() &&
data.cameraModels()[0].imageHeight()>0 &&
data.cameraModels()[0].imageWidth()>0)
{
// decimate from RGB image size
int targetSize = data.cameraModels()[0].imageHeight() / _imagePreDecimation;
if(targetSize >= data.depthRaw().rows)
{
decimationDepth = 1;
}
else
{
decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
}
}
UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth);
std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
for(unsigned int i=0; i<cameraModels.size(); ++i)
{
@@ -4298,21 +4316,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
if(!cameraModels.empty())
{
if(decimatedData.depthRaw().rows == decimatedData.imageRaw().rows &&
decimatedData.depthRaw().cols == decimatedData.imageRaw().cols)
{
decimatedData.setRGBDImage(
util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation),
cameraModels);
}
else
{
decimatedData.setRGBDImage(
util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
decimatedData.depthOrRightRaw(),
cameraModels);
}
decimatedData.setRGBDImage(
util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
cameraModels);
}
else
{
@@ -4348,13 +4355,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
depthMask = util2d::interpolate(decimatedData.depthRaw(), imageMono.rows/decimatedData.depthRaw().rows, 0.1f);
}
else if(_imagePreDecimation > 1)
else
{
UWARN("%s=%d is not compatible between RGB and depth images, the depth mask cannot be used! (decimated RGB=%dx%d, depth=%dx%d)",
Parameters::kMemImagePreDecimation().c_str(),
_imagePreDecimation,
UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection (%s=%d).",
Parameters::kMemDepthAsMask().c_str(),
imageMono.cols, imageMono.rows,
decimatedData.depthRaw().cols, decimatedData.depthRaw().rows);
decimatedData.depthRaw().cols, decimatedData.depthRaw().rows,
Parameters::kMemImagePreDecimation().c_str(), _imagePreDecimation);
}
}
@@ -4368,14 +4375,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// A: Adjust keypoint position so that descriptors are correctly extracted
// B: In case we provided corresponding 3D features
if(preDecimation > 1 || useProvided3dPoints)
if(_imagePreDecimation > 1 || useProvided3dPoints)
{
float decimationRatio = 1.0f / float(preDecimation);
double log2value = log(double(preDecimation))/log(2.0);
float decimationRatio = 1.0f / float(_imagePreDecimation);
double log2value = log(double(_imagePreDecimation))/log(2.0);
for(unsigned int i=0; i < keypoints.size(); ++i)
{
cv::KeyPoint & kpt = keypoints[i];
if(preDecimation > 1)
if(_imagePreDecimation > 1)
{
kpt.pt.x *= decimationRatio;
kpt.pt.y *= decimationRatio;
@@ -4863,11 +4870,25 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
else
{
if(!data.rightRaw().empty() ||
(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
int decimationDepth = _imagePreDecimation;
if( !data.cameraModels().empty() &&
data.cameraModels()[0].imageHeight()>0 &&
data.cameraModels()[0].imageWidth()>0)
{
depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
// decimate from RGB image size
int targetSize = data.cameraModels()[0].imageHeight() / _imagePreDecimation;
if(targetSize >= data.depthRaw().rows)
{
decimationDepth = 1;
}
else
{
decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
}
}
UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePostDecimation, data.depthOrRightRaw().rows, decimationDepth);
depthOrRightImage = util2d::decimate(depthOrRightImage, decimationDepth);
image = util2d::decimate(image, _imagePostDecimation);
for(unsigned int i=0; i<cameraModels.size(); ++i)
{

40
corelib/src/Odometry.cpp
View File

@@ -559,19 +559,17 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UTimer time;
Transform t;
int decimationRgb = abs(_imageDecimation);
if((_imageDecimation > 1 || _imageDecimation < -1) && !data.imageRaw().empty())
if(_imageDecimation > 1 && !data.imageRaw().empty())
{
// Decimation of images with calibrations
SensorData decimatedData = data;
int decimationDepth = abs(_imageDecimation);
if(_imageDecimation<0 &&
!data.cameraModels().empty() &&
int decimationDepth = _imageDecimation;
if( !data.cameraModels().empty() &&
data.cameraModels()[0].imageHeight()>0 &&
data.cameraModels()[0].imageWidth()>0)
{
// decimate from RGB image size
int targetSize = data.cameraModels()[0].imageHeight() / decimationRgb;
int targetSize = data.cameraModels()[0].imageHeight() / _imageDecimation;
if(targetSize >= data.depthRaw().rows)
{
decimationDepth = 1;
@@ -581,14 +579,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
}
}
UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, decimationRgb, data.depthOrRightRaw().rows, decimationDepth);
UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imageDecimation, data.depthOrRightRaw().rows, decimationDepth);
cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), decimationRgb);
cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), _imageDecimation);
cv::Mat depthRight = util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth);
std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
for(unsigned int i=0; i<cameraModels.size(); ++i)
{
cameraModels[i] = cameraModels[i].scaled(1.0/double(decimationRgb));
cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
}
if(!cameraModels.empty())
{
@@ -599,7 +597,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(decimationRgb));
stereoModel.scale(1.0/double(_imageDecimation));
}
decimatedData.setStereoImage(rgbLeft, depthRight, stereoModel);
}
@@ -610,12 +608,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
// transform back the keypoints in the original image
std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
double log2value = log(double(decimationRgb))/log(2.0);
double log2value = log(double(_imageDecimation))/log(2.0);
for(unsigned int i=0; i<kpts.size(); ++i)
{
kpts[i].pt.x *= decimationRgb;
kpts[i].pt.y *= decimationRgb;
kpts[i].size *= decimationRgb;
kpts[i].pt.x *= _imageDecimation;
kpts[i].pt.y *= _imageDecimation;
kpts[i].size *= _imageDecimation;
kpts[i].octave += log2value;
}
data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
@@ -626,19 +624,19 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty());
for(unsigned int i=0; i<info->newCorners.size(); ++i)
{
info->refCorners[i].x *= decimationRgb;
info->refCorners[i].y *= decimationRgb;
info->refCorners[i].x *= _imageDecimation;
info->refCorners[i].y *= _imageDecimation;
if(!info->refCorners.empty())
{
info->newCorners[i].x *= decimationRgb;
info->newCorners[i].y *= decimationRgb;
info->newCorners[i].x *= _imageDecimation;
info->newCorners[i].y *= _imageDecimation;
}
}
for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
{
iter->second.pt.x *= decimationRgb;
iter->second.pt.y *= decimationRgb;
iter->second.size *= decimationRgb;
iter->second.pt.x *= _imageDecimation;
iter->second.pt.y *= _imageDecimation;
iter->second.size *= _imageDecimation;
iter->second.octave += log2value;
}
}

14
corelib/src/RegistrationVis.cpp
View File

@@ -394,6 +394,13 @@ Transform RegistrationVis::computeTransformationImpl(
{
depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().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,
fromSignature.sensorData().depthRaw().rows, fromSignature.sensorData().depthRaw().cols);
}
}
kptsFrom = _detectorFrom->generateKeypoints(
@@ -613,6 +620,13 @@ Transform RegistrationVis::computeTransformationImpl(
{
depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().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,
toSignature.sensorData().depthRaw().rows, toSignature.sensorData().depthRaw().cols);
}
}
kptsTo = _detectorTo->generateKeypoints(