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

MERGE branch STM 325:449 into trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@450 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-03-03 01:46:30 +00:00
parent c4aff5f20a
commit a2eb8bcab6
100 changed files with 76652 additions and 5164 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/BayesFilter.cpp
+93 -117
View File
@@ -19,15 +19,17 @@
#include "BayesFilter.h"
#include "Memory.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
#include "utilite/UtiLite.h"
namespace rtabmap {
BayesFilter::BayesFilter(const ParametersMap & parameters) :
_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr())
_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
_predictionOnNonNullActionsOnly(Parameters::defaultBayesPredictionOnNonNullActionsOnly())
{
this->setPredictionLC(Parameters::defaultBayesPredictionLC());
this->parseParameters(parameters);
@@ -47,6 +49,11 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
{
this->setPredictionLC((*iter).second);
}
if((iter=parameters.find(Parameters::kBayesPredictionOnNonNullActionsOnly())) != parameters.end())
{
_predictionOnNonNullActionsOnly = uStr2Bool((*iter).second.c_str());
}
}
void BayesFilter::setVirtualPlacePrior(float virtualPlacePrior)
@@ -73,23 +80,18 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
std::list<std::string> strValues = uSplit(prediction, ' ');
if(strValues.size() < 2)
{
ULOGGER_ERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
UERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
}
else
{
std::vector<double> tmpValues(strValues.size());
int i=0;
bool valid = true;
float sum = 0;;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = std::atof((*iter).c_str());
sum += tmpValues[i];
if(i>1)
{
sum += tmpValues[i]; // add a second time
}
if(tmpValues[i] < 0 || tmpValues[i]>1)
//UINFO("%d=%e", i, tmpValues[i]);
if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
{
valid = false;
break;
@@ -97,9 +99,9 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
++i;
}
if(!valid || sum <= 0 || sum > 1.001)
if(!valid)
{
ULOGGER_ERROR("The prediction is not valid (the sum must be between >0 && <=1, sum=%f), negative values are not allowed (prediction=\"%s\")", sum, prediction.c_str());
UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str());
}
else
{
@@ -119,7 +121,7 @@ std::string BayesFilter::getPredictionLCStr() const
std::string values;
for(unsigned int i=0; i<_predictionLC.size(); ++i)
{
values.append(uNumber2str(_predictionLC[i]));
values.append(uNumber2Str(_predictionLC[i]));
if(i+1 < _predictionLC.size())
{
values.append(" ");
@@ -167,16 +169,10 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
// Recursive Bayes estimation...
// STEP 1 - Prediction : Prior*lastPosterior
prediction = cvCreateMat(likelihood.size(), likelihood.size(), CV_32FC1);
std::map<int, int> likelihoodKeys;
int index = 0;
for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
{
likelihoodKeys.insert(likelihoodKeys.end(), std::pair<int, int>(iter->first, index++));
}
if(this->generatePrediction(prediction, memory, likelihoodKeys))
if(this->generatePrediction(prediction, memory, uKeys(likelihood)))
{
ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), prediction->rows, prediction->cols);
//std::cout << "Prediction=" << cv::Mat(prediction) << std::endl;
// Adjust the last posterior if some images were
// reactivated or removed from the working memory
@@ -188,12 +184,18 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
posterior->data.fl[j++] = (*i).second;
}
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior->rows, _posterior.size());
//std::cout << "LastPosterior=" << cv::Mat(posterior) << std::endl;
// Multiply prediction matrix with the last posterior
// (m,m) X (m,1) = (m,1)
prior = cvCreateMat(likelihood.size(), 1, CV_32FC1);
cvMatMul(prediction, posterior, prior);
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
//std::cout << "ResultingPrior=" << cv::Mat(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;
@@ -231,7 +233,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
return _posterior;
}
bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory, const std::map<int, int> & likelihoodIds) const
bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory, const std::vector<int> & ids) const
{
ULOGGER_DEBUG("");
UTimer timer;
@@ -239,97 +241,108 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
UTimer timerGlobal;
timerGlobal.start();
if(!likelihoodIds.size() ||
if(!memory ||
prediction == 0 ||
prediction->rows != prediction->cols ||
(unsigned int)prediction->rows != likelihoodIds.size()/*||
prediction->type != CV_32FC1*/ ||
(unsigned int)prediction->rows != ids.size() ||
_predictionLC.size() < 2 ||
!memory)
!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 ?!?");
}
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
}
//int rows = prediction->rows;
cvSetZero(prediction);
int cols = prediction->cols;
// Each priors are column vectors
unsigned int i=0;
// Each prior is a column vector
ULOGGER_DEBUG("_predictionLC.size()=%d",_predictionLC.size());
for(std::map<int, int>::const_iterator iter=likelihoodIds.begin(); iter!=likelihoodIds.end(); ++iter)
for(unsigned int i=0; i<ids.size(); ++i)
{
if(iter->first > 0)
int loopSignId = ids[i];
if(loopSignId > 0)
{
// Create the sum of 2 gaussians around the loop closure
int loopClosureId = iter->first;
// Set high values (gaussians curves) to loop closure neighbors
const Signature * loopSign = memory->getSignature(loopClosureId);
if(!loopSign)
float sum = 0.0f; // sum values added
float totalModelValues = 0.0f;
for(unsigned int j=0; j<_predictionLC.size(); ++j)
{
ULOGGER_ERROR("loopSign %d is not found?!?", loopClosureId);
totalModelValues += _predictionLC[j];
}
// LoopID
prediction->data.fl[i + i*cols] += _predictionLC[1];
// look up for each neighbors (RECURSIVE)
this->addNeighborProb(prediction, i, memory, likelihoodIds, loopSign, 1);
//ULOGGER_DEBUG("neighbor prob for %d, neighbors=%d, time = %fs", loopSign->id(), loopSign->getNeighborIds().size(), timer.ticks());
float totalModelValues = _predictionLC[0] + _predictionLC[1];
for(unsigned int j=2; j<_predictionLC.size(); ++j)
// 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])
{
totalModelValues += _predictionLC[j]*2;
float delta = totalModelValues-_predictionLC[0]-sum;
prediction->data.fl[i + i*cols] += delta;
sum+=delta;
}
//Add values of not found neighbors to the loop closure
float sum = 0;
for(int j=0; j<cols; ++j)
float allOtherPlacesValue = 0;
if(totalModelValues < 1)
{
sum += prediction->data.fl[i + j*cols];
}
if(sum < (totalModelValues-_predictionLC[0]))
{
float gap = (totalModelValues-_predictionLC[0]) - sum;
prediction->data.fl[i + i*cols] += gap;
sum += gap;
}
// add virtual place prob
if(likelihoodIds.begin()->first < 0)
{
sum += prediction->data.fl[i] = _predictionLC[0];
allOtherPlacesValue = 1.0f - totalModelValues;
}
// Set all loop events to small values according to the model
if(totalModelValues < 1.0f)
if(allOtherPlacesValue > 0 && cols>1)
{
float value = (1.0f-totalModelValues) / float(cols);
for(int j=0; j<cols; ++j)
float value = allOtherPlacesValue / float(cols - 1);
for(int j=ids[0] < 0?1:0; j<cols; ++j)
{
if(!prediction->data.fl[i + j*cols])
if(prediction->data.fl[i + j*cols] == 0)
{
sum += prediction->data.fl[i + j*cols] = value;
prediction->data.fl[i + j*cols] = value;
sum += prediction->data.fl[i + j*cols];
}
}
}
//normalize this row,
for(int j=0; j<cols; ++j)
//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)
{
prediction->data.fl[i + j*cols] /= sum;
for(int j=ids[0] < 0?1:0; j<cols; ++j)
{
prediction->data.fl[i + j*cols] *= maxNorm / sum;
}
sum = maxNorm;
}
// ADD virtual place prob
if(ids[0] < 0)
{
prediction->data.fl[i] = _predictionLC[0];
sum += prediction->data.fl[i];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test = %f", prediction->data.fl[i + j*cols]);
// 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
{
@@ -368,7 +381,6 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
}
}
}
++i;
}
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
@@ -402,57 +414,21 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
_posterior = newPosterior;
}
//recursive...
float BayesFilter::addNeighborProb(CvMat * prediction, unsigned int row, const Memory * memory, const std::map<int, int> & likelihoodIds, const Signature * s, unsigned int level) const
float BayesFilter::addNeighborProb(CvMat * prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
{
if(!likelihoodIds.size() ||
prediction == 0 ||
prediction->rows != prediction->cols ||
(unsigned int)prediction->rows != likelihoodIds.size() ||
_predictionLC.size() < 2 ||
!memory ||
!prediction ||
level<1)
if((unsigned int)prediction->cols != idToIndexMap.size() ||
(unsigned int)prediction->rows != idToIndexMap.size())
{
ULOGGER_ERROR( "fail");
return 0;
UFATAL("Requirements no met");
}
if(level+1 >= _predictionLC.size() || !s)
{
return 0;
}
double value = _predictionLC[level+1];
float sum=0;
const NeighborsMap & neighbors = s->getNeighbors();
for(NeighborsMap::const_iterator iter=neighbors.begin(); iter!= neighbors.end(); ++iter)
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
int index = uValue(likelihoodIds, iter->first, -1);
int index = uValue(idToIndexMap, iter->first, -1);
if(index >= 0)
{
bool alreadyAdded = false;
// the value can be already added in the recursion
if(value > prediction->data.fl[row + index*prediction->cols])
{
sum -= prediction->data.fl[row + index*prediction->cols];
prediction->data.fl[row + index*prediction->cols] = value;
sum += value;
}
else
{
alreadyAdded = true;
}
if(!alreadyAdded && level+1 < _predictionLC.size())
{
sum += addNeighborProb(prediction, row, memory, likelihoodIds, memory->getSignature(iter->first), level+1);
}
}
else
{
//ULOGGER_DEBUG("BayesFilter::generatePrediction(...) F (id %d) Not found for loop %d", loopSign->getNeighborForward(), loopClosureId);
sum += prediction->data.fl[col + index*prediction->cols] = _predictionLC[iter->second+1];
}
}
return sum;
corelib/src/BayesFilter.h
+4 -2
View File
@@ -51,17 +51,19 @@ public:
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...}
bool isPredictionOnNonNullActionsOnly() const {return _predictionOnNonNullActionsOnly;}
bool generatePrediction(CvMat * prediction, const Memory * memory, const std::map<int, int> & likelihoodIds) const;
float addNeighborProb(CvMat * prediction, unsigned int row, const Memory * memory, const std::map<int, int> & likelihoodIds, const Signature * s, unsigned int level) const;
bool generatePrediction(CvMat * prediction, const Memory * memory, const std::vector<int> & ids) const;
private:
void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
float addNeighborProb(CvMat * prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const;
private:
std::map<int, float> _posterior;
float _virtualPlacePrior;
std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
bool _predictionOnNonNullActionsOnly;
};
} // namespace rtabmap
corelib/src/CMakeLists.txt
+83 -4
View File
@@ -21,30 +21,109 @@ SET(SRC_FILES
KeypointDescriptor.cpp
VerifyHypotheses.cpp
NearestNeighbor.cpp
ColorTable.cpp
)
SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_BINARY_DIR}
${UTILITE_INCLUDE_DIR}
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${SQLITE3_INCLUDE_DIR}
${ZLIB_INCLUDE_DIRS}
)
SET(LIBRARIES
${UTILITE_LIBRARY}
${UTILITE_LIBRARIES}
${OpenCV_LIBS}
${SQLITE3_LIBRARY}
${ZLIB_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 resources]"
COMMENT "[Creating database resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
)
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
)
####################################
# Generate resources files END
####################################
# Make sure the compiler can find include files from our library.
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
@@ -58,7 +137,7 @@ ENDIF(WIN32)
# Add binary that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_LIBRARY(corelib ${SRC_FILES} ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h)
ADD_LIBRARY(corelib ${SRC_FILES} ${RESOURCES})
TARGET_LINK_LIBRARIES(corelib ${LIBRARIES})
SET_TARGET_PROPERTIES(
corelib/src/Camera.cpp
+82 -107
View File
@@ -54,10 +54,10 @@ CamKeypointTreatment::~CamKeypointTreatment()
}
void CamKeypointTreatment::process(SMState * smState) const
{
if(smState && smState->getImage() && smState->getKeypoints().size() == 0 && smState->getSensors().size() == 0)
if(_keypointDetector && _keypointDescriptor && smState && smState->getImage() && smState->getKeypoints().size() == 0 && smState->getSensors().empty())
{
std::list<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(smState->getImage());
std::list<std::vector<float> > descriptors = _keypointDescriptor->generateDescriptors(smState->getImage(), keypoints);
std::vector<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(smState->getImage());
cv::Mat descriptors = _keypointDescriptor->generateDescriptors(smState->getImage(), keypoints);
smState->setSensors(descriptors);
smState->setKeypoints(keypoints);
}
@@ -90,6 +90,9 @@ void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
case kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
break;
case kDetectorFast:
_keypointDetector = new FASTDetector(parameters);
break;
case kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
@@ -117,20 +120,17 @@ void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
}
switch(descriptorStrategy)
{
case kDescriptorColorSurf:
// see decorator pattern...
_keypointDescriptor = new ColorDescriptor(parameters, new SURFDescriptor(parameters));
break;
case kDescriptorLaplacianSurf:
// see decorator pattern...
_keypointDescriptor = new LaplacianDescriptor(parameters, new SURFDescriptor(parameters));
break;
case kDescriptorSift:
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case kDescriptorHueSurf:
// see decorator pattern...
_keypointDescriptor = new HueDescriptor(parameters, new SURFDescriptor(parameters));
case kDescriptorBrief:
_keypointDescriptor = new BRIEFDescriptor(parameters);
break;
case kDescriptorColor:
_keypointDescriptor = new ColorDescriptor(parameters);
break;
case kDescriptorHue:
_keypointDescriptor = new HueDescriptor(parameters);
break;
case kDescriptorSurf:
default:
@@ -174,12 +174,25 @@ Camera::Camera(float imageRate,
UEventsManager::addHandler(this);
}
Camera::~Camera(void)
Camera::~Camera()
{
this->kill();
join(true);
delete _postThreatement;
}
SMState * Camera::takeSMState()
{
std::list<std::vector<float> > actions;
IplImage * img = this->takeImage(&actions);
if(img)
{
SMState * smState = new SMState(cv::Mat(), actions);
smState->setImage(img);
return smState;
}
return 0;
}
void Camera::mainLoop()
{
State state = kStateCapturing;
@@ -231,36 +244,6 @@ void Camera::pushNewState(State newState, const ParametersMap & parameters)
void Camera::handleEvent(UEvent* anEvent)
{
if(anEvent->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * cameraEvent = (CameraEvent*)anEvent;
if(cameraEvent->getCode() == CameraEvent::kCodeCtrl)
{
CameraEvent::Cmd cmd = cameraEvent->getCommand();
if(cmd == CameraEvent::kCmdPause)
{
if(this->isRunning())
{
this->kill();
}
else
{
this->start();
}
}
else if(cmd == CameraEvent::kCmdChangeParam)
{
// TODO : Put in global Parameters ?
_imageRate = cameraEvent->getImageRate();
_autoRestart = cameraEvent->getAutoRestart();
}
else
{
ULOGGER_DEBUG("Camera::handleEvent(Util::Event* anEvent) : command undefined...");
}
}
}
if(anEvent->getClassName().compare("ParamEvent") == 0)
{
if(this->isIdle())
@@ -281,7 +264,7 @@ void Camera::process()
{
UTimer timer;
ULOGGER_DEBUG("Camera::process()");
SMState * smState = this->takeImage();
SMState * smState = this->takeSMState();
if(smState)
{
_postThreatement->process(smState);
@@ -336,7 +319,7 @@ CameraImages::CameraImages(const std::string & path,
CameraImages::~CameraImages(void)
{
this->kill();
join(true);
if(_dir)
{
delete _dir;
@@ -361,11 +344,19 @@ bool CameraImages::init()
{
ULOGGER_ERROR("Directory path not valid \"%s\"", _path.c_str());
}
else if(_dir->getFileNames().size() == 0)
{
UWARN("Directory is empty \"%s\"", _path.c_str());
}
return _dir != 0;
}
SMState * CameraImages::takeImage()
IplImage * CameraImages::takeImage(std::list<std::vector<float> > * actions)
{
if(actions)
{
actions->clear();
}
IplImage * img = 0;
if(_dir)
{
@@ -406,6 +397,10 @@ SMState * CameraImages::takeImage()
}
}
}
else
{
UWARN("Directory is not set, camera must be initialized.");
}
if(img &&
getImageWidth() &&
getImageHeight() &&
@@ -422,11 +417,7 @@ SMState * CameraImages::takeImage()
cvReleaseImage(&img);
img = resampledImg;
}
if(img)
{
return new SMState(img);
}
return 0;
return img;
}
@@ -461,7 +452,7 @@ CameraVideo::CameraVideo(const std::string & fileName,
CameraVideo::~CameraVideo()
{
this->kill();
join(true);
if(_capture)
{
cvReleaseCapture(&_capture);
@@ -503,8 +494,12 @@ bool CameraVideo::init()
return true;
}
SMState * CameraVideo::takeImage()
IplImage * CameraVideo::takeImage(std::list<std::vector<float> > * actions)
{
if(actions)
{
actions->clear();
}
IplImage * img = 0; // Null image
if(_capture)
{
@@ -528,9 +523,9 @@ SMState * CameraVideo::takeImage()
getImageHeight() != (unsigned int)img->height)
{
// declare a destination IplImage object with correct size, depth and channels
IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()) ,
(int)(getImageHeight()) ),
img->depth, img->nChannels );
IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()),(int)(getImageHeight())),
img->depth,
img->nChannels );
//use cvResize to resize source to a destination image (linear interpolation)
cvResize(img, resampledImg);
@@ -541,11 +536,7 @@ SMState * CameraVideo::takeImage()
img = cvCloneImage(img);
}
if(img)
{
return new SMState(img);
}
return 0;
return img;
}
@@ -558,14 +549,14 @@ SMState * CameraVideo::takeImage()
// CameraDatabase
/////////////////////////
CameraDatabase::CameraDatabase(const std::string & path,
bool ignoreChildren,
bool loadActions,
float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, autoRestart, imageWidth, imageHeight),
_path(path),
_ignoreChildren(ignoreChildren),
_loadActions(loadActions),
_indexIter(_ids.begin()),
_dbDriver(0)
{
@@ -573,7 +564,7 @@ CameraDatabase::CameraDatabase(const std::string & path,
CameraDatabase::~CameraDatabase(void)
{
this->kill();
join(true);
if(_dbDriver)
{
_dbDriver->closeConnection();
@@ -614,37 +605,41 @@ bool CameraDatabase::init()
return true;
}
SMState * CameraDatabase::takeImage()
IplImage * CameraDatabase::takeImage(std::list<std::vector<float> > * actions)
{
if(actions)
{
actions->clear();
}
IplImage * img = 0;
if(_dbDriver && _indexIter != _ids.end())
{
if(_ignoreChildren)
// Get image
_dbDriver->getImage(*_indexIter, &img);
// Get actions from its previous neighbor
if(actions && _loadActions)
{
bool ignore = true;
while(img == 0 && _indexIter != _ids.end() && ignore)
if(*_indexIter-1 > 0)
{
ignore = false;
int loopId = 0;
if(_dbDriver->getLoopClosureId(*_indexIter, loopId))
NeighborsMultiMap neighbors;
_dbDriver->loadNeighbors(*_indexIter-1, neighbors);
for(NeighborsMultiMap::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(loopId == *_indexIter+1)
if(iter->first>*_indexIter-1 && iter->second.actions().size())
{
ignore = true;
}
else
{
_dbDriver->getImage(*_indexIter, &img);
*actions = iter->second.actions();
break;
}
}
++_indexIter;
if(actions->size() == 0)
{
UWARN("actions from previous %d to current %d are null", *_indexIter-1, *_indexIter);
}
}
}
else
{
_dbDriver->getImage(*_indexIter, &img);
++_indexIter;
}
++_indexIter;
}
else if(!_dbDriver)
{
@@ -672,27 +667,7 @@ SMState * CameraDatabase::takeImage()
img = resampledImg;
}
if(img)
{
SMState * smState = new SMState(img);
if(_dbDriver && _indexIter!=_ids.begin())
{
std::set<int>::iterator iter = _indexIter;
--iter;
std::map<int, std::list<std::vector<float> > > neighbors;
_dbDriver->loadNeighbors(*iter, neighbors);
for(std::map<int, std::list<std::vector<float> > >::iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
{
if(i->first > *iter && i->second.size())
{
smState->setActuators(i->second);
break;
}
}
}
return smState;
}
return 0;
return img;
}
} // namespace rtabmap
corelib/src/ColorTable.cpp
+67723
View File
File diff suppressed because it is too large Load Diff
corelib/src/ColorTable.h
+40
View File
@@ -0,0 +1,40 @@
#ifndef COLORTABLE_H
#define COLORTABLE_H
#include <vector>
namespace rtabmap
{
class ColorTable
{
public:
ColorTable(int size);
virtual ~ColorTable() {}
static unsigned char INDEXED_TABLE_8[24];
static unsigned char INDEXED_TABLE_16[48];
static unsigned char INDEXED_TABLE_32[96];
static unsigned char INDEXED_TABLE_64[192];
static unsigned char INDEXED_TABLE_128[384];
static unsigned char INDEXED_TABLE_256[768];
static unsigned char INDEXED_TABLE_512[1536];
static unsigned char INDEXED_TABLE_1024[3076];
static unsigned char INDEXED_TABLE_65536[196608];
int size() const {return _size;}
unsigned short getIndex(unsigned char r, unsigned char g, unsigned char b) const;
void getRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
unsigned short getNNIndex(unsigned char r, unsigned char g, unsigned char b) const;
void getNNRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
private:
int _size;
std::vector<unsigned short> _rgb2indexed;
unsigned char * _indexedTable;
};
} // namespace rtabmap
#endif // COLORTABLE_H
corelib/src/DBDriver.cpp
+121 -134
View File
@@ -19,7 +19,7 @@
#include "rtabmap/core/DBDriver.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "VWDictionary.h"
#include "utilite/UConversion.h"
#include "utilite/UMath.h"
@@ -32,6 +32,7 @@ namespace rtabmap {
DBDriver::DBDriver(const ParametersMap & parameters) :
_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
_imagesCompressed(Parameters::defaultDbImagesCompressed()),
_asyncWaiting(true),
_emptyTrashesTime(0)
{
@@ -40,7 +41,7 @@ DBDriver::DBDriver(const ParametersMap & parameters) :
DBDriver::~DBDriver()
{
this->kill();
join(true);
this->emptyTrashes();
}
@@ -55,24 +56,31 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
{
_minWordsToSave = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
{
_imagesCompressed = uStr2Bool((*iter).second.c_str());
}
}
void DBDriver::closeConnection()
{
this->kill();
UDEBUG("isRunning=%d", this->isRunning());
this->join(true);
UDEBUG("");
this->emptyTrashes();
_dbSafeAccessMutex.lock();
this->disconnectDatabaseQuery();
_dbSafeAccessMutex.unlock();
UDEBUG("");
}
bool DBDriver::openConnection(const std::string & url)
bool DBDriver::openConnection(const std::string & url, bool overwritten)
{
UDEBUG("");
_url = url;
_dbSafeAccessMutex.lock();
if(this->connectDatabaseQuery(url))
if(this->connectDatabaseQuery(url, overwritten))
{
this->start();
_dbSafeAccessMutex.unlock();
return true;
}
@@ -103,6 +111,7 @@ void DBDriver::mainLoop()
{
UDEBUG("");
this->emptyTrashes();
UDEBUG("");
this->kill(); // Do it only once
UDEBUG("");
}
@@ -243,19 +252,15 @@ bool DBDriver::getSignature(int signatureId, Signature ** s)
*s = 0;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::map<int, Signature*>::iterator i=_trashSignatures.begin(); i!=_trashSignatures.end();)
if(_trashSignatures.size())
{
if(i->first == signatureId)
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, Signature*>::iterator iter =_trashSignatures.find(signatureId);
if(iter != _trashSignatures.end())
{
*s = i->second;
_trashSignatures.erase(i++);
break;
}
else
{
++i;
*s = iter->second;
_trashSignatures.erase(iter);
}
}
}
@@ -278,15 +283,15 @@ bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
*vw = 0;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::map<int, VisualWord*>::iterator i=_trashVisualWords.begin(); i!=_trashVisualWords.end(); ++i)
if(_trashVisualWords.size())
{
if((*i).first == wordId)
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, VisualWord*>::iterator iter = _trashVisualWords.find(wordId);
if(iter != _trashVisualWords.end())
{
*vw = (*i).second;
_trashVisualWords.erase(i);
break;
*vw = iter->second;
_trashVisualWords.erase(iter);
}
}
}
@@ -308,7 +313,7 @@ bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
ULOGGER_DEBUG("");
std::list<KeypointSignature *> toSaveK;
std::list<Signature *> toSave;
std::list<Signature *> toUpdate;
if(this->isConnected() && signatures.size())
{
@@ -318,13 +323,9 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
toUpdate.push_back(*i);
}
else if((*i)->signatureType().compare("KeypointSignature") == 0)
{
toSaveK.push_back((KeypointSignature *)(*i));
}
else
{
ULOGGER_ERROR("Unknown signature type ?!?");
toSave.push_back(*i);
}
}
@@ -332,9 +333,9 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
this->updateQuery(toUpdate);
}
if(toSaveK.size())
if(toSave.size())
{
this->saveQuery(toSaveK);
this->saveQuery(toSave);
}
}
return false;
@@ -358,7 +359,7 @@ bool DBDriver::loadLastSignatures(std::list<Signature *> & signatures) const
return r;
}
bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures, bool onlyParents)
bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
{
UDEBUG("");
// look up in the trash before the database
@@ -376,15 +377,9 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
{
if(sIter->first == *iter)
{
if((onlyParents && sIter->second->getLoopClosureId() == 0) || !onlyParents)
{
signatures.push_back(sIter->second);
_trashSignatures.erase(sIter++);
}
else
{
++sIter;
}
signatures.push_back(sIter->second);
_trashSignatures.erase(sIter++);
valueFound = true;
break;
}
@@ -409,7 +404,64 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadKeypointSignaturesQuery(ids, signatures, onlyParents);
r = this->loadKeypointSignaturesQuery(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)
{
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;
}
@@ -429,23 +481,27 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
// look up in the trash before the database
std::list<int> ids = wordIds;
std::map<int, VisualWord*>::iterator wIter;
std::list<VisualWord *> puttedBack;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
if(_trashVisualWords.size())
{
wIter = _trashVisualWords.find(*iter);
if(wIter != _trashVisualWords.end())
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
{
//UDEBUG("put back word %d from trash", *iter);
vws.push_back(wIter->second);
_trashVisualWords.erase(wIter);
iter = ids.erase(iter);
}
else
{
++iter;
wIter = _trashVisualWords.find(*iter);
if(wIter != _trashVisualWords.end())
{
UDEBUG("put back word %d from trash", *iter);
puttedBack.push_back(wIter->second);
_trashVisualWords.erase(wIter);
iter = ids.erase(iter);
}
else
{
++iter;
}
}
}
}
@@ -456,10 +512,12 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
_dbSafeAccessMutex.lock();
r = this->loadWordsQuery(ids, vws);
_dbSafeAccessMutex.unlock();
uAppend(vws, puttedBack);
return r;
}
else if(vws.size())
else if(puttedBack.size())
{
uAppend(vws, puttedBack);
return true;
}
return false;
@@ -571,78 +629,27 @@ bool DBDriver::deleteUnreferencedWords() const
return false;
}
bool DBDriver::addNeighbor(int id, int newNeighbor, int oldNeighbor)
{
bool r = false;
Signature * s = 0;
_trashesMutex.lock();
s = uValue(_trashSignatures, id, s);
if(s)
{
std::list<std::vector<float> > actions = uValue(s->getNeighbors(), oldNeighbor, std::list<std::vector<float> >());
s->addNeighbor(newNeighbor, actions);
r = true;
}
_trashesMutex.unlock();
if(!r)
{
_dbSafeAccessMutex.lock();
r = this->addNeighborQuery(id, newNeighbor, oldNeighbor);
_dbSafeAccessMutex.unlock();
}
return r;
}
bool DBDriver::removeNeighbor(int id, int neighbor)
{
bool r = false;
Signature * s = 0;
_trashesMutex.lock();
s = uValue(_trashSignatures, id, s);
if(s)
{
s->removeNeighbor(neighbor);
r = true;
}
_trashesMutex.unlock();
if(!r)
{
r = executeNoResult("DELETE FROM Neighbor WHERE sid=" + uNumber2str(id) + " AND nid=" + uNumber2str(neighbor));
}
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getImage(int id, IplImage ** img) const
{
CvMat * compressed = 0;
_dbSafeAccessMutex.lock();
bool result = this->getImageCompressedQuery(id, &compressed);
if(compressed)
{
(*img) = cvDecodeImage(compressed, CV_LOAD_IMAGE_ANYCOLOR);
cvReleaseMat(&compressed);
}
bool result = this->getImageQuery(id, img);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors) const
bool DBDriver::getNeighborIds(int signatureId, std::list<int> & neighbors, bool onlyWithActions) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getNeighborIdsQuery(signatureId, neighbors);
r = this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::loadNeighbors(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const
bool DBDriver::loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const
{
bool r;
_dbSafeAccessMutex.lock();
@@ -662,21 +669,11 @@ bool DBDriver::getWeight(int signatureId, int & weight) const
}
//TODO Check also in the trash ?
bool DBDriver::getLoopClosureId(int signatureId, int & loopId) const
bool DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLoopClosureIdQuery(signatureId, loopId);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getImageCompressed(int id, CvMat ** compressed) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getImageCompressedQuery(id, compressed);
r = this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
_dbSafeAccessMutex.unlock();
return r;
}
@@ -721,16 +718,6 @@ bool DBDriver::getSurfNi(int signatureId, int & ni) const
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getChildrenIds(int signatureId, std::list<int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getChildrenIdsQuery(signatureId, ids);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const
{
bool r;
corelib/src/DBDriverSqlite3.cpp
+1373 -993
View File
File diff suppressed because it is too large Load Diff
corelib/src/DBDriverSqlite3.h
+29 -10
View File
@@ -32,13 +32,16 @@ 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);
void setCacheSize(unsigned int cacheSize);
void setSynchronous(int synchronous);
void setTempStore(int tempStore);
private:
virtual bool connectDatabaseQuery(const std::string & url);
virtual bool connectDatabaseQuery(const std::string & url, bool overwirtten = false);
virtual void disconnectDatabaseQuery();
virtual bool isConnectedQuery() const;
virtual long getMemoryUsedQuery() const; // In bytes
@@ -47,15 +50,13 @@ private:
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) const;
virtual bool getNeighborIdsQuery(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) const;
virtual bool getWeightQuery(int signatureId, int & weight) const;
virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const;
virtual bool addNeighborQuery(int id, int newNeighbor, int oldNeighbor) const;
virtual bool 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 KeypointSignature * ss) const;
virtual bool saveQuery(const std::list<KeypointSignature *> & signatures) const;
virtual bool saveQuery(const std::list<Signature *> & signatures) const;
// Load objects
virtual bool loadQuery(VWDictionary * dictionary) const;
@@ -63,18 +64,34 @@ private:
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 loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool onlyParents = false) 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, std::map<int, std::list<std::vector<float> > > & neighbors) const;
virtual bool loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const;
bool loadNeighborsQuery(std::list<Signature *> & signatures) const;
virtual bool getImageCompressedQuery(int id, CvMat ** compressed) const;
virtual bool getImageQuery(int id, IplImage ** image) const;
virtual bool getAllSignatureIdsQuery(std::set<int> & ids) const;
virtual bool getLastSignatureIdQuery(int & id) const;
virtual bool getLastVisualWordIdQuery(int & id) const;
virtual bool getSurfNiQuery(int signatureId, int & ni) const;
virtual bool getChildrenIdsQuery(int signatureId, std::list<int> & ids) const;
virtual bool getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int, int> & ids) const;
private:
std::string queryStepSignature() const;
std::string queryStepImage() const;
std::string queryStepNeighborLink() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
std::string queryStepSensors() const;
int stepSignature(sqlite3_stmt * ppStmt, const Signature * s) const;
int stepImage(sqlite3_stmt * ppStmt, int id, const IplImage * img) const;
int stepNeighborLink(sqlite3_stmt * ppStmt, int signatureId, const NeighborLink & n) 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;
int stepSensors(sqlite3_stmt * ppStmt, const SMSignature * s) const;
private:
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
@@ -83,6 +100,8 @@ private:
bool _dbInMemory;
unsigned int _cacheSize;
int _journalMode;
int _synchronous;
int _tempStore;
};
}
corelib/src/KeypointDescriptor.cpp
+97 -114
View File
@@ -31,72 +31,31 @@
namespace rtabmap {
KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
_childDescriptor(childDescriptor)
KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KeypointDescriptor::~KeypointDescriptor()
{
if(_childDescriptor)
{
delete _childDescriptor;
}
}
void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
{
if(_childDescriptor)
{
_childDescriptor->parseParameters(parameters);
}
}
std::list<std::vector<float> > KeypointDescriptor::generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
// see decorator pattern...
std::list<std::vector<float> > descriptors = this->_generateDescriptors(image, keypoints);
std::list<std::vector<float> > childDescriptors;
if(_childDescriptor)
{
childDescriptors = _childDescriptor->generateDescriptors(image, keypoints);
if(childDescriptors.size() && childDescriptors.size() == descriptors.size())
{
std::list<std::vector<float> >::iterator iterDesc = descriptors.begin();
std::list<std::vector<float> >::iterator iterChild = childDescriptors.begin();
for(; iterDesc!=descriptors.end(); ++iterDesc, ++iterChild)
{
iterDesc->insert(iterDesc->end(), iterChild->begin(), iterChild->end());
}
}
}
return descriptors;
}
void KeypointDescriptor::setChildDescriptor(KeypointDescriptor * childDescriptor)
{
if(_childDescriptor)
{
delete _childDescriptor;
}
_childDescriptor = childDescriptor;
}
//////////////////////////
//SURFDescriptor
//////////////////////////
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
KeypointDescriptor(parameters, childDescriptor)
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters)
{
_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
_surf.extended = Parameters::defaultSURFExtended();
_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
_surf.nOctaves = Parameters::defaultSURFOctaves();
_params.hessianThreshold = Parameters::defaultSURFHessianThreshold();
_params.extended = Parameters::defaultSURFExtended();
_params.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
_params.nOctaves = Parameters::defaultSURFOctaves();
_params.upright = Parameters::defaultSURFUpright();
_gpuVersion = Parameters::defaultSURFGpuVersion();
_upright = Parameters::defaultSURFUpright();
this->parseParameters(parameters);
}
@@ -109,35 +68,35 @@ void SURFDescriptor::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_surf.extended = uStr2Bool((*iter).second.c_str());
_params.extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_surf.hessianThreshold = std::atof((*iter).second.c_str()); // is it needed for the descriptor?
_params.hessianThreshold = std::atof((*iter).second.c_str()); // is it needed for the descriptor?
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_surf.nOctaveLayers = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
_params.nOctaveLayers = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_surf.nOctaves = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
_params.nOctaves = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_params.upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
cv::Mat SURFDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
std::list<std::vector<float> > descriptors;
cv::Mat descriptors;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
@@ -159,33 +118,35 @@ std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplIma
{
img = cv::Mat(image);
}
cv::Mat mask;
std::vector<cv::KeyPoint> k = uListToVector(keypoints);
std::vector<float> d;
#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(_surf.hessianThreshold, _surf.nOctaves, _surf.nOctaveLayers, _surf.extended, 0.01f, _upright);
surfGpu.uploadKeypoints(k, 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
{
_surf(img, mask, k, d, true); // Opencv surf descriptors
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
extractor.compute(img, keypoints, descriptors);
}
#else
_surf(img, mask, k, d, true); // Opencv surf descriptors
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
extractor.compute(img, keypoints, descriptors);
#endif
unsigned int dim = _surf.descriptorSize();
for(unsigned int i=0; i<d.size(); i+=dim)
{
descriptors.push_back(std::vector<float>(d.begin()+i, d.begin()+i+dim));
}
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
@@ -196,8 +157,8 @@ std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplIma
//////////////////////////
//SIFTDescriptor
//////////////////////////
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
KeypointDescriptor(parameters, childDescriptor)
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters)
{
this->parseParameters(parameters);
}
@@ -212,10 +173,10 @@ void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
KeypointDescriptor::parseParameters(parameters);
}
std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
cv::Mat SIFTDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
std::list<std::vector<float> > descriptors;
cv::Mat descriptors;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
@@ -237,17 +198,8 @@ std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplIma
{
img = cv::Mat(image);
}
cv::Mat mask;
std::vector<cv::KeyPoint> k = uListToVector(keypoints);
cv::Mat d;
cv::SIFT sift(_commonParams, cv::SIFT::DetectorParams(), _descriptorParams);
sift(img, mask, k, d, true); // Opencv surf descriptors
unsigned int dim = sift.descriptorSize();
//ULOGGER_DEBUG("row=%d, col=%d, type=%d (float=%d)", d.rows, d.cols, d.type(), CV_32F);
for(int i=0; i<d.rows; ++i)
{
descriptors.push_back(std::vector<float>(d.ptr<float>(i), d.ptr<float>(i)+dim));
}
cv::SiftDescriptorExtractor extractor(_descriptorParams, _commonParams);
extractor.compute(img, keypoints, descriptors);
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
@@ -256,35 +208,60 @@ std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplIma
}
//////////////////////////
//LaplacianDescriptor
//BRIEFDescriptor
//////////////////////////
LaplacianDescriptor::LaplacianDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
KeypointDescriptor(parameters, childDescriptor)
BRIEFDescriptor::BRIEFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_size(Parameters::defaultBRIEFSize())
{
this->parseParameters(parameters);
}
LaplacianDescriptor::~LaplacianDescriptor()
BRIEFDescriptor::~BRIEFDescriptor()
{
}
void LaplacianDescriptor::parseParameters(const ParametersMap & parameters)
void BRIEFDescriptor::parseParameters(const ParametersMap & parameters)
{
// No parameter...
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kBRIEFSize())) != parameters.end())
{
_size = std::atoi((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
std::list<std::vector<float> > LaplacianDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
cv::Mat BRIEFDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
std::list<std::vector<float> > descriptors;
//create descriptors...
for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
cv::Mat descriptors;
if(!image)
{
std::vector<float> laplacian(1);
laplacian[0] = uSign(key->response);
descriptors.push_back(laplacian);
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// BRIEF support only grayscale images ?
IplImage * imageGrayScale = 0;
if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
{
imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(imageGrayScale)
{
img = cv::Mat(imageGrayScale);
}
else
{
img = cv::Mat(image);
}
cv::BriefDescriptorExtractor brief(_size);
brief.compute(img, keypoints, descriptors);
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
}
return descriptors;
}
@@ -292,8 +269,8 @@ std::list<std::vector<float> > LaplacianDescriptor::_generateDescriptors(const I
//////////////////////////
//ColorDescriptor
//////////////////////////
ColorDescriptor::ColorDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
KeypointDescriptor(parameters, childDescriptor)
ColorDescriptor::ColorDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters)
{
this->parseParameters(parameters);
}
@@ -308,10 +285,10 @@ void ColorDescriptor::parseParameters(const ParametersMap & parameters)
KeypointDescriptor::parseParameters(parameters);
}
std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
cv::Mat ColorDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
std::list<std::vector<float> > descriptors;
cv::Mat descriptors;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
@@ -335,7 +312,9 @@ std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplIm
}
//create descriptors...
for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
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
@@ -380,11 +359,11 @@ std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplIm
}
}
}
for(int i=0; i<6; ++i)
for(int j=0; j<6; ++j)
{
d[i] /= 255; // Normalize between 0 and 1
descriptors.at<float>(i,j) = d[j] / 255; // Normalize between 0 and 1
}
descriptors.push_back(std::vector<float>(d, d + sizeof(d) / sizeof(float)));
++i;
}
if(imageConverted)
@@ -409,8 +388,8 @@ void ColorDescriptor::getCircularROI(int R, std::vector<int> & RxV) const
//////////////////////////
//HueDescriptor
//////////////////////////
HueDescriptor::HueDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
ColorDescriptor(parameters, childDescriptor)
HueDescriptor::HueDescriptor(const ParametersMap & parameters) :
ColorDescriptor(parameters)
{
this->parseParameters(parameters);
}
@@ -425,10 +404,10 @@ void HueDescriptor::parseParameters(const ParametersMap & parameters)
KeypointDescriptor::parseParameters(parameters);
}
std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
cv::Mat HueDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
std::list<std::vector<float> > descriptors;
cv::Mat descriptors;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
@@ -452,7 +431,9 @@ std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImag
}
//create descriptors...
for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
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;
@@ -512,7 +493,9 @@ std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImag
r = float(img(center.y+dyd, center.x+dxd)[2]) / 255.0f;
d[1] = rgb2hue(r, g, b);
descriptors.push_back(std::vector<float>(d, d + sizeof(d) / sizeof(float)));
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*2], 2*sizeof(float));
++i;
}
if(imageConverted)
corelib/src/KeypointDetector.cpp
+129 -67
View File
@@ -60,10 +60,10 @@ void KeypointDetector::parseParameters(const ParametersMap & parameters)
}
}
std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * image)
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * image)
{
ULOGGER_DEBUG("");
std::list<cv::KeyPoint> keypoints;
std::vector<cv::KeyPoint> keypoints;
if(image)
{
UTimer timer;
@@ -97,28 +97,40 @@ std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * ima
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, std::list<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
for(std::list<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
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::list<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
hessianMap.insert(std::pair<float, std::vector<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
}
// Remove them from the signature
int removed = 0;
unsigned int stopIndex = hessianMap.size()-_wordsPerImageTarget;
std::multimap<float, std::list<cv::KeyPoint>::iterator>::iterator iter = hessianMap.begin();
for(unsigned int k=0; k < stopIndex && iter!=hessianMap.end(); ++k, ++iter)
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)
{
keypoints.erase(iter->second);
++removed;
kptsTmp[k] = *iter->second;
// Adjust keypoint position to raw image
kptsTmp[k].pt.x += roi.x;
kptsTmp[k].pt.y += roi.y;
}
if(iter->first!=0)
{
_adaptiveResponseThr = iter->first;
}
keypoints = kptsTmp;
ULOGGER_DEBUG("%d keypoints removed, (kept %d)", removed, keypoints.size());
}
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
{
@@ -133,12 +145,14 @@ std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * ima
ULOGGER_DEBUG("new _adaptiveResponseThr=%f", _adaptiveResponseThr);
ULOGGER_DEBUG("adjusting hessian threshold time = %f s", timer.ticks());
}
// Adjust keypoint position to raw image
for(std::list<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
else if(roi.x || roi.y)
{
iter->pt.x += roi.x;
iter->pt.y += 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
@@ -232,14 +246,14 @@ cv::Rect KeypointDetector::computeRoi(const IplImage * image) const
SURFDetector::SURFDetector(const ParametersMap & parameters) :
KeypointDetector(parameters)
{
_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
_surf.extended = Parameters::defaultSURFExtended();
_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
_surf.nOctaves = Parameters::defaultSURFOctaves();
_params.hessianThreshold = Parameters::defaultSURFHessianThreshold();
_params.extended = Parameters::defaultSURFExtended();
_params.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
_params.nOctaves = Parameters::defaultSURFOctaves();
_gpuVersion = Parameters::defaultSURFGpuVersion();
_upright = Parameters::defaultSURFUpright();
_params.upright = Parameters::defaultSURFUpright();
this->parseParameters(parameters);
this->setAdaptiveResponseThr(_surf.hessianThreshold);
this->setAdaptiveResponseThr(_params.hessianThreshold);
}
SURFDetector::~SURFDetector()
@@ -251,24 +265,24 @@ void SURFDetector::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_surf.extended = uStr2Bool((*iter).second.c_str());
_params.extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_surf.hessianThreshold = std::atof((*iter).second.c_str());
this->setAdaptiveResponseThr(_surf.hessianThreshold);
_params.hessianThreshold = std::atof((*iter).second.c_str());
this->setAdaptiveResponseThr(_params.hessianThreshold);
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_surf.nOctaveLayers = std::atoi((*iter).second.c_str());
_params.nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_surf.nOctaves = std::atoi((*iter).second.c_str());
_params.nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_surf.nOctaves = std::atoi((*iter).second.c_str());
_params.nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
@@ -276,15 +290,15 @@ void SURFDetector::parseParameters(const ParametersMap & parameters)
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
_params.upright = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::list<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::list<cv::KeyPoint> keypoints;
std::vector<cv::KeyPoint> keypoints;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
@@ -307,32 +321,31 @@ std::list<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image,
img = cv::Mat(image);
}
cv::SURF surf = _surf;
CvSURFParams params = _params;
if(this->isUsingAdaptiveResponseThr())
{
surf.hessianThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
params.hessianThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
}
cv::Mat imgRoi(img, roi);
std::vector<cv::KeyPoint> k;
#if OPENCV_SURF_GPU
if(_gpuVersion )
{
cv::gpu::GpuMat imgGpu(imgRoi);
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(surf.hessianThreshold, surf.nOctaves, surf.nOctaveLayers, surf.extended, 0.01f, _upright);
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, k);
surfGpu.downloadKeypoints(keypointsGpu, keypoints);
}
else
{
surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
detector.detect(imgRoi, keypoints);
}
#else
surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
detector.detect(imgRoi, keypoints);
#endif
keypoints = uVectorToList(k);
if(imageGrayScale)
{
@@ -372,10 +385,10 @@ void SIFTDetector::parseParameters(const ParametersMap & parameters)
KeypointDetector::parseParameters(parameters);
}
std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::list<cv::KeyPoint> keypoints;
std::vector<cv::KeyPoint> keypoints;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
@@ -403,13 +416,10 @@ std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image,
{
detectorParam.threshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
}
cv::Mat mask;
cv::SIFT sift(_commonParams, detectorParam);
cv::SiftFeatureDetector detector(detectorParam, _commonParams);
cv::Mat imgRoi(img, roi);
std::vector<cv::KeyPoint> k;
sift(imgRoi, mask, k); // Opencv surf keypoints
keypoints = uVectorToList(k);
detector.detect(imgRoi, keypoints); // Opencv surf keypoints
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
@@ -424,13 +434,13 @@ std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image,
StarDetector::StarDetector(const ParametersMap & parameters) :
KeypointDetector(parameters)
{
_star.lineThresholdBinarized = Parameters::defaultStarLineThresholdBinarized();
_star.lineThresholdProjected = Parameters::defaultStarLineThresholdProjected();
_star.maxSize = Parameters::defaultStarMaxSize();
_star.responseThreshold = Parameters::defaultStarResponseThreshold();
_star.suppressNonmaxSize = Parameters::defaultStarSuppressNonmaxSize();
_params.lineThresholdBinarized = Parameters::defaultStarLineThresholdBinarized();
_params.lineThresholdProjected = Parameters::defaultStarLineThresholdProjected();
_params.maxSize = Parameters::defaultStarMaxSize();
_params.responseThreshold = Parameters::defaultStarResponseThreshold();
_params.suppressNonmaxSize = Parameters::defaultStarSuppressNonmaxSize();
this->parseParameters(parameters);
this->setAdaptiveResponseThr(_star.responseThreshold);
this->setAdaptiveResponseThr(_params.responseThreshold);
}
StarDetector::~StarDetector()
@@ -443,32 +453,32 @@ void StarDetector::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kStarLineThresholdBinarized())) != parameters.end())
{
_star.lineThresholdBinarized = std::atoi((*iter).second.c_str());
_params.lineThresholdBinarized = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarLineThresholdProjected())) != parameters.end())
{
_star.lineThresholdProjected = std::atoi((*iter).second.c_str());
_params.lineThresholdProjected = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarMaxSize())) != parameters.end())
{
_star.maxSize = std::atoi((*iter).second.c_str());
_params.maxSize = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarResponseThreshold())) != parameters.end())
{
_star.responseThreshold = int(std::atof((*iter).second.c_str()));
this->setAdaptiveResponseThr(_star.responseThreshold);
_params.responseThreshold = int(std::atof((*iter).second.c_str()));
this->setAdaptiveResponseThr(_params.responseThreshold);
}
if((iter=parameters.find(Parameters::kStarSuppressNonmaxSize())) != parameters.end())
{
_star.suppressNonmaxSize = std::atoi((*iter).second.c_str());
_params.suppressNonmaxSize = std::atoi((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::list<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::list<cv::KeyPoint> keypoints;
std::vector<cv::KeyPoint> keypoints;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
@@ -476,20 +486,72 @@ std::list<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image,
}
cv::Mat img(image);
cv::Mat mask;
// TODO More testing needed with the star detector, NN search distance must be changed to 0.8
//find keypoints with the star detector
cv::StarDetector star = _star;
CvStarDetectorParams params = _params;
if(this->isUsingAdaptiveResponseThr())
{
star.responseThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
params.responseThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
}
// Get keypoints with the star detector
cv::Mat imgRoi(img, roi);
std::vector<cv::KeyPoint> k;
star(imgRoi, k);
keypoints = uVectorToList(k);
cv::StarFeatureDetector detector(params);
detector.detect(imgRoi, keypoints);
return keypoints;
}
//////////////////////////
//FastDetector
//////////////////////////
FASTDetector::FASTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_threshold(Parameters::defaultFASTThreshold()),
_nonmaxSuppression(Parameters::defaultFASTNonmaxSuppression())
{
this->parseParameters(parameters);
this->setAdaptiveResponseThr(_threshold);
}
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 IplImage * image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image)
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
cv::Mat img(image);
cv::Mat imgRoi(img, roi);
int threshold = _threshold;
if(this->isUsingAdaptiveResponseThr())
{
threshold = (int)this->getAdaptiveResponseThr(); // use the adaptive threshold
}
cv::FastFeatureDetector fast(threshold, _nonmaxSuppression);
// Get keypoints with the fast detector
fast.detect(imgRoi, keypoints);
return keypoints;
}
corelib/src/KeypointMemory.cpp
+143 -225
View File
@@ -19,7 +19,7 @@
#include "KeypointMemory.h"
#include "VWDictionary.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "utilite/UtiLite.h"
#include "rtabmap/core/Parameters.h"
@@ -43,7 +43,6 @@ KeypointMemory::KeypointMemory(const ParametersMap & parameters) :
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
_sensorStateOnly(Parameters::defaultKpSensorStateOnly()),
_tfIdfNormalized(Parameters::defaultKpTfIdfNormalized())
{
_vwd = new VWDictionary(parameters);
@@ -95,11 +94,6 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
_parallelized = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpSensorStateOnly())) != parameters.end())
{
_sensorStateOnly = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpTfIdfNormalized())) != parameters.end())
{
_tfIdfNormalized = uStr2Bool((*iter).second.c_str());
@@ -133,6 +127,9 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
case kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
break;
case kDetectorFast:
_keypointDetector = new FASTDetector(parameters);
break;
case kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
@@ -160,20 +157,17 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
}
switch(descriptorStrategy)
{
case kDescriptorColorSurf:
// see decorator pattern...
_keypointDescriptor = new ColorDescriptor(parameters, new SURFDescriptor(parameters));
break;
case kDescriptorLaplacianSurf:
// see decorator pattern...
_keypointDescriptor = new LaplacianDescriptor(parameters, new SURFDescriptor(parameters));
break;
case kDescriptorSift:
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case kDescriptorHueSurf:
// see decorator pattern...
_keypointDescriptor = new HueDescriptor(parameters, new SURFDescriptor(parameters));
case kDescriptorBrief:
_keypointDescriptor = new BRIEFDescriptor(parameters);
break;
case kDescriptorColor:
_keypointDescriptor = new ColorDescriptor(parameters);
break;
case kDescriptorHue:
_keypointDescriptor = new HueDescriptor(parameters);
break;
case kDescriptorSurf:
default:
@@ -207,7 +201,7 @@ KeypointMemory::DetectorStrategy KeypointMemory::detectorStrategy() const
bool KeypointMemory::init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
{
ULOGGER_DEBUG("KeypointMemory::init()");
UDEBUG("");
// This will open a connection to the database,
// this calls also clear()
bool success = Memory::init(dbDriverName, dbUrl, dbOverwritten, parameters);
@@ -217,8 +211,8 @@ bool KeypointMemory::init(const std::string & dbDriverName, const std::string &
{
UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary...")));
_dbDriver->load(_vwd);
ULOGGER_DEBUG("%d words loaded!", _vwd->getVisualWords().size());
UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary, done! (") + uNumber2str(int(_vwd->getVisualWords().size())) + " loaded)"));
UDEBUG("%d words loaded!", _vwd->getVisualWords().size());
UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary, done! (") + uNumber2Str(int(_vwd->getVisualWords().size())) + " loaded)"));
}
// Enable loaded signatures
@@ -290,7 +284,7 @@ void KeypointMemory::clear()
if(_dbDriver)
{
_dbDriver->kill();
_dbDriver->join(true);
cleanUnusedWords();
_dbDriver->emptyTrashes();
@@ -302,7 +296,7 @@ void KeypointMemory::clear()
// all signatures with the old word to the active one...
//_dbDriver->changeWordsRef(_wordRefsToChange);
//remove old values
_dbDriver->deleteUnreferencedWords();
//_dbDriver->deleteUnreferencedWords();
// _dbDriver->commit();
//}
ULOGGER_DEBUG("");
@@ -328,40 +322,6 @@ void KeypointMemory::preUpdate()
}
}
// TODO Really useful?
/*void KeypointMemory::postUpdate()
{
ULOGGER_DEBUG("");
// Detect if the last signature is a bad one. If the signature has less than 15% of
// the average words/signature.
KeypointSignature * ss = dynamic_cast<KeypointSignature *>(this->_getLastSignature());
float ratio = 0;
if(ss)
{
ratio = float(uUniqueKeys(ss->getWords()).size()) / float(ss->getWords().size());
}
int nbCommonWords = 0;
ULOGGER_DEBUG("_workingMem.size() = %d, _stMem.size()=%d", _workingMem.size(), _stMem.size());
int treeSize= _workingMem.size() + _stMem.size();//Don't count the virtual place
if(treeSize > 0)
{
nbCommonWords = _vwd->getTotalActiveReferences() / treeSize;
}
ULOGGER_DEBUG("ratio=%f, treeSize=%d, nbCommonWords=%d", ratio, treeSize, nbCommonWords);
if(//(ratio < _badSignRatio) ||
(nbCommonWords && ss && ss->getWords().size() < _badSignRatio * nbCommonWords))
{
ULOGGER_WARN("id %d is a bad signature", ss->id());
this->disableWordsRef(ss->id());
ss->removeAllWords();
}
}*/
// NON class method! only used in merge()
std::multimap<int, cv::KeyPoint> getMostDescriptiveWords(const std::multimap<int, cv::KeyPoint> & words, int max, const std::set<int> & ignoredIds)
{
@@ -398,49 +358,57 @@ std::multimap<int, cv::KeyPoint> getMostDescriptiveWords(const std::multimap<int
return mostDescriptiveWords;
}
void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrategy s)
std::multimap<int, cv::KeyPoint> KeypointMemory::getWords(int signatureId) const
{
// The signatures must be KeypointSignature
const KeypointSignature * sFrom = dynamic_cast<const KeypointSignature *>(from);
KeypointSignature * sTo = dynamic_cast<KeypointSignature *>(to);
UTimer timer;
timer.start();
if(sFrom && sTo)
std::multimap<int, cv::KeyPoint> words;
if(signatureId>0)
{
if(s == kUseOnlyFromMerging)
const Signature * s = this->getSignature(signatureId);
if(s)
{
this->disableWordsRef(sTo->id());
sTo->setWords(sFrom->getWords());
const KeypointSignature * ks = dynamic_cast<const KeypointSignature*>(s);
if(ks)
{
words = ks->getWords();
}
}
else if(_dbDriver)
{
std::list<int> ids;
ids.push_back(signatureId);
std::list<Signature *> signatures;
_dbDriver->loadKeypointSignatures(ids, signatures);
if(signatures.size())
{
const KeypointSignature * ks = dynamic_cast<const KeypointSignature*>(signatures.front());
if(ks)
{
words = ks->getWords();
}
}
for(std::list<Signature *>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
{
delete *iter;
}
}
}
return words;
}
std::list<int> id;
id.push_back(sTo->id());
this->enableWordsRef(id);
// Set old image to new merged signature
sTo->setImage(sFrom->getImage());
}
else if(s == kUseOnlyDestMerging)
{
// do nothing... already "merged"
}
std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore)
{
if(!_tfIdfLikelihoodUsed)
{
return Memory::computeLikelihood(signature, ids, maximumScore);
}
else
{
ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
}
ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
}
std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signature, const std::set<int> & signatureIds) const
{
//return Memory::computeLikelihood(signature, signatureIds);
// TODO cleanup , old way...
if(_tfIdfLikelihoodUsed)
{
// TODO cleanup , old way...
UTimer timer;
timer.start();
std::map<int, float> likelihood;
std::map<int, float> calculatedWordsRatio;
maximumScore = 0;
const KeypointSignature * newSurf = dynamic_cast<const KeypointSignature *>(signature);
if(!newSurf)
@@ -448,61 +416,34 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
ULOGGER_ERROR("The signature is not a KeypointSignature");
return likelihood; // Must be a KeypointSignature *
}
if(signatureIds.size() == 0)
else if(ids.empty())
{
const std::map<int, int> & wm = this->getWorkingMem();
for(std::map<int, int>::const_iterator iter = wm.begin(); iter!=wm.end(); ++iter)
{
likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
if(_tfIdfNormalized)
{
const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(iter->first));
float wordsCountRatio = -1; // default invalid
if(s)
{
if(s->getWords().size() > newSurf->getWords().size())
{
wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
}
else if(newSurf->getWords().size())
{
wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
}
calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
}
else
{
calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
}
}
}
UWARN("ids list is empty");
return likelihood;
}
else
for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
for(std::set<int>::const_iterator i=signatureIds.begin(); i != signatureIds.end(); ++i)
likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, 0));
if(_tfIdfNormalized)
{
likelihood.insert(likelihood.end(), std::pair<int, float>(*i, 0));
if(_tfIdfNormalized)
const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(*iter));
float wordsCountRatio = -1; // default invalid
if(s)
{
const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(*i));
float wordsCountRatio = -1; // default invalid
if(s)
if(s->getWords().size() > newSurf->getWords().size())
{
if(s->getWords().size() > newSurf->getWords().size())
{
wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
}
else if(newSurf->getWords().size())
{
wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
}
calculatedWordsRatio.insert(std::pair<int, float>(*i, wordsCountRatio));
wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
}
else
else if(newSurf->getWords().size())
{
calculatedWordsRatio.insert(std::pair<int, float>(*i, wordsCountRatio));
wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
}
calculatedWordsRatio.insert(std::pair<int, float>(*iter, wordsCountRatio));
}
else
{
calculatedWordsRatio.insert(std::pair<int, float>(*iter, wordsCountRatio));
}
}
}
@@ -518,7 +459,7 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
const VisualWord * vw;
float normalizationRatio;
N = likelihood.size();
N = this->getSignatures().size();
if(N)
{
@@ -572,15 +513,12 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
}
}
}
maximumScore = log(N);
}
ULOGGER_DEBUG("compute likelihood... %f s", timer.ticks());
ULOGGER_DEBUG("compute likelihood, maximumScore=%f... %f s", maximumScore, timer.ticks());
return likelihood;
}
else
{
return Memory::computeLikelihood(signature, signatureIds);
}
}
@@ -599,11 +537,34 @@ int KeypointMemory::getNi(int signatureId) const
return ni;
}
void KeypointMemory::copyData(const Signature * from, Signature * to)
{
// The signatures must be KeypointSignature
const KeypointSignature * sFrom = dynamic_cast<const KeypointSignature *>(from);
KeypointSignature * sTo = dynamic_cast<KeypointSignature *>(to);
UTimer timer;
timer.start();
if(sFrom && sTo)
{
this->disableWordsRef(sTo->id());
sTo->setWords(sFrom->getWords());
std::list<int> id;
id.push_back(sTo->id());
this->enableWordsRef(id);
}
else
{
ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
}
ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
}
class PreUpdateThread : public UThreadNode
{
public:
PreUpdateThread(VWDictionary * vwp) : _vwp(vwp) {}
~PreUpdateThread() {}
virtual ~PreUpdateThread() {}
private:
void mainLoop() {
if(_vwp)
@@ -621,8 +582,8 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
UTimer timer;
timer.start();
std::list<cv::KeyPoint> keypoints;
std::list<std::vector<float> > descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
const IplImage * image = 0;
if(smState)
@@ -657,7 +618,7 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
}
else
{
if(smState->getSensors().size() >= _badSignRatio * nbCommonWords)
if(smState->getSensors().rows >= _badSignRatio * nbCommonWords)
{
descriptors = smState->getSensors();
keypoints = smState->getKeypoints();
@@ -672,62 +633,29 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
}
std::list<int> wordIds;
if(descriptors.size())
if(descriptors.rows)
{
unsigned int descriptorSize = descriptors.begin()->size();
if(_parallelized)
{
ULOGGER_DEBUG("time descriptor and memory update (%d of size=%d) = %fs", (int)descriptors.size(), (int)descriptorSize, timer.ticks());
ULOGGER_DEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, timer.ticks());
}
else
{
ULOGGER_DEBUG("time descriptor (%d of size=%d) = %fs", (int)descriptors.size(), (int)descriptorSize, timer.ticks());
ULOGGER_DEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, timer.ticks());
}
//append actuators
if(!_sensorStateOnly && smState->getActuators().size())
{
const std::list<std::vector<float> > & actuators = smState->getActuators();
unsigned int actuatorSize = actuators.begin()->size();
if(actuatorSize > descriptorSize)
{
UERROR("Actuator's size (%d) is larger than descriptor size (%d)", actuatorSize, descriptorSize);
}
for(std::list<std::vector<float> >::const_iterator iter = actuators.begin(); iter!=actuators.end(); ++iter)
{
std::vector<float> descriptor(descriptorSize);
// normalize actuator values
std::vector<float> actuatorNormalized = uNormalize(*iter);
for(unsigned int i=0; i<descriptorSize; ++i)
{
if(i<actuatorSize)
{
descriptor[i] = actuatorNormalized[i];
}
else
{
descriptor[i] = 0;
}
}
descriptors.push_back(descriptor);
}
ULOGGER_DEBUG("time setup actuators (%d of length %d) like descriptors %fs", (int)actuators.size(), (int)actuatorSize, timer.ticks());
}
wordIds = _vwd->addNewWords(descriptors, descriptorSize, id);
wordIds = _vwd->addNewWords(descriptors, id);
ULOGGER_DEBUG("time addNewWords %fs", timer.ticks());
}
else
else if(id>0)
{
ULOGGER_WARN("id %d is a bad signature", id);
UDEBUG("id %d is a bad signature", id);
}
std::multimap<int, cv::KeyPoint> words;
if(wordIds.size() > 0)
{
std::list<cv::KeyPoint>::iterator kpIter = keypoints.begin();
std::vector<cv::KeyPoint>::iterator kpIter = keypoints.begin();
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
{
if(kpIter != keypoints.end())
@@ -737,6 +665,7 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
}
else
{
UWARN("Words (%d) and keypoints(%d) are not the same size ?!?", (int)wordIds.size(), (int)keypoints.size());
words.insert(std::pair<int, cv::KeyPoint >(*iter, cv::KeyPoint()));
}
}
@@ -787,7 +716,7 @@ void KeypointMemory::disableWordsRef(int signatureId)
void KeypointMemory::cleanUnusedWords()
{
ULOGGER_DEBUG("");
UINFO("");
if(_vwd->isIncremental())
{
std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
@@ -799,7 +728,7 @@ void KeypointMemory::cleanUnusedWords()
for(unsigned int i=0; i<removedWords.size(); ++i)
{
if(_dbDriver)
if(_dbDriver && !removedWords[i]->isSaved())
{
_dbDriver->asyncSave(removedWords[i]);
}
@@ -809,7 +738,7 @@ void KeypointMemory::cleanUnusedWords()
}
}
}
ULOGGER_DEBUG("%d words removed...", removedWords.size());
UINFO("%d words removed...", removedWords.size());
}
}
@@ -834,23 +763,9 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
//Find words in the signature which they are not in the current dictionary
for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
{
if(_vwd->getWord(*k) == 0)
if(_vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
{
//std::map<int,int>::iterator iter = _wordRefsToChange.find(*k);
//if(iter != _wordRefsToChange.end())
//{
// ss->changeWordsRef(iter->first, iter->second);
// uniqueKeys.push_back(iter->second);
//}
//else
if(oldWordIds.find(*k) == oldWordIds.end())
{
oldWordIds.insert(oldWordIds.end(), *k);
}
else
{
//UDEBUG("*k=%d", *k);
}
oldWordIds.insert(oldWordIds.end(), *k);
}
}
}
@@ -862,7 +777,8 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
std::list<VisualWord *> vws;
if(oldWordIds.size() && _dbDriver)
{
_dbDriver->loadWords(std::list<int>(oldWordIds.begin(), oldWordIds.end()), vws); // get the descriptors
// get the descriptors
_dbDriver->loadWords(std::list<int>(oldWordIds.begin(), oldWordIds.end()), vws);
}
ULOGGER_DEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks());
@@ -879,11 +795,11 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
{
//ULOGGER_DEBUG("Match found %d with %d", (*iterVws)->id(), vwActiveIds[i]);
refsToChange.insert(refsToChange.end(), std::pair<int, int>((*iterVws)->id(), vwActiveIds[i]));
if((*iterVws)->isSaved() || !_dbDriver)
if((*iterVws)->isSaved())
{
delete (*iterVws);
}
else
else if(_dbDriver)
{
_dbDriver->asyncSave(*iterVws);
}
@@ -891,7 +807,7 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
else
{
//add to dictionary
_vwd->addWord(*iterVws);
_vwd->addWord(*iterVws); // take ownership
}
++i;
}
@@ -930,7 +846,7 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
ULOGGER_DEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks());
}
int KeypointMemory::forget(const std::list<int> & ignoredIds)
int KeypointMemory::forget(const std::set<int> & ignoredIds)
{
ULOGGER_DEBUG("");
int signaturesRemoved = 0;
@@ -946,12 +862,20 @@ int KeypointMemory::forget(const std::list<int> & ignoredIds)
// dictionary to respect the limit.
while(wordsRemoved < newWords)
{
KeypointSignature * s = dynamic_cast<KeypointSignature *>(this->getRemovableSignature(ignoredIds));
if(s)
std::list<Signature *> signatures = this->getRemovableSignatures(1, ignoredIds);
if(signatures.size())
{
++signaturesRemoved;
this->moveToTrash(s);
wordsRemoved = _vwd->getUnusedWordsSize();
KeypointSignature * s = dynamic_cast<KeypointSignature *>(signatures.front());
if(s)
{
++signaturesRemoved;
this->moveToTrash(s);
wordsRemoved = _vwd->getUnusedWordsSize();
}
else
{
break;
}
}
else
{
@@ -968,7 +892,7 @@ int KeypointMemory::forget(const std::list<int> & ignoredIds)
return signaturesRemoved;
}
int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched)
std::set<int> KeypointMemory::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
@@ -977,7 +901,6 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
ULOGGER_DEBUG("");
UTimer timer;
std::list<int> idsToLoad;
unsigned int touched = 0;
std::map<int, int>::iterator wmIter;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
@@ -985,16 +908,10 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
{
if(!maxLoaded || idsToLoad.size() < maxLoaded)
{
//When loaded from the long-term memory, the signature
// is automatically added on top of the working memory
idsToLoad.push_back(*i);
UINFO("Loading location %d from database...", *i);
}
}
else if(touched < maxTouched)
{
this->touch(*i);
}
++touched;
}
ULOGGER_DEBUG("idsToLoad = %d", idsToLoad.size());
@@ -1002,8 +919,9 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
std::list<Signature *> reactivatedSigns;
if(_dbDriver)
{
_dbDriver->loadKeypointSignatures(idsToLoad, reactivatedSigns, true);
_dbDriver->loadKeypointSignatures(idsToLoad, reactivatedSigns);
}
timeDbAccess = timer.getElapsedTime();
std::list<int> idsLoaded;
for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
{
@@ -1013,7 +931,7 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
}
this->enableWordsRef(idsLoaded);
ULOGGER_DEBUG("time = %fs", timer.ticks());
return reactivatedSigns.size();
return std::set<int>(idsToLoad.begin(), idsToLoad.end());
}
void KeypointMemory::moveToTrash(Signature * s)
corelib/src/KeypointMemory.h
+9 -7
View File
@@ -23,6 +23,7 @@
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "Memory.h"
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap {
@@ -34,8 +35,8 @@ class KeypointDescriptor;
class RTABMAP_EXP KeypointMemory : public Memory
{
public:
enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorUndef};
enum DescriptorStrategy {kDescriptorSurf, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef};
enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
enum DescriptorStrategy {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
public:
KeypointMemory(const ParametersMap & parameters = ParametersMap());
@@ -43,9 +44,9 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::set<int> & signatureIds = std::set<int>()) const;
virtual int forget(const std::list<int> & ignoredIds = std::list<int>());
virtual int reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched);
virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore);
virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
virtual void dumpMemory(std::string directory) const;
virtual void dumpSignatures(const char * fileNameSign) const;
@@ -54,6 +55,7 @@ public:
const KeypointDetector * getKeypointDetector() const {return _keypointDetector;}
const KeypointDescriptor * getKeypointDescriptor() const {return _keypointDescriptor;}
const VWDictionary * getVWD() const {return _vwd;}
std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
DetectorStrategy detectorStrategy() const;
protected:
@@ -62,10 +64,11 @@ protected:
virtual void clear();
virtual void moveToTrash(Signature * s);
virtual void preUpdate();
virtual void merge(const Signature * from, Signature * to, MergingStrategy s);
private:
virtual void copyData(const Signature * from, Signature * to);
virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false);
void disableWordsRef(int signatureId);
void enableWordsRef(const std::list<int> & signatureIds);
void cleanUnusedWords();
@@ -81,7 +84,6 @@ private:
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
bool _sensorStateOnly;
bool _tfIdfNormalized;
};
corelib/src/Memory.cpp
+898 -533
View File
File diff suppressed because it is too large Load Diff
corelib/src/Memory.h
+28 -24
View File
@@ -35,6 +35,7 @@
namespace rtabmap {
class Signature;
class NeighborLink;
class DBDriver;
class Node;
class SMState;
@@ -46,8 +47,6 @@ public:
static const int kIdVirtual;
static const int kIdInvalid;
enum MergingStrategy{kUseOnlyFromMerging, kUseOnlyDestMerging};
public:
Memory(const ParametersMap & parameters = ParametersMap());
virtual ~Memory();
@@ -55,31 +54,34 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
bool update(const SMState * rawData, std::map<std::string, float> & stats);
virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::set<int> & signatureIds = std::set<int>()) const;
virtual int forget(const std::list<int> & ignoredIds = std::list<int>());
virtual int reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched);
virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore);
virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
int cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
void addLoopClosureLink(int oldId, int newId, bool rehearsal = false);
void getNeighborsId(std::map<int,int> & ids, int signatureId, unsigned int margin, bool checkInDatabase = true, int ignoredId = 0) const;
bool addLoopClosureLink(int oldId, int newId);
std::map<int, int> getNeighborsId(double & dbAccessTime, int signatureId, unsigned int margin, int maxCheckedInDatabase = -1, bool onlyWithActions = false, bool incrementMarginOnLoop = false, bool ignoreSTM = true, bool ignoreLoopIds = false) const;
float compareOneToOne(const std::vector<int> & idsA, const std::vector<int> & idsB);
//getters
unsigned int getWorkingMemSize() const {return _workingMem.size();}
unsigned int getStMemSize() const {return _stMem.size();};
const std::map<int, int> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
std::list<int> getChildrenIds(int signatureId) const;
std::list<NeighborLink> getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure = false, bool lookInDatabase = false) const;
void getLoopClosureIds(int signatureId, std::set<int> & loopClosureIds, std::set<int> & childLoopClosureIds, bool lookInDatabase = false) const;
bool isRawDataKept() const {return _rawDataKept;}
float getSimilarityThr() const {return _similarityThreshold;}
std::map<int, int> getWeights() const;
int getWeight(int id) const;
const std::vector<int> & getLastBaseIds() const {return _lastBaseIds;}
float getSimilarityOnlyLast() const {return _similarityOnlyLast;}
const Signature * getLastSignature() const;
int getDatabaseMemoryUsed() const; // in bytes
double getDbSavingTime() const;
IplImage * getImage(int id) const;
bool isDatabaseCleaned() const {return _databaseCleaned;}
bool isCommonSignatureUsed() const {return _commonSignatureUsed;}
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
@@ -93,7 +95,6 @@ public:
void setSimilarityOnlyLast(int similarityOnlyLast) {_similarityOnlyLast = similarityOnlyLast;}
void setOldSignatureRatio(float oldSignatureRatio);
void setMaxStMemSize(unsigned int maxStMemSize);
void setDelayRequired(int delayRequired);
void setRecentWmRatio(float recentWmRatio);
void setCommonSignatureUsed(bool commonSignatureUsed);
void setRawDataKept(bool rawDataKept) {_rawDataKept = rawDataKept;}
@@ -109,7 +110,6 @@ public:
protected:
virtual void preUpdate();
virtual void postUpdate() {}
virtual void merge(const Signature * from, Signature * to, MergingStrategy s) = 0;
virtual void addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >());
virtual void clear();
@@ -118,41 +118,45 @@ protected:
void addSignatureToWm(Signature * signature);
Signature * _getSignature(int id) const;
Signature * _getLastSignature();
Signature * getRemovableSignature(const std::list<int> & ignoredIds = std::list<int>(), bool onlyLoopedSignatures = false);
std::list<Signature *> getRemovableSignatures(int count, const std::set<int> & ignoredIds = std::set<int>());
int getNextId();
void initCountId();
int rehearsal(const Signature * signature, bool onlyLast, float & similarity);
void touch(int signatureId);
void rehearsal(Signature * signature, std::map<std::string, float> & stats);
const std::map<int, Signature*> & getSignatures() const {return _signatures;}
private:
void createVirtualSignature(Signature ** signature);
virtual void copyData(const Signature * from, Signature * to) = 0;
virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false) = 0;
void createVirtualSignature(Signature ** signature);
void cleanGraph(const Node * root);
protected:
DBDriver * _dbDriver;
private:
// parameters
float _similarityThreshold;
bool _similarityOnlyLast;
bool _rawDataKept;
int _idCount;
Signature * _lastSignature;
int _lastLoopClosureId;
bool _incrementalMemory;
unsigned int _maxStMemSize;
bool _commonSignatureUsed;
bool _databaseCleaned; //if true, delete old signatures in the database
int _delayRequired;
float _recentWmRatio;
bool _dataMergedOnRehearsal;
int _idCount;
Signature * _lastSignature;
int _lastLoopClosureId;
bool _memoryChanged; // False by default, become true when Memory::update() is called.
bool _merging;
int _signaturesAdded;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem;
std::map<int, int> _workingMem; // id, timeStamp
std::set<int> _stMem; // id
std::set<int> _workingMem; // id,age
std::vector<int> _lastBaseIds;
std::map<int, std::map<int, float> > _similaritiesMap;
};
} // namespace rtabmap
corelib/src/Parameters.cpp
+1 -22
View File
@@ -24,9 +24,8 @@
namespace rtabmap
{
Parameters * Parameters::instance_ = 0;
UDestroyer<Parameters> Parameters::destroyer_;
ParametersMap Parameters::parameters_;
Parameters Parameters::instance_;
Parameters::Parameters()
{
@@ -37,30 +36,10 @@ Parameters::~Parameters()
}
const ParametersMap & Parameters::getDefaultParameters()
{
return Parameters::getInstance()->getParameters();
}
Parameters * Parameters::getInstance()
{
if(!instance_)
{
instance_ = new Parameters();
destroyer_.setDoomed(instance_);
}
return instance_;
}
const ParametersMap & Parameters::getParameters() const
{
return parameters_;
}
void Parameters::addParameter(const std::string & key, const std::string & value)
{
parameters_.insert(ParametersPair(key, value));
}
std::string Parameters::getDefaultWorkingDirectory()
{
std::string path = UDirectory::homeDir();
corelib/src/Rtabmap.cpp
+541 -434
View File
File diff suppressed because it is too large Load Diff
corelib/src/RtabmapEvent.cpp
+3 -2
View File
@@ -115,7 +115,6 @@ void Statistics::setLoopClosureImage(const IplImage * loopClosureImage)
Statistics & Statistics::operator=(const Statistics & s)
{
ULOGGER_DEBUG("");
_data = s.data();
if(_refImage)
{
@@ -143,7 +142,9 @@ Statistics & Statistics::operator=(const Statistics & s)
_weights = s.weights();
_refWords = s.refWords();
_loopWords = s.loopWords();
_refMotionMask = s.refMotionMask();
_loopMotionMask = s.loopMotionMask();
_actions = s.getActions();
return *this;
}
corelib/src/SMMemory.cpp
+607
View File
@@ -0,0 +1,607 @@
/*
* 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 "SMMemory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "utilite/UtiLite.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/SMState.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 "ColorTable.h"
namespace rtabmap {
SMMemory::SMMemory(const ParametersMap & parameters) :
Memory(parameters),
_useLogPolar(Parameters::defaultSMLogPolarUsed()),
_useVotingScheme(Parameters::defaultSMVotingSchemeUsed()),
_colorTable(0),
_useMotionMask(Parameters::defaultSMMotionMaskUsed())
{
this->parseParameters(parameters);
if(!_colorTable)
{
int i=1;
this->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::kSMVotingSchemeUsed())) != parameters.end())
{
this->setVotingScheme(uStr2Bool((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kSMMotionMaskUsed())) != parameters.end())
{
_useMotionMask = 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::setVotingScheme(bool useVotingScheme)
{
_useVotingScheme = useVotingScheme;
_dictionary.clear();
if(_useVotingScheme)
{
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
this->updateDictionary(i->second);
}
}
}
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->getSensors());
sTo->setMotionMask(sFrom->getMotionMask());
}
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 SMState * smState, bool keepRawData)
{
UDEBUG("");
UTimer timer;
timer.start();
UTimer timerDetails;
timerDetails.start();
std::vector<int> sensors;
const std::vector<int> * sensorsPrevious = 0;
std::vector<unsigned char> motionMask;
const IplImage * image = 0;
IplImage * polar = 0;
IplImage * indexed = 0;
const SMSignature * previousSignature = dynamic_cast<const SMSignature *>(this->getLastSignature());
if(previousSignature)
{
UDEBUG("");
sensorsPrevious = &previousSignature->getSensors();
}
if(smState)
{
image = smState->getImage();
// sensors
if(!smState->getSensors().empty() == 0 && image && image->imageSize)
{
if(image->depth != IPL_DEPTH_8U && image->nChannels != 3)
{
UFATAL("Only IplImage depth of IPL_DEPTH_8U and 3 channels (BGR) is supported.");
}
UDEBUG("depth=%d, alpha=%d, widthStep=%d, width=%d, height=%d, nChannels=%d, imageSize=%d,", image->depth, image->alphaChannel, image->widthStep, image->width, image->height, image->nChannels, image->imageSize);
if(_useLogPolar)
{
// Log-polar transform
int radius = image->height < image->width ? image->height/2: image->width/2;
CvSize polarSize = cvSize(64, 128);
float M = polarSize.width/std::log(radius);
polar = cvCreateImage( polarSize, 8, 3 );
cvLogPolar( image, polar, cvPoint2D32f(image->width/2,image->height/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;
sensors = std::vector<int>(polar->width*polar->height);
if(_useVotingScheme && (_dictionary.empty() || _dictionary.size() != sensors.size()))
{
_dictionary = std::vector<std::map<int, std::set<int> > >(sensors.size());
}
if(_useMotionMask)
{
motionMask = std::vector<unsigned char>(sensors.size(), 0);
}
bool updateMask = sensorsPrevious && sensorsPrevious->size() == motionMask.size();
int k=0;
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;
_colorTable->getRgb(index, r, g , b);
if(_useMotionMask && updateMask && sensorsPrevious->at(k) != sensors[k])
{
motionMask[k] = 1;
}
if(!_dictionary.empty())
{
std::set<int> sensorId;
sensorId.insert(id);
std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
ret = _dictionary[k].insert(std::make_pair(sensors[k], sensorId));
if(ret.second == false)
{
ret.first->second.insert(id);
}
}
++k;
}
}
UDEBUG("indexing time = %fs", timerDetails.ticks());
//cv::Mat indPolar;
//fromIndPolar = cvCreateImage(cvGetSize(image), 8, 3);
//cvLogPolar(polar, fromIndPolar, cvPoint2D32f(image->width/2,image->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
//UDEBUG("back from polar time = %fs", timerDetails());
//image = polar;
}
else
{
// IND transform
indexed = cvCloneImage(image);
unsigned char * data = (unsigned char *)indexed->imageData;
sensors = std::vector<int>(indexed->width*indexed->height);
if(_useVotingScheme && (_dictionary.empty() || _dictionary.size() != sensors.size()))
{
_dictionary = std::vector<std::map<int, std::set<int> > >(sensors.size());
}
if(_useMotionMask)
{
motionMask = std::vector<unsigned char>(sensors.size(), 0);
}
bool updateMask = sensorsPrevious && sensorsPrevious->size() == motionMask.size();
int k=0;
int sum=0;
for(int i=0; i<indexed->height; ++i)
{
for(int j=0; j<indexed->width; ++j)
{
unsigned char & b = data[i*indexed->widthStep+j*3+0];
unsigned char & g = data[i*indexed->widthStep+j*3+1];
unsigned char & r = data[i*indexed->widthStep+j*3+2];
int index = (int)_colorTable->getIndex(r, g, b);
sensors[k] = index;
_colorTable->getRgb(index, r, g , b);
if(_useMotionMask && updateMask && sensorsPrevious->at(k) != sensors[k])
{
motionMask[k] = 1;
++sum;
}
if(!_dictionary.empty())
{
std::set<int> sensorId;
sensorId.insert(id);
std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
ret = _dictionary[k].insert(std::make_pair(sensors[k], sensorId));
if(ret.second == false)
{
ret.first->second.insert(id);
}
}
++k;
}
}
image = indexed;
UDEBUG("sum=%d, indexing time = %fs", sum, timerDetails.ticks());
}
}
else
{
std::vector<float> sensorsMerged;
int buf;
smState->getSensorsMerged(sensorsMerged, buf);
sensors = std::vector<int>(sensorsMerged.size());
if(_useVotingScheme && (_dictionary.empty() || _dictionary.size() != sensorsMerged.size()))
{
_dictionary = std::vector<std::map<int, std::set<int> > >(sensorsMerged.size());
}
if(_useMotionMask)
{
motionMask = std::vector<unsigned char>(sensors.size(), 0);
}
bool updateMask = sensorsPrevious && sensorsPrevious->size() == motionMask.size();
for(unsigned int i=0; i<sensorsMerged.size(); ++i)
{
if(sensorsMerged[i]>0 && sensorsMerged[i]<1)
{
UWARN("Conversion from float to int may lost precision...");
}
sensors[i] = (int)sensorsMerged[i];
if(_useMotionMask && updateMask && sensorsPrevious->at(i) != sensors[i])
{
motionMask[i] = 1;
}
if(!_dictionary.empty())
{
std::set<int> sensorId;
sensorId.insert(id);
std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
ret = _dictionary[i].insert(std::make_pair(sensors[i], sensorId));
if(ret.second == false)
{
ret.first->second.insert(id);
}
}
}
}
}
SMSignature * s = new SMSignature(sensors, motionMask, id, image, keepRawData);
if(polar)
{
cvReleaseImage(&polar);
}
if(indexed)
{
cvReleaseImage(&indexed);
}
ULOGGER_DEBUG("time new signature (id=%d) %fs", id, timer.ticks());
return s;
}
std::map<int, float> SMMemory::computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore)
{
if(!_useVotingScheme)
{
return Memory::computeLikelihood(signature, ids, maximumScore);
}
else
{
UTimer timer;
timer.start();
std::map<int, float> likelihood;
maximumScore = 0;
const SMSignature * query = dynamic_cast<const SMSignature *>(signature);
if(!query)
{
ULOGGER_ERROR("The signature is not a SMSignature");
return likelihood; // Must be a SMSignature *
}
else if(ids.empty())
{
UWARN("ids list is empty");
return likelihood;
}
UDEBUG("Likelihood for %d", query->id());
const std::vector<int> & sensors = query->getSensors();
if(_dictionary.size() != sensors.size())
{
UERROR("Dictionary (%d) and sensor (%d) are not the same size!", (int)_dictionary.size(), (int)sensors.size());
return likelihood;
}
const std::vector<unsigned char> & mask = query->getMotionMask();
bool maskUsed = false;
if(mask.size() != 0 && mask.size() != sensors.size())
{
UWARN("mask's size (%d) and sensor's size (%d) are not equal", (int)mask.size(), (int)sensors.size());
}
else if(mask.size())
{
maskUsed = true;
}
// prepare likelihood
for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
likelihood.insert(likelihood.end(), std::make_pair(*iter, 0.0f));
}
//float nwi; // nwi is the number of a specific word referenced by a place
//float ni; // ni is the total of words referenced by a place
float nw; // nw is the number of places referenced by a specific word
float N; // N is the total number of places
float logNnw;
N = this->getSignatures().size();
if(N)
{
for(unsigned int i=0; i<sensors.size(); ++i)
{
if(!maskUsed || mask[i])
{
// "Inverted index"
std::map<int, std::set<int> >::iterator iter = _dictionary[i].find(sensors[i]);
if(iter == _dictionary[i].end())
{
UERROR("Sensor %d not found in dictionary ?!?", sensors[i]);
}
else
{
nw = iter->second.size();
if(nw)
{
if(nw > N)
{
for(std::set<int>::iterator jter = iter->second.begin(); jter!=iter->second.end(); ++jter)
{
UERROR("sensor pos %d, refid = %d", (int)i, *jter);
}
UFATAL("id=%d, N = %f, nw=%f", signature->id(), N, nw);
}
logNnw = log10(N/nw);
if(logNnw)
{
for(std::set<int>::iterator jter = iter->second.begin(); jter!=iter->second.end(); ++jter)
{
std::map<int, float>::iterator kter = likelihood.find(*jter);
if(kter != likelihood.end())
{
kter->second += logNnw;
}
}
}
}
}
}
}
}
if(sensors.size())
{
maximumScore = log(N) * float(sensors.size());
}
ULOGGER_DEBUG("compute likelihood, maximumScore=%f... %f s", maximumScore, timer.ticks());
return likelihood;
}
}
void SMMemory::moveToTrash(Signature * s)
{
if(_useVotingScheme)
{
UTimer timer;
SMSignature * sm = dynamic_cast<SMSignature *>(s);
if(sm && sm->id() > 0)
{
const std::vector<int> & sensors = sm->getSensors();
if(sensors.size() == _dictionary.size())
{
for(unsigned int i=0; i<sensors.size(); ++i)
{
std::map<int, std::set<int> >::iterator iter = _dictionary[i].find(sensors[i]);
if(iter != _dictionary[i].end())
{
if(!iter->second.erase(sm->id()))
{
UWARN("Sensor id %d not found in dictionary at pos %d", sm->id(), (int)i);
}
}
else
{
UWARN("Sensor value %d at sensor pos %d is not found in dictionary", sensors[i], (int)i);
}
}
}
else
{
UWARN("Dictionary size (%d) is not the same as the sensor (%d), signId=%d", (int)_dictionary.size(), (int)sensors.size(), sm->id());
}
}
UDEBUG("time=%fs", timer.ticks());
}
Memory::moveToTrash(s);
}
Signature * SMMemory::getSignatureLtMem(int id)
{
Signature * s = Memory::getSignatureLtMem(id);
if(_useVotingScheme && s)
{
this->updateDictionary(s);
}
return s;
}
bool SMMemory::init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
{
UDEBUG("");
bool success = Memory::init(dbDriverName, dbUrl, dbOverwritten, parameters);
if(_useVotingScheme)
{
// Update sensory dictionary
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
this->updateDictionary(i->second);
}
}
return success;
}
void SMMemory::updateDictionary(const Signature * s)
{
if(s)
{
const SMSignature * sm = dynamic_cast<const SMSignature *>(s);
if(sm)
{
const std::vector<int> & sensors = sm->getSensors();
if(_dictionary.empty())
{
_dictionary = std::vector<std::map<int, std::set<int> > >(sensors.size());
}
if(sensors.size() == _dictionary.size())
{
for(unsigned int i=0; i<sensors.size(); ++i)
{
std::set<int> sensorId;
sensorId.insert(sm->id());
std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
ret = _dictionary[i].insert(std::make_pair(sensors[i], sensorId));
if(ret.second == false)
{
ret.first->second.insert(sm->id());
}
}
}
else if(_dictionary.size())
{
UWARN("Loaded signature %d with size (%d) doesn't have the same size as the dicitonary (%d)", sm->id(), (int)sensors.size(), (int)_dictionary.size());
}
}
}
else
{
UFATAL("Signature must not be null!");
}
}
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/SMMemory.h
+64
View File
@@ -0,0 +1,64 @@
/*
* 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 SIMPLEMEMORY_H_
#define SIMPLEMEMORY_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "Memory.h"
namespace rtabmap {
class ColorTable;
class SMSignature;
class RTABMAP_EXP SMMemory : public Memory
{
public:
SMMemory(const ParametersMap & parameters = ParametersMap());
virtual ~SMMemory();
virtual void parseParameters(const ParametersMap & parameters);
virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore);
virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
void setRoi(const std::string & roi);
void setVotingScheme(bool useVotingScheme);
void setColorTable(int size);
protected:
virtual void moveToTrash(Signature * s);
virtual Signature * getSignatureLtMem(int id);
private:
virtual void copyData(const Signature * from, Signature * to);
virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false);
void updateDictionary(const Signature * s);
private:
bool _useLogPolar;
bool _useVotingScheme;
ColorTable * _colorTable;
bool _useMotionMask;
std::vector<std::map<int, std::set<int> > > _dictionary;
};
}
#endif /* KEYPOINTMEMORY_H_ */
corelib/src/Signature.cpp
+184 -44
View File
@@ -17,16 +17,36 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "Memory.h"
#include <opencv2/highgui/highgui.hpp>
#include "VerifyHypotheses.h"
#include "rtabmap/core/SMState.h"
#include "utilite/UtiLite.h"
namespace rtabmap
{
bool NeighborLink::updateIds(int idFrom, int idTo)
{
bool modified = false;
if(_id == idFrom)
{
_id = 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);
@@ -39,16 +59,12 @@ Signature::~Signature()
Signature::Signature(int id, const IplImage * image, bool keepImage) :
_id(id),
_weight(0),
_loopClosureId(0),
_image(0),
_saved(false),
_width(0),
_height(0)
_modified(true)
{
if(image)
{
_width = image->width;
_height = image->height;
if(keepImage)
{
_image = cvCloneImage(image);
@@ -68,10 +84,11 @@ void Signature::setImage(const IplImage * image)
{
cvReleaseImage(&_image);
_image = cvCloneImage(image);
_modified = true;
}
else
{
UWARN("Parameter is null or no image is saved.");
UDEBUG("Parameter is null or no image is saved.");
}
}
@@ -111,47 +128,60 @@ IplImage * Signature::decompressImage(const CvMat * imageCompressed)
return cvDecodeImage(imageCompressed, CV_LOAD_IMAGE_ANYCOLOR);
}
void Signature::addNeighbors(const NeighborsMap & neighbors)
void Signature::addNeighbors(const NeighborsMultiMap & neighbors)
{
for(NeighborsMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
for(NeighborsMultiMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
{
this->addNeighbor(i->first, i->second);
//UDEBUG("%d -> %d, a=%d", this->id(), i->first, i->second.size());
this->addNeighbor(i->second);
}
}
void Signature::addNeighbor(int neighbor, const std::list<std::vector<float> > & actions)
void Signature::addNeighbor(const NeighborLink & neighbor)
{
ULOGGER_DEBUG("Adding neighbor %d to %d with %d actions", neighbor, this->id(), actions.size());
std::pair<NeighborsMap::iterator, bool> inserted = _neighbors.insert(std::pair<int, std::list<std::vector<float> > >(neighbor, actions));
//UDEBUG("%d -> %d, a=%d", this->id(), neighbor, actions.size());
if(!inserted.second)
UDEBUG("Add neighbor %d to %d", neighbor.id(), this->id());
/*std::string baseIdsDebug;
const std::vector<int> & baseIds = neighbor.baseIds();
for(unsigned int i=0; i<baseIds.size(); ++i)
{
ULOGGER_ERROR("neighbor %d already added to %d", neighbor, this->id());
return;
}
if(neighbor == _id)
{
ULOGGER_ERROR("same Id ? (%d)", neighbor, this->id());
return;
baseIdsDebug.append(uNumber2str(baseIds[i]));
if(i+1 < baseIds.size())
{
baseIdsDebug.append(", ");
}
}
UDEBUG("Adding neighbor %d to %d with %d actions, %d baseIds = [%s]", neighbor.id(), this->id(), neighbor.actions().size(), neighbor.baseIds().size(), baseIdsDebug.c_str());
*/
_neighbors.insert(std::pair<int, NeighborLink>(neighbor.id(), neighbor));
_neighborsModified = true;
}
void Signature::removeNeighbor(int neighbor)
void Signature::changeNeighborIds(int idFrom, int idTo)
{
ULOGGER_DEBUG("Removing neighbor %d to %d", neighbor, this->id());
// we delete the first found because there is not supposed
// to have more than one occurrence of this neighbor (see addNeighbor())
int erased = _neighbors.erase(neighbor);
if(!erased)
std::pair<NeighborsMultiMap::iterator, NeighborsMultiMap::iterator> pair = _neighbors.equal_range(idFrom);
if(pair.first != _neighbors.end() && pair.first != pair.second)
{
ULOGGER_WARN("neighbor %d not found in %d", neighbor, this->id());
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);
for(std::list<NeighborLink>::iterator iter=linksToAdd.begin(); iter!=linksToAdd.end(); ++iter)
{
_neighbors.insert(std::pair<int, NeighborLink>(iter->id(), *iter));
}
_modified = true;
_neighborsModified = true;
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
}
//KeypointSignature
KeypointSignature::KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
@@ -191,19 +221,6 @@ float KeypointSignature::compareTo(const Signature * s) const
HypVerificatorEpipolarGeo::findPairsDirect(words, _words, pairs, pairsId);
similarity = float(pairs.size()) / float(totalWords);
// Adjust similarity with the ratio of words between the signatures
/*float ratio = 1;
if(_words.size() > words.size() && _words.size())
{
ratio = float(words.size()) / float(_words.size());
}
else
{
ratio = float(_words.size()) / float(words.size());
}
similarity *= ratio;*/
}
}
return similarity;
@@ -215,6 +232,7 @@ void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
if(kps.size())
{
_words.erase(oldWordId);
_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
{
_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
@@ -240,4 +258,126 @@ void KeypointSignature::removeWord(int wordId)
_words.erase(wordId);
}
//SMSignature
SMSignature::SMSignature(
const std::vector<int> & sensors,
const std::vector<unsigned char> & motionMask,
int id,
const IplImage * image,
bool keepRawData) :
Signature(id, image, keepRawData),
_sensors(sensors),
_motionMask(motionMask)
{
if(_sensors.size() != _motionMask.size() && _motionMask.size() > 0)
{
UFATAL("Sensors and mask must have the same size (%d vs %d)", (int)_sensors.size(), (int)_motionMask.size());
}
UDEBUG("sensors=%d", (int)_sensors.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::vector<int> & sensorsB = sm->getSensors();
const std::vector<unsigned char> & motionMaskB = sm->getMotionMask();
if(_sensors.size() == sensorsB.size() && _sensors.size()) //Compatible
{
bool appearanceOnly = false;
if(appearanceOnly)
{
std::multiset<int> sensorsSetA(_sensors.begin(), _sensors.end());
std::multiset<int> sensorsSetB(sensorsB.begin(), sensorsB.end());
std::set<int> ids(_sensors.begin(), _sensors.end());
std::multiset<int>::iterator iterA;
std::multiset<int>::iterator iterB;
float realPairsCount = 0;
for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
{
iterA = sensorsSetA.find(*i);
iterB = sensorsSetB.find(*i);
while(iterA != sensorsSetA.end() && iterB != sensorsSetB.end() && *iterA == *iterB && *iterA == *i)
{
++iterA;
++iterB;
++realPairsCount;
}
}
similarity = realPairsCount / float(_sensors.size());
}
else if(_motionMask.size() == _sensors.size() &&
_motionMask.size() == motionMaskB.size())
{
int sum = 0;
int maskSumA = 0;
int maskSumB = 0;
// compare sensors
for(unsigned int i=0; i<_sensors.size(); ++i)
{
maskSumA += _motionMask[i];
maskSumB += motionMaskB[i];
sum += _sensors.at(i) == sensorsB.at(i) && _motionMask[i] && motionMaskB[i] ? 1 : 0;
}
int totalSize = maskSumA>maskSumB?maskSumA:maskSumB;
if(totalSize)
{
similarity = float(sum)/float(totalSize);
}
}
else
{
int sum = 0;
// compare sensors
for(unsigned int i=0; i<_sensors.size(); ++i)
{
sum += _sensors.at(i) == sensorsB.at(i) ? 1 : 0;
}
similarity = float(sum)/float(_sensors.size());
}
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 signatures : nb sensors A=%d B=%d", (int)_sensors.size(), (int)sensorsB.size());
}
}
else if(s)
{
UWARN("Only SM signatures are compared. (type tested=%s)", s->signatureType().c_str());
}
return similarity;
}
bool SMSignature::isBadSignature() const
{
if(_sensors.size() == 0)
return true;
return false;
}
} //namespace rtabmap
corelib/src/Signature.h
-131
View File
@@ -1,131 +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/>.
*/
#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>
//TODO : add copy constructor
namespace rtabmap
{
class Memory;
typedef std::map<int, std::list<std::vector<float> > > NeighborsMap;
class RTABMAP_EXP Signature
{
public:
static CvMat * compressImage(const IplImage * image);
static IplImage * decompressImage(const CvMat * imageCompressed);
public:
virtual ~Signature();
/**
* Must return a value between >=0 and <=1 (1 means 100% similarity)
*/
virtual float compareTo(const Signature * signature) const = 0;
virtual bool isBadSignature() const = 0;
virtual std::string signatureType() const = 0;
const IplImage * getImage() const;
void setImage(const IplImage * image);
int id() const {return _id;}
void addNeighbors(const NeighborsMap & neighbors);
void addNeighbor(int neighborId, const std::list<std::vector<float> > & actions);
void removeNeighbor(int neighborId);
bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
void setWeight(int weight) {_weight = weight;}
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
void setWidth(int width) {_width = width;}
void setHeight(int height) {_height = height;}
void setSaved(bool saved) {_saved = saved;}
const NeighborsMap & getNeighbors() const {return _neighbors;}
int getWeight() const {return _weight;}
int getLoopClosureId() const {return _loopClosureId;}
int getWidth() const {return _width;}
int getHeight() const {return _height;}
bool isSaved() const {return _saved;}
protected:
Signature(int id, const IplImage * image = 0, bool keepImage = false);
private:
int _id;
NeighborsMap _neighbors; // id, [action1, action2, ...] All actions must have the same length
int _weight;
int _loopClosureId;
IplImage * _image;
bool _saved; // If it's saved to bd
int _width; // pixels
int _height; // pixels
};
class KeypointDetector;
class VWDictionary;
class RTABMAP_EXP KeypointSignature :
public Signature
{
public:
KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
int id,
const IplImage * image = 0,
bool keepRawData = false);
KeypointSignature(int id);
virtual ~KeypointSignature();
virtual float compareTo(const Signature * signature) const;
virtual bool isBadSignature() const;
virtual std::string signatureType() const {return "KeypointSignature";};
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;}
private:
// 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>
bool _enabled;
};
} // namespace rtabmap
corelib/src/VWDictionary.cpp
+30 -40
View File
@@ -20,7 +20,7 @@
#include "VWDictionary.h"
#include "VisualWord.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "NearestNeighbor.h"
#include "rtabmap/core/Parameters.h"
@@ -406,24 +406,23 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
}
}
std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > & descriptors,
unsigned int dim,
std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
int signatureId)
{
UTimer timer;
std::list<int> wordIds;
ULOGGER_DEBUG("");
if(_dim && _dim != dim && dim)
if(_dim && _dim != descriptors.cols && descriptors.cols)
{
ULOGGER_WARN("Descriptor size has changed! (%d to %d), Nearest neighbor approaches may not work with different descriptor sizes.", _dim, dim);
ULOGGER_WARN("Descriptor size has changed! (%d to %d), Nearest neighbor approaches may not work with different descriptor sizes.", _dim, descriptors.cols);
}
else if(!dim)
else if(!descriptors.cols)
{
ULOGGER_ERROR("Descriptor size is null?!?");
return wordIds;
}
_dim = dim;
if (descriptors.size() == 0 || !_dim)
_dim = descriptors.cols;
if (descriptors.empty() || !_dim)
{
ULOGGER_ERROR("Parameters don't fit the requirements of this method");
return wordIds;
@@ -442,31 +441,24 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
{
std::list<VisualWord *> newWords;
cv::Mat results(descriptors.size(), k, CV_32SC1); // results index
cv::Mat results(descriptors.rows, k, CV_32SC1); // results index
cv::Mat dists;
if(_nn->isDist64F())
{
dists = cv::Mat(descriptors.size(), k, CV_64FC1); // Distance results are CV_64FC1;
dists = cv::Mat(descriptors.rows, k, CV_64FC1); // Distance results are CV_64FC1;
}
else
{
dists = cv::Mat(descriptors.size(), k, CV_32FC1); // Distance results are CV_32FC1
dists = cv::Mat(descriptors.rows, k, CV_32FC1); // Distance results are CV_32FC1
}
cv::Mat newPts(descriptors.size(), _dim, CV_32F); // SURF descriptors are CV_32F
// fill the request matrix
std::list<std::vector<float> >::const_iterator itDesc = descriptors.begin();
for(unsigned int i=0; i<descriptors.size(); ++itDesc, ++i)
cv::Mat newPts; // SURF descriptors are CV_32F
if(descriptors.type()!=CV_32F)
{
float * rowFl = newPts.ptr<float>(i);
if(itDesc->size() == _dim)
{
memcpy(rowFl, (const float *)itDesc->data(), _dim*sizeof(float));
}
else
{
ULOGGER_WARN("Descriptors are not the same size! The result may be wrong...");
}
descriptors.convertTo(newPts, CV_32F); // make sure it's CV_32F
}
else
{
newPts = descriptors;
}
UTimer timerLocal;
@@ -480,7 +472,7 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
}
//
for(unsigned int i = 0; i < descriptors.size(); ++i)
for(int i = 0; i < descriptors.rows; ++i)
{
// Check if this descriptor matches with a word from the last signature (a word not already added to the tree)
std::map<float, int> fullResults; // Contains results from the kd-tree search and the naive search in new words
@@ -531,7 +523,10 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
}
else
{
UWARN("Not enough nearest neighbors found! fullResults=%d (descriptor %d)", fullResults.size(), i);
if(!_dataTree.empty())
{
UWARN("Not enough nearest neighbors found! fullResults=%d (descriptor %d)", fullResults.size(), i);
}
badDist = true; // Rejected
}
}
@@ -566,10 +561,9 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
ULOGGER_DEBUG("Naive NN");
UTimer timer;
timer.start();
std::list<std::vector<float> >::const_iterator itDesc = descriptors.begin();
for(; itDesc!=descriptors.end();++itDesc)
for(int i=0; i<descriptors.rows; ++i)
{
const float* d = itDesc->data();
const float* d = descriptors.ptr<float>(i);
std::map<float, int> results;
naiveNNSearch(uValuesList(_visualWords), d, _dim, results, k);
@@ -871,14 +865,14 @@ void VWDictionary::addWord(VisualWord * vw)
}
// dist = (euclidean dist)^2, "k" nearest neighbors
void VWDictionary::naiveNNSearch(const std::list<VisualWord *> & words, const float * d, unsigned int length, std::map<float, int> & results, unsigned int k) const
void VWDictionary::naiveNNSearch(const std::list<VisualWord *> & words, const float * d, int length, std::map<float, int> & results, unsigned int k) const
{
double total_cost = 0;
double t0, t1, t2, t3;
const float * dw = 0;
bool goodMatch;
if(!words.size() && k > 0)
if(!words.size() || k == 0)
{
return;
}
@@ -896,7 +890,7 @@ void VWDictionary::naiveNNSearch(const std::list<VisualWord *> & words, const fl
total_cost += t0*t0;
// compare descriptors
unsigned int i = 0;
int i = 0;
if(length>=4)
{
for(; i <= length-4; i += 4 )
@@ -1036,16 +1030,12 @@ void VWDictionary::getCommonWords(unsigned int nbCommonWords, int totalSign, std
const VisualWord * VWDictionary::getWord(int id) const
{
return uValue(_visualWords, id);
return uValue(_visualWords, id, (VisualWord *)0);
}
void VWDictionary::setWordSaved(int id, bool saved)
const VisualWord * VWDictionary::getUnusedWord(int id) const
{
VisualWord * w = uValue(_visualWords, id);
if(w)
{
w->setSaved(saved);
}
return uValue(_unusedWords, id, (VisualWord *)0);
}
std::vector<VisualWord*> VWDictionary::getUnusedWords() const
corelib/src/VWDictionary.h
+5 -6
View File
@@ -50,18 +50,17 @@ public:
virtual void update();
virtual std::list<int> addNewWords(
const std::list<std::vector<float> > & descriptors,
unsigned int dim,
const cv::Mat & descriptors,
int signatureId);
virtual void addWord(VisualWord * vw);
virtual std::vector<int> findNN(const std::list<VisualWord *> & vws, bool searchInNewlyAddedWords = true) const;
void naiveNNSearch(const std::list<VisualWord *> & words, const float * d, unsigned int length, std::map<float, int> & results, unsigned int k) const;
void naiveNNSearch(const std::list<VisualWord *> & words, const float * d, int length, std::map<float, int> & results, unsigned int k) const;
void addWordRef(int wordId, int signatureId);
void removeAllWordRef(int wordId, int signatureId);
const VisualWord * getWord(int id) const;
void setWordSaved(int id, bool saved);
const VisualWord * getUnusedWord(int id) const;
void setLastWordId(int id) {_lastWordId = id;}
void getCommonWords(unsigned int nbCommonWords, int totalSign, std::list<int> & commonWords) const;
const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
@@ -105,12 +104,12 @@ private:
float _nndrRatio;
unsigned int _maxLeafs;
std::string _dictionaryPath; // a pre-computed dictionary (.txt)
unsigned int _dim;
int _dim;
int _lastWordId;
NearestNeighbor * _nn;
cv::Mat _dataTree;
std::map<int ,int> _mapIndexId;
std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>
std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
};
} // namespace rtabmap
corelib/src/VerifyHypotheses.cpp
+1 -1
View File
@@ -19,7 +19,7 @@
#include "VerifyHypotheses.h"
#include "rtabmap/core/Parameters.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include <cstdlib>
#include <opencv2/calib3d/calib3d.hpp>
corelib/src/VisualWord.cpp
+1 -1
View File
@@ -24,7 +24,7 @@
namespace rtabmap
{
VisualWord::VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId) :
VisualWord::VisualWord(int id, const float * descriptor, int dim, int signatureId) :
_id(id),
_saved(false),
_totalReferences(0)
corelib/src/VisualWord.h
+3 -3
View File
@@ -31,7 +31,7 @@ class SignatureSurf;
class RTABMAP_EXP VisualWord
{
public:
VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId = 0);
VisualWord(int id, const float * descriptor, int dim, int signatureId = 0);
~VisualWord();
void addRef(int signatureId);
@@ -40,7 +40,7 @@ public:
int getTotalReferences() const {return _totalReferences;}
int id() const {return _id;}
const float * getDescriptor() const {return _descriptor;}
unsigned int getDim() const {return _dim;}
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;}
@@ -49,7 +49,7 @@ public:
private:
int _id;
float * _descriptor;
unsigned int _dim;
int _dim;
bool _saved; // If it's saved to bd
int _totalReferences;
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
+55 -32
View File
@@ -23,14 +23,30 @@ CREATE TABLE Signature (
id INTEGER NOT NULL,
type VARCHAR NOT NULL,
weight INTEGER,
loopClosureId INTEGER,
image BLOB,
imgWidth INTEGER,
imgHeight INTEGER,
loopClosureIds BLOB,
childLoopClosureIds BLOB,
timeEnter DATE,
PRIMARY KEY (id),
FOREIGN KEY (type) REFERENCES SignatureType(type),
FOREIGN KEY (loopClosureId) REFERENCES Signature(id)
FOREIGN KEY (type) REFERENCES SignatureType(type)
);
CREATE TABLE Image (
id INTEGER NOT NULL,
width INTEGER NOT NULL,
height INTEGER NOT NULL,
channels INTEGER NOT NULL,
compressed CHAR NOT NULL,
data BLOB,
timeEnter DATE,
PRIMARY KEY (id)
);
CREATE TABLE SMState (
id INTEGER NOT NULL,
sensors BLOB,
motionMask BLOB,
timeEnter DATE,
FOREIGN KEY (id) REFERENCES Signature(id)
);
CREATE TABLE Neighbor (
@@ -38,8 +54,7 @@ CREATE TABLE Neighbor (
nid INTEGER NOT NULL,
actionSize INTEGER,
actions BLOB,
timeEnter DATE,
PRIMARY KEY (sid, nid),
baseIds BLOB,
FOREIGN KEY (sid) REFERENCES Signature(id),
FOREIGN KEY (nid) REFERENCES Signature(id)
);
@@ -66,7 +81,6 @@ CREATE TABLE Map_SS_VW (
size INTEGER NOT NULL,
dir FLOAT NOT NULL,
hessian FLOAT NOT NULL,
timeEnter DATE,
FOREIGN KEY (signatureId) REFERENCES Signature(id),
FOREIGN KEY (visualWordId) REFERENCES VisualWord(id)
);
@@ -90,20 +104,38 @@ CREATE TABLE StatisticsAfterRunSurf (
CREATE TRIGGER insert_Signature BEFORE INSERT ON Signature
WHEN NOT EXISTS (SELECT type FROM SignatureType WHERE SignatureType.type = NEW.type)
BEGIN
SELECT RAISE(ABORT, 'Foreign key constraint failed');
SELECT RAISE(ABORT, 'Foreign key Signature.type constraint failed');
END;
CREATE TRIGGER insert_Neighbor BEFORE INSERT ON Neighbor
CREATE TRIGGER insert_SMState BEFORE INSERT ON SMState
WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.id)
BEGIN
SELECT RAISE(ABORT, 'Foreign key SMState.id constraint failed');
END;
--CREATE TRIGGER insert_Neighbor_unique BEFORE INSERT ON Neighbor
--WHEN NEW.sid = NEW.nid
--BEGIN
-- SELECT RAISE(ABORT, 'Cannot add self references');
--END;
CREATE TRIGGER insert_Neighbor_sid BEFORE INSERT ON Neighbor
WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.sid)
BEGIN
SELECT RAISE(ABORT, 'Foreign key constraint failed');
SELECT RAISE(ABORT, 'Foreign key Neighbor.sid constraint failed');
END;
--Commented before a link can be added before the neighbor is saved...
--CREATE TRIGGER insert_Neighbor_nid BEFORE INSERT ON Neighbor
--WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.nid)
--BEGIN
-- SELECT RAISE(ABORT, 'Foreign key Neighbor.nid constraint failed');
--END;
CREATE TRIGGER insert_Map_SS_VW BEFORE INSERT ON Map_SS_VW
WHEN NOT EXISTS (SELECT type FROM Signature WHERE Signature.id = NEW.signatureId AND type='surf')
--OR NOT EXISTS (SELECT id FROM VisualWord WHERE VisualWord.id = NEW.visualWordId)
WHEN NOT EXISTS (SELECT type FROM Signature WHERE Signature.id = NEW.signatureId AND type='KeypointSignature')
BEGIN
SELECT RAISE(ABORT, 'Foreign key constraint failed');
SELECT RAISE(ABORT, 'KeypointSignature type constraint failed');
END;
-- Creating a trigger for timeEnter
@@ -112,21 +144,11 @@ BEGIN
UPDATE Signature SET timeEnter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Neighbor_timeEnter AFTER INSERT ON Neighbor
BEGIN
UPDATE Neighbor SET timeEnter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_VisualWord_timeEnter AFTER INSERT ON VisualWord
BEGIN
UPDATE VisualWord SET timeEnter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Map_SS_VW_timeEnter AFTER INSERT ON Map_SS_VW
BEGIN
UPDATE Map_SS_VW SET timeEnter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_StatisticsAfterRun_timeEnter AFTER INSERT ON StatisticsAfterRun
BEGIN
UPDATE StatisticsAfterRun SET timeEnter = DATETIME('NOW') WHERE rowid = new.rowid;
@@ -142,18 +164,19 @@ END;
-- INDEXES
-- *******************************************************************
CREATE INDEX IDX_Map_SS_VW_SignatureId on Map_SS_VW (signatureId);
CREATE INDEX IDX_Map_SS_VW_VisualWordId on Map_SS_VW (visualWordId);
CREATE INDEX IDX_Signature_Id on Signature (id);
CREATE INDEX IDX_VisualWord_Id on VisualWord (id);
CREATE INDEX IDX_Signature_TimeEnter on Signature (timeEnter);
CREATE INDEX IDX_VisualWord_TimeEnter on VisualWord (timeEnter);
-- CREATE INDEX IDX_Map_SS_VW_VisualWordId on Map_SS_VW (visualWordId);
-- CREATE INDEX IDX_Signature_Id on Signature (id);
-- CREATE INDEX IDX_VisualWord_Id on VisualWord (id);
CREATE INDEX IDX_SMState_Id on SMState (id);
-- CREATE INDEX IDX_Signature_TimeEnter on Signature (timeEnter);
-- CREATE INDEX IDX_VisualWord_TimeEnter on VisualWord (timeEnter);
CREATE INDEX IDX_Neighbor_Sid on Neighbor (sid);
-- *******************************************************************
-- Data
-- *******************************************************************
INSERT INTO SignatureType(type) VALUES ('fourier');
INSERT INTO SignatureType(type) VALUES ('surf');
INSERT INTO SignatureType(type) VALUES ('KeypointSignature');
INSERT INTO SignatureType(type) VALUES ('SMSignature');
-- *******************************************************************
-- TESTS