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

Memory: Fixed features with no id (-1) ignored for triangulation from motion. Dictionary: matching with negative word ids is now allowed

Browse Source
This commit is contained in:
matlabbe
2018-07-26 20:41:54 -04:00
parent cfdee23d33
commit b63590bf1d
5 changed files with 37 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Statistics.h
View File

@@ -106,6 +106,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Odometry_variance_lin,); RTABMAP_STATS(Memory, Odometry_variance_lin,);
RTABMAP_STATS(Memory, Distance_travelled, m); RTABMAP_STATS(Memory, Distance_travelled, m);
RTABMAP_STATS(Memory, RAM_usage, MB); RTABMAP_STATS(Memory, RAM_usage, MB);
RTABMAP_STATS(Memory, Triangulated_points, );
RTABMAP_STATS(Timing, Memory_update, ms); RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Neighbor_link_refining, ms); RTABMAP_STATS(Timing, Neighbor_link_refining, ms);

33
corelib/src/Memory.cpp
View File

@@ -3990,6 +3990,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
allWordIds[quantizedToRawIndices[i]] = *iter; allWordIds[quantizedToRawIndices[i]] = *iter;
++i; ++i;
} }
for(i=0; i<(int)allWordIds.size(); ++i)
{
if(allWordIds[i] < 0)
{
allWordIds[i] = -1*(i+1);
}
}
wordIds = uVectorToList(allWordIds); wordIds = uVectorToList(allWordIds);
} }
@@ -4049,22 +4056,25 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
Signature * previousS = _signatures.rbegin()->second; Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull()) if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
{ {
UDEBUG("Previous pose(%d) = %s", previousS->id(), previousS->getPose().prettyPrint().c_str());
UDEBUG("Current pose(%d) = %s", id, pose.prettyPrint().c_str());
Transform cameraTransform = pose.inverse() * previousS->getPose(); Transform cameraTransform = pose.inverse() * previousS->getPose();
Signature cpPrevious(-2); Signature cpPrevious(2);
// IDs should be unique so that registration doesn't override them
std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords()); std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors()); std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels()); cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end())); cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end())); cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
Signature cpCurrent(-1); Signature cpCurrent(1);
uniqueWords = uMultimapToMapUnique(words); uniqueWords = uMultimapToMapUnique(words);
uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors); uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
cpCurrent.sensorData().setCameraModels(data.cameraModels()); cpCurrent.sensorData().setCameraModels(data.cameraModels());
cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end())); cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end())); cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
// This will force comparing descriptors between both images directly // This will force comparing descriptors between both images directly
Transform tmpt = _registrationPipeline->computeTransformation(cpCurrent, cpPrevious, cameraTransform); Transform tmpt = _registrationPipeline->computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
UDEBUG("t=%s", tmpt.prettyPrint().c_str()); UDEBUG("t=%s", tmpt.prettyPrint().c_str());
// compute 3D words by epipolar geometry with the previous signature // compute 3D words by epipolar geometry with the previous signature
@@ -4077,32 +4087,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("inliers=%d", (int)inliers.size()); UDEBUG("inliers=%d", (int)inliers.size());
// words3D should have the same size than words // words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(words.size() == words3D.size()); UASSERT(words.size() == words3D.size());
int added3DPointsWithoutDepth = 0; int added3DPointsWithoutDepth = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter) for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{ {
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first); std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first); std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
if(iter3D == words3D.end()) UASSERT(iter3D!=words3D.end());
{ if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
++added3DPointsWithoutDepth;
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
{ {
iter3D->second = jter->second; iter3D->second = jter->second;
++added3DPointsWithoutDepth; ++added3DPointsWithoutDepth;
} }
} }
UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth); UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
t = timer.ticks(); t = timer.ticks();
UASSERT(words3D.size() == words.size()); UASSERT(words3D.size() == words.size());

37
corelib/src/VWDictionary.cpp
View File

@@ -601,21 +601,18 @@ int VWDictionary::getNextId()
void VWDictionary::addWordRef(int wordId, int signatureId) void VWDictionary::addWordRef(int wordId, int signatureId)
{ {
if(signatureId > 0) VisualWord * vw = 0;
vw = uValue(_visualWords, wordId, vw);
if(vw)
{ {
VisualWord * vw = 0; vw->addRef(signatureId);
vw = uValue(_visualWords, wordId, vw); _totalActiveReferences += 1;
if(vw)
{
vw->addRef(signatureId);
_totalActiveReferences += 1;
_unusedWords.erase(vw->id()); _unusedWords.erase(vw->id());
} }
else else
{ {
UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size()); UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
}
} }
} }
@@ -636,8 +633,6 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn, std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
int signatureId) int signatureId)
{ {
UASSERT(signatureId > 0);
UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows); UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows);
UTimer timer; UTimer timer;
std::list<int> wordIds; std::list<int> wordIds;
@@ -818,7 +813,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
index = *((size_t*)&results.at<int>(i, j)); index = *((size_t*)&results.at<int>(i, j));
} }
int id = uValue(_mapIndexId, index); int id = uValue(_mapIndexId, index);
if(d >= 0.0f && id > 0) if(d >= 0.0f && id != 0)
{ {
fullResults.insert(std::pair<float, int>(d, id)); fullResults.insert(std::pair<float, int>(d, id));
} }
@@ -834,7 +829,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
{ {
float d = matches.at(i).at(j).distance; float d = matches.at(i).at(j).distance;
int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx); int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
if(d >= 0.0f && id > 0) if(d >= 0.0f && id != 0)
{ {
fullResults.insert(std::pair<float, int>(d, id)); fullResults.insert(std::pair<float, int>(d, id));
} }
@@ -857,7 +852,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
{ {
float d = matchesNewWords.at(0).at(j).distance; float d = matchesNewWords.at(0).at(j).distance;
int id = newWordsId[matchesNewWords.at(0).at(j).trainIdx]; int id = newWordsId[matchesNewWords.at(0).at(j).trainIdx];
if(d >= 0.0f && id > 0) if(d >= 0.0f && id != 0)
{ {
fullResults.insert(std::pair<float, int>(d, id)); fullResults.insert(std::pair<float, int>(d, id));
} }
@@ -1176,7 +1171,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
index = *((size_t*)&results.at<int>(i, j)); index = *((size_t*)&results.at<int>(i, j));
} }
int id = uValue(_mapIndexId, index); int id = uValue(_mapIndexId, index);
if(d >= 0.0f && id > 0) if(d >= 0.0f && id != 0)
{ {
fullResults.insert(std::pair<float, int>(d, id)); fullResults.insert(std::pair<float, int>(d, id));
} }
@@ -1188,7 +1183,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
{ {
float d = matches.at(i).at(j).distance; float d = matches.at(i).at(j).distance;
int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx); int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
if(d >= 0.0f && id > 0) if(d >= 0.0f && id != 0)
{ {
fullResults.insert(std::pair<float, int>(d, id)); fullResults.insert(std::pair<float, int>(d, id));
} }
@@ -1202,7 +1197,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
{ {
float d = matchesNotIndexed.at(i).at(j).distance; float d = matchesNotIndexed.at(i).at(j).distance;
int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx); int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
if(d >= 0.0f && id > 0) if(d >= 0.0f && id != 0)
{ {
fullResults.insert(std::pair<float, int>(d, id)); fullResults.insert(std::pair<float, int>(d, id));
} }

2
guilib/src/DatabaseViewer.cpp
View File

@@ -4283,7 +4283,7 @@ void DatabaseViewer::updateWordsMatching()
QList<int> ids = wordsA.uniqueKeys(); QList<int> ids = wordsA.uniqueKeys();
for(int i=0; i<ids.size(); ++i) for(int i=0; i<ids.size(); ++i)
{ {
if(wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1) if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
{ {
// PINK features // PINK features
ui_->graphicsView_A->setFeatureColor(ids[i], Qt::magenta); ui_->graphicsView_A->setFeatureColor(ids[i], Qt::magenta);

6
guilib/src/ui/preferencesDialog.ui
View File

@@ -94,7 +94,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>-253</y>
<width>678</width> <width>678</width>
<height>2811</height> <height>2811</height>
</rect> </rect>
@@ -117,7 +117,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>0</number> <number>20</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -15035,7 +15035,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<property name="text"> <property name="text">
<string>NNDR ratio <string>NNDR ratio
(A matching pair is accepted, if its distance is closer than X times the distance of the second nearest neighbor) (A matching pair is accepted, if its distance is closer than X times the distance of the second nearest neighbor)
Lower the ratio -&gt; higher the precision. 0 means disabled, matching the nearest.</string> Lower the ratio -&gt; higher the precision.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>