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

FlannIndex: changed how detection of index rebuilt is done (to handle index that always rebuild on addPoints()). CameraImages: Fixed null scan local transform

Browse Source
This commit is contained in:
matlabbe
2016-08-23 11:21:56 -04:00
parent c8cb961108
commit 48f54cf2f2
2 changed files with 26 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/src/CameraRGB.cpp
View File

@@ -93,6 +93,7 @@ CameraImages::CameraImages(const std::string & path,
_dir(0), _dir(0),
_countScan(0), _countScan(0),
_scanDir(0), _scanDir(0),
_scanLocalTransform(Transform::getIdentity()),
_scanMaxPts(0), _scanMaxPts(0),
_scanDownsampleStep(1), _scanDownsampleStep(1),
_scanVoxelSize(0.0f), _scanVoxelSize(0.0f),

49
corelib/src/FlannIndex.cpp
View File

@@ -307,22 +307,21 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
} }
UASSERT(features.type() == featuresType_); UASSERT(features.type() == featuresType_);
UASSERT(features.cols == featuresDim_); UASSERT(features.cols == featuresDim_);
bool indexRebuilt = false;
size_t removedPts = 0;
if(featuresType_ == CV_8UC1) if(featuresType_ == CV_8UC1)
{ {
rtflann::Matrix<unsigned char> points(features.data, features.rows, features.cols); rtflann::Matrix<unsigned char> points(features.data, features.rows, features.cols);
rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_; rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
removedPts = index->removedCount();
index->addPoints(points, 0); index->addPoints(points, 0);
// Rebuild index if it doubles in size // Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
{ {
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
{
addedDescriptors_.erase(*iter);
}
removedIndexes_.clear();
index->buildIndex(); index->buildIndex();
} }
// if no more removed points, the index has been rebuilt
indexRebuilt = index->removedCount() == 0 && removedPts>0;
} }
else else
{ {
@@ -330,53 +329,55 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
if(useDistanceL1_) if(useDistanceL1_)
{ {
rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_; rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
removedPts = index->removedCount();
index->addPoints(points, 0); index->addPoints(points, 0);
// Rebuild index if it doubles in size // Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
{ {
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
{
addedDescriptors_.erase(*iter);
}
removedIndexes_.clear();
index->buildIndex(); index->buildIndex();
} }
// if no more removed points, the index has been rebuilt
indexRebuilt = index->removedCount() == 0 && removedPts>0;
} }
else if(featuresDim_ <= 3) else if(featuresDim_ <= 3)
{ {
rtflann::Index<rtflann::L2_Simple<float> > * index = (rtflann::Index<rtflann::L2_Simple<float> >*)index_; rtflann::Index<rtflann::L2_Simple<float> > * index = (rtflann::Index<rtflann::L2_Simple<float> >*)index_;
removedPts = index->removedCount();
index->addPoints(points, 0); index->addPoints(points, 0);
// Rebuild index if it doubles in size // Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
{ {
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
{
addedDescriptors_.erase(*iter);
}
removedIndexes_.clear();
index->buildIndex(); index->buildIndex();
} }
// if no more removed points, the index has been rebuilt
indexRebuilt = index->removedCount() == 0 && removedPts>0;
} }
else else
{ {
rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_; rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
removedPts = index->removedCount();
index->addPoints(points, 0); index->addPoints(points, 0);
// Rebuild index if it doubles in size // Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
{ {
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
{
addedDescriptors_.erase(*iter);
}
removedIndexes_.clear();
index->buildIndex(); index->buildIndex();
} }
// if no more removed points, the index has been rebuilt
indexRebuilt = index->removedCount() == 0 && removedPts>0;
} }
} }
if(indexRebuilt)
{
UASSERT(removedPts == removedIndexes_.size());
// clean not used features
for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
{
addedDescriptors_.erase(*iter);
}
removedIndexes_.clear();
}
addedDescriptors_.insert(std::make_pair(nextIndex_, features)); addedDescriptors_.insert(std::make_pair(nextIndex_, features));
int r = nextIndex_; int r = nextIndex_;