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

Merged Audio branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@560 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-06-24 17:19:34 +00:00
parent 06fb556e78
commit 17b8e10ed8
111 changed files with 8370 additions and 9779 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/BayesFilter.cpp
+38 -43
View File
@@ -17,8 +17,8 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "BayesFilter.h"
#include "Memory.h"
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
@@ -160,9 +160,9 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
UTimer timer;
timer.start();
CvMat * prediction = 0;
CvMat * prior = 0;
CvMat * posterior = 0;
cv::Mat prediction;
cv::Mat prior;
cv::Mat posterior;
float sum = 0;
int j=0;
@@ -171,27 +171,26 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
prediction = cvCreateMat(likelihood.size(), likelihood.size(), CV_32FC1);
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;
ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), prediction.rows, prediction.cols);
//std::cout << "Prediction=" << prediction << std::endl;
// Adjust the last posterior if some images were
// reactivated or removed from the working memory
posterior = cvCreateMat(likelihood.size(), 1, CV_32FC1);
posterior = cv::Mat(likelihood.size(), 1, CV_32FC1);
this->updatePosterior(memory, uKeys(likelihood));
j=0;
for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
posterior->data.fl[j++] = (*i).second;
((float*)posterior.data)[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;
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior.rows, _posterior.size());
//std::cout << "LastPosterior=" << posterior << std::endl;
// Multiply prediction matrix with the last posterior
// (m,m) X (m,1) = (m,1)
prior = cvCreateMat(likelihood.size(), 1, CV_32FC1);
cvMatMul(prediction, posterior, prior);
prior = prediction * posterior;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
//std::cout << "ResultingPrior=" << cv::Mat(prior) << std::endl;
//std::cout << "ResultingPrior=" << prior << std::endl;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
std::vector<float> likelihoodValues = uValues(likelihood);
@@ -204,7 +203,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
std::map<int, float>::iterator p =_posterior.find((*i).first);
if(p!= _posterior.end())
{
(*p).second = (*i).second * prior->data.fl[j++];
(*p).second = (*i).second * ((float*)prior.data)[j++];
sum+=(*p).second;
}
else
@@ -226,14 +225,10 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
}
cvReleaseMat(&prediction);
cvReleaseMat(&prior);
cvReleaseMat(&posterior);
return _posterior;
}
bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory, const std::vector<int> & ids) const
bool BayesFilter::generatePrediction(cv::Mat & prediction, const Memory * memory, const std::vector<int> & ids) const
{
ULOGGER_DEBUG("");
UTimer timer;
@@ -242,9 +237,9 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
timerGlobal.start();
if(!memory ||
prediction == 0 ||
prediction->rows != prediction->cols ||
(unsigned int)prediction->rows != ids.size() ||
prediction.empty() ||
prediction.rows != prediction.cols ||
(unsigned int)prediction.rows != ids.size() ||
_predictionLC.size() < 2 ||
!ids.size())
{
@@ -262,9 +257,9 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
}
//int rows = prediction->rows;
cvSetZero(prediction);
int cols = prediction->cols;
//int rows = prediction.rows;
prediction = cv::Mat::zeros(prediction.rows, prediction.cols, prediction.type());
int cols = prediction.cols;
// Each prior is a column vector
ULOGGER_DEBUG("_predictionLC.size()=%d",_predictionLC.size());
@@ -291,7 +286,7 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
if(sum < totalModelValues-_predictionLC[0])
{
float delta = totalModelValues-_predictionLC[0]-sum;
prediction->data.fl[i + i*cols] += delta;
((float*)prediction.data)[i + i*cols] += delta;
sum+=delta;
}
@@ -307,10 +302,10 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
float value = allOtherPlacesValue / float(cols - 1);
for(int j=ids[0] < 0?1:0; j<cols; ++j)
{
if(prediction->data.fl[i + j*cols] == 0)
if(((float*)prediction.data)[i + j*cols] == 0)
{
prediction->data.fl[i + j*cols] = value;
sum += prediction->data.fl[i + j*cols];
((float*)prediction.data)[i + j*cols] = value;
sum += ((float*)prediction.data)[i + j*cols];
}
}
}
@@ -321,7 +316,7 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
{
for(int j=ids[0] < 0?1:0; j<cols; ++j)
{
prediction->data.fl[i + j*cols] *= maxNorm / sum;
((float*)prediction.data)[i + j*cols] *= maxNorm / sum;
}
sum = maxNorm;
}
@@ -329,14 +324,14 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
// ADD virtual place prob
if(ids[0] < 0)
{
prediction->data.fl[i] = _predictionLC[0];
sum += prediction->data.fl[i];
((float*)prediction.data)[i] = _predictionLC[0];
sum += ((float*)prediction.data)[i];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test col=%d = %f", i, prediction->data.fl[i + j*cols]);
// ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
//}
if(sum<0.99 || sum > 1.01)
@@ -351,16 +346,16 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
{
if(cols>1) // The first must be the virtual place
{
prediction->data.fl[i] = _virtualPlacePrior;
((float*)prediction.data)[i] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(cols-1);
for(int j=1; j<cols; j++)
{
prediction->data.fl[i + j*cols] = val;
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
prediction->data.fl[i] = 1;
((float*)prediction.data)[i] = 1;
}
}
else
@@ -372,12 +367,12 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
float val = 1.0/cols;
for(int j=0; j<cols; j++)
{
prediction->data.fl[i + j*cols] = val;
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
prediction->data.fl[i] = 1;
((float*)prediction.data)[i] = 1;
}
}
}
@@ -414,10 +409,10 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
_posterior = newPosterior;
}
float BayesFilter::addNeighborProb(CvMat * prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
{
if((unsigned int)prediction->cols != idToIndexMap.size() ||
(unsigned int)prediction->rows != idToIndexMap.size())
if((unsigned int)prediction.cols != idToIndexMap.size() ||
(unsigned int)prediction.rows != idToIndexMap.size())
{
UFATAL("Requirements no met");
}
@@ -428,7 +423,7 @@ float BayesFilter::addNeighborProb(CvMat * prediction, unsigned int col, const s
int index = uValue(idToIndexMap, iter->first, -1);
if(index >= 0)
{
sum += prediction->data.fl[col + index*prediction->cols] = _predictionLC[iter->second+1];
sum += ((float*)prediction.data)[col + index*prediction.cols] = _predictionLC[iter->second+1];
}
}
return sum;
corelib/src/BayesFilter.h
-71
View File
@@ -1,71 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef BAYESFILTER_H_
#define BAYESFILTER_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <list>
#include <set>
#include "utilite/UEventsHandler.h"
#include "rtabmap/core/Parameters.h"
namespace rtabmap {
class Memory;
class Signature;
class RTABMAP_EXP BayesFilter
{
public:
BayesFilter(const ParametersMap & parameters = ParametersMap());
virtual ~BayesFilter();
virtual void parseParameters(const ParametersMap & parameters);
const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
void reset();
//setters
void setVirtualPlacePrior(float virtualPlacePrior);
void setPredictionLC(const std::string & prediction);
//getters
const std::map<int, float> & getPosterior() const {return _posterior;}
float getVirtualPlacePrior() const {return _virtualPlacePrior;}
const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
bool isPredictionOnNonNullActionsOnly() const {return _predictionOnNonNullActionsOnly;}
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
#endif /* BAYESFILTER_H_ */
corelib/src/CMakeLists.txt
+8 -12
View File
@@ -10,8 +10,10 @@ SET(SRC_FILES
DBDriverFactory.cpp
DBDriver.cpp
DBDriverSqlite3.cpp
DBReader.cpp
Camera.cpp
Micro.cpp
EpipolarGeometry.cpp
VisualWord.cpp
VWDictionary.cpp
@@ -34,6 +36,7 @@ SET(INCLUDE_DIRS
${OpenCV_INCLUDE_DIRS}
${SQLITE3_INCLUDE_DIR}
${ZLIB_INCLUDE_DIRS}
${FFTW3F_INCLUDE_DIRS}
)
SET(LIBRARIES
@@ -41,6 +44,7 @@ SET(LIBRARIES
${OpenCV_LIBS}
${SQLITE3_LIBRARY}
${ZLIB_LIBRARIES}
${FFTW3F_LIBRARIES}
)
####################################
@@ -128,27 +132,19 @@ SET(RESOURCES
# Make sure the compiler can find include files from our library.
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
IF(WIN32)
IF(BUILD_SHARED_LIBS)
ADD_DEFINITIONS(-DRTABMAP_EXPORTS)
ELSE()
ADD_DEFINITIONS(-DRTABMAP_EXPORTS_STATIC)
ENDIF()
ENDIF(WIN32)
# Add binary that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_LIBRARY(corelib ${SRC_FILES} ${RESOURCES})
TARGET_LINK_LIBRARIES(corelib ${LIBRARIES})
ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES})
TARGET_LINK_LIBRARIES(rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES(
corelib
rtabmap_corelib
PROPERTIES
OUTPUT_NAME ${PROJECT_PREFIX}_core
INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
)
INSTALL(TARGETS corelib
INSTALL(TARGETS rtabmap_corelib
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)
corelib/src/Camera.cpp
+222 -335
View File
@@ -18,31 +18,44 @@
*/
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraEvent.h"
#include "utilite/UEventsManager.h"
#include "utilite/UConversion.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/DBDriverFactory.h"
#include "rtabmap/core/KeypointDescriptor.h"
#include "rtabmap/core/KeypointDetector.h"
#include "rtabmap/core/SMState.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include "utilite/UFile.h"
#include "utilite/UDirectory.h"
#include "utilite/UTimer.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/imgproc/imgproc.hpp>
namespace rtabmap
{
void CamPostTreatment::process(SMState * smState) const
Camera::Camera(float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
_imageRate(imageRate),
_id(id),
_autoRestart(autoRestart),
_imageWidth(imageWidth),
_imageHeight(imageHeight),
_framesDropped(framesDropped),
_featuresExtracted(false),
_keypointDetector(0),
_keypointDescriptor(0)
{
//no threatment...
}
CamKeypointTreatment::~CamKeypointTreatment()
Camera::~Camera()
{
UEventsManager::removeHandler(this);
join(true);
if(_keypointDetector)
{
delete _keypointDetector;
@@ -52,48 +65,56 @@ CamKeypointTreatment::~CamKeypointTreatment()
delete _keypointDescriptor;
}
}
void CamKeypointTreatment::process(SMState * smState) const
void Camera::setFeaturesExtracted(bool featuresExtracted, KeypointDetector::DetectorType detector, KeypointDescriptor::DescriptorType descriptor)
{
if(_keypointDetector && _keypointDescriptor && smState && smState->getImage() && smState->getKeypoints().size() == 0 && smState->getSensors().empty())
_featuresExtracted = featuresExtracted;
if(detector != KeypointDetector::kDetectorUndef || descriptor != KeypointDescriptor::kDescriptorUndef)
{
std::vector<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(smState->getImage());
cv::Mat descriptors = _keypointDescriptor->generateDescriptors(smState->getImage(), keypoints);
smState->setSensors(descriptors);
smState->setKeypoints(keypoints);
ParametersMap pm;
pm.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str((int)detector)));
pm.insert(ParametersPair(Parameters::kKpDescriptorStrategy(), uNumber2Str((int)detector)));
this->parseParameters(pm);
}
}
void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
void Camera::parseParameters(const ParametersMap & parameters)
{
UDEBUG("");
ParametersMap::const_iterator iter;
//Keypoint detector
DetectorStrategy detectorStrategy = kDetectorUndef;
KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
{
detectorStrategy = (DetectorStrategy)std::atoi((*iter).second.c_str());
detector = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
}
DetectorStrategy currentDetectorStrategy = this->detectorStrategy();
if(!_keypointDetector || ( detectorStrategy!=kDetectorUndef && (detectorStrategy != currentDetectorStrategy) ) )
//Keypoint descriptor
KeypointDescriptor::DescriptorType descriptor = KeypointDescriptor::kDescriptorUndef;
if((iter=parameters.find(Parameters::kKpDescriptorStrategy())) != parameters.end())
{
ULOGGER_DEBUG("new detector strategy %d", int(detectorStrategy));
descriptor = (KeypointDescriptor::DescriptorType)std::atoi((*iter).second.c_str());
}
if(detector!=KeypointDetector::kDetectorUndef)
{
ULOGGER_DEBUG("new detector strategy %d", int(detector));
if(_keypointDetector)
{
delete _keypointDetector;
_keypointDetector = 0;
}
switch(detectorStrategy)
switch(detector)
{
case kDetectorStar:
case KeypointDetector::kDetectorStar:
_keypointDetector = new StarDetector(parameters);
break;
case kDetectorSift:
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
break;
case kDetectorFast:
case KeypointDetector::kDetectorFast:
_keypointDetector = new FASTDetector(parameters);
break;
case kDetectorSurf:
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
break;
@@ -104,35 +125,29 @@ void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
_keypointDetector->parseParameters(parameters);
}
//Keypoint descriptor
DescriptorStrategy descriptorStrategy = kDescriptorUndef;
if((iter=parameters.find(Parameters::kKpDescriptorStrategy())) != parameters.end())
if(descriptor!=KeypointDescriptor::kDescriptorUndef)
{
descriptorStrategy = (DescriptorStrategy)std::atoi((*iter).second.c_str());
}
if(!_keypointDescriptor || descriptorStrategy!=kDescriptorUndef)
{
ULOGGER_DEBUG("new descriptor strategy %d", int(descriptorStrategy));
ULOGGER_DEBUG("new descriptor strategy %d", int(descriptor));
if(_keypointDescriptor)
{
delete _keypointDescriptor;
_keypointDescriptor = 0;
}
switch(descriptorStrategy)
switch(descriptor)
{
case kDescriptorSift:
case KeypointDescriptor::kDescriptorSift:
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case kDescriptorBrief:
case KeypointDescriptor::kDescriptorBrief:
_keypointDescriptor = new BRIEFDescriptor(parameters);
break;
case kDescriptorColor:
case KeypointDescriptor::kDescriptorColor:
_keypointDescriptor = new ColorDescriptor(parameters);
break;
case kDescriptorHue:
case KeypointDescriptor::kDescriptorHue:
_keypointDescriptor = new HueDescriptor(parameters);
break;
case kDescriptorSurf:
case KeypointDescriptor::kDescriptorSurf:
default:
_keypointDescriptor = new SURFDescriptor(parameters);
break;
@@ -142,55 +157,11 @@ void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
{
_keypointDescriptor->parseParameters(parameters);
}
CamPostTreatment::parseParameters(parameters);
}
CamKeypointTreatment::DetectorStrategy CamKeypointTreatment::detectorStrategy() const
void Camera::mainLoopBegin()
{
DetectorStrategy strategy = kDetectorUndef;
StarDetector * star = dynamic_cast<StarDetector*>(_keypointDetector);
SURFDetector * surf = dynamic_cast<SURFDetector*>(_keypointDetector);
if(star)
{
strategy = kDetectorStar;
}
else if(surf)
{
strategy = kDetectorSurf;
}
return strategy;
}
Camera::Camera(float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight) :
_imageRate(imageRate),
_autoRestart(autoRestart),
_imageWidth(imageWidth),
_imageHeight(imageHeight)
{
_postThreatement = new CamPostTreatment();
UEventsManager::addHandler(this);
}
Camera::~Camera()
{
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;
_frameRateTimer.start();
}
void Camera::mainLoop()
@@ -220,16 +191,6 @@ void Camera::mainLoop()
}
}
// ownership is transferred
void Camera::setPostThreatement(CamPostTreatment * strategy)
{
if(strategy)
{
delete _postThreatement;
_postThreatement = strategy;
}
}
void Camera::pushNewState(State newState, const ParametersMap & parameters)
{
ULOGGER_DEBUG("to %d", newState);
@@ -260,29 +221,105 @@ void Camera::handleEvent(UEvent* anEvent)
}
}
cv::Mat Camera::takeImage()
{
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
bool tmp = _featuresExtracted;
_featuresExtracted = false; // no need to extract descriptors/keypoints in this function
cv::Mat img = takeImage(descriptors, keypoints);
_featuresExtracted = tmp;
return img;
}
cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & keypoints)
{
descriptors = cv::Mat();
keypoints.clear();
if(_imageRate>0)
{
int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_frameRateTimer.getElapsedTime() < 1.0/double(_imageRate)-0.000001)
{
//
}
double slept = _frameRateTimer.getElapsedTime();
_frameRateTimer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
}
cv::Mat img;
if(!this->isKilled())
{
UTimer timer;
img = this->captureImage();
UDEBUG("Time capturing image = %fs", timer.ticks());
if(img.depth() != CV_8U)
{
UWARN("Images should have already 8U depth !?");
cv::Mat tmp = img;
img = cv::Mat();
tmp.convertTo(img, CV_8U);
UDEBUG("Time converting image to 8U = %fs", timer.ticks());
}
if(!img.empty())
{
if(_featuresExtracted && _keypointDetector && _keypointDescriptor)
{
keypoints = _keypointDetector->generateKeypoints(img);
descriptors = _keypointDescriptor->generateDescriptors(img, keypoints);
UDEBUG("Post treatment time = %fs", timer.ticks());
}
if(_framesDropped)
{
unsigned int count = 0;
while(count++ < _framesDropped)
{
cv::Mat tmp = this->captureImage();
if(!tmp.empty())
{
UDEBUG("frame dropped (%d/%d)", (int)count, (int)_framesDropped);
}
else
{
break;
}
}
UDEBUG("Frames dropped time = %fs", timer.ticks());
}
}
}
return img;
}
void Camera::process()
{
UTimer timer;
ULOGGER_DEBUG("Camera::process()");
SMState * smState = this->takeSMState();
if(smState)
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat img = this->takeImage(descriptors, keypoints);
if(!img.empty())
{
_postThreatement->process(smState);
this->post(new SMStateEvent(smState));
double elapsed = timer.ticks();
UDEBUG("Post treatment time = %fs", elapsed);
if(_imageRate>0)
if(_featuresExtracted)
{
float sleepTime = 1000.0f/_imageRate - 1000.0f*elapsed;
if(sleepTime > 0)
{
UDEBUG("Now sleeping for = %fms", sleepTime);
uSleep(sleepTime);
}
this->post(new CameraEvent(descriptors, keypoints, img, _id));
}
else
{
this->post(new CameraEvent(img, _id));
}
}
else
else if(!this->isKilled())
{
if(_autoRestart)
{
@@ -292,7 +329,7 @@ void Camera::process()
{
ULOGGER_DEBUG("Camera::process() : no more images...");
this->kill();
this->post(new CameraEvent());
this->post(new CameraEvent(_id));
}
}
}
@@ -307,12 +344,13 @@ CameraImages::CameraImages(const std::string & path,
float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, autoRestart, imageWidth, imageHeight),
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
Camera(imageRate, autoRestart, imageWidth, imageHeight, framesDropped, id),
_path(path),
_startAt(startAt),
_refreshDir(refreshDir),
_dir(0),
_count(0)
{
}
@@ -320,60 +358,48 @@ CameraImages::CameraImages(const std::string & path,
CameraImages::~CameraImages(void)
{
join(true);
if(_dir)
{
delete _dir;
_dir = 0;
}
}
bool CameraImages::init()
{
if(_dir)
{
delete _dir;
_dir = 0;
}
_dir = new UDirectory(_path, "jpg ppm png bmp pnm");
UDEBUG("");
_dir = UDirectory(_path, "jpg ppm png bmp pnm");
_count = 0;
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
{
_path.append("/");
}
if(!_dir)
if(!_dir.isValid())
{
ULOGGER_ERROR("Directory path not valid \"%s\"", _path.c_str());
ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
}
else if(_dir->getFileNames().size() == 0)
else if(_dir.getFileNames().size() == 0)
{
UWARN("Directory is empty \"%s\"", _path.c_str());
}
return _dir != 0;
return _dir.isValid();
}
IplImage * CameraImages::takeImage(std::list<std::vector<float> > * actions)
cv::Mat CameraImages::captureImage()
{
if(actions)
{
actions->clear();
}
IplImage * img = 0;
if(_dir)
UDEBUG("");
cv::Mat img;
if(_dir.isValid())
{
if(_refreshDir)
{
_dir->update();
_dir.update();
}
if(_startAt == 0)
{
const std::list<std::string> & fileNames = _dir->getFileNames();
const std::list<std::string> & fileNames = _dir.getFileNames();
if(fileNames.size())
{
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
{
_lastFileName = *fileNames.rbegin();
std::string fullPath = _path + _lastFileName;
img = cvLoadImage(fullPath.c_str(), CV_LOAD_IMAGE_COLOR);
img = cv::imread(fullPath.c_str());
}
}
}
@@ -381,18 +407,31 @@ IplImage * CameraImages::takeImage(std::list<std::vector<float> > * actions)
{
std::string fileName;
std::string fullPath;
fileName = _dir->getNextFileName();
fileName = _dir.getNextFileName();
if(fileName.size())
{
fullPath = _path + fileName;
while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
while(++_count < _startAt && (fileName = _dir.getNextFileName()).size())
{
fullPath = _path + fileName;
}
if(fileName.size())
{
ULOGGER_DEBUG("Loading image : %s\n", fullPath.c_str());
img = cvLoadImage(fullPath.c_str(), CV_LOAD_IMAGE_COLOR);
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
#else
img = cv::imread(fullPath.c_str(), -1);
#endif
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(img.depth() != CV_8U)
{
// The depth should be 8U
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
IplImage * i = cvLoadImage(fullPath.c_str());
img = cv::Mat(i, true);
cvReleaseImage(&i);
}
}
}
}
@@ -401,22 +440,18 @@ IplImage * CameraImages::takeImage(std::list<std::vector<float> > * actions)
{
UWARN("Directory is not set, camera must be initialized.");
}
if(img &&
if(!img.empty() &&
getImageWidth() &&
getImageHeight() &&
getImageWidth() != (unsigned int)img->width &&
getImageHeight() != (unsigned int)img->height)
getImageWidth() != (unsigned int)img.cols &&
getImageHeight() != (unsigned int)img.rows)
{
// declare a destination IplImage object with correct size, depth and channels
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);
cvReleaseImage(&img);
img = resampledImg;
cv::Mat resampled;
cv::resize(img, resampled, cv::Size(getImageWidth(), getImageHeight()));
img = resampled;
}
UDEBUG("");
return img;
}
@@ -429,9 +464,10 @@ CameraVideo::CameraVideo(int usbDevice,
float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, autoRestart, imageWidth, imageHeight),
_capture(0),
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
Camera(imageRate, autoRestart, imageWidth, imageHeight, framesDropped, id),
_src(kUsbDevice),
_usbDevice(usbDevice)
{
@@ -442,10 +478,11 @@ CameraVideo::CameraVideo(const std::string & fileName,
float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, autoRestart, imageWidth, imageHeight),
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
Camera(imageRate, autoRestart, imageWidth, imageHeight, framesDropped, id),
_fileName(fileName),
_capture(0),
_src(kVideoFile)
{
}
@@ -453,221 +490,71 @@ CameraVideo::CameraVideo(const std::string & fileName,
CameraVideo::~CameraVideo()
{
join(true);
if(_capture)
{
cvReleaseCapture(&_capture);
}
_capture.release();
}
bool CameraVideo::init()
{
if(_capture)
if(_capture.isOpened())
{
cvReleaseCapture(&_capture);
_capture = 0;
_capture.release();
}
if(_src == kUsbDevice)
{
ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, getImageWidth(), getImageHeight());
_capture = cvCaptureFromCAM(_usbDevice);
if(_capture && getImageWidth() && getImageHeight())
_capture.open(_usbDevice);
if(getImageWidth() && getImageHeight())
{
cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_WIDTH, double(getImageWidth()));
cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_HEIGHT, double(getImageHeight()));
_capture.set(CV_CAP_PROP_FRAME_WIDTH, double(getImageWidth()));
_capture.set(CV_CAP_PROP_FRAME_HEIGHT, double(getImageHeight()));
}
}
else if(_src == kVideoFile)
{
ULOGGER_DEBUG("CameraVideo::init() filename=\"%s\"", _fileName.c_str());
_capture = cvCaptureFromAVI(_fileName.c_str());
ULOGGER_DEBUG("Camera: filename=\"%s\"", _fileName.c_str());
_capture.open(_fileName.c_str());
}
else
{
ULOGGER_ERROR("CameraVideo::init() Unknown source...");
ULOGGER_ERROR("Camera: Unknown source...");
}
if(!_capture)
if(!_capture.isOpened())
{
ULOGGER_ERROR("CameraVideo::init() Failed to create a capture object!");
ULOGGER_ERROR("Camera: Failed to create a capture object!");
_capture.release();
return false;
}
return true;
}
IplImage * CameraVideo::takeImage(std::list<std::vector<float> > * actions)
cv::Mat CameraVideo::captureImage()
{
if(actions)
cv::Mat img; // Null image
if(_capture.isOpened())
{
actions->clear();
}
IplImage * img = 0; // Null image
if(_capture)
{
if(!cvGrabFrame(_capture)){ // capture a frame
ULOGGER_WARN("CameraVideo: Could not grab a frame, the end of the feed may be reached...");
}
else
{
img=cvRetrieveFrame(_capture); // retrieve the captured frame
}
_capture.read(img);
}
else
{
ULOGGER_WARN("CameraVideo::takeImage() The camera must be initialized before requesting an image.");
ULOGGER_WARN("The camera must be initialized before requesting an image.");
}
if(img &&
if(!img.empty() &&
getImageWidth() &&
getImageHeight() &&
getImageWidth() != (unsigned int)img->width &&
getImageHeight() != (unsigned int)img->height)
getImageWidth() != (unsigned int)img.cols &&
getImageHeight() != (unsigned int)img.rows)
{
// declare a destination IplImage object with correct size, depth and channels
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);
img = resampledImg;
}
else if(img)
{
img = cvCloneImage(img);
}
return img;
}
/////////////////////////
// CameraDatabase
/////////////////////////
CameraDatabase::CameraDatabase(const std::string & path,
bool loadActions,
float imageRate,
bool autoRestart,
unsigned int imageWidth,
unsigned int imageHeight) :
Camera(imageRate, autoRestart, imageWidth, imageHeight),
_path(path),
_loadActions(loadActions),
_indexIter(_ids.begin()),
_dbDriver(0)
{
}
CameraDatabase::~CameraDatabase(void)
{
join(true);
if(_dbDriver)
{
_dbDriver->closeConnection();
delete _dbDriver;
}
}
bool CameraDatabase::init()
{
if(_dbDriver)
{
_dbDriver->closeConnection();
delete _dbDriver;
_dbDriver = 0;
}
_ids.clear();
_indexIter = _ids.begin();
std::string driverType = "sqlite3";
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
_dbDriver = rtabmap::DBDriverFactory::createDBDriver(driverType, parameters);
if(!_dbDriver)
{
ULOGGER_ERROR("CameraDatabase::init() can't create \"%s\" driver",driverType.c_str());
return false;
}
else if(!_dbDriver->openConnection(_path.c_str()))
{
ULOGGER_ERROR("CameraDatabase::init() Can't read database \"%s\"",_path.c_str());
return false;
cv::Mat resampled;
cv::resize(img, resampled, cv::Size(getImageWidth(), getImageHeight()));
return resampled;
}
else
{
_dbDriver->getAllSignatureIds(_ids);
_indexIter = _ids.begin();
// clone required
return img.clone();
}
return true;
}
IplImage * CameraDatabase::takeImage(std::list<std::vector<float> > * actions)
{
if(actions)
{
actions->clear();
}
IplImage * img = 0;
if(_dbDriver && _indexIter != _ids.end())
{
// Get image
_dbDriver->getImage(*_indexIter, &img);
// Get actions from its previous neighbor
if(actions && _loadActions)
{
if(*_indexIter-1 > 0)
{
NeighborsMultiMap neighbors;
_dbDriver->loadNeighbors(*_indexIter-1, neighbors);
for(NeighborsMultiMap::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(iter->first>*_indexIter-1 && iter->second.actions().size())
{
*actions = iter->second.actions();
break;
}
}
if(actions->size() == 0)
{
UWARN("actions from previous %d to current %d are null", *_indexIter-1, *_indexIter);
}
}
}
++_indexIter;
}
else if(!_dbDriver)
{
ULOGGER_WARN("The camera must be initialized first...");
}
else if(_ids.size() == 0)
{
ULOGGER_WARN("The database \"%s\" is empty...", _path.c_str());
}
if(img &&
getImageWidth() &&
getImageHeight() &&
getImageWidth() != (unsigned int)img->width &&
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 );
//use cvResize to resize source to a destination image (linear interpolation)
cvResize(img, resampledImg);
cvReleaseImage(&img);
img = resampledImg;
}
return img;
}
} // namespace rtabmap
corelib/src/ColorTable.cpp
+7 -9
View File
@@ -5,7 +5,7 @@
*, Author: MatLab
*/
#include "ColorTable.h"
#include "rtabmap/core/ColorTable.h"
#include "ColorIndexes65536_bin_zip.h"
#include "ColorIndexes1024_bin_zip.h"
#include "ColorIndexes512_bin_zip.h"
@@ -75,21 +75,19 @@ ColorTable::ColorTable(int size) :
uLongf totalUncompressed = uncompressed.size();
UDEBUG("zip bytes=%d, uncompressed prediction=%d", (int)bytes.size(), (int)totalUncompressed);
int err_code = uncompress((Bytef*)uncompressed.data(), &totalUncompressed, (const Bytef*)bytes.data(), bytes.size());
if(err_code == Z_OK)
UDEBUG("totalUncompressed=%ld", totalUncompressed);
if(err_code == Z_MEM_ERROR)
{
UDEBUG("Ok! totalUncompressed=%ld", totalUncompressed);
}
else if(err_code == Z_MEM_ERROR)
{
UFATAL("Z_MEM_ERROR");
UFATAL("Z_MEM_ERROR : Insufficient memory.");
}
else if(err_code == Z_BUF_ERROR)
{
UFATAL("Z_BUF_ERROR");
UFATAL("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
}
else if(err_code == Z_DATA_ERROR)
{
UFATAL("Z_DATA_ERROR");
UFATAL("Z_DATA_ERROR : The compressed data (referenced by source) was corrupted.");
}
_rgb2indexed = std::vector<unsigned short>(uncompressed.size()/sizeof(unsigned short));
corelib/src/ColorTable.h
-42
View File
@@ -1,42 +0,0 @@
#ifndef COLORTABLE_H
#define COLORTABLE_H
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <vector>
namespace rtabmap
{
class RTABMAP_EXP 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
+47 -41
View File
@@ -20,7 +20,8 @@
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/Signature.h"
#include "VWDictionary.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/VisualWord.h"
#include "utilite/UConversion.h"
#include "utilite/UMath.h"
#include "utilite/ULogger.h"
@@ -33,7 +34,6 @@ DBDriver::DBDriver(const ParametersMap & parameters) :
_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
_imagesCompressed(Parameters::defaultDbImagesCompressed()),
_asyncWaiting(true),
_emptyTrashesTime(0)
{
this->parseParameters(parameters);
@@ -109,16 +109,12 @@ long DBDriver::getMemoryUsed() const
void DBDriver::mainLoop()
{
UDEBUG("");
this->emptyTrashes();
UDEBUG("");
this->kill(); // Do it only once
UDEBUG("");
}
void DBDriver::killCleanup()
{
UDEBUG("");
}
void DBDriver::beginTransaction() const
@@ -166,8 +162,6 @@ void DBDriver::emptyTrashes(bool async)
_trashSignatures.clear();
_trashVisualWords.clear();
_asyncWaiting = true;
_dbSafeAccessMutex.lock();
}
_trashesMutex.unlock();
@@ -219,32 +213,35 @@ void DBDriver::emptyTrashes(bool async)
void DBDriver::asyncSave(Signature * s)
{
_trashesMutex.lock();
if(s)
{
_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
if(_trashSignatures.size() > _minSignaturesToSave && this->isRunning() && _asyncWaiting)
UDEBUG("s=%d", s->id());
_trashesMutex.lock();
{
ULOGGER_DEBUG("(Sign) Releasing addSem...");
_asyncWaiting = false;
this->start();
_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
if(_trashSignatures.size() > _minSignaturesToSave && this->isIdle())
{
this->start();
}
}
_trashesMutex.unlock();
}
_trashesMutex.unlock();
}
void DBDriver::asyncSave(VisualWord * vw)
{
_trashesMutex.lock();
if(vw)
{
_trashVisualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
if(_trashVisualWords.size() > _minWordsToSave && this->isRunning() && _asyncWaiting)
_trashesMutex.lock();
{
ULOGGER_DEBUG("(Word) Releasing addSem...");
_asyncWaiting = false;
this->start();
_trashVisualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
if(_trashVisualWords.size() > _minWordsToSave && this->isIdle())
{
this->start();
}
}
_trashesMutex.unlock();
}
_trashesMutex.unlock();
}
bool DBDriver::getSignature(int signatureId, Signature ** s)
@@ -350,11 +347,11 @@ bool DBDriver::load(VWDictionary * dictionary) const
return r;
}
bool DBDriver::loadLastSignatures(std::list<Signature *> & signatures) const
bool DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadLastSignaturesQuery(signatures);
r = this->loadLastNodesQuery(signatures);
_dbSafeAccessMutex.unlock();
return r;
}
@@ -605,7 +602,7 @@ bool DBDriver::deleteAllObsoleteSSVWLinks() const
if(this->isConnected())
{
std::string query;
query += "DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT id FROM VisualWord WHERE id = Map_SS_VW.visualWordId);";
query += "DELETE FROM Map_Node_Word WHERE NOT EXISTS (SELECT id FROM Word WHERE id = Map_Node_Word.word_id);";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
@@ -620,7 +617,7 @@ bool DBDriver::deleteUnreferencedWords() const
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::string query = "DELETE FROM visualword WHERE id NOT IN (SELECT visualWordid FROM map_ss_vw);";
std::string query = "DELETE FROM Word WHERE id NOT IN (SELECT word_id FROM Map_Node_Word);";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
_dbSafeAccessMutex.unlock();
@@ -630,16 +627,25 @@ bool DBDriver::deleteUnreferencedWords() const
}
//TODO Check also in the trash ?
bool DBDriver::getImage(int id, IplImage ** img) const
bool DBDriver::getRawData(int id, std::list<Sensor> & rawData) const
{
_dbSafeAccessMutex.lock();
bool result = this->getImageQuery(id, img);
bool result = this->getRawDataQuery(id, rawData);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getNeighborIds(int signatureId, std::list<int> & neighbors, bool onlyWithActions) const
bool DBDriver::getActuatorData(int id, std::list<Actuator> & data) const
{
_dbSafeAccessMutex.lock();
bool result = this->getActuatorDataQuery(id, data);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions) const
{
bool r;
_dbSafeAccessMutex.lock();
@@ -679,50 +685,50 @@ bool DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::
}
//TODO Check also in the trash ?
bool DBDriver::getAllSignatureIds(std::set<int> & ids) const
bool DBDriver::getAllNodeIds(std::set<int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getAllSignatureIdsQuery(ids);
r = this->getAllNodeIdsQuery(ids);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLastSignatureId(int & id) const
bool DBDriver::getLastNodeId(int & id) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLastSignatureIdQuery(id);
r = this->getLastNodeIdQuery(id);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLastVisualWordId(int & id) const
bool DBDriver::getLastWordId(int & id) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLastVisualWordIdQuery(id);
r = this->getLastWordIdQuery(id);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getSurfNi(int signatureId, int & ni) const
bool DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getSurfNiQuery(signatureId, ni);
r = this->getInvertedIndexNiQuery(signatureId, ni);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const
bool DBDriver::getHighestWeightedNodeIds(unsigned int count, std::multimap<int, int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getHighestWeightedSignaturesQuery(count, ids);
r = this->getHighestWeightedNodeIdsQuery(count, ids);
_dbSafeAccessMutex.unlock();
return r;
}
@@ -733,7 +739,7 @@ bool DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int proce
if(this->isConnected())
{
std::stringstream query;
query << "INSERT INTO StatisticsAfterRun(stMemSize,lastSignAdded,processMemUsed,databaseMemUsed) values("
query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used) values("
<< stMemSize << ","
<< lastSignAdded << ","
<< processMemUsed << ","
@@ -751,7 +757,7 @@ bool DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
if(this->isConnected())
{
std::stringstream query;
query << "INSERT INTO StatisticsAfterRunSurf(dictionarySize) values(" << dictionarySize << ");";
query << "INSERT INTO StatisticsDictionary(dictionary_size) values(" << dictionarySize << ");";
bool r = this->executeNoResultQuery(query.str());
return r;
corelib/src/DBDriverSqlite3.cpp
+799 -895
View File
File diff suppressed because it is too large Load Diff
corelib/src/DBDriverSqlite3.h
+18 -16
View File
@@ -50,7 +50,7 @@ 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::list<int> & neighbors, bool onlyWithActions = false) const;
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
virtual bool getWeightQuery(int signatureId, int & weight) const;
virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
@@ -60,7 +60,7 @@ private:
// Load objects
virtual bool loadQuery(VWDictionary * dictionary) const;
virtual bool loadLastSignaturesQuery(std::list<Signature *> & signatures) const;
virtual bool loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual bool loadQuery(int signatureId, Signature ** s) const;
virtual bool loadQuery(int wordId, VisualWord ** vw) const;
virtual bool loadQuery(int signatureId, KeypointSignature * ss) const;
@@ -69,28 +69,30 @@ private:
virtual bool loadSMSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual bool loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const;
bool loadNeighborsQuery(std::list<Signature *> & signatures) const;
bool loadLinksQuery(std::list<Signature *> & signatures) 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 getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int, int> & ids) const;
virtual bool getRawDataQuery(int id, std::list<Sensor> & rawData) const;
virtual bool getActuatorDataQuery(int id, std::list<Actuator> & data) const;
virtual bool getAllNodeIdsQuery(std::set<int> & ids) const;
virtual bool getLastNodeIdQuery(int & id) const;
virtual bool getLastWordIdQuery(int & id) const;
virtual bool getInvertedIndexNiQuery(int signatureId, int & ni) const;
virtual bool getHighestWeightedNodeIdsQuery(unsigned int count, std::multimap<int, int> & ids) const;
private:
std::string queryStepSignature() const;
std::string queryStepImage() const;
std::string queryStepNeighborLink() const;
std::string queryStepNode() const;
std::string queryStepSensor() const;
std::string queryStepLink() const;
std::string queryStepActuator() 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 stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
int stepSensor(sqlite3_stmt * ppStmt, int id, int num, const std::vector<int> & data, const Sensor & sensor) const;
int stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, int actuator_id, const std::vector<int> & baseIds) const;
int stepActuator(sqlite3_stmt * ppStmt, int id, int num, const Actuator & actuator) const;
int stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
int stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
int stepSensors(sqlite3_stmt * ppStmt, const SMSignature * s) const;
private:
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
corelib/src/DBReader.cpp
+194
View File
@@ -0,0 +1,194 @@
/*
* DBReader.cpp
*
* Created on: 2012-06-13
* Author: mathieu
*/
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/SensorimotorEvent.h"
#include "rtabmap/core/DBDriverFactory.h"
#include <utilite/ULogger.h>
#include <utilite/UEventsManager.h>
#include <utilite/UFile.h>
namespace rtabmap {
DBReader::DBReader(const std::string & databasePath,
float frameRate,
const std::set<Sensor::Type> & sensorTypes,
const std::set<Actuator::Type> & actuatorTypes) :
_path(databasePath),
_frameRate(frameRate),
_sensorTypes(sensorTypes),
_actuatorTypes(actuatorTypes),
_dbDriver(0),
_currentId(_ids.end())
{
}
DBReader::~DBReader()
{
if(_dbDriver)
{
_dbDriver->closeConnection();
delete _dbDriver;
}
}
bool DBReader::init()
{
if(_dbDriver)
{
_dbDriver->closeConnection();
delete _dbDriver;
_dbDriver = 0;
}
_ids.clear();
_currentId=_ids.end();
if(!UFile::exists(_path))
{
UERROR("Database path does not exist (%s)", _path.c_str());
return false;
}
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
_dbDriver = DBDriverFactory::createDBDriver("sqlite3", parameters);
if(!_dbDriver)
{
UERROR("Driver doesn't exist.");
return false;
}
if(!_dbDriver->openConnection(_path))
{
UERROR("Can't open database %s", _path.c_str());
delete _dbDriver;
_dbDriver = 0;
return false;
}
_dbDriver->getAllNodeIds(_ids);
_currentId = _ids.begin();
return true;
}
void DBReader::setFrameRate(float frameRate)
{
if(frameRate >= 0.0f)
{
_frameRate = frameRate;
}
}
void DBReader::mainLoopBegin()
{
_timer.start();
}
void DBReader::mainLoop()
{
std::list<Sensor> sensors;
std::list<Actuator> actuators;
this->getNextSensorimotorState(sensors, actuators);
if(!sensors.empty() || !actuators.empty())
{
UEventsManager::post(new SensorimotorEvent(sensors, actuators));
}
else if(!this->isKilled())
{
UDEBUG("no more sensorimotor states...");
this->kill();
UEventsManager::post(new SensorimotorEvent());
}
}
void DBReader::getNextSensorimotorState(std::list<Sensor> & sensors, std::list<Actuator> & actuators)
{
sensors.clear();
actuators.clear();
if(_dbDriver)
{
float frameRate = _frameRate;
if(frameRate>0.0f)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
if(!this->isKilled() && _currentId != _ids.end())
{
//sensors
_dbDriver->getRawData(*_currentId, sensors);
//actuators
NeighborsMultiMap neighbors;
++_currentId;
if(_currentId != _ids.end())
{
_dbDriver->getActuatorData(*_currentId, actuators);
}
UDEBUG("sensors.size=%d actuators.size=%d", sensors.size(), actuators.size());
//filtering for types wanted
if(_sensorTypes.size())
{
for(std::list<Sensor>::iterator jter=sensors.begin(); jter!=sensors.end();)
{
if(_sensorTypes.find((Sensor::Type)jter->type()) == _sensorTypes.end())
{
jter = sensors.erase(jter);
}
else
{
++jter;
}
}
}
if(_actuatorTypes.size())
{
for(std::list<Actuator>::iterator jter=actuators.begin(); jter!=actuators.end();)
{
if(_actuatorTypes.find((Actuator::Type)jter->type()) == _actuatorTypes.end())
{
jter = actuators.erase(jter);
}
else
{
++jter;
}
}
}
UDEBUG("after filtering sensors.size=%d actuators.size=%d", sensors.size(), actuators.size());
}
}
else
{
UERROR("Not initialized...");
}
}
} /* namespace rtabmap */
corelib/src/EpipolarGeometry.cpp
+350 -42
View File
@@ -19,9 +19,13 @@
#include "rtabmap/core/EpipolarGeometry.h"
#include "utilite/ULogger.h"
#include "utilite/UTimer.h"
#include "utilite/UStl.h"
#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>
namespace rtabmap
{
@@ -41,71 +45,375 @@ void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Ve
return;
}
CvMat * w = cvCreateMat(3, 3, CV_64FC1);
CvMat * u = cvCreateMat(3, 3, CV_64FC1);
CvMat * v = cvCreateMat(3, 3, CV_64FC1);
CvMat f = fundamentalMatrix;
cvSVD(&f, w, u, v);
cv::SVD svd(fundamentalMatrix);
cv::Mat u = svd.u;
cv::Mat v = svd.vt;
cv::Mat w = svd.w;
// v is for image 1
// u is for image 2
e1[0] = v->data.db[0*3+2];// /v->data.db[2*3+2];
e1[1] = v->data.db[1*3+2];// /v->data.db[2*3+2];
e1[2] = v->data.db[2*3+2];// /v->data.db[2*3+2];
e1[0] = v.at<double>(0,2);// /v->data.db[2*3+2];
e1[1] = v.at<double>(1,2);// /v->data.db[2*3+2];
e1[2] = v.at<double>(2,2);// /v->data.db[2*3+2];
e2[0] = u->data.db[0*3+2];// /u->data.db[2*3+2];
e2[1] = u->data.db[1*3+2];// /u->data.db[2*3+2];
e2[2] = u->data.db[2*3+2];// /u->data.db[2*3+2];
cvReleaseMat(&w);
cvReleaseMat(&u);
cvReleaseMat(&v);
e2[0] = u.at<double>(0,2);// /u->data.db[2*3+2];
e2[1] = u.at<double>(1,2);// /u->data.db[2*3+2];
e2[2] = u.at<double>(2,2);// /u->data.db[2*3+2];
}
// P2 = [M | t] = [[e']_x * F | e']
void findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2)
//Assuming P0 = [eye(3) zeros(3,1)]
// x1 and x2 are 2D points
// return camera matrix P (3x4) matrix
cv::Mat findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const cv::Mat & x2)
{
if(p2.rows != 3 || p2.cols != 4 || fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
{
ULOGGER_ERROR("Matrices are not the good size... ");
return;
return cv::Mat();
}
if(p2.type()!= CV_64FC1 || fundamentalMatrix.type() != CV_64FC1)
if(fundamentalMatrix.type() != CV_64FC1)
{
ULOGGER_ERROR("Matrices are not the good type...");
return;
return cv::Mat();
}
if(e2[0] == 0 && e2[1] == 0 && e2[2] == 0)
// P matrix 3x4
cv::Mat p = cv::Mat::zeros(3, 4, CV_64FC1);
// P0 matrix 3X4
cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
p0.at<double>(0,0) = 1;
p0.at<double>(1,1) = 1;
p0.at<double>(2,2) = 1;
// cv::SVD doesn't five same results as cvSVD ?!? cvSVD return same values as in MatLab
/*cv::SVD svd(fundamentalMatrix);
cv::Mat u = svd.u;
cv::Mat v = svd.vt;
cv::Mat s = svd.w;
cv::Mat e = u.col(2);*/
CvMat F = fundamentalMatrix;
cv::Mat u(3,3,CV_64F);
cv::Mat v(3,3,CV_64F);
cv::Mat s(3,3,CV_64F);
CvMat U = u;
CvMat S = s;
CvMat V = v;
cvSVD(&F, &S, &U, &V, CV_SVD_U_T|CV_SVD_V_T); // F = U D V^T
u = u.t();
//
// INFO: may be required to multiply by -1 the last column of U
// TODO: Is any way to detect when it is required to do that ? When
// it is wrong, triangulated points have their Z value below 1 (between 0 and 1)...
//
/*u.at<double>(0,2) = -u.at<double>(0,2);
u.at<double>(1,2) = -u.at<double>(1,2);
u.at<double>(2,2) = -u.at<double>(2,2);*/
v = v.t();
cv::Mat e = u.col(2);
//std::cout << "u=" << u << std::endl;
//std::cout << "v=" << v << std::endl;
//std::cout << "s=" << s << std::endl;
// skew matrix 3X3
cv::Mat skew = cv::Mat::zeros( 3, 3, CV_64FC1);
skew.at<double>(0,1) = -1;
skew.at<double>(1,0) = 1;
skew.at<double>(2,2) = 1;
cv::Mat r;
cv::Mat x4d;
cv::Mat x = x1.col(0); // just take one point
cv::Mat xp = x2.col(0); // just take one point
// INFO: There 4 cases of P, only one have the points in
// front of the two cameras (positive z).
// Case 1 : P = [U*W*V' e];
r = u*skew*v.t();
p.at<double>(0,0) = r.at<double>(0,0);
p.at<double>(0,1) = r.at<double>(0,1);
p.at<double>(0,2) = r.at<double>(0,2);
p.at<double>(1,0) = r.at<double>(1,0);
p.at<double>(1,1) = r.at<double>(1,1);
p.at<double>(1,2) = r.at<double>(1,2);
p.at<double>(2,0) = r.at<double>(2,0);
p.at<double>(2,1) = r.at<double>(2,1);
p.at<double>(2,2) = r.at<double>(2,2);
p.at<double>(0,3) = e.at<double>(0,0);
p.at<double>(1,3) = e.at<double>(1,0);
p.at<double>(2,3) = e.at<double>(2,0);
cv::triangulatePoints(p0, p, x, xp, x4d);
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
cv::Mat xt1 = p0*x4d;
cv::Mat xt2 = p*x4d;
if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
{
cv::Vec3d e1;
findEpipolesFromF(fundamentalMatrix, e1, e2);
// Case 2 : P = [U*W*V' -e];
p.at<double>(0,3) = -e.at<double>(0,0);
p.at<double>(1,3) = -e.at<double>(1,0);
p.at<double>(2,3) = -e.at<double>(2,0);
cv::triangulatePoints(p0, p, x, xp, x4d);
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
xt1 = p0*x4d;
xt2 = p*x4d;
if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
{
// Case 3 : P = [U*W'*V' e];
r = u*skew.t()*v.t();
p.at<double>(0,0) = r.at<double>(0,0);
p.at<double>(0,1) = r.at<double>(0,1);
p.at<double>(0,2) = r.at<double>(0,2);
p.at<double>(1,0) = r.at<double>(1,0);
p.at<double>(1,1) = r.at<double>(1,1);
p.at<double>(1,2) = r.at<double>(1,2);
p.at<double>(2,0) = r.at<double>(2,0);
p.at<double>(2,1) = r.at<double>(2,1);
p.at<double>(2,2) = r.at<double>(2,2);
p.at<double>(0,3) = e.at<double>(0,0);
p.at<double>(1,3) = e.at<double>(1,0);
p.at<double>(2,3) = e.at<double>(2,0);
p.col(3) = e;
cv::triangulatePoints(p0, p, x, xp, x4d);
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
xt1 = p0*x4d;
xt2 = p*x4d;
if(xt1.at<double>(2,0) < 0 || xt2.at<double>(2,0) < 0)
{
// Case 4 : P = [U*W'*V' -e];
p.at<double>(0,3) = -e.at<double>(0,0);
p.at<double>(1,3) = -e.at<double>(1,0);
p.at<double>(2,3) = -e.at<double>(2,0);
cv::triangulatePoints(p0, p, x, xp, x4d);
x4d.at<double>(0) = x4d.at<double>(0)/x4d.at<double>(3);
x4d.at<double>(1) = x4d.at<double>(1)/x4d.at<double>(3);
x4d.at<double>(2) = x4d.at<double>(2)/x4d.at<double>(3);
x4d.at<double>(3) = x4d.at<double>(3)/x4d.at<double>(3);
xt1 = p0*x4d;
xt2 = p*x4d;
UDEBUG("Case 4");
}
else
{
UDEBUG("Case 3");
}
}
else
{
UDEBUG("Case 2");
}
}
else
{
UDEBUG("Case 1");
}
return p;
}
cv::Mat findFFromWords(
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
std::vector<uchar> & status,
double ransacParam1,
double ransacParam2)
{
status = std::vector<uchar>(pairs.size(), 0);
//Convert Keypoints to a structure that OpenCV understands
//3 dimensions (Homogeneous vectors)
cv::Mat points1(1, pairs.size(), CV_32FC2);
cv::Mat points2(1, pairs.size(), CV_32FC2);
float * points1data = points1.ptr<float>(0);
float * points2data = points2.ptr<float>(0);
// Fill the points here ...
int i=0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::const_iterator iter = pairs.begin();
iter != pairs.end();
++iter )
{
points1data[i*2] = (*iter).second.first.pt.x;
points1data[i*2+1] = (*iter).second.first.pt.y;
points2data[i*2] = (*iter).second.second.pt.x;
points2data[i*2+1] = (*iter).second.second.pt.y;
++i;
}
double e2_sd[3*3] = { 0., -e2[2], e2[1],
e2[2], 0., -e2[0],
-e2[1], e2[0], 0. };
CvMat e2_smt = cvMat( 3, 3, CV_64FC1, e2_sd );
cv::Mat e2_sm(&e2_smt); //;
UTimer timer;
timer.start();
cv::Mat m = e2_sm*fundamentalMatrix;
// Find the fundamental matrix
cv::Mat fundamentalMatrix = cv::findFundamentalMat(
points1,
points2,
status,
cv::FM_RANSAC,
ransacParam1,
ransacParam2);
p2.at<double>(0,0) = m.at<double>(0,0);
p2.at<double>(0,1) = m.at<double>(0,1);
p2.at<double>(0,2) = m.at<double>(0,2);
p2.at<double>(1,0) = m.at<double>(1,0);
p2.at<double>(1,1) = m.at<double>(1,1);
p2.at<double>(1,2) = m.at<double>(1,2);
p2.at<double>(2,0) = m.at<double>(2,0);
p2.at<double>(2,1) = m.at<double>(2,1);
p2.at<double>(2,2) = m.at<double>(2,2);
ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
p2.at<double>(0,3) = e2[0];
p2.at<double>(1,3) = e2[1];
p2.at<double>(2,3) = e2[2];
// Fundamental matrix is valid ?
bool fundMatFound = false;
UASSERT(fundamentalMatrix.type() == CV_64FC1);
if(fundamentalMatrix.cols==3 && fundamentalMatrix.rows==3 &&
(fundamentalMatrix.at<double>(0,0) != 0.0 ||
fundamentalMatrix.at<double>(0,1) != 0.0 ||
fundamentalMatrix.at<double>(0,2) != 0.0 ||
fundamentalMatrix.at<double>(1,0) != 0.0 ||
fundamentalMatrix.at<double>(1,1) != 0.0 ||
fundamentalMatrix.at<double>(1,2) != 0.0 ||
fundamentalMatrix.at<double>(2,0) != 0.0 ||
fundamentalMatrix.at<double>(2,1) != 0.0 ||
fundamentalMatrix.at<double>(2,2) != 0.0) )
{
fundMatFound = true;
}
ULOGGER_DEBUG("fm_count=%d...", fundMatFound);
if(fundMatFound)
{
// Show the fundamental matrix
UDEBUG(
"F = [%f %f %f;%f %f %f;%f %f %f]",
fundamentalMatrix.ptr<double>(0)[0],
fundamentalMatrix.ptr<double>(0)[1],
fundamentalMatrix.ptr<double>(0)[2],
fundamentalMatrix.ptr<double>(0)[3],
fundamentalMatrix.ptr<double>(0)[4],
fundamentalMatrix.ptr<double>(0)[5],
fundamentalMatrix.ptr<double>(0)[6],
fundamentalMatrix.ptr<double>(0)[7],
fundamentalMatrix.ptr<double>(0)[8]);
}
return fundamentalMatrix;
}
void findRTFromP(
const cv::Mat & p,
cv::Mat & r,
cv::Mat & t)
{
UASSERT(p.cols == 4 && p.rows == 3);
UDEBUG("");
r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
UDEBUG("");
r = -r.inv();
UDEBUG("r=%d %d, t=%d", r.cols, r.rows, p.col(3).rows);
t = r*p.col(3);
UDEBUG("");
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
*/
int findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
std::multimap<int, cv::KeyPoint>::const_iterator iterA;
std::multimap<int, cv::KeyPoint>::const_iterator iterB;
pairs.clear();
int realPairsCount = 0;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
iterA = wordsA.find(*i);
iterB = wordsB.find(*i);
while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
++iterA;
++iterB;
++realPairsCount;
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int realPairsCount = 0;
pairs.clear();
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
if(ptsA.size() == 1 && ptsB.size() == 1)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
++realPairsCount;
}
else if(ptsA.size()>1 && ptsB.size()>1)
{
// just update the count
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
* realPairsCount = 5
*/
int findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
UTimer timer;
timer.start();
const std::list<int> & ids = uUniqueKeys(wordsA);
pairs.clear();
int realPairsCount = 0;;
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
{
for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
}
}
}
ULOGGER_DEBUG("time = %f", timer.ticks());
return realPairsCount;
}
} // namespace rtabmap
corelib/src/KeypointDescriptor.cpp
+102 -87
View File
@@ -26,6 +26,9 @@
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/gpu/gpu.hpp>
#include <opencv2/core/version.hpp>
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
#include <opencv2/nonfree/features2d.hpp>
#endif
#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
@@ -48,14 +51,14 @@ void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
//SURFDescriptor
//////////////////////////
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters)
KeypointDescriptor(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
_params.hessianThreshold = Parameters::defaultSURFHessianThreshold();
_params.extended = Parameters::defaultSURFExtended();
_params.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
_params.nOctaves = Parameters::defaultSURFOctaves();
_params.upright = Parameters::defaultSURFUpright();
_gpuVersion = Parameters::defaultSURFGpuVersion();
this->parseParameters(parameters);
}
@@ -68,23 +71,27 @@ void SURFDescriptor::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_params.extended = uStr2Bool((*iter).second.c_str());
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_params.hessianThreshold = std::atof((*iter).second.c_str()); // is it needed for the descriptor?
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_params.nOctaveLayers = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_params.nOctaves = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_params.upright = uStr2Bool((*iter).second.c_str());
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
@@ -93,32 +100,31 @@ void SURFDescriptor::parseParameters(const ParametersMap & parameters)
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SURFDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
cv::Mat SURFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
IplImage * imageGrayScale = 0;
if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(imageGrayScale)
if(!imageGrayScale.empty())
{
img = cv::Mat(imageGrayScale);
img = imageGrayScale;
}
else
{
img = cv::Mat(image);
img = image;
}
#if OPENCV_SURF_GPU
/*#if OPENCV_SURF_GPU
if(_gpuVersion)
{
std::vector<float> d;
@@ -142,15 +148,15 @@ cv::Mat SURFDescriptor::generateDescriptors(const IplImage * image, std::vector<
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
extractor.compute(img, keypoints, descriptors);
}
#else*/
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SURF extractor(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#else
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
cv::SurfDescriptorExtractor extractor(_nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#endif
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
}
//#endif
return descriptors;
}
@@ -158,7 +164,12 @@ cv::Mat SURFDescriptor::generateDescriptors(const IplImage * image, std::vector<
//SIFTDescriptor
//////////////////////////
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters)
KeypointDescriptor(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
@@ -170,40 +181,64 @@ SIFTDescriptor::~SIFTDescriptor()
void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SIFTDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
IplImage * imageGrayScale = 0;
if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(imageGrayScale)
if(!imageGrayScale.empty())
{
img = cv::Mat(imageGrayScale);
img = imageGrayScale;
}
else
{
img = cv::Mat(image);
img = image;
}
cv::SiftDescriptorExtractor extractor(_descriptorParams, _commonParams);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT extractor(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
extractor.compute(img, keypoints, descriptors);
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
}
#else
cv::SIFT extractor(cv::SIFT::DescriptorParams::GET_DEFAULT_MAGNIFICATION(),
cv::SIFT::DescriptorParams::DEFAULT_IS_NORMALIZE,
true,
cv::SIFT::CommonParams::DEFAULT_NOCTAVES,
_nOctaveLayers);
extractor(img, cv::Mat(), keypoints, descriptors, true);
#endif
return descriptors;
}
@@ -231,38 +266,32 @@ void BRIEFDescriptor::parseParameters(const ParametersMap & parameters)
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat BRIEFDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
cv::Mat BRIEFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// BRIEF support only grayscale images ?
IplImage * imageGrayScale = 0;
if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(imageGrayScale)
if(!imageGrayScale.empty())
{
img = cv::Mat(imageGrayScale);
img = imageGrayScale;
}
else
{
img = cv::Mat(image);
img = image;
}
cv::BriefDescriptorExtractor brief(_size);
brief.compute(img, keypoints, descriptors);
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
}
return descriptors;
}
@@ -285,30 +314,29 @@ void ColorDescriptor::parseParameters(const ParametersMap & parameters)
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat ColorDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
cv::Mat ColorDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
IplImage * imageConverted = 0;
if(image->nChannels != 3 || image->depth != IPL_DEPTH_8U)
cv::Mat imageConverted;
if(image.channels() != 3 || image.depth() != CV_8U)
{
imageConverted = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 3);
cvCvtColor(image, imageConverted, CV_GRAY2BGR);
cv::cvtColor(image, imageConverted, CV_GRAY2BGR);
}
cv::Mat imgMat;
if(imageConverted)
if(!imageConverted.empty())
{
imgMat = cv::Mat(imageConverted);
imgMat = imageConverted;
}
else
{
imgMat = cv::Mat(image);
imgMat = image;
}
//create descriptors...
@@ -365,12 +393,6 @@ cv::Mat ColorDescriptor::generateDescriptors(const IplImage * image, std::vector
}
++i;
}
if(imageConverted)
{
cvReleaseImage(&imageConverted);
}
return descriptors;
}
@@ -404,30 +426,29 @@ void HueDescriptor::parseParameters(const ParametersMap & parameters)
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat HueDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
cv::Mat HueDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
IplImage * imageConverted = 0;
if(image->nChannels != 3 || image->depth != IPL_DEPTH_8U)
cv::Mat imageConverted;
if(image.channels() != 3 || image.depth() != CV_8U)
{
imageConverted = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 3);
cvCvtColor(image, imageConverted, CV_GRAY2BGR);
cv::cvtColor(image, imageConverted, CV_GRAY2BGR);
}
cv::Mat imgMat;
if(imageConverted)
if(!imageConverted.empty())
{
imgMat = cv::Mat(imageConverted);
imgMat = imageConverted;
}
else
{
imgMat = cv::Mat(image);
imgMat = image;
}
//create descriptors...
@@ -497,12 +518,6 @@ cv::Mat HueDescriptor::generateDescriptors(const IplImage * image, std::vector<c
memcpy(rowFl, &d[i*2], 2*sizeof(float));
++i;
}
if(imageConverted)
{
cvReleaseImage(&imageConverted);
}
return descriptors;
}
corelib/src/KeypointDetector.cpp
+103 -141
View File
@@ -18,15 +18,18 @@
*/
#include "rtabmap/core/KeypointDetector.h"
#include "VWDictionary.h"
#include "rtabmap/core/VWDictionary.h"
#include "utilite/ULogger.h"
#include "utilite/UTimer.h"
#include "utilite/UStl.h"
#include "rtabmap/core/Parameters.h"
#include "utilite/UConversion.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/gpu/gpu.hpp>
#include <opencv2/imgproc/imgproc.hpp>
//#include <opencv2/gpu/gpu.hpp>
#include <opencv2/core/version.hpp>
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
#include <opencv2/nonfree/features2d.hpp>
#endif
#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
@@ -35,8 +38,6 @@ namespace rtabmap
KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_usingAdaptiveResponseThr(Parameters::defaultKpUsingAdaptiveResponseThr()),
_adaptiveResponseThr(1),
_roiRatios(std::vector<float>(4, 0.0f))
{
this->setRoi(Parameters::defaultKpRoiRatios());
@@ -50,21 +51,17 @@ void KeypointDetector::parseParameters(const ParametersMap & parameters)
{
_wordsPerImageTarget = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpUsingAdaptiveResponseThr())) != parameters.end())
{
_usingAdaptiveResponseThr = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
}
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * image)
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & image)
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image)
if(!image.empty())
{
UTimer timer;
timer.start();
@@ -79,17 +76,8 @@ std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * i
// Variable hessian threshold
if(_wordsPerImageTarget > 0)
{
ULOGGER_DEBUG("_adaptiveResponseThr=%f", _adaptiveResponseThr);
if(keypoints.size() > 0)
{
if(keypoints.size() > _wordsPerImageTarget)
{
_adaptiveResponseThr *= 1+((float(keypoints.size())/float(_wordsPerImageTarget)-1)/1000);
}
else if(keypoints.size() < _wordsPerImageTarget)
{
_adaptiveResponseThr *= 1-((1-float(keypoints.size())/float(_wordsPerImageTarget))/1);
}
// 10% margin...
if(keypoints.size() > 1.1 * _wordsPerImageTarget)
{
@@ -115,12 +103,8 @@ std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * i
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());
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
}
else if(roi.x || roi.y)
{
@@ -132,18 +116,8 @@ std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * i
}
}
}
else
{
_adaptiveResponseThr /= 2;
}
if(_adaptiveResponseThr < this->getMinimumResponseThr())
{
_adaptiveResponseThr = this->getMinimumResponseThr();
}
ULOGGER_DEBUG("new _adaptiveResponseThr=%f", _adaptiveResponseThr);
ULOGGER_DEBUG("adjusting hessian threshold time = %f s", timer.ticks());
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
}
else if(roi.x || roi.y)
{
@@ -193,16 +167,16 @@ void KeypointDetector::setRoi(const std::string & roi)
}
}
cv::Rect KeypointDetector::computeRoi(const IplImage * image) const
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
{
if(image && _roiRatios.size() == 4)
if(!image.empty() && _roiRatios.size() == 4)
{
cv::Rect roi(0, 0, image->width, image->height);
float width = image.cols;
float height = image.rows;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
float width = image->width;
float height = image->height;
//left roi
if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
{
@@ -244,16 +218,15 @@ cv::Rect KeypointDetector::computeRoi(const IplImage * image) const
//SURFDetector
//////////////////////////
SURFDetector::SURFDetector(const ParametersMap & parameters) :
KeypointDetector(parameters)
KeypointDetector(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
_params.hessianThreshold = Parameters::defaultSURFHessianThreshold();
_params.extended = Parameters::defaultSURFExtended();
_params.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
_params.nOctaves = Parameters::defaultSURFOctaves();
_gpuVersion = Parameters::defaultSURFGpuVersion();
_params.upright = Parameters::defaultSURFUpright();
this->parseParameters(parameters);
this->setAdaptiveResponseThr(_params.hessianThreshold);
}
SURFDetector::~SURFDetector()
@@ -265,70 +238,62 @@ void SURFDetector::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_params.extended = uStr2Bool((*iter).second.c_str());
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_params.hessianThreshold = std::atof((*iter).second.c_str());
this->setAdaptiveResponseThr(_params.hessianThreshold);
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_params.nOctaveLayers = std::atoi((*iter).second.c_str());
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_params.nOctaves = std::atoi((*iter).second.c_str());
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_params.nOctaves = std::atoi((*iter).second.c_str());
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_params.upright = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
IplImage * imageGrayScale = 0;
if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(imageGrayScale)
if(!imageGrayScale.empty())
{
img = cv::Mat(imageGrayScale);
img = imageGrayScale;
}
else
{
img = cv::Mat(image);
}
CvSURFParams params = _params;
if(this->isUsingAdaptiveResponseThr())
{
params.hessianThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
img = image;
}
cv::Mat imgRoi(img, roi);
#if OPENCV_SURF_GPU
/*#if OPENCV_SURF_GPU
if(_gpuVersion )
{
cv::gpu::GpuMat imgGpu(imgRoi);
@@ -342,15 +307,15 @@ std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * imag
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
detector.detect(imgRoi, keypoints);
}
#else
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
#else*/
cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, cv::Mat(), keypoints);
#endif
//#endif
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
}
return keypoints;
}
@@ -358,12 +323,14 @@ std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * imag
//SIFTDetector
//////////////////////////
SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters)
KeypointDetector(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
_detectorParams.threshold = Parameters::defaultSIFTThreshold();
_detectorParams.edgeThreshold = Parameters::defaultSIFTEdgeThreshold();
this->parseParameters(parameters);
this->setAdaptiveResponseThr(_detectorParams.threshold);
}
SIFTDetector::~SIFTDetector()
@@ -373,57 +340,62 @@ SIFTDetector::~SIFTDetector()
void SIFTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTThreshold())) != parameters.end())
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_detectorParams.threshold = std::atof((*iter).second.c_str());
this->setAdaptiveResponseThr(_detectorParams.threshold);
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_detectorParams.edgeThreshold = std::atof((*iter).second.c_str());
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
IplImage * imageGrayScale = 0;
if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(imageGrayScale)
if(!imageGrayScale.empty())
{
img = cv::Mat(imageGrayScale);
img = imageGrayScale;
}
else
{
img = cv::Mat(image);
img = image;
}
cv::SIFT::DetectorParams detectorParam = _detectorParams;
if(this->isUsingAdaptiveResponseThr())
{
detectorParam.threshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
}
cv::SiftFeatureDetector detector(detectorParam, _commonParams);
cv::Mat imgRoi(img, roi);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT detector(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
detector.detect(imgRoi, keypoints); // Opencv surf keypoints
if(imageGrayScale)
{
cvReleaseImage(&imageGrayScale);
}
#else
cv::SIFT detector(_contrastThreshold, _edgeThreshold, cv::SIFT::CommonParams::DEFAULT_NOCTAVES, _nOctaveLayers);
detector(imgRoi, cv::Mat(), keypoints); // Opencv surf keypoints
#endif
return keypoints;
}
@@ -432,15 +404,14 @@ std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * imag
//StarDetector
//////////////////////////
StarDetector::StarDetector(const ParametersMap & parameters) :
KeypointDetector(parameters)
KeypointDetector(parameters),
_maxSize(Parameters::defaultStarMaxSize()),
_responseThreshold(Parameters::defaultStarResponseThreshold()),
_lineThresholdProjected(Parameters::defaultStarLineThresholdProjected()),
_lineThresholdBinarized(Parameters::defaultStarLineThresholdBinarized()),
_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(_params.responseThreshold);
}
StarDetector::~StarDetector()
@@ -453,51 +424,47 @@ void StarDetector::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kStarLineThresholdBinarized())) != parameters.end())
{
_params.lineThresholdBinarized = std::atoi((*iter).second.c_str());
_lineThresholdBinarized = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarLineThresholdProjected())) != parameters.end())
{
_params.lineThresholdProjected = std::atoi((*iter).second.c_str());
_lineThresholdProjected = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarMaxSize())) != parameters.end())
{
_params.maxSize = std::atoi((*iter).second.c_str());
_maxSize = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarResponseThreshold())) != parameters.end())
{
_params.responseThreshold = int(std::atof((*iter).second.c_str()));
this->setAdaptiveResponseThr(_params.responseThreshold);
_responseThreshold = int(std::atof((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kStarSuppressNonmaxSize())) != parameters.end())
{
_params.suppressNonmaxSize = std::atoi((*iter).second.c_str());
_suppressNonmaxSize = std::atoi((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> StarDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
cv::Mat img(image);
// TODO More testing needed with the star detector, NN search distance must be changed to 0.8
//find keypoints with the star detector
CvStarDetectorParams params = _params;
if(this->isUsingAdaptiveResponseThr())
{
params.responseThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
}
// Get keypoints with the star detector
cv::Mat imgRoi(img, roi);
cv::StarFeatureDetector detector(params);
cv::StarDetector detector(_maxSize, _responseThreshold, _lineThresholdProjected, _lineThresholdBinarized, _suppressNonmaxSize);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, keypoints);
#endif
return keypoints;
}
@@ -510,7 +477,6 @@ FASTDetector::FASTDetector(const ParametersMap & parameters) :
_nonmaxSuppression(Parameters::defaultFASTNonmaxSuppression())
{
this->parseParameters(parameters);
this->setAdaptiveResponseThr(_threshold);
}
FASTDetector::~FASTDetector()
@@ -531,11 +497,11 @@ void FASTDetector::parseParameters(const ParametersMap & parameters)
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> FASTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> FASTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image)
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
@@ -543,12 +509,8 @@ std::vector<cv::KeyPoint> FASTDetector::_generateKeypoints(const IplImage * imag
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);
cv::FastFeatureDetector fast(_threshold, _nonmaxSuppression);
// Get keypoints with the fast detector
fast.detect(imgRoi, keypoints);
corelib/src/KeypointMemory.cpp
+93 -81
View File
@@ -17,18 +17,18 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "KeypointMemory.h"
#include "VWDictionary.h"
#include "rtabmap/core/KeypointMemory.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/VisualWord.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/KeypointDetector.h"
#include "rtabmap/core/KeypointDescriptor.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "NearestNeighbor.h"
#include "VerifyHypotheses.h"
#include "rtabmap/core/NearestNeighbor.h"
#include "rtabmap/core/VerifyHypotheses.h"
#include "utilite/UStl.h"
#include <opencv2/core/core.hpp>
#include <set>
@@ -105,13 +105,12 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
}
//Keypoint detector
DetectorStrategy detectorStrategy = kDetectorUndef;
KeypointDetector::DetectorType detectorStrategy = KeypointDetector::kDetectorUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
{
detectorStrategy = (DetectorStrategy)std::atoi((*iter).second.c_str());
detectorStrategy = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
}
DetectorStrategy currentDetectorStrategy = this->detectorStrategy();
if(!_keypointDetector || ( detectorStrategy!=kDetectorUndef && (detectorStrategy != currentDetectorStrategy) ) )
if(!_keypointDetector || detectorStrategy!=KeypointDetector::kDetectorUndef)
{
ULOGGER_DEBUG("new detector strategy %d", int(detectorStrategy));
if(_keypointDetector)
@@ -121,16 +120,16 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
}
switch(detectorStrategy)
{
case kDetectorStar:
case KeypointDetector::kDetectorStar:
_keypointDetector = new StarDetector(parameters);
break;
case kDetectorSift:
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
break;
case kDetectorFast:
case KeypointDetector::kDetectorFast:
_keypointDetector = new FASTDetector(parameters);
break;
case kDetectorSurf:
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
break;
@@ -142,12 +141,12 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
}
//Keypoint descriptor
DescriptorStrategy descriptorStrategy = kDescriptorUndef;
KeypointDescriptor::DescriptorType descriptorStrategy = KeypointDescriptor::kDescriptorUndef;
if((iter=parameters.find(Parameters::kKpDescriptorStrategy())) != parameters.end())
{
descriptorStrategy = (DescriptorStrategy)std::atoi((*iter).second.c_str());
descriptorStrategy = (KeypointDescriptor::DescriptorType)std::atoi((*iter).second.c_str());
}
if(!_keypointDescriptor || descriptorStrategy!=kDescriptorUndef)
if(!_keypointDescriptor || descriptorStrategy!=KeypointDescriptor::kDescriptorUndef)
{
ULOGGER_DEBUG("new descriptor strategy %d", int(descriptorStrategy));
if(_keypointDescriptor)
@@ -157,19 +156,19 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
}
switch(descriptorStrategy)
{
case kDescriptorSift:
case KeypointDescriptor::kDescriptorSift:
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case kDescriptorBrief:
case KeypointDescriptor::kDescriptorBrief:
_keypointDescriptor = new BRIEFDescriptor(parameters);
break;
case kDescriptorColor:
case KeypointDescriptor::kDescriptorColor:
_keypointDescriptor = new ColorDescriptor(parameters);
break;
case kDescriptorHue:
case KeypointDescriptor::kDescriptorHue:
_keypointDescriptor = new HueDescriptor(parameters);
break;
case kDescriptorSurf:
case KeypointDescriptor::kDescriptorSurf:
default:
_keypointDescriptor = new SURFDescriptor(parameters);
break;
@@ -183,22 +182,6 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
Memory::parseParameters(parameters);
}
KeypointMemory::DetectorStrategy KeypointMemory::detectorStrategy() const
{
DetectorStrategy strategy = kDetectorUndef;
StarDetector * star = dynamic_cast<StarDetector*>(_keypointDetector);
SURFDetector * surf = dynamic_cast<SURFDetector*>(_keypointDetector);
if(star)
{
strategy = kDetectorStar;
}
else if(surf)
{
strategy = kDetectorSurf;
}
return strategy;
}
bool KeypointMemory::init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
{
UDEBUG("");
@@ -241,10 +224,10 @@ bool KeypointMemory::init(const std::string & dbDriverName, const std::string &
// TODO : Use only the parent method (in Memory)
// 1- Put "setEnabled" in the abstract Signature class, so this method is accessible from Memory
void KeypointMemory::addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions)
void KeypointMemory::addSignatureToStm(Signature * signature, const std::list<Actuator> & actuators)
{
ULOGGER_DEBUG("");
Memory::addSignatureToStm(signature, actions);
Memory::addSignatureToStm(signature, actuators);
UTimer timer;
KeypointSignature * ss = dynamic_cast<KeypointSignature *>(signature);
@@ -300,15 +283,15 @@ void KeypointMemory::clear()
// _dbDriver->commit();
//}
ULOGGER_DEBUG("");
_dbDriver->start();
}
else
{
cleanUnusedWords();
}
_commonWords.clear();
if(_vwd)
{
_vwd->clear();
}
}
void KeypointMemory::preUpdate()
@@ -337,11 +320,6 @@ std::multimap<int, cv::KeyPoint> getMostDescriptiveWords(const std::multimap<int
responseWordMap.insert(std::pair<float, std::pair<int, const cv::KeyPoint *> >(itKey->second.response, std::pair<int, const cv::KeyPoint *>(itKey->first, &(itKey->second))));
}
}
int endIndex = 0;
if(responseWordMap.size() > (unsigned int)max)
{
endIndex = responseWordMap.size() - max;
}
//add them
int i=0;
@@ -532,7 +510,7 @@ int KeypointMemory::getNi(int signatureId) const
}
else
{
_dbDriver->getSurfNi(signatureId, ni);
_dbDriver->getInvertedIndexNi(signatureId, ni);
}
return ni;
}
@@ -576,7 +554,7 @@ private:
VWDictionary * _vwp;
};
Signature * KeypointMemory::createSignature(int id, const SMState * smState, bool keepRawData)
Signature * KeypointMemory::createSignature(int id, const std::list<Sensor> & sensors, bool keepRawData)
{
PreUpdateThread preUpdateThread(_vwd);
@@ -584,46 +562,67 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
timer.start();
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
const IplImage * image = 0;
if(smState)
int treeSize= this->getWorkingMemSize() + this->getStMemSize();
int nbCommonWords = 0;
if(treeSize > 0)
{
int treeSize= this->getWorkingMemSize() + this->getStMemSize();
int nbCommonWords = 0;
if(treeSize > 0)
{
nbCommonWords = _vwd->getTotalActiveReferences() / treeSize;
}
nbCommonWords = _vwd->getTotalActiveReferences() / treeSize;
}
if(_parallelized)
{
preUpdateThread.start();
}
if(_parallelized)
{
preUpdateThread.start();
}
if(smState->getSensors().empty())
std::list<Sensor>::const_iterator iterImage = sensors.end();
std::list<Sensor>::const_iterator iterImageFeatures = sensors.end();
for(std::list<Sensor>::const_iterator iter=sensors.begin(); iter!=sensors.end(); ++iter)
{
if(iter->type() == Sensor::kTypeImage)
{
image = smState->getImage();
if(image && _keypointDetector)
iterImage = iter;
}
else if(iter->type() == Sensor::kTypeImageFeatures2d)
{
iterImageFeatures = iter;
}
}
if(iterImage == sensors.end() && iterImageFeatures == sensors.end())
{
UERROR("Keypoint memory only supports image[features] sensor type.");
return 0;
}
if(iterImageFeatures != sensors.end())
{
// DESCRIPTORS
if(iterImageFeatures->data().rows && iterImageFeatures->data().rows >= _badSignRatio * nbCommonWords)
{
if(iterImageFeatures->data().type() == CV_32F)
{
keypoints = _keypointDetector->generateKeypoints(image);
ULOGGER_DEBUG("time keypoints = %fs", timer.ticks());
descriptors = iterImageFeatures->data();
keypoints = iterImageFeatures->getKeypoints();
}
ULOGGER_DEBUG("ratio=%f, treeSize=%d, nbCommonWords=%d", _badSignRatio, treeSize, nbCommonWords);
if(keypoints.size() && keypoints.size() >= _badSignRatio * nbCommonWords)
else
{
descriptors = _keypointDescriptor->generateDescriptors(image, keypoints);
UERROR("Descriptors must be CV_32F.");
}
}
else
}
else
{
// IMAGE RAW
if(_keypointDetector)
{
if(smState->getSensors().rows >= _badSignRatio * nbCommonWords)
{
descriptors = smState->getSensors();
keypoints = smState->getKeypoints();
}
image = smState->getImage();
keypoints = _keypointDetector->generateKeypoints(iterImage->data());
ULOGGER_DEBUG("time keypoints = %fs", timer.ticks());
}
ULOGGER_DEBUG("ratio=%f, treeSize=%d, nbCommonWords=%d", _badSignRatio, treeSize, nbCommonWords);
if(keypoints.size() && keypoints.size() >= _badSignRatio * nbCommonWords)
{
descriptors = _keypointDescriptor->generateDescriptors(iterImage->data(), keypoints);
}
}
@@ -665,13 +664,26 @@ 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());
if(keypoints.size())
{
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()));
}
}
}
KeypointSignature * ks = new KeypointSignature(words, id, image, keepRawData);
KeypointSignature * ks;
std::list<Sensor> tmp;
if(keepRawData && iterImage != sensors.end())
{
UDEBUG("");
//Just keep image if there's one
tmp.push_back(*iterImage);
}
ks = new KeypointSignature(words, id, tmp);
ULOGGER_DEBUG("time new signature (id=%d) %fs", id, timer.ticks());
if(words.size())
{
corelib/src/KeypointMemory.h
-92
View File
@@ -1,92 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef KEYPOINTMEMORY_H_
#define KEYPOINTMEMORY_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "Memory.h"
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap {
class VWDictionary;
class VisualWord;
class KeypointDetector;
class KeypointDescriptor;
class RTABMAP_EXP KeypointMemory : public Memory
{
public:
enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
enum DescriptorStrategy {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
public:
KeypointMemory(const ParametersMap & parameters = ParametersMap());
virtual ~KeypointMemory();
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 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;
void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
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:
virtual Signature * getSignatureLtMem(int id);
virtual void addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >());
virtual void clear();
virtual void moveToTrash(Signature * s);
virtual void preUpdate();
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();
int getNi(int signatureId) const;
private:
std::list<int> _commonWords;
VWDictionary * _vwd;
KeypointDetector * _keypointDetector;
KeypointDescriptor * _keypointDescriptor;
//std::map<int, int> _wordRefsToChange;
bool _reactivatedWordsComparedToNewWords;
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
bool _tfIdfNormalized;
};
}
#endif /* KEYPOINTMEMORY_H_ */
corelib/src/Memory.cpp
+258 -236
View File
@@ -17,14 +17,19 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Memory.h"
#include <utilite/UEventsManager.h>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <utilite/UConversion.h>
#include <utilite/UProcessInfo.h>
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriverFactory.h"
#include "rtabmap/core/DBDriver.h"
#include "utilite/UtiLite.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/SMState.h"
#include "rtabmap/core/Sensor.h"
#include "Node.h"
namespace rtabmap {
@@ -90,7 +95,7 @@ bool Memory::init(const std::string & dbDriverName, const std::string & dbUrl, b
// Load the last working memory...
UEventsManager::post(new RtabmapEventInit(std::string("Loading last signatures...")));
std::list<Signature*> dbSignatures;
_dbDriver->loadLastSignatures(dbSignatures);
_dbDriver->loadLastNodes(dbSignatures);
for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
{
_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
@@ -112,7 +117,7 @@ bool Memory::init(const std::string & dbDriverName, const std::string & dbUrl, b
}
// Last id
_dbDriver->getLastSignatureId(_idCount);
_dbDriver->getLastNodeId(_idCount);
_idCount += 1;
}
else
@@ -125,7 +130,7 @@ bool Memory::init(const std::string & dbDriverName, const std::string & dbUrl, b
_idCount = kIdStart;
}
_dbDriver->start();
this->setCommonSignatureUsed(_commonSignatureUsed);
ULOGGER_DEBUG("ids start with %d", _idCount);
return success;
@@ -206,7 +211,7 @@ void Memory::preUpdate()
_signaturesAdded = 0;
}
bool Memory::update(const SMState * smState, std::map<std::string, float> & stats)
bool Memory::update(const std::list<Sensor> & sensors, const std::list<Actuator> & actuators, std::map<std::string, float> & stats)
{
ULOGGER_DEBUG("");
UTimer timer;
@@ -226,24 +231,19 @@ bool Memory::update(const SMState * smState, std::map<std::string, float> & stat
//============================================================
// Create a signature with the image received.
//============================================================
Signature * signature = this->createSignature(this->getNextId(), smState, this->isRawDataKept());
Signature * signature = this->createSignature(this->getNextId(), sensors, this->isRawDataKept());
if (signature == 0)
{
UFATAL("Failed to create a signature");
UERROR("Failed to create a signature");
return false;
}
// It will be added to the short-term memory, no need to delete it...
if(smState)
{
this->addSignatureToStm(signature, smState->getActuators());
}
else
{
this->addSignatureToStm(signature);
}
this->addSignatureToStm(signature, actuators);
_lastSignature = signature;
if(_lastLoopClosureId == 0)
if(_lastLoopClosureId == 0 && !signature->isBadSignature())
{
// If not set use the new one added
_lastLoopClosureId = signature->id();
@@ -264,29 +264,6 @@ bool Memory::update(const SMState * smState, std::map<std::string, float> & stat
stats.insert(std::pair<std::string, float>(std::string("TimingMem/Rehearsal/ms"), t));
ULOGGER_DEBUG("time rehearsal=%f ms", t);
//============================================================
// Update the common signature
//============================================================
if(_commonSignatureUsed)
{
Signature * s = _getSignature(kIdVirtual);
if(s == 0)
{
s = this->createSignature(kIdVirtual, 0); // Create a virtual place
_signatures.insert(std::pair<int, Signature *>(s->id(), s));
_workingMem.insert(s->id());
}
}
else
{
// remove virtual signature
Signature * s = _getSignature(kIdVirtual);
if(s)
{
this->moveToTrash(s);
}
}
//============================================================
// Transfer the oldest signature of the short-term memory to the working memory
//============================================================
@@ -349,24 +326,30 @@ void Memory::setRecentWmRatio(float recentWmRatio)
void Memory::setCommonSignatureUsed(bool commonSignatureUsed)
{
_commonSignatureUsed = commonSignatureUsed;
if(!_commonSignatureUsed)
if(_commonSignatureUsed)
{
this->moveToTrash(this->_getSignature(kIdVirtual));
_workingMem.insert(kIdVirtual);
}
else
{
// remove virtual signature
_workingMem.erase(kIdVirtual);
}
}
void Memory::addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions)
void Memory::addSignatureToStm(Signature * signature, const std::list<Actuator> & actuators)
{
UTimer timer;
// add signature on top of the short-term memory
if(signature)
{
UDEBUG("adding %d with a=%d", signature->id(), (int)actions.size());
UDEBUG("adding %d with a=%d", signature->id(), (int)actuators.size());
// Update neighbors
if(_stMem.size())
{
// In terms of sensorimotor learning...
_signatures.at(*_stMem.rbegin())->addNeighbor(NeighborLink(signature->id(), actions, _lastBaseIds));
// If you want to use Bayes's parameter _predictionOnNonNullActionsOnly with no actions, dummy actions must be sent (null actions are not handled in sqlite3 driver)
_signatures.at(*_stMem.rbegin())->addNeighbor(NeighborLink(signature->id(), _lastBaseIds, actuators, signature->id()));
// actions are not backward compatible, so set null actions
signature->addNeighbor(NeighborLink(*_stMem.rbegin()));
}
@@ -433,7 +416,7 @@ Signature * Memory::getSignatureLtMem(int id)
return s;
}
std::list<NeighborLink> Memory::getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure, bool lookInDatabase) const
std::list<NeighborLink> Memory::getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure, bool lookInDatabase, bool onlyWithActions) const
{
std::list<NeighborLink> links;
Signature * sTop = uValue(_signatures, signatureId, (Signature*)0);
@@ -450,7 +433,10 @@ std::list<NeighborLink> Memory::getNeighborLinks(int signatureId, bool ignoreNei
const NeighborsMultiMap & neighbors = s->getNeighbors();
for(NeighborsMultiMap::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
links.push_back(iter->second);
if(!onlyWithActions || (onlyWithActions && iter->second.actuators().size()))
{
links.push_back(iter->second);
}
}
if(!ignoreNeighborByLoopClosure)
{
@@ -486,7 +472,10 @@ std::list<NeighborLink> Memory::getNeighborLinks(int signatureId, bool ignoreNei
const NeighborsMultiMap & neighbors = s->getNeighbors();
for(NeighborsMultiMap::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
links.push_back(iter->second);
if(!onlyWithActions || (onlyWithActions && iter->second.actuators().size()))
{
links.push_back(iter->second);
}
}
const std::set<int> & childIds = s->getChildLoopClosureIds();
for(std::set<int>::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter)
@@ -506,7 +495,10 @@ std::list<NeighborLink> Memory::getNeighborLinks(int signatureId, bool ignoreNei
_dbDriver->loadNeighbors(signatureId, neighbors);
for(NeighborsMultiMap::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
links.push_back(iter->second);
if(!onlyWithActions || (onlyWithActions && iter->second.actuators().size()))
{
links.push_back(iter->second);
}
}
}
else
@@ -536,7 +528,6 @@ std::map<int, int> Memory::getNeighborsId(double & dbAccessTime,
{
return ids;
}
bool someLoadedFromDb = false;
int nbLoadedFromDb = 0;
std::list<int> currentMargin;
std::list<int> nextMargin;
@@ -546,127 +537,118 @@ std::map<int, int> Memory::getNeighborsId(double & dbAccessTime,
{
currentMargin = nextMargin;
nextMargin.clear();
someLoadedFromDb = false;
// first pass: count number of node in current margin in database
for(std::list<int>::iterator jter = currentMargin.begin(); jter!=currentMargin.end();++jter)
{
if(!uContains(ids, *jter))
{
const Signature * s = this->getSignature(*jter);
if(!s)
{
++nbLoadedFromDb;
}
}
}
for(std::list<int>::iterator jter = currentMargin.begin(); jter!=currentMargin.end(); ++jter)
{
if(ids.insert(std::pair<int, int>(*jter, m)).second)
{
// Look up in the short time memory if all ids are here, if not... load them from the database
const Signature * s = this->getSignature(*jter);
std::list<int> neighborIds;
std::list<int> loopIds;
std::list<int> childIds;
std::set<int> tmpNeighborIds;
std::set<int> tmpLoopClosureIds;
std::set<int> tmpChildLoopClosureIds;
const std::set<int> * neighborIds = &tmpNeighborIds;
const std::set<int> * loopClosureIds = &tmpLoopClosureIds;
const std::set<int> * childLoopClosureIds = &tmpChildLoopClosureIds;
if(s)
{
const NeighborsMultiMap & neighbors = s->getNeighbors();
const std::set<int> & loopClosureIds = s->getLoopClosureIds();
const std::set<int> & childLoopClosureIds = s->getChildLoopClosureIds();
int lastId = -1;
for(NeighborsMultiMap::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
if(onlyWithActions)
{
if( (ignoreLoopIds || (loopClosureIds.find(iter->first) == loopClosureIds.end() && childLoopClosureIds.find(iter->first) == childLoopClosureIds.end())) &&
(!onlyWithActions || iter->second.actions().size()) &&
(!ignoreSTM || (!_stMem.size() || iter->first < *_stMem.begin()) ) &&
lastId != iter->first)
{
neighborIds.push_back(iter->first);
}
lastId = iter->first; // just to ignore duplicates
neighborIds = &s->getNeighborsWithActuators();
}
else
{
neighborIds = &s->getNeighborsAll();
}
if(!ignoreLoopIds)
{
for(std::set<int>::const_iterator iter=loopClosureIds.begin(); iter!=loopClosureIds.end(); ++iter)
{
if( *iter &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin()) ))
{
loopIds.push_back(*iter);
}
}
for(std::set<int>::const_iterator iter=childLoopClosureIds.begin(); iter!=childLoopClosureIds.end(); ++iter)
{
if( *iter &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin()) ))
{
childIds.push_back(*iter);
}
}
loopClosureIds = &s->getLoopClosureIds();
childLoopClosureIds = &s->getChildLoopClosureIds();
}
}
else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
{
someLoadedFromDb = true;
std::set<int> loopClosureIds;
std::set<int> childLoopClosureIds;
UTimer timer;
_dbDriver->getNeighborIds(*jter, tmpNeighborIds, onlyWithActions);
if(!ignoreLoopIds)
{
UTimer timer;
_dbDriver->getLoopClosureIds(*jter, loopClosureIds, childLoopClosureIds);
dbAccessTime += timer.getElapsedTime();
for(std::set<int>::const_iterator iter = loopClosureIds.begin(); iter!=loopClosureIds.end(); ++iter)
{
if( *iter &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin()) ))
{
loopIds.push_back(*iter);
}
}
for(std::set<int>::const_iterator iter = childLoopClosureIds.begin(); iter!=childLoopClosureIds.end(); ++iter)
{
if( *iter &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin()) ))
{
childIds.push_back(*iter);
}
}
_dbDriver->getLoopClosureIds(*jter, tmpLoopClosureIds, tmpChildLoopClosureIds);
}
UTimer timer;
_dbDriver->getNeighborIds(*jter, neighborIds, onlyWithActions);
dbAccessTime += timer.getElapsedTime();
if(neighborIds.size() == 0)
}
// Neighbor links
for(std::set<int>::const_iterator iter=neighborIds->begin(); iter!=neighborIds->end(); ++iter)
{
if( (ignoreLoopIds || (loopClosureIds->find(*iter) == loopClosureIds->end() && childLoopClosureIds->find(*iter) == childLoopClosureIds->end())) &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin()) ) &&
!uContains(ids, *iter))
{
UERROR("Signature %d doesn't have neighbor!?", *jter);
nextMargin.push_back(*iter);
}
for(std::list<int>::iterator iter = neighborIds.begin(); iter!=neighborIds.end();)
}
// Parent links
for(std::set<int>::const_iterator iter=loopClosureIds->begin(); iter!=loopClosureIds->end(); ++iter)
{
if( *iter &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin()) ) &&
!uContains(ids, *iter))
{
if( (ignoreLoopIds || (loopClosureIds.find(*iter) == loopClosureIds.end() && childLoopClosureIds.find(*iter) == childLoopClosureIds.end())) &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin())))
if(incrementMarginOnLoop)
{
++iter;
nextMargin.push_back(*iter);
}
else
{
iter = neighborIds.erase(iter);
const Signature * s = this->getSignature(*iter);
if(!s)
{
// update db count because it's on current margin
++nbLoadedFromDb;
}
currentMargin.push_back(*iter);
}
}
}
//Priority on neighbors
nextMargin.insert(nextMargin.end(), neighborIds.rbegin(), neighborIds.rend());
if(incrementMarginOnLoop)
//Child links
for(std::set<int>::const_iterator iter=childLoopClosureIds->begin(); iter!=childLoopClosureIds->end(); ++iter)
{
//LoopIds
nextMargin.insert(nextMargin.end(), loopIds.rbegin(), loopIds.rend());
//ChildIds
nextMargin.insert(nextMargin.end(), childIds.rbegin(), childIds.rend());
}
else
{
//LoopIds
currentMargin.insert(currentMargin.end(), loopIds.rbegin(), loopIds.rend());
//ChildIds
currentMargin.insert(currentMargin.end(), childIds.rbegin(), childIds.rend());
if( *iter &&
(!ignoreSTM || (!_stMem.size() || *iter < *_stMem.begin()) ) &&
!uContains(ids, *iter))
{
if(incrementMarginOnLoop)
{
nextMargin.push_back(*iter);
}
else
{
const Signature * s = this->getSignature(*iter);
if(!s)
{
// update db count because it's on current margin
++nbLoadedFromDb;
}
currentMargin.push_back(*iter);
}
}
}
}
}
if(someLoadedFromDb)
{
// number of margin...
++nbLoadedFromDb;
}
++m;
}
return ids;
@@ -713,7 +695,7 @@ std::set<int> Memory::getAllSignatureIds() const
std::set<int> ids;
if(_dbDriver)
{
_dbDriver->getAllSignatureIds(ids);
_dbDriver->getAllNodeIds(ids);
for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
{
ids.insert(iter->first);
@@ -736,35 +718,21 @@ void Memory::clear()
if(_dbDriver && (_stMem.size() || _workingMem.size()))
{
unsigned int memSize = _workingMem.size() + _stMem.size();
if(_workingMem.size() && *_workingMem.begin() < 0)
{
--memSize;
}
if(memSize != _signatures.size())
{
// this is only a safe check...not supposed to occur.
ULOGGER_ERROR("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d", _workingMem.size(), _stMem.size(), _signatures.size());
}
if(_workingMem.size() && *_workingMem.begin() < 0)
{
--memSize;
}
ULOGGER_DEBUG("Adding statistics after run...");
_dbDriver->addStatisticsAfterRun(memSize, _lastSignature?_lastSignature->id():0, UProcessInfo::getMemoryUsage(), _dbDriver->getMemoryUsed());
}
ULOGGER_DEBUG("");
int minId = -1;
std::map<int, Signature*>::iterator minIter = _signatures.begin();
while(minIter != _signatures.end())
{
if(minIter->first > 0)
{
minId = minIter->first;
break;
}
++minIter;
}
ULOGGER_DEBUG("");
//Get the tree root (parents)
std::map<int, Signature*> mem = _signatures;
for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
@@ -776,7 +744,7 @@ void Memory::clear()
}
}
if(_workingMem.size() != 0)
if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && *_workingMem.begin() == kIdVirtual))
{
ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
}
@@ -804,6 +772,7 @@ void Memory::clear()
/**
* Compute the likelihood of the signature with some others in the memory.
* Important: Assuming that all other ids are under 'signature' id.
* If an error occurs, the result is empty.
*/
std::map<int, float> Memory::computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore)
@@ -827,14 +796,24 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
float sumSimilarity = 0.0f;
float maxSim = 0.0f;
int maxId = 0;
std::map<int, std::map<int, float> >::iterator iterB = _similaritiesMap.find(signature->id());
if(iterB == _similaritiesMap.end())
{
iterB = _similaritiesMap.insert(_similaritiesMap.end(), std::make_pair(signature->id(), std::map<int, float>()));
}
for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
const Signature * sB = this->getSignature(*iter);
if(!sB)
float sim = 0;
if(*iter > 0)
{
UFATAL("Signature %d not wfound in WM ?!?", *iter);
const Signature * sB = this->getSignature(*iter);
if(!sB)
{
UFATAL("Signature %d not wfound in WM ?!?", *iter);
}
sim = signature->compareTo(sB);
iterB->second.insert(iterB->second.end(), std::make_pair(sB->id(), sim));
}
float sim = signature->compareTo(sB);
likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
sumSimilarity += sim;
UDEBUG("sim %d with %d = %f", signature->id(), *iter, sim);
@@ -960,12 +939,15 @@ std::map<int, int> Memory::getWeights() const
std::map<int, int> weights;
for(std::set<int>::const_iterator iter=_workingMem.begin(); iter!=_workingMem.end(); ++iter)
{
const Signature * s = this->getSignature(*iter);
if(!s)
if(*iter > 0)
{
UFATAL("Location %d must exist in memory", *iter);
const Signature * s = this->getSignature(*iter);
if(!s)
{
UFATAL("Location %d must exist in memory", *iter);
}
weights.insert(weights.end(), std::make_pair(*iter, s->getWeight()));
}
weights.insert(weights.end(), std::make_pair(*iter, s->getWeight()));
}
return weights;
}
@@ -1007,7 +989,7 @@ int Memory::cleanup(const std::list<int> & ignoredIds)
int signaturesRemoved = 0;
// bad signature
if(_lastSignature->isBadSignature())
if(_lastSignature->isBadSignature() || !_incrementalMemory)
{
moveToTrash(_lastSignature);
++signaturesRemoved;
@@ -1028,7 +1010,9 @@ void Memory::joinTrashThread()
{
if(_dbDriver)
{
UDEBUG("");
_dbDriver->join();
UDEBUG("");
}
}
@@ -1117,6 +1101,13 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastLoopClosureId);
}
// Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal).
Signature * lastInSTM = 0;
if(_stMem.size())
{
lastInSTM = _signatures.at(*_stMem.begin());
}
for(std::set<int>::const_iterator memIter = wm.begin(); memIter != wm.end(); ++memIter)
{
if( (recentWmImmunized && *memIter > _lastLoopClosureId) ||
@@ -1124,7 +1115,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
{
// ignore recent memory
}
else if(*memIter > 0 && ignoredIds.find(*memIter) == ignoredIds.end())
else if(*memIter > 0 && ignoredIds.find(*memIter) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasNeighbor(*memIter)))
{
Signature * s = this->_getSignature(*memIter);
if(s)
@@ -1140,6 +1131,18 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
break;
}
}
// Its neighbors must not be in STM to be removable, rehearsal issue
if(!foundInSTM)
{
for(std::set<int>::iterator iter = _stMem.begin(); iter!=_stMem.end(); ++iter)
{
if(s->hasNeighbor(*iter))
{
foundInSTM = true;
break;
}
}
}
if(!foundInSTM)
{
// looped signature priority to be transferred
@@ -1167,16 +1170,6 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
++iter)
{
bool removable = true;
const std::set<int> & childIds = iter->second->getChildLoopClosureIds();
for(std::set<int>::const_iterator jter = childIds.begin(); jter != childIds.end(); ++jter)
{
// if the child is not in WM or if it is added to the removable list
removable = _workingMem.find(*jter) == _workingMem.end() || addedSignatures.find(*jter) != addedSignatures.end();
if(!removable)
{
break;
}
}
if(removable)
{
if(!recentWmImmunized)
@@ -1336,16 +1329,13 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
{
Signature * s = _getSignature(*iter);
if(s)
// don't modify old links of oldS (cameraDatabase needs this info
// to reload properly the actions).
if(s && s->id() > oldS->id())
{
// don't modify old links (cameraDatabase needs this info
// to reload properly the actions).
if(s->id()!=oldS->id())
{
s->changeNeighborIds(oldS->id(), newS->id());
}
s->changeNeighborIds(oldS->id(), newS->id());
}
else
else if(!s)
{
UERROR("Location %d is not in RAM?!?", *iter);
}
@@ -1358,7 +1348,8 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
{
NeighborLink link = iter->second;
link.updateIds(oldS->id(), newS->id());
if(link.id() == newS->id() && link.baseIds().size())
// Forward links have baseIds set, Backward links have baseIds null
if(link.toId() == newS->id() && link.baseIds().size())
{
// Limit the number of self references to STM size (baseIds size)
bool allSameIds = true;
@@ -1374,16 +1365,33 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
if(!allSameIds || (allSameIds && !allSameIdsAdded))
{
newS->addNeighbor(link);
allSameIdsAdded = true;
if(allSameIds && !allSameIdsAdded)
{
allSameIdsAdded = true;
}
}
else if (link.actions().size() && link.actions().front().size())
else if (link.actuators().size() && link.actuators().size())
{
// Show warning when actions are used.
UWARN("Ignored self reference link because base ids are all the same. (id=%d)", newS->id());
}
else
{
UDEBUG("Ignored self reference link because base ids are all the same. (id=%d)", newS->id());
}
}
else
else if(link.toId() != newS->id())
{
Signature * s = this->_getSignature(link.toId());
if(s)
{
// modify neighbor "from"
s->changeNeighborIds(oldS->id(), newS->id());
}
else
{
UERROR("Didn't find neighbor %d of %d in RAM...", link.toId(), oldS->id());
}
newS->addNeighbor(link);
}
}
@@ -1406,7 +1414,7 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
}
else
{
UERROR("A location (%d) in WM/STM cannot be transferred if its loop closure id is in STM", *iter);
UERROR("A location (%d, child of %d) in WM/STM cannot be transferred if its loop closure id is in STM", *iter, oldS->id());
}
}
@@ -1423,7 +1431,26 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
{
this->copyData(oldS, newS);
// Set old image to new signature
newS->setImage(oldS->getImage());
if(this->isRawDataKept())
{
if(oldS->getRawData().size() == 0)
{
// try load from database
if(oldS->isSaved() && _dbDriver)
{
std::list<Sensor> rawData;
if(_dbDriver->getRawData(oldS->id(), rawData))
{
newS->setRawData(rawData);
}
UDEBUG("Loaded raw data from database");
}
}
else
{
newS->setRawData(oldS->getRawData());
}
}
}
// remove old location
@@ -1498,12 +1525,6 @@ void Memory::rehearsal(Signature * signature, std::map<std::string, float> & sta
// Get parents to compare...
std::set<int> mem = _stMem;
mem.erase(signature->id());
if(mem.size())
{
// A loop closure cannot happen on the last location of STM
// (for neighbor links redirection issue).
mem.erase(mem.begin());
}
if(_similarityOnlyLast && mem.size())
{
@@ -1537,6 +1558,7 @@ void Memory::rehearsal(Signature * signature, std::map<std::string, float> & sta
ULOGGER_DEBUG("Comparing with last signatures...");
float value = 0;
int id = 0;
int nbMerged = 0;
float maxValue = 0;
int maxId = 0;
for(std::map<int, float>::iterator iter=similarities.begin(); iter!=similarities.end(); ++iter)
@@ -1548,53 +1570,50 @@ void Memory::rehearsal(Signature * signature, std::map<std::string, float> & sta
maxId = id;
maxValue = value;
}
}
if(maxValue > _similarityThreshold)
{
if(_incrementalMemory)
if(value > _similarityThreshold)
{
this->addLoopClosureLink(maxId, signature->id());
}
else
{
Signature * s = _signatures.at(maxId);
if(s)
if(_incrementalMemory)
{
signature->setWeight(signature->getWeight() + 1 + s->getWeight());
this->addLoopClosureLink(id, signature->id());
}
else
{
UFATAL("not supposed to happen");
Signature * s = _signatures.at(id);
if(s)
{
signature->setWeight(signature->getWeight() + 1 + s->getWeight());
}
else
{
UFATAL("not supposed to happen");
}
}
++nbMerged;
}
stats.insert(std::pair<std::string, float>(std::string("Memory/Rehearsal Closure/"), 1.0f));
}
else
{
stats.insert(std::pair<std::string, float>(std::string("Memory/Rehearsal Closure/"), 0.0f));
}
stats.insert(std::pair<std::string, float>(std::string("Memory/Rehearsal Max Id/"), maxId));
stats.insert(std::pair<std::string, float>(std::string("Memory/Rehearsal Max Value/"), maxValue));
stats.insert(std::pair<std::string, float>(std::string("Memory/Rehearsal nbMerged/"), nbMerged));
UDEBUG("maxId=%d, maxSim=%f, t=%fs", maxId, maxValue, timer.ticks());
}
// The data returned must be released
IplImage * Memory::getImage(int id) const
std::list<Sensor> Memory::getRawData(int id) const
{
IplImage * img = 0;
std::list<Sensor> data;
const Signature * s = this->getSignature(id);
if(s && s->getImage())
if(s && s->getRawData().size())
{
img = cvCloneImage(s->getImage());
data = s->getRawData();
}
else if(_dbDriver)
{
_dbDriver->getImage(id, &img);
_dbDriver->getRawData(id, data);
}
return img;
return data;
}
void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
@@ -1622,7 +1641,8 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
if(ids.size() == 0)
{
_dbDriver->getAllSignatureIds(ids);
_dbDriver->getAllNodeIds(ids);
UDEBUG("ids.size()=%d", ids.size());
for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
{
ids.insert(iter->first);
@@ -1646,7 +1666,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
_dbDriver->getWeight(id, weight);
for(NeighborsMultiMap::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(_signatures.find(iter->first) == _signatures.end())
if(id!=iter->first && _signatures.find(iter->first) == _signatures.end())
{
int weightNeighbor = 0;
_dbDriver->getWeight(iter->first, weightNeighbor);
@@ -1656,8 +1676,8 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
weight,
iter->first,
weightNeighbor,
(int)iter->second.actions().size(),
iter->second.actions().size()>0?colorA:colorB);
(int)iter->second.actuators().size(),
iter->second.actuators().size()>0?colorA:colorB);
}
}
std::set<int> loopIds;
@@ -1695,6 +1715,7 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
{
//if(linksAdded.find(std::pair<int, int>(id, iter->first)) == linksAdded.end() &&
// linksAdded.find(std::pair<int, int>(iter->first, id)) == linksAdded.end())
if(id != iter->first)
{
int weightNeighbor = 0;
const Signature * s = this->getSignature(iter->first);
@@ -1712,8 +1733,8 @@ void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
weight,
iter->first,
weightNeighbor,
(int)iter->second.actions().size(),
iter->second.actions().size()>0?colorA:colorB);
(int)iter->second.actuators().size(),
iter->second.actuators().size()>0?colorA:colorB);
}
}
const std::set<int> & loopIds = i->second->getLoopClosureIds();
@@ -1784,7 +1805,7 @@ void Memory::cleanLTM(int maxDepth)
}
//Look in the database
UDEBUG("highestWeightedSignatures.size()=%d", highestWeightedSignatures.size());
_dbDriver->getHighestWeightedSignatures(wmSize, highestWeightedSignatures);
_dbDriver->getHighestWeightedNodeIds(wmSize, highestWeightedSignatures);
UDEBUG("highestWeightedSignatures.size()=%d", highestWeightedSignatures.size());
std::set<int> weightedSignatures;
@@ -1820,8 +1841,8 @@ void Memory::cleanGraph(const Node * root)
std::list<std::list<int> > paths;
root->expand(paths);
int i=0;
/*for(std::list<std::list<int> >::iterator iter=paths.begin(); iter!=paths.end();++iter)
/*int i=0;
for(std::list<std::list<int> >::iterator iter=paths.begin(); iter!=paths.end();++iter)
{
std::stringstream str;
std::list<int> & path = *iter;
@@ -1877,8 +1898,8 @@ void Memory::cleanGraph(const Node * root)
}
}
i=0;
/*for(std::list<std::list<int> >::iterator iter=paths.begin(); iter!=paths.end();++iter)
/*i=0;
for(std::list<std::list<int> >::iterator iter=paths.begin(); iter!=paths.end();++iter)
{
std::stringstream str;
std::list<int> & path = *iter;
@@ -1987,9 +2008,9 @@ void Memory::cleanGraph(const Node * root)
else if(_dbDriver)
{
// remove reference from active neighbors
std::list<int> neighbors;
std::set<int> neighbors;
_dbDriver->getNeighborIds(*iter, neighbors);
for(std::list<int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
for(std::set<int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
{
s = this->_getSignature(*jter);
if(s)
@@ -2005,7 +2026,8 @@ void Memory::cleanGraph(const Node * root)
UDEBUG("Removing %s", strToRemove.c_str());
if(_dbDriver && !strToRemove.empty())
{
_dbDriver->executeNoResult(std::string("UPDATE Signature SET loopClosureId=-1 WHERE ") + strToRemove);
UWARN("Not implemented...");
//_dbDriver->executeNoResult(std::string("UPDATE Node SET loopClosureId=-1 WHERE ") + strToRemove);
}
UDEBUG("time=%fs", timer.ticks());
corelib/src/Memory.h
-164
View File
@@ -1,164 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef MEMORY_H_
#define MEMORY_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "utilite/UEventsHandler.h"
#include "rtabmap/core/Parameters.h"
#include "utilite/UVariant.h"
#include <typeinfo>
#include <list>
#include <map>
#include <set>
#include "utilite/UStl.h"
#include <opencv2/core/core.hpp>
namespace rtabmap {
class Signature;
class NeighborLink;
class DBDriver;
class Node;
class SMState;
class RTABMAP_EXP Memory
{
public:
static const int kIdStart;
static const int kIdVirtual;
static const int kIdInvalid;
public:
Memory(const ParametersMap & parameters = ParametersMap());
virtual ~Memory();
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::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();
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::set<int> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
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 isCommonSignatureUsed() const {return _commonSignatureUsed;}
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
const Signature * getSignature(int id) const;
bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
//setters
void setSimilarityThreshold(float similarityThreshold);
void setSimilarityOnlyLast(int similarityOnlyLast) {_similarityOnlyLast = similarityOnlyLast;}
void setOldSignatureRatio(float oldSignatureRatio);
void setMaxStMemSize(unsigned int maxStMemSize);
void setRecentWmRatio(float recentWmRatio);
void setCommonSignatureUsed(bool commonSignatureUsed);
void setRawDataKept(bool rawDataKept) {_rawDataKept = rawDataKept;}
void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const;
virtual void dumpSignatures(const char * fileNameSign) const {}
void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
void cleanLocalGraph(int id, unsigned int margin);
void cleanLTM(int maxDepth = 10);
void createGraph(Node * parent, unsigned int maxDepth, const std::set<int> & endIds = std::set<int>());
protected:
virtual void preUpdate();
virtual void postUpdate() {}
virtual void addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >());
virtual void clear();
virtual void moveToTrash(Signature * s);
virtual Signature * getSignatureLtMem(int id);
void addSignatureToWm(Signature * signature);
Signature * _getSignature(int id) const;
std::list<Signature *> getRemovableSignatures(int count, const std::set<int> & ignoredIds = std::set<int>());
int getNextId();
void initCountId();
void rehearsal(Signature * signature, std::map<std::string, float> & stats);
const std::map<int, Signature*> & getSignatures() const {return _signatures;}
private:
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;
bool _incrementalMemory;
unsigned int _maxStMemSize;
bool _commonSignatureUsed;
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; // id
std::set<int> _workingMem; // id,age
std::vector<int> _lastBaseIds;
std::map<int, std::map<int, float> > _similaritiesMap;
};
} // namespace rtabmap
#endif /* MEMORY_H_ */
corelib/src/Micro.cpp
+383
View File
@@ -0,0 +1,383 @@
/*
* Micro.cpp
*
* Created on: Mar 5, 2012
* Author: MatLab
*/
#include "rtabmap/core/Micro.h"
#include "utilite/UAudioRecorderMic.h"
#include "utilite/UAudioRecorderFile.h"
#include <utilite/UEventsManager.h>
#include <utilite/UFile.h>
#include <utilite/UMath.h>
#include <fftw3.h>
namespace rtabmap {
Micro::Micro(MicroEvent::Type eventType,
int deviceId,
int fs,
int frameLength,
int channels,
int bytesPerSample,
int id) :
_eventType(eventType),
_recorder(0),
_simulateFreq(false),
_out(0),
_id(id)
{
UASSERT(eventType == MicroEvent::kTypeFrame || eventType == MicroEvent::kTypeFrameFreq || eventType == MicroEvent::kTypeFrameFreqSqrdMagn);
UASSERT(deviceId >= 0);
UASSERT(frameLength > 0 && frameLength % 2 == 0);
_recorder = new UAudioRecorderMic(deviceId, fs, frameLength, bytesPerSample, channels);
}
Micro::Micro(MicroEvent::Type eventType,
const std::string & path,
bool simulateFrameRate,
int frameLength,
int id,
bool playWhileRecording) :
_eventType(eventType),
_recorder(0),
_simulateFreq(simulateFrameRate),
_out(0),
_id(id)
{
UASSERT(eventType == MicroEvent::kTypeFrame || eventType == MicroEvent::kTypeFrameFreq || eventType == MicroEvent::kTypeFrameFreqSqrdMagn);
UASSERT(frameLength > 0 && frameLength % 2 == 0);
if(playWhileRecording)
{
simulateFrameRate = false;
}
_recorder = new UAudioRecorderFile(path, playWhileRecording, frameLength);
}
Micro::~Micro()
{
UDEBUG("");
join(true);
if(_recorder)
{
delete _recorder;
}
if(_out)
{
fftwf_destroy_plan((fftwf_plan)_p);
fftwf_free(_out);
_out = 0;
}
}
bool Micro::init()
{
if(!_recorder->init())
{
UERROR("Recorder initialization failed!");
return false;
}
// init FFTW stuff
if(_out)
{
fftwf_destroy_plan((fftwf_plan)_p);
fftwf_free(_out);
_out = 0;
_in.clear();
}
int N = _recorder->frameLength();
_in.resize(N);
_out = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * N);
_p = fftwf_plan_dft_r2c_1d(N, _in.data(), _out, 0);
_window = uHamming(N);
return true;
}
void Micro::stop()
{
if(this->isRunning())
{
this->kill();
}
else if(_recorder && _recorder->isRunning())
{
_recorder->join(true);
}
}
void Micro::startRecorder()
{
if(_recorder)
{
_recorder->start();
_timer.start();
}
}
void Micro::mainLoopBegin()
{
this->startRecorder();
}
void Micro::mainLoop()
{
if(!_recorder)
{
UERROR("Recorder not initialized");
this->kill();
return;
}
if(this->isRunning())
{
bool noMoreFrames = true;
if(_eventType == MicroEvent::kTypeFrame)
{
UDEBUG("");
cv::Mat data = this->getFrame();
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(data, 2, _recorder->fs(), _recorder->channels(), _id));
}
}
else if(_eventType == MicroEvent::kTypeFrameFreq)
{
UDEBUG("");
cv::Mat freq;
cv::Mat data = this->getFrame(freq, false);
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(MicroEvent::kTypeFrameFreq, freq, _recorder->fs(), _recorder->channels(), _id));
}
}
else if(_eventType == MicroEvent::kTypeFrameFreqSqrdMagn)
{
UDEBUG("");
cv::Mat freq;
cv::Mat data = this->getFrame(freq, true);
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(MicroEvent::kTypeFrameFreqSqrdMagn, freq, _recorder->fs(), _recorder->channels(), _id));
}
}
else
{
UFATAL("Not supposed to be here...");
}
if(noMoreFrames)
{
if(this->isRunning())
{
UEventsManager::post(new MicroEvent(_id));
}
this->kill();
}
}
}
void Micro::mainLoopKill()
{
if(_recorder)
{
_recorder->join(true);
}
}
cv::Mat Micro::getFrame()
{
cv::Mat data;
std::vector<char> frame;
if(!_recorder)
{
UERROR("Micro is not initialized...");
return data;
}
int frameLength = _recorder->frameLength();
int fs = _recorder->fs();
int channels = _recorder->channels();
int bytesPerSample = _recorder->bytesPerSample();
if(_simulateFreq && fs)
{
int sleepTime = ((double(frameLength)/double(fs) - _timer.getElapsedTime()) * 1000.0) + 0.5;
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < double(frameLength)/double(fs)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, double(frameLength)/double(fs));
}
if(_recorder->getNextFrame(frame, true) && int(frame.size()) == frameLength * channels * bytesPerSample)
{
UASSERT(bytesPerSample == 1 || bytesPerSample == 2 || bytesPerSample == 4);
if(bytesPerSample == 1)
{
data = cv::Mat(channels, frameLength, CV_8S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<char>(j, i/(channels*bytesPerSample)) = *((char*)&frame[i + j*bytesPerSample]);
}
}
}
else if(bytesPerSample == 2)
{
data = cv::Mat(channels, frameLength, CV_16S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<short>(j, i/(channels*bytesPerSample)) = *((short*)&frame[i + j*bytesPerSample]);
}
}
}
else if(bytesPerSample == 4)
{
data = cv::Mat(channels, frameLength, CV_32S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<int>(j, i/(channels*bytesPerSample)) = *((int*)&frame[i + j*bytesPerSample]);
}
}
}
}
else
{
UDEBUG("No more frames...");
}
return data;
}
cv::Mat Micro::getFrame(cv::Mat & frameFreq, bool sqrdMagn)
{
cv::Mat frame = this->getFrame();
if(!frame.empty())
{
UASSERT(frame.depth() == CV_8S || frame.depth() == CV_16S || frame.depth() == CV_32S);
cv::Mat timeSample(frame.rows, frame.cols, CV_32F);
for(int i=0; i<frame.cols; ++i)
{
// for each channels
for(int j=0; j<frame.rows; ++j)
{
if(frame.depth() == CV_8S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<char>(j, i)) / float(1<<7); // between 0 and 1
}
else if(frame.depth() == CV_16S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<short>(j, i)) / float(1<<15); // between 0 and 1
}
else if(frame.depth() == CV_32S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<int>(j, i)) / float(1<<31); // between 0 and 1
}
}
}
int size = timeSample.cols/2+1;
if(sqrdMagn)
{
frameFreq = cv::Mat(timeSample.rows, size, CV_32F);
}
else
{
frameFreq = cv::Mat(timeSample.rows, size * 2, CV_32F); // [re, im, re, im, ...]
}
// for each channels
for(int j=0; j<timeSample.rows; ++j)
{
cv::Mat row = timeSample.row(j);
cv::Mat rowFreq = frameFreq.row(j);
memcpy(_in.data(), row.data, row.cols*sizeof(float));
fftwf_execute((fftwf_plan)_p); /* repeat as needed */
float re;
float im;
for(int i=0; i<size; ++i)
{
re = float(_out[i][0]);
im = float(_out[i][1]);
if(sqrdMagn)
{
frameFreq.at<float>(0, i) = re*re+im*im; // squared magnitude
}
else
{
frameFreq.at<float>(0, i*2) = re;
frameFreq.at<float>(0, i*2+1) = im;
}
}
}
}
return frame;
}
int Micro::fs()
{
int fs = 0;
if(_recorder)
{
fs = _recorder->fs();
}
return fs;
}
int Micro::bytesPerSample()
{
int bytes = 0;
if(_recorder)
{
bytes = _recorder->bytesPerSample();
}
return bytes;
}
int Micro::channels()
{
int channels = 0;
if(_recorder)
{
channels = _recorder->channels();
}
return channels;
}
int Micro::nfft()
{
int n = 0;
if(_recorder)
{
n = _recorder->frameLength();
}
return n?n/2+1:0;
}
}
corelib/src/NearestNeighbor.cpp
+7 -47
View File
@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "NearestNeighbor.h"
#include "rtabmap/core/NearestNeighbor.h"
#include "utilite/ULogger.h"
#include <opencv2/core/core.hpp>
@@ -27,70 +27,30 @@ namespace rtabmap
/////////////////////////
// KdTreeNN
/////////////////////////
KdTreeNN::KdTreeNN(const ParametersMap & parameters) :
_tree(0)
KdTreeNN::KdTreeNN(const ParametersMap & parameters)
{
ULOGGER_DEBUG("");
this->parseParameters(parameters);
}
KdTreeNN::~KdTreeNN()
{
if(_tree)
{
cvReleaseFeatureTree(_tree);
}
}
void KdTreeNN::setData(const cv::Mat & data)
{
if(_tree)
{
cvReleaseFeatureTree(_tree);
_tree = 0;
}
// convert to old style mat (data is not copied)
_dataMat = data;
_tree = cvCreateKDTree(&_dataMat);
//(data is not copied)
_tree.build(data);
}
void KdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
{
ULOGGER_DEBUG("");
if(_tree)
{
// convert to old style mat (data is not copied)
CvMat queriesMat = queries;
CvMat indicesMat = indices;
CvMat distsMat = dists;
cvFindFeatures(_tree, &queriesMat, &indicesMat, &distsMat, knn, emax);
}
else
{
ULOGGER_ERROR("The search tree is not created, setData() must be called first");
}
_tree.findNearest(queries, knn, emax, indices, cv::noArray(), dists);
}
void KdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
{
ULOGGER_DEBUG("");
CvMat dataMat = data;
CvFeatureTree * tree = cvCreateKDTree(&dataMat);
if(tree)
{
// convert to old style mat (data is not copied)
CvMat queriesMat = queries;
CvMat indicesMat = indices;
CvMat distsMat = dists;
cvFindFeatures(tree, &queriesMat, &indicesMat, &distsMat, knn, emax);
cvReleaseFeatureTree(tree);
}
else
{
ULOGGER_ERROR("The search tree creation failed ?!?");
}
cv::KDTree tree(data);
tree.findNearest(queries, knn, emax, indices, cv::noArray(), dists);
}
void KdTreeNN::parseParameters(const ParametersMap & parameters)
corelib/src/NearestNeighbor.h
-152
View File
@@ -1,152 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef NEARESTNEIGHBOR_H_
#define NEARESTNEIGHBOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <map>
#include "rtabmap/core/Parameters.h"
namespace rtabmap
{
class VisualWord;
class RTABMAP_EXP NearestNeighbor
{
public:
public:
virtual ~NearestNeighbor() {}
virtual void setData(const cv::Mat & data) = 0;
virtual void search(
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) = 0;
virtual void search(
const cv::Mat & data,
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) const = 0;
virtual bool isDist64F() const = 0;
virtual bool isDistSquared() const = 0;
virtual void parseParameters(const ParametersMap & parameters) {}
protected:
NearestNeighbor() {}
};
/////////////////////////
// KdTreeNN
/////////////////////////
class RTABMAP_EXP KdTreeNN : public NearestNeighbor
{
public:
KdTreeNN(const ParametersMap & parameters = ParametersMap());
virtual ~KdTreeNN();
virtual void setData(const cv::Mat & data);
virtual void search(const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64);
virtual void search(const cv::Mat & data,
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) const;
virtual bool isDist64F() const {return true;}
virtual bool isDistSquared() const {return false;}
virtual void parseParameters(const ParametersMap & parameters);
private:
CvFeatureTree * _tree;
CvMat _dataMat;
};
/////////////////////////
// FlannKdTreeNN
/////////////////////////
class RTABMAP_EXP FlannKdTreeNN : public NearestNeighbor
{
public:
enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
public:
FlannKdTreeNN(const ParametersMap & parameters = ParametersMap());
FlannKdTreeNN(Strategy s, const ParametersMap & parameters = ParametersMap());
virtual ~FlannKdTreeNN();
void setStrategy(Strategy s) {if(_strategy!=kUndefined) _strategy = s;}
virtual void setData(const cv::Mat & data);
virtual void search(const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64);
virtual void search(const cv::Mat & data,
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) const;
virtual bool isDist64F() const {return false;}
virtual bool isDistSquared() const {return true;}
virtual void parseParameters(const ParametersMap & parameters);
private:
cv::flann::Index * createIndex(const cv::Mat & data, Strategy s) const;
private:
cv::flann::Index * _treeFlannIndex;
Strategy _strategy;
};
}
#endif /* NEARESTNEIGHBOR_H_ */
corelib/src/Parameters.cpp
+3 -3
View File
@@ -45,9 +45,9 @@ std::string Parameters::getDefaultWorkingDirectory()
std::string path = UDirectory::homeDir();
if(!path.empty())
{
UDirectory::makeDir(path += "/Documents");
UDirectory::makeDir(path += "/RTAB-Map");
path += "/"; // add trailing separator
UDirectory::makeDir(path += UDirectory::separator() + "Documents");
UDirectory::makeDir(path += UDirectory::separator() + "RTAB-Map");
path += UDirectory::separator(); // add trailing separator
}
else
{
corelib/src/Rtabmap.cpp
+199 -167
View File
@@ -19,22 +19,24 @@
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/Version.h"
#include "rtabmap/core/SMState.h"
#include "rtabmap/core/Sensor.h"
#include "rtabmap/core/Actuator.h"
#include "rtabmap/core/SensorimotorEvent.h"
#include "rtabmap/core/KeypointDetector.h"
#include "rtabmap/core/Camera.h"
#include "VWDictionary.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Micro.h"
#include "VerifyHypotheses.h"
#include "rtabmap/core/VerifyHypotheses.h"
#include "KeypointMemory.h"
#include "SMMemory.h"
#include "BayesFilter.h"
#include "rtabmap/core/KeypointMemory.h"
#include "rtabmap/core/SMMemory.h"
#include "rtabmap/core/BayesFilter.h"
#include "utilite/UtiLite.h"
#include <utilite/UtiLite.h>
#include "SimpleIni.h"
@@ -55,14 +57,14 @@ const char * Rtabmap::kDefaultDatabaseName = "LTM.db";
Rtabmap::Rtabmap() :
_publishStats(Parameters::defaultRtabmapPublishStats()),
_publishImages(Parameters::defaultRtabmapPublishImages()),
_publishRawData(Parameters::defaultRtabmapPublishRawData()),
_publishPdf(Parameters::defaultRtabmapPublishPdf()),
_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
_publishKeypoints(Parameters::defaultKpPublishKeypoints()),
_publishMasks(Parameters::defaultSMPublishMasks()),
_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_smStateBufferMaxSize(Parameters::defaultRtabmapSMStateBufferSize()),
_sensorsBufferMaxSize(Parameters::defaultRtabmapSMStateBufferSize()),
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_retrievalThr(Parameters::defaultRtabmapRetrievalThr()),
@@ -72,6 +74,7 @@ Rtabmap::Rtabmap() :
_actionsSentRejectHyp(Parameters::defaultRtabmapActionsSentRejectHyp()),
_confidenceThr(Parameters::defaultRtabmapConfidenceThr()),
_likelihoodStdDevRemoved(Parameters::defaultRtabmapLikelihoodStdDevRemoved()),
_likelihoodNullValuesIgnored(Parameters::defaultRtabmapLikelihoodNullValuesIgnored()),
_lcHypothesisId(0),
_reactivateId(0),
_lastLcHypothesisValue(0),
@@ -85,8 +88,6 @@ Rtabmap::Rtabmap() :
{
ULOGGER_DEBUG("Working directory=%s", Parameters::defaultRtabmapWorkingDirectory().c_str());
this->setWorkingDirectory(Parameters::defaultRtabmapWorkingDirectory());
UEventsManager::addHandler(this);
}
Rtabmap::~Rtabmap() {
@@ -172,15 +173,6 @@ void Rtabmap::releaseAllStrategies()
}
}
void Rtabmap::startInit()
{
if(!_memory || !_vhStrategy || !_bayesFilter)
{
ULOGGER_DEBUG("Rtabmap thread started without all strategies defined...");
//this->killSafely();
}
}
void Rtabmap::pushNewState(State newState, const ParametersMap & parameters)
{
ULOGGER_DEBUG("to %d", newState);
@@ -192,7 +184,7 @@ void Rtabmap::pushNewState(State newState, const ParametersMap & parameters)
}
_stateMutex.unlock();
_newSMStateSem.release();
_sensorimotorAdded.release();
}
void Rtabmap::init(const ParametersMap & parameters)
@@ -242,9 +234,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
_publishStats = uStr2Bool(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapPublishImages())) != parameters.end())
if((iter=parameters.find(Parameters::kRtabmapPublishRawData())) != parameters.end())
{
_publishImages = uStr2Bool(iter->second.c_str());
_publishRawData = uStr2Bool(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapPublishPdf())) != parameters.end())
{
@@ -284,7 +276,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
}
if((iter=parameters.find(Parameters::kRtabmapSMStateBufferSize())) != parameters.end())
{
_smStateBufferMaxSize = std::atoi(iter->second.c_str());
_sensorsBufferMaxSize = std::atoi(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
@@ -314,6 +306,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
_likelihoodStdDevRemoved = uStr2Bool(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapLikelihoodNullValuesIgnored())) != parameters.end())
{
_likelihoodNullValuesIgnored = uStr2Bool(iter->second.c_str());
}
int signatureType = -1;
if((iter=parameters.find(Parameters::kMemSignatureType())) != parameters.end())
{
@@ -328,6 +324,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
UEventsManager::post(new RtabmapEventInit("Creating memory..."));
if(_memory)
{
delete _memory;
_memory = 0;
}
if(signatureType == 1)
{
_memory = new SMMemory(parameters);
@@ -459,20 +460,31 @@ int Rtabmap::getTotalMemSize() const
return memSize;
}
void Rtabmap::killCleanup()
void Rtabmap::clearBufferedSensors()
{
_smStateBufferMutex.lock();
_sensorimotorMutex.lock();
{
for(std::list<SMState *>::iterator i=_smStateBuffer.begin(); i!=_smStateBuffer.end(); ++i)
{
delete(*i);
}
_smStateBuffer.clear();
_sensorimotorBuffer.clear();
}
_smStateBufferMutex.unlock();
_sensorimotorMutex.unlock();
}
//this->addImage(0); // this will post the newImage semaphore
_newSMStateSem.release();
void Rtabmap::mainLoopBegin()
{
if(!_memory || !_vhStrategy || !_bayesFilter)
{
ULOGGER_DEBUG("Rtabmap thread started without all strategies defined...");
//this->killSafely();
}
}
void Rtabmap::mainLoopKill()
{
this->clearBufferedSensors();
// this will post the newData semaphore
_sensorimotorAdded.release();
}
void Rtabmap::mainLoop()
@@ -525,6 +537,9 @@ void Rtabmap::mainLoop()
case kStateDeletingMemory:
this->resetMemory(true);
break;
case kStateCleanSensorsBuffer:
this->clearBufferedSensors();
break;
default:
UFATAL("Invalid state !?!?");
break;
@@ -535,12 +550,9 @@ void Rtabmap::resetMemory(bool dbOverwritten)
{
if(_memory)
{
if(_memory)
{
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
_memory->init(DB_TYPE, _wDir + kDefaultDatabaseName, dbOverwritten);
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
}
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
_memory->init(DB_TYPE, _wDir + kDefaultDatabaseName, dbOverwritten);
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
if(_bayesFilter)
{
_bayesFilter->reset();
@@ -552,6 +564,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
// May be memory should be already created here, and use init above...
UFile::erase(_wDir + kDefaultDatabaseName);
}
this->clearBufferedSensors();
_reactivateId = 0;
_lastLcHypothesisValue = 0;
this->setupLogFiles(dbOverwritten);
@@ -559,11 +572,52 @@ void Rtabmap::resetMemory(bool dbOverwritten)
void Rtabmap::handleEvent(UEvent* event)
{
if(this->isRunning() && event->getClassName().compare("SMStateEvent") == 0)
if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
{
SMStateEvent * e = (SMStateEvent*)event;
SMState * data = e->getSMStateOwnership();
this->addSMState(data);
std::list<Sensor> sensors;
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeFeatures)
{
sensors.push_back(Sensor(e->descriptors(), e->keypoints(), e->cameraId()));
sensors.push_back(Sensor(e->image(), Sensor::kTypeImage, e->cameraId()));
}
else if(e->getCode() == CameraEvent::kCodeImage)
{
sensors.push_back(Sensor(e->image(), Sensor::kTypeImage, e->cameraId()));
}
if(sensors.size())
{
this->addSensorimotor(sensors, std::list<Actuator>());
}
}
else if(this->isRunning() && event->getClassName().compare("MicroEvent") == 0)
{
std::list<Sensor> sensors;
MicroEvent * e = (MicroEvent*)event;
if(e->getCode() == MicroEvent::kTypeFrameFreqSqrdMagn)
{
sensors.push_back(Sensor(e->frame(), Sensor::kTypeAudioFreqSqrdMagn, e->microId()));
}
else if(e->getCode() == MicroEvent::kTypeFrameFreq)
{
sensors.push_back(Sensor(e->frame(), Sensor::kTypeAudioFreq, e->microId()));
}
else if(e->getCode() == MicroEvent::kTypeFrame)
{
sensors.push_back(Sensor(e->frame(), Sensor::kTypeAudio, e->microId()));
}
if(sensors.size())
{
this->addSensorimotor(sensors, std::list<Actuator>());
}
}
else if(this->isRunning() && event->getClassName().compare("SensorimotorEvent") == 0)
{
SensorimotorEvent * e = (SensorimotorEvent*)event;
if(e->getCode() == SensorimotorEvent::kTypeData)
{
this->addSensorimotor(e->sensors(), e->actuators());
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
@@ -607,6 +661,11 @@ void Rtabmap::handleEvent(UEvent* event)
ULOGGER_DEBUG("CMD_DELETE_MEMORY");
pushNewState(kStateDeletingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdCleanSensorsBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
pushNewState(kStateCleanSensorsBuffer);
}
}
else if(event->getClassName().compare("ParamEvent") == 0)
{
@@ -617,7 +676,7 @@ void Rtabmap::handleEvent(UEvent* event)
void Rtabmap::process()
{
ULOGGER_DEBUG("");
UDEBUG("");
//============================================================
// Initialization
@@ -655,10 +714,11 @@ void Rtabmap::process()
const Signature * signature = 0;
const Signature * sLoop = 0;
SMState * smState = 0;
std::list<Sensor> sensors;
std::list<Actuator> actuators;
_lcHypothesisId = 0;
_actions.clear();
_actuators.clear();
int neighborSelected = _reactivateId;
int actionsChosen = 0; // for stats
@@ -668,43 +728,33 @@ void Rtabmap::process()
// Wait for an image...
//============================================================
ULOGGER_INFO("getting data...");
smState = this->getSMState();
if(!smState)
this->getSensorimotor(sensors, actuators);
if(!sensors.size())
{
ULOGGER_INFO("data is null...");
return;
}
else if(!_state.empty())
{
ULOGGER_INFO("State changed while waiting.. aborting processing...");
delete smState;
return;
}
else if(!_memory || !_vhStrategy || !_bayesFilter)
{
delete smState;
UWARN("RTAB-Map is not initialized, data received is ignored.");
ULOGGER_INFO("sensors list is null...");
return;
}
timer.start();
timerTotal.start();
if(!_memory || !_vhStrategy || !_bayesFilter)
{
UFATAL("RTAB-Map is not initialized, data received is ignored.");
}
//============================================================
// Memory Update : Location creation + Rehearsal
//============================================================
ULOGGER_INFO("Updating memory...");
if(!_memory->update(smState, memUpdateStats))
if(!_memory->update(sensors, actuators, memUpdateStats))
{
delete smState;
return;
}
signature = _memory->getLastSignature();
if(!signature)
{
ULOGGER_ERROR("Not supposed to be here...");
delete smState;
return;
UFATAL("Not supposed to be here...last signature is null?!?");
}
ULOGGER_INFO("Processing signature %d", signature->id());
refId = signature->id();
@@ -845,33 +895,24 @@ void Rtabmap::process()
// only send actions if rejectLoopReason!=3 (decreasing hypotheses)
if(sLoop && (_actionsSentRejectHyp || !rejectedHypothesis) && (_lastLcHypothesisValue > _confidenceThr))
{
UTimer t1;
std::list<NeighborLink> neighbors;
// TODO to verify
double dbAccessTime = 0.0;
std::map<int, int> ids = _memory->getNeighborsId(dbAccessTime, sLoop->id(), _bayesFilter->getPredictionLC().size()-1, 0);
for(std::map<int, int>::reverse_iterator iter = ids.rbegin(); iter!=ids.rend(); ++iter)
{
uAppend(neighbors, _memory->getNeighborLinks(iter->first, true));
}
std::list<NeighborLink> neighbors = _memory->getNeighborLinks(sLoop->id(), false, false, true);
float currentMaxSim = -1;
UINFO("Actions: neighbors.size=%d", neighbors.size());
for(std::list<NeighborLink>::const_reverse_iterator iter=neighbors.rbegin(); iter!=neighbors.rend() && currentMaxSim!=1.0f; ++iter)
{
if(iter->actions().size() && iter->actions().front().size())
float sim = _memory->compareOneToOne(iter->baseIds(), _memory->getLastBaseIds());
UDEBUG("Neighbor baseIds comparison with %d = %f", iter->toId(), sim);
if(sim > currentMaxSim)
{
float sim = _memory->compareOneToOne(iter->baseIds(), _memory->getLastBaseIds());
UDEBUG("Neighbor baseIds comparison with %d = %f", iter->id(), sim);
if(sim > currentMaxSim)
currentMaxSim = sim;
if(iter->actuators().size())
{
currentMaxSim = sim;
if(iter->actions().front().size())
{
_actions = iter->actions();
}
neighborSelected = iter->id();
_actuators = iter->actuators();
}
++actionsChosen;
neighborSelected = iter->toId();
}
++actionsChosen;
}
_reactivateId = neighborSelected;
}
@@ -880,7 +921,9 @@ void Rtabmap::process()
ULOGGER_INFO("timeActionSelection=%fs",timeActionSelection);
}// !isBadSignature
//============================================================
// Before retrieval, make sure the trash has finished
//============================================================
_memory->joinTrashThread();
timeEmptyingTrash = _memory->getDbSavingTime();
timeJoiningTrash = timer.ticks();
@@ -903,9 +946,9 @@ void Rtabmap::process()
double timeGetNeighborsSpaceDb = 0.0;
// Direct neighbors TIME
std::map<int, int> neighbors = _memory->getNeighborsId(timeGetNeighborsTimeDb, _reactivateId, margin, -1, _bayesFilter->isPredictionOnNonNullActionsOnly(), true, true, true);
std::map<int, int> neighbors = _memory->getNeighborsId(timeGetNeighborsTimeDb, _reactivateId, margin, _maxRetrieved, _bayesFilter->isPredictionOnNonNullActionsOnly(), true, true, true);
unsigned int m = 0;
//Priority to locations near in space (margin) then by time (index)
//Priority to locations near in time (direct neighbor) then by space (loop closure)
while(m < margin)
{
std::set<int> idsSorted;
@@ -984,18 +1027,14 @@ void Rtabmap::process()
// Data used for the statistics event and for the log files
int processMemoryUsed = UProcessInfo::getMemoryUsage()/(1024*1024); // MB
int databaseMemoryUsed = _memory->getDatabaseMemoryUsed(); // MB
float responseThr = 0;
int dictionarySize = 0;
int refWordsCount = 0;
int refUniqueWordsCount = 0;
const KeypointSignature * ssRef = 0;
const KeypointSignature * ssLoop = 0;
const SMSignature * smRef = 0;
const SMSignature * smLoop = 0;
int lcHypothesisReactivated = 0;
float rehearsalValue = uValue(memUpdateStats, std::string("Memory/Rehearsal Max Value/"), 0.0f);
KeypointMemory * kpMem = dynamic_cast<KeypointMemory *>(_memory);
SMMemory * smMem = dynamic_cast<SMMemory *>(_memory);
if(sLoop)
{
lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
@@ -1007,7 +1046,6 @@ void Rtabmap::process()
ssLoop = dynamic_cast<const KeypointSignature *>(sLoop);
}
ssRef = dynamic_cast<const KeypointSignature *>(signature);
responseThr = (float)kpMem->getKeypointDetector()->getAdaptiveResponseThr();
dictionarySize = kpMem->getVWD()->getVisualWords().size();
if(ssRef)
{
@@ -1019,14 +1057,6 @@ void Rtabmap::process()
ULOGGER_WARN("The new signature can't be casted to a KeypointSignature while the Memory is this type ?");
}
}
if(smMem)
{
if(sLoop)
{
smLoop = dynamic_cast<const SMSignature *>(sLoop);
}
smRef = dynamic_cast<const SMSignature *>(signature);
}
float vpLikelihood = 0.0f;
if(adjustedLikelihood.size() && adjustedLikelihood.begin()->first == -1)
@@ -1041,7 +1071,7 @@ void Rtabmap::process()
// only prepare statistics if required or when there is a loop closure
Statistics * stat = 0;
if(_lcHypothesisId || _actions.size() || _publishStats)
if(_lcHypothesisId || _actuators.size() || _publishStats)
{
ULOGGER_INFO("sending stats...");
stat = new Statistics();
@@ -1051,9 +1081,9 @@ void Rtabmap::process()
stat->setLoopClosureId(_lcHypothesisId);
ULOGGER_INFO("Loop closure detected! With id=%d", _lcHypothesisId);
}
if(_actions.size())
if(_actuators.size())
{
stat->setActions(_actions);
stat->setActuators(_actuators);
}
if(_publishStats && refId != Memory::kIdInvalid)
{
@@ -1069,7 +1099,7 @@ void Rtabmap::process()
stat->addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
stat->addStatistic(Statistics::kLoopReactivateId(), _reactivateId);
stat->addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
stat->addStatistic(Statistics::kLoopActions(), (int)_actions.size());
stat->addStatistic(Statistics::kLoopActions(), (int)_actuators.size());
stat->addStatistic(Statistics::kLoopActions_of(), neighborSelected);
stat->addStatistic(Statistics::kLoopActions_chosen(), actionsChosen);
@@ -1078,7 +1108,8 @@ void Rtabmap::process()
stat->addStatistic(Statistics::kMemoryDatabase_size(), (float)databaseMemoryUsed);
stat->addStatistic(Statistics::kMemoryProcess_memory_used(), (float)processMemoryUsed);
stat->addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
stat->addStatistic(Statistics::kMemoryImages_buffered(), (float)_smStateBuffer.size());
stat->addStatistic(Statistics::kMemoryImages_buffered(), (float)_sensorimotorBuffer.size());
stat->addStatistic(Statistics::kMemorySimilarities_map(), (float)_memory->getSimilaritiesMap().size());
// timing...
stat->addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -1098,31 +1129,30 @@ void Rtabmap::process()
// Surf specific parameters
stat->addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
stat->addStatistic(Statistics::kKeypointResponse_threshold(), responseThr);
//Epipolar geometry constraint
stat->addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
if(_publishImages)
if(_publishRawData)
{
stat->setRefImage(smState->getImage());
stat->setRefRawData(sensors); // raw data
if(sLoop)
{
lcHypothesisReactivated = 0;
if(sLoop && sLoop->isSaved())
if(sLoop->isSaved())
{
lcHypothesisReactivated = 1;
}
const IplImage * img = sLoop->getImage();
if(!img && _memory->isRawDataKept())
const std::list<Sensor> & data = sLoop->getRawData();
if(data.empty() && _memory->isRawDataKept())
{
IplImage * image = _memory->getImage(sLoop->id());
stat->setLoopClosureImage(&image); // The image will be released by the Statistics destructor
std::list<Sensor> d = _memory->getRawData(sLoop->id());
stat->setLoopClosureRawData(d);
}
else if(img)
else if(!data.empty())
{
stat->setLoopClosureImage(img); // The image will be copied
stat->setLoopClosureRawData(data);
}
}
}
@@ -1155,14 +1185,7 @@ void Rtabmap::process()
if(_publishMasks)
{
// Copy mask
if(smRef)
{
stat->setRefMotionMask(smRef->getMotionMask());
}
if(smLoop)
{
stat->setLoopMotionMask(smLoop->getMotionMask());
}
UWARN("Publish motion masks TODO");
}
}
@@ -1283,7 +1306,7 @@ void Rtabmap::process()
_lcHypothesisId,
hypothesis.first,
signaturesRemoved,
int(responseThr),
0,
refWordsCount,
dictionarySize,
int(_memory->getWorkingMemSize()),
@@ -1296,62 +1319,59 @@ void Rtabmap::process()
_reactivateId,
int(nonNulls.size()));
}
ULOGGER_INFO("Time logging = %f...", timer.ticks());
UINFO("Time logging = %f...", timer.ticks());
//ULogger::flush();
delete smState;
}
// ownership is transferred
void Rtabmap::addSMState(SMState * data)
void Rtabmap::addSensorimotor(const std::list<Sensor> & sensors, const std::list<Actuator> & actuators)
{
ULOGGER_DEBUG("");
if(!data)
UDEBUG("sensors %d, actuators %d", sensors.size(), actuators.size());
if(!sensors.size() && !actuators.size())
{
ULOGGER_ERROR("Data is null?!");
ULOGGER_ERROR("Sensors and actuators empty !?");
return;
}
bool notify = true;
_smStateBufferMutex.lock();
_sensorimotorMutex.lock();
{
while(_smStateBufferMaxSize > 0 && _smStateBuffer.size() >= (unsigned int)_smStateBufferMaxSize)
_sensorimotorBuffer.push_back(std::make_pair(sensors, actuators));
while(_sensorsBufferMaxSize > 0 && _sensorimotorBuffer.size() >= (unsigned int)_sensorsBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
delete _smStateBuffer.front();
_smStateBuffer.pop_front();
_sensorimotorBuffer.pop_front();
notify = false;
}
_smStateBuffer.push_back(data);
}
_smStateBufferMutex.unlock();
_sensorimotorMutex.unlock();
if(notify)
{
_newSMStateSem.release();
_sensorimotorAdded.release();
}
}
SMState * Rtabmap::getSMState()
void Rtabmap::getSensorimotor(std::list<Sensor> & sensors, std::list<Actuator> & actuators)
{
ULOGGER_DEBUG("");
SMState * data = 0;
sensors.clear();
actuators.clear();
ULOGGER_INFO("waiting for data");
_newSMStateSem.acquire();
_sensorimotorAdded.acquire();
ULOGGER_INFO("wake-up");
_smStateBufferMutex.lock();
_sensorimotorMutex.lock();
{
if(!_smStateBuffer.empty())
if(!_sensorimotorBuffer.empty())
{
data = _smStateBuffer.front();
_smStateBuffer.pop_front();
sensors = _sensorimotorBuffer.front().first;
actuators = _sensorimotorBuffer.front().second;
_sensorimotorBuffer.pop_front();
}
}
_smStateBufferMutex.unlock();
return data;
_sensorimotorMutex.unlock();
}
// SETTERS
@@ -1375,11 +1395,11 @@ void Rtabmap::setDataBufferSize(int size)
if(size < 0)
{
ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
_smStateBufferMaxSize = 0;
_sensorsBufferMaxSize = 0;
}
else
{
_smStateBufferMaxSize = size;
_sensorsBufferMaxSize = size;
}
}
@@ -1387,7 +1407,7 @@ void Rtabmap::setWorkingDirectory(std::string path)
{
if(path.size() && (path.at(path.size()-1) != '\\' || path.at(path.size()-1) != '/' ))
{
path += "/";
path += UDirectory::separator();
}
if(!path.empty() && UDirectory::exists(path))
@@ -1398,14 +1418,12 @@ void Rtabmap::setWorkingDirectory(std::string path)
_wDir = path;
if(_memory)
{
//clear all buffered images
join(true);
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
_memory->init(DB_TYPE, _wDir + kDefaultDatabaseName);
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
join(true); // this will clean a second time the image buffer (if some images were added during the memory initialization)
setupLogFiles();
this->start();
//clear all buffered images
this->clearBufferedSensors();
}
else
{
@@ -1419,12 +1437,26 @@ void Rtabmap::setWorkingDirectory(std::string path)
}
}
// ownership is transferred
void Rtabmap::process(SMState * data)
void Rtabmap::process(const std::list<Sensor> & data)
{
if(!this->isRunning())
{
this->addSMState(data);
this->addSensorimotor(data, std::list<Actuator>());
this->process();
}
else
{
UERROR("The core thread is running!");
}
}
void Rtabmap::process(const Sensor & data)
{
if(!this->isRunning())
{
std::list<Sensor> sensors;
sensors.push_back(data);
this->addSensorimotor(sensors, std::list<Actuator>());
this->process();
}
else
@@ -1490,7 +1522,7 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
std::list<float> values;
for(unsigned int i=0; i<allValues.size(); ++i)
{
if(allValues[i])
if(!_likelihoodNullValuesIgnored || allValues[i])
{
values.push_back(allValues[i]);
}
@@ -1517,7 +1549,7 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
for(std::map<int, float>::iterator iter=likelihood.begin(); iter!= likelihood.end(); ++iter)
{
float value = iter->second - min;
if(value > mean+stdDev && mean)
if(value > mean+(!_likelihoodStdDevRemoved?0:stdDev) && mean)
{
if(_likelihoodStdDevRemoved)
{
@@ -1656,7 +1688,7 @@ void Rtabmap::dumpPrediction() const
if(_memory && _bayesFilter)
{
const std::set<int> & wm = _memory->getWorkingMem();
CvMat * prediction = cvCreateMat(wm.size(), wm.size(), CV_32FC1);
cv::Mat prediction(wm.size(), wm.size(), CV_32FC1);
_bayesFilter->generatePrediction(prediction, _memory, std::vector<int>(wm.begin(), wm.end()));
FILE* fout = 0;
@@ -1669,11 +1701,11 @@ void Rtabmap::dumpPrediction() const
if(fout)
{
for(int i=0; i<prediction->rows; ++i)
for(int i=0; i<prediction.rows; ++i)
{
for(int j=0; j<prediction->cols; ++j)
for(int j=0; j<prediction.cols; ++j)
{
fprintf(fout, "%f ", prediction->data.fl[j + i*prediction->cols]);
fprintf(fout, "%f ",((float*)prediction.data)[j + i*prediction.cols]);
}
fprintf(fout, "\n");
}
corelib/src/RtabmapEvent.cpp
+6 -97
View File
@@ -32,31 +32,13 @@ const std::map<std::string, float> & Statistics::defaultData()
Statistics::Statistics() :
_extended(0),
_refImageId(0),
_loopClosureId(0),
_refImage(0),
_loopClosureImage(0)
_loopClosureId(0)
{
_defaultDataInitialized = true;
}
Statistics::Statistics(const Statistics & s) :
_extended(0),
_refImageId(0),
_loopClosureId(0),
_refImage(0),
_loopClosureImage(0)
{
*this = s;
}
Statistics::~Statistics()
{
if(_refImage)
{
cvReleaseImage(&_refImage);
}
if(_loopClosureImage)
{
cvReleaseImage(&_loopClosureImage);
}
}
// name format = "Grp/Name/unit"
@@ -65,87 +47,14 @@ void Statistics::addStatistic(const std::string & name, float value)
_data.insert(std::pair<std::string, float>(name, value));
}
//take the ownership of the image, the image will be
//deleted in the 'Statistics' destructor
void Statistics::setRefImage(IplImage ** refImage)
void Statistics::setRefRawData(const std::list<Sensor> & refRawData)
{
if(_refImage)
cvReleaseImage(&_refImage);
_refImage = *refImage;
_refRawData = refRawData;
}
// Copy the image
void Statistics::setRefImage(const IplImage * refImage)
void Statistics::setLoopClosureRawData(const std::list<Sensor> & loopClosureRawData)
{
if(_refImage)
cvReleaseImage(&_refImage);
if(refImage)
{
_refImage = cvCloneImage(refImage);
}
else
{
_refImage = 0;
}
}
//take the ownership of the image, the image will be
//deleted in the 'Statistics' destructor
void Statistics::setLoopClosureImage(IplImage ** loopClosureImage)
{
if(_loopClosureImage)
cvReleaseImage(&_loopClosureImage);
_loopClosureImage = *loopClosureImage;
}
// Copy the image
void Statistics::setLoopClosureImage(const IplImage * loopClosureImage)
{
if(_loopClosureImage)
cvReleaseImage(&_loopClosureImage);
if(loopClosureImage)
{
_loopClosureImage = cvCloneImage(loopClosureImage);
}
else
{
_loopClosureImage = 0;
}
}
Statistics & Statistics::operator=(const Statistics & s)
{
_data = s.data();
if(_refImage)
{
cvReleaseImage(&_refImage);
_refImage = 0;
}
if(_loopClosureImage)
{
cvReleaseImage(&_loopClosureImage);
_loopClosureImage = 0;
}
_extended = s.extended();
_refImageId = s.refImageId();
_loopClosureId = s.loopClosureId();
if(s.refImage())
{
_refImage = cvCloneImage(s.refImage());
}
if(s.loopClosureImage())
{
_loopClosureImage = cvCloneImage(s.loopClosureImage());
}
_posterior = s.posterior();
_likelihood = s.likelihood();
_weights = s.weights();
_refWords = s.refWords();
_loopWords = s.loopWords();
_refMotionMask = s.refMotionMask();
_loopMotionMask = s.loopMotionMask();
_actions = s.getActions();
return *this;
_loopClosureRawData = loopClosureRawData;
}
}
corelib/src/SMMemory.cpp
+145 -385
View File
@@ -17,12 +17,11 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "SMMemory.h"
#include "rtabmap/core/SMMemory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "utilite/UtiLite.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/SMState.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
@@ -32,7 +31,7 @@
#include <iostream>
#include <sstream>
#include <string>
#include "ColorTable.h"
#include "rtabmap/core/ColorTable.h"
namespace rtabmap {
@@ -40,15 +39,25 @@ namespace rtabmap {
SMMemory::SMMemory(const ParametersMap & parameters) :
Memory(parameters),
_useLogPolar(Parameters::defaultSMLogPolarUsed()),
_useVotingScheme(Parameters::defaultSMVotingSchemeUsed()),
_colorTable(0),
_useMotionMask(Parameters::defaultSMMotionMaskUsed())
_useMotionMask(Parameters::defaultSMMotionMaskUsed()),
_dBThreshold(Parameters::defaultSMAudioDBThreshold()),
_dBIndexing(Parameters::defaultSMAudioDBIndexing()),
_magnitudeInvariant(Parameters::defaultSMMagnitudeInvariant())
{
this->parseParameters(parameters);
if(!_colorTable)
{
int i=1;
this->setColorTable(i<<(Parameters::defaultSMColorTable() + 3));
// index 0 = 8, index 1 = 16...
if(Parameters::defaultSMColorTable() == 8)
{
setColorTable(65536);
}
else
{
int i=1;
setColorTable(i<<(Parameters::defaultSMColorTable() + 3));
}
}
}
@@ -69,14 +78,22 @@ void SMMemory::parseParameters(const ParametersMap & parameters)
{
_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::kSMAudioDBThreshold())) != parameters.end())
{
_dBThreshold = atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMAudioDBIndexing())) != parameters.end())
{
_dBIndexing = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMMagnitudeInvariant())) != parameters.end())
{
_magnitudeInvariant = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMColorTable())) != parameters.end())
{
// index 0 = 8, index 1 = 16...
@@ -94,20 +111,6 @@ void SMMemory::parseParameters(const ParametersMap & parameters)
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)
@@ -133,8 +136,7 @@ void SMMemory::copyData(const Signature * from, Signature * to)
timer.start();
if(sFrom && sTo)
{
sTo->setSensors(sFrom->getSensors());
sTo->setMotionMask(sFrom->getMotionMask());
sTo->setSensors(sFrom->getData());
}
else
{
@@ -143,63 +145,46 @@ void SMMemory::copyData(const Signature * from, Signature * to)
ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
}
Signature * SMMemory::createSignature(int id, const SMState * smState, bool keepRawData)
Signature * SMMemory::createSignature(int id, const std::list<Sensor> & rawSensors, bool keepRawData)
{
if(_useMotionMask)
{
UWARN("Using motion mask TODO");
}
UDEBUG("");
UTimer timer;
timer.start();
UTimer timerDetails;
timerDetails.start();
std::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();
std::list<std::vector<int> > postData;
//const SMSignature * previousSignature = dynamic_cast<const SMSignature *>(this->getLastSignature());
// sensors
if(!smState->getSensors().empty() == 0 && image && image->imageSize)
// Process all sensors
for(std::list<Sensor>::const_iterator iter = rawSensors.begin(); iter!=rawSensors.end(); ++iter)
{
if(iter->type() == Sensor::kTypeImage)
{
if(image->depth != IPL_DEPTH_8U && image->nChannels != 3)
{
UFATAL("Only IplImage depth of IPL_DEPTH_8U and 3 channels (BGR) is supported.");
}
UASSERT(iter->data().type() == CV_8UC3 && iter->data().channels() == 3);
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);
const cv::Mat & image = iter->data();
UDEBUG("depth=%d, width=%d, height=%d, nChannels=%d, imageSize=%d,", image.type(), image.cols, image.rows, image.channels(), image.total());
if(_useLogPolar)
{
// Log-polar transform
int radius = image->height < image->width ? image->height/2: image->width/2;
int radius = image.rows < image.cols ? image.rows/2: image.cols/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 );
IplImage * polar = cvCreateImage( polarSize, 8, 3 );
IplImage iplImg = image;
cvLogPolar(&iplImg, polar, cvPoint2D32f(image.cols/2,image.rows/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
UDEBUG("polar size= %d, %d, time=%fs", polar->width, polar->height, timerDetails.ticks());
// IND transform
unsigned char * data = (unsigned char *)polar->imageData;
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;
std::vector<int> sensors(polar->width*polar->height);
for(int i=0; i<polar->height; ++i)
{
for(int j=0; j<polar->width; ++j)
@@ -209,360 +194,135 @@ Signature * SMMemory::createSignature(int id, const SMState * smState, bool keep
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;
}
}
postData.push_back(sensors);
cvReleaseImage(&polar);
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;
std::vector<int> sensors(image.cols*image.rows);
int sum=0;
for(int i=0; i<indexed->height; ++i)
for(int i=0; i<image.rows; ++i)
{
for(int j=0; j<indexed->width; ++j)
cv::Mat row = image.row(i); // DON'T modify row! (it refers to const data)
for(int j=0; j<row.cols; j+=3)
{
unsigned char & b = 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])
unsigned char b = row.at<unsigned char>(j+0);
unsigned char g = row.at<unsigned char>(j+1);
unsigned char r = row.at<unsigned char>(j+2);
if(b && g && r)
{
motionMask[k] = 1;
++sum;
sensors[k] = (int)_colorTable->getIndex(r, g, b); // index
}
if(!_dictionary.empty())
else
{
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);
}
sensors[k] = 0; // null, will be ignored on likelihood computation
}
++k;
}
}
image = indexed;
postData.push_back(sensors);
UDEBUG("sum=%d, indexing time = %fs", sum, timerDetails.ticks());
}
}
} // end kTypeImage
else if(iter->type() == Sensor::kTypeAudioFreqSqrdMagn)
{
UASSERT(iter->data().type() == CV_32FC1);
const cv::Mat & data = iter->data();
int k = 0;
std::vector<int> sensors(data.cols, 0);
unsigned int index;
float max = uMax((float*)data.data, data.cols, index);
int maxLimit = -1; // FIXME Must be not hard coded
float minDB = -1000;// FIXME Must be not hard coded
UDEBUG("data.rows=%d, data.cols=%d, data.type=%d, max=%f at %d", data.rows, data.cols, data.type(), max, index);
if(_dBThreshold > 0)
{
maxLimit = max / std::pow(10.0f, _dBThreshold/10);
}
for(int i=0; i<data.cols; ++i)
{
float val = data.at<float>(0, i);
if(_dBIndexing && max)
{
if(val>=0.001f)
{
val = 10*std::log(val/max);// transform to dB
}
else
{
val = minDB;
}
}
if(!_dBIndexing && val <= maxLimit)
{
val = 0;
}
else if(_dBIndexing)
{
if(val <= minDB || (_dBThreshold && val <= -_dBThreshold))
{
val = 0;
}
else if(max)
{
if(_magnitudeInvariant)
{
val = -1; // ignore magnitude, just set it not null to say this frequency is here
}
else
{
val -= 1; // make sure high values are not null
}
}
}
sensors[k] = int(val);
if((!_dBIndexing && sensors[k]<0) || (_dBIndexing && sensors[k]>0))
{
UERROR("sensors[%d]=%d %f", k, sensors[k], data.at<float>(0,i));
}
++k;
}
postData.push_back(sensors);
} // end kTypeAudioFreqSqrdMagn
else if(iter->type() == Sensor::kTypeTwist)
{
UASSERT(iter->data().type() == CV_32FC1);
const cv::Mat & data = iter->data();
std::vector<int> sensors(data.cols);
for(int i=0; i<data.cols; ++i)
{
sensors[i] = (int)(data.at<float>(0, i)*100.0f);
}
postData.push_back(sensors);
} //end kTypeTwist
else
{
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);
}
}
}
UWARN("Sensor type (%d) not handled!", iter->type());
}
}
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)
if(keepRawData)
{
return Memory::computeLikelihood(signature, ids, maximumScore);
return new SMSignature(postData, id, rawSensors);
}
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!");
return new SMSignature(postData, id);
}
}
corelib/src/SMMemory.h
-64
View File
@@ -1,64 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef 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
+120 -167
View File
@@ -18,12 +18,12 @@
*/
#include "rtabmap/core/Signature.h"
#include "Memory.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Memory.h"
#include <opencv2/highgui/highgui.hpp>
#include "VerifyHypotheses.h"
#include "rtabmap/core/SMState.h"
#include "rtabmap/core/VerifyHypotheses.h"
#include "utilite/UtiLite.h"
#include <utilite/UtiLite.h>
namespace rtabmap
{
@@ -31,9 +31,9 @@ namespace rtabmap
bool NeighborLink::updateIds(int idFrom, int idTo)
{
bool modified = false;
if(_id == idFrom)
if(_toId == idFrom)
{
_id = idTo;
_toId = idTo;
modified = true;
}
for(unsigned int i=0; i<_baseIds.size(); ++i)
@@ -50,82 +50,23 @@ bool NeighborLink::updateIds(int idFrom, int idTo)
Signature::~Signature()
{
ULOGGER_DEBUG("id=%d", _id);
if(_image)
{
cvReleaseImage(&_image);
}
}
Signature::Signature(int id, const IplImage * image, bool keepImage) :
Signature::Signature(int id) :
_id(id),
_weight(0),
_image(0),
_saved(false),
_modified(true)
{
if(image)
{
if(keepImage)
{
_image = cvCloneImage(image);
}
}
}
// Warning, the image returned must be released
const IplImage * Signature::getImage() const
Signature::Signature(int id, const std::list<Sensor> & rawData) :
_id(id),
_weight(0),
_rawData(rawData),
_saved(false),
_modified(true)
{
return _image;
}
void Signature::setImage(const IplImage * image)
{
if(_image && image)
{
cvReleaseImage(&_image);
_image = cvCloneImage(image);
_modified = true;
}
else
{
UDEBUG("Parameter is null or no image is saved.");
}
}
// Warning, the matrix returned must be released
CvMat * Signature::compressImage(const IplImage * image)
{
if(!image)
{
UERROR("The parameter must not be null.");
return 0;
}
// Compress image
int params[3] = {0};
//JPEG compression
std::string format = "jpeg";
params[0] = CV_IMWRITE_JPEG_QUALITY;
params[1] = 80; // default: 80% quality
//PNG compression
//std::string format = "png";
//params[0] = CV_IMWRITE_PNG_COMPRESSION;
//params[1] = 9; // default: maximum compression
std::string extension = '.' + format;
return cvEncodeImage(extension.c_str(), image, params);
}
// Warning, the image returned must be released
IplImage * Signature::decompressImage(const CvMat * imageCompressed)
{
if(!imageCompressed)
{
UERROR("The parameter must not be null.");
return 0;
}
return cvDecodeImage(imageCompressed, CV_LOAD_IMAGE_ANYCOLOR);
}
void Signature::addNeighbors(const NeighborsMultiMap & neighbors)
@@ -138,21 +79,30 @@ void Signature::addNeighbors(const NeighborsMultiMap & neighbors)
void Signature::addNeighbor(const NeighborLink & neighbor)
{
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)
{
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());
*/
UDEBUG("Add neighbor %d to %d", neighbor.toId(), this->id());
_neighbors.insert(std::pair<int, NeighborLink>(neighbor.id(), neighbor));
if(ULogger::level() == ULogger::kDebug)
{
UTimer timer;
std::string baseIdsDebug;
const std::vector<int> & baseIds = neighbor.baseIds();
for(unsigned int i=0; i<baseIds.size(); ++i)
{
baseIdsDebug.append(uFormat("%d", baseIds[i]));
if(i+1 < baseIds.size())
{
baseIdsDebug.append(", ");
}
}
UDEBUG("Adding neighbor %d to %d with %d actions, %d baseIds = [%s] (time print=%fs)", neighbor.toId(), this->id(), neighbor.actuators().size(), neighbor.baseIds().size(), baseIdsDebug.c_str(), timer.getElapsedTime());
}
_neighbors.insert(std::pair<int, NeighborLink>(neighbor.toId(), neighbor));
if(neighbor.actuators().size())
{
_neighborsWithActuators.insert(neighbor.toId());
}
_neighborsAll.insert(neighbor.toId());
_neighborsModified = true;
}
@@ -170,11 +120,17 @@ void Signature::changeNeighborIds(int idFrom, int idTo)
linksToAdd.push_back(link);
}
_neighbors.erase(idFrom);
_neighborsWithActuators.erase(idFrom);
_neighborsAll.erase(idFrom);
for(std::list<NeighborLink>::iterator iter=linksToAdd.begin(); iter!=linksToAdd.end(); ++iter)
{
_neighbors.insert(std::pair<int, NeighborLink>(iter->id(), *iter));
_neighbors.insert(std::pair<int, NeighborLink>(iter->toId(), *iter));
if(iter->actuators().size())
{
_neighborsWithActuators.insert(iter->toId());
}
_neighborsAll.insert(iter->toId());
}
_modified = true;
_neighborsModified = true;
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
@@ -183,19 +139,24 @@ void Signature::changeNeighborIds(int idFrom, int idTo)
//KeypointSignature
KeypointSignature::KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
int id,
const IplImage * image,
bool keepRawData) :
Signature(id, image, keepRawData),
KeypointSignature::KeypointSignature(int id) :
Signature(id),
_enabled(false)
{
}
KeypointSignature::KeypointSignature(const std::multimap<int, cv::KeyPoint> & words,
int id) :
Signature(id),
_words(words),
_enabled(false)
{
}
KeypointSignature::KeypointSignature(int id) :
Signature(id),
KeypointSignature::KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
int id,
const std::list<Sensor> & rawData) :
Signature(id, rawData),
_words(words),
_enabled(false)
{
}
@@ -215,10 +176,9 @@ float KeypointSignature::compareTo(const Signature * s) const
if(words.size() != 0 && _words.size() != 0)
{
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
int totalWords = _words.size()>words.size()?_words.size():words.size();
HypVerificatorEpipolarGeo::findPairsDirect(words, _words, pairs, pairsId);
findPairs(words, _words, pairs);
similarity = float(pairs.size()) / float(totalWords);
}
@@ -240,12 +200,9 @@ void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
}
}
#define BAD_SIGNATURE_THRESHOLD 0 // elements
bool KeypointSignature::isBadSignature() const
{
if(_words.size() <= BAD_SIGNATURE_THRESHOLD)
return true;
return false;
return !_words.size();
}
void KeypointSignature::removeAllWords()
@@ -264,20 +221,21 @@ void KeypointSignature::removeWord(int 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)
const std::list<std::vector<int> > & data,
int id) :
Signature(id),
_data(data)
{
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());
UDEBUG("data=%d", (int)_data.size());
}
SMSignature::SMSignature(
const std::list<std::vector<int> > & data,
int id,
const std::list<Sensor> & rawData) :
Signature(id, rawData),
_data(data)
{
UDEBUG("data=%d", (int)_data.size());
}
SMSignature::SMSignature(int id) :
@@ -296,63 +254,59 @@ float SMSignature::compareTo(const Signature * s) const
if(sm)
{
const std::vector<int> & sensorsB = sm->getSensors();
const std::vector<unsigned char> & motionMaskB = sm->getMotionMask();
const std::list<std::vector<int> > & dataB = sm->getData();
//const std::vector<unsigned char> & motionMaskB = sm->getMotionMask();
if(_sensors.size() == sensorsB.size() && _sensors.size()) //Compatible
//if(_data.size() == sensorsB.size() && _data.size()) //Compatible
if(_data.size() == dataB.size()) //Compatible
{
bool appearanceOnly = false;
if(appearanceOnly)
std::vector<float> similarities(_data.size());
// compare sensors
std::list<std::vector<int> >::const_iterator iterA = _data.begin();
std::list<std::vector<int> >::const_iterator iterB = dataB.begin();
int j=0;
while(iterA != _data.end() && iterB != dataB.end())
{
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)
if(iterA->size() == iterB->size())
{
iterA = sensorsSetA.find(*i);
iterB = sensorsSetB.find(*i);
while(iterA != sensorsSetA.end() && iterB != sensorsSetB.end() && *iterA == *iterB && *iterA == *i)
int sum = 0;
int notNull = 0;
for(unsigned int i=0; i<iterA->size(); ++i)
{
++iterA;
++iterB;
++realPairsCount;
sum += iterA->at(i) && iterA->at(i) == iterB->at(i) ? 1 : 0;
notNull += iterA->at(i) || iterB->at(i) ? 1 : 0;
}
if(notNull)
{
similarities[j] = float(sum)/float(notNull);
}
else
{
similarities[j] = 1.0f; // example, silence == 100% silence
}
}
similarity = realPairsCount / float(_sensors.size());
else
{
UERROR("Data are not the same size (%d vs %d)", (int)iterA->size(), (int)iterB->size());
}
++iterA;
++iterB;
++j;
}
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
similarity = uMean(similarities);
if(ULogger::level() == ULogger::kDebug)
{
int sum = 0;
// compare sensors
for(unsigned int i=0; i<_sensors.size(); ++i)
std::string str;
for(unsigned int i=0; i<similarities.size(); ++i)
{
sum += _sensors.at(i) == sensorsB.at(i) ? 1 : 0;
str.append(uFormat("%f", similarities[i]));
if(i<similarities.size()-1)
{
str.append(", ");
}
}
similarity = float(sum)/float(_sensors.size());
UDEBUG("similarities (%d vs %d) = [%s]", this->id(), s->id(), str.c_str());
}
if(similarity<0 || similarity>1)
@@ -362,12 +316,12 @@ float SMSignature::compareTo(const Signature * s) const
}
else if(!s->isBadSignature() && !this->isBadSignature())
{
UWARN("Not compatible signatures : nb sensors A=%d B=%d", (int)_sensors.size(), (int)sensorsB.size());
UWARN("Not compatible nodes : nb sensors A=%d B=%d", (int)_data.size(), (int)dataB.size());
}
}
else if(s)
{
UWARN("Only SM signatures are compared. (type tested=%s)", s->signatureType().c_str());
UWARN("Only SM signatures are compared. (type tested=%s)", s->nodeType().c_str());
}
return similarity;
}
@@ -375,9 +329,8 @@ float SMSignature::compareTo(const Signature * s) const
bool SMSignature::isBadSignature() const
{
if(_sensors.size() == 0)
return true;
return false;
//return uSum(_data) == 0;
return !_data.size();
}
} //namespace rtabmap
corelib/src/SimpleIni.h
+133 -21
View File
@@ -5,7 +5,7 @@
<tr><th>File <td>SimpleIni.h
<tr><th>Author <td>Brodie Thiesfield [code at jellycan dot com]
<tr><th>Source <td>http://code.jellycan.com/simpleini/
<tr><th>Version <td>4.12
<tr><th>Version <td>4.15
</table>
Jump to the @link CSimpleIniTempl CSimpleIni @endlink interface documentation.
@@ -77,7 +77,7 @@
#1 On Windows you are better to use CSimpleIniA with SI_CONVERT_WIN32.<br>
#2 Only affects Windows. On Windows this uses MBCS functions and
so may fold case incorrectly leading to uncertain results.
-# Call Load() or LoadFile() to load and parse the INI configuration file
-# Call LoadData() or LoadFile() to load and parse the INI configuration file
-# Access and modify the data of the file using the following functions
<table>
<tr><td>GetAllSections <td>Return all section names
@@ -162,13 +162,16 @@
- Usage of the <mbstring.h> header on Windows can be disabled by defining
SI_NO_MBCS. This is defined automatically on Windows CE platforms.
@section contrib CONTRIBUTIONS
- 2010/05/03: Tobias Gehrig: added GetDoubleValue()
@section licence MIT LICENCE
The licence text below is the boilerplate "MIT Licence" used from:
http://www.opensource.org/licenses/mit-license.php
Copyright (c) 2006-2008, Brodie Thiesfield
Copyright (c) 2006-2012, Brodie Thiesfield
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -418,7 +421,7 @@ public:
return *this;
}
bool ConvertToStore(const SI_CHAR * a_pszString) {
size_t uLen = SizeToStore(a_pszString);
size_t uLen = SI_CONVERTER::SizeToStore(a_pszString);
if (uLen == (size_t)(-1)) {
return false;
}
@@ -456,6 +459,9 @@ public:
/** Deallocate all memory stored by this object */
void Reset();
/** Has any data been loaded */
bool IsEmpty() const { return m_data.empty(); }
/*-----------------------------------------------------------------------*/
/** @{ @name Settings */
@@ -578,7 +584,7 @@ public:
@return SI_Error See error definitions
*/
SI_Error Load(
SI_Error LoadData(
std::istream & a_istream
);
#endif // SI_SUPPORT_IOSTREAMS
@@ -589,8 +595,8 @@ public:
@return SI_Error See error definitions
*/
SI_Error Load(const std::string & a_strData) {
return Load(a_strData.c_str(), a_strData.size());
SI_Error LoadData(const std::string & a_strData) {
return LoadData(a_strData.c_str(), a_strData.size());
}
/** Load INI file data direct from memory
@@ -600,7 +606,7 @@ public:
@return SI_Error See error definitions
*/
SI_Error Load(
SI_Error LoadData(
const char * a_pData,
size_t a_uDataLen
);
@@ -879,6 +885,26 @@ public:
bool * a_pHasMultiple = NULL
) const;
/** Retrieve a numeric value for a specific key. If multiple keys are enabled
(see SetMultiKey) then only the first value associated with that key
will be returned, see GetAllValues for getting all values with multikey.
@param a_pSection Section to search
@param a_pKey Key to search for
@param a_nDefault Value to return if the key is not found
@param a_pHasMultiple Optionally receive notification of if there are
multiple entries for this key.
@return a_nDefault Key was not found in the section
@return other Value of the key
*/
double GetDoubleValue(
const SI_CHAR * a_pSection,
const SI_CHAR * a_pKey,
double a_nDefault = 0,
bool * a_pHasMultiple = NULL
) const;
/** Retrieve a boolean value for a specific key. If multiple keys are enabled
(see SetMultiKey) then only the first value associated with that key
will be returned, see GetAllValues for getting all values with multikey.
@@ -976,6 +1002,34 @@ public:
bool a_bForceReplace = false
);
/** Add or update a double value. This will always insert
when multiple keys are enabled.
@param a_pSection Section to add or update
@param a_pKey Key to add or update.
@param a_nValue Value to set.
@param a_pComment Comment to be associated with the key. See the
notes on SetValue() for comments.
@param a_bForceReplace Should all existing values in a multi-key INI
file be replaced with this entry. This option has
no effect if not using multi-key files. The
difference between Delete/SetDoubleValue and
SetDoubleValue with a_bForceReplace = true, is that
the load order and comment will be preserved this
way.
@return SI_Error See error definitions
@return SI_UPDATED Value was updated
@return SI_INSERTED Value was inserted
*/
SI_Error SetDoubleValue(
const SI_CHAR * a_pSection,
const SI_CHAR * a_pKey,
double a_nValue,
const SI_CHAR * a_pComment = NULL,
bool a_bForceReplace = false
);
/** Add or update a boolean value. This will always insert
when multiple keys are enabled.
@@ -1312,14 +1366,14 @@ CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::LoadFile(
}
// convert the raw data to unicode
SI_Error rc = Load(pData, uRead);
SI_Error rc = LoadData(pData, uRead);
delete[] pData;
return rc;
}
template<class SI_CHAR, class SI_STRLESS, class SI_CONVERTER>
SI_Error
CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::Load(
CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::LoadData(
const char * a_pData,
size_t a_uDataLen
)
@@ -1396,7 +1450,7 @@ CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::Load(
#ifdef SI_SUPPORT_IOSTREAMS
template<class SI_CHAR, class SI_STRLESS, class SI_CONVERTER>
SI_Error
CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::Load(
CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::LoadData(
std::istream & a_istream
)
{
@@ -1407,7 +1461,7 @@ CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::Load(
strData.append(szBuf);
}
while (a_istream.good());
return Load(strData);
return LoadData(strData);
}
#endif // SI_SUPPORT_IOSTREAMS
@@ -1878,7 +1932,7 @@ CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::AddEntry(
if (a_pComment) {
oKey.pComment = a_pComment;
}
typename TKeyVal::value_type oEntry(oKey, NULL);
typename TKeyVal::value_type oEntry(oKey, static_cast<const SI_CHAR *>(NULL));
iKey = keyval.insert(oEntry);
bInserted = true;
}
@@ -1994,6 +2048,68 @@ CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::SetLongValue(
return AddEntry(a_pSection, a_pKey, szOutput, a_pComment, a_bForceReplace, true);
}
template<class SI_CHAR, class SI_STRLESS, class SI_CONVERTER>
double
CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::GetDoubleValue(
const SI_CHAR * a_pSection,
const SI_CHAR * a_pKey,
double a_nDefault,
bool * a_pHasMultiple
) const
{
// return the default if we don't have a value
const SI_CHAR * pszValue = GetValue(a_pSection, a_pKey, NULL, a_pHasMultiple);
if (!pszValue || !*pszValue) return a_nDefault;
// convert to UTF-8/MBCS which for a numeric value will be the same as ASCII
char szValue[64] = { 0 };
SI_CONVERTER c(m_bStoreIsUtf8);
if (!c.ConvertToStore(pszValue, szValue, sizeof(szValue))) {
return a_nDefault;
}
char * pszSuffix = NULL;
double nValue = strtod(szValue, &pszSuffix);
// any invalid strings will return the default value
if (!pszSuffix || *pszSuffix) {
return a_nDefault;
}
return nValue;
}
template<class SI_CHAR, class SI_STRLESS, class SI_CONVERTER>
SI_Error
CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::SetDoubleValue(
const SI_CHAR * a_pSection,
const SI_CHAR * a_pKey,
double a_nValue,
const SI_CHAR * a_pComment,
bool a_bForceReplace
)
{
// use SetValue to create sections
if (!a_pSection || !a_pKey) return SI_FAIL;
// convert to an ASCII string
char szInput[64];
#if __STDC_WANT_SECURE_LIB__ && !_WIN32_WCE
sprintf_s(szInput, "%f", a_nValue);
#else // !__STDC_WANT_SECURE_LIB__
sprintf(szInput, "%f", a_nValue);
#endif // __STDC_WANT_SECURE_LIB__
// convert to output text
SI_CHAR szOutput[64];
SI_CONVERTER c(m_bStoreIsUtf8);
c.ConvertFromStore(szInput, strlen(szInput) + 1,
szOutput, sizeof(szOutput) / sizeof(SI_CHAR));
// actually add it
return AddEntry(a_pSection, a_pKey, szOutput, a_pComment, a_bForceReplace, true);
}
template<class SI_CHAR, class SI_STRLESS, class SI_CONVERTER>
bool
CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::GetBoolValue(
@@ -2349,11 +2465,11 @@ CSimpleIniTempl<SI_CHAR,SI_STRLESS,SI_CONVERTER>::Save(
if (m_bAllowMultiLine && IsMultiLineData(iValue->pItem)) {
// multi-line data needs to be processed specially to ensure
// that we use the correct newline format for the current system
a_oOutput.Write("<<<SI-END-OF-MULTILINE-TEXT" SI_NEWLINE_A);
a_oOutput.Write("<<<END_OF_TEXT" SI_NEWLINE_A);
if (!OutputMultiLineText(a_oOutput, convert, iValue->pItem)) {
return SI_FAIL;
}
a_oOutput.Write("SI-END-OF-MULTILINE-TEXT");
a_oOutput.Write("END_OF_TEXT");
}
else {
a_oOutput.Write(convert.Data());
@@ -2923,10 +3039,9 @@ public:
}
nError = U_ZERO_ERROR;
ucnv_resetToUnicode(m_pConverter);
int32_t nLen = ucnv_toUChars(m_pConverter, NULL, 0,
a_pInputData, (int32_t) a_uInputDataLen, &nError);
if (nError != U_BUFFER_OVERFLOW_ERROR) {
if (U_FAILURE(nError) && nError != U_BUFFER_OVERFLOW_ERROR) {
return (size_t) -1;
}
@@ -2963,7 +3078,6 @@ public:
}
nError = U_ZERO_ERROR;
ucnv_resetToUnicode(m_pConverter);
ucnv_toUChars(m_pConverter,
a_pOutputData, (int32_t) a_uOutputDataSize,
a_pInputData, (int32_t) a_uInputDataLen, &nError);
@@ -2998,10 +3112,9 @@ public:
}
nError = U_ZERO_ERROR;
ucnv_resetFromUnicode(m_pConverter);
int32_t nLen = ucnv_fromUChars(m_pConverter, NULL, 0,
a_pInputData, -1, &nError);
if (nError != U_BUFFER_OVERFLOW_ERROR) {
if (U_FAILURE(nError) && nError != U_BUFFER_OVERFLOW_ERROR) {
return (size_t) -1;
}
@@ -3037,7 +3150,6 @@ public:
}
nError = U_ZERO_ERROR;
ucnv_resetFromUnicode(m_pConverter);
ucnv_fromUChars(m_pConverter,
a_pOutputData, (int32_t) a_uOutputDataSize,
a_pInputData, -1, &nError);
corelib/src/VWDictionary.cpp
+23 -6
View File
@@ -17,12 +17,12 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "VWDictionary.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/VisualWord.h"
#include "VisualWord.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "NearestNeighbor.h"
#include "rtabmap/core/NearestNeighbor.h"
#include "rtabmap/core/Parameters.h"
#include "utilite/UtiLite.h"
@@ -232,7 +232,11 @@ void VWDictionary::setNNStrategy(NNStrategy strategy, const ParametersMap & para
switch(strategy)
{
case kNNKdTree:
_nn = new KdTreeNN(parameters);
//FIXME KdTreeNN is broken...
//_nn = new KdTreeNN(parameters);
//break;
UWARN("KdTree OpenCV is broken, setting nearest neighbor strategy to KdForest FLANN...");
_nn = new FlannKdTreeNN(parameters);
break;
case kNNFlannKdTree:
_nn = new FlannKdTreeNN(parameters);
@@ -248,9 +252,9 @@ void VWDictionary::setNNStrategy(NNStrategy strategy, const ParametersMap & para
}
else if(!_nn)
{
this->update();
_dataTree = cv::Mat();
}
this->update();
}
}
@@ -364,7 +368,10 @@ void VWDictionary::update()
void VWDictionary::clear()
{
ULOGGER_DEBUG("%d words destroyed", _visualWords.size());
if(_visualWords.size())
{
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", _visualWords.size());
}
for(std::map<int, VisualWord *>::iterator i=_visualWords.begin(); i!=_visualWords.end(); ++i)
{
delete (*i).second;
@@ -373,6 +380,9 @@ void VWDictionary::clear()
_lastNewWordsAddedCount = 0;
_totalActiveReferences = 0;
_lastWordId = 0;
_dataTree = cv::Mat();
_mapIndexId.clear();
_unusedWords.clear();
}
int VWDictionary::getNextId()
@@ -467,6 +477,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
if(!_dataTree.empty())
{
//Find nearest neighbors
UDEBUG("newPts.total()=%d ", newPts.total());
_nn->search(newPts, results, dists, k, _maxLeafs);
ULOGGER_DEBUG("Time to find nn = %f s", timerLocal.ticks());
}
@@ -537,6 +548,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
newWords.push_back(vw);
wordIds.push_back(vw->id());
UASSERT(vw->id()>0);
++newWordsCount;
}
else
@@ -544,6 +556,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
++dupWordsCount;
this->addWordRef(fullResults.begin()->second, signatureId);
wordIds.push_back(fullResults.begin()->second);
UASSERT(fullResults.begin()->second>0);
}
}
else if(fullResults.size())
@@ -552,6 +565,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
++dupWordsCount;
this->addWordRef(fullResults.begin()->second, signatureId);
wordIds.push_back(fullResults.begin()->second);
UASSERT(fullResults.begin()->second>0);
}
}
ULOGGER_DEBUG("naive search and add ref/words time = %f s", timerLocal.ticks());
@@ -601,12 +615,14 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
VisualWord * vw = new VisualWord(getNextId(), d, _dim, signatureId);
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
wordIds.push_back(vw->id());
UASSERT(vw->id()>0);
}
else
{
++dupWordsCount;
this->addWordRef(results.begin()->second, signatureId);
wordIds.push_back(results.begin()->second);
UASSERT(results.begin()->second>0);
}
}
else if(results.size())
@@ -615,6 +631,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
++dupWordsCount;
this->addWordRef(results.begin()->second, signatureId);
wordIds.push_back(results.begin()->second);
UASSERT(results.begin()->second>0);
}
}
ULOGGER_DEBUG("Naive search time = %fs", timer.ticks());
corelib/src/VWDictionary.h
-115
View File
@@ -1,115 +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 "VisualWord.h"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include "rtabmap/core/Parameters.h"
namespace rtabmap
{
class NearestNeighbor;
class DBDriver;
class RTABMAP_EXP VWDictionary
{
public:
enum NNStrategy{kNNNaive, kNNKdTree, kNNFlannKdTree, kNNUndef};
static const int ID_START;
static const int ID_INVALID;
public:
VWDictionary(const ParametersMap & parameters = ParametersMap());
virtual ~VWDictionary();
virtual void parseParameters(const ParametersMap & parameters);
virtual void update();
virtual std::list<int> addNewWords(
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, 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;
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;}
void setMinDist(float d);
float getMinDist() const {return _minDist;}
bool isMinDistUsed() const {return _minDistUsed;}
void setMinDistUsed(bool used) {_minDistUsed = used;}
void setNndrUsed(bool used) {_nndrUsed = used;}
bool isNndrUsed() const {return _nndrUsed;}
void setNndrRatio(float ratio);
float getNndrRatio() {return _nndrRatio;}
unsigned int getNotIndexedWordsCount() const {return _visualWords.size() - _mapIndexId.size();}
unsigned int getLastNewWordsAddedCount() const {return _lastNewWordsAddedCount;}
int getLastIndexedWordId() const;
int getTotalActiveReferences() const {return _totalActiveReferences;}
void setNNStrategy(NNStrategy strategy, const ParametersMap & parameters = ParametersMap());
NNStrategy nnStrategy() const;
bool isIncremental() const {return _incrementalDictionary;}
void setIncrementalDictionary(bool incrementalDictionary, const std::string & dictionaryPath);
void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
void clear();
std::vector<VisualWord *> getUnusedWords() const;
unsigned int getUnusedWordsSize() const {return _unusedWords.size();}
void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
protected:
int getNextId();
protected:
std::map<int, VisualWord *> _visualWords; //<id,VisualWord*>
unsigned int _lastNewWordsAddedCount;
int _totalActiveReferences; // keep track of all references for updating the common signature
private:
bool _incrementalDictionary;
bool _minDistUsed;
float _minDist; //euclidean distance ^ 2
bool _nndrUsed;
float _nndrRatio;
unsigned int _maxLeafs;
std::string _dictionaryPath; // a pre-computed dictionary (.txt)
int _dim;
int _lastWordId;
NearestNeighbor * _nn;
cv::Mat _dataTree;
std::map<int ,int> _mapIndexId;
std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
};
} // namespace rtabmap
corelib/src/VerifyHypotheses.cpp
+15 -325
View File
@@ -17,11 +17,12 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "VerifyHypotheses.h"
#include "rtabmap/core/VerifyHypotheses.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include <cstdlib>
#include <opencv2/calib3d/calib3d.hpp>
#include "utilite/UtiLite.h"
@@ -137,340 +138,29 @@ bool HypVerificatorEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
//bool allPairs = true;
int realPairsCount = 0;
findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
realPairsCount = findPairsOne(ssA->getWords(), ssB->getWords(), pairs, pairsId);
ULOGGER_DEBUG("%d %d", pairs.size(), pairsId.size());
int pairsCount = pairs.size();
ULOGGER_DEBUG("id(%d,%d) realPairsCount found=%d, pairsCount=%d...", ssA->id(), ssB->id(), realPairsCount, pairsCount);
int similarities = this->getTotalSimilarities(ssA->getWords(), ssB->getWords());
ULOGGER_DEBUG("realPairsCount=%d, "
"test1=%f%%, "
"test2=%f%%, "
"similarities/total=%f%%, "
"realP/similarities=%f%%, "
"(pairs/2)/similarities=%f%%",
realPairsCount,
float(realPairsCount)/(float(ssA->getWords().size() + ssB->getWords().size())/2),
float(pairs.size())/(float(ssA->getWords().size() + ssB->getWords().size())/2),
float(similarities)/float(ssA->getWords().size() + ssB->getWords().size()),
float(realPairsCount) / float(similarities),
float(pairs.size()) / float(similarities));
if(pairsCount < _matchCountMinAccepted)
if((int)pairs.size()<_matchCountMinAccepted)
{
return false;
}
//Convert Keypoints to a structure that OpenCV understands
//3 dimensions (Homogeneous vectors)
cv::Mat points1(1, pairs.size(), CV_32FC2);
cv::Mat points2(1, pairs.size(), CV_32FC2);
std::vector<uchar> status;
cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
float * points1data = points1.ptr<float>(0);
float * points2data = points2.ptr<float>(0);
// Fill the points here ...
int i=0;
for(std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::const_iterator iter = pairs.begin();
iter != pairs.end();
++iter )
int inliers = uSum(status);
if(inliers < _matchCountMinAccepted)
{
points1data[i*2] = (*iter).first.pt.x;
points1data[i*2+1] = (*iter).first.pt.y;
points2data[i*2] = (*iter).second.pt.x;
points2data[i*2+1] = (*iter).second.pt.y;
// the output of the correspondences can be easily copied in MatLab
/*if(i==0)
{
ULOGGER_DEBUG("pt x=[%f;%f;1;%d];,xp=[%f;%f;1;%d];",
(*iter).first.pt.x,
(*iter).first.pt.y,
Util::valueAt(pairsId,i),
(*iter).second.pt.x,
(*iter).second.pt.y,
Util::valueAt(pairsId,i));
}
else
{
ULOGGER_DEBUG("pt x=[x [%f;%f;1;%d]];,xp=[xp [%f;%f;1;%d]];",
(*iter).first.pt.x,
(*iter).first.pt.y,
Util::valueAt(pairsId,i),
(*iter).second.pt.x,
(*iter).second.pt.y,
Util::valueAt(pairsId,i));
}*/
++i;
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
return false;
}
UTimer timer;
timer.start();
// Find the fundamental matrix
cv::vector<uchar> status;
cv::Mat fundamentalMatrix = cv::findFundamentalMat(
points1,
points2,
status,
CV_FM_RANSAC,
_ransacParam1,
_ransacParam2);
ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
// Fundamental matrix is valid ?
bool fundMatFound = false;
if(fundamentalMatrix.type() != CV_64FC1)
else
{
ULOGGER_FATAL("fundamentalMatrix.type() != CV_64FC1");
UDEBUG("inliers = %d/%d", inliers, pairs.size());
return true;
}
if(fundamentalMatrix.cols==3 && fundamentalMatrix.rows==3 &&
(fundamentalMatrix.at<double>(0,0) != 0.0 ||
fundamentalMatrix.at<double>(0,1) != 0.0 ||
fundamentalMatrix.at<double>(0,2) != 0.0 ||
fundamentalMatrix.at<double>(1,0) != 0.0 ||
fundamentalMatrix.at<double>(1,1) != 0.0 ||
fundamentalMatrix.at<double>(1,2) != 0.0 ||
fundamentalMatrix.at<double>(2,0) != 0.0 ||
fundamentalMatrix.at<double>(2,1) != 0.0 ||
fundamentalMatrix.at<double>(2,2) != 0.0) )
{
fundMatFound = true;
}
ULOGGER_DEBUG("id(%d,%d) fm_count=%d...", ssA->id(), ssB->id(), fundMatFound);
if(fundMatFound)
{
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > inliers;
std::list<int> inliersId;
int goodCount = 0;
float total = 0;
std::list<std::pair<float, float> > ptsAddedA;
std::list<std::pair<float, float> > ptsAddedB;
cv::Mat x(3, 1, fundamentalMatrix.type());
cv::Mat xp(1, 3, fundamentalMatrix.type());
int i=0;
for(std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
{
//if(status[i])
{
if(uContains(ptsAddedA, std::pair<float, float>((*iter).first.pt.x, (*iter).first.pt.y)))
{
ULOGGER_DEBUG("already added point [%f,%f,1]", (*iter).first.pt.x, (*iter).first.pt.y);
}
else if(uContains(ptsAddedB, std::pair<float, float>((*iter).second.pt.x, (*iter).second.pt.y)))
{
ULOGGER_DEBUG("already added point [%f,%f,1]", (*iter).second.pt.x, (*iter).second.pt.y);
}
else
{
double * xData = x.ptr<double>(0);
double * xpData = xp.ptr<double>(0);
xData[0] = (*iter).first.pt.x;
xData[1] = (*iter).first.pt.y;
xData[2] = 1;
xpData[0] = (*iter).second.pt.x;
xpData[1] = (*iter).second.pt.y;
xpData[2] = 1;
cv::Mat r = xp * (fundamentalMatrix * x);
//if((r->data.fl[0] < 0 ? -r->data.fl[0]:r->data.fl[0]) < 1000000)
{
// Add only once a pair for the same id, used when a point matches with more than one...
ptsAddedA.push_back(std::pair<float, float>((*iter).first.pt.x, (*iter).first.pt.y));
ptsAddedB.push_back(std::pair<float, float>((*iter).second.pt.x, (*iter).second.pt.y));
if(status[i])
{
inliers.push_back(*iter);
inliersId.push_back(uValueAt(pairsId, i));
goodCount++;
}
//ULOGGER_DEBUG("[%d] status=%d, r->data.fl[0]=%f, Added!", Util::valueAt(pairsId,i), status[i], r.ptr<double>(0)[0]);
}
/*else
{
ULOGGER_DEBUG("status=%d, r->data.fl[0]=%f, Not added!", status->data.ptr[i], r->data.fl[0]);
}*/
total+=(r.ptr<double>(0)[0] < 0 ? -r.ptr<double>(0)[0]:r.ptr<double>(0)[0]);
}
}
/*else
{
ULOGGER_DEBUG("VHEpipolarGeo::doEpipolarGeometry() status=%d", status[i]);
}*/
++i;
}
ULOGGER_DEBUG("pairs/realPairs=%d/%d -> %d%%, goodCount=%d -> %d%%, good/real = %d%%, totalMean=%f",
pairsCount,
realPairsCount,
int(float(pairsCount)/float(realPairsCount*100)),
goodCount,
int(float(goodCount)/float(pairsCount*100)),
int(float(goodCount)/float(realPairsCount*100)),
total/float(realPairsCount));
// Show the fundamental matrix
ULOGGER_DEBUG(
"F = [%f %f %f;%f %f %f;%f %f %f]",
fundamentalMatrix.ptr<double>(0)[0],
fundamentalMatrix.ptr<double>(0)[1],
fundamentalMatrix.ptr<double>(0)[2],
fundamentalMatrix.ptr<double>(0)[3],
fundamentalMatrix.ptr<double>(0)[4],
fundamentalMatrix.ptr<double>(0)[5],
fundamentalMatrix.ptr<double>(0)[6],
fundamentalMatrix.ptr<double>(0)[7],
fundamentalMatrix.ptr<double>(0)[8]);
if(goodCount < _matchCountMinAccepted)
{
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d), min is %d", goodCount, _matchCountMinAccepted);
return false;
}
else
{
return true;
}
}
return false;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
*/
int HypVerificatorEpipolarGeo::findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
std::list<int> & pairsId)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
std::multimap<int, cv::KeyPoint>::const_iterator iterA;
std::multimap<int, cv::KeyPoint>::const_iterator iterB;
pairs.clear();
int realPairsCount = 0;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
iterA = wordsA.find(*i);
iterB = wordsB.find(*i);
while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
{
pairsId.push_back(*i);
pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second));
++iterA;
++iterB;
++realPairsCount;
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int HypVerificatorEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
std::list<int> & pairsId)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int realPairsCount = 0;
pairs.clear();
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
if(ptsA.size() == 1 && ptsB.size() == 1)
{
pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front()));
pairsId.push_back(*i);
++realPairsCount;
}
else if(ptsA.size()>1 && ptsB.size()>1)
{
// just update the count
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
* realPairsCount = 5
*/
int HypVerificatorEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
std::list<int> & pairsId)
{
UTimer timer;
timer.start();
const std::list<int> & ids = uUniqueKeys(wordsA);
pairs.clear();
int realPairsCount = 0;;
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
{
for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
{
pairsId.push_back(*iter);
pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter));
}
}
}
ULOGGER_DEBUG("time = %f", timer.ticks());
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [1 2 4 6]
* return 4
*/
std::list<int> HypVerificatorEpipolarGeo::findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB)
{
std::list<int> sameIds;
const std::list<int> & ids = uUniqueKeys(wordsA);
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(wordsB.find(*i) != wordsB.end())
{
sameIds.push_back(*i);
}
}
return sameIds;
}
int HypVerificatorEpipolarGeo::getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int total = 0;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
total += uValues(wordsA, *i).size();
total += uValues(wordsB, *i).size();
}
return total;
}
}
corelib/src/VerifyHypotheses.h
-103
View File
@@ -1,103 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef VERIFYHYPOTHESES_H_
#define VERIFYHYPOTHESES_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <list>
#include "rtabmap/core/Parameters.h"
#include "utilite/UEventsHandler.h"
#include <map>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap
{
class Signature;
// return always true, i.e, there is no verification
class RTABMAP_EXP HypVerificator
{
public:
HypVerificator(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificator() {}
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
};
/////////////////////////
// HypVerificatorSim
/////////////////////////
class HypVerificatorSim : public HypVerificator {
public:
HypVerificatorSim(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificatorSim();
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
private:
float _similarity;
};
/////////////////////////
// HypVerificatorEpipolarGeo
/////////////////////////
class KeypointSignature;
class RTABMAP_EXP HypVerificatorEpipolarGeo : public HypVerificator
{
public:
static int findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
static int findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
static int findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
static std::list<int> findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
public:
HypVerificatorEpipolarGeo(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificatorEpipolarGeo();
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
int getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
double getRansacParam1() const {return _ransacParam1;}
double getRansacParam2() const {return _ransacParam2;}
void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
private:
bool doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB);
private:
int _matchCountMinAccepted;
double _ransacParam1;
double _ransacParam2;
};
} // namespace rtabmap
#endif /* VERIFYHYPOTHESES_H_ */
corelib/src/VisualWord.cpp
+1 -1
View File
@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "VisualWord.h"
#include "rtabmap/core/VisualWord.h"
#include "utilite/ULogger.h"
#include "utilite/UStl.h"
corelib/src/VisualWord.h
-60
View File
@@ -1,60 +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>
namespace rtabmap
{
class SignatureSurf;
class RTABMAP_EXP VisualWord
{
public:
VisualWord(int id, const float * descriptor, int dim, int signatureId = 0);
~VisualWord();
void addRef(int signatureId);
int removeAllRef(int signatureId);
int getTotalReferences() const {return _totalReferences;}
int id() const {return _id;}
const float * getDescriptor() const {return _descriptor;}
int getDim() const {return _dim;}
const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
bool isSaved() const {return _saved;}
void setSaved(bool saved) {_saved = saved;}
private:
int _id;
float * _descriptor;
int _dim;
bool _saved; // If it's saved to bd
int _totalReferences;
std::map<int, int> _references; // (signature id , occurrence in the signature)
std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
};
} // namespace rtabmap
corelib/src/resources/DatabaseSchema.sql
+75 -160
View File
@@ -1,221 +1,136 @@
-- *******************************************************************
-- construct_avpd_db: Script for creating the database
-- Usage:
-- $ sqlite3 AvpdDatabase.db < DatabaseSchema.sql
-- $ sqlite3 LTM.db < DatabaseSchema.sql
--
-- *******************************************************************
-- *******************************************************************
-- CLEAN
-- *******************************************************************
/*DROP TABLE Signature;
DROP TABLE SignatureType;
DROP TABLE Neighbor;
DROP TABLE VisualWord;
DROP TABLE Map_SS_VW;
DROP TABLE StatisticsAfterRun;
DROP TABLE StatisticsAfterRunSurf;*/
/*DROP TABLE Node;
DROP TABLE Link;
DROP TABLE Sensor;
DROP TABLE Actuator;
DROP TABLE Word;
DROP TABLE Map_Node_Word;
DROP TABLE Statistics;
DROP TABLE StatisticsSurf;*/
-- *******************************************************************
-- CREATE
-- *******************************************************************
CREATE TABLE Signature (
CREATE TABLE Node (
id INTEGER NOT NULL,
type VARCHAR NOT NULL,
type INTEGER NOT NULL, -- 0=Keypoint, 1=Sensor
weight INTEGER,
loopClosureIds BLOB,
childLoopClosureIds BLOB,
timeEnter DATE,
PRIMARY KEY (id),
FOREIGN KEY (type) REFERENCES SignatureType(type)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Image (
CREATE TABLE Sensor (
id INTEGER NOT NULL,
num INTEGER NOT NULL,
type INTEGER NOT NULL, -- kTypeImage=0, kTypeImageFeatures2d, kTypeAudio, kTypeAudioFreq, kTypeAudioFreqSqrdMagn, kTypeJointState, kTypeNotSpecified
data BLOB, -- PostProcessed data (indexed integers)
raw_width INTEGER NOT NULL,
raw_height INTEGER NOT NULL,
raw_data_type INTEGER NOT NULL,
raw_compressed CHAR NOT NULL,
raw_data BLOB,
PRIMARY KEY (id, num)
);
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
actuator_id INTEGER,
base_ids BLOB,
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
CREATE TABLE Actuator (
id INTEGER NOT NULL,
num INTEGER NOT NULL,
type INTEGER NOT NULL, -- kTypeTwist=0, kTypeNotSpecified
width INTEGER NOT NULL,
height INTEGER NOT NULL,
channels INTEGER NOT NULL,
compressed CHAR NOT NULL,
data_type INTEGER NOT NULL,
data BLOB,
timeEnter DATE,
PRIMARY KEY (id)
PRIMARY KEY (id, num)
);
CREATE TABLE SMState (
--
CREATE TABLE Word (
id INTEGER NOT NULL,
sensors BLOB,
motionMask BLOB,
timeEnter DATE,
FOREIGN KEY (id) REFERENCES Signature(id)
);
CREATE TABLE Neighbor (
sid INTEGER NOT NULL,
nid INTEGER NOT NULL,
actionSize INTEGER,
actions BLOB,
baseIds BLOB,
FOREIGN KEY (sid) REFERENCES Signature(id),
FOREIGN KEY (nid) REFERENCES Signature(id)
);
CREATE TABLE SignatureType (
type VARCHAR NOT NULL,
PRIMARY KEY (type)
);
CREATE TABLE VisualWord (
id INTEGER NOT NULL,
descriptorSize INTEGER NOT NULL,
descriptor_size INTEGER NOT NULL,
descriptor BLOB NOT NULL,
timeEnter DATE,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Map_SS_VW (
signatureId INTEGER NOT NULL,
visualWordId INTEGER NOT NULL,
CREATE TABLE Map_Node_Word (
node_id INTEGER NOT NULL,
word_id INTEGER NOT NULL,
pos_x FLOAT NOT NULL,
pos_y FLOAT NOT NULL,
laplacian INTEGER NOT NULL,
size INTEGER NOT NULL,
dir FLOAT NOT NULL,
hessian FLOAT NOT NULL,
FOREIGN KEY (signatureId) REFERENCES Signature(id),
FOREIGN KEY (visualWordId) REFERENCES VisualWord(id)
response FLOAT NOT NULL,
FOREIGN KEY (node_id) REFERENCES Node(id),
FOREIGN KEY (word_id) REFERENCES Word(id)
);
CREATE TABLE StatisticsAfterRun (
stMemSize INTEGER,
lastSignAdded INTEGER,
processMemUsed INTEGER,
databaseMemUsed INTEGER,
timeEnter DATE
CREATE TABLE Statistics (
STM_size INTEGER,
last_sign_added INTEGER,
process_mem_used INTEGER,
database_mem_used INTEGER,
time_enter DATE
);
CREATE TABLE StatisticsAfterRunSurf (
dictionarySize INTEGER,
timeEnter DATE
CREATE TABLE StatisticsDictionary (
dictionary_size INTEGER,
time_enter DATE
);
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
CREATE TRIGGER insert_Signature BEFORE INSERT ON Signature
WHEN NOT EXISTS (SELECT type FROM SignatureType WHERE SignatureType.type = NEW.type)
CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word
WHEN NOT EXISTS (SELECT type FROM Node WHERE Node.id = NEW.node_id AND type=0)
BEGIN
SELECT RAISE(ABORT, 'Foreign key Signature.type constraint failed');
SELECT RAISE(ABORT, 'Keypoint type constraint failed');
END;
CREATE TRIGGER insert_SMState BEFORE INSERT ON SMState
WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.id)
-- Creating a trigger for time_enter
CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
BEGIN
SELECT RAISE(ABORT, 'Foreign key SMState.id constraint failed');
UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
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)
CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
BEGIN
SELECT RAISE(ABORT, 'Foreign key Neighbor.sid constraint failed');
UPDATE Word SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
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='KeypointSignature')
CREATE TRIGGER insert_Statistics_timeEnter AFTER INSERT ON Statistics
BEGIN
SELECT RAISE(ABORT, 'KeypointSignature type constraint failed');
UPDATE Statistics SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
-- Creating a trigger for timeEnter
CREATE TRIGGER insert_Signature_timeEnter AFTER INSERT ON Signature
CREATE TRIGGER insert_StatisticsDictionary_timeEnter AFTER INSERT ON StatisticsDictionary
BEGIN
UPDATE Signature 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_StatisticsAfterRun_timeEnter AFTER INSERT ON StatisticsAfterRun
BEGIN
UPDATE StatisticsAfterRun SET timeEnter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_StatisticsAfterRunSurf_timeEnter AFTER INSERT ON StatisticsAfterRunSurf
BEGIN
UPDATE StatisticsAfterRunSurf SET timeEnter = DATETIME('NOW') WHERE rowid = new.rowid;
UPDATE StatisticsDictionary SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
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_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);
CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
CREATE INDEX IDX_Sensor_Id on Sensor (id);
CREATE INDEX IDX_Link_from_id on Link (from_id);
-- *******************************************************************
-- Data
-- *******************************************************************
INSERT INTO SignatureType(type) VALUES ('KeypointSignature');
INSERT INTO SignatureType(type) VALUES ('SMSignature');
-- *******************************************************************
-- TESTS
-- *******************************************************************
-- *** Data Test ***
/*
INSERT INTO Signature VALUES(1, 'surf', null, null, null);
INSERT INTO Signature VALUES(2, 'surf', null, null, null);
INSERT INTO Signature VALUES(3, 'surf', null, null, null);
INSERT INTO VisualWord VALUES (1, 1, 2,'0.213213 0.4352323', null);
INSERT INTO VisualWord VALUES (2, 1, 2,'0.213213 0.4352323', null);
INSERT INTO VisualWord VALUES (3, 3, 2,'0.213213 0.4352323', null);
INSERT INTO Map_SS_VW VALUES (1, 1, 0,0,0,0,0, null);
INSERT INTO Map_SS_VW VALUES (2, 1, 0,0,0,0,0, null);
INSERT INTO Map_SS_VW VALUES (2, 2, 0,0,0,0,0, null);
*/
/*
-- For loading words
SELECT vw.id, vw.laplacian, vw.descriptorSize, vw.descriptor, m.signatureId FROM VisualWord as vw INNER JOIN Map_SS_VW as m on vw.id=m.visualWordId ORDER BY vw.id;
*/
-- Refreshing the dictionary
/*SELECT * FROM Map_SS_VW;
SELECT * FROM VisualWord;*/
/*
DELETE FROM VisualWord;
INSERT INTO VisualWord VALUES (1, 1, 2,'0.213213 0.4352323', null);
DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT id FROM VisualWord WHERE id = Map_SS_VW.visualWordId);
*/
/*SELECT * FROM Map_SS_VW;
SELECT * FROM VisualWord;*/
/*
-- Loading only signatures on the last short time memory based on DATE
INSERT INTO Signature VALUES(4, 'surf', null, null, null);
INSERT INTO Signature VALUES(5, 'surf', 4, null, null);
INSERT INTO Signature VALUES(6, 'surf', null, null, null);
INSERT INTO Map_SS_VW VALUES (4, 1, 0,0,0,0,0, null);
SELECT s.id FROM Signature AS s WHERE s.timeEnter >= (SELECT vw.timeEnter FROM VisualWord AS vw LIMIT 1) AND s.loopClosureId IS NULL;
*/