i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-10 11:59:50 +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
corelib/src/Memory.cpp
+2 -2
View File
@@ -585,11 +585,11 @@ void Memory::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str()); 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(_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()); UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
if(_imagePreDecimation == 0) if(_imagePreDecimation <= 0)
{ {
_imagePreDecimation = 1; _imagePreDecimation = 1;
} }
if(_imagePostDecimation == 0) if(_imagePostDecimation <= 0)
{ {
_imagePostDecimation = 1; _imagePostDecimation = 1;
} }
corelib/src/Odometry.cpp
+45 -17
View File
@@ -559,16 +559,36 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UTimer time; UTimer time;
Transform t; 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 // Decimation of images with calibrations
SensorData decimatedData = data; SensorData decimatedData = data;
cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), _imageDecimation); int decimationDepth = abs(_imageDecimation);
cv::Mat depthRight = util2d::decimate(decimatedData.depthOrRightRaw(), _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(); std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
for(unsigned int i=0; i<cameraModels.size(); ++i) 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()) if(!cameraModels.empty())
{ {
@@ -579,7 +599,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
StereoCameraModel stereoModel = decimatedData.stereoCameraModel(); StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
if(stereoModel.isValidForProjection()) if(stereoModel.isValidForProjection())
{ {
stereoModel.scale(1.0/double(_imageDecimation)); stereoModel.scale(1.0/double(decimationRgb));
} }
decimatedData.setStereoImage(rgbLeft, depthRight, stereoModel); 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 // transform back the keypoints in the original image
std::vector<cv::KeyPoint> kpts = decimatedData.keypoints(); 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) for(unsigned int i=0; i<kpts.size(); ++i)
{ {
kpts[i].pt.x *= _imageDecimation; kpts[i].pt.x *= decimationRgb;
kpts[i].pt.y *= _imageDecimation; kpts[i].pt.y *= decimationRgb;
kpts[i].size *= _imageDecimation; kpts[i].size *= decimationRgb;
kpts[i].octave += log2value; kpts[i].octave += log2value;
} }
data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors()); data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
@@ -603,19 +623,22 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
if(info) 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) for(unsigned int i=0; i<info->newCorners.size(); ++i)
{ {
info->refCorners[i].x *= _imageDecimation; info->refCorners[i].x *= decimationRgb;
info->refCorners[i].y *= _imageDecimation; info->refCorners[i].y *= decimationRgb;
info->newCorners[i].x *= _imageDecimation; if(!info->refCorners.empty())
info->newCorners[i].y *= _imageDecimation; {
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) for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
{ {
iter->second.pt.x *= _imageDecimation; iter->second.pt.x *= decimationRgb;
iter->second.pt.y *= _imageDecimation; iter->second.pt.y *= decimationRgb;
iter->second.size *= _imageDecimation; iter->second.size *= decimationRgb;
iter->second.octave += log2value; iter->second.octave += log2value;
} }
} }
@@ -829,6 +852,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{ {
UWARN("Odometry automatically reset to latest pose!"); UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose); this->reset(_pose);
if(info)
{
*info = OdometryInfo();
}
return this->computeTransform(data, Transform(), info);
} }
} }