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

Added sensorStateOnly parameter : dont use the actuator values in the SM state if true

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@99 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-06-13 18:29:21 +00:00
parent 196dc01a57
commit e2cad72bd0
7 changed files with 108 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -165,6 +165,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Kp, ReactivatedWordsComparedToNewWords, bool, true); //Reactivated words are compared to the last words added in the dictionary (which are not indexed)
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false); // Use of the td-idf strategy to compute the likelihood
RTABMAP_PARAM(Kp, Parallelized, bool, true); // If the dictionary update and signature creation were parallelized
RTABMAP_PARAM(Kp, SensorStateOnly, bool, false); // If using only sensors state (without actuators) for sensorimotor state nearest neighbor computation
RTABMAP_PARAM(Kp, TfIdfNormalized, bool, false); // If tf-idf weighting is normalized by the words count ratio between compared signatures
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0"); // Region of interest ratios [left, right, top, bottom]
RTABMAP_PARAM_STR(Kp, DictionaryPath, ""); // Path of the pre-computed dictionary

1
corelib/src/DBDriver.cpp
View File

@@ -579,7 +579,6 @@ bool DBDriver::addNeighbor(int id, int newNeighbor, int oldNeighbor)
s = uValue(_trashSignatures, id, s);
if(s)
{
const NeighborsMap & neighbors = s->getNeighbors();
std::list<std::vector<float> > actions = uValue(s->getNeighbors(), oldNeighbor, std::list<std::vector<float> >());
s->addNeighbor(newNeighbor, actions);
r = true;

288
corelib/src/KeypointMemory.cpp
View File

@@ -43,6 +43,7 @@ KeypointMemory::KeypointMemory(const ParametersMap & parameters) :
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
_sensorStateOnly(Parameters::defaultKpSensorStateOnly()),
_tfIdfNormalized(Parameters::defaultKpTfIdfNormalized())
{
_vwd = new VWDictionary(parameters);
@@ -94,6 +95,11 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
_parallelized = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpSensorStateOnly())) != parameters.end())
{
_sensorStateOnly = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpTfIdfNormalized())) != parameters.end())
{
_tfIdfNormalized = uStr2Bool((*iter).second.c_str());
@@ -314,10 +320,10 @@ void KeypointMemory::clear()
void KeypointMemory::preUpdate()
{
Memory::preUpdate();
this->cleanUnusedWords();
if(_vwd && !_parallelized)
{
//When parallelized, it is done in CreateSignature
this->cleanUnusedWords();
_vwd->update();
}
}
@@ -401,215 +407,20 @@ void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrate
timer.start();
if(sFrom && sTo)
{
const std::multimap<int, cv::KeyPoint> & wordsA = sFrom->getWords();
if(wordsA.size())
if(s == kUseOnlyFromMerging)
{
UTimer timer2;
const std::multimap<int, cv::KeyPoint> & wordsB = sTo->getWords();
int maxWords = wordsA.size()>wordsB.size()?wordsA.size():wordsB.size();
//if(_keypointDescriptor)
//{
// maxWords = _keypointDetector->getWordsPerImageTarget();
//}
this->disableWordsRef(sTo->id());
sTo->setWords(sFrom->getWords());
std::multimap<int, cv::KeyPoint> newWords;
if(s == kFullMerging)
{
// add features contained in each signatures
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
std::set<int> pairsIdSet;
HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator kpIt = pairs.begin();
std::list<int>::iterator idIt = pairsId.begin();
for(; kpIt != pairs.end() && idIt != pairsId.end(); ++kpIt, ++idIt)
{
newWords.insert(newWords.end(), std::pair<int, cv::KeyPoint>(*idIt, kpIt->second));
pairsIdSet.insert(pairsIdSet.end(), *idIt);
}
ULOGGER_DEBUG("newWords.size() = %d, time add pairs = %fs", newWords.size(), timer2.ticks());
// fill the merged target signature with the farthest features from each signatures
int dif = maxWords - (int)newWords.size();
if(dif>0)
{
//////////
// Add words with the highest response from the two signatures
//////////
std::multimap<int, cv::KeyPoint> allWords = wordsA;
allWords.insert(wordsB.begin(), wordsB.end());
allWords = getMostDescriptiveWords(allWords, dif, pairsIdSet);
newWords.insert(allWords.begin(), allWords.end());
//////////
//////////
// Add words by using the descriptor distance, TIME COMSUMMING! though more precise...
//////////
/*std::list<std::pair<const VisualWord*, const cv::KeyPoint *> > descriptors;
//get all descriptors from features not found in the two signatures
// for A
for(std::multimap<int, cv::KeyPoint>::const_iterator itKey = wordsA.begin(); itKey != wordsA.end(); ++itKey)
{
if(pairsIdSet.find(itKey->first) == pairsIdSet.end())
{
descriptors.push_back(std::pair<const VisualWord*, const cv::KeyPoint *>(_vwd->getWord(itKey->first), &(itKey->second)));
}
}
// for B
for(std::multimap<int, cv::KeyPoint>::const_iterator itKey = wordsB.begin(); itKey != wordsB.end(); ++itKey)
{
if(pairsIdSet.find(itKey->first) == pairsIdSet.end())
{
descriptors.push_back(std::pair<const VisualWord*, const cv::KeyPoint *>(_vwd->getWord(itKey->first), &(itKey->second)));
}
}
ULOGGER_DEBUG("descriptors.size()=%d, wordsA.size()=%d, wordsB.size()=%d", descriptors.size(), wordsA.size(), wordsB.size());
if(descriptors.size() > (unsigned int)dif)
{
FlannKdTreeNN nn;
nn.setStrategy(FlannKdTreeNN::kKDTree);
int k=2;
cv::Mat results(descriptors.size(), k, CV_32SC1); // results index
cv::Mat dists;
if(nn.isDist64F())
{
dists = cv::Mat(descriptors.size(), k, CV_64FC1); // Distance results are CV_64FC1;
}
else
{
dists = cv::Mat(descriptors.size(), k, CV_32FC1); // Distance results are CV_32FC1
}
cv::Mat data(descriptors.size(), descriptors.begin()->first->getDim(), CV_32F); // SURF descriptors are CV_32F
ULOGGER_DEBUG("time prepare nn = %fs", timer2.ticks());
nn.search(data, data, results, dists, k, descriptors.begin()->first->getDim());
ULOGGER_DEBUG("time nn = %fs", timer2.ticks());
//add keypoint which are farthest of their second neighbor
std::list<std::pair<const VisualWord *, const cv::KeyPoint *> >::iterator iter = descriptors.begin();
if(nn.isDist64F())
{
std::multimap<double, std::pair<int, const cv::KeyPoint *> > fartherWords;
for(unsigned int i=0; i<descriptors.size(); ++i)
{
if(fartherWords.size() >= (unsigned int)dif && dists.at<double>(i,1) > fartherWords.begin()->first)
{
fartherWords.erase(fartherWords.begin());
fartherWords.insert(std::pair<double, std::pair<int, const cv::KeyPoint *> >(dists.at<double>(i,1), std::pair<int, const cv::KeyPoint *>(iter->first->id(), iter->second)));
}
else if(fartherWords.size() < (unsigned int)dif)
{
fartherWords.insert(std::pair<double, std::pair<int, const cv::KeyPoint *> >(dists.at<double>(i,1), std::pair<int, const cv::KeyPoint *>(iter->first->id(), iter->second)));
}
++iter;
}
ULOGGER_DEBUG("fartherWords.size()=%d",fartherWords.size());
std::map<double, std::pair<int, const cv::KeyPoint *> >::iterator iterfw = fartherWords.begin();
for(; iterfw != fartherWords.end(); ++iterfw)
{
//ULOGGER_DEBUG("adding words %d", iterfw->second.first);
newWords.insert(std::pair<int, cv::KeyPoint>(iterfw->second.first, *(iterfw->second.second)));
}
}
else
{
std::multimap<float, std::pair<int, const cv::KeyPoint *> > fartherWords;
for(unsigned int i=0; i<descriptors.size(); ++i)
{
if(fartherWords.size() >= (unsigned int)dif && dists.at<float>(i,1) > fartherWords.begin()->first)
{
fartherWords.erase(fartherWords.begin());
fartherWords.insert(std::pair<float, std::pair<int, const cv::KeyPoint *> >(dists.at<float>(i,1), std::pair<int, const cv::KeyPoint *>(iter->first->id(), iter->second)));
}
else if(fartherWords.size() < (unsigned int)dif)
{
fartherWords.insert(std::pair<float, std::pair<int, const cv::KeyPoint *> >(dists.at<float>(i,1), std::pair<int, const cv::KeyPoint *>(iter->first->id(), iter->second)));
}
++iter;
}
ULOGGER_DEBUG("fartherWords.size()=%d",fartherWords.size());
std::map<float, std::pair<int, const cv::KeyPoint *> >::iterator iterfw = fartherWords.begin();
for(; iterfw != fartherWords.end(); ++iterfw)
{
//ULOGGER_DEBUG("adding words %d", iterfw->second.first);
newWords.insert(std::pair<int, cv::KeyPoint>(iterfw->second.first, *(iterfw->second.second)));
}
}
}
else
{
//add all
std::list<std::pair<const VisualWord*, const cv::KeyPoint *> >::iterator iter = descriptors.begin();
for(; iter != descriptors.end(); ++iter)
{
//ULOGGER_DEBUG("adding words %d", iter->first->id());
newWords.insert(std::pair<int, cv::KeyPoint>(iter->first->id(), *(iter->second)));
}
}*/
//////////
}
ULOGGER_DEBUG("time add to newWords = %fs", timer2.ticks());
ULOGGER_DEBUG("maxWords=%d, pairsCount=%d, dif = %d, newWords.size()=%d, _vwd->getUnusedWordsSize()=%d", maxWords, pairs.size(), dif, newWords.size(), _vwd->getUnusedWordsSize());
this->disableWordsRef(sTo->id());
sTo->setWords(newWords);
std::list<int> id;
id.push_back(sTo->id());
this->enableWordsRef(id);
}
else if(s == kUseOnlyFromMerging)
{
this->disableWordsRef(sTo->id());
if(wordsA.size() >= (unsigned int)maxWords)
{
sTo->setWords(wordsA);
}
else
{
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
std::set<int> ignoredIds(pairsId.begin(), pairsId.end());
int dif = maxWords - (int)wordsA.size();
newWords = getMostDescriptiveWords(wordsB, dif, ignoredIds);
newWords.insert(wordsA.begin(), wordsA.end());
sTo->setWords(newWords);
}
std::list<int> id;
id.push_back(sTo->id());
this->enableWordsRef(id);
// Set old image to new merged signature
sTo->setImage(sFrom->getImage());
}
else if(s == kUseOnlyDestMerging)
{
if(wordsB.size() >= (unsigned int)maxWords)
{
// do nothing... already "merged"
}
else
{
this->disableWordsRef(sTo->id());
if(wordsB.size() == 0)
{
ULOGGER_WARN("The merged signature is empty!");
}
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
std::set<int> ignoredIds(pairsId.begin(), pairsId.end());
int dif = maxWords - (int)wordsB.size();
newWords = getMostDescriptiveWords(wordsA, dif, ignoredIds);
newWords.insert(wordsB.begin(), wordsB.end());
sTo->setWords(newWords);
std::list<int> id;
id.push_back(sTo->id());
this->enableWordsRef(id);
}
}
std::list<int> id;
id.push_back(sTo->id());
this->enableWordsRef(id);
// Set old image to new merged signature
sTo->setImage(sFrom->getImage());
}
else if(s == kUseOnlyDestMerging)
{
// do nothing... already "merged"
}
}
else
@@ -816,12 +627,6 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
if(smState)
{
if(_parallelized)
{
this->cleanUnusedWords();
preUpdateThread.start();
}
int treeSize= this->getWorkingMemSize() + this->getStMemSize();
int nbCommonWords = 0;
if(treeSize > 0)
@@ -829,6 +634,11 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
nbCommonWords = _vwd->getTotalActiveReferences() / treeSize;
}
if(_parallelized)
{
preUpdateThread.start();
}
if(smState->getSensors().empty())
{
image = smState->getImage();
@@ -856,13 +666,57 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
}
}
preUpdateThread.join(); // Wait the dictionary to be updated
if(_parallelized)
{
preUpdateThread.join(); // Wait the dictionary to be updated
}
std::list<int> wordIds;
if(descriptors.size())
{
ULOGGER_DEBUG("time descriptor and memory update (size=%d) = %fs", descriptors.begin()->size(), timer.ticks());
wordIds = _vwd->addNewWords(descriptors, descriptors.begin()->size(), id);
unsigned int descriptorSize = descriptors.begin()->size();
if(_parallelized)
{
ULOGGER_DEBUG("time descriptor and memory update (size=%d) = %fs", descriptorSize, timer.ticks());
}
else
{
ULOGGER_DEBUG("time descriptor (size=%d) = %fs", descriptorSize, timer.ticks());
}
//append actuators
if(!_sensorStateOnly && smState->getActuators().size())
{
const std::list<std::vector<float> > & actuators = smState->getActuators();
unsigned int actuatorSize = actuators.begin()->size();
if(actuatorSize > descriptorSize)
{
UERROR("Actuator's size (%d) is larger than descriptor size (%d)", actuatorSize, descriptorSize);
}
for(std::list<std::vector<float> >::const_iterator iter = actuators.begin(); iter!=actuators.end(); ++iter)
{
std::vector<float> descriptor(descriptorSize);
// normalize actuator values
std::vector<float> actuatorNormalized = uNormalize(*iter);
for(unsigned int i=0; i<descriptorSize && i<actuatorSize; ++i)
{
if(i<actuatorSize)
{
descriptor[i] = actuatorNormalized[i];
}
else
{
descriptor[i] = 0;
}
}
descriptors.push_back(descriptor);
}
ULOGGER_DEBUG("time setup actuators (%d) like descriptors %fs", (int)actuatorSize, timer.ticks());
}
wordIds = _vwd->addNewWords(descriptors, descriptorSize, id);
ULOGGER_DEBUG("time addNewWords %fs", timer.ticks());
}
else

1
corelib/src/KeypointMemory.h
View File

@@ -81,6 +81,7 @@ private:
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
bool _sensorStateOnly;
bool _tfIdfNormalized;
};

2
corelib/src/Memory.h
View File

@@ -46,7 +46,7 @@ public:
static const int kIdVirtual;
static const int kIdInvalid;
enum MergingStrategy{kFullMerging, kUseOnlyFromMerging, kUseOnlyDestMerging};
enum MergingStrategy{kUseOnlyFromMerging, kUseOnlyDestMerging};
public:
Memory(const ParametersMap & parameters = ParametersMap());

1
guilib/src/PreferencesDialog.cpp
View File

@@ -159,6 +159,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->checkBox_kp_tfIdfLikelihoodUsed->setObjectName(Parameters::kKpTfIdfLikelihoodUsed().c_str());
_ui->checkBox_kp_tfIdfNormalized->setObjectName(Parameters::kKpTfIdfNormalized().c_str());
_ui->checkBox_kp_parallelized->setObjectName(Parameters::kKpParallelized().c_str());
_ui->checkBox_kp_sensorStateOnly->setObjectName(Parameters::kKpSensorStateOnly().c_str());
_ui->lineEdit_kp_roi->setObjectName(Parameters::kKpRoiRatios().c_str());
_ui->lineEdit_dictionaryPath->setObjectName(Parameters::kKpDictionaryPath().c_str());
connect(_ui->toolButton_dictionaryPath, SIGNAL(clicked()), this, SLOT(changeDictionaryPath()));

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

@@ -68,7 +68,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>0</number>
<number>7</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1113,8 +1113,8 @@ We can edit how will like the prediction used in the Bayes filter when there is
<rect>
<x>0</x>
<y>0</y>
<width>26</width>
<height>78</height>
<width>524</width>
<height>138</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
@@ -2009,7 +2009,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi0">
<property name="suffix">
<string> %</string>
@@ -2019,7 +2019,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_97">
<property name="text">
<string>Left ROI ratio (0 = no change)</string>
@@ -2029,7 +2029,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi1">
<property name="suffix">
<string> %</string>
@@ -2039,7 +2039,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="11" column="0">
<item row="12" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi2">
<property name="suffix">
<string> %</string>
@@ -2049,7 +2049,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="12" column="0">
<item row="13" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi3">
<property name="suffix">
<string> %</string>
@@ -2059,7 +2059,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_98">
<property name="text">
<string>Right ROI ratio (0 = no change)</string>
@@ -2069,7 +2069,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="11" column="1">
<item row="12" column="1">
<widget class="QLabel" name="label_99">
<property name="text">
<string>Top ROI ratio (0 = no change)</string>
@@ -2079,7 +2079,7 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="12" column="1">
<item row="13" column="1">
<widget class="QLabel" name="label_100">
<property name="text">
<string>Bottom ROI ratio (0 = no change)</string>
@@ -2089,14 +2089,14 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QLineEdit" name="lineEdit_kp_roi">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_101">
<property name="text">
<string>ROI ratios [left, right, top, bottom]</string>
@@ -2126,6 +2126,26 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_106">
<property name="text">
<string>If using only sensors state (without actuators) for the sensorimotor state nearest neighbor computation</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="checkBox_kp_sensorStateOnly">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>