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

merged master to scan_map branch

Browse Source
This commit is contained in:
matlabbe
2021-06-25 16:38:03 -04:00
parent 48ebf969bd eabf3e3d57
commit 350f995ee9
10 changed files with 140 additions and 70 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;
}
@@ -4304,6 +4304,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)
{
@@ -4311,21 +4329,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
{
@@ -4361,13 +4368,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);
}
}
@@ -4381,14 +4388,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;
@@ -4876,11 +4883,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(