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

4
corelib/include/rtabmap/core/Memory.h
View File

@@ -304,8 +304,8 @@ private:
bool _mapLabelsAdded;
bool _depthAsMask;
bool _stereoFromMotion;
int _imagePreDecimation;
int _imagePostDecimation;
unsigned int _imagePreDecimation;
unsigned int _imagePostDecimation;
bool _compressionParallelized;
float _laserScanDownsampleStepSize;
float _laserScanVoxelSize;

2
corelib/include/rtabmap/core/OccupancyGrid.h
View File

@@ -108,7 +108,7 @@ public:
private:
ParametersMap parameters_;
int cloudDecimation_;
unsigned int cloudDecimation_;
float cloudMaxDepth_;
float cloudMinDepth_;
std::vector<float> roiRatios_;

2
corelib/include/rtabmap/core/Odometry.h
View File

@@ -104,7 +104,7 @@ private:
bool _fillInfoData;
float _kalmanProcessNoise;
float _kalmanMeasurementNoise;
int _imageDecimation;
unsigned int _imageDecimation;
bool _alignWithGround;
bool _publishRAMUsage;
bool _imagesAlreadyRectified;

8
corelib/include/rtabmap/core/Parameters.h
View File

@@ -220,8 +220,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
RTABMAP_PARAM(Mem, DepthAsMask, bool, true, "Use depth image as mask when extracting features for vocabulary.");
RTABMAP_PARAM(Mem, StereoFromMotion, bool, false, uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
RTABMAP_PARAM(Mem, ImagePreDecimation, int, 1, "Image decimation (>=1) before features extraction.");
RTABMAP_PARAM(Mem, ImagePostDecimation, int, 1, "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
RTABMAP_PARAM(Mem, ImagePreDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
RTABMAP_PARAM(Mem, ImagePostDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
RTABMAP_PARAM(Mem, CompressionParallelized, bool, true, "Compression of sensor data is multi-threaded.");
RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()));
@@ -449,7 +449,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.3, "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, VisKeyFrameThr, int, 150, "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ImageDecimation, int, 1, "Decimation of the images before registration. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
RTABMAP_PARAM(Odom, ImageDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization.");
// Odometry Frame-to-Map
@@ -721,7 +721,7 @@ class RTABMAP_EXP Parameters
// Occupancy Grid
RTABMAP_PARAM(Grid, FromDepth, bool, true, "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
RTABMAP_PARAM(Grid, DepthDecimation, int, 4, uFormat("[%s=true] Decimation of the depth image before creating cloud. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, DepthDecimation, unsigned int, 4, uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, RangeMin, float, 0.0, "Minimum range from sensor.");
RTABMAP_PARAM(Grid, RangeMax, float, 5.0, "Maximum range from sensor. 0=inf.");
RTABMAP_PARAM_STR(Grid, DepthRoiRatios, "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));

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(