mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Merged Audio branch to trunk
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@560 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
+1
-20
@@ -1,20 +1 @@
|
||||
ADD_SUBDIRECTORY( src )
|
||||
ADD_SUBDIRECTORY( ConsoleApp )
|
||||
ADD_SUBDIRECTORY( ImagesJoiner )
|
||||
ADD_SUBDIRECTORY( WebcamCapture )
|
||||
ADD_SUBDIRECTORY( LogPolar )
|
||||
ADD_SUBDIRECTORY( ImagesDbExtractor )
|
||||
ADD_SUBDIRECTORY( ColorIndexesGenerator )
|
||||
|
||||
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
|
||||
ADD_SUBDIRECTORY( DatabaseViewer )
|
||||
ELSE()
|
||||
MESSAGE(STATUS "[WARNING] Qt4 not found, the databaseViewer program will not be built...")
|
||||
ENDIF()
|
||||
|
||||
IF(CPPUNIT_FOUND)
|
||||
ADD_SUBDIRECTORY( tests )
|
||||
#MESSAGE(STATUS "Tests are desactivated...")
|
||||
ELSE(CPPUNIT_FOUND)
|
||||
MESSAGE(STATUS "CppUnit is not found, tests for the ${PROJECT_NAME} project won't be compiled...")
|
||||
ENDIF(CPPUNIT_FOUND)
|
||||
ADD_SUBDIRECTORY( src )
|
||||
@@ -1,33 +0,0 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${UTILITE_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../include
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../src
|
||||
${ZLIB_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
SET(LIBRARIES
|
||||
${UTILITE_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
${ZLIB_LIBRARIES}
|
||||
)
|
||||
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
ADD_EXECUTABLE(colorIndexesGenerator ${SRC_FILES})
|
||||
TARGET_LINK_LIBRARIES(colorIndexesGenerator 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)
|
||||
|
||||
@@ -1,227 +0,0 @@
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <utilite/ULogger.h>
|
||||
#include <utilite/UTimer.h>
|
||||
#include "ColorTable.h"
|
||||
#include "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);
|
||||
}
|
||||
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;
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${UTILITE_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../include
|
||||
)
|
||||
|
||||
SET(LIBRARIES
|
||||
${UTILITE_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
)
|
||||
|
||||
# Make sure the compiler can find include files from our library.
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
# Add binary called "consoleApp" that is built from the source file "main.cpp".
|
||||
# The extension is automatically found.
|
||||
ADD_EXECUTABLE(consoleApp ${SRC_FILES})
|
||||
TARGET_LINK_LIBRARIES(consoleApp corelib ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( consoleApp
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-console)
|
||||
|
||||
INSTALL(TARGETS consoleApp
|
||||
RUNTIME DESTINATION bin COMPONENT runtime
|
||||
LIBRARY DESTINATION lib COMPONENT devel
|
||||
ARCHIVE DESTINATION lib COMPONENT devel)
|
||||
|
||||
@@ -1,577 +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 "rtabmap/core/Rtabmap.h"
|
||||
#include "rtabmap/core/Camera.h"
|
||||
#include "rtabmap/core/SMState.h"
|
||||
#include <utilite/UDirectory.h>
|
||||
#include <utilite/UFile.h>
|
||||
#include <utilite/UConversion.h>
|
||||
#include <utilite/UStl.h>
|
||||
#include <fstream>
|
||||
#include <queue>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <signal.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
#define GENERATED_GT_NAME "GroundTruth_generated.bmp"
|
||||
|
||||
void showUsage()
|
||||
{
|
||||
printf("\nUsage:\n"
|
||||
"rtabmap-console [options] \"path\"\n"
|
||||
" path For images, use the directory path. For videos or databases, use full\n "
|
||||
" path name\n"
|
||||
"Options:\n"
|
||||
" -t #.## Time threshold (ms)\n"
|
||||
" -rate #.## Acquisition time (seconds)\n"
|
||||
" -rateHz #.## Acquisition rate (Hz), for convenience\n"
|
||||
" -repeat # Repeat the process on the data set # times (minimum of 1)\n"
|
||||
" -createGT Generate a ground truth file\n"
|
||||
" -image_width # Force an image width (Default 0: original size used).\n"
|
||||
" The height must be also specified if changed.\n"
|
||||
" -image_height # Force an image height (Default 0: original size used)\n"
|
||||
" The height must be also specified if changed.\n"
|
||||
" -start_at # When \"path\" is a directory of images, set this parameter "
|
||||
" to start processing at image # (default 1)."
|
||||
" -\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n"
|
||||
" -SURF/HessianThreshold 150\n"
|
||||
" For parameters in table format, add ',' between values :\n"
|
||||
" -Kp/RoiRatios 0,0,0.1,0\n"
|
||||
" Default parameters can be found in ~/.rtabmap/rtabmap.ini\n"
|
||||
" -default_params Show default RTAB-Map's parameters (WARNING : \n"
|
||||
" parameters from rtabmap.ini (if exists) overwrite the default \n"
|
||||
" ones shown here)\n"
|
||||
" -debug Set Log level to Debug (Default Error)\n"
|
||||
" -info Set Log level to Info (Default Error)\n"
|
||||
" -warn Set Log level to Warning (Default Error)\n"
|
||||
" -exit_warn Set exit level to Warning (Default Fatal)\n"
|
||||
" -exit_error Set exit level to Error (Default Fatal)\n"
|
||||
" -v Get version of RTAB-Map\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// catch ctrl-c
|
||||
bool g_forever = true;
|
||||
void sighandler(int sig)
|
||||
{
|
||||
printf("\nSignal %d caught...\n", sig);
|
||||
g_forever = false;
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
signal(SIGABRT, &sighandler);
|
||||
signal(SIGTERM, &sighandler);
|
||||
signal(SIGINT, &sighandler);
|
||||
|
||||
/*for(int i=0; i<argc; i++)
|
||||
{
|
||||
printf("argv[%d] = %s\n", i, argv[i]);
|
||||
}*/
|
||||
const ParametersMap & defaultParameters = Parameters::getDefaultParameters();
|
||||
if(argc < 2)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
else if(argc == 2 && strcmp(argv[1], "-v") == 0)
|
||||
{
|
||||
printf("%s\n", Rtabmap::getVersion().c_str());
|
||||
exit(0);
|
||||
}
|
||||
else if(argc == 2 && strcmp(argv[1], "-default_params") == 0)
|
||||
{
|
||||
for(ParametersMap::const_iterator iter = defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
|
||||
{
|
||||
printf("%s=%s\n", iter->first.c_str(), iter->second.c_str());
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
std::string path;
|
||||
float timeThreshold = 0.0;
|
||||
float rate = 0.0;
|
||||
int loopDataset = 0;
|
||||
int repeat = 0;
|
||||
bool createGT = false;
|
||||
int imageWidth = 0;
|
||||
int imageHeight = 0;
|
||||
int startAt = 1;
|
||||
ParametersMap pm;
|
||||
ULogger::Level logLevel = ULogger::kError;
|
||||
ULogger::Level exitLevel = ULogger::kFatal;
|
||||
|
||||
for(int i=1; i<argc; ++i)
|
||||
{
|
||||
if(i == argc-1)
|
||||
{
|
||||
// The last must be the path
|
||||
path = argv[i];
|
||||
if(!UDirectory::exists(path.c_str()) && !UFile::exists(path.c_str()))
|
||||
{
|
||||
printf("Path not valid : %s\n", path.c_str());
|
||||
showUsage();
|
||||
exit(1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
if(strcmp(argv[i], "-t") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
timeThreshold = std::atof(argv[i]);
|
||||
if(timeThreshold < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-rate") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
rate = std::atof(argv[i]);
|
||||
if(rate < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-rateHz") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
rate = std::atof(argv[i]);
|
||||
if(rate < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
else if(rate)
|
||||
{
|
||||
rate = 1/rate;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-repeat") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
repeat = std::atoi(argv[i]);
|
||||
if(repeat < 1)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-image_width") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
imageWidth = std::atoi(argv[i]);
|
||||
if(imageWidth < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-image_height") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
imageHeight = std::atoi(argv[i]);
|
||||
if(imageHeight < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-start_at") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
startAt = std::atoi(argv[i]);
|
||||
if(startAt < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-createGT") == 0)
|
||||
{
|
||||
createGT = true;
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-debug") == 0)
|
||||
{
|
||||
logLevel = ULogger::kDebug;
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-info") == 0)
|
||||
{
|
||||
logLevel = ULogger::kInfo;
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-warn") == 0)
|
||||
{
|
||||
logLevel = ULogger::kWarning;
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-exit_warn") == 0)
|
||||
{
|
||||
exitLevel = ULogger::kWarning;
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-exit_error") == 0)
|
||||
{
|
||||
exitLevel = ULogger::kError;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check for RTAB-Map's parameters
|
||||
std::string key = argv[i];
|
||||
key = uSplit(key, '-').back();
|
||||
if(defaultParameters.find(key) != defaultParameters.end())
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
std::string value = argv[i];
|
||||
if(value.empty())
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
else
|
||||
{
|
||||
value = uReplaceChar(value, ',', ' ');
|
||||
}
|
||||
std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
|
||||
if(inserted.second == false)
|
||||
{
|
||||
inserted.first->second = value;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
printf("Unrecognized option : %s\n", argv[i]);
|
||||
showUsage();
|
||||
}
|
||||
|
||||
if(repeat && createGT)
|
||||
{
|
||||
printf("Cannot create a Ground truth if repeat is on.\n");
|
||||
showUsage();
|
||||
}
|
||||
else if((imageWidth && imageHeight == 0) ||
|
||||
(imageHeight && imageWidth == 0))
|
||||
{
|
||||
printf("If imageWidth is set, imageHeight must be too.\n");
|
||||
showUsage();
|
||||
}
|
||||
|
||||
UTimer timer;
|
||||
timer.start();
|
||||
std::queue<double> iterationMeanTime;
|
||||
|
||||
Camera * camera = 0;
|
||||
if(UDirectory::exists(path))
|
||||
{
|
||||
camera = new CameraImages(path, startAt, false, 0.0f, false, imageWidth, imageHeight);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::list<std::string> list = uSplit(path, '.');
|
||||
if(list.size() == 2 && list.back().compare("db") == 0)
|
||||
{
|
||||
camera = new CameraDatabase(path, true, 0.0f, false, imageWidth, imageHeight);
|
||||
}
|
||||
else
|
||||
{
|
||||
camera = new CameraVideo(path, 0.0f, false, imageWidth, imageHeight);
|
||||
}
|
||||
}
|
||||
|
||||
if(!camera || !camera->init())
|
||||
{
|
||||
printf("Camera init failed, using path \"%s\"\n", path.c_str());
|
||||
exit(1);
|
||||
}
|
||||
|
||||
std::map<int, int> groundTruth;
|
||||
|
||||
// Create tasks
|
||||
Rtabmap * rtabmap = new Rtabmap();
|
||||
rtabmap->init();
|
||||
rtabmap->setMaxTimeAllowed(timeThreshold); // in ms
|
||||
|
||||
//ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setType(ULogger::kTypeFile, rtabmap->getWorkingDir()+"/LogConsole.txt", false);
|
||||
ULogger::setBuffered(true);
|
||||
ULogger::setLevel(logLevel);
|
||||
ULogger::setExitLevel(exitLevel);
|
||||
|
||||
// Disable statistics (we don't need them)
|
||||
pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), uBool2Str(false)));
|
||||
rtabmap->init(pm);
|
||||
|
||||
printf("Avpd init time = %fs\n", timer.ticks());
|
||||
|
||||
// Start thread's task
|
||||
IplImage * image = 0;
|
||||
int loopClosureId;
|
||||
int count = 0;
|
||||
int countLoopDetected=0;
|
||||
|
||||
printf("\nParameters : \n");
|
||||
printf(" Data set : %s\n", path.c_str());
|
||||
printf(" Time threshold = %1.2f ms\n", timeThreshold);
|
||||
printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
|
||||
printf(" Repeating data set = %s\n", repeat?"true":"false");
|
||||
printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
|
||||
printf(" Camera starts at image %d (default 1)\n", startAt);
|
||||
if(createGT)
|
||||
{
|
||||
printf(" Creating the ground truth matrix.\n");
|
||||
}
|
||||
printf(" INFO: All other parameters are taken from the INI file located in \"~/.rtabmap\"\n");
|
||||
if(pm.size()>1)
|
||||
{
|
||||
printf(" Overwritten parameters :\n");
|
||||
for(ParametersMap::iterator iter = pm.begin(); iter!=pm.end(); ++iter)
|
||||
{
|
||||
printf(" %s=%s\n",iter->first.c_str(), iter->second.c_str());
|
||||
}
|
||||
}
|
||||
if(rtabmap->getWorkingMem().size() || rtabmap->getStMem().size())
|
||||
{
|
||||
printf("[Warning] RTAB-Map database is not empty (%s)\n", (rtabmap->getWorkingDir()+Rtabmap::kDefaultDatabaseName).c_str());
|
||||
}
|
||||
printf("\nProcessing images...\n");
|
||||
|
||||
//setup camera
|
||||
ParametersMap allParam;
|
||||
Rtabmap::readParameters(rtabmap->getIniFilePath().c_str(), allParam);
|
||||
pm.insert(allParam.begin(), allParam.end());
|
||||
CamKeypointTreatment imageToSMState(pm);
|
||||
|
||||
UTimer iterationTimer;
|
||||
int imagesProcessed = 0;
|
||||
std::list<std::vector<float> > teleopActions;
|
||||
int maxTeleopActions = 0; // TEST Lip6Indoor with 190, 0->disabled
|
||||
std::list<std::vector<float> > actions;
|
||||
while(loopDataset <= repeat && g_forever)
|
||||
{
|
||||
SMState * smState = camera->takeSMState();
|
||||
int i=0;
|
||||
while(smState && g_forever)
|
||||
{
|
||||
imageToSMState.process(smState);
|
||||
++imagesProcessed;
|
||||
iterationTimer.start();
|
||||
if(i<maxTeleopActions)
|
||||
{
|
||||
// ONLY TESTING HERE if maxTeleopActions>0
|
||||
std::vector<float> v(2);
|
||||
v[0] = 2;
|
||||
v[1] = 0;
|
||||
teleopActions.push_back(v);
|
||||
smState->setActuators(teleopActions);
|
||||
smState->setImage(image);
|
||||
}
|
||||
else
|
||||
{
|
||||
smState->setActuators(actions);
|
||||
smState->setImage(image);
|
||||
}
|
||||
rtabmap->process(smState);
|
||||
loopClosureId = rtabmap->getLoopClosureId();
|
||||
actions = rtabmap->getActions();
|
||||
if(rtabmap->getLoopClosureId())
|
||||
{
|
||||
++countLoopDetected;
|
||||
}
|
||||
smState = camera->takeSMState();
|
||||
if(++count % 100 == 0)
|
||||
{
|
||||
printf(" count = %d, loop closures = %d\n", count, countLoopDetected);
|
||||
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)
|
||||
{
|
||||
if(iter != wm.begin())
|
||||
{
|
||||
printf(";");
|
||||
}
|
||||
printf("%d,%d", iter->first, iter->second);
|
||||
}
|
||||
printf("]\n");
|
||||
}
|
||||
|
||||
// Update generated ground truth matrix
|
||||
if(createGT)
|
||||
{
|
||||
if(loopClosureId > 0)
|
||||
{
|
||||
groundTruth.insert(std::make_pair(i, loopClosureId-1));
|
||||
}
|
||||
}
|
||||
|
||||
++i;
|
||||
|
||||
double iterationTime = iterationTimer.ticks();
|
||||
|
||||
ULogger::flush();
|
||||
|
||||
if(rate)
|
||||
{
|
||||
float delta = rate - iterationTime;
|
||||
if(delta > 0)
|
||||
{
|
||||
uSleep(delta*1000);
|
||||
}
|
||||
}
|
||||
if(actions.size())
|
||||
{
|
||||
if(rtabmap->getLoopClosureId())
|
||||
{
|
||||
printf(" iteration(%d) actions=%d loop(%d) time=%fs *\n", count, (int)actions.size(), rtabmap->getLoopClosureId(), iterationTime);
|
||||
}
|
||||
else if(rtabmap->getReactivatedId())
|
||||
{
|
||||
printf(" iteration(%d) actions=%d high(%d) time=%fs\n", count, (int)actions.size(), rtabmap->getReactivatedId(), iterationTime);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf(" iteration(%d) actions=%d time=%fs\n", count, (int)actions.size(), iterationTime);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(rtabmap->getLoopClosureId())
|
||||
{
|
||||
printf(" iteration(%d) loop(%d) time=%fs *\n", count, rtabmap->getLoopClosureId(), iterationTime);
|
||||
}
|
||||
else if(rtabmap->getReactivatedId())
|
||||
{
|
||||
printf(" iteration(%d) high(%d) time=%fs\n", count, rtabmap->getReactivatedId(), iterationTime);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf(" iteration(%d) time=%fs\n", count, iterationTime);
|
||||
}
|
||||
}
|
||||
|
||||
if(timeThreshold && iterationTime > timeThreshold*100.0f)
|
||||
{
|
||||
printf(" ERROR, there is problem, too much time taken... %fs", iterationTime);
|
||||
break; // there is problem, don't continue
|
||||
}
|
||||
}
|
||||
++loopDataset;
|
||||
if(loopDataset <= repeat)
|
||||
{
|
||||
camera->init();
|
||||
printf(" Beginning loop %d...\n", loopDataset);
|
||||
}
|
||||
}
|
||||
printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
|
||||
printf(" Total time = %fs\n", timer.ticks());
|
||||
|
||||
if(imagesProcessed && createGT)
|
||||
{
|
||||
cv::Mat groundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U);
|
||||
|
||||
for(std::map<int, int>::iterator iter = groundTruth.begin(); iter!=groundTruth.end(); ++iter)
|
||||
{
|
||||
groundTruthMat.at<unsigned char>(iter->first, iter->second) = 255;
|
||||
}
|
||||
|
||||
// Generate the ground truth file
|
||||
printf("Generate ground truth to file %s, size of %d\n", (rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), groundTruthMat.rows);
|
||||
IplImage img = groundTruthMat;
|
||||
cvSaveImage((rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), &img);
|
||||
printf(" Creating ground truth file = %fs\n", timer.ticks());
|
||||
}
|
||||
|
||||
if(camera)
|
||||
{
|
||||
delete camera;
|
||||
camera = 0 ;
|
||||
}
|
||||
|
||||
if(rtabmap)
|
||||
{
|
||||
delete rtabmap;
|
||||
rtabmap = 0;
|
||||
}
|
||||
|
||||
printf(" Cleanup time = %fs\n", timer.ticks());
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,58 +0,0 @@
|
||||
### 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
|
||||
../../guilib/src/KeypointItem.cpp
|
||||
${moc_srcs}
|
||||
${moc_uis}
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../include
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../src
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${UTILITE_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
|
||||
)
|
||||
|
||||
INCLUDE(${QT_USE_FILE})
|
||||
|
||||
SET(LIBRARIES
|
||||
${UTILITE_LIBRARIES}
|
||||
${QT_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
#${QWT5_LIBRARY}
|
||||
)
|
||||
|
||||
#include files
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
# Add binary called "databaseViewer" that is built from the source file "main.cpp".
|
||||
# The extension is automatically found.
|
||||
ADD_EXECUTABLE(databaseViewer WIN32 ${SRC_FILES})
|
||||
TARGET_LINK_LIBRARIES(databaseViewer corelib ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( databaseViewer
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-databaseViewer)
|
||||
|
||||
INSTALL(TARGETS databaseViewer
|
||||
RUNTIME DESTINATION bin COMPONENT runtime
|
||||
LIBRARY DESTINATION lib COMPONENT devel
|
||||
ARCHIVE DESTINATION lib COMPONENT devel)
|
||||
@@ -1,598 +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 "KeypointMemory.h"
|
||||
#include "rtabmap/core/DBDriver.h"
|
||||
#include "../../guilib/src/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()
|
||||
{
|
||||
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"));
|
||||
memory_ = new rtabmap::KeypointMemory(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_ = path;
|
||||
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())
|
||||
{
|
||||
if(memory_->getSignature(id) > 0)
|
||||
{
|
||||
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));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
QMessageBox::critical(this, tr("Error"), tr("Signature %1 not found in database.").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)
|
||||
{
|
||||
ui_->label_indexA->setText(QString::number(value));
|
||||
ui_->label_actionsA->clear();
|
||||
ui_->label_parentsA->clear();
|
||||
ui_->label_childrenA->clear();
|
||||
if(value >= 0 && value < ids_.size())
|
||||
{
|
||||
ui_->graphicsView_A->scene()->clear();
|
||||
int id = ids_.at(value);
|
||||
ui_->label_idA->setText(QString::number(id));
|
||||
if(id>0)
|
||||
{
|
||||
// image
|
||||
QImage img;
|
||||
QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
|
||||
if(iter == imagesMap_.end())
|
||||
{
|
||||
if(memory_)
|
||||
{
|
||||
IplImage * image = memory_->getImage(id);
|
||||
if(image)
|
||||
{
|
||||
img = ipl2QImage(image);
|
||||
cvReleaseImage(&image);
|
||||
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_)
|
||||
{
|
||||
std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
|
||||
if(words.size())
|
||||
{
|
||||
drawKeypoints(words, ui_->graphicsView_A->scene());
|
||||
}
|
||||
}
|
||||
|
||||
if(!img.isNull())
|
||||
{
|
||||
ui_->graphicsView_A->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->id()>id-1 && iter->actions().size())
|
||||
{
|
||||
QString str;
|
||||
const std::list<std::vector<float> > & actions = iter->actions();
|
||||
unsigned int j=0;
|
||||
for(std::list<std::vector<float> >::const_iterator jter=actions.begin(); jter!=actions.end(); ++jter)
|
||||
{
|
||||
for(unsigned int i=0; i<jter->size(); ++i)
|
||||
{
|
||||
str.append(QString("%1 ").arg(jter->at(i)));
|
||||
}
|
||||
if(j+1 < actions.size())
|
||||
{
|
||||
str.append(QString("\n"));
|
||||
}
|
||||
++j;
|
||||
}
|
||||
if(str.size())
|
||||
{
|
||||
ui_->label_actionsA->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));
|
||||
}
|
||||
ui_->label_parentsA->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));
|
||||
}
|
||||
ui_->label_childrenA->setText(str);
|
||||
}
|
||||
}
|
||||
|
||||
ui_->label_idA->setText(QString::number(id));
|
||||
ui_->graphicsView_A->fitInView(ui_->graphicsView_A->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
|
||||
}
|
||||
else
|
||||
{
|
||||
ULOGGER_ERROR("Slider index out of range ?");
|
||||
}
|
||||
}
|
||||
|
||||
void MainWindow::sliderBValueChanged(int value)
|
||||
{
|
||||
ui_->label_indexB->setText(QString::number(value));
|
||||
ui_->label_actionsB->clear();
|
||||
ui_->label_parentsB->clear();
|
||||
ui_->label_childrenB->clear();
|
||||
if(value >= 0 && value < ids_.size())
|
||||
{
|
||||
ui_->graphicsView_B->scene()->clear();
|
||||
int id = ids_.at(value);
|
||||
ui_->label_idB->setText(QString::number(id));
|
||||
if(id>0)
|
||||
{
|
||||
//image
|
||||
QImage img;
|
||||
QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
|
||||
if(iter == imagesMap_.end())
|
||||
{
|
||||
if(memory_)
|
||||
{
|
||||
IplImage * image = memory_->getImage(id);
|
||||
if(image)
|
||||
{
|
||||
img = ipl2QImage(image);
|
||||
cvReleaseImage(&image);
|
||||
|
||||
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_)
|
||||
{
|
||||
std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
|
||||
if(words.size())
|
||||
{
|
||||
drawKeypoints(words, ui_->graphicsView_B->scene());
|
||||
}
|
||||
}
|
||||
|
||||
if(!img.isNull())
|
||||
{
|
||||
ui_->graphicsView_B->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->id()>id-1 && iter->actions().size())
|
||||
{
|
||||
QString str("");
|
||||
const std::list<std::vector<float> > & actions = iter->actions();
|
||||
unsigned int j=0;
|
||||
for(std::list<std::vector<float> >::const_iterator jter=actions.begin(); jter!=actions.end(); ++jter)
|
||||
{
|
||||
for(unsigned int i=0; i<jter->size(); ++i)
|
||||
{
|
||||
str.append(QString("%1 ").arg(jter->at(i)));
|
||||
}
|
||||
if(j+1 < actions.size())
|
||||
{
|
||||
str.append(QString("\n"));
|
||||
}
|
||||
++j;
|
||||
}
|
||||
if(str.size())
|
||||
{
|
||||
ui_->label_actionsB->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));
|
||||
}
|
||||
ui_->label_parentsB->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));
|
||||
}
|
||||
ui_->label_childrenB->setText(str);
|
||||
}
|
||||
}
|
||||
|
||||
ui_->label_idB->setText(QString::number(id));
|
||||
ui_->graphicsView_B->fitInView(ui_->graphicsView_B->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
|
||||
}
|
||||
else
|
||||
{
|
||||
ULOGGER_ERROR("Slider index out of range ?");
|
||||
}
|
||||
}
|
||||
|
||||
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) //fct recuperer sur le net, converti un ldImage en QImage
|
||||
{
|
||||
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;
|
||||
}
|
||||
@@ -1,72 +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;
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
class KeypointMemory;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
private:
|
||||
Ui_MainWindow * ui_;
|
||||
QMap<int, QByteArray> imagesMap_;
|
||||
QList<int> ids_;
|
||||
rtabmap::KeypointMemory * memory_;
|
||||
QString pathDatabase_;
|
||||
};
|
||||
|
||||
#endif /* MAINWINDOW_H_ */
|
||||
@@ -1,42 +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 <QtGui/QApplication>
|
||||
#include "MainWindow.h"
|
||||
#include "utilite/ULogger.h"
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
|
||||
QApplication * app = new QApplication(argc, argv);
|
||||
MainWindow * mainWindow = new MainWindow();
|
||||
mainWindow->showNormal();
|
||||
|
||||
// Now wait for application to finish
|
||||
app->connect( app, SIGNAL( lastWindowClosed() ),
|
||||
app, SLOT( quit() ) );
|
||||
app->exec();// MUST be called by the Main Thread
|
||||
|
||||
delete mainWindow;
|
||||
delete app;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,294 +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>600</width>
|
||||
<height>350</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>Images database viewer</string>
|
||||
</property>
|
||||
<widget class="QWidget" name="centralwidget">
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_3">
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_6">
|
||||
<item>
|
||||
<widget class="QGraphicsView" name="graphicsView_A"/>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="label_actionsA">
|
||||
<property name="text">
|
||||
<string>Actions</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="0" column="0">
|
||||
<widget class="QLabel" name="label_actionsA_2">
|
||||
<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="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>
|
||||
</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">
|
||||
<item>
|
||||
<widget class="QGraphicsView" name="graphicsView_B"/>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="label_actionsB">
|
||||
<property name="text">
|
||||
<string>Actions</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="0" column="0">
|
||||
<widget class="QLabel" name="label_actionsA_4">
|
||||
<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="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>
|
||||
</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>600</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>
|
||||
@@ -1,31 +0,0 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${UTILITE_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../include
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../src
|
||||
)
|
||||
|
||||
SET(LIBRARIES
|
||||
${UTILITE_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
)
|
||||
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
ADD_EXECUTABLE(imagesDbExtractor ${SRC_FILES})
|
||||
TARGET_LINK_LIBRARIES(imagesDbExtractor corelib ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( imagesDbExtractor
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesDbExtractor)
|
||||
|
||||
INSTALL(TARGETS imagesDbExtractor
|
||||
RUNTIME DESTINATION bin COMPONENT runtime
|
||||
LIBRARY DESTINATION lib COMPONENT devel
|
||||
ARCHIVE DESTINATION lib COMPONENT devel)
|
||||
|
||||
@@ -1,61 +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 <utilite/UConversion.h>
|
||||
#include <utilite/UDirectory.h>
|
||||
#include "SMMemory.h"
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
|
||||
std::string path = rtabmap::Parameters::defaultRtabmapWorkingDirectory() + "/LTM.db";
|
||||
if(argc > 1)
|
||||
{
|
||||
path = argv[1];
|
||||
}
|
||||
|
||||
|
||||
// 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))
|
||||
{
|
||||
UWARN("Can't open database \"%s\"", path.c_str());
|
||||
}
|
||||
|
||||
if(memory)
|
||||
{
|
||||
UINFO("Using database %s", path.c_str());
|
||||
std::string saveDirectory = "imagesExtracted/";
|
||||
if(!UDirectory::exists(saveDirectory))
|
||||
{
|
||||
UDirectory::makeDir(saveDirectory);
|
||||
}
|
||||
std::set<int> ids = memory->getAllSignatureIds();
|
||||
for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
|
||||
{
|
||||
IplImage * image = memory->getImage(*iter);
|
||||
if(image)
|
||||
{
|
||||
std::string fileName = uNumber2Str(*iter) + ".jpg";
|
||||
cvSaveImage((saveDirectory+fileName).c_str(), image);
|
||||
cvReleaseImage(&image);
|
||||
UINFO("Saved %s", (saveDirectory+fileName).c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,32 +0,0 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${UTILITE_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
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(imagesJoiner ${SRC_FILES})
|
||||
TARGET_LINK_LIBRARIES(imagesJoiner ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( imagesJoiner
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesJoiner)
|
||||
|
||||
INSTALL(TARGETS imagesJoiner
|
||||
RUNTIME DESTINATION bin COMPONENT runtime
|
||||
LIBRARY DESTINATION lib COMPONENT devel
|
||||
ARCHIVE DESTINATION lib COMPONENT devel)
|
||||
|
||||
@@ -1,176 +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 <opencv2/core/core.hpp>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
void showUsage()
|
||||
{
|
||||
printf("Usage:\n"
|
||||
"imagesJoiner.exe \"# (see below)\" \"type\" [option]\n"
|
||||
" # : Name pattern is how the file names are formatted (in number).\n"
|
||||
" Examples: 4 for pictures with 0001.jpg, 0002.jpg, ..., 0010.jpg, 0100.jpg, 1000.jpg, ...\n"
|
||||
" 1 for pictures with 1.jpg, 2.jpg, ..., 10.jpg, 100.jpg, 1000.jpg, ...\n"
|
||||
" type : is the extension (jpg, bmp, png...)\n"
|
||||
" Options:\n"
|
||||
" -inv option for copying odd images on the right\n"
|
||||
" -d # destination filename size\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
if(argc < 3)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
|
||||
bool inv = false;
|
||||
unsigned int sizeFileName = std::atoi(argv[1]);
|
||||
unsigned int sizeTargetFileName = sizeFileName;
|
||||
|
||||
std::string type = argv[2];
|
||||
for(int i=3; i<argc; ++i)
|
||||
{
|
||||
if(strcmp(argv[i], "-inv") == 0)
|
||||
{
|
||||
inv = true;
|
||||
printf(" Inversing option activated...\n");
|
||||
}
|
||||
if(strcmp(argv[i], "-d") == 0 && i+1<argc)
|
||||
{
|
||||
sizeTargetFileName = std::atoi(argv[i+1]);
|
||||
if(sizeTargetFileName < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
printf(" Target size file name option activated=%d\n",sizeTargetFileName);
|
||||
++i;
|
||||
}
|
||||
}
|
||||
|
||||
printf(" Format = %s\n", argv[1]);
|
||||
printf(" Type = %s\n", argv[2]);
|
||||
|
||||
std::string targetDirectory = "imagesJoined/";
|
||||
UDirectory::makeDir((UDirectory::currentDir(true) + "imagesJoined").c_str());
|
||||
|
||||
int counterJoined = 1;
|
||||
int counterImages = 1;
|
||||
bool imagesExist = true;
|
||||
|
||||
std::string fileNameA;
|
||||
std::string fileNameB;
|
||||
|
||||
while(imagesExist)
|
||||
{
|
||||
std::string fileNameTarget = uNumber2Str(counterJoined);
|
||||
if(inv)
|
||||
{
|
||||
fileNameA = uNumber2Str(counterImages+1);
|
||||
fileNameB = uNumber2Str(counterImages);
|
||||
}
|
||||
else
|
||||
{
|
||||
fileNameA = uNumber2Str(counterImages);
|
||||
fileNameB = uNumber2Str(counterImages+1);
|
||||
}
|
||||
|
||||
while(fileNameA.size() < sizeFileName)
|
||||
{
|
||||
fileNameA.insert(0, "0");
|
||||
}
|
||||
|
||||
while(fileNameB.size() < sizeFileName)
|
||||
{
|
||||
fileNameB.insert(0, "0");
|
||||
}
|
||||
|
||||
while(fileNameTarget.size() < sizeTargetFileName)
|
||||
{
|
||||
fileNameTarget.insert(0, "0");
|
||||
}
|
||||
|
||||
(fileNameTarget.insert(0, targetDirectory) += ".") += type;
|
||||
(fileNameA += ".") += type;
|
||||
(fileNameB += ".") += type;
|
||||
|
||||
IplImage * imageA = cvLoadImage(fileNameA.c_str(), CV_LOAD_IMAGE_COLOR);
|
||||
IplImage * imageB = cvLoadImage(fileNameB.c_str(), CV_LOAD_IMAGE_COLOR);
|
||||
|
||||
if(imageA && imageB)
|
||||
{
|
||||
CvSize sizeA = cvGetSize(imageA);
|
||||
CvSize sizeB = cvGetSize(imageB);
|
||||
CvSize targetSize = {0};
|
||||
targetSize.width = sizeA.width + sizeB.width;
|
||||
targetSize.height = sizeA.height > sizeB.height ? sizeA.height : sizeB.height;
|
||||
IplImage* targetImage = cvCreateImage(targetSize, imageA->depth, imageA->nChannels);
|
||||
if(targetImage)
|
||||
{
|
||||
cvSetImageROI( targetImage, cvRect( 0, 0, sizeA.width, sizeA.height ) );
|
||||
cvCopy( imageA, targetImage );
|
||||
cvSetImageROI( targetImage, cvRect( sizeA.width, 0, sizeB.width, sizeB.height ) );
|
||||
cvCopy( imageB, targetImage );
|
||||
cvResetImageROI( targetImage );
|
||||
|
||||
if(!cvSaveImage(fileNameTarget.c_str(), targetImage))
|
||||
{
|
||||
printf("Error : saving to \"%s\" goes wrong...\n", fileNameTarget.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Saved \"%s\" \n", fileNameTarget.c_str());
|
||||
}
|
||||
|
||||
cvReleaseImage(&targetImage);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Error : can't allocated the target image with size (%d,%d)\n", targetSize.width, targetSize.height);
|
||||
imagesExist = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
imagesExist = false;
|
||||
}
|
||||
|
||||
if(imageA)
|
||||
{
|
||||
cvReleaseImage(&imageA);
|
||||
}
|
||||
if(imageB)
|
||||
{
|
||||
cvReleaseImage(&imageB);
|
||||
}
|
||||
|
||||
counterJoined++;
|
||||
counterImages += 2;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${UTILITE_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${FFTW3F_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../include
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../src
|
||||
)
|
||||
|
||||
SET(LIBRARIES
|
||||
${UTILITE_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
${FFTW3F_LIBRARIES}
|
||||
)
|
||||
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
ADD_EXECUTABLE(logPolar ${SRC_FILES})
|
||||
TARGET_LINK_LIBRARIES(logPolar corelib ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( logPolar
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-logPolar)
|
||||
|
||||
INSTALL(TARGETS logPolar
|
||||
RUNTIME DESTINATION bin COMPONENT runtime
|
||||
LIBRARY DESTINATION lib COMPONENT devel
|
||||
ARCHIVE DESTINATION lib COMPONENT devel)
|
||||
|
||||
@@ -1,143 +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 "ColorTable.h"
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kDebug);
|
||||
|
||||
IplImage* src = 0;
|
||||
if( argc == 2)
|
||||
{
|
||||
src=cvLoadImage(argv[1],1);
|
||||
}
|
||||
else
|
||||
{
|
||||
rtabmap::CameraVideo cam(0,0,false, 640, 480);
|
||||
if(cam.init())
|
||||
{
|
||||
src = cam.takeImage();
|
||||
}
|
||||
}
|
||||
if(src)
|
||||
{
|
||||
UTimer timer;
|
||||
timer.start();
|
||||
// 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(&src);
|
||||
cvReleaseImage(&polar);
|
||||
cvReleaseImage(&src2);
|
||||
cvReleaseImage(&hsv);
|
||||
cvReleaseImage(&ind);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${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 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)
|
||||
|
||||
@@ -1,258 +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 "rtabmap/core/SMState.h"
|
||||
#include "utilite/UEventsManager.h"
|
||||
#include "utilite/UEventsHandler.h"
|
||||
#include "rtabmap/core/CameraEvent.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_)
|
||||
{
|
||||
cvNamedWindow("Webcam", CV_WINDOW_AUTOSIZE);
|
||||
}
|
||||
}
|
||||
virtual ~ImagesHandler()
|
||||
{
|
||||
if(show_)
|
||||
{
|
||||
cvDestroyWindow("Webcam");
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void handleEvent(UEvent * e)
|
||||
{
|
||||
if(e->getClassName().compare("SMStateEvent") == 0)
|
||||
{
|
||||
const rtabmap::SMStateEvent * event = (const rtabmap::SMStateEvent*)e;
|
||||
const rtabmap::SMState * sm = event->getSMState();
|
||||
const IplImage * image = 0;
|
||||
if(sm)
|
||||
{
|
||||
image = sm->getImage();
|
||||
}
|
||||
if(image)
|
||||
{
|
||||
if(show_)
|
||||
{
|
||||
cvShowImage("Webcam", image);
|
||||
}
|
||||
if(save_)
|
||||
{
|
||||
std::string fileName = targetDir_ + "/";
|
||||
fileName += uNumber2Str(id_++);
|
||||
fileName += ".";
|
||||
fileName += ext_;
|
||||
cvSaveImage(fileName.c_str(), 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)
|
||||
{
|
||||
cvWaitKey(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cin.ignore();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
+53
-52
@@ -1,52 +1,53 @@
|
||||
/*
|
||||
* 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 CAMERAEVENT_H_
|
||||
#define CAMERAEVENT_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "utilite/UEvent.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class RTABMAP_EXP CameraEvent :
|
||||
public UEvent
|
||||
{
|
||||
public:
|
||||
enum Code {
|
||||
kCodeNoMoreImages
|
||||
};
|
||||
|
||||
public:
|
||||
CameraEvent() :
|
||||
UEvent(kCodeNoMoreImages)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~CameraEvent() {}
|
||||
|
||||
virtual std::string getClassName() const {return std::string("CameraEvent");}
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
#endif /* CAMERAEVENT_H_ */
|
||||
/*
|
||||
* 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 ACTUATOR_H_
|
||||
#define ACTUATOR_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <utilite/UEvent.h>
|
||||
#include <list>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class Actuator
|
||||
{
|
||||
public:
|
||||
enum Type{kTypeTwist=0, kTypeNotSpecified};
|
||||
public:
|
||||
Actuator(const cv::Mat & data, Type type, int num = 0) :
|
||||
_data(data),
|
||||
_type(type),
|
||||
_num(num)
|
||||
{}
|
||||
const cv::Mat & data() const {return _data;}
|
||||
int type() const {return _type;}
|
||||
int num() const {return _num;}
|
||||
virtual ~Actuator() {};
|
||||
private:
|
||||
cv::Mat _data;
|
||||
int _type;
|
||||
int _num;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* ACTUATOR_H_ */
|
||||
@@ -53,11 +53,11 @@ public:
|
||||
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;
|
||||
bool generatePrediction(cv::Mat & 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;
|
||||
float addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const;
|
||||
|
||||
private:
|
||||
std::map<int, float> _posterior;
|
||||
@@ -22,60 +22,69 @@
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include "utilite/UThreadNode.h"
|
||||
#include "utilite/UEventsHandler.h"
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <utilite/UThreadNode.h>
|
||||
#include <utilite/UEventsHandler.h>
|
||||
#include <utilite/UEvent.h>
|
||||
#include <utilite/UDirectory.h>
|
||||
#include <utilite/UTimer.h>
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include "rtabmap/core/KeypointDetector.h"
|
||||
#include "rtabmap/core/KeypointDescriptor.h"
|
||||
#include <set>
|
||||
#include <stack>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
class UDirectory;
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class KeypointDetector;
|
||||
class KeypointDescriptor;
|
||||
class SMState;
|
||||
|
||||
/**
|
||||
* No treatment
|
||||
*/
|
||||
class RTABMAP_EXP CamPostTreatment
|
||||
class CameraEvent :
|
||||
public UEvent
|
||||
{
|
||||
public:
|
||||
CamPostTreatment(const ParametersMap & parameters = ParametersMap()) {
|
||||
this->parseParameters(parameters);
|
||||
}
|
||||
virtual ~CamPostTreatment() {}
|
||||
virtual void process(SMState * smState) const;
|
||||
virtual void parseParameters(const ParametersMap & parameters) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* Extract keypoints from the image
|
||||
*/
|
||||
class RTABMAP_EXP CamKeypointTreatment : public CamPostTreatment
|
||||
{
|
||||
public:
|
||||
enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
|
||||
enum DescriptorStrategy {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
|
||||
enum Code {
|
||||
kCodeFeatures,
|
||||
kCodeImage,
|
||||
kCodeNoMoreImages
|
||||
};
|
||||
|
||||
public:
|
||||
CamKeypointTreatment(const ParametersMap & parameters = ParametersMap()) :
|
||||
_keypointDetector(0),
|
||||
_keypointDescriptor(0)
|
||||
CameraEvent(const cv::Mat & image, int cameraId = 0) :
|
||||
UEvent(kCodeImage),
|
||||
_cameraId(cameraId),
|
||||
_image(image)
|
||||
{
|
||||
this->parseParameters(parameters);
|
||||
}
|
||||
virtual ~CamKeypointTreatment();
|
||||
virtual void process(SMState * smState) const;
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
DetectorStrategy detectorStrategy() const;
|
||||
CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int cameraId = 0) :
|
||||
UEvent(kCodeFeatures),
|
||||
_cameraId(cameraId),
|
||||
_image(image),
|
||||
_descriptors(descriptors),
|
||||
_keypoints(keypoints)
|
||||
{
|
||||
}
|
||||
CameraEvent(int cameraId = 0) :
|
||||
UEvent(kCodeNoMoreImages),
|
||||
_cameraId(cameraId)
|
||||
{
|
||||
}
|
||||
|
||||
int cameraId() const {return _cameraId;}
|
||||
|
||||
// Image or descriptors
|
||||
const cv::Mat & image() const {return _image;}
|
||||
const cv::Mat & descriptors() const {return _descriptors;}
|
||||
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
|
||||
|
||||
virtual ~CameraEvent() {}
|
||||
virtual std::string getClassName() const {return std::string("CameraEvent");}
|
||||
|
||||
private:
|
||||
KeypointDetector * _keypointDetector;
|
||||
KeypointDescriptor * _keypointDescriptor;
|
||||
int _cameraId;
|
||||
cv::Mat _image;
|
||||
cv::Mat _descriptors;
|
||||
std::vector<cv::KeyPoint> _keypoints;
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -91,17 +100,20 @@ public:
|
||||
|
||||
public:
|
||||
virtual ~Camera();
|
||||
|
||||
virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0) = 0;
|
||||
SMState * takeSMState();
|
||||
cv::Mat takeImage();
|
||||
cv::Mat takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & keypoints);
|
||||
virtual bool init() = 0;
|
||||
bool isPaused() const {return !this->isRunning();}
|
||||
bool isCapturing() const {return this->isRunning();}
|
||||
unsigned int getImageWidth() const {return _imageWidth;}
|
||||
unsigned int getImageHeight() const {return _imageHeight;}
|
||||
void setPostThreatement(CamPostTreatment * strategy); // ownership is transferred
|
||||
float getImageRate() const {return _imageRate;}
|
||||
bool isFeaturesExtracted() const {return _featuresExtracted;}
|
||||
void setFeaturesExtracted(bool featuresExtracted, KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef, KeypointDescriptor::DescriptorType descriptor = KeypointDescriptor::kDescriptorUndef);
|
||||
void setImageRate(float imageRate) {_imageRate = imageRate;}
|
||||
void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
int id() const {return _id;}
|
||||
|
||||
protected:
|
||||
/**
|
||||
@@ -109,15 +121,16 @@ protected:
|
||||
*
|
||||
* @param imageRate : image/second , 0 for fast as the camera can
|
||||
*/
|
||||
Camera(float imageRate = 0, bool autoRestart = false, unsigned int imageWidth = 0, unsigned int imageHeight = 0);
|
||||
Camera(float imageRate = 0, bool autoRestart = false, unsigned int imageWidth = 0, unsigned int imageHeight = 0, unsigned int framesDropped = 0, int id = 0);
|
||||
|
||||
virtual void handleEvent(UEvent* anEvent);
|
||||
virtual cv::Mat captureImage() = 0;
|
||||
|
||||
private:
|
||||
virtual void mainLoopBegin();
|
||||
virtual void mainLoop();
|
||||
void process();
|
||||
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
|
||||
virtual void parseParameters(const ParametersMap & parameters) {_postThreatement->parseParameters(parameters);}
|
||||
|
||||
private:
|
||||
float _imageRate;
|
||||
@@ -125,11 +138,16 @@ private:
|
||||
bool _autoRestart;
|
||||
unsigned int _imageWidth;
|
||||
unsigned int _imageHeight;
|
||||
CamPostTreatment * _postThreatement;
|
||||
unsigned int _framesDropped;
|
||||
UTimer _frameRateTimer;
|
||||
|
||||
UMutex _stateMutex;
|
||||
std::stack<State> _state;
|
||||
std::stack<ParametersMap> _stateParam;
|
||||
|
||||
bool _featuresExtracted;
|
||||
KeypointDetector * _keypointDetector;
|
||||
KeypointDescriptor * _keypointDescriptor;
|
||||
};
|
||||
|
||||
|
||||
@@ -147,19 +165,23 @@ public:
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
unsigned int imageHeight = 0,
|
||||
unsigned int framesDropped = 0,
|
||||
int id = 0);
|
||||
virtual ~CameraImages();
|
||||
|
||||
virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
|
||||
virtual bool init();
|
||||
|
||||
protected:
|
||||
virtual cv::Mat captureImage();
|
||||
|
||||
private:
|
||||
std::string _path;
|
||||
int _startAt;
|
||||
// If the list of files in the directory is refreshed
|
||||
// on each call of takeImage()
|
||||
bool _refreshDir;
|
||||
UDirectory * _dir;
|
||||
UDirectory _dir;
|
||||
int _count;
|
||||
std::string _lastFileName;
|
||||
|
||||
@@ -184,22 +206,29 @@ public:
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
unsigned int imageHeight = 0,
|
||||
unsigned int framesDropped = 0,
|
||||
int id = 0);
|
||||
CameraVideo(const std::string & fileName,
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
unsigned int imageHeight = 0,
|
||||
unsigned int framesDropped = 0,
|
||||
int id = 0);
|
||||
virtual ~CameraVideo();
|
||||
|
||||
virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
|
||||
virtual bool init();
|
||||
int getUsbDevice() {return _usbDevice;}
|
||||
|
||||
protected:
|
||||
virtual cv::Mat captureImage();
|
||||
|
||||
private:
|
||||
// File type
|
||||
std::string _fileName;
|
||||
|
||||
CvCapture* _capture;
|
||||
cv::VideoCapture _capture;
|
||||
Source _src;
|
||||
|
||||
// Usb camera
|
||||
@@ -207,36 +236,4 @@ private:
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// CameraDatabase
|
||||
/////////////////////////
|
||||
class DBDriver;
|
||||
class RTABMAP_EXP CameraDatabase :
|
||||
public Camera
|
||||
{
|
||||
public:
|
||||
CameraDatabase(const std::string & path,
|
||||
bool loadActions,
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
virtual ~CameraDatabase();
|
||||
|
||||
virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
|
||||
virtual bool init();
|
||||
|
||||
private:
|
||||
std::string _path;
|
||||
bool _loadActions;
|
||||
std::set<int>::iterator _indexIter;
|
||||
DBDriver * _dbDriver;
|
||||
std::set<int> _ids;
|
||||
};
|
||||
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -10,6 +10,16 @@ namespace rtabmap
|
||||
{
|
||||
class RTABMAP_EXP ColorTable
|
||||
{
|
||||
public:
|
||||
enum Size{kSize8 = 8,
|
||||
kSize16 = 16,
|
||||
kSize32 = 32,
|
||||
kSize64 = 64,
|
||||
kSize128 = 128,
|
||||
kSize256 = 256,
|
||||
kSize512 = 512,
|
||||
kSize1024 = 1024,
|
||||
kSize65536 = 65536};
|
||||
public:
|
||||
ColorTable(int size);
|
||||
virtual ~ColorTable() {}
|
||||
@@ -92,22 +92,23 @@ public:
|
||||
|
||||
// Load objects
|
||||
bool load(VWDictionary * dictionary) const;
|
||||
bool loadLastSignatures(std::list<Signature *> & signatures) const;
|
||||
bool loadLastNodes(std::list<Signature *> & signatures) const;
|
||||
bool loadKeypointSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
|
||||
bool loadSMSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
|
||||
bool loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws);
|
||||
|
||||
// Specific queries...
|
||||
bool getImage(int id, IplImage ** img) const;
|
||||
bool getNeighborIds(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) const;
|
||||
bool getRawData(int id, std::list<Sensor> & data) const;
|
||||
bool getActuatorData(int id, std::list<Actuator> & data) const;
|
||||
bool getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
|
||||
bool loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const;
|
||||
bool getWeight(int signatureId, int & weight) const;
|
||||
bool getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
|
||||
bool getAllSignatureIds(std::set<int> & ids) const;
|
||||
bool getLastSignatureId(int & id) const;
|
||||
bool getLastVisualWordId(int & id) const;
|
||||
bool getSurfNi(int signatureId, int & ni) const;
|
||||
bool getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const;
|
||||
bool getAllNodeIds(std::set<int> & ids) const;
|
||||
bool getLastNodeId(int & id) const;
|
||||
bool getLastWordId(int & id) const;
|
||||
bool getInvertedIndexNi(int signatureId, int & ni) const;
|
||||
bool getHighestWeightedNodeIds(unsigned int count, std::multimap<int, int> & ids) const;
|
||||
|
||||
protected:
|
||||
DBDriver(const ParametersMap & parameters = ParametersMap());
|
||||
@@ -122,7 +123,7 @@ private:
|
||||
|
||||
virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const = 0; // <oldWordId, activeWordId>
|
||||
virtual bool deleteWordsQuery(const std::vector<int> & ids) const = 0;
|
||||
virtual bool getNeighborIdsQuery(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) const = 0;
|
||||
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const = 0;
|
||||
virtual bool getWeightQuery(int signatureId, int & weight) const = 0;
|
||||
virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const = 0;
|
||||
|
||||
@@ -132,7 +133,7 @@ private:
|
||||
|
||||
// Load objects
|
||||
virtual bool loadQuery(VWDictionary * dictionary) const = 0;
|
||||
virtual bool loadLastSignaturesQuery(std::list<Signature *> & signatures) const = 0;
|
||||
virtual bool loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
|
||||
virtual bool loadQuery(int signatureId, Signature ** s) const = 0;
|
||||
virtual bool loadQuery(int wordId, VisualWord ** vw) const = 0;
|
||||
virtual bool loadQuery(int signatureId, KeypointSignature * ss) const = 0;
|
||||
@@ -142,12 +143,13 @@ private:
|
||||
virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
|
||||
virtual bool loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const = 0;
|
||||
|
||||
virtual bool getImageQuery(int id, IplImage ** image) const = 0;
|
||||
virtual bool getAllSignatureIdsQuery(std::set<int> & ids) const = 0;
|
||||
virtual bool getLastSignatureIdQuery(int & id) const = 0;
|
||||
virtual bool getLastVisualWordIdQuery(int & id) const = 0;
|
||||
virtual bool getSurfNiQuery(int signatureId, int & ni) const = 0;
|
||||
virtual bool getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int,int> & signatures) const = 0;
|
||||
virtual bool getRawDataQuery(int id, std::list<Sensor> & rawData) const = 0;
|
||||
virtual bool getActuatorDataQuery(int id, std::list<Actuator> & rawData) const = 0;
|
||||
virtual bool getAllNodeIdsQuery(std::set<int> & ids) const = 0;
|
||||
virtual bool getLastNodeIdQuery(int & id) const = 0;
|
||||
virtual bool getLastWordIdQuery(int & id) const = 0;
|
||||
virtual bool getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
|
||||
virtual bool getHighestWeightedNodeIdsQuery(unsigned int count, std::multimap<int,int> & signatures) const = 0;
|
||||
|
||||
private:
|
||||
//non-abstract methods
|
||||
@@ -167,7 +169,6 @@ private:
|
||||
unsigned int _minSignaturesToSave;
|
||||
unsigned int _minWordsToSave;
|
||||
bool _imagesCompressed;
|
||||
bool _asyncWaiting;
|
||||
double _emptyTrashesTime;
|
||||
std::string _url;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* DBReader.h
|
||||
*
|
||||
* Created on: 2012-06-13
|
||||
* Author: mathieu
|
||||
*/
|
||||
|
||||
#ifndef DBREADER_H_
|
||||
#define DBREADER_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "rtabmap/core/Sensor.h"
|
||||
#include "rtabmap/core/Actuator.h"
|
||||
|
||||
#include <utilite/UThreadNode.h>
|
||||
#include <utilite/UTimer.h>
|
||||
|
||||
#include <set>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class DBDriver;
|
||||
|
||||
class RTABMAP_EXP DBReader : public UThreadNode {
|
||||
public:
|
||||
DBReader(const std::string & databasePath,
|
||||
float frameRate = 0.0f,
|
||||
const std::set<Sensor::Type> & sensorTypes = std::set<Sensor::Type>(),
|
||||
const std::set<Actuator::Type> & actuatorTypes = std::set<Actuator::Type>());
|
||||
virtual ~DBReader();
|
||||
|
||||
bool init();
|
||||
void setFrameRate(float frameRate);
|
||||
void getNextSensorimotorState(std::list<Sensor> & sensors, std::list<Actuator> & actuators);
|
||||
|
||||
protected:
|
||||
virtual void mainLoopBegin();
|
||||
virtual void mainLoop();
|
||||
|
||||
private:
|
||||
std::string _path;
|
||||
float _frameRate;
|
||||
std::set<Sensor::Type> _sensorTypes;
|
||||
std::set<Actuator::Type> _actuatorTypes;
|
||||
|
||||
DBDriver * _dbDriver;
|
||||
UTimer _timer;
|
||||
std::set<int> _ids;
|
||||
std::set<int>::iterator _currentId;
|
||||
};
|
||||
|
||||
} /* namespace rtabmap */
|
||||
#endif /* DBREADER_H_ */
|
||||
@@ -22,12 +22,63 @@
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
//epipolar geometry
|
||||
void RTABMAP_EXP findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2);
|
||||
void RTABMAP_EXP findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2 = cv::Vec3d());
|
||||
void RTABMAP_EXP findEpipolesFromF(
|
||||
const cv::Mat & fundamentalMatrix,
|
||||
cv::Vec3d & e1,
|
||||
cv::Vec3d & e2);
|
||||
|
||||
cv::Mat RTABMAP_EXP findPFromF(
|
||||
const cv::Mat & fundamentalMatrix,
|
||||
const cv::Mat & x1,
|
||||
const cv::Mat & x2);
|
||||
|
||||
// return fundamental matrix
|
||||
// status -> inliers = 1, outliers = 0
|
||||
cv::Mat RTABMAP_EXP findFFromWords(
|
||||
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
|
||||
std::vector<uchar> & status,
|
||||
double ransacParam1 = 3.0,
|
||||
double ransacParam2 = 0.99);
|
||||
|
||||
// assume a canonical camera (without K)
|
||||
void RTABMAP_EXP findRTFromP(
|
||||
const cv::Mat & p,
|
||||
cv::Mat & r,
|
||||
cv::Mat & t);
|
||||
|
||||
/**
|
||||
* 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 RTABMAP_EXP 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);
|
||||
|
||||
/**
|
||||
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
|
||||
* realPairsCount = 5
|
||||
*/
|
||||
int RTABMAP_EXP 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);
|
||||
|
||||
/**
|
||||
* 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 RTABMAP_EXP 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);
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -31,10 +31,13 @@
|
||||
namespace rtabmap {
|
||||
|
||||
class RTABMAP_EXP KeypointDescriptor {
|
||||
public:
|
||||
enum DescriptorType {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
|
||||
|
||||
public:
|
||||
virtual ~KeypointDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const = 0;
|
||||
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
|
||||
|
||||
protected:
|
||||
KeypointDescriptor(const ParametersMap & parameters = ParametersMap());
|
||||
@@ -47,10 +50,15 @@ public:
|
||||
SURFDescriptor(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~SURFDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
|
||||
private:
|
||||
CvSURFParams _params;
|
||||
double _hessianThreshold;
|
||||
int _nOctaves;
|
||||
int _nOctaveLayers;
|
||||
bool _extended;
|
||||
bool _upright;
|
||||
|
||||
bool _gpuVersion;
|
||||
};
|
||||
|
||||
@@ -61,11 +69,14 @@ public:
|
||||
SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~SIFTDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
|
||||
private:
|
||||
cv::SIFT::CommonParams _commonParams;
|
||||
cv::SIFT::DescriptorParams _descriptorParams;
|
||||
int _nfeatures;
|
||||
int _nOctaveLayers;
|
||||
double _contrastThreshold;
|
||||
double _edgeThreshold;
|
||||
double _sigma;
|
||||
};
|
||||
|
||||
//BRIEFDescriptor
|
||||
@@ -75,7 +86,7 @@ public:
|
||||
BRIEFDescriptor(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~BRIEFDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
|
||||
private:
|
||||
int _size;
|
||||
@@ -88,7 +99,7 @@ public:
|
||||
ColorDescriptor(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~ColorDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
protected:
|
||||
void getCircularROI(int R, std::vector<int> & RxV) const;
|
||||
};
|
||||
@@ -100,7 +111,7 @@ public:
|
||||
HueDescriptor(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~HueDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
private:
|
||||
// assuming that rgb values are normalized [0,1]
|
||||
float rgb2hue(float r, float g, float b) const;
|
||||
|
||||
@@ -35,25 +35,22 @@ class VWDictionary;
|
||||
|
||||
class RTABMAP_EXP KeypointDetector
|
||||
{
|
||||
public:
|
||||
enum DetectorType {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
|
||||
|
||||
public:
|
||||
virtual ~KeypointDetector() {}
|
||||
std::vector<cv::KeyPoint> generateKeypoints(const IplImage * image);
|
||||
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image);
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
|
||||
double getAdaptiveResponseThr() const {return _adaptiveResponseThr;}
|
||||
virtual double getMinimumResponseThr() const = 0;
|
||||
bool isUsingAdaptiveResponseThr() const {return _usingAdaptiveResponseThr;}
|
||||
void setRoi(const std::string & roi);
|
||||
cv::Rect computeRoi(const IplImage * image) const;
|
||||
cv::Rect computeRoi(const cv::Mat & image) const;
|
||||
protected:
|
||||
KeypointDetector(const ParametersMap & parameters = ParametersMap());
|
||||
void setAdaptiveResponseThr(float adaptiveResponseThr) {_adaptiveResponseThr = adaptiveResponseThr;}
|
||||
private:
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const = 0;
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
|
||||
private:
|
||||
unsigned int _wordsPerImageTarget;
|
||||
bool _usingAdaptiveResponseThr;
|
||||
double _adaptiveResponseThr;
|
||||
std::vector<float> _roiRatios; // size 4
|
||||
};
|
||||
|
||||
@@ -64,11 +61,15 @@ public:
|
||||
SURFDetector(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~SURFDetector();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual double getMinimumResponseThr() const {return _params.hessianThreshold;};
|
||||
private:
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
|
||||
private:
|
||||
CvSURFParams _params;
|
||||
double _hessianThreshold;
|
||||
int _nOctaves;
|
||||
int _nOctaveLayers;
|
||||
bool _extended;
|
||||
bool _upright;
|
||||
|
||||
bool _gpuVersion;
|
||||
};
|
||||
|
||||
@@ -79,12 +80,14 @@ public:
|
||||
SIFTDetector(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~SIFTDetector();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual double getMinimumResponseThr() const {return _detectorParams.threshold;};
|
||||
private:
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
|
||||
private:
|
||||
cv::SIFT::CommonParams _commonParams;
|
||||
cv::SIFT::DetectorParams _detectorParams;
|
||||
int _nfeatures;
|
||||
int _nOctaveLayers;
|
||||
double _contrastThreshold;
|
||||
double _edgeThreshold;
|
||||
double _sigma;
|
||||
};
|
||||
|
||||
//StarDetector
|
||||
@@ -94,11 +97,14 @@ public:
|
||||
StarDetector(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~StarDetector();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual double getMinimumResponseThr() const {return (double)_params.responseThreshold;};
|
||||
private:
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
|
||||
private:
|
||||
CvStarDetectorParams _params;
|
||||
int _maxSize;
|
||||
int _responseThreshold;
|
||||
int _lineThresholdProjected;
|
||||
int _lineThresholdBinarized;
|
||||
int _suppressNonmaxSize;
|
||||
};
|
||||
|
||||
//FASTDetector
|
||||
@@ -108,9 +114,8 @@ public:
|
||||
FASTDetector(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~FASTDetector();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual double getMinimumResponseThr() const {return (double)_threshold;};
|
||||
private:
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
|
||||
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
|
||||
private:
|
||||
int _threshold;
|
||||
bool _nonmaxSuppression;
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "Memory.h"
|
||||
#include "rtabmap/core/Memory.h"
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
|
||||
namespace rtabmap {
|
||||
@@ -34,10 +34,6 @@ 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();
|
||||
@@ -56,18 +52,17 @@ public:
|
||||
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 addSignatureToStm(Signature * signature, const std::list<Actuator> & actions = std::list<Actuator>());
|
||||
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);
|
||||
virtual Signature * createSignature(int id, const std::list<Sensor> & sensors, bool keepRawData=false);
|
||||
|
||||
void disableWordsRef(int signatureId);
|
||||
void enableWordsRef(const std::list<int> & signatureIds);
|
||||
@@ -75,7 +70,6 @@ private:
|
||||
int getNi(int signatureId) const;
|
||||
|
||||
private:
|
||||
std::list<int> _commonWords;
|
||||
VWDictionary * _vwd;
|
||||
KeypointDetector * _keypointDetector;
|
||||
KeypointDescriptor * _keypointDescriptor;
|
||||
@@ -31,6 +31,8 @@
|
||||
#include <set>
|
||||
#include "utilite/UStl.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include "rtabmap/core/Sensor.h"
|
||||
#include "rtabmap/core/Actuator.h"
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
@@ -38,7 +40,6 @@ class Signature;
|
||||
class NeighborLink;
|
||||
class DBDriver;
|
||||
class Node;
|
||||
class SMState;
|
||||
|
||||
class RTABMAP_EXP Memory
|
||||
{
|
||||
@@ -52,17 +53,33 @@ public:
|
||||
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);
|
||||
bool update(const std::list<Sensor> & sensors,
|
||||
const std::list<Actuator> & actuators,
|
||||
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);
|
||||
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;
|
||||
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
|
||||
@@ -70,18 +87,25 @@ public:
|
||||
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;
|
||||
std::list<NeighborLink> getNeighborLinks(int signatureId,
|
||||
bool ignoreNeighborByLoopClosure = false,
|
||||
bool lookInDatabase = false,
|
||||
bool onlyWithActions = 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 std::map<int, std::map<int, float> > & getSimilaritiesMap() const {return _similaritiesMap;}
|
||||
const Signature * getLastSignature() const;
|
||||
int getDatabaseMemoryUsed() const; // in bytes
|
||||
double getDbSavingTime() const;
|
||||
IplImage * getImage(int id) const;
|
||||
std::list<Sensor> getRawData(int id) const;
|
||||
bool isCommonSignatureUsed() const {return _commonSignatureUsed;}
|
||||
std::set<int> getAllSignatureIds() const;
|
||||
bool memoryChanged() const {return _memoryChanged;}
|
||||
@@ -105,20 +129,24 @@ public:
|
||||
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>());
|
||||
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 addSignatureToStm(Signature * signature,
|
||||
const std::list<Actuator> & actuators = std::list<Actuator>());
|
||||
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>());
|
||||
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);
|
||||
@@ -127,7 +155,9 @@ protected:
|
||||
|
||||
private:
|
||||
virtual void copyData(const Signature * from, Signature * to) = 0;
|
||||
virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false) = 0;
|
||||
virtual Signature * createSignature(int id,
|
||||
const std::list<Sensor> & sensors,
|
||||
bool keepRawData=false) = 0;
|
||||
|
||||
void createVirtualSignature(Signature ** signature);
|
||||
void cleanGraph(const Node * root);
|
||||
@@ -0,0 +1,134 @@
|
||||
/*
|
||||
* Micro.h
|
||||
*
|
||||
* Created on: Mar 5, 2012
|
||||
* Author: MatLab
|
||||
*/
|
||||
|
||||
#ifndef MICRO_H_
|
||||
#define MICRO_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <utilite/UThreadNode.h>
|
||||
#include <utilite/UTimer.h>
|
||||
#include <utilite/UEvent.h>
|
||||
#include <utilite/ULogger.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
class UAudioRecorder;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class MicroEvent :
|
||||
public UEvent
|
||||
{
|
||||
public:
|
||||
enum Type {
|
||||
kTypeFrame,
|
||||
kTypeFrameFreq,
|
||||
kTypeFrameFreqSqrdMagn,
|
||||
kTypeNoMoreFrames
|
||||
};
|
||||
|
||||
public:
|
||||
// kTypeNoMoreFrames constructor
|
||||
MicroEvent(int microId = 0) :
|
||||
UEvent(kTypeNoMoreFrames),
|
||||
_sampleSize(0),
|
||||
_microId(microId)
|
||||
{
|
||||
}
|
||||
// kTypeFrame constructor
|
||||
MicroEvent(const cv::Mat & frame,
|
||||
int sampleSize,
|
||||
int fs,
|
||||
int channels,
|
||||
int microId = 0) :
|
||||
UEvent(kTypeFrame),
|
||||
_frame(frame),
|
||||
_sampleSize(sampleSize),
|
||||
_microId(microId)
|
||||
{
|
||||
}
|
||||
// kTypeFrameFreq and kTypeFrameFreqSqrdMagn constructors
|
||||
MicroEvent(Type frameType,
|
||||
const cv::Mat & frameFreq,
|
||||
int fs,
|
||||
int channels,
|
||||
int microId = 0) :
|
||||
UEvent(frameType),
|
||||
_frame(frameFreq),
|
||||
_sampleSize(sizeof(float)),
|
||||
_microId(microId)
|
||||
{
|
||||
UASSERT(frameType == kTypeFrameFreqSqrdMagn || frameType == kTypeFrameFreq);
|
||||
}
|
||||
|
||||
int type() const {return this->getCode();}
|
||||
const cv::Mat & frame() const {return _frame;}
|
||||
int sampleSize() const {return _sampleSize;}
|
||||
int microId() const {return _microId;}
|
||||
virtual ~MicroEvent() {}
|
||||
virtual std::string getClassName() const {return std::string("MicroEvent");}
|
||||
|
||||
private:
|
||||
cv::Mat _frame;
|
||||
int _sampleSize; // bytes
|
||||
int _fs; //sampling rate
|
||||
int _microId;
|
||||
};
|
||||
|
||||
class RTABMAP_EXP Micro : public UThreadNode
|
||||
{
|
||||
typedef float fftwf_complex[2];
|
||||
public:
|
||||
Micro(MicroEvent::Type eventType,
|
||||
int deviceId,
|
||||
int fs,
|
||||
int frameLength,
|
||||
int channels,
|
||||
int bytesPerSample,
|
||||
int id = 0);
|
||||
Micro(MicroEvent::Type eventType,
|
||||
const std::string & path,
|
||||
bool simulateFrameRate,
|
||||
int frameLength,
|
||||
int id = 0,
|
||||
bool playWhileRecording = false);
|
||||
virtual ~Micro();
|
||||
|
||||
bool init();
|
||||
void stop(); // same as kill() but handle the case where underlying recorder is running and not the micro.
|
||||
void startRecorder(); // must only be used if Micro::start() is not used
|
||||
|
||||
cv::Mat getFrame();
|
||||
cv::Mat getFrame(cv::Mat & frameFreq, bool sqrdMagn = false);
|
||||
|
||||
int fs();
|
||||
int bytesPerSample();
|
||||
int channels();
|
||||
int nfft();
|
||||
|
||||
protected:
|
||||
virtual void mainLoopBegin();
|
||||
virtual void mainLoop();
|
||||
virtual void mainLoopKill();
|
||||
|
||||
private:
|
||||
MicroEvent::Type _eventType;
|
||||
UAudioRecorder* _recorder;
|
||||
bool _simulateFreq;
|
||||
UTimer _timer;
|
||||
std::vector<float> _window;
|
||||
std::vector<float> _in;
|
||||
fftwf_complex * _out; // fftwf_complex
|
||||
void * _p; // fftwf_plan
|
||||
int _id;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* MICRO_H_ */
|
||||
@@ -70,6 +70,7 @@ protected:
|
||||
|
||||
/////////////////////////
|
||||
// KdTreeNN
|
||||
// FIXME KdTreeNN seems broken, it does not give same results as naive and FLANN
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP KdTreeNN : public NearestNeighbor
|
||||
{
|
||||
@@ -92,12 +93,11 @@ public:
|
||||
int knn = 1,
|
||||
int emax = 64) const;
|
||||
|
||||
virtual bool isDist64F() const {return true;}
|
||||
virtual bool isDist64F() const {return false;}
|
||||
virtual bool isDistSquared() const {return false;}
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
private:
|
||||
CvFeatureTree * _tree;
|
||||
CvMat _dataMat;
|
||||
cv::KDTree _tree;
|
||||
};
|
||||
|
||||
|
||||
@@ -122,13 +122,13 @@ class RTABMAP_EXP Parameters
|
||||
// Rtabmap parameters
|
||||
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0); // None 0, Similarity 1, Epipolar 2
|
||||
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true); // Publishing statistics
|
||||
RTABMAP_PARAM(Rtabmap, PublishImages, bool, true); // Publishing images
|
||||
RTABMAP_PARAM(Rtabmap, PublishRawData, bool, true); // Publishing raw data
|
||||
RTABMAP_PARAM(Rtabmap, PublishPdf, bool, true); // Publishing pdf
|
||||
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true); // Publishing likelihood
|
||||
RTABMAP_PARAM(Rtabmap, RetrievalThr, float, 0.0); // Reactivation threshold
|
||||
RTABMAP_PARAM(Rtabmap, TimeThr, float, 700.0); // Maximum time allowed for the detector (ms) (0 means infinity)
|
||||
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0); // Maximum signatures in the Working Memory (ms) (0 means infinity)
|
||||
RTABMAP_PARAM(Rtabmap, SMStateBufferSize, int, 1); // Data buffer size (0 min inf)
|
||||
RTABMAP_PARAM(Rtabmap, SMStateBufferSize, int, 0); // Data buffer size (0 min inf)
|
||||
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory()); // Working directory
|
||||
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2); // Maximum locations retrieved at the same time from LTM
|
||||
RTABMAP_PARAM(Rtabmap, SelectionNeighborhoodSummationUsed, bool, false); // Neighborhood summation for hypothesis selection
|
||||
@@ -136,15 +136,16 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(Rtabmap, ActionsSentRejectHyp, bool, true); // Actions sent also on rejected hypotheses (on decreasing hypotheses)
|
||||
RTABMAP_PARAM(Rtabmap, ConfidenceThr, float, 0.0); // Actions are not sent when the loop closure hypothesis is under the confidence threshold
|
||||
RTABMAP_PARAM(Rtabmap, LikelihoodStdDevRemoved, bool, true); // Remove std dev on likelihood normalization.
|
||||
RTABMAP_PARAM(Rtabmap, LikelihoodNullValuesIgnored, bool, true); // Ignore null values on likelihood normalization
|
||||
|
||||
// Hypotheses selection
|
||||
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.15); // Loop closing threshold
|
||||
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0.0); // The loop closure hypothesis must be over LoopRatio x lastHypothesisValue
|
||||
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0.9); // The loop closure hypothesis must be over LoopRatio x lastHypothesisValue
|
||||
|
||||
// Memory
|
||||
RTABMAP_PARAM(Mem, SimilarityThr, float, 0.20); // Similarity between the last signature and neighbor
|
||||
RTABMAP_PARAM(Mem, SimilarityOnlyLast, bool, false); // Only compare to the last signature in STM, otherwise it compares to all signatures in STM
|
||||
RTABMAP_PARAM(Mem, RawDataKept, bool, false); // Keep raw data
|
||||
RTABMAP_PARAM(Mem, RawDataKept, bool, true); // Keep raw data
|
||||
RTABMAP_PARAM(Mem, MaxStMemSize, unsigned int, 30); // Short-time memory size
|
||||
RTABMAP_PARAM(Mem, CommonSignatureUsed, bool, true); // A common signature/virtual place is automatically updated with id -1
|
||||
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true);
|
||||
@@ -165,7 +166,6 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(Kp, MaxLeafs, int, 64); // Maximum number of leafs checked (when using kd-trees)
|
||||
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0); // Surf detector 0, Star detector 1, SIFT detector 2, FAST detector 3
|
||||
RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0); // kDescriptorSurf=0, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef
|
||||
RTABMAP_PARAM(Kp, UsingAdaptiveResponseThr, bool, false);
|
||||
RTABMAP_PARAM(Kp, ReactivatedWordsComparedToNewWords, bool, true); //Reactivated words are compared to the last words added in the dictionary (which are not indexed)
|
||||
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false); // Use of the td-idf strategy to compute the likelihood
|
||||
RTABMAP_PARAM(Kp, Parallelized, bool, true); // If the dictionary update and signature creation were parallelized
|
||||
@@ -174,11 +174,13 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM_STR(Kp, DictionaryPath, ""); // Path of the pre-computed dictionary
|
||||
|
||||
// SM memory
|
||||
RTABMAP_PARAM(SM, PublishMasks, bool, true); // Publishing motion masks
|
||||
RTABMAP_PARAM(SM, PublishMasks, bool, false); // Publishing motion masks
|
||||
RTABMAP_PARAM(SM, MotionMaskUsed, bool, false); // Use motion mask
|
||||
RTABMAP_PARAM(SM, LogPolarUsed, bool, false); // Use log-polar images
|
||||
RTABMAP_PARAM(SM, VotingSchemeUsed, bool, false); // Use likelihood voting scheme
|
||||
RTABMAP_PARAM(SM, ColorTable, int, 8); // Color table size 0=8, 1=16, 2=32, 3=64, 4=128, 5=256, 6=512, 7=1024, 8=65536
|
||||
RTABMAP_PARAM(SM, AudioDBThreshold, float, 0.0f); // Audio dB threshold
|
||||
RTABMAP_PARAM(SM, AudioDBIndexing, bool, true); // dB (decibel) indexing (otherwise it's squared magnitude indexing)
|
||||
RTABMAP_PARAM(SM, MagnitudeInvariant, bool, false); // Make audio signature magnitude-invariant
|
||||
|
||||
//Database
|
||||
RTABMAP_PARAM(Db, MinSignaturesToSave, int, 20); // Minimum signatures needed in the trash to save them (empty trash thread)
|
||||
@@ -195,11 +197,14 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(SURF, HessianThreshold, float, 150.0);
|
||||
RTABMAP_PARAM(SURF, Octaves, int, 4);
|
||||
RTABMAP_PARAM(SURF, OctaveLayers, int, 2);
|
||||
RTABMAP_PARAM(SURF, GpuVersion, bool, false);
|
||||
RTABMAP_PARAM(SURF, Upright, bool, false); // U-SURF
|
||||
RTABMAP_PARAM(SURF, GpuVersion, bool, false);
|
||||
|
||||
RTABMAP_PARAM(SIFT, Threshold, double, 0.006667); // true=128, false=64
|
||||
RTABMAP_PARAM(SIFT, NFeatures, int, 0);
|
||||
RTABMAP_PARAM(SIFT, NOctaveLayers, int, 3);
|
||||
RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04);
|
||||
RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
|
||||
RTABMAP_PARAM(SIFT, Sigma, double, 1.6);
|
||||
|
||||
RTABMAP_PARAM(FAST, Threshold, int, 10);
|
||||
RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true);
|
||||
|
||||
@@ -17,8 +17,8 @@
|
||||
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef CTABMAP_H_
|
||||
#define CTABMAP_H_
|
||||
#ifndef RTABMAP_H_
|
||||
#define RTABMAP_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
@@ -29,6 +29,8 @@
|
||||
#include "utilite/UVariant.h"
|
||||
#include "rtabmap/core/RtabmapEvent.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include "rtabmap/core/Sensor.h"
|
||||
#include "rtabmap/core/Actuator.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <list>
|
||||
#include <stack>
|
||||
@@ -42,7 +44,6 @@ class Signature;
|
||||
class HypVerificator;
|
||||
class Memory;
|
||||
class BayesFilter;
|
||||
class SMState;
|
||||
|
||||
class RTABMAP_EXP Rtabmap :
|
||||
public UThreadNode,
|
||||
@@ -57,7 +58,8 @@ public:
|
||||
kStateDumpingMemory,
|
||||
kStateDumpingPrediction,
|
||||
kStateGeneratingGraph,
|
||||
kStateDeletingMemory
|
||||
kStateDeletingMemory,
|
||||
kStateCleanSensorsBuffer
|
||||
};
|
||||
|
||||
enum VhStrategy {kVhNone, kVhSim, kVhEpipolar, kVhUndef};
|
||||
@@ -76,18 +78,19 @@ public:
|
||||
Rtabmap();
|
||||
virtual ~Rtabmap();
|
||||
|
||||
// ownership is transferred
|
||||
void process(SMState * data);
|
||||
void process(const std::list<Sensor> & data);
|
||||
void process(const Sensor & data); // for convenience when only one sensor is used
|
||||
void dumpData();
|
||||
|
||||
void init(const ParametersMap & param);
|
||||
void init(const char * configFile = 0);
|
||||
void clearBufferedSensors();
|
||||
|
||||
const std::string & getWorkingDir() const {return _wDir;}
|
||||
int getLoopClosureId() const;
|
||||
int getReactivatedId() const;
|
||||
int getLastSignatureId() const;
|
||||
const std::list<std::vector<float> > & getActions() const {return _actions;}
|
||||
const std::list<Actuator> & getActuator() const {return _actuators;}
|
||||
std::list<int> getWorkingMem() const;
|
||||
std::set<int> getStMem() const;
|
||||
std::map<int, int> getWeights() const;
|
||||
@@ -110,12 +113,12 @@ protected:
|
||||
|
||||
private:
|
||||
virtual void mainLoop();
|
||||
virtual void killCleanup();
|
||||
virtual void startInit();
|
||||
virtual void mainLoopKill();
|
||||
virtual void mainLoopBegin();
|
||||
void process();
|
||||
void resetMemory(bool dbOverwritten = false);
|
||||
void addSMState(SMState * data); // ownership is transferred
|
||||
SMState * getSMState();
|
||||
void addSensorimotor(const std::list<Sensor> & sensors, const std::list<Actuator> & actuators);
|
||||
void getSensorimotor(std::list<Sensor> & sensors, std::list<Actuator> & actuators);
|
||||
void setupLogFiles(bool overwrite = false);
|
||||
void releaseAllStrategies();
|
||||
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
|
||||
@@ -125,14 +128,14 @@ private:
|
||||
private:
|
||||
// Modifiable parameters
|
||||
bool _publishStats;
|
||||
bool _publishImages;
|
||||
bool _publishRawData;
|
||||
bool _publishPdf;
|
||||
bool _publishLikelihood;
|
||||
bool _publishKeypoints;
|
||||
bool _publishMasks;
|
||||
float _maxTimeAllowed; // in ms
|
||||
unsigned int _maxMemoryAllowed; // signatures count in WM
|
||||
int _smStateBufferMaxSize;
|
||||
int _sensorsBufferMaxSize;
|
||||
float _loopThr;
|
||||
float _loopRatio;
|
||||
float _retrievalThr;
|
||||
@@ -142,20 +145,21 @@ private:
|
||||
bool _actionsSentRejectHyp;
|
||||
float _confidenceThr;
|
||||
bool _likelihoodStdDevRemoved;
|
||||
bool _likelihoodNullValuesIgnored;
|
||||
|
||||
int _lcHypothesisId;
|
||||
int _reactivateId;
|
||||
float _lastLcHypothesisValue;
|
||||
int _lastLoopClosureId;
|
||||
std::list<std::vector<float> > _actions;
|
||||
std::list<Actuator> _actuators;
|
||||
|
||||
UMutex _stateMutex;
|
||||
std::stack<State> _state;
|
||||
std::stack<ParametersMap> _stateParam;
|
||||
|
||||
std::list<SMState *> _smStateBuffer;
|
||||
UMutex _smStateBufferMutex;
|
||||
USemaphore _newSMStateSem;
|
||||
std::list<std::pair<std::list<Sensor>, std::list<Actuator> > > _sensorimotorBuffer;
|
||||
UMutex _sensorimotorMutex;
|
||||
USemaphore _sensorimotorAdded;
|
||||
|
||||
// Abstract classes containing all loop closure
|
||||
// strategies for a type of signature or configuration.
|
||||
@@ -171,6 +175,6 @@ private:
|
||||
std::string _graphFileName;
|
||||
};
|
||||
|
||||
#endif /* CTABMAP_H_ */
|
||||
#endif /* RTABMAP_H_ */
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -22,11 +22,13 @@
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "utilite/UEvent.h"
|
||||
#include "rtabmap/core/Sensor.h"
|
||||
#include "rtabmap/core/Actuator.h"
|
||||
#include <utilite/UEvent.h>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include "utilite/ULogger.h"
|
||||
#include <utilite/ULogger.h>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
@@ -63,6 +65,7 @@ class RTABMAP_EXP Statistics
|
||||
RTABMAP_STATS(Memory, Signatures_removed,);
|
||||
RTABMAP_STATS(Memory, Signatures_retrieved,);
|
||||
RTABMAP_STATS(Memory, Images_buffered,);
|
||||
RTABMAP_STATS(Memory, Similarities_map,);
|
||||
|
||||
RTABMAP_STATS(Timing, Memory_update, ms);
|
||||
RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
|
||||
@@ -90,7 +93,6 @@ public:
|
||||
|
||||
public:
|
||||
Statistics();
|
||||
Statistics(const Statistics & s);
|
||||
virtual ~Statistics();
|
||||
|
||||
// name format = "Grp/Name/unit"
|
||||
@@ -100,11 +102,9 @@ public:
|
||||
void setExtended(bool extended) {_extended = extended;}
|
||||
void setRefImageId(int refImageId) {_refImageId = refImageId;}
|
||||
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
|
||||
void setActions(const std::list<std::vector<float> > & actions) {_actions = actions;}
|
||||
void setRefImage(IplImage ** refImage);
|
||||
void setRefImage(const IplImage * refImage);
|
||||
void setLoopClosureImage(IplImage ** loopClosureImage);
|
||||
void setLoopClosureImage(const IplImage * loopClosureImage);
|
||||
void setActuators(const std::list<Actuator> & actuators) {_actuators = actuators;}
|
||||
void setRefRawData(const std::list<Sensor> & refRawData);
|
||||
void setLoopClosureRawData(const std::list<Sensor> & loopClosureRawData);
|
||||
void setWeights(const std::map<int, int> & weights) {_weights = weights;}
|
||||
void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
|
||||
void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
|
||||
@@ -117,9 +117,9 @@ public:
|
||||
bool extended() const {return _extended;}
|
||||
int refImageId() const {return _refImageId;}
|
||||
int loopClosureId() const {return _loopClosureId;}
|
||||
const std::list<std::vector<float> > & getActions() const {return _actions;}
|
||||
const IplImage * refImage() const {return _refImage;}
|
||||
const IplImage * loopClosureImage() const {return _loopClosureImage;}
|
||||
const std::list<Actuator> & getActuators() const {return _actuators;}
|
||||
const std::list<Sensor> & refRawData() const {return _refRawData;}
|
||||
const std::list<Sensor> & loopClosureRawData() const {return _loopClosureRawData;}
|
||||
const std::map<int, int> & weights() const {return _weights;}
|
||||
const std::map<int, float> & posterior() const {return _posterior;}
|
||||
const std::map<int, float> & likelihood() const {return _likelihood;}
|
||||
@@ -130,20 +130,17 @@ public:
|
||||
|
||||
const std::map<std::string, float> & data() const {return _data;}
|
||||
|
||||
|
||||
Statistics & operator=(const Statistics & s);
|
||||
|
||||
private:
|
||||
int _extended; // 0 -> only loop closure and last signature ID fields are filled
|
||||
|
||||
int _refImageId;
|
||||
int _loopClosureId;
|
||||
|
||||
std::list<std::vector<float> > _actions;
|
||||
std::list<Actuator> _actuators;
|
||||
|
||||
// extended data start here...
|
||||
IplImage * _refImage; // Released by the event destructor
|
||||
IplImage * _loopClosureImage; // Released by the event destructor
|
||||
std::list<Sensor> _refRawData;
|
||||
std::list<Sensor> _loopClosureRawData;
|
||||
|
||||
std::map<int, int> _weights;
|
||||
std::map<int, float> _posterior;
|
||||
@@ -191,7 +188,8 @@ public:
|
||||
kCmdDumpMemory,
|
||||
kCmdDumpPrediction,
|
||||
kCmdGenerateGraph,
|
||||
kCmdDeleteMemory};
|
||||
kCmdDeleteMemory,
|
||||
kCmdCleanSensorsBuffer};
|
||||
public:
|
||||
RtabmapEventCmd(Cmd cmd) :
|
||||
UEvent(0),
|
||||
|
||||
@@ -20,16 +20,12 @@
|
||||
#ifndef RTABMAPEXP_H
|
||||
#define RTABMAPEXP_H
|
||||
|
||||
#ifdef WIN32
|
||||
#ifdef RTABMAP_EXPORTS
|
||||
#if defined(_WIN32)
|
||||
#if defined(rtabmap_corelib_EXPORTS) || defined(rtabmap_guilib_EXPORTS)
|
||||
#define RTABMAP_EXP __declspec( dllexport )
|
||||
#else
|
||||
#ifdef RTABMAP_EXPORTS_STATIC
|
||||
#define RTABMAP_EXP
|
||||
#else
|
||||
#define RTABMAP_EXP __declspec( dllimport )
|
||||
#endif
|
||||
#endif
|
||||
#define RTABMAP_EXP __declspec( dllimport )
|
||||
#endif
|
||||
#else
|
||||
#define RTABMAP_EXP
|
||||
#endif
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "Memory.h"
|
||||
#include "rtabmap/core/Memory.h"
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
@@ -35,28 +35,21 @@ 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);
|
||||
virtual Signature * createSignature(int id, const std::list<Sensor> & sensors, bool keepRawData=false);
|
||||
|
||||
private:
|
||||
bool _useLogPolar;
|
||||
bool _useVotingScheme;
|
||||
ColorTable * _colorTable;
|
||||
bool _useMotionMask;
|
||||
std::vector<std::map<int, std::set<int> > > _dictionary;
|
||||
float _dBThreshold;
|
||||
bool _dBIndexing;
|
||||
bool _magnitudeInvariant;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -1,157 +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 SMPAIRVARIANT_H_
|
||||
#define SMPAIRVARIANT_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/highgui/highgui_c.h>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <utilite/ULogger.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
// SensoriMotor state
|
||||
class SMState
|
||||
{
|
||||
public:
|
||||
// Constructor 0 : ownership is transferred
|
||||
SMState(IplImage * image = 0) :
|
||||
_image(image)
|
||||
{}
|
||||
// Constructor 1
|
||||
SMState(const cv::Mat & sensors, const std::list<std::vector<float> > & actuators) :
|
||||
_sensors(sensors),
|
||||
_actuators(actuators),
|
||||
_image(0)
|
||||
{}
|
||||
// Constructor 2 : for convenience with ROS conversion...
|
||||
// Sensors and actuators vectors will be split into a list with smaller vectors of length sensorStep and actuatorStep respectively.
|
||||
SMState(const std::vector<float> & sensors, int sensorStep, const std::vector<float> & actuators, int actuatorStep) :
|
||||
_image(0)
|
||||
{
|
||||
if(sensorStep && sensors.size() % sensorStep != 0)
|
||||
{
|
||||
UERROR("Sensors must all have the same length.");
|
||||
}
|
||||
if(actuatorStep && actuators.size() % actuatorStep != 0)
|
||||
{
|
||||
UERROR("Actuators must all have the same length.");
|
||||
}
|
||||
for(unsigned int i=0; i<sensors.size() && i<i+sensorStep; i+=sensorStep)
|
||||
{
|
||||
_sensors.push_back(std::vector<float>(sensors.data()+i, sensors.data()+i+sensorStep));
|
||||
}
|
||||
for(unsigned int i=0; i<actuators.size() && i<i+actuatorStep; i+=actuatorStep)
|
||||
{
|
||||
_actuators.push_back(std::vector<float>(actuators.data()+i, actuators.data()+i+actuatorStep));
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~SMState()
|
||||
{
|
||||
if(_image)
|
||||
{
|
||||
cvReleaseImage(&_image);
|
||||
}
|
||||
}
|
||||
|
||||
const IplImage * getImage() const {return _image;}
|
||||
const std::vector<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
|
||||
const cv::Mat & getSensors() const {return _sensors;}
|
||||
const std::list<std::vector<float> > & getActuators() const {return _actuators;}
|
||||
void setSensors(const cv::Mat & sensors) {_sensors=sensors;}
|
||||
void setActuators(const std::list<std::vector<float> > & actuators) {_actuators=actuators;}
|
||||
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
|
||||
|
||||
//ownership is transferred
|
||||
void setImage(IplImage * image)
|
||||
{
|
||||
if(_image)
|
||||
{
|
||||
cvReleaseImage(&_image);
|
||||
}
|
||||
_image = image;
|
||||
}
|
||||
|
||||
void getSensorsMerged(std::vector<float> & sensors, int & step) const
|
||||
{
|
||||
sensors.clear();
|
||||
step = 0;
|
||||
if(!_sensors.empty())
|
||||
{
|
||||
// here we assume that all sensors have the same length
|
||||
step = _sensors.cols;
|
||||
sensors = std::vector<float>(_sensors.total());
|
||||
for(int i=0; i<_sensors.rows; ++i)
|
||||
{
|
||||
const float * rowFl = _sensors.ptr<float>(i);
|
||||
memcpy(&sensors[i*_sensors.cols], rowFl, _sensors.cols*sizeof(float));
|
||||
}
|
||||
}
|
||||
}
|
||||
void getActuatorsMerged(std::vector<float> & actuators, int & step) const
|
||||
{
|
||||
actuators.clear();
|
||||
step = 0;
|
||||
if(_actuators.size())
|
||||
{
|
||||
// here we assume that all sensors have the same length
|
||||
step = _actuators.front().size();
|
||||
for(std::list<std::vector<float> >::const_iterator iter = _actuators.begin();
|
||||
iter != _actuators.end();
|
||||
++iter)
|
||||
{
|
||||
actuators.insert(actuators.end(), iter->begin(), iter->end());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
cv::Mat _sensors; // descriptors
|
||||
std::list<std::vector<float> > _actuators;
|
||||
IplImage * _image;
|
||||
std::vector<cv::KeyPoint> _keypoints;
|
||||
};
|
||||
|
||||
// Sensorimotor state event
|
||||
// Take ownership of the state
|
||||
class SMStateEvent : public UEvent
|
||||
{
|
||||
public:
|
||||
SMStateEvent(SMState * state) :
|
||||
UEvent(0),
|
||||
_state(state) {}
|
||||
|
||||
virtual ~SMStateEvent() {if(_state) delete _state;}
|
||||
const SMState * getSMState() const {return _state;}
|
||||
SMState * getSMStateOwnership() {SMState * state = _state; _state=0; return state;}
|
||||
virtual std::string getClassName() const {return "SMStateEvent";} // TODO : macro?
|
||||
|
||||
private:
|
||||
SMState * _state;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* SMPAIRVARIANT_H_ */
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
|
||||
*
|
||||
* This file is part of RTAB-Map.
|
||||
*
|
||||
* RTAB-Map is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* RTAB-Map is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef SENSOR_H_
|
||||
#define SENSOR_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <utilite/UEvent.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class Sensor
|
||||
{
|
||||
public:
|
||||
enum Type{kTypeImage=0, kTypeImageFeatures2d, kTypeAudio, kTypeAudioFreq, kTypeAudioFreqSqrdMagn, kTypeJointState, kTypeTwist, kTypeNotSpecified};
|
||||
public:
|
||||
Sensor(const cv::Mat & data, Type type, int num = 0) :
|
||||
_data(data),
|
||||
_type(type),
|
||||
_num(num)
|
||||
{}
|
||||
Sensor(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, int num = 0) :
|
||||
_data(descriptors),
|
||||
_type(kTypeImageFeatures2d),
|
||||
_num(num),
|
||||
_keypoints(keypoints)
|
||||
{}
|
||||
const cv::Mat & data() const {return _data;}
|
||||
int type() const {return _type;}
|
||||
int num() const {return _num;}
|
||||
virtual ~Sensor() {};
|
||||
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
|
||||
const std::vector<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
|
||||
private:
|
||||
cv::Mat _data;
|
||||
int _type;
|
||||
int _num; // sensor number
|
||||
std::vector<cv::KeyPoint> _keypoints; // for convenience with kTypeImageFeatures
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* SENSOR_H_ */
|
||||
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* SensorimotorEvent.h
|
||||
*
|
||||
* Created on: 2012-05-27
|
||||
* Author: mathieu
|
||||
*/
|
||||
|
||||
#ifndef SENSORIMOTOREVENT_H_
|
||||
#define SENSORIMOTOREVENT_H_
|
||||
|
||||
#include "rtabmap/core/Sensor.h"
|
||||
#include "rtabmap/core/Actuator.h"
|
||||
#include <utilite/UEvent.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class SensorimotorEvent : public UEvent
|
||||
{
|
||||
public:
|
||||
enum Type {
|
||||
kTypeData,
|
||||
kTypeNoMoreData
|
||||
};
|
||||
|
||||
public:
|
||||
SensorimotorEvent() :
|
||||
UEvent(kTypeNoMoreData) {}
|
||||
SensorimotorEvent(const std::list<Sensor> & sensors,
|
||||
const std::list<Actuator> & actuators) :
|
||||
UEvent(kTypeData),
|
||||
sensors_(sensors),
|
||||
actuators_(actuators) {}
|
||||
virtual ~SensorimotorEvent() {}
|
||||
int type() const {return this->getCode();}
|
||||
virtual std::string getClassName() const {return "SensorimotorEvent";}
|
||||
const std::list<Sensor> & sensors() const {return sensors_;}
|
||||
const std::list<Actuator> & actuators() const {return actuators_;}
|
||||
private:
|
||||
std::list<Sensor> sensors_;
|
||||
std::list<Actuator> actuators_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif /* SENSORIMOTOREVENT_H_ */
|
||||
@@ -24,6 +24,8 @@
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include "rtabmap/core/Sensor.h"
|
||||
#include "rtabmap/core/Actuator.h"
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
@@ -37,21 +39,24 @@ namespace rtabmap
|
||||
class RTABMAP_EXP NeighborLink
|
||||
{
|
||||
public:
|
||||
NeighborLink(int id, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >(), const std::vector<int> & baseIds = std::vector<int>()) :
|
||||
_id(id),
|
||||
_actions(actions),
|
||||
NeighborLink(int toId, const std::vector<int> & baseIds = std::vector<int>(), const std::list<Actuator> & actuators = std::list<Actuator>(), int actuatorId = 0) :
|
||||
_toId(toId),
|
||||
_actuatorId(actuatorId),
|
||||
_actuators(actuators),
|
||||
_baseIds(baseIds)
|
||||
{}
|
||||
virtual ~NeighborLink() {}
|
||||
|
||||
int id() const {return _id;}
|
||||
const std::list<std::vector<float> > & actions() const {return _actions;}
|
||||
int toId() const {return _toId;}
|
||||
int actuatorId() const {return _actuatorId;}
|
||||
const std::list<Actuator> & actuators() const {return _actuators;}
|
||||
const std::vector<int> & baseIds() const {return _baseIds;}
|
||||
bool updateIds(int idFrom, int idTo);
|
||||
|
||||
private:
|
||||
int _id;
|
||||
std::list<std::vector<float> > _actions;
|
||||
int _toId;
|
||||
int _actuatorId;
|
||||
std::list<Actuator> _actuators;
|
||||
std::vector<int> _baseIds; // first is the nearest
|
||||
};
|
||||
|
||||
@@ -60,10 +65,6 @@ typedef std::multimap<int, NeighborLink> NeighborsMultiMap;
|
||||
|
||||
class RTABMAP_EXP Signature
|
||||
{
|
||||
public:
|
||||
static CvMat * compressImage(const IplImage * image);
|
||||
static IplImage * decompressImage(const CvMat * imageCompressed);
|
||||
|
||||
public:
|
||||
virtual ~Signature();
|
||||
|
||||
@@ -72,29 +73,35 @@ public:
|
||||
*/
|
||||
virtual float compareTo(const Signature * signature) const = 0;
|
||||
virtual bool isBadSignature() const = 0;
|
||||
virtual std::string signatureType() const = 0;
|
||||
virtual std::string nodeType() const = 0;
|
||||
|
||||
const IplImage * getImage() const;
|
||||
void setImage(const IplImage * image);
|
||||
void setRawData(const std::list<Sensor> & rawData) {_rawData = rawData;}
|
||||
const std::list<Sensor> & getRawData() const {return _rawData;}
|
||||
|
||||
int id() const {return _id;}
|
||||
|
||||
void addNeighbors(const NeighborsMultiMap & neighbors);
|
||||
void addNeighbor(const NeighborLink & neighbor);
|
||||
void removeNeighbor(int neighborId) {if(_neighbors.erase(neighborId)) _neighborsModified = true;}
|
||||
void removeNeighbor(int neighborId) {
|
||||
if(_neighbors.erase(neighborId))
|
||||
_neighborsModified = true;
|
||||
_neighborsWithActuators.erase(neighborId);
|
||||
_neighborsAll.erase(neighborId);}
|
||||
bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
|
||||
void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
|
||||
void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_modified=true;}
|
||||
void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_modified=true;}
|
||||
void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_modified=true;}
|
||||
void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
|
||||
void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_neighborsModified=true;}
|
||||
void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
|
||||
bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
|
||||
void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_modified=true;}
|
||||
void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_modified=true;}
|
||||
void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
|
||||
void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_neighborsModified=true;}
|
||||
void setSaved(bool saved) {_saved = saved;}
|
||||
void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
|
||||
void changeNeighborIds(int idFrom, int idTo);
|
||||
|
||||
const NeighborsMultiMap & getNeighbors() const {return _neighbors;}
|
||||
const std::set<int> & getNeighborsWithActuators() const {return _neighborsWithActuators;}
|
||||
const std::set<int> & getNeighborsAll() const {return _neighborsAll;}
|
||||
int getWeight() const {return _weight;}
|
||||
const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
|
||||
const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
|
||||
@@ -103,15 +110,18 @@ public:
|
||||
bool isNeighborsModified() const {return _neighborsModified;}
|
||||
|
||||
protected:
|
||||
Signature(int id, const IplImage * image = 0, bool keepImage = false);
|
||||
Signature(int id);
|
||||
Signature(int id, const std::list<Sensor> & rawData);
|
||||
|
||||
private:
|
||||
int _id;
|
||||
NeighborsMultiMap _neighbors; // id, neighborLink
|
||||
std::set<int> _neighborsWithActuators; // Hack, to increase efficiency of Memory::getNeighborIds()
|
||||
std::set<int> _neighborsAll; // Hack, to increase efficiency of Memory::getNeighborIds()
|
||||
int _weight;
|
||||
std::set<int> _loopClosureIds;
|
||||
std::set<int> _childLoopClosureIds;
|
||||
IplImage * _image;
|
||||
std::list<Sensor> _rawData;
|
||||
bool _saved; // If it's saved to bd
|
||||
bool _modified;
|
||||
bool _neighborsModified; // Optimization when updating signatures in database
|
||||
@@ -125,18 +135,20 @@ class RTABMAP_EXP KeypointSignature :
|
||||
public Signature
|
||||
{
|
||||
public:
|
||||
KeypointSignature(int id);
|
||||
KeypointSignature(
|
||||
const std::multimap<int, cv::KeyPoint> & words,
|
||||
int id);
|
||||
KeypointSignature(
|
||||
const std::multimap<int, cv::KeyPoint> & words,
|
||||
int id,
|
||||
const IplImage * image = 0,
|
||||
bool keepRawData = false);
|
||||
KeypointSignature(int id);
|
||||
const std::list<Sensor> & sensors);
|
||||
|
||||
virtual ~KeypointSignature();
|
||||
|
||||
virtual float compareTo(const Signature * signature) const;
|
||||
virtual bool isBadSignature() const;
|
||||
virtual std::string signatureType() const {return "KeypointSignature";};
|
||||
virtual std::string nodeType() const {return "KeypointSignature";};
|
||||
|
||||
void removeAllWords();
|
||||
void removeWord(int wordId);
|
||||
@@ -164,27 +176,24 @@ class RTABMAP_EXP SMSignature :
|
||||
{
|
||||
public:
|
||||
SMSignature(
|
||||
const std::vector<int> & sensors,
|
||||
const std::vector<unsigned char> & motionMask,
|
||||
const std::list<std::vector<int> > & data,
|
||||
int id);
|
||||
SMSignature(
|
||||
const std::list<std::vector<int> > & data,
|
||||
int id,
|
||||
const IplImage * image = 0,
|
||||
bool keepRawData = false);
|
||||
const std::list<Sensor> & rawData);
|
||||
SMSignature(int id);
|
||||
|
||||
virtual ~SMSignature();
|
||||
|
||||
virtual float compareTo(const Signature * signature) const;
|
||||
virtual bool isBadSignature() const;
|
||||
virtual std::string signatureType() const {return "SMSignature";};
|
||||
virtual std::string nodeType() const {return "SMSignature";};
|
||||
|
||||
void setSensors(const std::vector<int> & sensors) {_sensors= sensors;}
|
||||
const std::vector<int> & getSensors() const {return _sensors;}
|
||||
|
||||
void setMotionMask(const std::vector<unsigned char> & motionMask) {_motionMask= motionMask;}
|
||||
const std::vector<unsigned char> & getMotionMask() const {return _motionMask;}
|
||||
void setSensors(const std::list<std::vector<int> > & data) {_data = data;}
|
||||
const std::list<std::vector<int> > & getData() const {return _data;}
|
||||
private:
|
||||
std::vector<int> _sensors;
|
||||
std::vector<unsigned char> _motionMask;
|
||||
std::list<std::vector<int> > _data;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -21,7 +21,6 @@
|
||||
|
||||
#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>
|
||||
@@ -33,6 +32,7 @@ namespace rtabmap
|
||||
|
||||
class NearestNeighbor;
|
||||
class DBDriver;
|
||||
class VisualWord;
|
||||
|
||||
class RTABMAP_EXP VWDictionary
|
||||
{
|
||||
@@ -68,19 +68,12 @@ 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;}
|
||||
+38
-43
@@ -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;
|
||||
|
||||
@@ -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)
|
||||
|
||||
+222
-335
@@ -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
|
||||
|
||||
@@ -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));
|
||||
|
||||
+47
-41
@@ -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;
|
||||
|
||||
+799
-895
File diff suppressed because it is too large
Load Diff
@@ -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;
|
||||
|
||||
@@ -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 */
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
+103
-141
@@ -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);
|
||||
|
||||
@@ -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())
|
||||
{
|
||||
|
||||
+258
-236
@@ -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());
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
{
|
||||
|
||||
+199
-167
@@ -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");
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
+145
-385
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+120
-167
@@ -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
|
||||
|
||||
+133
-21
@@ -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);
|
||||
|
||||
@@ -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());
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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"
|
||||
|
||||
|
||||
@@ -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;
|
||||
*/
|
||||
|
||||
@@ -1,37 +0,0 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
Tests.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../include
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../src
|
||||
${CPPUNIT_INCLUDE_DIR}
|
||||
${UTILITE_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${SQLITE3_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
SET(LIBRARIES
|
||||
${UTILITE_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
${CPPUNIT_LIBRARY}
|
||||
${SQLITE3_LIBRARY}
|
||||
)
|
||||
|
||||
# Make sure the compiler can find include files from our library.
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
ADD_DEFINITIONS(${CPPUNIT_DEFINITIONS})
|
||||
|
||||
# Add binary called "testAvpdCore" that is built from the source file "main.cpp".
|
||||
# The extension is automatically found.
|
||||
ADD_EXECUTABLE(testCoreLib ${SRC_FILES})
|
||||
TARGET_LINK_LIBRARIES(testCoreLib corelib ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( testCoreLib
|
||||
PROPERTIES
|
||||
OUTPUT_NAME testCoreLib)
|
||||
|
||||
@@ -1,543 +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 <cppunit/config/SourcePrefix.h>
|
||||
#include "Tests.h"
|
||||
|
||||
//Headers for the test BEGIN
|
||||
#include "rtabmap/core/Camera.h"
|
||||
#include "rtabmap/core/Signature.h"
|
||||
#include "VWDictionary.h"
|
||||
#include "rtabmap/core/EpipolarGeometry.h"
|
||||
#include "rtabmap/core/Rtabmap.h"
|
||||
#include "rtabmap/core/SMState.h"
|
||||
#include "VerifyHypotheses.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include "BayesFilter.h"
|
||||
#include "KeypointMemory.h"
|
||||
|
||||
#include "rtabmap/core/RtabmapEvent.h"
|
||||
#include "rtabmap/core/CameraEvent.h"
|
||||
|
||||
#include "rtabmap/core/DBDriverFactory.h"
|
||||
#include "rtabmap/core/DBDriver.h"
|
||||
|
||||
//Util lib
|
||||
#include "utilite/UtiLite.h"
|
||||
|
||||
//Database
|
||||
#include <sqlite3.h>
|
||||
|
||||
#include <string.h>
|
||||
|
||||
//Headers for the test END
|
||||
|
||||
CPPUNIT_TEST_SUITE_REGISTRATION( Tests );
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
void Tests::testAvpd()
|
||||
{
|
||||
printf("\n");
|
||||
UTimer timer;
|
||||
timer.start();
|
||||
//Logger::setType(Logger::kTypeConsole);
|
||||
//Logger::setType(Logger::kTypeFile, "LogTestAvpdCore/testSurfStrategy.txt", false);
|
||||
//Logger::setLevel(Logger::kDebug);
|
||||
//Logger::setType(Logger::kTypeInvalid);
|
||||
|
||||
std::string path = "./data/samples";
|
||||
|
||||
CameraImages camera(path);
|
||||
CPPUNIT_ASSERT_MESSAGE("Camera initialization failed!\n", camera.init());
|
||||
|
||||
/* Create tasks */
|
||||
Rtabmap ctabmap;
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kRtabmapSMStateBufferSize(), "0"));
|
||||
parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "true"));
|
||||
parameters.insert(ParametersPair(Parameters::kMemRawDataKept(), "true"));
|
||||
parameters.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "true"));
|
||||
parameters.insert(ParametersPair(Parameters::kRtabmapTimeThr(), "0"));
|
||||
parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "500"));
|
||||
UDirectory::makeDir("./LogTestAvpdCore/");
|
||||
ctabmap.setWorkingDirectory("./LogTestAvpdCore/");
|
||||
ctabmap.init(parameters);
|
||||
|
||||
|
||||
/* Start thread's task */
|
||||
SMState * smState = 0;
|
||||
|
||||
smState = camera.takeSMState();
|
||||
int imgCount = 0;
|
||||
while(smState)
|
||||
{
|
||||
++imgCount;
|
||||
printf("Processing image %d/84...\n", imgCount);
|
||||
ctabmap.process(smState);
|
||||
smState = camera.takeSMState();
|
||||
}
|
||||
if(smState)
|
||||
{
|
||||
delete smState;
|
||||
smState = 0;
|
||||
}
|
||||
|
||||
CPPUNIT_ASSERT(imgCount == 84);
|
||||
|
||||
ctabmap.dumpData();
|
||||
|
||||
ULogger::write("testSurfStrategy end...");
|
||||
//ULOGGER_INFO("\nTime = %fs", timer.ticks());
|
||||
|
||||
|
||||
|
||||
//adjustLikelihood()
|
||||
std::map<int, float> likelihood;
|
||||
likelihood.clear();
|
||||
likelihood.insert(std::pair<int, float>(-1, 0));
|
||||
likelihood.insert(std::pair<int, float>(1, 0));
|
||||
likelihood.insert(std::pair<int, float>(2, 0));
|
||||
likelihood.insert(std::pair<int, float>(3, 0));
|
||||
ctabmap.adjustLikelihood(likelihood);
|
||||
CPPUNIT_ASSERT(likelihood.size() == 4);
|
||||
std::vector<float> values = uValues(likelihood);
|
||||
for(unsigned int i=0; i<values.size(); ++i)
|
||||
{
|
||||
CPPUNIT_ASSERT(values[i] == 1.0);
|
||||
}
|
||||
likelihood.clear();
|
||||
likelihood.insert(std::pair<int, float>(-1, 0.3));
|
||||
likelihood.insert(std::pair<int, float>(1, 0.4));
|
||||
likelihood.insert(std::pair<int, float>(2, 0.2));
|
||||
likelihood.insert(std::pair<int, float>(3, 0.9));
|
||||
ctabmap.adjustLikelihood(likelihood);
|
||||
CPPUNIT_ASSERT(likelihood.size() == 4);
|
||||
values = uValues(likelihood);
|
||||
// Result wanted generated by the TestAdjustLikelihood.m script (MatLab/Tests)
|
||||
int resultWanted2[4] = {1000,1000,1000,1309};
|
||||
for(unsigned int i=0; i<values.size(); ++i)
|
||||
{
|
||||
//ULOGGER_DEBUG("%d vs %d", (int)(values[i]*1000), resultWanted2[i]);
|
||||
CPPUNIT_ASSERT(int(values[i]*1000) == resultWanted2[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void Tests::testCamera()
|
||||
{
|
||||
std::string path;
|
||||
SMState * smState = 0;
|
||||
int count;
|
||||
|
||||
//CameraVideo class FIXME add a video in svn and reactivate this test
|
||||
/*path = "data/std_cam.avi";
|
||||
CameraVideo cameraVideo(path, false, 1);
|
||||
CPPUNIT_ASSERT( cameraVideo.init() );
|
||||
CPPUNIT_ASSERT( cameraVideo.isIdle() == true);
|
||||
image = cameraVideo.takeImage();
|
||||
count = 0;
|
||||
while(image)
|
||||
{
|
||||
cvReleaseImage(&image);
|
||||
image = 0;
|
||||
++count;
|
||||
if(count == 10)
|
||||
{
|
||||
break;
|
||||
}
|
||||
image = cameraVideo.takeImage();
|
||||
}
|
||||
if(image)
|
||||
cvReleaseImage(&image);
|
||||
CPPUNIT_ASSERT( count == 10 );*/
|
||||
|
||||
//CameraImages class
|
||||
path = "data/samples";
|
||||
CameraImages cameraImages(path, 1, false, 0, false, 80);
|
||||
CPPUNIT_ASSERT( cameraImages.init() );
|
||||
CPPUNIT_ASSERT( cameraImages.isIdle() == true);
|
||||
smState = cameraImages.takeSMState();
|
||||
count = 0;
|
||||
while(smState)
|
||||
{
|
||||
delete smState;
|
||||
smState = 0;
|
||||
++count;
|
||||
smState = cameraImages.takeSMState();
|
||||
}
|
||||
CPPUNIT_ASSERT( count == 84 );
|
||||
|
||||
//CameraDatabase class
|
||||
path = "./data/samples.db";
|
||||
CameraDatabase cameraDatabase(path, false, false); // ignoreChildren=false; loadActions=false
|
||||
CPPUNIT_ASSERT( cameraDatabase.init() );
|
||||
CPPUNIT_ASSERT( cameraDatabase.isIdle() == true);
|
||||
smState = cameraDatabase.takeSMState();
|
||||
count = 0;
|
||||
while(smState)
|
||||
{
|
||||
++count;
|
||||
delete smState;
|
||||
smState = 0;
|
||||
smState = cameraDatabase.takeSMState();
|
||||
}
|
||||
//ULOGGER_INFO("%d", count);
|
||||
CPPUNIT_ASSERT( count == 82 );
|
||||
}
|
||||
|
||||
void Tests::testDBDriverFactory()
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeFile, "LogTestAvpdCore/testSqlite3Database.txt", false);
|
||||
DBDriver * dbDriver = 0;
|
||||
|
||||
dbDriver = DBDriverFactory::createDBDriver("sqlite3");
|
||||
CPPUNIT_ASSERT( dbDriver );
|
||||
delete dbDriver;
|
||||
|
||||
dbDriver = DBDriverFactory::createDBDriver("unknownDriver");
|
||||
CPPUNIT_ASSERT( dbDriver == 0 );
|
||||
}
|
||||
|
||||
// TODO not finished
|
||||
void Tests::testSqlite3Database()
|
||||
{
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
|
||||
DBDriver * driver = DBDriverFactory::createDBDriver("sqlite3", parameters);
|
||||
|
||||
CPPUNIT_ASSERT(driver);
|
||||
CPPUNIT_ASSERT(driver->openConnection("LogTestAvpdCore/tmpDatabase.db"));
|
||||
delete driver;
|
||||
|
||||
//== Test SMSignature save/load ==//
|
||||
UFile::erase("LogTestAvpdCore/tmpDatabase.db");
|
||||
parameters.clear();
|
||||
parameters.insert(ParametersPair(Parameters::kMemSignatureType(), "1"));
|
||||
driver = DBDriverFactory::createDBDriver("sqlite3", parameters);
|
||||
CPPUNIT_ASSERT(driver);
|
||||
CPPUNIT_ASSERT(driver->openConnection("LogTestAvpdCore/tmpDatabase.db"));
|
||||
std::vector<int> sensors;
|
||||
std::vector<unsigned char> motionMask;
|
||||
std::vector<int> sensors1;
|
||||
std::vector<int> sensors2;
|
||||
bool keepRawData = true;
|
||||
CameraImages camera("./data/samples");
|
||||
CPPUNIT_ASSERT_MESSAGE("Camera initialization failed!\n", camera.init());
|
||||
|
||||
IplImage * image = camera.takeImage();
|
||||
CPPUNIT_ASSERT(image);
|
||||
SMSignature * sm1 = new SMSignature(sensors, motionMask, 1, image, keepRawData);
|
||||
sensors1 = sm1->getSensors();
|
||||
cvReleaseImage(&image);
|
||||
sm1->addNeighbor(NeighborLink(2));
|
||||
|
||||
image = camera.takeImage();
|
||||
CPPUNIT_ASSERT(image);
|
||||
SMSignature * sm2 = new SMSignature(sensors, motionMask, 2, image, keepRawData);
|
||||
sensors2 = sm2->getSensors();
|
||||
cvReleaseImage(&image);
|
||||
sm2->addNeighbor(NeighborLink(1));
|
||||
|
||||
//save them
|
||||
driver->asyncSave(sm1);
|
||||
sm1 = 0;
|
||||
driver->asyncSave(sm2);
|
||||
sm2 = 0;
|
||||
driver->emptyTrashes(false);
|
||||
|
||||
//load them
|
||||
Signature * s1 = 0;
|
||||
Signature * s2 = 0;
|
||||
driver->getSignature(1, &s1);
|
||||
driver->getSignature(2, &s2);
|
||||
CPPUNIT_ASSERT(s1);
|
||||
CPPUNIT_ASSERT(s2);
|
||||
sm1 = dynamic_cast<SMSignature*>(s1);
|
||||
sm2 = dynamic_cast<SMSignature*>(s2);
|
||||
CPPUNIT_ASSERT(sm1);
|
||||
CPPUNIT_ASSERT(sm2);
|
||||
|
||||
//compare
|
||||
CPPUNIT_ASSERT(sensors1.size() > 0 && sensors1.size() == sm1->getSensors().size());
|
||||
CPPUNIT_ASSERT(sensors2.size() > 0 && sensors2.size() == sm2->getSensors().size());
|
||||
for(unsigned int i=0; i<sensors1.size(); ++i)
|
||||
{
|
||||
CPPUNIT_ASSERT(sensors1.at(i) == sm1->getSensors().at(i));
|
||||
}
|
||||
for(unsigned int i=0; i<sensors2.size(); ++i)
|
||||
{
|
||||
CPPUNIT_ASSERT(sensors2.at(i) == sm2->getSensors().at(i));
|
||||
}
|
||||
CPPUNIT_ASSERT(sm1->getNeighbors().size() == 1);
|
||||
CPPUNIT_ASSERT(sm2->getNeighbors().size() == 1);
|
||||
|
||||
delete sm1;
|
||||
delete sm2;
|
||||
delete driver;
|
||||
}
|
||||
|
||||
// TODO add some tests to test when a signature is forgotten or reactivated
|
||||
void Tests::testBayesFilter()
|
||||
{
|
||||
BayesFilter bayes;
|
||||
|
||||
//Parameters checks
|
||||
CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == Parameters::defaultBayesVirtualPlacePriorThr() );
|
||||
CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare(Parameters::defaultBayesPredictionLC()) == 0 );
|
||||
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.01"));
|
||||
parameters.insert(ParametersPair(Parameters::kBayesPredictionLC(), "0.01 0.02 0.03 0.04"));
|
||||
bayes.parseParameters(parameters);
|
||||
CPPUNIT_ASSERT( uNumber2Str(bayes.getVirtualPlacePrior()).compare("0.01") == 0 );
|
||||
CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
|
||||
bayes.setVirtualPlacePrior(-1);
|
||||
CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == 0 );
|
||||
bayes.setVirtualPlacePrior(1.1);
|
||||
CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == 1 );
|
||||
bayes.setVirtualPlacePrior(0.6);
|
||||
CPPUNIT_ASSERT( uNumber2Str(bayes.getVirtualPlacePrior()).compare("0.6") == 0 );
|
||||
bayes.setPredictionLC("");
|
||||
CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
|
||||
bayes.setPredictionLC("0,01 0,02");
|
||||
CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
|
||||
bayes.setPredictionLC("0.01 0.02");
|
||||
CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02") == 0 );
|
||||
bayes.setPredictionLC("0.01 0.02 0.03");
|
||||
CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03") == 0 );
|
||||
|
||||
//reset parameters...
|
||||
bayes = BayesFilter();
|
||||
|
||||
//computePosterior()
|
||||
// Load memory with some data...
|
||||
KeypointMemory mem;
|
||||
|
||||
//parameters
|
||||
mem.setCommonSignatureUsed(true);
|
||||
mem.setMaxStMemSize(1);
|
||||
bayes.setPredictionLC("0.1 0.24 0.18 0.1 0.04 0.01");
|
||||
bayes.setVirtualPlacePrior(0.9);
|
||||
|
||||
std::map<int, float> likelihood;
|
||||
std::map<int, float> posterior;
|
||||
float sum;
|
||||
likelihood.insert(std::pair<int, float>(-1, 1));
|
||||
int result[100] = {0};
|
||||
int ri = 0;
|
||||
|
||||
for(int i=1; i<11; ++i)
|
||||
{
|
||||
//ULOGGER_DEBUG("--- %d ---", i);
|
||||
std::map<std::string, float> memStats;
|
||||
mem.update(0, memStats);
|
||||
posterior = bayes.computePosterior(&mem, likelihood);
|
||||
likelihood.insert(std::pair<int, float>(i, 1));
|
||||
sum = uSum(uValues(posterior));
|
||||
CPPUNIT_ASSERT(sum > 1.0-0.0001 && sum < 1.0+0.0001);
|
||||
for(std::map<int, float>::const_iterator iter=posterior.begin(); iter!= posterior.end(); ++iter)
|
||||
{
|
||||
ULOGGER_DEBUG("%f", (*iter).second);
|
||||
result[ri++] = int((*iter).second*1000);
|
||||
}
|
||||
while((ri) % 10 != 0)
|
||||
{
|
||||
result[ri++] = 0;
|
||||
}
|
||||
}
|
||||
// Result wanted generated by the TestBayesFilter.m script (MatLab/Tests)
|
||||
int resultWanted1[100] = {1000,0,0,0,0,0,0,0,0,0,900,99,0,0,0,0,0,0,0,0,820,117,62,0,0,0,0,0,0,0,756,111,82,50,0,0,0,0,0,0,704,103,84,67,40,0,0,0,0,0,663,96,82,69,54,32,0,0,0,0,631,90,79,69,58,44,26,0,0,0,604,84,76,68,58,48,36,21,0,0,583,79,73,66,58,49,40,30,17,0,567,74,69,64,58,50,41,33,25,14};
|
||||
for(int i=0; i<100; ++i)
|
||||
{
|
||||
ULOGGER_DEBUG("%d vs %d", result[i], resultWanted1[i]);
|
||||
CPPUNIT_ASSERT(result[i] >= resultWanted1[i]-1 && result[i] <= resultWanted1[i]+1);
|
||||
}
|
||||
}
|
||||
|
||||
void Tests::testKeypointMemory()
|
||||
{
|
||||
//Util::Logger::setLevel(Logger::kInfo);
|
||||
//Util::Logger::setType(Logger::kTypeConsole);
|
||||
|
||||
KeypointMemory mem;
|
||||
std::map<int, float> likelihood;
|
||||
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // It will be like read-only (database won't be changed in SVN)
|
||||
parameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "true"));
|
||||
mem.setCommonSignatureUsed(true);
|
||||
mem.setMaxStMemSize(1);
|
||||
mem.init("sqlite3", "./data/samples.db", false, parameters);
|
||||
//ULOGGER_INFO("mem.getStMemIds().size() = %d, mem.getStMemIds().size()=%d", mem.getWorkingMemIds().size(), mem.getStMemIds().size());
|
||||
CPPUNIT_ASSERT( mem.getWorkingMem().size() + mem.getStMem().size() == 83 );
|
||||
|
||||
//updateCommonSignature()
|
||||
const KeypointSignature * virtualPlace = dynamic_cast<const KeypointSignature *>(mem.getSignature(Memory::kIdVirtual));
|
||||
CPPUNIT_ASSERT(virtualPlace != 0);
|
||||
std::list<int> wordIds = uKeys(virtualPlace->getWords());
|
||||
//ULOGGER_INFO("wordIds.size()=%d", wordIds.size());
|
||||
// Result wanted generated by the TestUpdateCommonWords.m script (MatLab/Tests)
|
||||
int commonWordsWanted[163] = {5,8,9,21,22,23,27,30,32,34,36,40,45,53,61,62,64,67,68,77,85,86,97,98,99,100,103,106,122,124,125,127,129,131,143,157,161,164,168,169,172,175,181,182,187,196,197,208,210,217,218,219,220,234,235,237,244,252,286,307,308,310,315,327,328,346,347,350,355,362,372,373,387,393,394,398,404,412,423,428,429,441,442,456,465,470,476,495,496,498,506,520,558,572,584,585,587,596,620,634,637,643,646,647,658,667,668,681,709,721,726,741,766,777,785,790,791,793,808,809,880,896,906,908,919,933,940,954,958,974,981,1002,1023,1026,1058,1060,1071,1074,1097,1146,1152,1166,1215,1217,1229,1334,1341,1387,1478,1510,1539,1549,1566,1601,1624,1649,1693,1814,2046,2141,2424,2442,2674};
|
||||
int j=0;
|
||||
CPPUNIT_ASSERT(wordIds.size() == 163);
|
||||
for(std::list<int>::iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
|
||||
{
|
||||
//ULOGGER_INFO("%d vs %d", *i, commonWordsWanted[j]);
|
||||
CPPUNIT_ASSERT(*i == commonWordsWanted[j]);
|
||||
++j;
|
||||
}
|
||||
|
||||
//computeLikelihood()
|
||||
const KeypointSignature * lastSign = dynamic_cast<const KeypointSignature *>(mem.getLastSignature());
|
||||
CPPUNIT_ASSERT(lastSign != 0);
|
||||
wordIds = uKeys(lastSign->getWords());
|
||||
j=0;
|
||||
int signWordsRequired[136] = {9,24,27,37,39,40,45,46,64,64,67,67,68,68,69,69,79,79,81,85,87,95,109,113,115,117,122,124,127,135,135,139,142,143,145,157,159,161,168,181,188,188,191,192,196,197,208,208,211,213,219,239,241,247,256,257,258,307,310,343,558,587,643,760,896,960,1019,1023,1074,1197,1207,1292,1401,1403,1587,2151,2419,2571,2623,2654,2664,2674,2674,2674,2777,2780,2796,2808,2835,2844,2844,2851,2855,2862,2900,3036,3143,3238,3238,3315,3531,3531,3766,3803,4296,4405,4462,4502,4506,4509,4509,4516,4523,4533,4533,4534,4535,4536,4537,4538,4539,4540,4541,4542,4543,4544,4545,4546,4547,4548,4549,4550,4551,4552,4553,4554};
|
||||
CPPUNIT_ASSERT(wordIds.size() == 136);
|
||||
for(std::list<int>::iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
|
||||
{
|
||||
//ULOGGER_INFO("%d vs %d", *i, signWordsRequired[j]);
|
||||
CPPUNIT_ASSERT(*i == signWordsRequired[j]);
|
||||
++j;
|
||||
}
|
||||
float maximumScore;
|
||||
likelihood = mem.computeLikelihood(lastSign, std::list<int>(mem.getWorkingMem().begin(), mem.getWorkingMem().end()), maximumScore);
|
||||
//ULOGGER_INFO("likelihood.size() = %d", likelihood.size());
|
||||
std::vector<float> values = uValues(likelihood);
|
||||
int likelihoodWanted[82] = {109,157,263,203,87,66,78,49,60,40,47,43,43,55,102,102,147,0,38,61,64,74,69,103,39,20,44,33,14,14,20,12,18,8,59,19,41,26,45,117,124,173,223,74,0,10,17,53,33,24,33,43,52,68,119,124,146,159,28,68,59,115,71,95,37,18,16,49,9,28,20,9,15,11,10,35,45,73,18,92,167,219};
|
||||
CPPUNIT_ASSERT(values.size() == 82);
|
||||
for(unsigned int i=0; i<values.size(); ++i)
|
||||
{
|
||||
ULOGGER_INFO("%d vs %d", int(values[i]*1000), likelihoodWanted[i]);
|
||||
CPPUNIT_ASSERT(int(values[i]*1000) == likelihoodWanted[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void Tests::testVWDictionary()
|
||||
{
|
||||
VWDictionary dictionary;
|
||||
dictionary.setNndrUsed(false);
|
||||
std::list<cv::KeyPoint> keypoints;
|
||||
cv::Mat descriptors;
|
||||
unsigned int dim = 2;
|
||||
std::vector<float> v(dim);
|
||||
|
||||
keypoints.push_back(cv::KeyPoint(cv::Point2f(1,1), 10, 30, 500, 2, 0));
|
||||
v[0] = 3;
|
||||
v[1] = 4;
|
||||
descriptors = cv::Mat(0, 2, CV_32F);
|
||||
descriptors.push_back(v);
|
||||
|
||||
dictionary.addNewWords(descriptors, 1);
|
||||
CPPUNIT_ASSERT(dictionary.getVisualWords().size() == 1);
|
||||
|
||||
//Create a word with the next descriptor (the distance^2 with the first word added = 2)
|
||||
v[0] = 4;
|
||||
v[1] = 3;
|
||||
VisualWord word(2, v.data(), dim, 0);
|
||||
std::list<VisualWord*> words;
|
||||
words.push_back(&word);
|
||||
std::vector<int> ids;
|
||||
|
||||
//Naive
|
||||
dictionary.setNNStrategy(VWDictionary::kNNNaive);
|
||||
dictionary.setMinDistUsed(true);
|
||||
dictionary.setNndrUsed(false);
|
||||
dictionary.setMinDist(2.01f);
|
||||
ids = dictionary.findNN(words);
|
||||
CPPUNIT_ASSERT(ids.size() == 1);
|
||||
CPPUNIT_ASSERT(ids.front() == 1);
|
||||
|
||||
dictionary.setMinDistUsed(true);
|
||||
dictionary.setNndrUsed(false);
|
||||
dictionary.setMinDist(1.99f);
|
||||
ids = dictionary.findNN(words);
|
||||
CPPUNIT_ASSERT(ids.size() == 1);
|
||||
CPPUNIT_ASSERT(ids.front() == 0);
|
||||
|
||||
|
||||
//Opencv kdtree
|
||||
dictionary.setNNStrategy(VWDictionary::kNNKdTree);
|
||||
dictionary.setMinDistUsed(true);
|
||||
dictionary.setNndrUsed(false);
|
||||
dictionary.setMinDist(2.01f);
|
||||
ids = dictionary.findNN(words);
|
||||
CPPUNIT_ASSERT(ids.size() == 1);
|
||||
CPPUNIT_ASSERT(ids.front() == 1);
|
||||
|
||||
dictionary.setMinDistUsed(true);
|
||||
dictionary.setNndrUsed(false);
|
||||
dictionary.setMinDist(1.99f);
|
||||
ids = dictionary.findNN(words);
|
||||
CPPUNIT_ASSERT(ids.size() == 1);
|
||||
CPPUNIT_ASSERT(ids.front() == 0);
|
||||
|
||||
|
||||
//FLANN kdtree
|
||||
dictionary.setNNStrategy(VWDictionary::kNNFlannKdTree);
|
||||
dictionary.setMinDistUsed(true);
|
||||
dictionary.setNndrUsed(false);
|
||||
dictionary.setMinDist(2.01f);
|
||||
ids = dictionary.findNN(words);
|
||||
CPPUNIT_ASSERT(ids.size() == 1);
|
||||
CPPUNIT_ASSERT(ids.front() == 1);
|
||||
|
||||
dictionary.setMinDistUsed(true);
|
||||
dictionary.setNndrUsed(false);
|
||||
dictionary.setMinDist(1.99f);
|
||||
ids = dictionary.findNN(words);
|
||||
CPPUNIT_ASSERT(ids.size() == 1);
|
||||
CPPUNIT_ASSERT(ids.front() == 0);
|
||||
}
|
||||
|
||||
void Tests::testVerifyHypotheses()
|
||||
{
|
||||
std::multimap<int, cv::KeyPoint> wordsA;
|
||||
std::multimap<int, cv::KeyPoint> wordsB;
|
||||
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
|
||||
std::list<int> pairsId;
|
||||
|
||||
wordsA.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(0, 0, 1)));
|
||||
wordsA.insert(std::pair<int, cv::KeyPoint>(2, cv::KeyPoint(2, 2, 1)));
|
||||
wordsA.insert(std::pair<int, cv::KeyPoint>(3, cv::KeyPoint(3, 3, 1)));
|
||||
wordsA.insert(std::pair<int, cv::KeyPoint>(4, cv::KeyPoint(4, 4, 1)));
|
||||
wordsA.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
|
||||
wordsA.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));
|
||||
|
||||
wordsB.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(0, 0, 1)));
|
||||
wordsB.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(1, 1, 1)));
|
||||
wordsB.insert(std::pair<int, cv::KeyPoint>(2, cv::KeyPoint(2, 2, 1)));
|
||||
wordsB.insert(std::pair<int, cv::KeyPoint>(4, cv::KeyPoint(3, 3, 1)));
|
||||
wordsB.insert(std::pair<int, cv::KeyPoint>(5, cv::KeyPoint(4, 4, 1)));
|
||||
wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
|
||||
wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));
|
||||
|
||||
int total = HypVerificatorEpipolarGeo::findPairsAll(wordsA, wordsB, pairs, pairsId);
|
||||
|
||||
//printf("[%d,%d]\n", total, (int)pairs.size());
|
||||
CPPUNIT_ASSERT(total == 5);
|
||||
CPPUNIT_ASSERT(pairs.size() == 8 && pairsId.size() == 8);
|
||||
|
||||
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator pairsIter=pairs.begin();
|
||||
std::list<int>::iterator idIter = pairsId.begin();
|
||||
for(;pairsIter!=pairs.end() && idIter != pairsId.end(); ++pairsIter, ++idIter)
|
||||
{
|
||||
//printf("(%d)[%f,%f] [%f,%f]\n", *idIter, pairsIter->first.pt.x, pairsIter->first.pt.y, pairsIter->second.pt.x, pairsIter->second.pt.x);
|
||||
}
|
||||
}
|
||||
@@ -1,77 +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 TESTS_H
|
||||
#define TESTS_H
|
||||
|
||||
#include <cppunit/TestFixture.h>
|
||||
#include <cppunit/TestCaller.h>
|
||||
|
||||
#include <cppunit/extensions/HelperMacros.h>
|
||||
|
||||
#include "utilite/UDirectory.h"
|
||||
#include "utilite/ULogger.h"
|
||||
|
||||
class Tests : public CppUnit::TestFixture {
|
||||
|
||||
CPPUNIT_TEST_SUITE( Tests );
|
||||
//CPPUNIT_TEST( testAvpd );
|
||||
//CPPUNIT_TEST( testDBDriverFactory );
|
||||
CPPUNIT_TEST( testSqlite3Database );
|
||||
CPPUNIT_TEST( testBayesFilter );
|
||||
//CPPUNIT_TEST( testKeypointMemory );
|
||||
CPPUNIT_TEST( testCamera );
|
||||
//CPPUNIT_TEST( testVWDictionary );
|
||||
//CPPUNIT_TEST( testVerifyHypotheses );
|
||||
CPPUNIT_TEST_SUITE_END();
|
||||
|
||||
private:
|
||||
public:
|
||||
void setUp()
|
||||
{
|
||||
if(!UDirectory::exists("./LogTestLibCore"))
|
||||
{
|
||||
UDirectory::makeDir("./LogTestLibCore");
|
||||
}
|
||||
|
||||
ULogger::reset();
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kError);
|
||||
//ULogger::setLevel(ULogger::kDebug);
|
||||
}
|
||||
|
||||
void tearDown()
|
||||
{
|
||||
ULogger::reset();
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kError);
|
||||
//Util::Logger::setLevel(Util::Logger::kDebug);
|
||||
}
|
||||
|
||||
void testAvpd();
|
||||
void testCamera();
|
||||
void testDBDriverFactory();
|
||||
void testSqlite3Database();
|
||||
void testBayesFilter();
|
||||
void testKeypointMemory();
|
||||
void testVWDictionary();
|
||||
void testVerifyHypotheses();
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -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/>.
|
||||
*/
|
||||
|
||||
#include <cppunit/BriefTestProgressListener.h>
|
||||
#include <cppunit/CompilerOutputter.h>
|
||||
#include <cppunit/extensions/TestFactoryRegistry.h>
|
||||
#include <cppunit/TestResult.h>
|
||||
#include <cppunit/TestResultCollector.h>
|
||||
#include <cppunit/TestRunner.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
#ifdef _MSC_VER
|
||||
// Use Visual C++'s memory checking functionality
|
||||
#define _CRTDBG_MAP_ALLOC
|
||||
#include <crtdbg.h>
|
||||
#endif // _MSC_VER
|
||||
|
||||
int main( int argc, char **argv)
|
||||
{
|
||||
#ifdef _MSC_VER
|
||||
//_crtBreakAlloc = 189;
|
||||
_CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
|
||||
#endif // _MSC_VER
|
||||
|
||||
//Header of the test
|
||||
char header[150] = "-------------------------------------\n"
|
||||
"corelib Test\n"
|
||||
"-------------------------------------\n";
|
||||
CPPUNIT_NS::stdCOut() << header;
|
||||
|
||||
// Create the event manager and test controller
|
||||
CPPUNIT_NS::TestResult controller;
|
||||
|
||||
// Add a listener that colllects test result
|
||||
CPPUNIT_NS::TestResultCollector result;
|
||||
controller.addListener(&result);
|
||||
|
||||
// Add a listener that print dots as test run.
|
||||
CPPUNIT_NS::BriefTestProgressListener progress;
|
||||
controller.addListener(&progress);
|
||||
|
||||
// Add the top suite to the test runner
|
||||
CPPUNIT_NS::TestRunner runner;
|
||||
runner.addTest(CPPUNIT_NS::TestFactoryRegistry::getRegistry().makeTest());
|
||||
runner.run(controller);
|
||||
|
||||
// Print test in a compiler compatible format.
|
||||
CPPUNIT_NS::CompilerOutputter outputter( &result, CPPUNIT_NS::stdCOut() );
|
||||
outputter.write();
|
||||
|
||||
// If a path is passed in argument copy the result in it
|
||||
if (argc == 2)
|
||||
{
|
||||
std::ofstream myfile;
|
||||
myfile.open (argv[1]);
|
||||
CPPUNIT_NS::CompilerOutputter outputterFile( &result, myfile);
|
||||
myfile << header;
|
||||
outputterFile.write();
|
||||
myfile.close();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user