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

Merge branch 'master' of https://github.com/introlab/rtabmap into scan_map

Browse Source
This commit is contained in:
matlabbe
2021-06-24 19:53:23 -04:00
parent 11e9837b82 f5ab422c42
commit 656618ccb7
2 changed files with 47 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
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;
}

62
corelib/src/Odometry.cpp
View File

@@ -559,16 +559,36 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UTimer time;
Transform t;
if(_imageDecimation > 1 && !data.imageRaw().empty())
int decimationRgb = abs(_imageDecimation);
if((_imageDecimation > 1 || _imageDecimation < -1) && !data.imageRaw().empty())
{
// Decimation of images with calibrations
SensorData decimatedData = data;
cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), _imageDecimation);
cv::Mat depthRight = util2d::decimate(decimatedData.depthOrRightRaw(), _imageDecimation);
int decimationDepth = abs(_imageDecimation);
if(_imageDecimation<0 &&
!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;
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, decimationRgb, data.depthOrRightRaw().rows, decimationDepth);
cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), decimationRgb);
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(_imageDecimation));
cameraModels[i] = cameraModels[i].scaled(1.0/double(decimationRgb));
}
if(!cameraModels.empty())
{
@@ -579,7 +599,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(_imageDecimation));
stereoModel.scale(1.0/double(decimationRgb));
}
decimatedData.setStereoImage(rgbLeft, depthRight, stereoModel);
}
@@ -590,12 +610,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(_imageDecimation))/log(2.0);
double log2value = log(double(decimationRgb))/log(2.0);
for(unsigned int i=0; i<kpts.size(); ++i)
{
kpts[i].pt.x *= _imageDecimation;
kpts[i].pt.y *= _imageDecimation;
kpts[i].size *= _imageDecimation;
kpts[i].pt.x *= decimationRgb;
kpts[i].pt.y *= decimationRgb;
kpts[i].size *= decimationRgb;
kpts[i].octave += log2value;
}
data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
@@ -603,19 +623,22 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
if(info)
{
UASSERT(info->newCorners.size() == info->refCorners.size());
UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty());
for(unsigned int i=0; i<info->newCorners.size(); ++i)
{
info->refCorners[i].x *= _imageDecimation;
info->refCorners[i].y *= _imageDecimation;
info->newCorners[i].x *= _imageDecimation;
info->newCorners[i].y *= _imageDecimation;
info->refCorners[i].x *= decimationRgb;
info->refCorners[i].y *= decimationRgb;
if(!info->refCorners.empty())
{
info->newCorners[i].x *= decimationRgb;
info->newCorners[i].y *= decimationRgb;
}
}
for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
{
iter->second.pt.x *= _imageDecimation;
iter->second.pt.y *= _imageDecimation;
iter->second.size *= _imageDecimation;
iter->second.pt.x *= decimationRgb;
iter->second.pt.y *= decimationRgb;
iter->second.size *= decimationRgb;
iter->second.octave += log2value;
}
}
@@ -829,6 +852,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
if(info)
{
*info = OdometryInfo();
}
return this->computeTransform(data, Transform(), info);
}
}