i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 01:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged attention branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@657 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-12-11 18:05:05 +00:00
parent f9033809a2
commit 2836d4c48c
216 changed files with 7981 additions and 94889 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/BayesFilter.cpp
+352 -172
View File
@@ -17,9 +17,9 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/BayesFilter.h"
#include "BayesFilter.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
@@ -29,7 +29,8 @@ namespace rtabmap {
BayesFilter::BayesFilter(const ParametersMap & parameters) :
_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
_predictionOnNonNullActionsOnly(Parameters::defaultBayesPredictionOnNonNullActionsOnly())
_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
_totalPredictionLCValues(0.0f)
{
this->setPredictionLC(Parameters::defaultBayesPredictionLC());
this->parseParameters(parameters);
@@ -49,9 +50,9 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
{
this->setPredictionLC((*iter).second);
}
if((iter=parameters.find(Parameters::kBayesPredictionOnNonNullActionsOnly())) != parameters.end())
if((iter=parameters.find(Parameters::kBayesFullPredictionUpdate())) != parameters.end())
{
_predictionOnNonNullActionsOnly = uStr2Bool((*iter).second.c_str());
_fullPredictionUpdate = uStr2Bool((*iter).second.c_str());
}
}
@@ -108,6 +109,11 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
_predictionLC = tmpValues;
}
}
_totalPredictionLCValues = 0.0f;
for(unsigned int j=0; j<_predictionLC.size(); ++j)
{
_totalPredictionLCValues += _predictionLC[j];
}
}
const std::vector<double> & BayesFilter::getPredictionLC() const
@@ -133,6 +139,7 @@ std::string BayesFilter::getPredictionLCStr() const
void BayesFilter::reset()
{
_posterior.clear();
_prediction = cv::Mat();
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
@@ -160,7 +167,6 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
UTimer timer;
timer.start();
cv::Mat prediction;
cv::Mat prior;
cv::Mat posterior;
@@ -168,211 +174,176 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
int j=0;
// Recursive Bayes estimation...
// STEP 1 - Prediction : Prior*lastPosterior
prediction = cv::Mat(likelihood.size(), likelihood.size(), CV_32FC1);
if(this->generatePrediction(prediction, memory, uKeys(likelihood)))
_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)
{
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());
// 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());
((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;
}
bool BayesFilter::generatePrediction(cv::Mat & prediction, const Memory * memory, const std::vector<int> & ids) const
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
{
ULOGGER_DEBUG("");
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();
if(!memory ||
prediction.empty() ||
prediction.rows != prediction.cols ||
(unsigned int)prediction.rows != ids.size() ||
_predictionLC.size() < 2 ||
!ids.size())
{
ULOGGER_ERROR( "fail");
return false;
}
std::map<int, int> idToIndexMap;
for(unsigned int i=0; i<ids.size(); ++i)
{
if(ids[i] == 0)
{
UFATAL("Signature id is null ?!?");
}
UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
}
//int rows = prediction.rows;
prediction = cv::Mat::zeros(prediction.rows, prediction.cols, prediction.type());
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
// Each prior is a column vector
ULOGGER_DEBUG("_predictionLC.size()=%d",_predictionLC.size());
UDEBUG("_predictionLC.size()=%d",_predictionLC.size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
{
int loopSignId = ids[i];
if(loopSignId > 0)
if(idsDone.find(ids[i]) == idsDone.end())
{
// Set high values (gaussians curves) to loop closure neighbors
float sum = 0.0f; // sum values added
float totalModelValues = 0.0f;
for(unsigned int j=0; j<_predictionLC.size(); ++j)
if(ids[i] > 0)
{
totalModelValues += _predictionLC[j];
}
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
double dbAccessTime = 0.0;
std::map<int, int> neighbors = memory->getNeighborsId(dbAccessTime, loopSignId, _predictionLC.size()-1, 0, _predictionOnNonNullActionsOnly);
sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
// ADD values of not found neighbors to loop closure
if(sum < totalModelValues-_predictionLC[0])
{
float delta = totalModelValues-_predictionLC[0]-sum;
((float*)prediction.data)[i + i*cols] += delta;
sum+=delta;
}
float allOtherPlacesValue = 0;
if(totalModelValues < 1)
{
allOtherPlacesValue = 1.0f - totalModelValues;
}
// Set all loop events to small values according to the model
if(allOtherPlacesValue > 0 && cols>1)
{
float value = allOtherPlacesValue / float(cols - 1);
for(int j=ids[0] < 0?1:0; j<cols; ++j)
// 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(((float*)prediction.data)[i + j*cols] == 0)
if(memory->isInSTM(iter->first))
{
((float*)prediction.data)[i + j*cols] = value;
sum += ((float*)prediction.data)[i + j*cols];
neighbors.erase(iter++);
}
else
{
if(iter->second == 0)
{
idsLoopMargin.push_back(iter->second);
}
++iter;
}
}
}
//normalize this row
float maxNorm = 1 - (ids[0]<0?_predictionLC[0]:0); // 1 - virtual place probability
if(sum<maxNorm-0.0001 || sum>maxNorm+0.0001)
{
for(int j=ids[0] < 0?1:0; j<cols; ++j)
// should at least have 1 id in idsMarginLoop
if(idsLoopMargin.size() == 0)
{
((float*)prediction.data)[i + j*cols] *= maxNorm / sum;
UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]);
}
sum = maxNorm;
}
// ADD virtual place prob
if(ids[0] < 0)
{
((float*)prediction.data)[i] = _predictionLC[0];
sum += ((float*)prediction.data)[i];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
//}
if(sum<0.99 || sum > 1.01)
{
UWARN("Prediction is not normalized sum=%f", sum);
}
}
else
{
// Set the virtual place prior
if(_virtualPlacePrior > 0)
{
if(cols>1) // The first must be the virtual place
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
((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;
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
{
// Only for some tests...
// when _virtualPlacePrior=0, set all priors to the same value
if(cols>1)
// Set the virtual place prior
if(_virtualPlacePrior > 0)
{
float val = 1.0/cols;
for(int j=0; j<cols; j++)
if(cols>1) // The first must be the virtual place
{
((float*)prediction.data)[i + j*cols] = val;
((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 if(cols>0)
else
{
((float*)prediction.data)[i] = 1;
// 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;
}
}
}
}
@@ -380,7 +351,217 @@ bool BayesFilter::generatePrediction(cv::Mat & prediction, const Memory * memory
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
return true;
return prediction;
}
void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
{
UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);
int cols = prediction.cols;
// ADD values of not found neighbors to loop closure
if(addedProbabilitiesSum < _totalPredictionLCValues-_predictionLC[0])
{
float delta = _totalPredictionLCValues-_predictionLC[0]-addedProbabilitiesSum;
((float*)prediction.data)[index + index*cols] += delta;
addedProbabilitiesSum+=delta;
}
float allOtherPlacesValue = 0;
if(_totalPredictionLCValues < 1)
{
allOtherPlacesValue = 1.0f - _totalPredictionLCValues;
}
// Set all loop events to small values according to the model
if(allOtherPlacesValue > 0 && cols>1)
{
float value = allOtherPlacesValue / float(cols - 1);
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
if(((float*)prediction.data)[index + j*cols] == 0)
{
((float*)prediction.data)[index + j*cols] = value;
addedProbabilitiesSum += ((float*)prediction.data)[index + j*cols];
}
}
}
//normalize this row
float maxNorm = 1 - (virtualPlaceUsed?_predictionLC[0]:0); // 1 - virtual place probability
if(addedProbabilitiesSum<maxNorm-0.0001 || addedProbabilitiesSum>maxNorm+0.0001)
{
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
((float*)prediction.data)[index + j*cols] *= maxNorm / addedProbabilitiesSum;
}
addedProbabilitiesSum = maxNorm;
}
// ADD virtual place prob
if(virtualPlaceUsed)
{
((float*)prediction.data)[index] = _predictionLC[0];
addedProbabilitiesSum += ((float*)prediction.data)[index];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
//}
if(addedProbabilitiesSum<0.99 || addedProbabilitiesSum > 1.01)
{
UWARN("Prediction is not normalized sum=%f", addedProbabilitiesSum);
}
}
cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds) const
{
UTimer timer;
UDEBUG("");
UASSERT(memory &&
oldIds.size() &&
newIds.size() &&
oldIds.size() == (unsigned int)oldPrediction.cols &&
oldIds.size() == (unsigned int)oldPrediction.rows);
cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
// Create id to index maps
std::map<int, int> oldIdToIndexMap;
std::map<int, int> newIdToIndexMap;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
{
if(i<oldIds.size())
{
UASSERT(oldIds[i]);
oldIdToIndexMap.insert(oldIdToIndexMap.end(), std::make_pair(oldIds[i], i));
//UDEBUG("oldIdToIndexMap[%d] = %d", oldIds[i], i);
}
if(i<newIds.size())
{
UASSERT(newIds[i]);
newIdToIndexMap.insert(newIdToIndexMap.end(), std::make_pair(newIds[i], i));
//UDEBUG("newIdToIndexMap[%d] = %d", newIds[i], i);
}
}
UDEBUG("time creating id-index maps = %fs", timer.restart());
//Get removed ids
std::set<int> removedIds;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(!uContains(newIdToIndexMap, oldIds[i]))
{
removedIds.insert(removedIds.end(), oldIds[i]);
UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
}
}
UDEBUG("time getting removed ids = %fs", timer.restart());
int added = 0;
// get ids to update
std::set<int> idsToUpdate;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
{
if(i<oldIds.size())
{
if(removedIds.find(oldIds[i]) != removedIds.end())
{
unsigned int cols = oldPrediction.cols;
for(unsigned int j=0; j<cols; ++j)
{
if(((const float *)oldPrediction.data)[i + j*cols] != 0.0f &&
j!=i &&
removedIds.find(oldIds[j]) == removedIds.end())
{
//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
idsToUpdate.insert(oldIds[j]);
}
}
}
}
if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0);
float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
this->normalize(prediction, i, sum, newIds[0]<0);
++added;
for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(uContains(oldIdToIndexMap, iter->first) &&
removedIds.find(iter->first) == removedIds.end())
{
idsToUpdate.insert(iter->first);
}
}
}
}
UDEBUG("time getting ids to update = %fs", timer.restart());
// update modified/added ids
int modified = 0;
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{
std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0);
int index = newIdToIndexMap.at(*iter);
float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
this->normalize(prediction, index, sum, newIds[0]<0);
++modified;
}
UDEBUG("time updating modified/added ids = %fs", timer.restart());
//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
// copy not changed probabilities
int copied = 0;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
{
for(int j=0; j<oldPrediction.cols; ++j)
{
if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] != 0.0f)
{
//UDEBUG("i=%d, j=%d", i, j);
//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
((float *)prediction.data)[newIdToIndexMap.at(oldIds[i]) + newIdToIndexMap.at(oldIds[j])*prediction.cols] = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
}
}
++copied;
}
}
UDEBUG("time copying = %fs", timer.restart());
//update virtual place
if(newIds[0] < 0)
{
if(prediction.cols>1) // The first must be the virtual place
{
((float*)prediction.data)[0] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(prediction.cols-1);
for(int j=1; j<prediction.cols; j++)
{
((float*)prediction.data)[j*prediction.cols] = val;
}
}
else if(prediction.cols>0)
{
((float*)prediction.data)[0] = 1;
}
}
UDEBUG("time updating virtual place = %fs", timer.restart());
UDEBUG("Modified=%d, Added=%d, Copied=%d", modified, added, copied);
return prediction;
}
void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
@@ -411,11 +592,10 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
{
if((unsigned int)prediction.cols != idToIndexMap.size() ||
(unsigned int)prediction.rows != idToIndexMap.size())
{
UFATAL("Requirements no met");
}
UASSERT((unsigned int)prediction.cols == idToIndexMap.size() &&
(unsigned int)prediction.rows == idToIndexMap.size() &&
col < (unsigned int)prediction.cols &&
col < (unsigned int)prediction.rows);
float sum=0;
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
corelib/src/BayesFilter.h
+80
View File
@@ -0,0 +1,80 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef BAYESFILTER_H_
#define BAYESFILTER_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <list>
#include <set>
#include "utilite/UEventsHandler.h"
#include "rtabmap/core/Parameters.h"
namespace rtabmap {
class Memory;
class Signature;
class RTABMAP_EXP BayesFilter
{
public:
BayesFilter(const ParametersMap & parameters = ParametersMap());
virtual ~BayesFilter();
virtual void parseParameters(const ParametersMap & parameters);
const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
void reset();
//setters
void setVirtualPlacePrior(float virtualPlacePrior);
void setPredictionLC(const std::string & prediction);
//getters
const std::map<int, float> & getPosterior() const {return _posterior;}
float getVirtualPlacePrior() const {return _virtualPlacePrior;}
const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
private:
cv::Mat updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds) const;
void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
float addNeighborProb(cv::Mat & prediction,
unsigned int col,
const std::map<int, int> & neighbors,
const std::map<int, int> & idToIndexMap) const;
void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
private:
std::map<int, float> _posterior;
cv::Mat _prediction;
float _virtualPlacePrior;
std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
bool _fullPredictionUpdate;
float _totalPredictionLCValues;
};
} // namespace rtabmap
#endif /* BAYESFILTER_H_ */
corelib/src/CMakeLists.txt
+17 -83
View File
@@ -4,28 +4,20 @@ SET(SRC_FILES
RtabmapEvent.cpp
Memory.cpp
KeypointMemory.cpp
SMMemory.cpp
DBDriverFactory.cpp
DBDriver.cpp
DBDriverSqlite3.cpp
DBReader.cpp
Camera.cpp
Micro.cpp
EpipolarGeometry.cpp
VisualWord.cpp
VWDictionary.cpp
BayesFilter.cpp
Parameters.cpp
Signature.cpp
KeypointDetector.cpp
KeypointDescriptor.cpp
VerifyHypotheses.cpp
Features2d.cpp
NearestNeighbor.cpp
ColorTable.cpp
)
SET(INCLUDE_DIRS
@@ -35,93 +27,35 @@ SET(INCLUDE_DIRS
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${SQLITE3_INCLUDE_DIR}
${ZLIB_INCLUDE_DIRS}
${FFTW3F_INCLUDE_DIRS}
)
SET(LIBRARIES
${UTILITE_LIBRARIES}
${OpenCV_LIBS}
${SQLITE3_LIBRARY}
${ZLIB_LIBRARIES}
${FFTW3F_LIBRARIES}
)
####################################
# Generate resources files
####################################
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
COMMENT "[Creating database resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
SET(R
${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
)
#replace semicolons by spaces
foreach(arg ${R})
set(RESOURCES "${RESOURCES}" "${arg}")
endforeach(arg ${R})
SET(RESOURCES_HEADERS
${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes65536_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes65536.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes65536.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes1024_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes1024.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes1024.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes512_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes512.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes512.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes256_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes256.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes256.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes128_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes128.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes128.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes64_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes64.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes64.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes32_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes32.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes32.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes16_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes16.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes16.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes8_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes8.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes8.bin.zip
)
SET(RESOURCES
${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes65536_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes1024_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes512_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes256_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes128_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes64_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes32_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes16_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes8_bin_zip.h
OUTPUT ${RESOURCES_HEADERS}
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
COMMENT "[Creating resources]"
DEPENDS ${R}
)
####################################
@@ -134,7 +68,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
# Add binary that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES})
ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES_HEADERS})
TARGET_LINK_LIBRARIES(rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES(
corelib/src/Camera.cpp
+3 -19
View File
@@ -21,9 +21,7 @@
#include "utilite/UEventsManager.h"
#include "utilite/UConversion.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/DBDriverFactory.h"
#include "rtabmap/core/KeypointDescriptor.h"
#include "rtabmap/core/KeypointDetector.h"
#include "rtabmap/core/Features2d.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include "utilite/UFile.h"
@@ -125,15 +123,9 @@ void Camera::parseParameters(const ParametersMap & parameters)
}
switch(detector)
{
case KeypointDetector::kDetectorStar:
_keypointDetector = new StarDetector(parameters);
break;
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
break;
case KeypointDetector::kDetectorFast:
_keypointDetector = new FASTDetector(parameters);
break;
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
@@ -158,15 +150,6 @@ void Camera::parseParameters(const ParametersMap & parameters)
case KeypointDescriptor::kDescriptorSift:
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorBrief:
_keypointDescriptor = new BRIEFDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorColor:
_keypointDescriptor = new ColorDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorHue:
_keypointDescriptor = new HueDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorSurf:
default:
_keypointDescriptor = new SURFDescriptor(parameters);
@@ -511,7 +494,8 @@ CameraVideo::CameraVideo(const std::string & filePath,
int id) :
Camera(imageRate, autoRestart, imageWidth, imageHeight, framesDropped, id),
_filePath(filePath),
_src(kVideoFile)
_src(kVideoFile),
_usbDevice(0)
{
}
corelib/src/ColorTable.cpp
-67721
View File
File diff suppressed because it is too large Load Diff
corelib/src/DBDriver.cpp
+47 -352
View File
@@ -19,9 +19,8 @@
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/VisualWord.h"
#include "Signature.h"
#include "VisualWord.h"
#include "utilite/UConversion.h"
#include "utilite/UMath.h"
#include "utilite/ULogger.h"
@@ -31,8 +30,6 @@
namespace rtabmap {
DBDriver::DBDriver(const ParametersMap & parameters) :
_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
_imagesCompressed(Parameters::defaultDbImagesCompressed()),
_emptyTrashesTime(0)
{
@@ -48,14 +45,6 @@ DBDriver::~DBDriver()
void DBDriver::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kDbMinSignaturesToSave())) != parameters.end())
{
_minSignaturesToSave = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kDbMinWordsToSave())) != parameters.end())
{
_minWordsToSave = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
{
_imagesCompressed = uStr2Bool((*iter).second.c_str());
@@ -131,18 +120,15 @@ void DBDriver::commit() const
_transactionMutex.unlock();
}
bool DBDriver::executeNoResult(const std::string & sql) const
void DBDriver::executeNoResult(const std::string & sql) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->executeNoResultQuery(sql);
this->executeNoResultQuery(sql);
_dbSafeAccessMutex.unlock();
return r;
}
void DBDriver::emptyTrashes(bool async)
{
ULOGGER_DEBUG("");
if(async)
{
ULOGGER_DEBUG("Async emptying, start the trash thread");
@@ -153,11 +139,12 @@ void DBDriver::emptyTrashes(bool async)
UTimer totalTime;
totalTime.start();
std::vector<Signature*> signatures;
std::map<int, Signature*> signatures;
std::map<int, VisualWord*> visualWords;
_trashesMutex.lock();
{
signatures = uValues(_trashSignatures);
ULOGGER_DEBUG("signatures=%d, visualWords=%d", _trashSignatures.size(), _trashVisualWords.size());
signatures = _trashSignatures;
visualWords = _trashVisualWords;
_trashSignatures.clear();
_trashVisualWords.clear();
@@ -168,7 +155,6 @@ void DBDriver::emptyTrashes(bool async)
if(signatures.size() || visualWords.size())
{
ULOGGER_DEBUG("trashSignatures size = %d, trashVisualWords size = %d", signatures.size(), visualWords.size());
this->beginTransaction();
UTimer timer;
timer.start();
@@ -177,16 +163,16 @@ void DBDriver::emptyTrashes(bool async)
if(this->isConnected())
{
//Only one query to the database
this->saveOrUpdate(signatures);
this->saveOrUpdate(uValues(signatures));
}
for(std::vector<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
for(std::map<int, Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
delete *iter;
delete iter->second;
}
signatures.clear();
ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
}
ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
if(visualWords.size())
{
if(this->isConnected())
@@ -200,8 +186,9 @@ void DBDriver::emptyTrashes(bool async)
delete (*iter).second;
}
visualWords.clear();
ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
}
ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
this->commit();
}
@@ -219,10 +206,6 @@ void DBDriver::asyncSave(Signature * s)
_trashesMutex.lock();
{
_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
if(_trashSignatures.size() > _minSignaturesToSave && this->isIdle())
{
this->start();
}
}
_trashesMutex.unlock();
}
@@ -235,79 +218,13 @@ void DBDriver::asyncSave(VisualWord * vw)
_trashesMutex.lock();
{
_trashVisualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
if(_trashVisualWords.size() > _minWordsToSave && this->isIdle())
{
this->start();
}
}
_trashesMutex.unlock();
}
}
bool DBDriver::getSignature(int signatureId, Signature ** s)
{
*s = 0;
_trashesMutex.lock();
{
if(_trashSignatures.size())
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, Signature*>::iterator iter =_trashSignatures.find(signatureId);
if(iter != _trashSignatures.end())
{
*s = iter->second;
_trashSignatures.erase(iter);
}
}
}
_trashesMutex.unlock();
if(*s == 0)
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadQuery(signatureId, s);
_dbSafeAccessMutex.unlock();
return r;
}
return true;
}
bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
{
*vw = 0;
_trashesMutex.lock();
{
if(_trashVisualWords.size())
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, VisualWord*>::iterator iter = _trashVisualWords.find(wordId);
if(iter != _trashVisualWords.end())
{
*vw = iter->second;
_trashVisualWords.erase(iter);
}
}
}
_trashesMutex.unlock();
if(*vw == 0)
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadQuery(wordId, vw);
_dbSafeAccessMutex.unlock();
return r;
}
return true;
}
//Automatically begin and commit a transaction
bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
ULOGGER_DEBUG("");
std::list<Signature *> toSave;
@@ -335,28 +252,23 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
this->saveQuery(toSave);
}
}
return false;
}
bool DBDriver::load(VWDictionary * dictionary) const
void DBDriver::load(VWDictionary * dictionary) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadQuery(dictionary);
this->loadQuery(dictionary);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadLastNodesQuery(signatures);
this->loadLastNodesQuery(signatures);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
void DBDriver::loadSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
{
UDEBUG("");
// look up in the trash before the database
@@ -399,84 +311,16 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
UDEBUG("");
if(ids.size())
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadKeypointSignaturesQuery(ids, signatures);
this->loadSignaturesQuery(ids, signatures);
_dbSafeAccessMutex.unlock();
return r;
}
else if(signatures.size())
{
return true;
}
return false;
}
// TODO the same code of method loadKeypointSignatures() above is used here
bool DBDriver::loadSMSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws)
{
UDEBUG("");
// look up in the trash before the database
std::list<int> ids = signIds;
std::list<Signature*>::iterator sIter;
bool valueFound = false;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
{
valueFound = false;
for(std::map<int, Signature*>::iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end();)
{
if(sIter->first == *iter)
{
signatures.push_back(sIter->second);
_trashSignatures.erase(sIter++);
valueFound = true;
break;
}
else
{
++sIter;
}
}
if(valueFound)
{
iter = ids.erase(iter);
}
else
{
++iter;
}
}
}
_trashesMutex.unlock();
UDEBUG("");
if(ids.size())
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadSMSignaturesQuery(ids, signatures);
_dbSafeAccessMutex.unlock();
return r;
}
else if(signatures.size())
{
return true;
}
return false;
}
bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws)
{
if(!wordIds.size())
{
return false;
}
// look up in the trash before the database
std::list<int> ids = wordIds;
std::set<int> ids = wordIds;
std::map<int, VisualWord*>::iterator wIter;
std::list<VisualWord *> puttedBack;
_trashesMutex.lock();
@@ -485,7 +329,7 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
for(std::set<int>::iterator iter = ids.begin(); iter != ids.end();)
{
wIter = _trashVisualWords.find(*iter);
if(wIter != _trashVisualWords.end())
@@ -493,7 +337,7 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
UDEBUG("put back word %d from trash", *iter);
puttedBack.push_back(wIter->second);
_trashVisualWords.erase(wIter);
iter = ids.erase(iter);
ids.erase(iter++);
}
else
{
@@ -505,235 +349,90 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
_trashesMutex.unlock();
if(ids.size())
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadWordsQuery(ids, vws);
this->loadWordsQuery(ids, vws);
_dbSafeAccessMutex.unlock();
uAppend(vws, puttedBack);
return r;
}
else if(puttedBack.size())
{
uAppend(vws, puttedBack);
return true;
}
return false;
}
// <oldWordId, activeWordId>
bool DBDriver::changeWordsRef(const std::map<int, int> & refsToChange)
{
//Change references in the trash
KeypointSignature * s = 0;
UTimer timer;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
timer.start();
for(std::map<int, Signature *>::iterator iter = _trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
{
s = dynamic_cast<KeypointSignature*>(iter->second);
if(s)
{
for(std::map<int, int>::const_iterator jter = refsToChange.begin(); jter!=refsToChange.end(); ++jter)
{
s->changeWordsRef((*jter).first, (*jter).second);
}
}
}
ULOGGER_DEBUG("Trash changing words references time=%fs", timer.ticks());
}
_trashesMutex.unlock();
bool r;
_dbSafeAccessMutex.lock();
r = this->changeWordsRefQuery(refsToChange);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::deleteWords(const std::vector<int> & ids)
{
//Delete words in the trash
std::map<int, VisualWord*>::iterator iter;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(unsigned int i=0; i<ids.size(); ++i)
{
iter = _trashVisualWords.find(ids[i]);
if(iter != _trashVisualWords.end())
{
_trashVisualWords.erase(iter);
delete (*iter).second;
}
}
}
_trashesMutex.unlock();
bool r;
_dbSafeAccessMutex.lock();
r = this->deleteWordsQuery(ids);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::deleteAllVisualWords() const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::string query;
query += "DELETE FROM VisualWord;";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
_dbSafeAccessMutex.unlock();
return r;
}
return false;
}
bool DBDriver::deleteAllObsoleteSSVWLinks() const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::string query;
query += "DELETE FROM Map_Node_Word WHERE NOT EXISTS (SELECT id FROM Word WHERE id = Map_Node_Word.word_id);";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
_dbSafeAccessMutex.unlock();
return r;
}
return false;
}
bool DBDriver::deleteUnreferencedWords() const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::string query = "DELETE FROM Word WHERE id NOT IN (SELECT word_id FROM Map_Node_Word);";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
_dbSafeAccessMutex.unlock();
return r;
}
return false;
}
//TODO Check also in the trash ?
bool DBDriver::getRawData(int id, std::list<Sensor> & rawData) const
void DBDriver::getImage(int signatureId, cv::Mat & rawData) const
{
_dbSafeAccessMutex.lock();
bool result = this->getRawDataQuery(id, rawData);
this->getImageQuery(signatureId, rawData);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getActuatorData(int id, std::list<Actuator> & data) const
void DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions) const
{
_dbSafeAccessMutex.lock();
bool result = this->getActuatorDataQuery(id, data);
this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions) const
void DBDriver::loadNeighbors(int signatureId, std::set<int> & neighbors) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
this->loadNeighborsQuery(signatureId, neighbors);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const
void DBDriver::getWeight(int signatureId, int & weight) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadNeighborsQuery(signatureId, neighbors);
this->getWeightQuery(signatureId, weight);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getWeight(int signatureId, int & weight) const
void DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getWeightQuery(signatureId, weight);
this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
void DBDriver::getAllNodeIds(std::set<int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
this->getAllNodeIdsQuery(ids);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getAllNodeIds(std::set<int> & ids) const
void DBDriver::getLastNodeId(int & id) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getAllNodeIdsQuery(ids);
this->getLastIdQuery("Node", id);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLastNodeId(int & id) const
void DBDriver::getLastWordId(int & id) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLastNodeIdQuery(id);
this->getLastIdQuery("Word", id);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLastWordId(int & id) const
void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLastWordIdQuery(id);
this->getInvertedIndexNiQuery(signatureId, ni);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getInvertedIndexNiQuery(signatureId, ni);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::getHighestWeightedNodeIds(unsigned int count, std::multimap<int, int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getHighestWeightedNodeIdsQuery(count, ids);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
{
ULOGGER_DEBUG("");
if(this->isConnected())
@@ -745,13 +444,11 @@ bool DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int proce
<< processMemUsed << ","
<< databaseMemUsed << ");";
bool r = this->executeNoResultQuery(query.str());
return r;
this->executeNoResultQuery(query.str());
}
return false;
}
bool DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
void DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
{
ULOGGER_DEBUG("");
if(this->isConnected())
@@ -759,10 +456,8 @@ bool DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
std::stringstream query;
query << "INSERT INTO StatisticsDictionary(dictionary_size) values(" << dictionarySize << ");";
bool r = this->executeNoResultQuery(query.str());
return r;
this->executeNoResultQuery(query.str());
}
return false;
}
} // namespace rtabmap
corelib/src/DBDriverFactory.cpp
-70
View File
@@ -1,70 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/DBDriverFactory.h"
#include "DBDriverSqlite3.h"
#include "utilite/ULogger.h"
namespace rtabmap {
DBDriver * DBDriverFactory::createDBDriver(const std::string & dbDriverName, const ParametersMap & parameters)
{
// TODO Do it with dynamic link libraries...
// Find the driver...
// Link dynamically to the driver...
DBDriver * driver = 0;
// Static link
if(dbDriverName.compare("sqlite3") == 0)
{
driver = new DBDriverSqlite3(parameters);
}
else if(dbDriverName.compare("mysql") == 0)
{
// TODO mysql driver
ULOGGER_ERROR("mysql driver is not implemented!");
}
else if(dbDriverName.compare("postgresql") == 0)
{
// TODO postgresql driver
ULOGGER_ERROR("postgresql driver is not implemented!");
}
else if(dbDriverName.compare("oracle") == 0)
{
// TODO oracle driver
ULOGGER_ERROR("oracle driver is not implemented!");
}
else
{
ULOGGER_ERROR("Unknown driver \"%s\"", dbDriverName.c_str());
}
return driver;
}
DBDriverFactory::DBDriverFactory() {
}
DBDriverFactory::~DBDriverFactory() {
}
}
corelib/src/DBDriverSqlite3.cpp
+289 -2016
View File
File diff suppressed because it is too large Load Diff
corelib/src/DBDriverSqlite3.h
+26 -38
View File
@@ -22,7 +22,7 @@
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/DBDriver.h"
#include <opencv2/features2d/features2d.hpp>
#include <sqlite3.h>
namespace rtabmap {
@@ -32,7 +32,6 @@ public:
DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
virtual ~DBDriverSqlite3();
virtual std::string getDriverName() const {return "sqlite3";}
virtual void parseParameters(const ParametersMap & parameters);
void setDbInMemory(bool dbInMemory);
void setJournalMode(int journalMode);
@@ -46,55 +45,44 @@ private:
virtual bool isConnectedQuery() const;
virtual long getMemoryUsedQuery() const; // In bytes
virtual bool executeNoResultQuery(const std::string & sql) const;
virtual void executeNoResultQuery(const std::string & sql) const;
virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const; // <oldWordId, activeWordId>
virtual bool deleteWordsQuery(const std::vector<int> & ids) const;
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
virtual bool getWeightQuery(int signatureId, int & weight) const;
virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
virtual void getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
virtual void getWeightQuery(int signatureId, int & weight) const;
virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const;
virtual bool updateQuery(const std::list<Signature *> & signatures) const;
virtual bool saveQuery(const std::list<Signature *> & signatures) const;
virtual void saveQuery(const std::vector<VisualWord *> & visualWords) const;
virtual void updateQuery(const std::list<Signature *> & signatures) const;
virtual void saveQuery(const std::list<Signature *> & signatures) const;
// Load objects
virtual bool loadQuery(VWDictionary * dictionary) const;
virtual bool loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual bool loadQuery(int signatureId, Signature ** s) const;
virtual bool loadQuery(int wordId, VisualWord ** vw) const;
virtual bool loadQuery(int signatureId, KeypointSignature * ss) const;
virtual bool loadQuery(int signatureId, SMSignature * ss) const;
virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual bool loadSMSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual bool loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const;
bool loadLinksQuery(std::list<Signature *> & signatures) const;
virtual void loadQuery(VWDictionary * dictionary) const;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadNeighborsQuery(int signatureId, std::set<int> & neighbors) const;
virtual bool getRawDataQuery(int id, std::list<Sensor> & rawData) const;
virtual bool getActuatorDataQuery(int id, std::list<Actuator> & data) const;
virtual bool getAllNodeIdsQuery(std::set<int> & ids) const;
virtual bool getLastNodeIdQuery(int & id) const;
virtual bool getLastWordIdQuery(int & id) const;
virtual bool getInvertedIndexNiQuery(int signatureId, int & ni) const;
virtual bool getHighestWeightedNodeIdsQuery(unsigned int count, std::multimap<int, int> & ids) const;
virtual void getImageQuery(int nodeId, cv::Mat & image) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
private:
std::string queryStepNode() const;
std::string queryStepSensor() const;
std::string queryStepNodeToSensor() const;
std::string queryStepImage() const;
std::string queryStepLink() const;
std::string queryStepActuator() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
std::string queryStepSensors() const;
int stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
int stepSensor(sqlite3_stmt * ppStmt, int id, int num, const std::vector<int> & data, const Sensor & sensor) const;
int stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, int actuator_id, const std::vector<int> & baseIds) const;
int stepActuator(sqlite3_stmt * ppStmt, int id, int num, const Actuator & actuator) const;
int stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
int stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
void stepNodeToSensor(sqlite3_stmt * ppStmt, int nodeId, int sensorId, int num) const;
void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & image) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
private:
corelib/src/DBReader.cpp
+28 -61
View File
@@ -7,24 +7,20 @@
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/SensorimotorEvent.h"
#include "rtabmap/core/DBDriverFactory.h"
#include "DBDriverSqlite3.h"
#include <utilite/ULogger.h>
#include <utilite/UEventsManager.h>
#include <utilite/UFile.h>
#include "rtabmap/core/Camera.h"
namespace rtabmap {
DBReader::DBReader(const std::string & databasePath,
float frameRate,
const std::set<Sensor::Type> & sensorTypes,
const std::set<Actuator::Type> & actuatorTypes) :
float frameRate) :
_path(databasePath),
_frameRate(frameRate),
_sensorTypes(sensorTypes),
_actuatorTypes(actuatorTypes),
_dbDriver(0),
_currentId(_ids.end())
{
@@ -40,7 +36,7 @@ DBReader::~DBReader()
}
}
bool DBReader::init()
bool DBReader::init(int startIndex)
{
if(_dbDriver)
{
@@ -59,7 +55,7 @@ bool DBReader::init()
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
_dbDriver = DBDriverFactory::createDBDriver("sqlite3", parameters);
_dbDriver = new DBDriverSqlite3(parameters);
if(!_dbDriver)
{
UERROR("Driver doesn't exist.");
@@ -75,6 +71,18 @@ bool DBReader::init()
_dbDriver->getAllNodeIds(_ids);
_currentId = _ids.begin();
if(startIndex>0 && _ids.size())
{
std::set<int>::iterator iter = _ids.lower_bound(startIndex);
if(iter == _ids.end())
{
UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, *_ids.rbegin());
}
else
{
_currentId = iter;
}
}
return true;
}
@@ -94,27 +102,23 @@ void DBReader::mainLoopBegin()
void DBReader::mainLoop()
{
std::list<Sensor> sensors;
std::list<Actuator> actuators;
this->getNextSensorimotorState(sensors, actuators);
if(!sensors.empty() || !actuators.empty())
cv::Mat image;
this->getNextImage(image);
if(!image.empty())
{
UEventsManager::post(new SensorimotorEvent(sensors, actuators));
UEventsManager::post(new CameraEvent(image));
}
else if(!this->isKilled())
{
UDEBUG("no more sensorimotor states...");
UDEBUG("no more images...");
this->kill();
UEventsManager::post(new SensorimotorEvent());
UEventsManager::post(new CameraEvent());
}
}
void DBReader::getNextSensorimotorState(std::list<Sensor> & sensors, std::list<Actuator> & actuators)
void DBReader::getNextImage(cv::Mat & image)
{
sensors.clear();
actuators.clear();
if(_dbDriver)
{
float frameRate = _frameRate;
@@ -140,49 +144,12 @@ void DBReader::getNextSensorimotorState(std::list<Sensor> & sensors, std::list<A
if(!this->isKilled() && _currentId != _ids.end())
{
//sensors
_dbDriver->getRawData(*_currentId, sensors);
//actuators
NeighborsMultiMap neighbors;
_dbDriver->getImage(*_currentId, image);
++_currentId;
if(_currentId != _ids.end())
if(image.empty())
{
_dbDriver->getActuatorData(*_currentId, actuators);
UWARN("No image loaded from the database!");
}
UDEBUG("sensors.size=%d actuators.size=%d", sensors.size(), actuators.size());
//filtering for types wanted
if(_sensorTypes.size())
{
for(std::list<Sensor>::iterator jter=sensors.begin(); jter!=sensors.end();)
{
if(_sensorTypes.find((Sensor::Type)jter->type()) == _sensorTypes.end())
{
jter = sensors.erase(jter);
}
else
{
++jter;
}
}
}
if(_actuatorTypes.size())
{
for(std::list<Actuator>::iterator jter=actuators.begin(); jter!=actuators.end();)
{
if(_actuatorTypes.find((Actuator::Type)jter->type()) == _actuatorTypes.end())
{
jter = actuators.erase(jter);
}
else
{
++jter;
}
}
}
UDEBUG("after filtering sensors.size=%d actuators.size=%d", sensors.size(), actuators.size());
}
}
else
corelib/src/EpipolarGeometry.cpp
+75 -7
View File
@@ -18,9 +18,11 @@
*/
#include "rtabmap/core/EpipolarGeometry.h"
#include "Signature.h"
#include "utilite/ULogger.h"
#include "utilite/UTimer.h"
#include "utilite/UStl.h"
#include "utilite/UMath.h"
#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
@@ -30,8 +32,74 @@
namespace rtabmap
{
/////////////////////////
// HypVerificatorEpipolarGeo
/////////////////////////
EpipolarGeometry::EpipolarGeometry(const ParametersMap & parameters) :
_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
_ransacParam1(Parameters::defaultVhEpRansacParam1()),
_ransacParam2(Parameters::defaultVhEpRansacParam2())
{
this->parseParameters(parameters);
}
EpipolarGeometry::~EpipolarGeometry() {
}
void EpipolarGeometry::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
{
_matchCountMinAccepted = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam1())) != parameters.end())
{
_ransacParam1 = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam2())) != parameters.end())
{
_ransacParam2 = std::atof((*iter).second.c_str());
}
}
bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
{
if(ssA == 0 || ssB == 0)
{
return false;
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
if((int)pairs.size()<_matchCountMinAccepted)
{
return false;
}
std::vector<uchar> status;
cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
int inliers = uSum(status);
if(inliers < _matchCountMinAccepted)
{
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
return false;
}
else
{
UDEBUG("inliers = %d/%d", inliers, pairs.size());
return true;
}
}
//STATIC STUFF
//Epipolar geometry
void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
void EpipolarGeometry::findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
{
if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
{
@@ -66,7 +134,7 @@ void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Ve
//Assuming P0 = [eye(3) zeros(3,1)]
// x1 and x2 are 2D points
// return camera matrix P (3x4) matrix
cv::Mat findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const cv::Mat & x2)
cv::Mat EpipolarGeometry::findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const cv::Mat & x2)
{
if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
@@ -229,7 +297,7 @@ cv::Mat findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const
return p;
}
cv::Mat findFFromWords(
cv::Mat EpipolarGeometry::findFFromWords(
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
std::vector<uchar> & status,
double ransacParam1,
@@ -312,7 +380,7 @@ cv::Mat findFFromWords(
return fundamentalMatrix;
}
void findRTFromP(
void EpipolarGeometry::findRTFromP(
const cv::Mat & p,
cv::Mat & r,
cv::Mat & t)
@@ -331,7 +399,7 @@ void findRTFromP(
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
*/
int findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
@@ -359,7 +427,7 @@ int findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
int EpipolarGeometry::findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
@@ -388,7 +456,7 @@ int findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
* realPairsCount = 5
*/
int findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
corelib/src/Features2d.cpp
+619
View File
@@ -0,0 +1,619 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/Features2d.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include "utilite/ULogger.h"
#include "utilite/UMath.h"
#include "utilite/ULogger.h"
#include "utilite/UTimer.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/gpu/gpu.hpp>
#include <opencv2/core/version.hpp>
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
#include <opencv2/nonfree/features2d.hpp>
#endif
#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
namespace rtabmap {
/////////////////////
// KeypointDescriptor
/////////////////////
KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KeypointDescriptor::~KeypointDescriptor()
{
}
void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
{
}
//////////////////////////
//SURFDescriptor
//////////////////////////
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDescriptor::~SURFDescriptor()
{
}
void SURFDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SURFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
/*#if OPENCV_SURF_GPU
if(_gpuVersion)
{
std::vector<float> d;
cv::gpu::GpuMat imgGpu(img);
cv::gpu::GpuMat descriptorsGpu;
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(_params.hessianThreshold, _params.nOctaves, _params.nOctaveLayers, _params.extended, 0.01f, _params.upright);
surfGpu.uploadKeypoints(keypoints, keypointsGpu);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
surfGpu.downloadDescriptors(descriptorsGpu, d);
unsigned int dim = _params.extended?128:64;
descriptors = cv::Mat(d.size()/dim, dim, CV_32F);
for(int i=0; i<descriptors.rows; ++i)
{
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*dim], dim*sizeof(float));
}
}
else
{
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
extractor.compute(img, keypoints, descriptors);
}
#else*/
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SURF extractor(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#else
cv::SurfDescriptorExtractor extractor(_nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#endif
//#endif
return descriptors;
}
//////////////////////////
//SIFTDescriptor
//////////////////////////
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDescriptor::~SIFTDescriptor()
{
}
void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT extractor(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
extractor.compute(img, keypoints, descriptors);
#else
cv::SIFT extractor(cv::SIFT::DescriptorParams::GET_DEFAULT_MAGNIFICATION(),
cv::SIFT::DescriptorParams::DEFAULT_IS_NORMALIZE,
true,
cv::SIFT::CommonParams::DEFAULT_NOCTAVES,
_nOctaveLayers);
extractor(img, cv::Mat(), keypoints, descriptors, true);
#endif
return descriptors;
}
/////////////////////
// KeypointDetector
/////////////////////
KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f))
{
this->setRoi(Parameters::defaultKpRoiRatios());
this->parseParameters(parameters);
}
void KeypointDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
{
_wordsPerImageTarget = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
}
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & image)
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image.empty())
{
UTimer timer;
timer.start();
cv::Rect roi = computeRoi(image);
// Get keypoints
keypoints = this->_generateKeypoints(image, roi);
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
//clip the number of words... to _wordsPerImageTarget
// Variable hessian threshold
if(_wordsPerImageTarget > 0)
{
if(keypoints.size() > 0)
{
// 10% margin...
if(keypoints.size() > 1.1 * _wordsPerImageTarget)
{
ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, std::vector<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
for(std::vector<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, std::vector<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
}
// Remove them from the signature
int removed = hessianMap.size()-_wordsPerImageTarget;
std::multimap<float, std::vector<cv::KeyPoint>::iterator>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(_wordsPerImageTarget);
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = *iter->second;
// Adjust keypoint position to raw image
kptsTmp[k].pt.x += roi.x;
kptsTmp[k].pt.y += roi.y;
}
keypoints = kptsTmp;
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
else
{
ULOGGER_ERROR("Image is null!");
}
return keypoints;
}
void KeypointDetector::setRoi(const std::string & roi)
{
std::list<std::string> strValues = uSplit(roi, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = std::atof((*iter).c_str());
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
}
}
}
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
{
if(!image.empty() && _roiRatios.size() == 4)
{
float width = image.cols;
float height = image.rows;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
{
roi.x = width * _roiRatios[0];
}
//right roi
roi.width = width - roi.x;
if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
{
roi.width -= width * _roiRatios[1];
}
//top roi
if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
{
roi.y = height * _roiRatios[2];
}
//bottom roi
roi.height = height - roi.y;
if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
{
roi.height -= height * _roiRatios[3];
}
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
return roi;
}
else
{
UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
return cv::Rect();
}
}
//////////////////////////
//SURFDetector
//////////////////////////
SURFDetector::SURFDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDetector::~SURFDetector()
{
}
void SURFDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
/*#if OPENCV_SURF_GPU
if(_gpuVersion )
{
cv::gpu::GpuMat imgGpu(imgRoi);
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.extended, 0.01f, params.upright);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
surfGpu.downloadKeypoints(keypointsGpu, keypoints);
}
else
{
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
detector.detect(imgRoi, keypoints);
}
#else*/
cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, cv::Mat(), keypoints);
#endif
//#endif
return keypoints;
}
//////////////////////////
//SIFTDetector
//////////////////////////
SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDetector::~SIFTDetector()
{
}
void SIFTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT detector(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
detector.detect(imgRoi, keypoints); // Opencv surf keypoints
#else
cv::SIFT detector(_contrastThreshold, _edgeThreshold, cv::SIFT::CommonParams::DEFAULT_NOCTAVES, _nOctaveLayers);
detector(imgRoi, cv::Mat(), keypoints); // Opencv surf keypoints
#endif
return keypoints;
}
}
corelib/src/KeypointDescriptor.cpp
-539
View File
@@ -1,539 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/KeypointDescriptor.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include "utilite/ULogger.h"
#include "utilite/UMath.h"
#include "utilite/ULogger.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/gpu/gpu.hpp>
#include <opencv2/core/version.hpp>
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
#include <opencv2/nonfree/features2d.hpp>
#endif
#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
namespace rtabmap {
KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KeypointDescriptor::~KeypointDescriptor()
{
}
void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
{
}
//////////////////////////
//SURFDescriptor
//////////////////////////
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDescriptor::~SURFDescriptor()
{
}
void SURFDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SURFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
/*#if OPENCV_SURF_GPU
if(_gpuVersion)
{
std::vector<float> d;
cv::gpu::GpuMat imgGpu(img);
cv::gpu::GpuMat descriptorsGpu;
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(_params.hessianThreshold, _params.nOctaves, _params.nOctaveLayers, _params.extended, 0.01f, _params.upright);
surfGpu.uploadKeypoints(keypoints, keypointsGpu);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
surfGpu.downloadDescriptors(descriptorsGpu, d);
unsigned int dim = _params.extended?128:64;
descriptors = cv::Mat(d.size()/dim, dim, CV_32F);
for(int i=0; i<descriptors.rows; ++i)
{
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*dim], dim*sizeof(float));
}
}
else
{
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
extractor.compute(img, keypoints, descriptors);
}
#else*/
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SURF extractor(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#else
cv::SurfDescriptorExtractor extractor(_nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#endif
//#endif
return descriptors;
}
//////////////////////////
//SIFTDescriptor
//////////////////////////
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDescriptor::~SIFTDescriptor()
{
}
void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT extractor(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
extractor.compute(img, keypoints, descriptors);
#else
cv::SIFT extractor(cv::SIFT::DescriptorParams::GET_DEFAULT_MAGNIFICATION(),
cv::SIFT::DescriptorParams::DEFAULT_IS_NORMALIZE,
true,
cv::SIFT::CommonParams::DEFAULT_NOCTAVES,
_nOctaveLayers);
extractor(img, cv::Mat(), keypoints, descriptors, true);
#endif
return descriptors;
}
//////////////////////////
//BRIEFDescriptor
//////////////////////////
BRIEFDescriptor::BRIEFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_size(Parameters::defaultBRIEFSize())
{
this->parseParameters(parameters);
}
BRIEFDescriptor::~BRIEFDescriptor()
{
}
void BRIEFDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kBRIEFSize())) != parameters.end())
{
_size = std::atoi((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat BRIEFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// BRIEF support only grayscale images ?
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::BriefDescriptorExtractor brief(_size);
brief.compute(img, keypoints, descriptors);
return descriptors;
}
//////////////////////////
//ColorDescriptor
//////////////////////////
ColorDescriptor::ColorDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters)
{
this->parseParameters(parameters);
}
ColorDescriptor::~ColorDescriptor()
{
}
void ColorDescriptor::parseParameters(const ParametersMap & parameters)
{
// No parameter...
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat ColorDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
cv::Mat imageConverted;
if(image.channels() != 3 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageConverted, CV_GRAY2BGR);
}
cv::Mat imgMat;
if(!imageConverted.empty())
{
imgMat = imageConverted;
}
else
{
imgMat = image;
}
//create descriptors...
descriptors = cv::Mat(keypoints.size(), 6, CV_32F);
int i=0;
for(std::vector<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
{
int grayMax = -1; // grayValue
int grayMin = -1; // grayValue
float d[6] = {0};
std::vector<int> RxV;
cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
int R = cvRound(key->size*1.2/9.*2);
this->getCircularROI(R, RxV);
cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
// find the brighter and darker pixels
for( int dy = -R; dy <= R; ++dy )
{
int Rx = RxV[abs(dy)];
for( int dx = -Rx; dx <= Rx; ++dx )
{
if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
{
//bgr
uchar b = img(center.y+dy, center.x+dx)[0];
uchar g = img(center.y+dy, center.x+dx)[1];
uchar r = img(center.y+dy, center.x+dx)[2];
int gray = b*0.114 + g*0.587 + r*0.299;
if(grayMax<0 || gray > grayMax)
{
grayMax = gray;
d[0] = b;
d[1] = g;
d[2] = r;
}
if(grayMin<0 || gray < grayMin)
{
grayMin = gray;
d[3] = b;
d[4] = g;
d[5] = r;
}
}
else
{
//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
}
}
}
for(int j=0; j<6; ++j)
{
descriptors.at<float>(i,j) = d[j] / 255; // Normalize between 0 and 1
}
++i;
}
return descriptors;
}
// the function returns x boundary coordinates of
// the circle for each y. RxV[y1] = x1 means that
// when y=y1, -x1 <=x<=x1 is inside the circle
// (from OpenCv doc, C++ Cheatsheet)
void ColorDescriptor::getCircularROI(int R, std::vector<int> & RxV) const
{
RxV.resize(R+1);
for( int y = 0; y <= R; y++ )
RxV[y] = cvRound(sqrt(double(R*R - y*y)));
}
//////////////////////////
//HueDescriptor
//////////////////////////
HueDescriptor::HueDescriptor(const ParametersMap & parameters) :
ColorDescriptor(parameters)
{
this->parseParameters(parameters);
}
HueDescriptor::~HueDescriptor()
{
}
void HueDescriptor::parseParameters(const ParametersMap & parameters)
{
// No parameter...
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat HueDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
cv::Mat imageConverted;
if(image.channels() != 3 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageConverted, CV_GRAY2BGR);
}
cv::Mat imgMat;
if(!imageConverted.empty())
{
imgMat = imageConverted;
}
else
{
imgMat = image;
}
//create descriptors...
descriptors = cv::Mat(keypoints.size(), 2, CV_32F);
int i=0;
for(std::vector<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
{
int intensityMax = -1;
int intensityMin = -1;
float d[2] = {0};
std::vector<int> RxV;
cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
int R = cvRound(key->size*1.2/9.*2);
this->getCircularROI(R, RxV);
cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
// find the brighter and darker pixels using the intensity
int dxb=0;
int dyb=0;
int dxd=0;
int dyd=0;
for( int dy = -R; dy <= R; ++dy )
{
int Rx = RxV[abs(dy)];
for( int dx = -Rx; dx <= Rx; ++dx )
{
if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
{
//bgr
float b = float(img(center.y+dy, center.x+dx)[0]) / 255.0f;
float g = float(img(center.y+dy, center.x+dx)[1]) / 255.0f;
float r = float(img(center.y+dy, center.x+dx)[2]) / 255.0f;
int intensity = rgb2intensity(r, g, b);
if(intensityMax<0 || intensity > intensityMax)
{
intensityMax = intensity;
dxb = dx;
dyb = dy;
}
if(intensityMin<0 || intensity < intensityMin)
{
intensityMin = intensity;
dxd = dx;
dyd = dy;
}
}
else
{
//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
}
}
}
// brighter
float b = float(img(center.y+dyb, center.x+dxb)[0]) / 255.0f;
float g = float(img(center.y+dyb, center.x+dxb)[1]) / 255.0f;
float r = float(img(center.y+dyb, center.x+dxb)[2]) / 255.0f;
d[0] = rgb2hue(r, g, b);
// darker
b = float(img(center.y+dyd, center.x+dxd)[0]) / 255.0f;
g = float(img(center.y+dyd, center.x+dxd)[1]) / 255.0f;
r = float(img(center.y+dyd, center.x+dxd)[2]) / 255.0f;
d[1] = rgb2hue(r, g, b);
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*2], 2*sizeof(float));
++i;
}
return descriptors;
}
// assuming that rgb values are normalized [0,1]
float HueDescriptor::rgb2hue(float r, float g, float b) const
{
double pi = 3.14159265359;
if(b<=g)
{
return acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b))))/pi;
}
else
{
return (pi-acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b)))))/pi;
}
}
}
corelib/src/KeypointDetector.cpp
-479
View File
@@ -36,485 +36,6 @@
namespace rtabmap
{
KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f))
{
this->setRoi(Parameters::defaultKpRoiRatios());
this->parseParameters(parameters);
}
void KeypointDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
{
_wordsPerImageTarget = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
}
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & image)
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image.empty())
{
UTimer timer;
timer.start();
cv::Rect roi = computeRoi(image);
// Get keypoints
keypoints = this->_generateKeypoints(image, roi);
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
//clip the number of words... to _wordsPerImageTarget
// Variable hessian threshold
if(_wordsPerImageTarget > 0)
{
if(keypoints.size() > 0)
{
// 10% margin...
if(keypoints.size() > 1.1 * _wordsPerImageTarget)
{
ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, std::vector<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
for(std::vector<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, std::vector<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
}
// Remove them from the signature
int removed = hessianMap.size()-_wordsPerImageTarget;
std::multimap<float, std::vector<cv::KeyPoint>::iterator>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(_wordsPerImageTarget);
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = *iter->second;
// Adjust keypoint position to raw image
kptsTmp[k].pt.x += roi.x;
kptsTmp[k].pt.y += roi.y;
}
keypoints = kptsTmp;
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
else
{
ULOGGER_ERROR("Image is null!");
}
return keypoints;
}
void KeypointDetector::setRoi(const std::string & roi)
{
std::list<std::string> strValues = uSplit(roi, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = std::atof((*iter).c_str());
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
}
}
}
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
{
if(!image.empty() && _roiRatios.size() == 4)
{
float width = image.cols;
float height = image.rows;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
{
roi.x = width * _roiRatios[0];
}
//right roi
roi.width = width - roi.x;
if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
{
roi.width -= width * _roiRatios[1];
}
//top roi
if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
{
roi.y = height * _roiRatios[2];
}
//bottom roi
roi.height = height - roi.y;
if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
{
roi.height -= height * _roiRatios[3];
}
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
return roi;
}
else
{
UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
return cv::Rect();
}
}
//////////////////////////
//SURFDetector
//////////////////////////
SURFDetector::SURFDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDetector::~SURFDetector()
{
}
void SURFDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
/*#if OPENCV_SURF_GPU
if(_gpuVersion )
{
cv::gpu::GpuMat imgGpu(imgRoi);
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.extended, 0.01f, params.upright);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
surfGpu.downloadKeypoints(keypointsGpu, keypoints);
}
else
{
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
detector.detect(imgRoi, keypoints);
}
#else*/
cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, cv::Mat(), keypoints);
#endif
//#endif
return keypoints;
}
//////////////////////////
//SIFTDetector
//////////////////////////
SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDetector::~SIFTDetector()
{
}
void SIFTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT detector(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
detector.detect(imgRoi, keypoints); // Opencv surf keypoints
#else
cv::SIFT detector(_contrastThreshold, _edgeThreshold, cv::SIFT::CommonParams::DEFAULT_NOCTAVES, _nOctaveLayers);
detector(imgRoi, cv::Mat(), keypoints); // Opencv surf keypoints
#endif
return keypoints;
}
//////////////////////////
//StarDetector
//////////////////////////
StarDetector::StarDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_maxSize(Parameters::defaultStarMaxSize()),
_responseThreshold(Parameters::defaultStarResponseThreshold()),
_lineThresholdProjected(Parameters::defaultStarLineThresholdProjected()),
_lineThresholdBinarized(Parameters::defaultStarLineThresholdBinarized()),
_suppressNonmaxSize(Parameters::defaultStarSuppressNonmaxSize())
{
this->parseParameters(parameters);
}
StarDetector::~StarDetector()
{
}
void StarDetector::parseParameters(const ParametersMap & parameters)
{
ULOGGER_WARN("The StarDetector parameters can't be changed on ROS (this is an issue with the default (and too old) opencv revision used in ROS)");
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kStarLineThresholdBinarized())) != parameters.end())
{
_lineThresholdBinarized = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarLineThresholdProjected())) != parameters.end())
{
_lineThresholdProjected = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarMaxSize())) != parameters.end())
{
_maxSize = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarResponseThreshold())) != parameters.end())
{
_responseThreshold = int(std::atof((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kStarSuppressNonmaxSize())) != parameters.end())
{
_suppressNonmaxSize = std::atoi((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> StarDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
cv::Mat img(image);
// Get keypoints with the star detector
cv::Mat imgRoi(img, roi);
cv::StarDetector detector(_maxSize, _responseThreshold, _lineThresholdProjected, _lineThresholdBinarized, _suppressNonmaxSize);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, keypoints);
#endif
return keypoints;
}
//////////////////////////
//FastDetector
//////////////////////////
FASTDetector::FASTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_threshold(Parameters::defaultFASTThreshold()),
_nonmaxSuppression(Parameters::defaultFASTNonmaxSuppression())
{
this->parseParameters(parameters);
}
FASTDetector::~FASTDetector()
{
}
void FASTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kFASTThreshold())) != parameters.end())
{
_threshold = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kFASTNonmaxSuppression())) != parameters.end())
{
_nonmaxSuppression = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> FASTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
cv::Mat img(image);
cv::Mat imgRoi(img, roi);
cv::FastFeatureDetector fast(_threshold, _nonmaxSuppression);
// Get keypoints with the fast detector
fast.detect(imgRoi, keypoints);
return keypoints;
}
}
corelib/src/KeypointMemory.cpp
-1003
View File
File diff suppressed because it is too large Load Diff
corelib/src/Memory.cpp
+1143 -809
View File
File diff suppressed because it is too large Load Diff
corelib/src/Micro.cpp
-383
View File
@@ -1,383 +0,0 @@
/*
* Micro.cpp
*
* Created on: Mar 5, 2012
* Author: MatLab
*/
#include "rtabmap/core/Micro.h"
#include "utilite/UAudioRecorderMic.h"
#include "utilite/UAudioRecorderFile.h"
#include <utilite/UEventsManager.h>
#include <utilite/UFile.h>
#include <utilite/UMath.h>
#include <fftw3.h>
namespace rtabmap {
Micro::Micro(MicroEvent::Type eventType,
int deviceId,
int fs,
int frameLength,
int channels,
int bytesPerSample,
int id) :
_eventType(eventType),
_recorder(0),
_simulateFreq(false),
_out(0),
_id(id)
{
UASSERT(eventType == MicroEvent::kTypeFrame || eventType == MicroEvent::kTypeFrameFreq || eventType == MicroEvent::kTypeFrameFreqSqrdMagn);
UASSERT(deviceId >= 0);
UASSERT(frameLength > 0 && frameLength % 2 == 0);
_recorder = new UAudioRecorderMic(deviceId, fs, frameLength, bytesPerSample, channels);
}
Micro::Micro(MicroEvent::Type eventType,
const std::string & path,
bool simulateFrameRate,
int frameLength,
int id,
bool playWhileRecording) :
_eventType(eventType),
_recorder(0),
_simulateFreq(simulateFrameRate),
_out(0),
_id(id)
{
UASSERT(eventType == MicroEvent::kTypeFrame || eventType == MicroEvent::kTypeFrameFreq || eventType == MicroEvent::kTypeFrameFreqSqrdMagn);
UASSERT(frameLength > 0 && frameLength % 2 == 0);
if(playWhileRecording)
{
simulateFrameRate = false;
}
_recorder = new UAudioRecorderFile(path, playWhileRecording, frameLength);
}
Micro::~Micro()
{
UDEBUG("");
join(true);
if(_recorder)
{
delete _recorder;
}
if(_out)
{
fftwf_destroy_plan((fftwf_plan)_p);
fftwf_free(_out);
_out = 0;
}
}
bool Micro::init()
{
if(!_recorder->init())
{
UERROR("Recorder initialization failed!");
return false;
}
// init FFTW stuff
if(_out)
{
fftwf_destroy_plan((fftwf_plan)_p);
fftwf_free(_out);
_out = 0;
_in.clear();
}
int N = _recorder->frameLength();
_in.resize(N);
_out = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * N);
_p = fftwf_plan_dft_r2c_1d(N, _in.data(), _out, 0);
_window = uHamming(N);
return true;
}
void Micro::stop()
{
if(this->isRunning())
{
this->kill();
}
else if(_recorder && _recorder->isRunning())
{
_recorder->join(true);
}
}
void Micro::startRecorder()
{
if(_recorder)
{
_recorder->start();
_timer.start();
}
}
void Micro::mainLoopBegin()
{
this->startRecorder();
}
void Micro::mainLoop()
{
if(!_recorder)
{
UERROR("Recorder not initialized");
this->kill();
return;
}
if(this->isRunning())
{
bool noMoreFrames = true;
if(_eventType == MicroEvent::kTypeFrame)
{
UDEBUG("");
cv::Mat data = this->getFrame();
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(data, 2, _recorder->fs(), _recorder->channels(), _id));
}
}
else if(_eventType == MicroEvent::kTypeFrameFreq)
{
UDEBUG("");
cv::Mat freq;
cv::Mat data = this->getFrame(freq, false);
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(MicroEvent::kTypeFrameFreq, freq, _recorder->fs(), _recorder->channels(), _id));
}
}
else if(_eventType == MicroEvent::kTypeFrameFreqSqrdMagn)
{
UDEBUG("");
cv::Mat freq;
cv::Mat data = this->getFrame(freq, true);
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(MicroEvent::kTypeFrameFreqSqrdMagn, freq, _recorder->fs(), _recorder->channels(), _id));
}
}
else
{
UFATAL("Not supposed to be here...");
}
if(noMoreFrames)
{
if(this->isRunning())
{
UEventsManager::post(new MicroEvent(_id));
}
this->kill();
}
}
}
void Micro::mainLoopKill()
{
if(_recorder)
{
_recorder->join(true);
}
}
cv::Mat Micro::getFrame()
{
cv::Mat data;
std::vector<char> frame;
if(!_recorder)
{
UERROR("Micro is not initialized...");
return data;
}
int frameLength = _recorder->frameLength();
int fs = _recorder->fs();
int channels = _recorder->channels();
int bytesPerSample = _recorder->bytesPerSample();
if(_simulateFreq && fs)
{
int sleepTime = ((double(frameLength)/double(fs) - _timer.getElapsedTime()) * 1000.0) + 0.5;
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < double(frameLength)/double(fs)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, double(frameLength)/double(fs));
}
if(_recorder->getNextFrame(frame, true) && int(frame.size()) == frameLength * channels * bytesPerSample)
{
UASSERT(bytesPerSample == 1 || bytesPerSample == 2 || bytesPerSample == 4);
if(bytesPerSample == 1)
{
data = cv::Mat(channels, frameLength, CV_8S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<char>(j, i/(channels*bytesPerSample)) = *((char*)&frame[i + j*bytesPerSample]);
}
}
}
else if(bytesPerSample == 2)
{
data = cv::Mat(channels, frameLength, CV_16S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<short>(j, i/(channels*bytesPerSample)) = *((short*)&frame[i + j*bytesPerSample]);
}
}
}
else if(bytesPerSample == 4)
{
data = cv::Mat(channels, frameLength, CV_32S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<int>(j, i/(channels*bytesPerSample)) = *((int*)&frame[i + j*bytesPerSample]);
}
}
}
}
else
{
UDEBUG("No more frames...");
}
return data;
}
cv::Mat Micro::getFrame(cv::Mat & frameFreq, bool sqrdMagn)
{
cv::Mat frame = this->getFrame();
if(!frame.empty())
{
UASSERT(frame.depth() == CV_8S || frame.depth() == CV_16S || frame.depth() == CV_32S);
cv::Mat timeSample(frame.rows, frame.cols, CV_32F);
for(int i=0; i<frame.cols; ++i)
{
// for each channels
for(int j=0; j<frame.rows; ++j)
{
if(frame.depth() == CV_8S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<char>(j, i)) / float(1<<7); // between 0 and 1
}
else if(frame.depth() == CV_16S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<short>(j, i)) / float(1<<15); // between 0 and 1
}
else if(frame.depth() == CV_32S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<int>(j, i)) / float(1<<31); // between 0 and 1
}
}
}
int size = timeSample.cols/2+1;
if(sqrdMagn)
{
frameFreq = cv::Mat(timeSample.rows, size, CV_32F);
}
else
{
frameFreq = cv::Mat(timeSample.rows, size * 2, CV_32F); // [re, im, re, im, ...]
}
// for each channels
for(int j=0; j<timeSample.rows; ++j)
{
cv::Mat row = timeSample.row(j);
cv::Mat rowFreq = frameFreq.row(j);
memcpy(_in.data(), row.data, row.cols*sizeof(float));
fftwf_execute((fftwf_plan)_p); /* repeat as needed */
float re;
float im;
for(int i=0; i<size; ++i)
{
re = float(_out[i][0]);
im = float(_out[i][1]);
if(sqrdMagn)
{
frameFreq.at<float>(0, i) = re*re+im*im; // squared magnitude
}
else
{
frameFreq.at<float>(0, i*2) = re;
frameFreq.at<float>(0, i*2+1) = im;
}
}
}
}
return frame;
}
int Micro::fs()
{
int fs = 0;
if(_recorder)
{
fs = _recorder->fs();
}
return fs;
}
int Micro::bytesPerSample()
{
int bytes = 0;
if(_recorder)
{
bytes = _recorder->bytesPerSample();
}
return bytes;
}
int Micro::channels()
{
int channels = 0;
if(_recorder)
{
channels = _recorder->channels();
}
return channels;
}
int Micro::nfft()
{
int n = 0;
if(_recorder)
{
n = _recorder->frameLength();
}
return n?n/2+1:0;
}
}
corelib/src/NearestNeighbor.cpp
+11 -49
View File
@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/NearestNeighbor.h"
#include "NearestNeighbor.h"
#include "utilite/ULogger.h"
#include <opencv2/core/core.hpp>
@@ -25,61 +25,24 @@ namespace rtabmap
{
/////////////////////////
// KdTreeNN
// FlannNN
/////////////////////////
KdTreeNN::KdTreeNN(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KdTreeNN::~KdTreeNN()
{
}
void KdTreeNN::setData(const cv::Mat & data)
{
//(data is not copied)
_tree.build(data);
}
void KdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
{
_tree.findNearest(queries, knn, emax, indices, cv::noArray(), dists);
}
void KdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
{
cv::KDTree tree(data);
tree.findNearest(queries, knn, emax, indices, cv::noArray(), dists);
}
void KdTreeNN::parseParameters(const ParametersMap & parameters)
{
NearestNeighbor::parseParameters(parameters);
}
/////////////////////////
// FlannKdTreeNN
/////////////////////////
FlannKdTreeNN::FlannKdTreeNN(const ParametersMap & parameters) :
FlannNN::FlannNN(Strategy strategy, const ParametersMap & parameters) :
_treeFlannIndex(0),
_strategy(kKDTree)
_strategy(strategy)
{
ULOGGER_DEBUG("");
this->parseParameters(parameters);
}
FlannKdTreeNN::~FlannKdTreeNN() {
FlannNN::~FlannNN() {
if(_treeFlannIndex)
{
delete _treeFlannIndex;
}
}
void FlannKdTreeNN::setData(const cv::Mat & data)
void FlannNN::setData(const cv::Mat & data)
{
if(_treeFlannIndex)
{
@@ -88,10 +51,9 @@ void FlannKdTreeNN::setData(const cv::Mat & data)
}
_treeFlannIndex = createIndex(data, _strategy); // using 4 randomized trees
//_treeFlannIndex = new cv::flann::Index(_dataTree, cv::flann::AutotunedIndexParams(0.9, 0.01, 0, 0.1)); // use autotuned parameters
}
void FlannKdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
void FlannNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
{
ULOGGER_DEBUG("");
if(_treeFlannIndex)
@@ -105,7 +67,7 @@ void FlannKdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat &
}
}
void FlannKdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
void FlannNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
{
ULOGGER_DEBUG("");
cv::flann::Index * index = createIndex(data, _strategy);
@@ -114,13 +76,13 @@ void FlannKdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Ma
delete index;
}
void FlannKdTreeNN::parseParameters(const ParametersMap & parameters)
void FlannNN::parseParameters(const ParametersMap & parameters)
{
NearestNeighbor::parseParameters(parameters);
}
enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
cv::flann::Index * FlannKdTreeNN::createIndex(const cv::Mat & data, Strategy s) const
cv::flann::Index * FlannNN::createIndex(const cv::Mat & data, Strategy s) const
{
cv::flann::Index * index = 0;
switch(s)
corelib/src/NearestNeighbor.h
+76
View File
@@ -0,0 +1,76 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef NEARESTNEIGHBOR_H_
#define NEARESTNEIGHBOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <map>
#include "rtabmap/core/Parameters.h"
namespace rtabmap
{
/////////////////////////
// FlannNN
/////////////////////////
class RTABMAP_EXP FlannNN
{
public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Strategy enum ?!
enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
public:
FlannNN(Strategy s = kKDTree, const ParametersMap & parameters = ParametersMap());
virtual ~FlannNN();
void setStrategy(Strategy s) {if(_strategy!=kUndefined) _strategy = s;}
virtual void setData(const cv::Mat & data);
virtual void search(const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64);
virtual void search(const cv::Mat & data,
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) const;
virtual void parseParameters(const ParametersMap & parameters);
private:
cv::flann::Index * createIndex(const cv::Mat & data, Strategy s) const;
private:
cv::flann::Index * _treeFlannIndex;
Strategy _strategy;
};
}
#endif /* NEARESTNEIGHBOR_H_ */
corelib/src/Node.h
-98
View File
@@ -1,98 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef NODE_H_
#define NODE_H_
namespace rtabmap {
class Node
{
public:
Node(int id, Node * parent = 0) :
_parent(parent),
_id(id)
{
if(_parent)
{
_parent->addChild(this);
}
}
virtual ~Node()
{
//We copy the set because when a child is destroyed, it is removed from its parent.
std::set<Node*> children = _children;
_children.clear();
for(std::set<Node*>::iterator iter=children.begin(); iter!=children.end(); ++iter)
{
delete *iter;
}
children.clear();
if(_parent)
{
_parent->removeChild(this);
}
}
int id() const {return _id;}
bool isAncestor(int id) const
{
if(_parent)
{
if(_parent->id() == id)
{
return true;
}
return _parent->isAncestor(id);
}
return false;
}
void expand(std::list<std::list<int> > & paths, std::list<int> currentPath = std::list<int>()) const
{
currentPath.push_back(_id);
if(_children.size() == 0)
{
paths.push_back(currentPath);
return;
}
for(std::set<Node*>::const_iterator iter=_children.begin(); iter!=_children.end(); ++iter)
{
(*iter)->expand(paths, currentPath);
}
}
private:
void addChild(Node * child)
{
_children.insert(child);
}
void removeChild(Node * child)
{
_children.erase(child);
}
private:
std::set<Node*> _children;
Node * _parent;
int _id;
};
}
#endif /* NODE_H_ */
corelib/src/Parameters.cpp
-5
View File
@@ -35,11 +35,6 @@ Parameters::~Parameters()
{
}
const ParametersMap & Parameters::getDefaultParameters()
{
return parameters_;
}
std::string Parameters::getDefaultWorkingDirectory()
{
std::string path = UDirectory::homeDir();
corelib/src/Rtabmap.cpp
+409 -599
View File
File diff suppressed because it is too large Load Diff
corelib/src/RtabmapEvent.cpp
+4 -4
View File
@@ -47,14 +47,14 @@ void Statistics::addStatistic(const std::string & name, float value)
_data.insert(std::pair<std::string, float>(name, value));
}
void Statistics::setRefRawData(const std::list<Sensor> & refRawData)
void Statistics::setRefImage(const cv::Mat & image)
{
_refRawData = refRawData;
_refImage = image;
}
void Statistics::setLoopClosureRawData(const std::list<Sensor> & loopClosureRawData)
void Statistics::setLoopImage(const cv::Mat & image)
{
_loopClosureRawData = loopClosureRawData;
_loopImage = image;
}
}
corelib/src/SMMemory.cpp
-367
View File
@@ -1,367 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/SMMemory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "utilite/UtiLite.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/core/core.hpp>
#include <set>
#include <iostream>
#include <sstream>
#include <string>
#include "rtabmap/core/ColorTable.h"
namespace rtabmap {
SMMemory::SMMemory(const ParametersMap & parameters) :
Memory(parameters),
_useLogPolar(Parameters::defaultSMLogPolarUsed()),
_colorTable(0),
_useMotionMask(Parameters::defaultSMMotionMaskUsed()),
_dBThreshold(Parameters::defaultSMAudioDBThreshold()),
_dBIndexing(Parameters::defaultSMAudioDBIndexing()),
_magnitudeInvariant(Parameters::defaultSMMagnitudeInvariant())
{
this->parseParameters(parameters);
if(!_colorTable)
{
// index 0 = 8, index 1 = 16...
if(Parameters::defaultSMColorTable() == 8)
{
setColorTable(65536);
}
else
{
int i=1;
setColorTable(i<<(Parameters::defaultSMColorTable() + 3));
}
}
}
SMMemory::~SMMemory()
{
ULOGGER_DEBUG("");
if(this->memoryChanged())
{
this->clear();
}
delete _colorTable;
}
void SMMemory::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSMLogPolarUsed())) != parameters.end())
{
_useLogPolar = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMMotionMaskUsed())) != parameters.end())
{
_useMotionMask = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMAudioDBThreshold())) != parameters.end())
{
_dBThreshold = atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMAudioDBIndexing())) != parameters.end())
{
_dBIndexing = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMMagnitudeInvariant())) != parameters.end())
{
_magnitudeInvariant = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMColorTable())) != parameters.end())
{
// index 0 = 8, index 1 = 16...
if(atoi((*iter).second.c_str()) == 8)
{
setColorTable(65536);
}
else
{
int i=1;
setColorTable(i<<(atoi((*iter).second.c_str()) + 3));
}
}
Memory::parseParameters(parameters);
}
void SMMemory::setColorTable(int size)
{
if(_colorTable)
{
if(_colorTable->size() != size)
{
delete _colorTable;
_colorTable = new ColorTable(size);
}
}
else
{
_colorTable = new ColorTable(size);
}
}
void SMMemory::copyData(const Signature * from, Signature * to)
{
// The signatures must be SMSignature
const SMSignature * sFrom = dynamic_cast<const SMSignature *>(from);
SMSignature * sTo = dynamic_cast<SMSignature *>(to);
UTimer timer;
timer.start();
if(sFrom && sTo)
{
sTo->setSensors(sFrom->getData());
}
else
{
ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
}
ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
}
Signature * SMMemory::createSignature(int id, const std::list<Sensor> & rawSensors, bool keepRawData)
{
if(_useMotionMask)
{
UWARN("Using motion mask TODO");
}
UDEBUG("");
UTimer timer;
timer.start();
UTimer timerDetails;
timerDetails.start();
std::list<std::vector<int> > postData;
//const SMSignature * previousSignature = dynamic_cast<const SMSignature *>(this->getLastSignature());
// Process all sensors
for(std::list<Sensor>::const_iterator iter = rawSensors.begin(); iter!=rawSensors.end(); ++iter)
{
if(iter->type() == Sensor::kTypeImage)
{
UASSERT(iter->data().type() == CV_8UC3 && iter->data().channels() == 3);
const cv::Mat & image = iter->data();
UDEBUG("depth=%d, width=%d, height=%d, nChannels=%d, imageSize=%d,", image.type(), image.cols, image.rows, image.channels(), image.total());
if(_useLogPolar)
{
// Log-polar transform
int radius = image.rows < image.cols ? image.rows/2: image.cols/2;
CvSize polarSize = cvSize(64, 128);
float M = polarSize.width/std::log(radius);
IplImage * polar = cvCreateImage( polarSize, 8, 3 );
IplImage iplImg = image;
cvLogPolar(&iplImg, polar, cvPoint2D32f(image.cols/2,image.rows/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
UDEBUG("polar size= %d, %d, time=%fs", polar->width, polar->height, timerDetails.ticks());
// IND transform
unsigned char * data = (unsigned char *)polar->imageData;
int k=0;
std::vector<int> sensors(polar->width*polar->height);
for(int i=0; i<polar->height; ++i)
{
for(int j=0; j<polar->width; ++j)
{
unsigned char & b = data[i*polar->widthStep+j*3+0];
unsigned char & g = data[i*polar->widthStep+j*3+1];
unsigned char & r = data[i*polar->widthStep+j*3+2];
int index = (int)_colorTable->getIndex(r, g, b);
sensors[k] = index;
++k;
}
}
postData.push_back(sensors);
cvReleaseImage(&polar);
UDEBUG("indexing time = %fs", timerDetails.ticks());
}
else
{
// IND transform
int k=0;
std::vector<int> sensors(image.cols*image.rows);
int sum=0;
for(int i=0; i<image.rows; ++i)
{
cv::Mat row = image.row(i); // DON'T modify row! (it refers to const data)
for(int j=0; j<row.cols; j+=3)
{
unsigned char b = row.at<unsigned char>(j+0);
unsigned char g = row.at<unsigned char>(j+1);
unsigned char r = row.at<unsigned char>(j+2);
if(b && g && r)
{
sensors[k] = (int)_colorTable->getIndex(r, g, b); // index
}
else
{
sensors[k] = 0; // null, will be ignored on likelihood computation
}
++k;
}
}
postData.push_back(sensors);
UDEBUG("sum=%d, indexing time = %fs", sum, timerDetails.ticks());
}
} // end kTypeImage
else if(iter->type() == Sensor::kTypeAudioFreqSqrdMagn)
{
UASSERT(iter->data().type() == CV_32FC1);
const cv::Mat & data = iter->data();
int k = 0;
std::vector<int> sensors(data.cols, 0);
unsigned int index;
float max = uMax((float*)data.data, data.cols, index);
int maxLimit = -1; // FIXME Must be not hard coded
float minDB = -1000;// FIXME Must be not hard coded
UDEBUG("data.rows=%d, data.cols=%d, data.type=%d, max=%f at %d", data.rows, data.cols, data.type(), max, index);
if(_dBThreshold > 0)
{
maxLimit = max / std::pow(10.0f, _dBThreshold/10);
}
for(int i=0; i<data.cols; ++i)
{
float val = data.at<float>(0, i);
if(_dBIndexing && max)
{
if(val>=0.001f)
{
val = 10*std::log(val/max);// transform to dB
}
else
{
val = minDB;
}
}
if(!_dBIndexing && val <= maxLimit)
{
val = 0;
}
else if(_dBIndexing)
{
if(val <= minDB || (_dBThreshold && val <= -_dBThreshold))
{
val = 0;
}
else if(max)
{
if(_magnitudeInvariant)
{
val = -1; // ignore magnitude, just set it not null to say this frequency is here
}
else
{
val -= 1; // make sure high values are not null
}
}
}
sensors[k] = int(val);
if((!_dBIndexing && sensors[k]<0) || (_dBIndexing && sensors[k]>0))
{
UERROR("sensors[%d]=%d %f", k, sensors[k], data.at<float>(0,i));
}
++k;
}
postData.push_back(sensors);
} // end kTypeAudioFreqSqrdMagn
else if(iter->type() == Sensor::kTypeTwist)
{
UASSERT(iter->data().type() == CV_32FC1);
const cv::Mat & data = iter->data();
std::vector<int> sensors(data.cols);
for(int i=0; i<data.cols; ++i)
{
sensors[i] = (int)(data.at<float>(0, i)*100.0f);
}
postData.push_back(sensors);
} //end kTypeTwist
else
{
UWARN("Sensor type (%d) not handled!", iter->type());
}
}
ULOGGER_DEBUG("time new signature (id=%d) %fs", id, timer.ticks());
if(keepRawData)
{
return new SMSignature(postData, id, rawSensors);
}
else
{
return new SMSignature(postData, id);
}
}
std::set<int> SMMemory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
{
// get the signatures, if not in the working memory, they
// will be loaded from the database in an more efficient way
// than how it is done in the Memory
ULOGGER_DEBUG("");
UTimer timer;
std::list<int> idsToLoad;
std::map<int, int>::iterator wmIter;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!this->getSignature(*i) && !uContains(idsToLoad, *i))
{
if(!maxLoaded || idsToLoad.size() < maxLoaded)
{
idsToLoad.push_back(*i);
}
}
}
ULOGGER_DEBUG("idsToLoad = %d", idsToLoad.size());
std::list<Signature *> reactivatedSigns;
if(_dbDriver)
{
_dbDriver->loadSMSignatures(idsToLoad, reactivatedSigns);
}
timeDbAccess = timer.getElapsedTime();
for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
{
//append to working memory
this->addSignatureToWm(*i);
}
ULOGGER_DEBUG("time = %fs", timer.ticks());
return std::set<int>(idsToLoad.begin(), idsToLoad.end());
}
} // namespace rtabmap
corelib/src/Signature.cpp
+50 -250
View File
@@ -17,176 +17,94 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/Signature.h"
#include "Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Memory.h"
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/VerifyHypotheses.h"
#include <utilite/UtiLite.h>
namespace rtabmap
{
bool NeighborLink::updateIds(int idFrom, int idTo)
{
bool modified = false;
if(_toId == idFrom)
{
_toId = idTo;
modified = true;
}
for(unsigned int i=0; i<_baseIds.size(); ++i)
{
if(_baseIds[i] == idFrom)
{
_baseIds[i] = idTo;
modified = true;
}
}
return modified;
}
Signature::~Signature()
{
ULOGGER_DEBUG("id=%d", _id);
}
Signature::Signature(int id) :
_id(id),
_weight(0),
_saved(false),
_modified(true)
Signature::Signature(
int id,
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & image) :
_id(id),
_weight(0),
_saved(false),
_modified(true),
_neighborsModified(true),
_words(words),
_enabled(false),
_image(image)
{
}
Signature::Signature(int id, const std::list<Sensor> & rawData) :
_id(id),
_weight(0),
_rawData(rawData),
_saved(false),
_modified(true)
void Signature::addNeighbors(const std::set<int> & neighbors)
{
}
void Signature::addNeighbors(const NeighborsMultiMap & neighbors)
{
for(NeighborsMultiMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
for(std::set<int>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
{
this->addNeighbor(i->second);
this->addNeighbor(*i);
}
}
void Signature::addNeighbor(const NeighborLink & neighbor)
void Signature::addNeighbor(int neighbor)
{
UDEBUG("Add neighbor %d to %d", neighbor.toId(), this->id());
if(ULogger::level() == ULogger::kDebug)
{
UTimer timer;
std::string baseIdsDebug;
const std::vector<int> & baseIds = neighbor.baseIds();
for(unsigned int i=0; i<baseIds.size(); ++i)
{
baseIdsDebug.append(uFormat("%d", baseIds[i]));
if(i+1 < baseIds.size())
{
baseIdsDebug.append(", ");
}
}
UDEBUG("Adding neighbor %d to %d with %d actions, %d baseIds = [%s] (time print=%fs)", neighbor.toId(), this->id(), neighbor.actuators().size(), neighbor.baseIds().size(), baseIdsDebug.c_str(), timer.getElapsedTime());
}
_neighbors.insert(std::pair<int, NeighborLink>(neighbor.toId(), neighbor));
if(neighbor.actuators().size())
{
_neighborsWithActuators.insert(neighbor.toId());
}
_neighborsAll.insert(neighbor.toId());
UDEBUG("Add neighbor %d to %d", neighbor, this->id());
_neighbors.insert(neighbor);
_neighborsModified = true;
}
void Signature::removeNeighbor(int neighborId)
{
int count = _neighbors.erase(neighborId);
if(count)
{
_neighborsModified = true;
}
}
void Signature::removeNeighbors()
{
if(_neighbors.size())
_neighborsModified = true;
_neighbors.clear();
}
void Signature::changeNeighborIds(int idFrom, int idTo)
{
std::pair<NeighborsMultiMap::iterator, NeighborsMultiMap::iterator> pair = _neighbors.equal_range(idFrom);
if(pair.first != _neighbors.end() && pair.first != pair.second)
if(_neighbors.find(idFrom) != _neighbors.end())
{
std::list<NeighborLink> linksToAdd;
for(NeighborsMultiMap::iterator iter = pair.first; iter!=pair.second; ++iter)
{
NeighborLink link = iter->second;
link.updateIds(idFrom, idTo);
linksToAdd.push_back(link);
}
_neighbors.erase(idFrom);
_neighborsWithActuators.erase(idFrom);
_neighborsAll.erase(idFrom);
for(std::list<NeighborLink>::iterator iter=linksToAdd.begin(); iter!=linksToAdd.end(); ++iter)
{
_neighbors.insert(std::pair<int, NeighborLink>(iter->toId(), *iter));
if(iter->actuators().size())
{
_neighborsWithActuators.insert(iter->toId());
}
_neighborsAll.insert(iter->toId());
}
_neighbors.insert(idTo);
_neighborsModified = true;
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
//KeypointSignature
KeypointSignature::KeypointSignature(int id) :
Signature(id),
_enabled(false)
float Signature::compareTo(const Signature * s) const
{
}
KeypointSignature::KeypointSignature(const std::multimap<int, cv::KeyPoint> & words,
int id) :
Signature(id),
_words(words),
_enabled(false)
{
}
KeypointSignature::KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
int id,
const std::list<Sensor> & rawData) :
Signature(id, rawData),
_words(words),
_enabled(false)
{
}
KeypointSignature::~KeypointSignature()
{
}
float KeypointSignature::compareTo(const Signature * s) const
{
const KeypointSignature * ss = dynamic_cast<const KeypointSignature *>(s);
float similarity = 0;
if(ss) //Compatible
float similarity = 0.0f;
const std::multimap<int, cv::KeyPoint> & words = s->getWords();
if(words.size() != 0 && _words.size() != 0)
{
const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
int totalWords = _words.size()>words.size()?_words.size():words.size();
EpipolarGeometry::findPairs(words, _words, pairs);
if(words.size() != 0 && _words.size() != 0)
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
int totalWords = _words.size()>words.size()?_words.size():words.size();
findPairs(words, _words, pairs);
similarity = float(pairs.size()) / float(totalWords);
}
similarity = float(pairs.size()) / float(totalWords);
}
return similarity;
}
void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
void Signature::changeWordsRef(int oldWordId, int activeWordId)
{
std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
if(kps.size())
@@ -200,137 +118,19 @@ void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
}
}
bool KeypointSignature::isBadSignature() const
bool Signature::isBadSignature() const
{
return !_words.size();
}
void KeypointSignature::removeAllWords()
void Signature::removeAllWords()
{
_words.clear();
}
void KeypointSignature::removeWord(int wordId)
void Signature::removeWord(int wordId)
{
_words.erase(wordId);
}
//SMSignature
SMSignature::SMSignature(
const std::list<std::vector<int> > & data,
int id) :
Signature(id),
_data(data)
{
UDEBUG("data=%d", (int)_data.size());
}
SMSignature::SMSignature(
const std::list<std::vector<int> > & data,
int id,
const std::list<Sensor> & rawData) :
Signature(id, rawData),
_data(data)
{
UDEBUG("data=%d", (int)_data.size());
}
SMSignature::SMSignature(int id) :
Signature(id)
{
}
SMSignature::~SMSignature()
{
}
float SMSignature::compareTo(const Signature * s) const
{
const SMSignature * sm = dynamic_cast<const SMSignature *>(s);
float similarity = 0;
if(sm)
{
const std::list<std::vector<int> > & dataB = sm->getData();
//const std::vector<unsigned char> & motionMaskB = sm->getMotionMask();
//if(_data.size() == sensorsB.size() && _data.size()) //Compatible
if(_data.size() == dataB.size()) //Compatible
{
std::vector<float> similarities(_data.size());
// compare sensors
std::list<std::vector<int> >::const_iterator iterA = _data.begin();
std::list<std::vector<int> >::const_iterator iterB = dataB.begin();
int j=0;
while(iterA != _data.end() && iterB != dataB.end())
{
if(iterA->size() == iterB->size())
{
int sum = 0;
int notNull = 0;
for(unsigned int i=0; i<iterA->size(); ++i)
{
sum += iterA->at(i) && iterA->at(i) == iterB->at(i) ? 1 : 0;
notNull += iterA->at(i) || iterB->at(i) ? 1 : 0;
}
if(notNull)
{
similarities[j] = float(sum)/float(notNull);
}
else
{
similarities[j] = 1.0f; // example, silence == 100% silence
}
}
else
{
UERROR("Data are not the same size (%d vs %d)", (int)iterA->size(), (int)iterB->size());
}
++iterA;
++iterB;
++j;
}
similarity = uMean(similarities);
if(ULogger::level() == ULogger::kDebug)
{
std::string str;
for(unsigned int i=0; i<similarities.size(); ++i)
{
str.append(uFormat("%f", similarities[i]));
if(i<similarities.size()-1)
{
str.append(", ");
}
}
UDEBUG("similarities (%d vs %d) = [%s]", this->id(), s->id(), str.c_str());
}
if(similarity<0 || similarity>1)
{
UERROR("Something wrong! similarity is not between 0 and 1 (%f)", similarity);
}
}
else if(!s->isBadSignature() && !this->isBadSignature())
{
UWARN("Not compatible nodes : nb sensors A=%d B=%d", (int)_data.size(), (int)dataB.size());
}
}
else if(s)
{
UWARN("Only SM signatures are compared. (type tested=%s)", s->nodeType().c_str());
}
return similarity;
}
bool SMSignature::isBadSignature() const
{
//return uSum(_data) == 0;
return !_data.size();
}
} //namespace rtabmap
corelib/src/Signature.h
+108
View File
@@ -0,0 +1,108 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <map>
#include <list>
#include <vector>
#include <set>
namespace rtabmap
{
class Memory;
class RTABMAP_EXP Signature
{
public:
Signature(int id,
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & image = cv::Mat());
virtual ~Signature();
/**
* Must return a value between >=0 and <=1 (1 means 100% similarity).
*/
float compareTo(const Signature * signature) const;
bool isBadSignature() const;
int id() const {return _id;}
void addNeighbors(const std::set<int> & neighbors);
void addNeighbor(int neighbor);
void removeNeighbor(int neighborId);
void removeNeighbors();
bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_neighborsModified=true;}
void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_neighborsModified=true;}
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
void changeNeighborIds(int idFrom, int idTo);
const std::set<int> & getNeighbors() const {return _neighbors;}
int getWeight() const {return _weight;}
const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
bool isSaved() const {return _saved;}
bool isModified() const {return _modified || _neighborsModified;}
bool isNeighborsModified() const {return _neighborsModified;}
//visual words stuff
void removeAllWords();
void removeWord(int wordId);
void changeWordsRef(int oldWordId, int activeWordId);
void setWords(const std::multimap<int, cv::KeyPoint> & words) {_enabled = false;_words = words;}
bool isEnabled() const {return _enabled;}
void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
void setImage(const cv::Mat & image) {_image = image;}
const cv::Mat & getImage() const {return _image;}
private:
int _id;
std::set<int> _neighbors; // id
int _weight;
std::set<int> _loopClosureIds;
std::set<int> _childLoopClosureIds;
bool _saved; // If it's saved to bd
bool _modified;
bool _neighborsModified; // Optimization when updating signatures in database
// Contains all words (Some can be duplicates -> if a word appears 2
// times in the signature, it will be 2 times in this list)
// Words match with the CvSeq keypoints and descriptors
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled;
cv::Mat _image;
};
} // namespace rtabmap
corelib/src/VWDictionary.cpp
+15 -78
View File
@@ -18,11 +18,11 @@
*/
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/VisualWord.h"
#include "VisualWord.h"
#include "rtabmap/core/Signature.h"
#include "Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/NearestNeighbor.h"
#include "NearestNeighbor.h"
#include "rtabmap/core/Parameters.h"
#include "utilite/UtiLite.h"
@@ -231,15 +231,8 @@ void VWDictionary::setNNStrategy(NNStrategy strategy, const ParametersMap & para
}
switch(strategy)
{
case kNNKdTree:
//FIXME KdTreeNN is broken...
//_nn = new KdTreeNN(parameters);
//break;
UWARN("KdTree OpenCV is broken, setting nearest neighbor strategy to KdForest FLANN...");
_nn = new FlannKdTreeNN(parameters);
break;
case kNNFlannKdTree:
_nn = new FlannKdTreeNN(parameters);
_nn = new FlannNN(FlannNN::kKDTree, parameters);
break;
case kNNNaive:
default:
@@ -261,13 +254,7 @@ void VWDictionary::setNNStrategy(NNStrategy strategy, const ParametersMap & para
VWDictionary::NNStrategy VWDictionary::nnStrategy() const
{
NNStrategy strategy = kNNUndef;
KdTreeNN * kdTree = dynamic_cast<KdTreeNN*>(_nn);
FlannKdTreeNN * flannKdTree = dynamic_cast<FlannKdTreeNN*>(_nn);
if(kdTree)
{
strategy = kNNKdTree;
}
else if(flannKdTree)
if(_nn)
{
strategy = kNNFlannKdTree;
}
@@ -338,7 +325,7 @@ void VWDictionary::update()
}
// Create the kd-Tree
_dataTree = cv::Mat::zeros(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
_dataTree = cv::Mat(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
{
@@ -355,7 +342,7 @@ void VWDictionary::update()
}
}
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d",_mapIndexId.size(), _visualWords.size());
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), _dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
// Update the nearest neighbor algorithm
@@ -453,14 +440,8 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
cv::Mat results(descriptors.rows, k, CV_32SC1); // results index
cv::Mat dists;
if(_nn->isDist64F())
{
dists = cv::Mat(descriptors.rows, k, CV_64FC1); // Distance results are CV_64FC1;
}
else
{
dists = cv::Mat(descriptors.rows, k, CV_32FC1); // Distance results are CV_32FC1
}
dists = cv::Mat(descriptors.rows, k, CV_32FC1); // Distance results are CV_32FC1
cv::Mat newPts; // SURF descriptors are CV_32F
if(descriptors.type()!=CV_32F)
{
@@ -494,19 +475,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
for(unsigned int j=0; j<k; ++j)
{
float dist;
if(_nn->isDist64F())
{
dist = (float)dists.at<double>(i,j);
}
else
{
dist = dists.at<float>(i,j);
}
if(!_nn->isDistSquared())
{
dist*=dist;
}
dist = dists.at<float>(i,j);
fullResults.insert(std::pair<float, int>(dist, uValue(_mapIndexId, results.at<int>(i,j))));
}
}
@@ -664,16 +633,8 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
cv::Mat dists;
cv::Mat resultsNotIndexed(vws.size(), k, CV_32SC1);
cv::Mat distsNotIndexed;
if(_nn->isDist64F())
{
dists = cv::Mat(vws.size(), k, CV_64FC1); // Distance results are CV_64FC1;
distsNotIndexed = cv::Mat(vws.size(), k, CV_64FC1); // Distance results are CV_64FC1;
}
else
{
dists = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
distsNotIndexed = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
}
dists = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
distsNotIndexed = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
cv::Mat newPts(vws.size(), _dim, CV_32F); // SURF descriptors are CV_32F
// fill the request matrix
@@ -740,36 +701,12 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
float dist;
if(!_dataTree.empty())
{
if(_nn->isDist64F())
{
dist = (float)dists.at<double>(i,j);
}
else
{
dist = dists.at<float>(i,j);
}
if(!_nn->isDistSquared())
{
dist*=dist;
}
dist = dists.at<float>(i,j);
fullResults.insert(std::pair<float, int>(dist, uValue(_mapIndexId, results.at<int>(i,j))));
}
if(searchInNewlyAddedWords && unreferencedWordsCount)
{
if(_nn->isDist64F())
{
dist = (float)distsNotIndexed.at<double>(i,j);
}
else
{
dist = distsNotIndexed.at<float>(i,j);
}
if(!_nn->isDistSquared())
{
dist*=dist;
}
dist = distsNotIndexed.at<float>(i,j);
fullResults.insert(std::pair<float, int>(dist, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at<int>(i,j))));
}
}
@@ -1038,7 +975,7 @@ void VWDictionary::getCommonWords(unsigned int nbCommonWords, int totalSign, std
}
else
{
commonWords = uValues(countMap);
commonWords = uValuesList(countMap);
}
ULOGGER_DEBUG("time = %f s", timer.ticks());
}
corelib/src/VerifyHypotheses.cpp
-166
View File
@@ -1,166 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/VerifyHypotheses.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include <cstdlib>
#include "utilite/UtiLite.h"
namespace rtabmap
{
HypVerificator::HypVerificator(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
void HypVerificator::parseParameters(const ParametersMap & parameters)
{
}
bool HypVerificator::verify(const Signature * ref, const Signature * hyp)
{
UDEBUG("");
return ref && hyp && !ref->isBadSignature() && !hyp->isBadSignature();
}
/////////////////////////
// HypVerificatorSim
/////////////////////////
HypVerificatorSim::HypVerificatorSim(const ParametersMap & parameters) :
HypVerificator(parameters),
_similarity(Parameters::defaultVhSimilarity())
{
this->parseParameters(parameters);
}
HypVerificatorSim::~HypVerificatorSim()
{
}
void HypVerificatorSim::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhSimilarity())) != parameters.end())
{
_similarity = std::atof((*iter).second.c_str());
}
HypVerificator::parseParameters(parameters);
}
bool HypVerificatorSim::verify(const Signature * ref, const Signature * hyp)
{
UDEBUG("");
if(ref && hyp)
{
return ref->compareTo(hyp) >= _similarity;
}
return false;
}
/////////////////////////
// HypVerificatorEpipolarGeo
/////////////////////////
HypVerificatorEpipolarGeo::HypVerificatorEpipolarGeo(const ParametersMap & parameters) :
HypVerificator(parameters),
_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
_ransacParam1(Parameters::defaultVhEpRansacParam1()),
_ransacParam2(Parameters::defaultVhEpRansacParam2())
{
this->parseParameters(parameters);
}
HypVerificatorEpipolarGeo::~HypVerificatorEpipolarGeo() {
}
void HypVerificatorEpipolarGeo::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
{
_matchCountMinAccepted = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam1())) != parameters.end())
{
_ransacParam1 = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam2())) != parameters.end())
{
_ransacParam2 = std::atof((*iter).second.c_str());
}
HypVerificator::parseParameters(parameters);
}
bool HypVerificatorEpipolarGeo::verify(const Signature * ref, const Signature * hyp)
{
UDEBUG("");
const KeypointSignature * ssRef = dynamic_cast<const KeypointSignature *>(ref);
const KeypointSignature * ssHyp = dynamic_cast<const KeypointSignature *>(hyp);
if(ssRef && ssHyp)
{
return doEpipolarGeometry(ssHyp, ssRef);
}
return false;
}
bool HypVerificatorEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB)
{
if(ssA == 0 || ssB == 0)
{
return false;
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
if((int)pairs.size()<_matchCountMinAccepted)
{
return false;
}
std::vector<uchar> status;
cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
int inliers = uSum(status);
if(inliers < _matchCountMinAccepted)
{
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
return false;
}
else
{
UDEBUG("inliers = %d/%d", inliers, pairs.size());
return true;
}
}
}
corelib/src/VisualWord.cpp
+1 -1
View File
@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/VisualWord.h"
#include "VisualWord.h"
#include "utilite/ULogger.h"
#include "utilite/UStl.h"
corelib/src/VisualWord.h
+60
View File
@@ -0,0 +1,60 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
namespace rtabmap
{
class SignatureSurf;
class RTABMAP_EXP VisualWord
{
public:
VisualWord(int id, const float * descriptor, int dim, int signatureId = 0);
~VisualWord();
void addRef(int signatureId);
int removeAllRef(int signatureId);
int getTotalReferences() const {return _totalReferences;}
int id() const {return _id;}
const float * getDescriptor() const {return _descriptor;}
int getDim() const {return _dim;}
const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
bool isSaved() const {return _saved;}
void setSaved(bool saved) {_saved = saved;}
private:
int _id;
float * _descriptor;
int _dim;
bool _saved; // If it's saved to db
int _totalReferences;
std::map<int, int> _references; // (signature id , occurrence in the signature)
std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
};
} // namespace rtabmap
corelib/src/resources/ColorIndexes1024.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes128.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes16.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes256.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes32.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes512.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes64.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes65536.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/ColorIndexes8.bin.zip
BIN
View File
Binary file not shown.
corelib/src/resources/DatabaseSchema.sql
+9 -38
View File
@@ -1,5 +1,5 @@
-- *******************************************************************
-- construct_avpd_db: Script for creating the database
-- DatabaseSchema: Script for creating the database
-- Usage:
-- $ sqlite3 LTM.db < DatabaseSchema.sql
--
@@ -8,60 +8,37 @@
-- *******************************************************************
-- CLEAN
-- *******************************************************************
/*DROP TABLE Node;
DROP TABLE Link;
DROP TABLE Sensor;
DROP TABLE Actuator;
DROP TABLE Word;
DROP TABLE Map_Node_Word;
DROP TABLE Statistics;
DROP TABLE StatisticsSurf;*/
/*DROP TABLE Node;*/
-- *******************************************************************
-- CREATE
-- *******************************************************************
CREATE TABLE Node (
id INTEGER NOT NULL,
type INTEGER NOT NULL, -- 0=Keypoint, 1=Sensor
weight INTEGER,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Sensor (
CREATE TABLE Image (
id INTEGER NOT NULL,
num INTEGER NOT NULL,
type INTEGER NOT NULL, -- kTypeImage=0, kTypeImageFeatures2d, kTypeAudio, kTypeAudioFreq, kTypeAudioFreqSqrdMagn, kTypeJointState, kTypeNotSpecified
data BLOB, -- PostProcessed data (indexed integers)
raw_width INTEGER NOT NULL,
raw_height INTEGER NOT NULL,
raw_data_type INTEGER NOT NULL,
raw_compressed CHAR NOT NULL,
raw_data BLOB,
PRIMARY KEY (id, num)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
actuator_id INTEGER,
base_ids BLOB,
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
CREATE TABLE Actuator (
id INTEGER NOT NULL,
num INTEGER NOT NULL,
type INTEGER NOT NULL, -- kTypeTwist=0, kTypeNotSpecified
width INTEGER NOT NULL,
height INTEGER NOT NULL,
data_type INTEGER NOT NULL,
data BLOB,
PRIMARY KEY (id, num)
);
--
CREATE TABLE Word (
id INTEGER NOT NULL,
@@ -92,17 +69,17 @@ CREATE TABLE Statistics (
);
CREATE TABLE StatisticsDictionary (
dictionary_size INTEGER,
time_enter DATE
dictionary_size INTEGER,
time_enter DATE
);
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word
WHEN NOT EXISTS (SELECT type FROM Node WHERE Node.id = NEW.node_id AND type=0)
WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
BEGIN
SELECT RAISE(ABORT, 'Keypoint type constraint failed');
SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
END;
-- Creating a trigger for time_enter
@@ -121,16 +98,10 @@ BEGIN
UPDATE Statistics SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_StatisticsDictionary_timeEnter AFTER INSERT ON StatisticsDictionary
BEGIN
UPDATE StatisticsDictionary SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
-- *******************************************************************
-- INDEXES
-- *******************************************************************
CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
CREATE INDEX IDX_Sensor_Id on Sensor (id);
CREATE INDEX IDX_Link_from_id on Link (from_id);