i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 06:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged Audio branch to trunk

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

Before

Width:  |  Height:  |  Size: 2.4 MiB

After

Width:  |  Height:  |  Size: 2.4 MiB

guilib/Mockup/MockupAll.pptx → doc/Mockup/MockupAll.pptx
View File
guilib/CMakeLists.txt
+1 -2
View File
@@ -1,2 +1 @@
ADD_SUBDIRECTORY( src )
ADD_SUBDIRECTORY( ExamplePlot )
ADD_SUBDIRECTORY( src )
guilib/ExamplePlot/CMakeLists.txt
-36
View File
@@ -1,36 +0,0 @@
ADD_DEFINITIONS(-DPLOT_WIDGET_OUT_OF_LIB)
### Qt Gui stuff ###
SET(headers_ui
../src/Plot.h
MainWindow.h
)
#This will generate moc_* for Qt
QT4_WRAP_CPP(moc_srcs ${headers_ui})
### Qt Gui stuff end###
SET(SRC_FILES
../src/Plot.cpp
main.cpp
${moc_srcs}
)
SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/../src
)
INCLUDE(${QT_USE_FILE})
SET(LIBRARIES
${QT_LIBRARIES}
)
#include files
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
# create an executable file named "examplePlot" from the source files
ADD_EXECUTABLE(examplePlot ${SRC_FILES})
# Linking with Qt libraries
TARGET_LINK_LIBRARIES(examplePlot ${LIBRARIES})
guilib/ExamplePlot/MainWindow.h
-66
View File
@@ -1,66 +0,0 @@
#ifndef MAINWINDOW_H_
#define MAINWINDOW_H_
#include <QtGui/QMainWindow>
#include <QtGui/QSlider>
#include <QtGui/QHBoxLayout>
#include <QtCore/QTimer>
#include "Plot.h"
// Note : compiled with -DPLOT_WIDGET_OUT_OF_LIB to
// remove rtabmap's namespace and UtiLite dependency of Plot.
class MainWindow : public QMainWindow
{
Q_OBJECT
public:
MainWindow() {
//Plot
Plot * plot = new Plot(this);
plot->setObjectName("Figure 1");
plot->setMaxVisibleItems(50);
plot->showRefreshRate(true);
PlotCurve * curveA = new PlotCurve("Curve A", this);
PlotCurve * curveB = new PlotCurve("Curve B", this);
curveA->setPen(QPen(Qt::red));
curveB->setPen(QPen(Qt::blue));
plot->addCurve(curveA); // ownership transferred
plot->addCurve(curveB); // ownership transferred
connect(this, SIGNAL(valueUpdatedA(float)), curveA, SLOT(addValue(float)));
connect(this, SIGNAL(valueUpdatedB(float)), curveB, SLOT(addValue(float)));
//Control
QSlider * slider = new QSlider(Qt::Vertical, this);
slider->setMinimum(1); // Hz
slider->setMaximum(100); // Hz
slider->setValue(10);
connect(slider, SIGNAL(valueChanged(int)), this, SLOT(setRate(int)));
// layout
QWidget * placeHolder = new QWidget(this);
this->setCentralWidget(placeHolder);
QHBoxLayout * hlayout = new QHBoxLayout(placeHolder);
hlayout->addWidget(plot, 1);
hlayout->addWidget(slider);
connect(&timer_, SIGNAL(timeout()), this, SLOT(updateCounter()));
setRate(slider->value());
qsrand(1);
}
~MainWindow() {}
public slots:
void updateCounter() {
emit valueUpdatedA(qrand() % 100);
emit valueUpdatedB(qrand() % 50);
}
void setRate(int rate) {
timer_.start(1000/rate);
}
signals:
void valueUpdatedA(float);
void valueUpdatedB(float);
private:
QTimer timer_;
};
#endif /* MAINWINDOW_H_ */
guilib/ExamplePlot/main.cpp
-12
View File
@@ -1,12 +0,0 @@
#include <QtGui/QApplication>
#include "MainWindow.h"
int main(int argc, char *argv[]) {
QApplication app(argc, argv);
MainWindow mainWindow;
mainWindow.show();
app.connect( &app, SIGNAL( lastWindowClosed() ),
&app, SLOT( quit() ) );
return app.exec();
}
guilib/src/ImageView.h → guilib/include/rtabmap/gui/ImageView.h
+6 -1
View File
@@ -20,6 +20,8 @@
#ifndef IMAGEVIEW_H_
#define IMAGEVIEW_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <QtGui/QGraphicsView>
#include <QtCore/QRectF>
@@ -28,7 +30,7 @@ class QMenu;
namespace rtabmap {
class ImageView : public QGraphicsView {
class RTABMAP_EXP ImageView : public QGraphicsView {
Q_OBJECT
@@ -42,6 +44,9 @@ public:
bool isFeaturesShown();
bool isLinesShown();
void setFeaturesShown(bool shown);
void setLinesShown(bool shown);
protected:
virtual void contextMenuEvent(QContextMenuEvent * e);
virtual void wheelEvent(QWheelEvent * e);
guilib/src/KeypointItem.h → guilib/include/rtabmap/gui/KeypointItem.h
+3 -1
View File
@@ -20,6 +20,8 @@
#ifndef KEYPOINTITEM_H_
#define KEYPOINTITEM_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <QtGui/QGraphicsEllipseItem>
#include <QtGui/QGraphicsTextItem>
#include <QtGui/QPen>
@@ -27,7 +29,7 @@
namespace rtabmap {
class KeypointItem : public QGraphicsEllipseItem
class RTABMAP_EXP KeypointItem : public QGraphicsEllipseItem
{
public:
KeypointItem(qreal x, qreal y, int r, const QString & info, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
guilib/include/rtabmap/gui/MainWindow.h
+27 -2
View File
@@ -30,6 +30,9 @@
namespace rtabmap {
class Camera;
class Micro;
class CameraMicro;
class DBReader;
}
class QGraphicsScene;
@@ -85,6 +88,7 @@ protected:
virtual void resizeEvent(QResizeEvent* anEvent);
private slots:
void beep();
void startDetection();
void pauseDetection();
void stopDetection();
@@ -95,6 +99,8 @@ private slots:
void selectImages();
void selectVideo();
void selectStream();
void selectMic();
void selectAudioFile();
void selectDatabase();
void resetTheMemory();
void dumpTheMemory();
@@ -112,6 +118,15 @@ private slots:
void changeImgRateSetting();
void changeTimeLimitSetting();
void captureScreen();
void setAspectRatio(int w, int h);
void setAspectRatio16_9();
void setAspectRatio16_10();
void setAspectRatio4_3();
void setAspectRatio240p();
void setAspectRatio360p();
void setAspectRatio480p();
void setAspectRatio720p();
void setAspectRatio1080p();
signals:
void statsReceived(const rtabmap::Statistics &);
@@ -128,13 +143,18 @@ signals:
private:
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
void setupMainLayout(bool vertical);
void updateSelectSourceMenu(int type);
void updateSelectSourceImageMenu(int type);
void updateSelectSourceAudioMenu(int type);
void updateSelectSourceDatabase(bool used);
private:
Ui_mainWindow * _ui;
State _state;
rtabmap::Camera * _camera;
rtabmap::Micro * _mic;
rtabmap::CameraMicro * _cameraMic;
rtabmap::DBReader * _dbReader;
SrcType _srcType;
QString _srcPath;
@@ -156,8 +176,13 @@ private:
PdfPlotCurve * _posteriorCurve;
PdfPlotCurve * _likelihoodCurve;
UPlotCurve * _audioCurve;
UPlotCurve * _audioCurveLoop;
DetailedProgressDialog * _initProgressDialog;
QActionGroup * _selectSourceGrp;
QActionGroup * _selectSourceImageGrp;
QActionGroup * _selectSourceAudioGrp;
QString _graphSavingFileName;
QString _autoScreenCaptureFormat;
guilib/include/rtabmap/gui/PreferencesDialog.h
+34 -12
View File
@@ -38,11 +38,10 @@ class QMainWindow;
class QLineEdit;
class QSlider;
class QProgressDialog;
class UPlotCurve;
namespace rtabmap {
class PlotCurve;
class RTABMAP_EXP PreferencesDialog : public QDialog
{
Q_OBJECT
@@ -64,8 +63,13 @@ public:
kSrcUndef,
kSrcUsbDevice,
kSrcImages,
kSrcVideo,
kSrcDatabase
kSrcVideo
};
enum SrcAudio {
kSrcAudioUndef,
kSrcAudioMicDevice,
kSrcAudioFile
};
public:
@@ -102,13 +106,17 @@ public:
QString getWorkingDirectory();
// source panel
double getGeneralImageRate() const;
double getGeneralInputRate() const;
bool isSourceImageUsed() const;
bool isSourceAudioUsed() const;
bool isSourceDatabaseUsed() const;
bool getGeneralAutoRestart() const;
bool getGeneralCameraKeypoints() const;
int getSourceType() const;
QString getSourceTypeStr() const;
int getSourceImageType() const;
QString getSourceImageTypeStr() const;
int getSourceWidth() const;
int getSourceHeight() const;
int getFramesDropped() const;
QString getSourceImagesPath() const; //Images group
QString getSourceImagesSuffix() const; //Images group
int getSourceImagesSuffixIndex() const; //Images group
@@ -116,31 +124,44 @@ public:
bool getSourceImagesRefreshDir() const; //Images group
QString getSourceVideoPath() const; //Video group
int getSourceUsbDeviceId() const; //UsbDevice group
QString getSourceDatabasePath() const; //Database group
bool getSourceDatabaseIgnoreChildren() const; //Database group
bool getSourceDatabaseLoadActions() const; //Database group
int getSourceAudioType() const; //Audio group
QString getSourceAudioTypeStr() const; //Audio group
int getSourceMicDevice() const; //Audio group
int getSourceMicFs() const; //Audio group
int getSourceMicSampleSize() const; //Audio group
QString getSourceAudioPath() const; //Audio group
bool getSourceAudioPlayWhileRecording() const; //Audio group
QString getSourceDatabasePath() const; //Database group
//
bool isImagesKept() const;
float getTimeLimit() const;
int getMemoryType() const;
//specific
bool isStatisticsPublished() const;
double getLoopThr() const;
double getRetrievalThr() const;
double getVpThr() const;
double getExpThr() const;
//
void disableSourceAudio();
void disableGeneralCameraKeypoints();
signals:
void settingsChanged(PreferencesDialog::PANEL_FLAGS);
void settingsChanged(rtabmap::ParametersMap);
public slots:
void setInputRate(double value);
void setHardThr(int value);
void setRetrievalThr(int value);
void setImgRate(double value);
void setAutoRestart(bool value);
void setTimeLimit(float value);
void selectSource(Src src = kSrcUndef);
void selectSourceImage(Src src = kSrcUndef);
void selectSourceAudio(SrcAudio = kSrcAudioUndef);
void selectSourceDatabase(bool user = false);
private slots:
void closeDialog ( QAbstractButton * button );
@@ -160,6 +181,7 @@ private slots:
void changeDictionaryPath();
void readSettingsEnd();
void setupTreeView();
void updateBasicParameter();
protected:
virtual void showEvent ( QShowEvent * event );
guilib/src/qtipl.h → guilib/include/rtabmap/gui/qtipl.h
+3 -1
View File
@@ -20,12 +20,14 @@
#ifndef QTIPL_H
#define QTIPL_H
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <QtGui/QImage>
#include <opencv2/core/core.hpp>
namespace rtabmap {
QImage Ipl2QImage(const IplImage *newImage);
QImage RTABMAP_EXP Ipl2QImage(const IplImage *newImage, int alpha = 255);
}
guilib/src/CMakeLists.txt
+7 -16
View File
@@ -6,8 +6,7 @@ SET(headers_ui
../include/${PROJECT_PREFIX}/gui/PreferencesDialog.h
./AboutDialog.h
./ConsoleWidget.h
./ImageView.h
./Plot.h
../include/${PROJECT_PREFIX}/gui/ImageView.h
./PdfPlot.h
./StatsToolBox.h
./DetailedProgressDialog.h
@@ -44,7 +43,6 @@ SET(SRC_FILES
./KeypointItem.cpp
./qtipl.cpp
./ImageView.cpp
./Plot.cpp
./PdfPlot.cpp
./StatsToolBox.cpp
./DetailedProgressDialog.cpp
@@ -57,6 +55,7 @@ SET(SRC_FILES
)
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${CMAKE_CURRENT_SOURCE_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}
${UTILITE_INCLUDE_DIRS}
@@ -75,27 +74,19 @@ SET(LIBRARIES
#include files
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
IF(WIN32)
IF(BUILD_SHARED_LIBS)
ADD_DEFINITIONS(-DRTABMAP_EXPORTS)
ELSE()
ADD_DEFINITIONS(-DRTABMAP_EXPORTS_STATIC)
ENDIF()
ENDIF(WIN32)
# create an executable file named "AvpdGui" from the source files
ADD_LIBRARY(guilib ${SRC_FILES})
# create a library from the source files
ADD_LIBRARY(rtabmap_guilib ${SRC_FILES})
# Linking with Qt libraries
TARGET_LINK_LIBRARIES(guilib corelib ${LIBRARIES})
TARGET_LINK_LIBRARIES(rtabmap_guilib rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES(
guilib
rtabmap_guilib
PROPERTIES
OUTPUT_NAME ${PROJECT_PREFIX}_gui
INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
)
INSTALL(TARGETS guilib
INSTALL(TARGETS rtabmap_guilib
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)
guilib/src/CameraMicro.h
+84
View File
@@ -0,0 +1,84 @@
/*
* CameraMicro.h
*
* Created on: 2012-05-27
* Author: mathieu
*/
#ifndef CAMERAMICRO_H_
#define CAMERAMICRO_H_
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Micro.h"
#include "rtabmap/core/SensorimotorEvent.h"
#include <utilite/UThreadNode.h>
#include <utilite/UEventsManager.h>
namespace rtabmap
{
class CameraMicro : public UThreadNode
{
public:
CameraMicro(Camera * camera, Micro * micro) :
camera_(camera),
micro_(micro)
{
}
protected:
void mainLoopBegin()
{
if(!camera_ || !micro_ )
{
UERROR("Camera and/or Micro are null");
this->kill();
return;
}
micro_->startRecorder();
}
void mainLoop()
{
cv::Mat frameFreq;
// frame rate should depend on micro and camera frame rate
// (getFrame() and takeImage() are blocking calls)
cv::Mat frame = micro_->getFrame(frameFreq, true);
if(frame.empty())
{
if(this->isRunning())
{
UEventsManager::post(new MicroEvent(MicroEvent::kTypeNoMoreFrames));
this->kill();
}
return;
}
cv::Mat image = camera_->takeImage();
if(image.empty())
{
UEventsManager::post(new CameraEvent(CameraEvent::kCodeNoMoreImages));
this->kill();
return;
}
std::list<Sensor> sensors;
sensors.push_back(Sensor(image, Sensor::kTypeImage));
sensors.push_back(Sensor(frameFreq, Sensor::kTypeAudioFreqSqrdMagn));
UEventsManager::post(new SensorimotorEvent(sensors, std::list<Actuator>()));
}
void mainLoopKill()
{
if(micro_)
{
micro_->stop();
}
}
private:
Camera * camera_;
Micro * micro_;
};
}
#endif /* CAMERAMICRO_H_ */
guilib/src/ConsoleWidget.cpp
+3 -3
View File
@@ -74,16 +74,16 @@ void ConsoleWidget::handleEvent(UEvent * anEvent)
{
if(logEvent->getCode() == ULogger::kFatal)
{
_timer.start(0);
QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, 0));
}
else
{
_timer.start(OLD_TIME);
QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, OLD_TIME));
}
}
else
{
_timer.start(0);
QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, 0));
}
if(logEvent->getCode() == ULogger::kFatal)
guilib/src/ImageView.cpp
+14 -2
View File
@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "ImageView.h"
#include "rtabmap/gui/ImageView.h"
#include <QtGui/QWheelEvent>
#include <QtCore/qmath.h>
@@ -26,7 +26,7 @@
#include <QtCore/QDir>
#include <QtGui/QAction>
#include "utilite/ULogger.h"
#include "KeypointItem.h"
#include "rtabmap/gui/KeypointItem.h"
namespace rtabmap {
@@ -73,11 +73,23 @@ bool ImageView::isFeaturesShown()
return _showFeatures->isChecked();
}
void ImageView::setFeaturesShown(bool shown)
{
_showFeatures->setChecked(shown);
this->updateItemsShown();
}
bool ImageView::isLinesShown()
{
return _showLines->isChecked();
}
void ImageView::setLinesShown(bool shown)
{
_showLines->setChecked(shown);
this->updateItemsShown();
}
void ImageView::contextMenuEvent(QContextMenuEvent * e)
{
QAction * action = _menu->exec(e->globalPos());
guilib/src/KeypointItem.cpp
+1 -1
View File
@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "KeypointItem.h"
#include "rtabmap/gui/KeypointItem.h"
#include <QtGui/QPen>
#include <QtGui/QBrush>
guilib/src/MainWindow.cpp
+931 -389
View File
File diff suppressed because it is too large Load Diff
guilib/src/PdfPlot.cpp
+4 -4
View File
@@ -23,7 +23,7 @@
namespace rtabmap {
PdfPlotItem::PdfPlotItem(float dataX, float dataY, float width, int childCount) :
PlotItem(dataX, dataY, width),
UPlotItem(dataX, dataY, width),
_img(0),
_imagesRef(0)
{
@@ -85,7 +85,7 @@ void PdfPlotItem::showDescription(bool shown)
if(_img)
_img->setVisible(false);
}
PlotItem::showDescription(shown);
UPlotItem::showDescription(shown);
}
@@ -93,7 +93,7 @@ void PdfPlotItem::showDescription(bool shown)
PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, QByteArray> * imagesMapRef = 0, QObject * parent) :
PlotCurve(name, parent),
UPlotCurve(name, parent),
_imagesMapRef(imagesMapRef)
{
@@ -106,7 +106,7 @@ PdfPlotCurve::~PdfPlotCurve()
void PdfPlotCurve::clear()
{
PlotCurve::clear();
UPlotCurve::clear();
}
void PdfPlotCurve::setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap, int lastId)
guilib/src/PdfPlot.h
+3 -3
View File
@@ -20,11 +20,11 @@
#ifndef PDFPLOT_H_
#define PDFPLOT_H_
#include "Plot.h"
#include <utilite/UPlot.h>
namespace rtabmap {
class PdfPlotItem : public PlotItem
class PdfPlotItem : public UPlotItem
{
public:
PdfPlotItem(float dataX, float dataY, float width, int childCount = -1);
@@ -47,7 +47,7 @@ private:
};
class PdfPlotCurve : public PlotCurve
class PdfPlotCurve : public UPlotCurve
{
Q_OBJECT
guilib/src/Plot.cpp
-2387
View File
File diff suppressed because it is too large Load Diff
guilib/src/Plot.h
-393
View File
@@ -1,393 +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 PLOT_H_
#define PLOT_H_
// Uncomment this to disable all references to RTAB-Map and UtiLite dependencies
//#define PLOT_WIDGET_OUT_OF_LIB
#include <QtGui/QFrame>
#include <QtCore/QList>
#include <QtCore/QMap>
#include <QtGui/QPen>
#include <QtGui/QBrush>
#include <QtGui/QGraphicsEllipseItem>
#include <QtCore/QMutex>
#include <QtGui/QLabel>
#include <QtGui/QPushButton>
#include <QtCore/QTime>
class QGraphicsView;
class QGraphicsScene;
class QGraphicsItem;
class QFormLayout;
#ifndef PLOT_WIDGET_OUT_OF_LIB
namespace rtabmap {
#endif
class PlotItem : public QGraphicsEllipseItem
{
public:
PlotItem(qreal dataX, qreal dataY, qreal width=2);
PlotItem(const QPointF & data, qreal width=2);
virtual ~PlotItem();
public:
void setNextItem(PlotItem * nextItem);
void setPreviousItem(PlotItem * previousItem);
void setData(const QPointF & data);
PlotItem * nextItem() const {return _nextItem;}
PlotItem * previousItem() const {return _previousItem;};
const QPointF & data() const {return _data;}
protected:
virtual void hoverEnterEvent(QGraphicsSceneHoverEvent * event);
virtual void hoverLeaveEvent(QGraphicsSceneHoverEvent * event);
virtual void focusInEvent(QFocusEvent * event);
virtual void focusOutEvent(QFocusEvent * event);
virtual void keyReleaseEvent(QKeyEvent * keyEvent);
virtual void showDescription(bool shown);
private:
QPointF _data;
QGraphicsTextItem * _text;
PlotItem * _previousItem;
PlotItem * _nextItem;
};
class Plot;
class PlotCurve : public QObject
{
Q_OBJECT
public:
PlotCurve(const QString & name, QObject * parent = 0);
PlotCurve(const QString & name, const QVector<PlotItem *> data, QObject * parent = 0);
PlotCurve(const QString & name, const QVector<float> & x, const QVector<float> & y, QObject * parent = 0);
virtual ~PlotCurve();
const QPen & pen() const {return _pen;}
const QBrush & brush() const {return _brush;}
void setPen(const QPen & pen);
void setBrush(const QBrush & brush);
void setDefaultStepX(float stepX) {_defaultStepX = stepX;}
QString name() const {return _name;}
int itemsSize() const;
QPointF getItemData(int index);
void setStartX(float startX) {_startX = startX;}
bool isVisible() const {return _visible;}
void setData(QVector<PlotItem*> & data); // take the ownership
void setData(const QVector<float> & x, const QVector<float> & y);
void getData(QVector<float> & x, QVector<float> & y) const; // only call in Qt MainThread
void draw(QPainter * painter);
public slots:
virtual void clear();
void setVisible(bool visible);
void addValue(PlotItem * data); // take the ownership
void addValue(float y);
void addValue(float x, float y);
void addValue(const QString & y);
void addValues(QVector<PlotItem *> & data); // take the ownership
void addValues(const QVector<float> & xs, const QVector<float> & ys);
void addValues(const QVector<float> & ys);
signals:
void dataChanged(const PlotCurve *);
protected:
friend class Plot;
void attach(Plot * plot);
void detach(Plot * plot);
void updateMinMax();
const QVector<float> & getMinMax() const {return _minMax;}
int removeItem(int index);
void _addValue(PlotItem * data);;
virtual bool isMinMaxValid() const {return _minMax.size();}
virtual void update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, bool allDataKept);
QList<QGraphicsItem *> _items;
Plot * _plot;
private:
void removeItem(PlotItem * item);
private:
QString _name;
QPen _pen;
QBrush _brush;
float _defaultStepX;
float _startX;
bool _visible;
bool _valuesShown;
QVector<float> _minMax; // minX, maxX, minY, maxY
};
class ThresholdCurve : public PlotCurve
{
Q_OBJECT
public:
ThresholdCurve(const QString & name, float thesholdValue, Qt::Orientation orientation = Qt::Horizontal, QObject * parent = 0);
virtual ~ThresholdCurve();
public slots:
void setThreshold(float threshold);
void setOrientation(Qt::Orientation orientation);
protected:
friend class Plot;
virtual void update(float scaleX, float scaleY, float offsetX, float offsetY, float xDir, float yDir, bool allDataKept);
virtual bool isMinMaxValid() const {return false;}
private:
Qt::Orientation _orientation;
};
class PlotAxis : public QWidget
{
public:
PlotAxis(Qt::Orientation orientation = Qt::Horizontal, float min=0, float max=1, QWidget * parent = 0);
virtual ~PlotAxis();
public:
void setAxis(float & min, float & max);
int border() const {return _border;}
int step() const {return _step;}
int count() const {return _count;}
void setReversed(bool reversed); // Vertical :bottom->up, horizontal :right->left
protected:
virtual void paintEvent(QPaintEvent * event);
private:
Qt::Orientation _orientation;
float _min;
float _max;
int _count;
int _step;
bool _reversed;
int _gradMaxDigits;
int _border;
};
class PlotLegendItem : public QPushButton
{
Q_OBJECT
public:
PlotLegendItem(const PlotCurve * curve, QWidget * parent = 0);
virtual ~PlotLegendItem();
const PlotCurve * curve() const {return _curve;}
signals:
void legendItemRemoved(const PlotCurve *);
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
private:
const PlotCurve * _curve;
QMenu * _menu;
QAction * _aChangeText;
QAction * _aResetText;
QAction * _aRemoveCurve;
QAction * _aCopyToClipboard;
};
class PlotLegend : public QWidget
{
Q_OBJECT
public:
PlotLegend(QWidget * parent = 0);
virtual ~PlotLegend();
void setFlat(bool on);
bool isFlat() const {return _flat;}
void addItem(const PlotCurve * curve);
QPixmap createSymbol(const QPen & pen, const QBrush & brush);
bool remove(const PlotCurve * curve);
public slots:
void removeLegendItem(const PlotCurve * curve);
signals:
void legendItemRemoved(const PlotCurve * curve);
void legendItemToggled(const PlotCurve * curve, bool toggled);
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
private slots:
void redirectToggled(bool);
private:
bool _flat;
QMenu * _menu;
QAction * _aUseFlatButtons;
};
class OrientableLabel : public QLabel
{
Q_OBJECT
public:
OrientableLabel(const QString & text, Qt::Orientation orientation = Qt::Horizontal, QWidget * parent = 0);
virtual ~OrientableLabel();
Qt::Orientation orientation() const {return _orientation;}
void setOrientation(Qt::Orientation orientation);
QSize sizeHint() const;
QSize minimumSizeHint() const;
protected:
virtual void paintEvent(QPaintEvent* event);
private:
Qt::Orientation _orientation;
};
/*
* TODO It could be cool to right-click on a dot in the
* plot to create a new plot to monitor the dot value changes.
*/
class Plot : public QWidget
{
Q_OBJECT
public:
Plot(QWidget * parent = 0);
virtual ~Plot();
PlotCurve * addCurve(const QString & curveName);
bool addCurve(PlotCurve * curves);
QStringList curveNames();
bool contains(const QString & curveName);
void removeCurves();
ThresholdCurve * addThreshold(const QString & name, float value, Qt::Orientation orientation = Qt::Horizontal);
QString title() const {return this->objectName();}
QPen getRandomPenColored();
void showLegend(bool shown);
void showGrid(bool shown);
void showRefreshRate(bool shown);
void keepAllData(bool kept);
void showXAxis(bool shown) {_horizontalAxis->setVisible(shown);}
void showYAxis(bool shown) {_verticalAxis->setVisible(shown);}
void setVariableXAxis() {_fixedAxis[0] = false;}
void setVariableYAxis() {_fixedAxis[1] = false;}
void setFixedXAxis(float x1, float x2);
void setFixedYAxis(float y1, float y2);
void setMaxVisibleItems(int maxVisibleItems);
void setTitle(const QString & text);
void setXLabel(const QString & text);
void setYLabel(const QString & text, Qt::Orientation orientation = Qt::Vertical);
void setWorkingDirectory(const QString & workingDirectory);
void setGraphicsView(bool on);
QRectF sceneRect() const;
public slots:
void removeCurve(const PlotCurve * curve);
void showCurve(const PlotCurve * curve, bool shown);
void updateAxis(); //reset axis and recompute it with all curves minMax
void clearData();
private slots:
void captureScreen();
void updateAxis(const PlotCurve * curve);
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
virtual void paintEvent(QPaintEvent * event);
virtual void resizeEvent(QResizeEvent * event);
private:
friend class PlotCurve;
void addItem(QGraphicsItem * item);
private:
void replot(QPainter * painter);
bool updateAxis(float x, float y);
bool updateAxis(float x1, float x2, float y1, float y2);
void setupUi();
void createActions();
void createMenus();
void selectScreenCaptureFormat();
private:
PlotLegend * _legend;
QGraphicsView * _view;
QGraphicsItem * _sceneRoot;
QWidget * _graphicsViewHolder;
float _axisMaximums[4]; // {x1->x2, y1->y2}
bool _axisMaximumsSet[4]; // {x1->x2, y1->y2}
bool _fixedAxis[2];
PlotAxis * _verticalAxis;
PlotAxis * _horizontalAxis;
int _penStyleCount;
int _maxVisibleItems;
QList<QGraphicsLineItem *> hGridLines;
QList<QGraphicsLineItem *> vGridLines;
QList<PlotCurve*> _curves;
QLabel * _title;
QLabel * _xLabel;
OrientableLabel * _yLabel;
QLabel * _refreshRate;
QString _workingDirectory;
QTime _refreshIntervalTime;
int _lowestRefreshRate;
QTime _refreshStartTime;
QString _autoScreenCaptureFormat;
QMenu * _menu;
QAction * _aShowLegend;
QAction * _aShowGrid;
QAction * _aKeepAllData;
QAction * _aLimit0;
QAction * _aLimit10;
QAction * _aLimit50;
QAction * _aLimit100;
QAction * _aLimit500;
QAction * _aLimit1000;
QAction * _aLimitCustom;
QAction * _aAddVerticalLine;
QAction * _aAddHorizontalLine;
QAction * _aChangeTitle;
QAction * _aChangeXLabel;
QAction * _aChangeYLabel;
QAction * _aYLabelVertical;
QAction * _aShowRefreshRate;
QAction * _aSaveFigure;
QAction * _aAutoScreenCapture;
QAction * _aClearData;
QAction * _aGraphicsView;
};
#ifndef PLOT_WIDGET_OUT_OF_LIB
}
#endif
#endif /* PLOT_H_ */
guilib/src/PreferencesDialog.cpp
+360 -73
View File
@@ -26,13 +26,13 @@
#include <QtGui/QMainWindow>
#include <QtCore/QTimer>
#include <QtGui/QProgressDialog>
#include "utilite/ULogger.h"
#include "ui_preferencesDialog.h"
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Parameters.h"
#include "utilite/UConversion.h"
#include "Plot.h"
#include "utilite/UStl.h"
#include <utilite/ULogger.h>
#include <utilite/UConversion.h>
#include <utilite/UPlot.h>
#include <utilite/UStl.h>
#define DEFAULT_GUI_IMAGES_KEPT true
#define DEFAULT_LOGGER_LEVEL 2
@@ -80,42 +80,64 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->horizontalSlider_keypointsOpacity, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
//Source panel
_ui->stackedWidget_2->setCurrentIndex(_ui->source_comboBox_type->currentIndex());
connect(_ui->source_comboBox_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_2, SLOT(setCurrentIndex(int)));
connect(_ui->source_comboBox_type, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
//Image source
connect(_ui->groupBox_sourceImage, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
_ui->stackedWidget_image->setCurrentIndex(_ui->source_comboBox_image_type->currentIndex());
connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_image, SLOT(setCurrentIndex(int)));
connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->general_checkBox_autoRestart, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->general_checkBox_cameraKeypoints, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//usbDevice group
connect(_ui->source_usbDevice_spinBox_id, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_imgWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_imgheight, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_spinBox_framesDropped, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//images group
connect(_ui->source_images_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSource()));
connect(_ui->source_images_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceImage()));
connect(_ui->source_images_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_images_spinBox_startPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_images_refreshDir, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//video group
connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSource()));
connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceImage()));
connect(_ui->source_video_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
//Audio source
connect(_ui->groupBox_sourceAudio, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
_ui->stackedWidget_audio->setCurrentIndex(_ui->source_comboBox_audio_type->currentIndex());
connect(_ui->source_comboBox_audio_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_audio, SLOT(setCurrentIndex(int)));
connect(_ui->source_comboBox_audio_type, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//audioDevice group
connect(_ui->source_audio_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceAudio()));
connect(_ui->source_micDevice, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_micFs, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_micSampleSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_audio_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_checkBox_playWhileRecording, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//database group
connect(_ui->source_database_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSource()));
connect(_ui->source_database_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_database_checkBox_loadActions, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_database_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceDatabase()));
connect(_ui->groupBox_sourceDatabase, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
//Rtabmap basic
connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
connect(_ui->general_doubleSpinBox_hardThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_hardThr_2, SLOT(setValue(double)));
connect(_ui->surf_doubleSpinBox_hessianThr, SIGNAL(valueChanged(double)), _ui->surf_doubleSpinBox_hessianThr_2, SLOT(setValue(double)));
connect(_ui->general_doubleSpinBox_similarityThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_similarityThr_2, SLOT(setValue(double)));
connect(_ui->general_spinBox_imagesBufferSize, SIGNAL(valueChanged(int)), _ui->general_spinBox_imagesBufferSize_2, SLOT(setValue(int)));
connect(_ui->general_checkBox_publishStats, SIGNAL(clicked(bool)), _ui->general_checkBox_publishStats_2, SLOT(setChecked(bool)));
connect(_ui->general_spinBox_maxStMemSize, SIGNAL(valueChanged(int)), _ui->general_spinBox_maxStMemSize_2, SLOT(setValue(int)));
connect(_ui->comboBox_signatureType, SIGNAL(currentIndexChanged(int)), _ui->comboBox_signatureType_2, SLOT(setCurrentIndex(int)));
connect(_ui->lineEdit_workingDirectory, SIGNAL(textChanged(const QString &)), _ui->lineEdit_workingDirectory_2, SLOT(setText(const QString &)));
connect(_ui->general_doubleSpinBox_timeThr_2, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr, SLOT(setValue(double)));
connect(_ui->general_doubleSpinBox_hardThr_2, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_hardThr, SLOT(setValue(double)));
connect(_ui->surf_doubleSpinBox_hessianThr_2, SIGNAL(valueChanged(double)), _ui->surf_doubleSpinBox_hessianThr, SLOT(setValue(double)));
connect(_ui->general_spinBox_imagesBufferSize_2, SIGNAL(valueChanged(int)), _ui->general_spinBox_imagesBufferSize, SLOT(setValue(int)));
connect(_ui->general_checkBox_publishStats_2, SIGNAL(clicked(bool)), _ui->general_checkBox_publishStats, SLOT(setChecked(bool)));
connect(_ui->general_doubleSpinBox_timeThr_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->general_doubleSpinBox_hardThr_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->surf_doubleSpinBox_hessianThr_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->general_doubleSpinBox_similarityThr_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->general_spinBox_imagesBufferSize_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->general_spinBox_maxStMemSize_2, SIGNAL(editingFinished()), this, SLOT(updateBasicParameter()));
connect(_ui->general_checkBox_publishStats, SIGNAL(stateChanged(int)), this, SLOT(updateBasicParameter()));
connect(_ui->general_checkBox_publishStats_2, SIGNAL(stateChanged(int)), this, SLOT(updateBasicParameter()));
connect(_ui->comboBox_signatureType_2, SIGNAL(currentIndexChanged(int)), _ui->comboBox_signatureType, SLOT(setCurrentIndex(int)));
connect(_ui->lineEdit_workingDirectory_2, SIGNAL(textChanged(const QString &)), _ui->lineEdit_workingDirectory, SLOT(setText(const QString &)));
connect(_ui->toolButton_workingDirectory_2, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));
@@ -123,7 +145,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Rtabmap
_ui->general_doubleSpinBox_retrievalThr->setObjectName(Parameters::kRtabmapRetrievalThr().c_str());
_ui->general_checkBox_publishStats->setObjectName(Parameters::kRtabmapPublishStats().c_str());
_ui->general_checkBox_publishImages->setObjectName(Parameters::kRtabmapPublishImages().c_str());
_ui->general_checkBox_publishRawData->setObjectName(Parameters::kRtabmapPublishRawData().c_str());
_ui->general_checkBox_publishPdf->setObjectName(Parameters::kRtabmapPublishPdf().c_str());
_ui->general_checkBox_publishLikelihood->setObjectName(Parameters::kRtabmapPublishLikelihood().c_str());
_ui->general_doubleSpinBox_timeThr->setObjectName(Parameters::kRtabmapTimeThr().c_str());
@@ -133,6 +155,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_neighborhoodSummation->setObjectName(Parameters::kRtabmapSelectionNeighborhoodSummationUsed().c_str());
_ui->general_checkBox_likelihoodUsed->setObjectName(Parameters::kRtabmapSelectionLikelihoodUsed().c_str());
_ui->general_checkBox_likelihoodStdDevRemoved->setObjectName(Parameters::kRtabmapLikelihoodStdDevRemoved().c_str());
_ui->general_checkBox_likelihoodNullValuesIgnored->setObjectName(Parameters::kRtabmapLikelihoodNullValuesIgnored().c_str());
_ui->general_checkBox_actionsSentRejectHyp->setObjectName(Parameters::kRtabmapActionsSentRejectHyp().c_str());
_ui->general_doubleSpinBox_confidenceThr->setObjectName(Parameters::kRtabmapConfidenceThr().c_str());
_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
@@ -182,7 +205,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->surf_spinBox_maxLeafs->setObjectName(Parameters::kKpMaxLeafs().c_str());
_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpWordsPerImage().c_str());
_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
_ui->checkBox_kp_usingAdaptiveResponseThr->setObjectName(Parameters::kKpUsingAdaptiveResponseThr().c_str());
_ui->general_checkBox_reactivatedWordsComparedToNewWords->setObjectName(Parameters::kKpReactivatedWordsComparedToNewWords().c_str());
_ui->checkBox_kp_tfIdfLikelihoodUsed->setObjectName(Parameters::kKpTfIdfLikelihoodUsed().c_str());
_ui->checkBox_kp_tfIdfNormalized->setObjectName(Parameters::kKpTfIdfNormalized().c_str());
@@ -196,8 +218,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->surf_spinBox_octaves->setObjectName(Parameters::kSURFOctaves().c_str());
_ui->surf_spinBox_octaveLayers->setObjectName(Parameters::kSURFOctaveLayers().c_str());
_ui->checkBox_surfExtended->setObjectName(Parameters::kSURFExtended().c_str());
_ui->surf_checkBox_gpuVersion->setObjectName(Parameters::kSURFGpuVersion().c_str());
_ui->surf_checkBox_upright->setObjectName(Parameters::kSURFUpright().c_str());
_ui->surf_checkBox_gpuVersion->setObjectName(Parameters::kSURFGpuVersion().c_str());
//Star detector
_ui->star_spinBox_maxSize->setObjectName(Parameters::kStarMaxSize().c_str());
@@ -207,8 +229,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->star_spinBox_suppressNonmaxSize->setObjectName(Parameters::kStarSuppressNonmaxSize().c_str());
//SIFT detector
_ui->sift_doubleSpinBox_Thr->setObjectName(Parameters::kSIFTThreshold().c_str());
_ui->sift_spinBox_nFeatures->setObjectName(Parameters::kSIFTNFeatures().c_str());
_ui->sift_spinBox_nOctaveLayers->setObjectName(Parameters::kSIFTNOctaveLayers().c_str());
_ui->sift_doubleSpinBox_contrastThr->setObjectName(Parameters::kSIFTContrastThreshold().c_str());
_ui->sift_doubleSpinBox_edgeThr->setObjectName(Parameters::kSIFTEdgeThreshold().c_str());
_ui->sift_doubleSpinBox_sigma->setObjectName(Parameters::kSIFTSigma().c_str());
//FAST detector
_ui->fast_spinBox_Threshold->setObjectName(Parameters::kFASTThreshold().c_str());
@@ -221,8 +246,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->checkBox_publishMasks->setObjectName(Parameters::kSMPublishMasks().c_str());
_ui->checkBox_motionMaskUsed->setObjectName(Parameters::kSMMotionMaskUsed().c_str());
_ui->checkBox_logpolar->setObjectName(Parameters::kSMLogPolarUsed().c_str());
_ui->checkBox_votingScheme->setObjectName(Parameters::kSMVotingSchemeUsed().c_str());
_ui->sm_colorTable_comboBox->setObjectName(Parameters::kSMColorTable().c_str());
_ui->sm_audioDBThreshold->setObjectName(Parameters::kSMAudioDBThreshold().c_str());
_ui->checkBox_dBIndexing->setObjectName(Parameters::kSMAudioDBIndexing().c_str());
_ui->checkBox_audioMagnitudeInvariant->setObjectName(Parameters::kSMMagnitudeInvariant().c_str());
// verifyHypotheses
_ui->comboBox_vh_strategy->setObjectName(Parameters::kRtabmapVhStrategy().c_str());
@@ -438,6 +465,7 @@ void PreferencesDialog::closeDialog ( QAbstractButton * button )
break;
case QDialogButtonBox::AcceptRole:
updateBasicParameter();// make that changes without editing finished signal are updated.
if((_obsoletePanels & kPanelAll) || _parameters.size())
{
if(validateForm())
@@ -463,6 +491,7 @@ void PreferencesDialog::resetApply ( QAbstractButton * button )
switch(role)
{
case QDialogButtonBox::ApplyRole:
updateBasicParameter();// make that changes without editing finished signal are updated.
if(validateForm())
{
writeSettings();
@@ -493,20 +522,38 @@ void PreferencesDialog::resetSettings(int panelNumber)
}
else if(boxes.at(panelNumber)->objectName() == "groupBox_source0")
{
_ui->groupBox_sourceImage->setChecked(true);
_ui->general_doubleSpinBox_imgRate->setValue(1.0);
_ui->source_spinBox_imgWidth->setValue(0);
_ui->source_spinBox_imgheight->setValue(0);
_ui->source_spinBox_framesDropped->setValue(0);
_ui->general_checkBox_autoRestart->setChecked(false);
_ui->general_checkBox_cameraKeypoints->setChecked(false);
_ui->groupBox_sourceAudio->setChecked(false);
_ui->source_micDevice->setValue(0);
_ui->source_micFs->setValue(48000);
_ui->source_micSampleSize->setValue(2);
_ui->groupBox_sourceDatabase->setChecked(false);
}
else if(boxes.at(panelNumber)->objectName() == "groupBox_rtabmap_basic0")
{
_ui->general_doubleSpinBox_timeThr_2->setValue(Parameters::defaultRtabmapTimeThr());
_ui->general_doubleSpinBox_hardThr_2->setValue(Parameters::defaultRtabmapLoopThr());
_ui->surf_doubleSpinBox_hessianThr_2->setValue(Parameters::defaultSURFHessianThreshold());
_ui->general_doubleSpinBox_similarityThr_2->setValue(Parameters::defaultMemSimilarityThr());
_ui->general_spinBox_imagesBufferSize_2->setValue(Parameters::defaultRtabmapSMStateBufferSize());
_ui->general_spinBox_maxStMemSize_2->setValue(Parameters::defaultMemMaxStMemSize());
_ui->general_checkBox_publishStats_2->setChecked(Parameters::defaultRtabmapPublishStats());
_ui->lineEdit_workingDirectory_2->setText(Parameters::defaultRtabmapWorkingDirectory().c_str());
// match the advanced (spin and doubleSpin boxes)
_ui->general_doubleSpinBox_timeThr->setValue(Parameters::defaultRtabmapTimeThr());
_ui->general_doubleSpinBox_hardThr->setValue(Parameters::defaultRtabmapLoopThr());
_ui->surf_doubleSpinBox_hessianThr->setValue(Parameters::defaultSURFHessianThreshold());
_ui->general_doubleSpinBox_similarityThr->setValue(Parameters::defaultMemSimilarityThr());
_ui->general_spinBox_imagesBufferSize->setValue(Parameters::defaultRtabmapSMStateBufferSize());
_ui->general_spinBox_maxStMemSize->setValue(Parameters::defaultMemMaxStMemSize());
}
else
{
@@ -600,12 +647,14 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
QSettings settings(path, QSettings::IniFormat);
settings.beginGroup("Camera");
_ui->groupBox_sourceImage->setChecked(settings.value("imageUsed", _ui->groupBox_sourceImage->isChecked()).toBool());
_ui->general_doubleSpinBox_imgRate->setValue(settings.value("imgRate", _ui->general_doubleSpinBox_imgRate->value()).toDouble());
_ui->general_checkBox_autoRestart->setChecked(settings.value("autoRestart", _ui->general_checkBox_autoRestart->isChecked()).toBool());
_ui->general_checkBox_cameraKeypoints->setChecked(settings.value("cameraKeypoints", _ui->general_checkBox_cameraKeypoints->isChecked()).toBool());
_ui->source_comboBox_type->setCurrentIndex(settings.value("type", _ui->source_comboBox_type->currentIndex()).toInt());
_ui->source_comboBox_image_type->setCurrentIndex(settings.value("type", _ui->source_comboBox_image_type->currentIndex()).toInt());
_ui->source_spinBox_imgWidth->setValue(settings.value("imgWidth",_ui->source_spinBox_imgWidth->value()).toInt());
_ui->source_spinBox_imgheight->setValue(settings.value("imgHeight",_ui->source_spinBox_imgheight->value()).toInt());
_ui->source_spinBox_framesDropped->setValue(settings.value("framesDropped",_ui->source_spinBox_framesDropped->value()).toInt());
//usbDevice group
settings.beginGroup("usbDevice");
_ui->source_usbDevice_spinBox_id->setValue(settings.value("id",_ui->source_usbDevice_spinBox_id->value()).toInt());
@@ -620,12 +669,22 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("video");
_ui->source_video_lineEdit_path->setText(settings.value("path", _ui->source_video_lineEdit_path->text()).toString());
settings.endGroup(); // video
//database group
settings.beginGroup("database");
_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
_ui->source_database_checkBox_loadActions->setChecked(settings.value("loadActions",_ui->source_database_checkBox_loadActions->isChecked()).toBool());
settings.endGroup(); // usbDevice
settings.endGroup(); // Camera
settings.beginGroup("Audio");
_ui->groupBox_sourceAudio->setChecked(settings.value("audioUsed", _ui->groupBox_sourceAudio->isChecked()).toBool());
_ui->source_comboBox_audio_type->setCurrentIndex(settings.value("type", _ui->source_comboBox_audio_type->currentIndex()).toInt());
_ui->source_micDevice->setValue(settings.value("id",_ui->source_micDevice->value()).toInt());
_ui->source_micFs->setValue(settings.value("fs",_ui->source_micFs->value()).toInt());
_ui->source_micSampleSize->setValue(settings.value("sampleSize",_ui->source_micSampleSize->value()).toInt());
_ui->source_audio_lineEdit_path->setText(settings.value("path",_ui->source_audio_lineEdit_path->text()).toString());
_ui->source_checkBox_playWhileRecording->setChecked(settings.value("playWhileRecording",_ui->source_checkBox_playWhileRecording->isChecked()).toBool());
settings.endGroup(); // Audio
settings.beginGroup("Database");
_ui->groupBox_sourceDatabase->setChecked(settings.value("databaseUsed", _ui->groupBox_sourceDatabase->isChecked()).toBool());
_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
settings.endGroup(); // Database
}
void PreferencesDialog::readCoreSettings(const QString & filePath)
@@ -740,13 +799,14 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
}
QSettings settings(path, QSettings::IniFormat);
settings.beginGroup("Camera");
settings.setValue("imageUsed", _ui->groupBox_sourceImage->isChecked());
settings.setValue("imgRate", _ui->general_doubleSpinBox_imgRate->value());
settings.setValue("autoRestart", _ui->general_checkBox_autoRestart->isChecked());
settings.setValue("cameraKeypoints", _ui->general_checkBox_cameraKeypoints->isChecked());
settings.setValue("type", _ui->source_comboBox_type->currentIndex());
settings.setValue("type", _ui->source_comboBox_image_type->currentIndex());
settings.setValue("imgWidth", _ui->source_spinBox_imgWidth->value());
settings.setValue("imgHeight", _ui->source_spinBox_imgheight->value());
settings.setValue("framesDropped", _ui->source_spinBox_framesDropped->value());
//usbDevice group
settings.beginGroup("usbDevice");
settings.setValue("id", _ui->source_usbDevice_spinBox_id->value());
@@ -761,13 +821,24 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
settings.beginGroup("video");
settings.setValue("path", _ui->source_video_lineEdit_path->text());
settings.endGroup(); //video
//database group
settings.beginGroup("database");
settings.setValue("path", _ui->source_database_lineEdit_path->text());
settings.setValue("loadActions", _ui->source_database_checkBox_loadActions->isChecked());
settings.endGroup(); //usbDevice
settings.endGroup(); // Camera
settings.beginGroup("Audio");
settings.setValue("audioUsed", _ui->groupBox_sourceAudio->isChecked());
settings.setValue("type", _ui->source_comboBox_audio_type->currentIndex());
settings.setValue("id", _ui->source_micDevice->value());
settings.setValue("fs", _ui->source_micFs->value());
settings.setValue("sampleSize", _ui->source_micSampleSize->value());
settings.setValue("path", _ui->source_audio_lineEdit_path->text());
settings.setValue("playWhileRecording", _ui->source_checkBox_playWhileRecording->isChecked());
settings.endGroup(); // Audio
settings.beginGroup("Database");
settings.setValue("databaseUsed", _ui->groupBox_sourceDatabase->isChecked());
settings.setValue("path", _ui->source_database_lineEdit_path->text());
settings.endGroup();
}
void PreferencesDialog::writeCoreSettings(const QString & filePath)
@@ -940,32 +1011,33 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri
return value;
}
void PreferencesDialog::selectSource(Src src)
void PreferencesDialog::selectSourceImage(Src src)
{
ULOGGER_DEBUG("PreferencesDialog::selectSource()");
ULOGGER_DEBUG("");
bool fromPrefDialog = false;
//bool modified = false;
if(src == kSrcUndef)
{
fromPrefDialog = true;
if(_ui->source_comboBox_type->currentIndex() == 1)
if(_ui->source_comboBox_image_type->currentIndex() == 1)
{
src = kSrcImages;
}
else if(_ui->source_comboBox_type->currentIndex() == 2)
else if(_ui->source_comboBox_image_type->currentIndex() == 2)
{
src = kSrcVideo;
}
else if(_ui->source_comboBox_type->currentIndex() == 3)
{
src = kSrcDatabase;
}
else
{
src = kSrcUsbDevice;
}
}
else
{
// from user
_ui->groupBox_sourceImage->setChecked(true);
}
if(src == kSrcImages)
{
@@ -979,7 +1051,7 @@ void PreferencesDialog::selectSource(Src src)
QFileInfoList files = dir.entryInfoList();
if(!files.empty())
{
_ui->source_comboBox_type->setCurrentIndex(1);
_ui->source_comboBox_image_type->setCurrentIndex(1);
_ui->source_images_lineEdit_path->setText(path);
_ui->source_images_spinBox_startPos->setValue(1);
_ui->source_images_refreshDir->setChecked(false);
@@ -994,31 +1066,107 @@ void PreferencesDialog::selectSource(Src src)
}
else if(src == kSrcVideo)
{
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg)"));
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg *.mp4)"));
QFile file(path);
if(!path.isEmpty() && file.exists())
{
_ui->source_comboBox_type->setCurrentIndex(2);
_ui->source_comboBox_image_type->setCurrentIndex(2);
_ui->source_video_lineEdit_path->setText(path);
}
}
else if(src == kSrcDatabase)
{
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_database_lineEdit_path->text(), tr("Databases (*.db)"));
QFile file(path);
if(!path.isEmpty() && file.exists())
{
_ui->source_comboBox_type->setCurrentIndex(3);
_ui->source_database_lineEdit_path->setText(path);
}
}
else
{
_ui->source_comboBox_type->setCurrentIndex(0);
_ui->source_comboBox_image_type->setCurrentIndex(0);
}
if(!fromPrefDialog && _obsoletePanels)
{
_ui->groupBox_sourceDatabase->setChecked(false);
if(validateForm())
{
this->writeSettings();
}
else
{
this->readSettingsBegin();
}
}
}
void PreferencesDialog::selectSourceAudio(SrcAudio src)
{
ULOGGER_DEBUG("");
bool fromPrefDialog = false;
//bool modified = false;
if(src == kSrcAudioUndef)
{
fromPrefDialog = true;
if(_ui->source_comboBox_audio_type->currentIndex() == 1)
{
src = kSrcAudioFile;
}
else
{
src = kSrcAudioMicDevice;
}
}
else
{
// from user
_ui->groupBox_sourceAudio->setChecked(true);
}
if(src == kSrcAudioFile)
{
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_audio_lineEdit_path->text(), tr("Audio files (*.wav *.mp3)"));
QFile file(path);
if(!path.isEmpty() && file.exists())
{
_ui->source_comboBox_audio_type->setCurrentIndex(1);
_ui->source_audio_lineEdit_path->setText(path);
}
}
else
{
_ui->source_comboBox_audio_type->setCurrentIndex(0);
}
if(!fromPrefDialog && _obsoletePanels)
{
_ui->groupBox_sourceDatabase->setChecked(false);
if(validateForm())
{
this->writeSettings();
}
else
{
this->readSettingsBegin();
}
}
}
void PreferencesDialog::selectSourceDatabase(bool user)
{
ULOGGER_DEBUG("");
if(user)
{
// from user
_ui->groupBox_sourceDatabase->setChecked(true);
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_database_lineEdit_path->text(), tr("RTAB-Map database files (*.db)"));
QFile file(path);
if(!path.isEmpty() && file.exists())
{
_ui->source_database_lineEdit_path->setText(path);
}
if(user && _obsoletePanels)
{
_ui->groupBox_sourceImage->setChecked(false);
_ui->groupBox_sourceAudio->setChecked(false);
if(validateForm())
{
this->writeSettings();
@@ -1166,13 +1314,13 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
{
if(value == 0) // 0 surf
{
this->addParameters(_ui->groupBox_descriptor_surf2);
this->addParameters(_ui->groupBox_detector_surf2);
}
else if(value == 3) // 1 sift
else if(value == 1) // 1 sift
{
// no panel
this->addParameters(_ui->groupBox_detector_sift2);
}
else if(value == 5) // 2 brief
else if(value == 2) // 2 brief
{
this->addParameters(_ui->groupBox_descriptor_brief2);
}
@@ -1284,6 +1432,55 @@ void PreferencesDialog::addParameters(const QGroupBox * box)
}
}
void PreferencesDialog::updateBasicParameter()
{
// This method is used to update basic/advanced referred parameters, see above editingFinished()
// basic to advanced (advanced to basic must be done by connecting signal valueChanged())
if(sender() == _ui->general_doubleSpinBox_timeThr_2)
{
_ui->general_doubleSpinBox_timeThr->setValue(_ui->general_doubleSpinBox_timeThr_2->value());
}
else if(sender() == _ui->general_doubleSpinBox_hardThr_2)
{
_ui->general_doubleSpinBox_hardThr->setValue(_ui->general_doubleSpinBox_hardThr_2->value());
}
else if(sender() == _ui->surf_doubleSpinBox_hessianThr_2)
{
_ui->surf_doubleSpinBox_hessianThr->setValue(_ui->surf_doubleSpinBox_hessianThr_2->value());
}
else if(sender() == _ui->general_doubleSpinBox_similarityThr_2)
{
_ui->general_doubleSpinBox_similarityThr->setValue(_ui->general_doubleSpinBox_similarityThr_2->value());
}
else if(sender() == _ui->general_spinBox_imagesBufferSize_2)
{
_ui->general_spinBox_imagesBufferSize->setValue(_ui->general_spinBox_imagesBufferSize_2->value());
}
else if(sender() == _ui->general_spinBox_maxStMemSize_2)
{
_ui->general_spinBox_maxStMemSize->setValue(_ui->general_spinBox_maxStMemSize_2->value());
}
else if(sender() == _ui->general_checkBox_publishStats)
{
_ui->general_checkBox_publishStats_2->setChecked(_ui->general_checkBox_publishStats->isChecked());
}
else if(sender() == _ui->general_checkBox_publishStats_2)
{
_ui->general_checkBox_publishStats->setChecked(_ui->general_checkBox_publishStats_2->isChecked());
}
else
{
//update all values (only those using editingFinished signal)
_ui->general_doubleSpinBox_timeThr->setValue(_ui->general_doubleSpinBox_timeThr_2->value());
_ui->general_doubleSpinBox_hardThr->setValue(_ui->general_doubleSpinBox_hardThr_2->value());
_ui->surf_doubleSpinBox_hessianThr->setValue(_ui->surf_doubleSpinBox_hessianThr_2->value());
_ui->general_doubleSpinBox_similarityThr->setValue(_ui->general_doubleSpinBox_similarityThr_2->value());
_ui->general_spinBox_imagesBufferSize->setValue(_ui->general_spinBox_imagesBufferSize_2->value());
_ui->general_spinBox_maxStMemSize->setValue(_ui->general_spinBox_maxStMemSize_2->value());
}
}
void PreferencesDialog::makeObsoleteGeneralPanel()
{
ULOGGER_DEBUG("");
@@ -1292,6 +1489,16 @@ void PreferencesDialog::makeObsoleteGeneralPanel()
void PreferencesDialog::makeObsoleteSourcePanel()
{
if(sender() == _ui->groupBox_sourceDatabase && _ui->groupBox_sourceDatabase->isChecked())
{
_ui->groupBox_sourceImage->setChecked(false);
_ui->groupBox_sourceAudio->setChecked(false);
}
else if((sender() == _ui->groupBox_sourceImage && _ui->groupBox_sourceImage->isChecked()) ||
(sender() == _ui->groupBox_sourceAudio && _ui->groupBox_sourceAudio->isChecked()))
{
_ui->groupBox_sourceDatabase->setChecked(false);
}
ULOGGER_DEBUG("");
_obsoletePanels = _obsoletePanels | kPanelSource;
}
@@ -1399,7 +1606,7 @@ void PreferencesDialog::updatePredictionPlot()
}
_ui->predictionPlot->removeCurves();
_ui->predictionPlot->addCurve(new PlotCurve("Prediction", dataX, dataY, _ui->predictionPlot));
_ui->predictionPlot->addCurve(new UPlotCurve("Prediction", dataX, dataY, _ui->predictionPlot));
}
void PreferencesDialog::setupKpRoiPanel()
@@ -1501,10 +1708,22 @@ int PreferencesDialog::getKeypointsOpacity() const
}
// Source
double PreferencesDialog::getGeneralImageRate() const
double PreferencesDialog::getGeneralInputRate() const
{
return _ui->general_doubleSpinBox_imgRate->value();
}
bool PreferencesDialog::isSourceImageUsed() const
{
return _ui->groupBox_sourceImage->isChecked();
}
bool PreferencesDialog::isSourceAudioUsed() const
{
return _ui->groupBox_sourceAudio->isChecked();
}
bool PreferencesDialog::isSourceDatabaseUsed() const
{
return _ui->groupBox_sourceDatabase->isChecked();
}
bool PreferencesDialog::getGeneralAutoRestart() const
{
return _ui->general_checkBox_autoRestart->isChecked();
@@ -1513,13 +1732,13 @@ bool PreferencesDialog::getGeneralCameraKeypoints() const
{
return _ui->general_checkBox_cameraKeypoints->isChecked();
}
int PreferencesDialog::getSourceType() const
int PreferencesDialog::getSourceImageType() const
{
return _ui->source_comboBox_type->currentIndex();
return _ui->source_comboBox_image_type->currentIndex();
}
QString PreferencesDialog::getSourceTypeStr() const
QString PreferencesDialog::getSourceImageTypeStr() const
{
return _ui->source_comboBox_type->currentText();
return _ui->source_comboBox_image_type->currentText();
}
int PreferencesDialog::getSourceWidth() const
{
@@ -1529,6 +1748,10 @@ int PreferencesDialog::getSourceHeight() const
{
return _ui->source_spinBox_imgheight->value();
}
int PreferencesDialog::getFramesDropped() const
{
return _ui->source_spinBox_framesDropped->value();
}
QString PreferencesDialog::getSourceImagesPath() const
{
return _ui->source_images_lineEdit_path->text();
@@ -1549,15 +1772,43 @@ int PreferencesDialog::getSourceUsbDeviceId() const
{
return _ui->source_usbDevice_spinBox_id->value();
}
int PreferencesDialog::getSourceAudioType() const
{
return _ui->source_comboBox_audio_type->currentIndex();
}
QString PreferencesDialog::getSourceAudioTypeStr() const
{
return _ui->source_comboBox_audio_type->currentText();
}
int PreferencesDialog::getSourceMicDevice() const
{
return _ui->source_micDevice->value();
}
int PreferencesDialog::getSourceMicFs() const
{
return _ui->source_micFs->value();
}
int PreferencesDialog::getSourceMicSampleSize() const
{
return _ui->source_micSampleSize->value();
}
QString PreferencesDialog::getSourceAudioPath() const
{
return _ui->source_audio_lineEdit_path->text();
}
bool PreferencesDialog::getSourceAudioPlayWhileRecording() const
{
return _ui->source_checkBox_playWhileRecording->isChecked();
}
QString PreferencesDialog::getSourceDatabasePath() const
{
return _ui->source_database_lineEdit_path->text();
}
bool PreferencesDialog::getSourceDatabaseLoadActions() const
{
return _ui->source_database_checkBox_loadActions->isChecked();
}
bool PreferencesDialog::isStatisticsPublished() const
{
return _ui->general_checkBox_publishStats->isChecked();
}
double PreferencesDialog::getLoopThr() const
{
return _ui->general_doubleSpinBox_hardThr->value();
@@ -1579,6 +1830,10 @@ float PreferencesDialog::getTimeLimit() const
{
return _ui->general_doubleSpinBox_timeThr->value();
}
int PreferencesDialog::getMemoryType() const
{
return _ui->comboBox_signatureType->currentIndex();
}
/*** SETTERS ***/
void PreferencesDialog::setHardThr(int value)
@@ -1617,7 +1872,7 @@ void PreferencesDialog::setRetrievalThr(int value)
}
}
void PreferencesDialog::setImgRate(double value)
void PreferencesDialog::setInputRate(double value)
{
ULOGGER_DEBUG("imgRate=%2.2f", value);
if(_ui->general_doubleSpinBox_imgRate->value() != value)
@@ -1668,4 +1923,36 @@ void PreferencesDialog::setTimeLimit(float value)
}
}
void PreferencesDialog::disableSourceAudio()
{
if(_ui->groupBox_sourceAudio->isChecked())
{
_ui->groupBox_sourceAudio->setChecked(false);
if(validateForm())
{
this->writeSettings();
}
else
{
this->readSettingsBegin();
}
}
}
void PreferencesDialog::disableGeneralCameraKeypoints()
{
if(_ui->general_checkBox_cameraKeypoints->isChecked())
{
_ui->general_checkBox_cameraKeypoints->setChecked(false);
if(validateForm())
{
this->writeSettings();
}
else
{
this->readSettingsBegin();
}
}
}
}
guilib/src/StatsToolBox.cpp
+11 -11
View File
@@ -31,8 +31,8 @@
#include <QtGui/QToolBox>
#include <QtGui/QDialog>
#include "Plot.h"
#include "utilite/ULogger.h"
#include <utilite/UPlot.h>
#include <utilite/ULogger.h>
namespace rtabmap {
@@ -54,7 +54,7 @@ StatItem::~StatItem()
void StatItem::setValue(float x, float y)
{
_value->setText(QString::number(y, 'g', 4));
_value->setText(QString::number(y, 'g', 3));
_xValue = x;
emit valueChanged(x,y);
}
@@ -247,17 +247,17 @@ void StatsToolBox::updateStat(const QString & statFullName, float x, float y)
void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
{
QWidget * fig = _figures.value(plotName, (QWidget*)0);
Plot * plot = 0;
UPlot * plot = 0;
if(fig)
{
plot = fig->findChild<Plot *>(plotName);
plot = fig->findChild<UPlot *>(plotName);
}
if(plot)
{
// if not already in the plot
if(!plot->contains(stat->objectName()))
{
PlotCurve * curve = new PlotCurve(stat->objectName(), plot);
UPlotCurve * curve = new UPlotCurve(stat->objectName(), plot);
curve->setPen(plot->getRandomPenColored());
connect(stat, SIGNAL(valueChanged(float, float)), curve, SLOT(addValue(float, float)));
if(!plot->addCurve(curve))
@@ -289,7 +289,7 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
figure->setAttribute(Qt::WA_DeleteOnClose, true);
connect(figure, SIGNAL(destroyed(QObject*)), this, SLOT(figureDeleted(QObject*)));
//Plot
Plot * newPlot = new Plot(figure);
UPlot * newPlot = new UPlot(figure);
newPlot->setWorkingDirectory(_workingDirectory);
newPlot->setMaxVisibleItems(50);
newPlot->setObjectName(newPlotName);
@@ -298,7 +298,7 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
figure->setSizeGripEnabled(true);
//Add a new curve linked to the statBox
PlotCurve * curve = new PlotCurve(stat->objectName(), newPlot);
UPlotCurve * curve = new UPlotCurve(stat->objectName(), newPlot);
curve->setPen(newPlot->getRandomPenColored());
connect(stat, SIGNAL(valueChanged(float, float)), curve, SLOT(addValue(float, float)));
if(!newPlot->addCurve(curve))
@@ -358,7 +358,7 @@ void StatsToolBox::contextMenuEvent(QContextMenuEvent * event)
{
for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
{
QList<Plot *> plots = i.value()->findChildren<Plot *>();
QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
if(plots.size() == 1)
{
QStringList names = plots[0]->curveNames();
@@ -403,7 +403,7 @@ void StatsToolBox::getFiguresSetup(QList<int> & curvesPerFigure, QStringList & c
curveNames.clear();
for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
{
QList<Plot *> plots = i.value()->findChildren<Plot *>();
QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
if(plots.size() == 1)
{
QStringList names = plots[0]->curveNames();
@@ -449,7 +449,7 @@ void StatsToolBox::setWorkingDirectory(const QString & workingDirectory)
_workingDirectory = workingDirectory;
for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
{
QList<Plot *> plots = i.value()->findChildren<Plot *>();
QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
if(plots.size() == 1)
{
plots[0]->setWorkingDirectory(_workingDirectory);
guilib/src/TwistWidget.cpp
+15 -14
View File
@@ -30,22 +30,26 @@ void TwistGridWidget::addTwist(float x, float y, float z, float roll, float pitc
{
if(_grid->itemAtPosition(row, col+1))
{
QLayoutItem * item = _grid->itemAtPosition(row, col+1);
if(item)
{
QWidget * w = item->widget();
if(w)
{
_grid->removeItem(item);
w->deleteLater();
}
}
TwistWidget * w = (TwistWidget*)_grid->itemAtPosition(row, col+1)->widget();
w->setData(x, y, z, roll, pitch, yaw);
this->update();
}
else
{
_grid->addWidget(new TwistWidget(x, y, z, roll, pitch, yaw, this), row, col+1);
}
_grid->addWidget(new TwistWidget(x, y, z, roll, pitch, yaw, this), row, col+1);
}
TwistWidget::TwistWidget(float x, float y, float z, float roll, float pitch, float yaw, QWidget * parent, Qt::WindowFlags f) :
QWidget(parent, f)
{
this->setData(x, y, z, roll, pitch, yaw);
this->setFixedSize(SIZE,SIZE);
this->setMinimumSize(SIZE,SIZE);
}
void TwistWidget::setData(float x, float y, float z, float roll, float pitch, float yaw)
{
if(qAbs(z) > 0.00001)
{
@@ -60,9 +64,6 @@ TwistWidget::TwistWidget(float x, float y, float z, float roll, float pitch, flo
_roll = roll;
_pitch = pitch;
_yaw = yaw;
this->setFixedSize(SIZE,SIZE);
this->setMinimumSize(SIZE,SIZE);
}
void TwistWidget::paintEvent(QPaintEvent * event)
guilib/src/TwistWidget.h
+2
View File
@@ -36,6 +36,8 @@ public:
TwistWidget(float x, float y, float z, float roll, float pitch, float yaw, QWidget * parent, Qt::WindowFlags f = 0);
virtual ~TwistWidget() {}
void setData(float x, float y, float z, float roll, float pitch, float yaw);
protected:
virtual void paintEvent(QPaintEvent * event);
guilib/src/qtipl.cpp
+4 -4
View File
@@ -17,14 +17,14 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "qtipl.h"
#include "rtabmap/gui/qtipl.h"
#include "utilite/ULogger.h"
#include <opencv2/core/core_c.h>
namespace rtabmap {
// TODO : support only from gray 8bits ?
QImage Ipl2QImage(const IplImage *newImage)
QImage Ipl2QImage(const IplImage *newImage, int alpha)
{
QImage qtemp;
if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
@@ -33,13 +33,13 @@ QImage Ipl2QImage(const IplImage *newImage)
int y;
char* data = newImage->imageData;
qtemp= QImage(newImage->width, newImage->height,QImage::Format_RGB32 );
qtemp= QImage(newImage->width, newImage->height,QImage::Format_ARGB32 );
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]);
*p = qRgba(data[x * newImage->nChannels+2], data[x * newImage->nChannels+1],data[x * newImage->nChannels], alpha);
}
}
}
guilib/src/ui/mainWindow.ui
+229 -155
View File
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>842</width>
<height>457</height>
<width>1056</width>
<height>642</height>
</rect>
</property>
<property name="windowTitle">
@@ -21,7 +21,7 @@
<set>QMainWindow::AllowNestedDocks|QMainWindow::AllowTabbedDocks|QMainWindow::AnimatedDocks|QMainWindow::VerticalTabs</set>
</property>
<widget class="QWidget" name="centralwidget">
<layout class="QHBoxLayout" name="horizontalLayout_5">
<layout class="QHBoxLayout" name="horizontalLayout_3">
<property name="spacing">
<number>0</number>
</property>
@@ -29,100 +29,70 @@
<number>0</number>
</property>
<item>
<widget class="QFrame" name="frame">
<property name="frameShape">
<enum>QFrame::Box</enum>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="spacing">
<number>0</number>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_9">
<item>
<widget class="QLabel" name="label_refId">
<property name="minimumSize">
<size>
<width>0</width>
<height>18</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="rtabmap::ImageView" name="imageView_source">
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
</widget>
</item>
</layout>
</widget>
<item>
<widget class="QLabel" name="label_refId">
<property name="minimumSize">
<size>
<width>0</width>
<height>18</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="rtabmap::ImageView" name="imageView_source">
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QFrame" name="frame_2">
<property name="frameShape">
<enum>QFrame::Box</enum>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="spacing">
<number>0</number>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
<widget class="QLabel" name="label_matchId">
<property name="minimumSize">
<size>
<width>0</width>
<height>18</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="rtabmap::ImageView" name="imageView_loopClosure">
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
</widget>
</item>
</layout>
</widget>
<item>
<widget class="QLabel" name="label_matchId">
<property name="minimumSize">
<size>
<width>0</width>
<height>18</height>
</size>
</property>
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="rtabmap::ImageView" name="imageView_loopClosure">
<property name="verticalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAlwaysOff</enum>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
@@ -131,8 +101,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>842</width>
<height>22</height>
<width>1056</width>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -145,6 +115,20 @@
<property name="title">
<string>Edit</string>
</property>
<widget class="QMenu" name="menuAspect_ratio">
<property name="title">
<string>Aspect ratio</string>
</property>
<addaction name="action16_9"/>
<addaction name="action16_10"/>
<addaction name="action4_3"/>
<addaction name="separator"/>
<addaction name="action1080p"/>
<addaction name="action720p"/>
<addaction name="action480p"/>
<addaction name="action360p"/>
<addaction name="action240p"/>
</widget>
<addaction name="actionApply_settings_to_the_detector"/>
<addaction name="actionClear_cache"/>
<addaction name="separator"/>
@@ -157,6 +141,7 @@
<addaction name="actionReset_the_memory"/>
<addaction name="separator"/>
<addaction name="actionAuto_screen_capture"/>
<addaction name="menuAspect_ratio"/>
</widget>
<widget class="QMenu" name="menu6">
<property name="title">
@@ -174,9 +159,23 @@
<property name="title">
<string>Select source</string>
</property>
<addaction name="actionStream"/>
<addaction name="actionImages"/>
<addaction name="actionVideo"/>
<widget class="QMenu" name="menuImage">
<property name="title">
<string>Image</string>
</property>
<addaction name="actionUsbCamera"/>
<addaction name="actionImageFiles"/>
<addaction name="actionVideo"/>
</widget>
<widget class="QMenu" name="menuAudio">
<property name="title">
<string>Audio</string>
</property>
<addaction name="actionMic"/>
<addaction name="actionAudioFile"/>
</widget>
<addaction name="menuImage"/>
<addaction name="menuAudio"/>
<addaction name="actionDatabase"/>
</widget>
<addaction name="menuSelect_source"/>
@@ -230,7 +229,7 @@
<widget class="QWidget" name="dockWidgetContents">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="rtabmap::Plot" name="posteriorPlot" native="true"/>
<widget class="UPlot" name="posteriorPlot" native="true"/>
</item>
</layout>
</widget>
@@ -491,7 +490,7 @@
<widget class="QWidget" name="dockWidgetContents_4">
<layout class="QVBoxLayout" name="verticalLayout_6">
<item>
<widget class="rtabmap::Plot" name="likelihoodPlot" native="true"/>
<widget class="UPlot" name="likelihoodPlot" native="true"/>
</item>
</layout>
</widget>
@@ -529,55 +528,41 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_audioProcessedFrames">
<property name="windowTitle">
<string>Audio Processed Frames</string>
</property>
<attribute name="dockWidgetArea">
<number>2</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_9">
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<widget class="USpectrogram" name="widget_audioProcessedFrames" native="true"/>
</item>
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_audioLoopFrames">
<property name="windowTitle">
<string>Audio Loop Frames</string>
</property>
<attribute name="dockWidgetArea">
<number>2</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_10">
<layout class="QVBoxLayout" name="verticalLayout_11">
<item>
<widget class="USpectrogram" name="widget_audioLoopFrames" native="true"/>
</item>
</layout>
</widget>
</widget>
<action name="actionExit">
<property name="text">
<string>Exit</string>
</property>
</action>
<action name="actionStream">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Usb device</string>
</property>
</action>
<action name="actionVideo">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Video...</string>
</property>
</action>
<action name="actionImages">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Images...</string>
</property>
</action>
<action name="actionSURF">
<property name="text">
<string>SURF</string>
</property>
</action>
<action name="actionFourier">
<property name="text">
<string>Fourier</string>
</property>
</action>
<action name="actionSIFT">
<property name="text">
<string>SIFT</string>
</property>
</action>
<action name="actionCenSurE">
<property name="text">
<string>CenSurE</string>
</property>
</action>
<action name="actionHelp">
<property name="enabled">
<bool>false</bool>
@@ -616,6 +601,9 @@
<property name="text">
<string>Pause</string>
</property>
<property name="toolTip">
<string>Pause</string>
</property>
</action>
<action name="actionPause_on_match">
<property name="checkable">
@@ -653,14 +641,6 @@
<string>Dump the memory</string>
</property>
</action>
<action name="actionDatabase">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Database...</string>
</property>
</action>
<action name="actionPause_when_a_loop_hypothesis_is_rejected">
<property name="checkable">
<bool>true</bool>
@@ -716,18 +696,106 @@
<string>Show working directory in file browser</string>
</property>
</action>
<action name="action16_9">
<property name="text">
<string>16:9</string>
</property>
</action>
<action name="action16_10">
<property name="text">
<string>16:10</string>
</property>
</action>
<action name="action4_3">
<property name="text">
<string>4:3</string>
</property>
</action>
<action name="action360p">
<property name="text">
<string>360p</string>
</property>
</action>
<action name="action480p">
<property name="text">
<string>480p</string>
</property>
</action>
<action name="action720p">
<property name="text">
<string>720p</string>
</property>
</action>
<action name="action1080p">
<property name="text">
<string>1080p</string>
</property>
</action>
<action name="action240p">
<property name="text">
<string>240p</string>
</property>
</action>
<action name="actionUsbCamera">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Usb camera</string>
</property>
</action>
<action name="actionImageFiles">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Images...</string>
</property>
</action>
<action name="actionVideo">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Video...</string>
</property>
</action>
<action name="actionMic">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Mic</string>
</property>
</action>
<action name="actionAudioFile">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>File...</string>
</property>
</action>
<action name="actionDatabase">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Database...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
<class>rtabmap::Plot</class>
<class>UPlot</class>
<extends>QWidget</extends>
<header>Plot.h</header>
<header>utilite/UPlot.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>rtabmap::ImageView</class>
<extends>QGraphicsView</extends>
<header>ImageView.h</header>
<header>../include/rtabmap/gui/ImageView.h</header>
</customwidget>
<customwidget>
<class>rtabmap::StatsToolBox</class>
@@ -747,6 +815,12 @@
<header>TwistWidget.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>USpectrogram</class>
<extends>QWidget</extends>
<header>utilite/USpectrogram.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources>
<include location="../GuiLib.qrc"/>
guilib/src/ui/preferencesDialog.ui
+1044 -719
View File
File diff suppressed because it is too large Load Diff
corelib/tests/CMakeLists.txt → tests/CMakeLists.txt
+2 -4
View File
@@ -5,9 +5,7 @@ SET(SRC_FILES
)
SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}/../src
${PROJECT_SOURCE_DIR}/corelib/include
${CPPUNIT_INCLUDE_DIR}
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
@@ -29,7 +27,7 @@ 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})
TARGET_LINK_LIBRARIES(testCoreLib rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES( testCoreLib
PROPERTIES
corelib/tests/Tests.cpp → tests/Tests.cpp
+42 -81
View File
@@ -23,17 +23,16 @@
//Headers for the test BEGIN
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Signature.h"
#include "VWDictionary.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/SMState.h"
#include "VerifyHypotheses.h"
#include "rtabmap/core/VerifyHypotheses.h"
#include "rtabmap/core/Parameters.h"
#include "BayesFilter.h"
#include "KeypointMemory.h"
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/KeypointMemory.h"
#include "rtabmap/core/VisualWord.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/DBDriverFactory.h"
#include "rtabmap/core/DBDriver.h"
@@ -68,7 +67,7 @@ void Tests::testAvpd()
CPPUNIT_ASSERT_MESSAGE("Camera initialization failed!\n", camera.init());
/* Create tasks */
Rtabmap ctabmap;
Rtabmap rtabmap;
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kRtabmapSMStateBufferSize(), "0"));
parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "true"));
@@ -77,31 +76,26 @@ void Tests::testAvpd()
parameters.insert(ParametersPair(Parameters::kRtabmapTimeThr(), "0"));
parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "500"));
UDirectory::makeDir("./LogTestAvpdCore/");
ctabmap.setWorkingDirectory("./LogTestAvpdCore/");
ctabmap.init(parameters);
rtabmap.setWorkingDirectory("./LogTestAvpdCore/");
rtabmap.init(parameters);
/* Start thread's task */
SMState * smState = 0;
cv::Mat image;
smState = camera.takeSMState();
image = camera.takeImage();
int imgCount = 0;
while(smState)
while(!image.empty())
{
++imgCount;
printf("Processing image %d/84...\n", imgCount);
ctabmap.process(smState);
smState = camera.takeSMState();
}
if(smState)
{
delete smState;
smState = 0;
rtabmap.process(Sensor(image, Sensor::kTypeImage));
image = camera.takeImage();
}
CPPUNIT_ASSERT(imgCount == 84);
ctabmap.dumpData();
rtabmap.dumpData();
ULogger::write("testSurfStrategy end...");
//ULOGGER_INFO("\nTime = %fs", timer.ticks());
@@ -115,7 +109,7 @@ void Tests::testAvpd()
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);
rtabmap.adjustLikelihood(likelihood);
CPPUNIT_ASSERT(likelihood.size() == 4);
std::vector<float> values = uValues(likelihood);
for(unsigned int i=0; i<values.size(); ++i)
@@ -127,7 +121,7 @@ void Tests::testAvpd()
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);
rtabmap.adjustLikelihood(likelihood);
CPPUNIT_ASSERT(likelihood.size() == 4);
values = uValues(likelihood);
// Result wanted generated by the TestAdjustLikelihood.m script (MatLab/Tests)
@@ -142,7 +136,7 @@ void Tests::testAvpd()
void Tests::testCamera()
{
std::string path;
SMState * smState = 0;
cv::Mat image;
int count;
//CameraVideo class FIXME add a video in svn and reactivate this test
@@ -172,33 +166,14 @@ void Tests::testCamera()
CameraImages cameraImages(path, 1, false, 0, false, 80);
CPPUNIT_ASSERT( cameraImages.init() );
CPPUNIT_ASSERT( cameraImages.isIdle() == true);
smState = cameraImages.takeSMState();
image = cameraImages.takeImage();
count = 0;
while(smState)
while(!image.empty())
{
delete smState;
smState = 0;
++count;
smState = cameraImages.takeSMState();
image = cameraImages.takeImage();
}
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()
@@ -232,26 +207,26 @@ void Tests::testSqlite3Database()
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;
std::list<std::vector<int> > sensors;
std::list<std::vector<int> > sensors1;
std::list<std::vector<int> > sensors2;
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);
cv::Mat image = camera.takeImage();
CPPUNIT_ASSERT(!image.empty());
std::list<Sensor> rawData;
rawData.push_back(Sensor(image, Sensor::kTypeImage));
SMSignature * sm1 = new SMSignature(sensors, 1, rawData);
sensors1 = sm1->getData();
sm1->addNeighbor(NeighborLink(2));
image = camera.takeImage();
CPPUNIT_ASSERT(image);
SMSignature * sm2 = new SMSignature(sensors, motionMask, 2, image, keepRawData);
sensors2 = sm2->getSensors();
cvReleaseImage(&image);
CPPUNIT_ASSERT(!image.empty());
rawData.clear();
rawData.push_back(Sensor(image, Sensor::kTypeImage));
SMSignature * sm2 = new SMSignature(sensors, 2, rawData);
sensors2 = sm2->getData();
sm2->addNeighbor(NeighborLink(1));
//save them
@@ -274,16 +249,8 @@ void Tests::testSqlite3Database()
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(sensors1.size() > 0 && sensors1.size() == sm1->getData().size());
CPPUNIT_ASSERT(sensors2.size() > 0 && sensors2.size() == sm2->getData().size());
CPPUNIT_ASSERT(sm1->getNeighbors().size() == 1);
CPPUNIT_ASSERT(sm2->getNeighbors().size() == 1);
@@ -346,7 +313,9 @@ void Tests::testBayesFilter()
{
//ULOGGER_DEBUG("--- %d ---", i);
std::map<std::string, float> memStats;
mem.update(0, memStats);
std::list<Sensor> emptySensors;
std::list<Actuator> emptyActuators;
mem.update(emptySensors, emptyActuators, memStats);
posterior = bayes.computePosterior(&mem, likelihood);
likelihood.insert(std::pair<int, float>(i, 1));
sum = uSum(uValues(posterior));
@@ -510,8 +479,7 @@ 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;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
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)));
@@ -528,16 +496,9 @@ void Tests::testVerifyHypotheses()
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);
int total = findPairsAll(wordsA, wordsB, pairs);
//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);
}
CPPUNIT_ASSERT(pairs.size() == 8);
}
corelib/tests/Tests.h → tests/Tests.h
View File
corelib/tests/main.cpp → tests/main.cpp
View File
tools/CMakeLists.txt
+14
View File
@@ -0,0 +1,14 @@
ADD_SUBDIRECTORY( ConsoleApp )
ADD_SUBDIRECTORY( ImagesJoiner )
ADD_SUBDIRECTORY( WebcamCapture )
ADD_SUBDIRECTORY( Polar )
ADD_SUBDIRECTORY( ImagesDbExtractor )
ADD_SUBDIRECTORY( ColorIndexesGenerator )
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
ADD_SUBDIRECTORY( DatabaseViewer )
ADD_SUBDIRECTORY( EpipolarGeometry )
ELSE()
MESSAGE(STATUS "[WARNING] Qt4 not found, the databaseViewer and epipolarGeometry programs will not be built...")
ENDIF()
corelib/ColorIndexesGenerator/CMakeLists.txt → tools/ColorIndexesGenerator/CMakeLists.txt
+2 -3
View File
@@ -4,11 +4,10 @@ SET(SRC_FILES
)
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${CMAKE_CURRENT_SOURCE_DIR}
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}/../src
${ZLIB_INCLUDE_DIRS}
)
@@ -21,7 +20,7 @@ SET(LIBRARIES
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(colorIndexesGenerator ${SRC_FILES})
TARGET_LINK_LIBRARIES(colorIndexesGenerator corelib ${LIBRARIES})
TARGET_LINK_LIBRARIES(colorIndexesGenerator rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES( colorIndexesGenerator
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-colorIndexesGenerator)
corelib/ColorIndexesGenerator/main.cpp → tools/ColorIndexesGenerator/main.cpp
+4 -3
View File
@@ -3,8 +3,8 @@
#include <fstream>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include "ColorTable.h"
#include "NearestNeighbor.h"
#include "rtabmap/core/ColorTable.h"
#include "rtabmap/core/NearestNeighbor.h"
#include <zlib.h>
#include <utilite/UConversion.h>
@@ -61,6 +61,7 @@ void zerr(int ret)
break;
case Z_VERSION_ERROR:
fputs("zlib version mismatch!\n", stderr);
break;
}
UFATAL("");
}
@@ -190,7 +191,7 @@ int main(int argc, char** argv)
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);
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);
corelib/ConsoleApp/CMakeLists.txt → tools/ConsoleApp/CMakeLists.txt
+2 -1
View File
@@ -4,6 +4,7 @@ SET(SRC_FILES
)
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${CMAKE_CURRENT_SOURCE_DIR}
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
@@ -21,7 +22,7 @@ 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})
TARGET_LINK_LIBRARIES(consoleApp rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES( consoleApp
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-console)
corelib/ConsoleApp/main.cpp → tools/ConsoleApp/main.cpp
+36 -68
View File
@@ -21,7 +21,6 @@
#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>
@@ -51,7 +50,8 @@ void showUsage()
" 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 "
" -frames_dropped # Frames dropped from source (Default 0: no frames dropped)\n"
" -start_at # When \"path\" is a directory of images, set this parameter\n"
" to start processing at image # (default 1)."
" -\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n"
" -SURF/HessianThreshold 150\n"
@@ -117,6 +117,7 @@ int main(int argc, char * argv[])
int imageWidth = 0;
int imageHeight = 0;
int startAt = 1;
int framesDropped = 0;
ParametersMap pm;
ULogger::Level logLevel = ULogger::kError;
ULogger::Level exitLevel = ULogger::kFatal;
@@ -241,6 +242,23 @@ int main(int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-frames_dropped") == 0)
{
++i;
if(i < argc)
{
framesDropped = std::atoi(argv[i]);
if(framesDropped < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-start_at") == 0)
{
++i;
@@ -342,19 +360,11 @@ int main(int argc, char * argv[])
Camera * camera = 0;
if(UDirectory::exists(path))
{
camera = new CameraImages(path, startAt, false, 0.0f, false, imageWidth, imageHeight);
camera = new CameraImages(path, startAt, false, 1/rate, false, imageWidth, imageHeight, framesDropped);
}
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);
}
camera = new CameraVideo(path, 1/rate, false, imageWidth, imageHeight, framesDropped);
}
if(!camera || !camera->init())
@@ -383,7 +393,6 @@ int main(int argc, char * argv[])
printf("Avpd init time = %fs\n", timer.ticks());
// Start thread's task
IplImage * image = 0;
int loopClosureId;
int count = 0;
int countLoopDetected=0;
@@ -395,6 +404,7 @@ int main(int argc, char * argv[])
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);
printf(" Camera frames dropped %d (default 0)\n", framesDropped);
if(createGT)
{
printf(" Creating the ground truth matrix.\n");
@@ -418,45 +428,29 @@ int main(int argc, char * argv[])
ParametersMap allParam;
Rtabmap::readParameters(rtabmap->getIniFilePath().c_str(), allParam);
pm.insert(allParam.begin(), allParam.end());
CamKeypointTreatment imageToSMState(pm);
camera->setFeaturesExtracted(true);
camera->parseParameters(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();
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat img = camera->takeImage(descriptors, keypoints);
int i=0;
while(smState && g_forever)
while(!img.empty() && 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);
rtabmap->process(Sensor(descriptors, keypoints));
loopClosureId = rtabmap->getLoopClosureId();
actions = rtabmap->getActions();
if(rtabmap->getLoopClosureId())
{
++countLoopDetected;
}
smState = camera->takeSMState();
img = camera->takeImage(descriptors, keypoints);
if(++count % 100 == 0)
{
printf(" count = %d, loop closures = %d\n", count, countLoopDetected);
@@ -488,43 +482,17 @@ int main(int argc, char * argv[])
ULogger::flush();
if(rate)
if(rtabmap->getLoopClosureId())
{
float delta = rate - iterationTime;
if(delta > 0)
{
uSleep(delta*1000);
}
printf(" iteration(%d) loop(%d) time=%fs *\n", count, rtabmap->getLoopClosureId(), iterationTime);
}
if(actions.size())
else if(rtabmap->getReactivatedId())
{
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);
}
printf(" iteration(%d) high(%d) time=%fs\n", count, rtabmap->getReactivatedId(), 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);
}
printf(" iteration(%d) time=%fs\n", count, iterationTime);
}
if(timeThreshold && iterationTime > timeThreshold*100.0f)
corelib/DatabaseViewer/CMakeLists.txt → tools/DatabaseViewer/CMakeLists.txt
+3 -5
View File
@@ -18,15 +18,13 @@ QT4_WRAP_CPP(moc_srcs ${headers_ui})
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}
${PROJECT_SOURCE_DIR}/corelib/include
${PROJECT_SOURCE_DIR}/guilib/include
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
@@ -47,7 +45,7 @@ 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})
TARGET_LINK_LIBRARIES(databaseViewer rtabmap_corelib rtabmap_guilib ${LIBRARIES})
SET_TARGET_PROPERTIES( databaseViewer
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-databaseViewer)
corelib/DatabaseViewer/MainWindow.cpp → tools/DatabaseViewer/MainWindow.cpp
+152 -198
View File
@@ -29,9 +29,10 @@
#include <utilite/UConversion.h>
#include <opencv2/core/core_c.h>
#include <utilite/UTimer.h>
#include "KeypointMemory.h"
#include "rtabmap/core/KeypointMemory.h"
#include "rtabmap/core/SMMemory.h"
#include "rtabmap/core/DBDriver.h"
#include "../../guilib/src/KeypointItem.h"
#include "rtabmap/gui/KeypointItem.h"
MainWindow::MainWindow(QWidget * parent) :
QMainWindow(parent),
@@ -80,33 +81,47 @@ MainWindow::~MainWindow()
void MainWindow::openDatabase()
{
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
if(!path.isEmpty())
QStringList types;
types << "Keypoint" << "Sensorimotor";
bool ok = false;
QString type = QInputDialog::getItem(this, tr("Select database type"), tr("Type"), types, 0, false, &ok);
if(ok && !type.isEmpty())
{
if(memory_)
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
if(!path.isEmpty())
{
delete memory_;
memory_ = 0;
imagesMap_.clear();
ids_.clear();
}
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();
std::string driverType = "sqlite3";
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
if(type.compare("Keypoint") == 0)
{
memory_ = new rtabmap::KeypointMemory(parameters);
}
else
{
memory_ = new rtabmap::SMMemory(parameters);
}
if(!memory_)
{
QMessageBox::warning(this, "Database error", tr("Can't create database driver \"%1\"").arg(driverType.c_str()));
}
else if(!memory_->init(driverType, path.toStdString()))
{
QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
}
else
{
pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
updateIds();
}
}
}
@@ -238,30 +253,24 @@ void MainWindow::generateLocalGraph()
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)
{
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)
{
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));
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("Signature %1 not found in database.").arg(id));
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
}
}
}
@@ -298,139 +307,44 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
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 ?");
}
this->update(value,
ui_->label_indexA,
ui_->label_actionsA,
ui_->label_parentsA,
ui_->label_childrenA,
ui_->graphicsView_A,
ui_->label_idA);
}
void MainWindow::sliderBValueChanged(int value)
{
ui_->label_indexB->setText(QString::number(value));
ui_->label_actionsB->clear();
ui_->label_parentsB->clear();
ui_->label_childrenB->clear();
this->update(value,
ui_->label_indexB,
ui_->label_actionsB,
ui_->label_parentsB,
ui_->label_childrenB,
ui_->graphicsView_B,
ui_->label_idB);
}
void MainWindow::update(int value,
QLabel * labelIndex,
QLabel * labelActions,
QLabel * labelParents,
QLabel * labelChildren,
QGraphicsView * view,
QLabel * labelId)
{
UTimer timer;
labelIndex->setText(QString::number(value));
labelActions->clear();
labelParents->clear();
labelChildren->clear();
if(value >= 0 && value < ids_.size())
{
ui_->graphicsView_B->scene()->clear();
view->scene()->clear();
int id = ids_.at(value);
ui_->label_idB->setText(QString::number(id));
labelId->setText(QString::number(id));
if(id>0)
{
//image
@@ -440,19 +354,29 @@ void MainWindow::sliderBValueChanged(int value)
{
if(memory_)
{
IplImage * image = memory_->getImage(id);
if(image)
std::list<rtabmap::Sensor> sensors = memory_->getRawData(id);
if(sensors.size())
{
img = ipl2QImage(image);
cvReleaseImage(&image);
if(!img.isNull())
std::list<rtabmap::Sensor>::const_iterator jter = sensors.begin();
for(; jter!=sensors.end(); ++jter)
{
QByteArray ba;
QBuffer buffer(&ba);
buffer.open(QIODevice::WriteOnly);
img.save(&buffer, "BMP"); // writes image into ba in BMP format
imagesMap_.insert(id, ba);
if(jter->type() == rtabmap::Sensor::kTypeImage)
{
break; //Stop to first
}
}
if(jter != sensors.end())
{
IplImage iplImg = jter->data();
img = ipl2QImage(&iplImg);
if(!img.isNull())
{
QByteArray ba;
QBuffer buffer(&ba);
buffer.open(QIODevice::WriteOnly);
img.save(&buffer, "BMP"); // writes image into ba in BMP format
imagesMap_.insert(id, ba);
}
}
}
}
@@ -462,18 +386,18 @@ void MainWindow::sliderBValueChanged(int value)
img.loadFromData(iter.value(), "BMP");
}
if(memory_)
if(memory_ && dynamic_cast<rtabmap::KeypointMemory*>(memory_))
{
std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
std::multimap<int, cv::KeyPoint> words = dynamic_cast<rtabmap::KeypointMemory*>(memory_)->getWords(id);
if(words.size())
{
drawKeypoints(words, ui_->graphicsView_B->scene());
drawKeypoints(words, view->scene());
}
}
if(!img.isNull())
{
ui_->graphicsView_B->scene()->addPixmap(QPixmap::fromImage(img));
view->scene()->addPixmap(QPixmap::fromImage(img));
}
else
{
@@ -486,18 +410,47 @@ void MainWindow::sliderBValueChanged(int value)
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())
if(iter->toId()>id-1 && iter->actuators().size())
{
QString str("");
const std::list<std::vector<float> > & actions = iter->actions();
QString str;
const std::list<rtabmap::Actuator> & actuators = iter->actuators();
unsigned int j=0;
for(std::list<std::vector<float> >::const_iterator jter=actions.begin(); jter!=actions.end(); ++jter)
for(std::list<rtabmap::Actuator>::const_iterator jter=actuators.begin(); jter!=actuators.end(); ++jter)
{
for(unsigned int i=0; i<jter->size(); ++i)
if(jter->data().elemSize() == 1)
{
str.append(QString("%1 ").arg(jter->at(i)));
for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
{
str.append(QString("%1 ").arg(*(char*)(jter->data().data + i)));
}
}
if(j+1 < actions.size())
else if(jter->data().elemSize() == 2)
{
for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
{
str.append(QString("%1 ").arg(*(short*)(jter->data().data + i)));
}
}
else if(jter->data().elemSize() == 4)
{
for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
{
if(jter->data().type() & CV_32F)
{
str.append(QString("%1 ").arg(*(float*)(jter->data().data + i)));
}
else
{
str.append(QString("%1 ").arg(*(int*)(jter->data().data + i)));
}
}
}
else
{
UERROR("not handled element size %d", jter->data().elemSize());
break;
}
if(j+1 < actuators.size())
{
str.append(QString("\n"));
}
@@ -505,7 +458,7 @@ void MainWindow::sliderBValueChanged(int value)
}
if(str.size())
{
ui_->label_actionsB->setText(str);
labelActions->setText(str);
}
break;
}
@@ -523,7 +476,7 @@ void MainWindow::sliderBValueChanged(int value)
{
str.append(QString("%1 ").arg(*iter));
}
ui_->label_parentsB->setText(str);
labelParents->setText(str);
}
if(children.size())
{
@@ -532,17 +485,18 @@ void MainWindow::sliderBValueChanged(int value)
{
str.append(QString("%1 ").arg(*iter));
}
ui_->label_childrenB->setText(str);
labelChildren->setText(str);
}
}
ui_->label_idB->setText(QString::number(id));
ui_->graphicsView_B->fitInView(ui_->graphicsView_B->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
labelId->setText(QString::number(id));
view->fitInView(view->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
}
else
{
ULOGGER_ERROR("Slider index out of range ?");
}
UINFO("Time = %fs", timer.ticks());
}
void MainWindow::sliderAMoved(int value)
@@ -571,7 +525,7 @@ void MainWindow::sliderBMoved(int value)
}
}
QImage MainWindow::ipl2QImage(const IplImage *newImage) //fct recuperer sur le net, converti un ldImage en QImage
QImage MainWindow::ipl2QImage(const IplImage *newImage)
{
QImage qtemp;
if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
corelib/DatabaseViewer/MainWindow.h → tools/DatabaseViewer/MainWindow.h
+11 -2
View File
@@ -31,10 +31,12 @@
class Ui_MainWindow;
class QGraphicsScene;
class QGraphicsView;
class QLabel;
namespace rtabmap
{
class KeypointMemory;
class Memory;
}
class MainWindow : public QMainWindow
@@ -60,12 +62,19 @@ private:
void updateIds();
QImage ipl2QImage(const IplImage *newImage);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene);
void update(int value,
QLabel * labelIndex,
QLabel * labelActions,
QLabel * labelParents,
QLabel * labelChildren,
QGraphicsView * view,
QLabel * labelId);
private:
Ui_MainWindow * ui_;
QMap<int, QByteArray> imagesMap_;
QList<int> ids_;
rtabmap::KeypointMemory * memory_;
rtabmap::Memory * memory_;
QString pathDatabase_;
};
corelib/DatabaseViewer/main.cpp → tools/DatabaseViewer/main.cpp
View File

Some files were not shown because too many files have changed in this diff Show More