void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
{
// look in the trash
_trashesMutex.lock();
if(_trashSignatures.size())
{
for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
{
bool hasNeighbors = !ignoreChildren;
if(ignoreChildren)
{
for(std::map<int, Link>::const_iterator nIter = sIter->second->getLinks().begin();
nIter!=sIter->second->getLinks().end();
++nIter)
{
if(nIter->second.type() == Link::kNeighbor)
{
hasNeighbors = true;
break;
}
}
}
if(hasNeighbors)
{
ids.insert(sIter->first);
}
}
std::vector<int> keys = uKeys(_trashSignatures);
}
_trashesMutex.unlock();
_dbSafeAccessMutex.lock();
this->getAllNodeIdsQuery(ids, ignoreChildren);
_dbSafeAccessMutex.unlock();
}
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
{
if(!_fullPredictionUpdate && !_prediction.empty())
{
return updatePrediction(_prediction, memory, uKeys(_posterior), ids);
}
UDEBUG("");
UASSERT(memory &&
_predictionLC.size() >= 2 &&
ids.size());
UTimer timer;
timer.start();
UTimer timerGlobal;
timerGlobal.start();
std::map<int, int> idToIndexMap;
for(unsigned int i=0; i<ids.size(); ++i)
{
UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
}
//int rows = prediction.rows;
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
// Each prior is a column vector
UDEBUG("_predictionLC.size()=%d",_predictionLC.size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
{
if(idsDone.find(ids[i]) == idsDone.end())
{
if(ids[i] > 0)
{
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0);
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
{
if(memory->isInSTM(iter->first))
{
neighbors.erase(iter++);
}
else
{
if(iter->second == 0)
{
idsLoopMargin.push_back(iter->second);
}
++iter;
}
}
// should at least have 1 id in idsMarginLoop
if(idsLoopMargin.size() == 0)
{
UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]);
}
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
float sum = 0.0f; // sum values added
sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
idsDone.insert(*iter);
this->normalize(prediction, i, sum, ids[0]<0);
}
}
else
{
// Set the virtual place prior
if(_virtualPlacePrior > 0)
{
if(cols>1) // The first must be the virtual place
{
((float*)prediction.data)[i] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(cols-1);
for(int j=1; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
else
{
// Only for some tests...
// when _virtualPlacePrior=0, set all priors to the same value
if(cols>1)
{
float val = 1.0/cols;
for(int j=0; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
}
}
}
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
return prediction;
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
{
ULOGGER_DEBUG("");
if(!memory)
{
ULOGGER_ERROR("Memory is Null!");
return _posterior;
}
if(!likelihood.size())
{
ULOGGER_ERROR("likelihood is empty!");
return _posterior;
}
if(_predictionLC.size() < 2)
{
ULOGGER_ERROR("Prediction is not valid!");
return _posterior;
}
UTimer timer;
timer.start();
cv::Mat prior;
cv::Mat posterior;
float sum = 0;
int j=0;
// Recursive Bayes estimation...
// STEP 1 - Prediction : Prior*lastPosterior
_prediction = this->generatePrediction(memory, uKeys(likelihood));
ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
//std::cout << "Prediction=" << _prediction << std::endl;
// Adjust the last posterior if some images were
// reactivated or removed from the working memory
posterior = cv::Mat(likelihood.size(), 1, CV_32FC1);
this->updatePosterior(memory, uKeys(likelihood));
j=0;
for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
((float*)posterior.data)[j++] = (*i).second;
}
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior.rows, _posterior.size());
//std::cout << "LastPosterior=" << posterior << std::endl;
// Multiply prediction matrix with the last posterior
// (m,m) X (m,1) = (m,1)
prior = _prediction * posterior;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
//std::cout << "ResultingPrior=" << prior << std::endl;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
std::vector<float> likelihoodValues = uValues(likelihood);
//std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
// STEP 2 - Update : Multiply with observations (likelihood)
j=0;
for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
{
std::map<int, float>::iterator p =_posterior.find((*i).first);
if(p!= _posterior.end())
{
(*p).second = (*i).second * ((float*)prior.data)[j++];
sum+=(*p).second;
}
else
{
ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
}
}
ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
//std::cout << "Posterior (before normalization)=" << _posterior << std::endl;
// Normalize
ULOGGER_DEBUG("sum=%f", sum);
if(sum != 0)
{
for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
(*i).second /= sum;
}
}
ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
//std::cout << "Posterior=" << _posterior << std::endl;
return _posterior;
}
