i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged attention branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@657 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-12-11 18:05:05 +00:00
parent f9033809a2
commit 2836d4c48c
216 changed files with 7983 additions and 94891 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
tools/CMakeLists.txt
View File

@@ -1,10 +1,8 @@
ADD_SUBDIRECTORY( ConsoleApp )
ADD_SUBDIRECTORY( ImagesJoiner )
ADD_SUBDIRECTORY( WebcamCapture )
ADD_SUBDIRECTORY( Polar )
ADD_SUBDIRECTORY( ImagesDbExtractor )
ADD_SUBDIRECTORY( ColorIndexesGenerator )
ADD_SUBDIRECTORY( VocabularyComparison )
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
ADD_SUBDIRECTORY( DatabaseViewer )

32
tools/ColorIndexesGenerator/CMakeLists.txt
View File

@@ -1,32 +0,0 @@
SET(SRC_FILES
main.cpp
)
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${CMAKE_CURRENT_SOURCE_DIR}
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${ZLIB_INCLUDE_DIRS}
)
SET(LIBRARIES
${UTILITE_LIBRARIES}
${OpenCV_LIBRARIES}
${ZLIB_LIBRARIES}
)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(colorIndexesGenerator ${SRC_FILES})
TARGET_LINK_LIBRARIES(colorIndexesGenerator rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES( colorIndexesGenerator
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-colorIndexesGenerator)
INSTALL(TARGETS colorIndexesGenerator
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)

228
tools/ColorIndexesGenerator/main.cpp
View File

@@ -1,228 +0,0 @@
#include <opencv2/core/core.hpp>
#include <iostream>
#include <fstream>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include "rtabmap/core/ColorTable.h"
#include "rtabmap/core/NearestNeighbor.h"
#include <zlib.h>
#include <utilite/UConversion.h>
#define FILE_NAME_PREFIX "ColorIndexes"
#define FILE_NAME_SUFFIX ".bin"
#define CHUNK 16384
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-colorIndexesGenerator size\n"
" size Supported : 8, 16, 32, 64, 128, 256, 512, 1024, 65536\n");
exit(1);
}
cv::Mat generateColorTable()
{
cv::Mat colorTable(256*256*256, 3, CV_32F);
for(int b=0; b<256; ++b)
{
for(int g=0; g<256; ++g)
{
for(int r=0; r<256; ++r)
{
colorTable.at<float>(b*256*256 + g*256 + r, 0) = (float)r;
colorTable.at<float>(b*256*256 + g*256 + r, 1) = (float)g;
colorTable.at<float>(b*256*256 + g*256 + r, 2) = (float)b;
UDEBUG("r=%f, g=%f, b=%f", (float)r , (float)g, (float)b);
}
}
}
return colorTable;
}
/* report a zlib or i/o error */
void zerr(int ret)
{
fputs("zpipe: ", stderr);
switch (ret) {
case Z_ERRNO:
if (ferror(stdin))
fputs("error reading stdin\n", stderr);
if (ferror(stdout))
fputs("error writing stdout\n", stderr);
break;
case Z_STREAM_ERROR:
fputs("invalid compression level\n", stderr);
break;
case Z_DATA_ERROR:
fputs("invalid or incomplete deflate data\n", stderr);
break;
case Z_MEM_ERROR:
fputs("out of memory\n", stderr);
break;
case Z_VERSION_ERROR:
fputs("zlib version mismatch!\n", stderr);
break;
}
UFATAL("");
}
/* Compress from file source to file dest until EOF on source.
def() returns Z_OK on success, Z_MEM_ERROR if memory could not be
allocated for processing, Z_STREAM_ERROR if an invalid compression
level is supplied, Z_VERSION_ERROR if the version of zlib.h and the
version of the library linked do not match, or Z_ERRNO if there is
an error reading or writing the files. */
int def(FILE *source, FILE *dest, int level)
{
int ret, flush;
unsigned have;
z_stream strm;
unsigned char in[CHUNK];
unsigned char out[CHUNK];
/* allocate deflate state */
strm.zalloc = Z_NULL;
strm.zfree = Z_NULL;
strm.opaque = Z_NULL;
ret = deflateInit(&strm, level);
if (ret != Z_OK)
return ret;
UDEBUG("");
/* compress until end of file */
do {
UDEBUG("");
strm.avail_in = fread(in, 1, CHUNK, source);
if (ferror(source)) {
(void)deflateEnd(&strm);
return Z_ERRNO;
}
flush = feof(source) ? Z_FINISH : Z_NO_FLUSH;
strm.next_in = in;
/* run deflate() on input until output buffer not full, finish
compression if all of source has been read in */
do {
UDEBUG("");
strm.avail_out = CHUNK;
strm.next_out = out;
ret = deflate(&strm, flush); /* no bad return value */
assert(ret != Z_STREAM_ERROR); /* state not clobbered */
have = CHUNK - strm.avail_out;
if (fwrite(out, 1, have, dest) != have || ferror(dest)) {
(void)deflateEnd(&strm);
return Z_ERRNO;
}
} while (strm.avail_out == 0);
assert(strm.avail_in == 0); /* all input will be used */
/* done when last data in file processed */
} while (flush != Z_FINISH);
assert(ret == Z_STREAM_END); /* stream will be complete */
/* clean up and return */
(void)deflateEnd(&strm);
return Z_OK;
}
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
ULogger::setPrintWhere(false);
UTimer timer;
if(argc<2)
{
showUsage();
}
int size = atoi(argv[1]);
rtabmap::FlannKdTreeNN nn;
cv::Mat colorTable;
switch(size)
{
case 8:
colorTable = cv::Mat(8, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_8);
break;
case 16:
colorTable = cv::Mat(16, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_16);
break;
case 32:
colorTable = cv::Mat(32, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_32);
break;
case 64:
colorTable = cv::Mat(64, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_64);
break;
case 128:
colorTable = cv::Mat(128, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_128);
break;
case 256:
colorTable = cv::Mat(256, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_256);
break;
case 512:
colorTable = cv::Mat(512, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_512);
break;
case 1024:
colorTable = cv::Mat(1024, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_1024);
break;
case 65536:
colorTable = cv::Mat(65536, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_65536);
break;
default:
printf("\nSize not supported...\n");
showUsage();
break;
}
cv::Mat data;
colorTable.convertTo(data, CV_32F);
if(data.rows > 0x10000)
{
UFATAL("Color table is too big (%d > 2^16)", data.rows);
}
UINFO("Generating full color cube...");
cv::Mat queries = generateColorTable();
UINFO("Generating full color cube... done!");
cv::Mat dists(queries.rows, 1, CV_32F);
cv::Mat indices = cv::Mat(queries.rows, 1, CV_32S);
UINFO("Nearest neighbor searching (queries=%d, indexes=%d)...", queries.rows, data.rows);
nn.search(data, queries, indices, dists);
UINFO("Nearest neighbor searching (queries=%d, indexes=%d)... done!", queries.rows, data.rows);
std::string fileName = uFormat("%s%d%s", FILE_NAME_PREFIX, size, FILE_NAME_SUFFIX);
UINFO("Saving color indexed table to %s...", fileName.c_str());
std::ofstream outfile(fileName.c_str(), std::ios_base::out | std::ios_base::binary);
unsigned short index;
for(int i=0; i<indices.rows; ++i)
{
UDEBUG("%d = %d", i, indices.at<int>(i,0));
index = (unsigned short)indices.at<int>(i,0); // assume indexes are under 2^16
outfile.write((const char *)&index, sizeof(unsigned short));
}
outfile.close();
UINFO("Saving color indexed table to %s... done!", fileName.c_str());
std::string zipFileName = fileName + std::string(".zip");
UINFO("Compressing file %s to %s...", fileName.c_str(), zipFileName.c_str());
FILE * input = fopen(fileName.c_str(), "rb");
FILE * compressed = fopen(zipFileName.c_str(), "wb");
int result = def(input, compressed, Z_DEFAULT_COMPRESSION);
if(result != Z_OK)
{
zerr(result);
UERROR("");
}
fclose(input);
fclose(compressed);
UINFO("Compressing file %s to %s... done!", fileName.c_str(), zipFileName.c_str());
UINFO("Total time = %f s", timer.getElapsedTime());
return 0;
}

42
tools/ConsoleApp/main.cpp
View File

@@ -380,9 +380,9 @@ int main(int argc, char * argv[])
rtabmap->init();
rtabmap->setMaxTimeAllowed(timeThreshold); // in ms
//ULogger::setType(ULogger::kTypeConsole);
ULogger::setType(ULogger::kTypeFile, rtabmap->getWorkingDir()+"/LogConsole.txt", false);
ULogger::setBuffered(true);
ULogger::setType(ULogger::kTypeConsole);
//ULogger::setType(ULogger::kTypeFile, rtabmap->getWorkingDir()+"/LogConsole.txt", false);
//ULogger::setBuffered(true);
ULogger::setLevel(logLevel);
ULogger::setExitLevel(exitLevel);
@@ -432,28 +432,38 @@ int main(int argc, char * argv[])
camera->parseParameters(pm);
UTimer iterationTimer;
UTimer rtabmapTimer;
int imagesProcessed = 0;
std::list<std::vector<float> > teleopActions;
while(loopDataset <= repeat && g_forever)
{
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat img = camera->takeImage(descriptors, keypoints);
cv::Mat cvImg = camera->takeImage(descriptors, keypoints);
Image img(cvImg, descriptors, keypoints);
int i=0;
double maxIterationTime = 0.0;
int maxIterationTimeId = 0;
while(!img.empty() && g_forever)
{
++imagesProcessed;
iterationTimer.start();
rtabmap->process(Sensor(descriptors, keypoints));
rtabmapTimer.start();
rtabmap->process(img);
double rtabmapTime = rtabmapTimer.elapsed();
loopClosureId = rtabmap->getLoopClosureId();
if(rtabmap->getLoopClosureId())
{
++countLoopDetected;
}
img = camera->takeImage(descriptors, keypoints);
cvImg = camera->takeImage(descriptors, keypoints);
img = Image(cvImg, descriptors, keypoints);
if(++count % 100 == 0)
{
printf(" count = %d, loop closures = %d\n", count, countLoopDetected);
printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",
count, countLoopDetected, maxIterationTimeId, maxIterationTime);
maxIterationTime = 0.0;
maxIterationTimeId = 0;
std::map<int, int> wm = rtabmap->getWeights();
printf(" WM(%d)=[", (int)wm.size());
for(std::map<int, int>::iterator iter=wm.begin(); iter!=wm.end();++iter)
@@ -480,24 +490,32 @@ int main(int argc, char * argv[])
double iterationTime = iterationTimer.ticks();
if(rtabmapTime > maxIterationTime)
{
maxIterationTime = rtabmapTime;
maxIterationTimeId = count;
}
ULogger::flush();
if(rtabmap->getLoopClosureId())
{
printf(" iteration(%d) loop(%d) time=%fs *\n", count, rtabmap->getLoopClosureId(), iterationTime);
printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n",
count, rtabmap->getLoopClosureId(), rtabmap->getLcHypValue(), rtabmapTime, iterationTime);
}
else if(rtabmap->getReactivatedId())
{
printf(" iteration(%d) high(%d) time=%fs\n", count, rtabmap->getReactivatedId(), iterationTime);
printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
count, rtabmap->getReactivatedId(), rtabmap->getLcHypValue(), rtabmapTime, iterationTime);
}
else
{
printf(" iteration(%d) time=%fs\n", count, iterationTime);
printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime);
}
if(timeThreshold && iterationTime > timeThreshold*100.0f)
if(timeThreshold && rtabmapTime > timeThreshold*100.0f)
{
printf(" ERROR, there is problem, too much time taken... %fs", iterationTime);
printf(" ERROR, there is problem, too much time taken... %fs", rtabmapTime);
break; // there is problem, don't continue
}
}

20
tools/DatabaseViewer/CMakeLists.txt
View File

@@ -1,25 +1,7 @@
### Qt Gui stuff ###
SET(headers_ui
./MainWindow.h
)
SET(uis
./ui/MainWindow.ui
)
#Generate .h files from the .ui files
QT4_WRAP_UI(moc_uis ${uis})
#This will generate moc_* for Qt
QT4_WRAP_CPP(moc_srcs ${headers_ui})
### Qt Gui stuff end###
SET(SRC_FILES
./main.cpp
./MainWindow.cpp
${moc_srcs}
${moc_uis}
)
SET(INCLUDE_DIRS
@@ -27,7 +9,6 @@ SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/guilib/include
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
)
INCLUDE(${QT_USE_FILE})
@@ -36,7 +17,6 @@ SET(LIBRARIES
${UTILITE_LIBRARIES}
${QT_LIBRARIES}
${OpenCV_LIBRARIES}
#${QWT5_LIBRARY}
)
#include files

552
tools/DatabaseViewer/MainWindow.cpp
View File

@@ -1,552 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "MainWindow.h"
#include "ui_MainWindow.h"
#include <QtGui/QMessageBox>
#include <QtGui/QFileDialog>
#include <QtGui/QInputDialog>
#include <QtCore/QBuffer>
#include <QtCore/QTextStream>
#include <utilite/ULogger.h>
#include <utilite/UDirectory.h>
#include <utilite/UConversion.h>
#include <opencv2/core/core_c.h>
#include <utilite/UTimer.h>
#include "rtabmap/core/KeypointMemory.h"
#include "rtabmap/core/SMMemory.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/gui/KeypointItem.h"
MainWindow::MainWindow(QWidget * parent) :
QMainWindow(parent),
memory_(0)
{
pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map"; //use home directory by default
if(!UDirectory::exists(pathDatabase_.toStdString()))
{
pathDatabase_ = QDir::homePath();
}
ui_ = new Ui_MainWindow();
ui_->setupUi(this);
connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
// connect actions with custom slots
connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
connect(ui_->actionClean_database, SIGNAL(triggered()), this, SLOT(cleanDatabase()));
connect(ui_->actionClean_local_graph, SIGNAL(triggered()), this, SLOT(cleanLocalGraph()));
ui_->graphicsView_A->setScene(new QGraphicsScene(this));
ui_->graphicsView_B->setScene(new QGraphicsScene(this));
ui_->horizontalSlider_A->setTracking(false);
ui_->horizontalSlider_B->setTracking(false);
ui_->horizontalSlider_A->setEnabled(false);
ui_->horizontalSlider_B->setEnabled(false);
connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
}
MainWindow::~MainWindow()
{
delete ui_;
if(memory_)
{
delete memory_;
}
}
void MainWindow::openDatabase()
{
QStringList types;
types << "Keypoint" << "Sensorimotor";
bool ok = false;
QString type = QInputDialog::getItem(this, tr("Select database type"), tr("Type"), types, 0, false, &ok);
if(ok && !type.isEmpty())
{
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
if(!path.isEmpty())
{
if(memory_)
{
delete memory_;
memory_ = 0;
imagesMap_.clear();
ids_.clear();
}
std::string driverType = "sqlite3";
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
if(type.compare("Keypoint") == 0)
{
memory_ = new rtabmap::KeypointMemory(parameters);
}
else
{
memory_ = new rtabmap::SMMemory(parameters);
}
if(!memory_)
{
QMessageBox::warning(this, "Database error", tr("Can't create database driver \"%1\"").arg(driverType.c_str()));
}
else if(!memory_->init(driverType, path.toStdString()))
{
QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
}
else
{
pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
updateIds();
}
}
}
}
void MainWindow::updateIds()
{
if(!memory_)
{
return;
}
std::set<int> ids = memory_->getAllSignatureIds();
ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
ids_.prepend(0);
UDEBUG("Loaded %d ids", ids_.size());
if(ids_.size())
{
ui_->horizontalSlider_A->setMinimum(0);
ui_->horizontalSlider_B->setMinimum(0);
ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
ui_->horizontalSlider_A->setSliderPosition(0);
ui_->horizontalSlider_B->setSliderPosition(0);
ui_->horizontalSlider_A->setEnabled(true);
ui_->horizontalSlider_B->setEnabled(true);
ui_->label_idA->setText("0");
ui_->label_idB->setText("0");
}
else
{
ui_->horizontalSlider_A->setEnabled(false);
ui_->horizontalSlider_B->setEnabled(false);
ui_->label_idA->setText("NaN");
ui_->label_idB->setText("NaN");
}
}
void MainWindow::cleanDatabase()
{
if(!memory_ || !ids_.size())
{
QMessageBox::warning(this, tr("Cannot clean the database"), tr("A database (not empty) must must loaded first...\nUse File->Open database."));
return;
}
bool ok;
int depth = QInputDialog::getInt(this, tr("Depth around the weighted locations?"), tr("Depth"), 10, 1, 1000, 1, &ok);
if(ok)
{
UINFO("Cleaning...");
//Remove all signatures with null weight and between two intersections
memory_->cleanLTM(depth);
//dbDriver_->executeNoResult(std::string("DELETE FROM Signature WHERE loopClosureId!=0;"));
//dbDriver_->executeNoResult(std::string("DELETE FROM Neighbor WHERE NOT EXISTS (SELECT * FROM Signature WHERE Signature.id = Neighbor.sid);"));
//dbDriver_->executeNoResult(std::string("DELETE FROM Neighbor WHERE NOT EXISTS (SELECT * FROM Signature WHERE Signature.id = Neighbor.nid);"));
// Clean links
//dbDriver_->executeNoResult(std::string("DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT * FROM Signature WHERE Signature.id = signatureId);"));
// Clean words
//dbDriver_->deleteUnreferencedWords();
updateIds();
UINFO("Finished cleaning!");
}
}
void MainWindow::cleanLocalGraph()
{
if(!ids_.size() || !memory_)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
return;
}
bool ok = false;
int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
if(ok)
{
int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
if(ok)
{
UTimer timer;
UINFO("Cleaning local graph for location %d", id);
memory_->cleanLocalGraph(id, margin);
updateIds();
UINFO("time=%fs", timer.ticks());
}
}
}
void MainWindow::generateGraph()
{
if(!memory_)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
return;
}
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
if(!path.isEmpty())
{
memory_->generateGraph(path.toStdString());
}
}
void MainWindow::generateLocalGraph()
{
if(!ids_.size() || !memory_)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
return;
}
bool ok = false;
int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
if(ok)
{
int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
if(ok)
{
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
if(!path.isEmpty())
{
double dbAccessTime = 0.0;
std::map<int, int> ids = memory_->getNeighborsId(dbAccessTime, id, margin, -1, false, false, false);
if(ids.size() > 0)
{
ids.insert(std::pair<int,int>(id, 0));
std::set<int> idsSet;
for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
idsSet.insert(idsSet.end(), iter->first);
UINFO("Node %d", iter->first);
}
UINFO("idsSet=%d", idsSet.size());
memory_->generateGraph(path.toStdString(), idsSet);
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
}
}
}
}
}
void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene)
{
if(!scene)
{
return;
}
rtabmap::KeypointItem * item = 0;
int alpha = 70;
for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
{
const cv::KeyPoint & r = (*i).second;
int id = (*i).first;
QString info = QString( "WordRef = %1\n"
"Laplacian = %2\n"
"Dir = %3\n"
"Hessian = %4\n"
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
scene->addItem(item);
item->setZValue(1);
}
}
void MainWindow::sliderAValueChanged(int value)
{
this->update(value,
ui_->label_indexA,
ui_->label_actionsA,
ui_->label_parentsA,
ui_->label_childrenA,
ui_->graphicsView_A,
ui_->label_idA);
}
void MainWindow::sliderBValueChanged(int value)
{
this->update(value,
ui_->label_indexB,
ui_->label_actionsB,
ui_->label_parentsB,
ui_->label_childrenB,
ui_->graphicsView_B,
ui_->label_idB);
}
void MainWindow::update(int value,
QLabel * labelIndex,
QLabel * labelActions,
QLabel * labelParents,
QLabel * labelChildren,
QGraphicsView * view,
QLabel * labelId)
{
UTimer timer;
labelIndex->setText(QString::number(value));
labelActions->clear();
labelParents->clear();
labelChildren->clear();
if(value >= 0 && value < ids_.size())
{
view->scene()->clear();
int id = ids_.at(value);
labelId->setText(QString::number(id));
if(id>0)
{
//image
QImage img;
QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
if(iter == imagesMap_.end())
{
if(memory_)
{
std::list<rtabmap::Sensor> sensors = memory_->getRawData(id);
if(sensors.size())
{
std::list<rtabmap::Sensor>::const_iterator jter = sensors.begin();
for(; jter!=sensors.end(); ++jter)
{
if(jter->type() == rtabmap::Sensor::kTypeImage)
{
break; //Stop to first
}
}
if(jter != sensors.end())
{
IplImage iplImg = jter->data();
img = ipl2QImage(&iplImg);
if(!img.isNull())
{
QByteArray ba;
QBuffer buffer(&ba);
buffer.open(QIODevice::WriteOnly);
img.save(&buffer, "BMP"); // writes image into ba in BMP format
imagesMap_.insert(id, ba);
}
}
}
}
}
else
{
img.loadFromData(iter.value(), "BMP");
}
if(memory_ && dynamic_cast<rtabmap::KeypointMemory*>(memory_))
{
std::multimap<int, cv::KeyPoint> words = dynamic_cast<rtabmap::KeypointMemory*>(memory_)->getWords(id);
if(words.size())
{
drawKeypoints(words, view->scene());
}
}
if(!img.isNull())
{
view->scene()->addPixmap(QPixmap::fromImage(img));
}
else
{
ULOGGER_DEBUG("Image is empty");
}
// actions
if(id-1 > 0)
{
std::list<rtabmap::NeighborLink> links = memory_->getNeighborLinks(id-1, true, true);
for(std::list<rtabmap::NeighborLink>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->toId()>id-1 && iter->actuators().size())
{
QString str;
const std::list<rtabmap::Actuator> & actuators = iter->actuators();
unsigned int j=0;
for(std::list<rtabmap::Actuator>::const_iterator jter=actuators.begin(); jter!=actuators.end(); ++jter)
{
if(jter->data().elemSize() == 1)
{
for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
{
str.append(QString("%1 ").arg(*(char*)(jter->data().data + i)));
}
}
else if(jter->data().elemSize() == 2)
{
for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
{
str.append(QString("%1 ").arg(*(short*)(jter->data().data + i)));
}
}
else if(jter->data().elemSize() == 4)
{
for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
{
if(jter->data().type() & CV_32F)
{
str.append(QString("%1 ").arg(*(float*)(jter->data().data + i)));
}
else
{
str.append(QString("%1 ").arg(*(int*)(jter->data().data + i)));
}
}
}
else
{
UERROR("not handled element size %d", jter->data().elemSize());
break;
}
if(j+1 < actuators.size())
{
str.append(QString("\n"));
}
++j;
}
if(str.size())
{
labelActions->setText(str);
}
break;
}
}
}
// loops
std::set<int> parents;
std::set<int> children;
memory_->getLoopClosureIds(id, parents, children, true);
if(parents.size())
{
QString str;
for(std::set<int>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
{
str.append(QString("%1 ").arg(*iter));
}
labelParents->setText(str);
}
if(children.size())
{
QString str;
for(std::set<int>::iterator iter=children.begin(); iter!=children.end(); ++iter)
{
str.append(QString("%1 ").arg(*iter));
}
labelChildren->setText(str);
}
}
labelId->setText(QString::number(id));
view->fitInView(view->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
}
else
{
ULOGGER_ERROR("Slider index out of range ?");
}
UINFO("Time = %fs", timer.ticks());
}
void MainWindow::sliderAMoved(int value)
{
ui_->label_indexA->setText(QString::number(value));
if(value>=0 && value < ids_.size())
{
ui_->label_idA->setText(QString::number(ids_.at(value)));
}
else
{
ULOGGER_ERROR("Slider index out of range ?");
}
}
void MainWindow::sliderBMoved(int value)
{
ui_->label_indexB->setText(QString::number(value));
if(value>=0 && value < ids_.size())
{
ui_->label_idB->setText(QString::number(ids_.at(value)));
}
else
{
ULOGGER_ERROR("Slider index out of range ?");
}
}
QImage MainWindow::ipl2QImage(const IplImage *newImage)
{
QImage qtemp;
if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
{
int x;
int y;
char* data = newImage->imageData;
qtemp= QImage(newImage->width, newImage->height,QImage::Format_RGB32 );
for( y = 0; y < newImage->height; y++, data +=newImage->widthStep )
{
for( x = 0; x < newImage->width; x++)
{
uint *p = (uint*)qtemp.scanLine (y) + x;
*p = qRgb(data[x * newImage->nChannels+2], data[x * newImage->nChannels+1],data[x * newImage->nChannels]);
}
}
}
else
{
ULOGGER_ERROR("Wrong IplImage format");
}
return qtemp;
}

81
tools/DatabaseViewer/MainWindow.h
View File

@@ -1,81 +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 MAINWINDOW_H_
#define MAINWINDOW_H_
#include <QtGui/QMainWindow>
#include <QtCore/QByteArray>
#include <QtCore/QMap>
#include <QtCore/QSet>
#include <QtGui/QImage>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <set>
class Ui_MainWindow;
class QGraphicsScene;
class QGraphicsView;
class QLabel;
namespace rtabmap
{
class Memory;
}
class MainWindow : public QMainWindow
{
Q_OBJECT
public:
MainWindow(QWidget * parent = 0);
virtual ~MainWindow();
private slots:
void openDatabase();
void cleanDatabase();
void cleanLocalGraph();
void generateGraph();
void generateLocalGraph();
void sliderAValueChanged(int);
void sliderBValueChanged(int);
void sliderAMoved(int);
void sliderBMoved(int);
private:
void updateIds();
QImage ipl2QImage(const IplImage *newImage);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene);
void update(int value,
QLabel * labelIndex,
QLabel * labelActions,
QLabel * labelParents,
QLabel * labelChildren,
QGraphicsView * view,
QLabel * labelId);
private:
Ui_MainWindow * ui_;
QMap<int, QByteArray> imagesMap_;
QList<int> ids_;
rtabmap::Memory * memory_;
QString pathDatabase_;
};
#endif /* MAINWINDOW_H_ */

4
tools/DatabaseViewer/main.cpp
View File

@@ -18,7 +18,7 @@
*/
#include <QtGui/QApplication>
#include "MainWindow.h"
#include "rtabmap/gui/DatabaseViewer.h"
#include "utilite/ULogger.h"
int main(int argc, char * argv[])
@@ -27,7 +27,7 @@ int main(int argc, char * argv[])
ULogger::setLevel(ULogger::kInfo);
QApplication * app = new QApplication(argc, argv);
MainWindow * mainWindow = new MainWindow();
DatabaseViewer * mainWindow = new DatabaseViewer();
mainWindow->showNormal();
// Now wait for application to finish

324
tools/DatabaseViewer/ui/MainWindow.ui
View File

@@ -1,324 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>MainWindow</class>
<widget class="QMainWindow" name="MainWindow">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>720</width>
<height>471</height>
</rect>
</property>
<property name="windowTitle">
<string>Database viewer</string>
</property>
<widget class="QWidget" name="centralwidget">
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<layout class="QVBoxLayout" name="verticalLayout_6" stretch="1,0,0">
<item>
<widget class="QGraphicsView" name="graphicsView_A"/>
</item>
<item>
<widget class="QScrollArea" name="scrollArea_2">
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAsNeeded</enum>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents_2">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>344</width>
<height>81</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="0" column="0">
<widget class="QLabel" name="label_actionsA_2">
<property name="text">
<string>Actions</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_actionsA">
<property name="text">
<string>Actions</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_parentsA_2">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_parentsA">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_2">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_childrenA">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<layout class="QVBoxLayout" name="verticalLayout_4">
<item>
<widget class="QLabel" name="label_2">
<property name="text">
<string>Index :</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>Id :</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<widget class="QLabel" name="label_indexA">
<property name="text">
<string>indexA</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_idA">
<property name="text">
<string>idA</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_A">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_5" stretch="1,0,0">
<item>
<widget class="QGraphicsView" name="graphicsView_B"/>
</item>
<item>
<widget class="QScrollArea" name="scrollArea">
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAsNeeded</enum>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>344</width>
<height>81</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
<item row="0" column="0">
<widget class="QLabel" name="label_actionsA_4">
<property name="text">
<string>Actions</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_actionsB">
<property name="text">
<string>Actions</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_parentsA_4">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_parentsB">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_3">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_childrenB">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="label_5">
<property name="text">
<string>Index :</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_4">
<property name="text">
<string>Id :</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLabel" name="label_indexB">
<property name="text">
<string>indexB</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_idB">
<property name="text">
<string>idB</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_B">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</widget>
<widget class="QMenuBar" name="menubar">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>720</width>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
<property name="title">
<string>File</string>
</property>
<addaction name="actionOpen_database"/>
<addaction name="separator"/>
<addaction name="actionQuit"/>
</widget>
<widget class="QMenu" name="menuEdit">
<property name="title">
<string>Edit</string>
</property>
<addaction name="actionGenerate_graph_dot"/>
<addaction name="actionGenerate_local_graph_dot"/>
<addaction name="actionClean_database"/>
<addaction name="actionClean_local_graph"/>
</widget>
<addaction name="menuFile"/>
<addaction name="menuEdit"/>
</widget>
<widget class="QStatusBar" name="statusbar"/>
<action name="actionOpen_database">
<property name="text">
<string>Open database</string>
</property>
</action>
<action name="actionQuit">
<property name="text">
<string>Quit</string>
</property>
</action>
<action name="actionGenerate_graph_dot">
<property name="text">
<string>Generate graph (.dot) ...</string>
</property>
</action>
<action name="actionGenerate_graph_only_weighted_locations">
<property name="text">
<string>Generate graph (only weighted locations) ...</string>
</property>
</action>
<action name="actionClean_database">
<property name="text">
<string>Clean database</string>
</property>
</action>
<action name="actionGenerate_local_graph_dot">
<property name="text">
<string>Generate local graph (.dot) ...</string>
</property>
</action>
<action name="actionClean_local_graph">
<property name="text">
<string>Clean local graph ...</string>
</property>
</action>
</widget>
<resources/>
<connections/>
</ui>

13
tools/EpipolarGeometry/main.cpp
View File

@@ -10,10 +10,9 @@
#include <utilite/UStl.h>
#include <utilite/UMath.h>
#include <opencv2/calib3d/calib3d.hpp>
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/KeypointDescriptor.h"
#include "rtabmap/core/KeypointDetector.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/gui/ImageView.h"
#include "rtabmap/gui/qtipl.h"
@@ -200,13 +199,13 @@ int main(int argc, char** argv)
// Find pairs
timer.start();
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
findPairsUnique(words1, words2, pairs);
EpipolarGeometry::findPairsUnique(words1, words2, pairs);
UINFO("find pairs = %d ms", timer.elapsed());
// Find fundamental matrix
timer.start();
std::vector<uchar> status;
cv::Mat fundamentalMatrix = findFFromWords(pairs, status);
cv::Mat fundamentalMatrix = EpipolarGeometry::findFFromWords(pairs, status);
UINFO("inliers = %d/%d", uSum(status), pairs.size());
UINFO("find F = %d ms", timer.elapsed());
if(!fundamentalMatrix.empty())
@@ -290,7 +289,7 @@ int main(int argc, char** argv)
std::cout<<"K=" << k << std::endl;
timer.start();
//std::cout<<"e=" << e << std::endl;
cv::Mat p = findPFromF(e, x1, x2);
cv::Mat p = EpipolarGeometry::findPFromF(e, x1, x2);
cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
p0.at<double>(0,0) = 1;
p0.at<double>(1,1) = 1;
@@ -333,7 +332,7 @@ int main(int argc, char** argv)
//Show rotation/translation of the second camera
cv::Mat r;
cv::Mat t;
findRTFromP(p, r, t);
EpipolarGeometry::findRTFromP(p, r, t);
std::cout<< "R=" << r << std::endl;
std::cout<< "t=" << t << std::endl;

26
tools/ImagesDbExtractor/main.cpp
View File

@@ -7,7 +7,7 @@
#include <utilite/UTimer.h>
#include <utilite/UConversion.h>
#include <utilite/UDirectory.h>
#include "rtabmap/core/SMMemory.h"
#include "rtabmap/core/Memory.h"
int main(int argc, char** argv)
{
@@ -22,15 +22,10 @@ int main(int argc, char** argv)
// Open database
std::string driverType = "sqlite3";
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
rtabmap::SMMemory * memory = new rtabmap::SMMemory(parameters);
if(!memory)
{
UWARN("Can't create database driver \"%s\"", driverType.c_str());
}
else if(!memory->init(driverType, path))
rtabmap::Memory * memory = new rtabmap::Memory(parameters);
if(!memory->init(path))
{
UWARN("Can't open database \"%s\"", path.c_str());
}
@@ -46,17 +41,10 @@ int main(int argc, char** argv)
std::set<int> ids = memory->getAllSignatureIds();
for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
std::list<rtabmap::Sensor> sensors = memory->getRawData(*iter);
for(std::list<rtabmap::Sensor>::const_iterator jter=sensors.begin(); jter!=sensors.end(); ++jter)
{
int k=0;
if(jter->type() == rtabmap::Sensor::kTypeImage)
{
std::string fileName = uFormat("%d-%d.png", *iter, k++);
cv::imwrite(saveDirectory+fileName, jter->data());
UINFO("Saved %s", (saveDirectory+fileName).c_str());
}
}
cv::Mat image = memory->getImage(*iter);
std::string fileName = uFormat("%d.png", *iter);
cv::imwrite(saveDirectory+fileName, image);
UINFO("Saved %s", (saveDirectory+fileName).c_str());
}
}

144
tools/Polar/main.cpp
View File

@@ -1,144 +0,0 @@
#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/imgproc_c.h>
#include <iostream>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <fftw3.h>
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/ColorTable.h"
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kDebug);
cv::Mat srcMat;
if( argc == 2)
{
srcMat=cvLoadImage(argv[1],1);
}
else
{
rtabmap::CameraVideo cam(0,0,false, 640, 480);
if(cam.init())
{
srcMat = cam.takeImage();
}
}
if(!srcMat.empty())
{
UTimer timer;
timer.start();
IplImage srfRef = srcMat;
IplImage * src = &srfRef;
// Log-polar transform
int radius = src->height < src->width ? src->height/2: src->width/2;
CvSize polarSize = cvSize(64, 128);
float M = polarSize.width/std::log(radius);
UDEBUG("src size=(%d,%d) radius=%d, M=%f", src->width, src->height, radius, M);
IplImage* polar = cvCreateImage( polarSize, 8, 3 );
IplImage* src2 = cvCreateImage( cvGetSize(src), 8, 3 );
cvLogPolar( src, polar, cvPoint2D32f(src->width/2,src->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
cvLogPolar( polar, src2, cvPoint2D32f(src->width/2,src->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
UDEBUG("logpolar time=%fs", timer.ticks());
// HSV transform
IplImage* hsv = cvCreateImage( cvGetSize(polar), IPL_DEPTH_8U, 3 );
cvCvtColor( polar, hsv, CV_BGR2HSV );
UDEBUG("bgr->hsv time=%fs", timer.ticks());
// Fetch H channel
cv::Mat hsvMat(hsv);
cv::vector<cv::Mat> channels;
split(hsvMat, channels);
cv::Mat hMat;
channels[0].convertTo(hMat, CV_32F);
hMat /= 255.0f;
UDEBUG("fetch h channel time=%fs", timer.ticks());
// DFT transform
cv::Mat dftMat;
cv::dft(hMat, dftMat, cv::DFT_ROWS);
UDEBUG("dft opencv time=%fs", timer.ticks());
// FFT transform
float in[hMat.cols];
fftwf_complex * out;
fftwf_plan p;
cv::Mat fftMat(hMat.rows, hMat.cols/2+1, CV_32F);
out = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * fftMat.cols);
p = fftwf_plan_dft_r2c_1d(hMat.cols, in, out, 0);
UDEBUG("fft create plan time=%fs", timer.ticks());
if(p==0)
{
UFATAL("cannot create a plan");
}
for(int i=0; i<hMat.rows; ++i)
{
cv::Mat hRow = hMat(cv::Range(i,i+1), cv::Range(0,hMat.cols));
memcpy(in, hRow.data, sizeof(float)*hRow.cols);
fftwf_execute(p); /* repeat as needed */
for(int j=0; j<fftMat.cols; ++j)
{
cv::Mat fftRow = fftMat(cv::Range(i,i+1), cv::Range(0,fftMat.cols));
float re = (float)out[j][0];
float im = (float)out[j][1];
fftRow.at<float>(0,j) = sqrt(re*re+im*im); // TODO keep only real of the complex instead of the module ?
}
}
UDEBUG("fft time=%fs", timer.ticks());
fftwf_destroy_plan(p);
fftwf_free(out);
UDEBUG("fft cleanup time=%fs", timer.ticks());
//std::cout << dftMat << std::endl;
//std::cout << fftMat << std::endl;
//UDEBUG("hMat row=%d cols=%d", hMat.rows, hMat.cols);
//UDEBUG("hMat row=%d cols=%d, channels=%d", dftMat.rows, dftMat.cols, dftMat.channels());
IplImage * ind = cvCloneImage(src);
unsigned char * imageData = (unsigned char *)ind->imageData;
rtabmap::ColorTable colorTable(65536);
UDEBUG("widthStep=%d", ind->widthStep);
for(int i=0; i<ind->height; ++i)
{
for(int j=0; j<ind->width; ++j)
{
unsigned char & b = imageData[i*ind->widthStep+j*3+0];
unsigned char & g = imageData[i*ind->widthStep+j*3+1];
unsigned char & r = imageData[i*ind->widthStep+j*3+2];
int index = (int)colorTable.getIndex(r, g, b);
colorTable.getRgb(index, r, g , b);
}
}
cvNamedWindow( "log-original", 1 );
cvShowImage( "log-original", src );
cvNamedWindow( "log-polar", 1 );
cvShowImage( "log-polar", polar );
cvNamedWindow( "inverse log-polar", 1 );
cvShowImage( "inverse log-polar", src2 );
cvNamedWindow( "hsv", 1 );
cvShowImage( "hsv", hsv );
cvNamedWindow( "ind", 1 );
cvShowImage( "ind", ind );
UDEBUG("show time=%fs", timer.ticks());
cvWaitKey();
cvReleaseImage(&polar);
cvReleaseImage(&src2);
cvReleaseImage(&hsv);
cvReleaseImage(&ind);
}
return 0;
}

12
tools/Polar/CMakeLists.txt → tools/VocabularyComparison/CMakeLists.txt
View File

@@ -3,24 +3,22 @@ SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${FFTW3F_INCLUDE_DIRS}
)
SET(LIBRARIES
${UTILITE_LIBRARIES}
${OpenCV_LIBRARIES}
${FFTW3F_LIBRARIES}
)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(logPolar main.cpp)
TARGET_LINK_LIBRARIES(logPolar rtabmap_corelib ${LIBRARIES})
ADD_EXECUTABLE(vocabularyComparison main.cpp)
TARGET_LINK_LIBRARIES(vocabularyComparison rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES( logPolar
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-logPolar)
SET_TARGET_PROPERTIES( vocabularyComparison
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-vocabularyComparison)
INSTALL(TARGETS logPolar
INSTALL(TARGETS vocabularyComparison
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)

262
tools/VocabularyComparison/main.cpp Normal file
View File

@@ -0,0 +1,262 @@
#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/flann/miniflann.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <utilite/UConversion.h>
#include <utilite/UStl.h>
#include <utilite/UMath.h>
#include <fstream>
#include <vector>
#include <list>
#include <string>
#include <iostream>
void showUsage()
{
printf("Usage:\n"
"vocabularyComparison.exe \"dictionary/path\"\n"
" Dictionary path example: \"data/Dictionary49k.txt\""
" Note that 400 first descriptors in the file are used as queries.\n");
exit(1);
}
int main(int argc, char * argv[])
{
if(argc < 2)
{
showUsage();
}
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kDebug);
std::string dictionaryPath = argv[argc-1];
std::list<std::vector<float> > objectDescriptors;
//std::list<std::vector<float> > descriptors;
std::map<int, std::vector<float> > descriptors;
unsigned int dimension = 0;
UTimer timer;
int objectDescriptorsSize= 400;
std::ifstream file;
if(!dictionaryPath.empty())
{
file.open(dictionaryPath.c_str(), std::ifstream::in);
}
if(file.good())
{
UDEBUG("Loading the dictionary from \"%s\"", dictionaryPath.c_str());
// first line is the header
std::string str;
std::list<std::string> strList;
std::getline(file, str);
strList = uSplitNumChar(str);
for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
{
if(uIsDigit(iter->at(0)))
{
dimension = std::atoi(iter->c_str());
break;
}
}
if(dimension == 0 || dimension > 1000)
{
UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
}
else
{
int descriptorsLoaded = 0;
// Process all words
while(file.good())
{
std::getline(file, str);
strList = uSplit(str);
if(strList.size() == dimension+1)
{
//first one is the visual word id
std::list<std::string>::iterator iter = strList.begin();
int id = atoi(iter->c_str());
++iter;
std::vector<float> descriptor(dimension);
unsigned int i=0;
//get descriptor
for(;i<dimension && iter != strList.end(); ++i, ++iter)
{
descriptor[i] = std::atof(iter->c_str());
}
if(i != dimension)
{
UERROR("");
}
if(++descriptorsLoaded<=objectDescriptorsSize)
{
objectDescriptors.push_back(descriptor);
}
else
{
//descriptors.push_back(descriptor);
descriptors.insert(std::make_pair(id, descriptor));
}
}
else if(str.size())
{
UWARN("Cannot parse line \"%s\"", str.c_str());
}
}
}
UDEBUG("Time loading dictionary = %fs, dimension=%d", timer.ticks(), dimension);
}
else
{
UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
}
file.close();
if(descriptors.size() && objectDescriptors.size() && dimension)
{
cv::Mat dataTree;
cv::Mat queries;
UDEBUG("Creating data structures...");
// Create the data structure
dataTree = cv::Mat(descriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
{//scope
//std::list<std::vector<float> >::const_iterator iter = descriptors.begin();
std::map<int, std::vector<float> >::const_iterator iter = descriptors.begin();
for(unsigned int i=0; i < descriptors.size(); ++i, ++iter)
{
UTimer tim;
//memcpy(dataTree.ptr<float>(i), iter->data(), dimension*sizeof(float));
memcpy(dataTree.ptr<float>(i), iter->second.data(), dimension*sizeof(float));
//if(i%100==0)
// UDEBUG("i=%d/%d tim=%fs", i, descriptors.size(), tim.ticks());
}
}
queries = cv::Mat(objectDescriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
{//scope
std::list<std::vector<float> >::const_iterator iter = objectDescriptors.begin();
for(unsigned int i=0; i < objectDescriptors.size(); ++i, ++iter)
{
UTimer tim;
memcpy(queries.ptr<float>(i), iter->data(), dimension*sizeof(float));
//if(i%100==0)
// UDEBUG("i=%d/%d tim=%fs", i, objectDescriptors.size(), tim.ticks());
}
}
UDEBUG("descriptors.size()=%d, objectDescriptorsSize=%d, copying data = %f s",descriptors.size(), objectDescriptors.size(), timer.ticks());
UDEBUG("Creating indexes...");
cv::flann::Index * linearIndex = new cv::flann::Index(dataTree, cv::flann::LinearIndexParams());
UDEBUG("Time to create linearIndex = %f s", timer.ticks());
cv::flann::Index * kdTreeIndex1 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(1));
UDEBUG("Time to create kdTreeIndex1 = %f s", timer.ticks());
cv::flann::Index * kdTreeIndex4 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(4));
UDEBUG("Time to create kdTreeIndex4 = %f s", timer.ticks());
cv::flann::Index * kMeansIndex = new cv::flann::Index(dataTree, cv::flann::KMeansIndexParams());
UDEBUG("Time to create kMeansIndex = %f s", timer.ticks());
cv::flann::Index * compositeIndex = new cv::flann::Index(dataTree, cv::flann::CompositeIndexParams());
UDEBUG("Time to create compositeIndex = %f s", timer.ticks());
//cv::flann::Index * autoTunedIndex = new cv::flann::Index(dataTree, cv::flann::AutotunedIndexParams());
//UDEBUG("Time to create autoTunedIndex = %f s", timer.ticks());
UDEBUG("Search indexes...");
int k=2; // 2 nearest neighbors
cv::Mat results(queries.rows, k, CV_32SC1); // results index
cv::Mat dists(queries.rows, k, CV_32FC1); // Distance results are CV_32FC1
linearIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
cv::Mat transposedLinear = dists.t();
UDEBUG("Time to search linearIndex = %f s", timer.ticks());
kdTreeIndex1->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
cv::Mat transposed = dists.t();
UDEBUG("Time to search kdTreeIndex1 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
kdTreeIndex4->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search kdTreeIndex4 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
kMeansIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search kMeansIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
compositeIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search compositeIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
//autoTunedIndex->knnSearch(queries, results, dists, k);
//UDEBUG("Time to search autoTunedIndex = %f s", timer.ticks());
delete linearIndex;
delete kdTreeIndex1;
delete kdTreeIndex4;
delete kMeansIndex;
delete compositeIndex;
//delete autoTunedIndex;
}
return 0;
}

34
tools/WebcamCapture/CMakeLists.txt
View File

@@ -1,34 +0,0 @@
SET(SRC_FILES
main.cpp
)
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${CMAKE_CURRENT_SOURCE_DIR}
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/../include
)
SET(LIBRARIES
${UTILITE_LIBRARIES}
${OpenCV_LIBS}
)
# Make sure the compiler can find include files from our library.
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
# Add binary called "imagesJoiner" that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_EXECUTABLE(webcamCapture ${SRC_FILES})
TARGET_LINK_LIBRARIES(webcamCapture rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES( webcamCapture
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-webcamCapture)
INSTALL(TARGETS webcamCapture
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)

251
tools/WebcamCapture/main.cpp
View File

@@ -1,251 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "utilite/ULogger.h"
#include "utilite/UTimer.h"
#include "utilite/UDirectory.h"
#include "utilite/UConversion.h"
#include "rtabmap/core/Camera.h"
#include "utilite/UEventsManager.h"
#include "utilite/UEventsHandler.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
void showUsage()
{
printf("Usage:\n"
"webcamCapture [option]\n"
" -device #: Id of the webcam (default 0)\n"
" -width #: Image width (default 640)\n"
" -height #: Image height (default 480)\n"
" -fps #.#: Frame rate (Hz) (default 2.0)\n"
" -hide : Image not shown while capturing (default true)\n"
" -dir \"path\": Path of the images saved (default \"./imagesCaptured\")\n"
" -save : Save images captured (default true)\n"
" -startId #: Image id to start with (default 1)\n"
" -ext \"ext\": Image extension (default \"jpg\")\n"
" -debug: Debug trace\n");
exit(1);
}
class ImagesHandler : public UEventsHandler
{
public:
ImagesHandler(const std::string & targetDir, const std::string & ext, int startId, bool show, bool save) :
targetDir_(targetDir),
ext_(ext),
startId_(startId),
show_(show),
save_(save),
id_(startId)
{
if(show_)
{
cv::namedWindow("Webcam", CV_WINDOW_AUTOSIZE);
}
}
virtual ~ImagesHandler()
{
if(show_)
{
cv::destroyWindow("Webcam");
}
}
protected:
virtual void handleEvent(UEvent * e)
{
if(e->getClassName().compare("SMStateEvent") == 0)
{
const rtabmap::CameraEvent * event = (const rtabmap::CameraEvent*)e;
cv::Mat image = event->image();
if(!image.empty())
{
if(show_)
{
cv::imshow("Webcam", image);
}
if(save_)
{
std::string fileName = targetDir_ + "/";
fileName += uNumber2Str(id_++);
fileName += ".";
fileName += ext_;
cv::imwrite(fileName, image);
printf("Image %s saved!\n", fileName.c_str());
}
}
else
{
printf("Image is null?!?\n");
}
}
}
private:
std::string targetDir_;
std::string ext_;
int startId_;
bool show_;
bool save_;
int id_;
};
int main(int argc, char * argv[])
{
bool show = true;
int usbDevice = 0;
int imageWidth = 640;
int imageHeight = 480;
float imageRate = 2.0;
int startId = 1;
std::string extension = "jpg";
bool save = false;
std::string targetDirectory = UDirectory::currentDir(true) + "imagesCaptured";
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "-h") == 0 ||
strcmp(argv[i], "-help") == 0 ||
strcmp(argv[i], "?") == 0)
{
showUsage();
}
if(strcmp(argv[i], "-device") == 0 && i+1<argc)
{
usbDevice = std::atoi(argv[i+1]);
if(usbDevice < 0 || usbDevice > 100)
{
showUsage();
}
++i;
}
if(strcmp(argv[i], "-width") == 0 && i+1<argc)
{
imageWidth = std::atoi(argv[i+1]);
if(imageWidth < 0)
{
showUsage();
}
++i;
}
if(strcmp(argv[i], "-height") == 0 && i+1<argc)
{
imageHeight = std::atoi(argv[i+1]);
if(imageHeight < 0)
{
showUsage();
}
++i;
}
if(strcmp(argv[i], "-hz") == 0 && i+1<argc)
{
imageRate = std::atof(argv[i+1]);
if(imageRate < 0)
{
showUsage();
}
++i;
}
if(strcmp(argv[i], "-hide") == 0 && i+1<argc)
{
show = false;
}
if(strcmp(argv[i], "-dir") == 0 && i+1<argc)
{
targetDirectory = argv[i+1];
++i;
}
if(strcmp(argv[i], "-save") == 0 && i+1<argc)
{
save = true;
}
if(strcmp(argv[i], "-debug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
ULogger::setType(ULogger::kTypeConsole);
}
if(strcmp(argv[i], "-startId") == 0 && i+1<argc)
{
startId = std::atoi(argv[i+1]);
++i;
}
if(strcmp(argv[i], "-ext") == 0 && i+1<argc)
{
extension = argv[i+1];
++i;
}
}
printf("Parameters:\n"
" device=%d\n"
" width=%d\n"
" height=%d\n"
" hz=%f\n"
" show=%s\n"
" dir=%s\n"
" extension=%s\n"
" startId=%d\n"
" save=%s\n",
usbDevice,
imageWidth,
imageHeight,
imageRate,
uBool2Str(show).c_str(),
targetDirectory.c_str(),
extension.c_str(),
startId,
uBool2Str(save).c_str());
UDirectory::makeDir(targetDirectory);
rtabmap::CameraVideo cam(usbDevice, imageRate, false, imageWidth, imageHeight);
if(!cam.init())
{
printf("Can't initialize the camera...\n");
return 1;
}
ImagesHandler imgHandler(targetDirectory, extension, startId, show, save);
UEventsManager::addHandler(&imgHandler);
cam.start();
if(show)
{
cv::waitKey(0);
}
else
{
std::cin.ignore();
}
return 0;
}