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

removed ImagesDbExtractor tool (already inside databaseViewer)

Browse Source 
add optional delay to DBReader
added more options to odometryViewer tool


git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1264 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-02-28 22:11:36 +00:00
parent 669e7363cc
commit ac626a846e
13 changed files with 235 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/DBReader.h
View File

@@ -27,7 +27,8 @@ class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender {
public:
DBReader(const std::string & databasePath,
float frameRate = 0.0f,
bool odometryIgnored = false);
bool odometryIgnored = false,
float delayToStartSec = 0.0f);
virtual ~DBReader();
bool init(int startIndex=0);
@@ -42,6 +43,7 @@ private:
std::string _path;
float _frameRate;
bool _odometryIgnored;
float _delayToStartSec;
DBDriver * _dbDriver;
UTimer _timer;

8
corelib/src/DBReader.cpp
View File

@@ -20,10 +20,12 @@ namespace rtabmap {
DBReader::DBReader(const std::string & databasePath,
float frameRate,
bool odometryIgnored) :
bool odometryIgnored,
float delayToStartSec) :
_path(databasePath),
_frameRate(frameRate),
_odometryIgnored(odometryIgnored),
_delayToStartSec(delayToStartSec),
_dbDriver(0),
_currentId(_ids.end())
{
@@ -100,6 +102,10 @@ void DBReader::setFrameRate(float frameRate)
void DBReader::mainLoopBegin()
{
if(_delayToStartSec > 0.0f)
{
uSleep(_delayToStartSec*1000.0f);
}
_timer.start();
}

14
corelib/src/Odometry.cpp
View File

@@ -187,7 +187,7 @@ Transform OdometryBinary::computeTransform(Image & image)
if(_lastKeypoints.size())
{
if(newDescriptors.rows)
if(newDescriptors.rows && newDescriptors.rows > _lastKeypoints.size()/2) // at least 50% keypoints
{
cv::Mat results;
cv::Mat dists;
@@ -333,6 +333,11 @@ Transform OdometryBinary::computeTransform(Image & image)
UWARN("Not enough inliers %d < %d", oi, this->getMinInliers());
}
}
else if(newDescriptors.rows)
{
UWARN("At least 50%% keypoints of the last image required. New=%d last=%d",
newDescriptors.rows, _lastKeypoints.size());
}
else
{
UWARN("No feature extracted!");
@@ -437,7 +442,12 @@ Transform OdometryBOW::computeTransform(Image & image)
if(previousSignature && newSignature)
{
Transform transform;
if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
if(newSignature->getWords3().size() < previousSignature->getWords3().size()/2)
{
UWARN("At least 50%% keypoints of the last image required. New=%d last=%d",
newSignature->getWords3().size(), previousSignature->getWords3().size());
}
else if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new

3
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -106,6 +106,9 @@ public:
const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
void setCameraTargetLocked();
void setCameraTargetFollow();
public slots:
void render();
void setBackgroundColor(const QColor & color);

10
guilib/src/CloudViewer.cpp
View File

@@ -536,6 +536,16 @@ void CloudViewer::setCloudPointSize(const std::string & id, int size)
}
}
void CloudViewer::setCameraTargetLocked()
{
_aLockCamera->setChecked(true);
}
void CloudViewer::setCameraTargetFollow()
{
_aFollowCamera->setChecked(true);
}
Eigen::Vector3f rotatePointAroundAxe(
const Eigen::Vector3f & point,
const Eigen::Vector3f & axis,

11
guilib/src/DatabaseViewer.cpp
View File

@@ -132,14 +132,19 @@ bool DatabaseViewer::openDatabase(const QString & path)
UDEBUG("Open database \"%s\"", path.toStdString().c_str());
if(QFile::exists(path))
{
QStringList types;
types << "Keypoint" << "Sensorimotor";
if(memory_)
{
delete memory_;
memory_ = 0;
ids_.clear();
idToIndex_.clear();
neighborLinks_.clear();
loopLinks_.clear();
graphes_.clear();
poses_.clear();
links_.clear();
scans_.clear();
ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
}
std::string driverType = "sqlite3";

2
guilib/src/PreferencesDialog.cpp
View File

@@ -2658,7 +2658,7 @@ void PreferencesDialog::testOdometry(OdomTest test)
window->setMinimumHeight(600);
connect( window, SIGNAL(destroyed(QObject*)), this, SLOT(cleanOdometryTest()) );
OdometryViewer * odomViewer = new OdometryViewer(100, 2, 0.0, window);
OdometryViewer * odomViewer = new OdometryViewer(10, 2, 0.0, window);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(odomViewer);

1
tools/CMakeLists.txt
View File

@@ -1,7 +1,6 @@
ADD_SUBDIRECTORY( ConsoleApp )
ADD_SUBDIRECTORY( ImagesJoiner )
ADD_SUBDIRECTORY( ImagesDbExtractor )
ADD_SUBDIRECTORY( VocabularyComparison )
ADD_SUBDIRECTORY( OdometryViewer )
ADD_SUBDIRECTORY( DataRecorder )

7
tools/DataRecorder/main.cpp
View File

@@ -1,5 +1,6 @@
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/CameraOpenni.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/CameraThread.h>
@@ -97,6 +98,12 @@ int main (int argc, char * argv[])
}
fileName = argv[argc-1]; // the last is the output path
if(UFile::getExtension(fileName.toStdString()).compare("db") != 0)
{
printf("Database names must end with .db extension\n");
showUsage();
}
UINFO("Output = %s", fileName.toStdString().c_str());
UINFO("Show = %s", show?"true":"false");
UINFO("Openni = %s", openni?"true":"false");

30
tools/ImagesDbExtractor/CMakeLists.txt
View File

@@ -1,30 +0,0 @@
SET(SRC_FILES
main.cpp
)
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/utilite/include
${PROJECT_SOURCE_DIR}/corelib/include
${CMAKE_CURRENT_SOURCE_DIR}
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
SET(LIBRARIES
${UTILITE_LIBRARIES}
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
)
add_definitions(${PCL_DEFINITIONS})
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(imagesDbExtractor ${SRC_FILES})
TARGET_LINK_LIBRARIES(imagesDbExtractor rtabmap_core rtabmap_utilite ${LIBRARIES})
SET_TARGET_PROPERTIES( imagesDbExtractor
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesDbExtractor)

53
tools/ImagesDbExtractor/main.cpp
View File

@@ -1,53 +0,0 @@
#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UDirectory.h>
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/util3d.h"
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
std::string path = rtabmap::Parameters::defaultRtabmapWorkingDirectory() + "/rtabmap.db";
if(argc > 1)
{
path = argv[1];
}
// Open database
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
rtabmap::Memory * memory = new rtabmap::Memory(parameters);
if(!memory->init(path))
{
UWARN("Can't open database \"%s\"", path.c_str());
}
if(memory)
{
UINFO("Using database %s", path.c_str());
std::string saveDirectory = "imagesExtracted/";
if(!UDirectory::exists(saveDirectory))
{
UDirectory::makeDir(saveDirectory);
}
std::set<int> ids = memory->getAllSignatureIds();
for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
cv::Mat image = rtabmap::util3d::uncompressImage(memory->getImage(*iter));
std::string fileName = uFormat("%d.png", *iter);
cv::imwrite(saveDirectory+fileName, image);
UINFO("Saved %s", (saveDirectory+fileName).c_str());
}
}
return 0;
}

179
tools/OdometryViewer/main.cpp
View File

@@ -1,9 +1,11 @@
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/gui/OdometryViewer.h>
#include <rtabmap/core/CameraOpenni.h>
#include <rtabmap/core/DBReader.h>
#include <QtGui/QApplication>
void showUsage()
@@ -15,7 +17,12 @@ void showUsage()
" -bin Use binary odometry (FAST+BRIEF)\n"
" -icp Use ICP odometry\n"
"\n"
" -in #.# Inliers maximum distance (default 0.005 m)\n"
" -hz #.# Camera rate (default 0, 0 means as fast as the camera can)\n"
" -db \"input.db\" Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
" -clouds # Maximum clouds shown (default 10, zero means inf)\n"
" -sec #.# Delay (seconds) before reading the database (if set)\n"
"\n"
" -in #.# Inliers maximum distance, features/ICP (default 0.005 m)\n"
" -max # Max features used for matching (default 0=inf)\n"
" -min # Minimum inliers to accept the transform (default 20)\n"
" -depth #.# Maximum features depth (default 5.0 m)\n"
@@ -23,16 +30,22 @@ void showUsage()
" -lu # Linear update (default 0.0 m)\n"
" -au # Angular update (default 0.0 radian)\n"
" -reset # Reset countdown (default 0 = disabled)\n"
"\n"
" -bin_brief_bytes # BRIEF bytes (default 32)\n"
" -bin_fast_thr # FAST threshold (default 30)\n"
" -bin_lsh Use nearest neighbor LSH (default brute force hamming)\n"
"\n"
" -d # ICP decimation (default 4)\n"
" -v # ICP voxel size (default 0.005)\n"
" -s # ICP samples (default 0, not used if voxel is set.)\n"
" -f #.# ICP fitness (default 0.01)\n"
"\n"
" -debug Log debug messages\n"
"\n"
"Examples:\n"
" odometryViewer -bow 0 SURF example\n"
" odometryViewer -bow 1 SIFT example\n"
" odometryViewer -bin FAST/BRIEF example\n"
" odometryViewer -bin -hz 10 FAST/BRIEF example\n"
" odometryViewer -icp -in 0.05 -i 30 ICP example\n");
exit(1);
}
@@ -43,6 +56,8 @@ int main (int argc, char * argv[])
ULogger::setLevel(ULogger::kInfo);
// parse arguments
float rate = 0.0;
std::string inputDatabase;
int odomType = 0; // 0=bow 1=bin 2=ICP
int bowType = 0;
float distance = 0.005;
@@ -57,6 +72,11 @@ int main (int argc, char * argv[])
float voxel = 0.005;
int samples = 10000;
float fitness = 0.01f;
int maxClouds = 10;
int briefBytes = 32;
int fastThr = 30;
bool useLSH = false;
float sec = 0.0f;
for(int i=1; i<argc; ++i)
{
@@ -78,6 +98,75 @@ int main (int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-hz") == 0)
{
++i;
if(i < argc)
{
rate = std::atof(argv[i]);
if(rate < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-db") == 0)
{
++i;
if(i < argc)
{
inputDatabase = argv[i];
if(UFile::getExtension(inputDatabase).compare("db") != 0)
{
printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-clouds") == 0)
{
++i;
if(i < argc)
{
maxClouds = std::atoi(argv[i]);
if(maxClouds < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-sec") == 0)
{
++i;
if(i < argc)
{
sec = std::atof(argv[i]);
if(sec < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-in") == 0)
{
++i;
@@ -282,6 +371,45 @@ int main (int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-bin_brief_bytes") == 0)
{
++i;
if(i < argc)
{
briefBytes = std::atoi(argv[i]);
if(briefBytes < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-bin_fast_thr") == 0)
{
++i;
if(i < argc)
{
fastThr = std::atoi(argv[i]);
if(fastThr < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-bin_lsh") == 0)
{
useLSH = true;
continue;
}
if(strcmp(argv[i], "-bin") == 0)
{
odomType = 1;
@@ -302,6 +430,17 @@ int main (int argc, char * argv[])
showUsage();
}
if(inputDatabase.size())
{
UINFO("Using database input \"%s\"", inputDatabase.c_str());
}
else
{
UINFO("Using OpenNI camera");
}
UINFO("Camera rate = %f Hz", rate);
UINFO("Maximum clouds shown = %d", maxClouds);
UINFO("Delay = %f s", sec);
UINFO("Odometry used = %s", odomType==0?bowType==0?"Bag-of-words SURF":"Bag-of-words SIFT":odomType==1?"Binary (FAST+BRIEF)":"ICP");
UINFO("Inlier/ICP maximum correspondences distance = %f", distance);
UINFO("Max features = %d", maxWords);
@@ -315,10 +454,12 @@ int main (int argc, char * argv[])
UINFO("Cloud voxel size = %f", voxel);
UINFO("Cloud samples = %d", samples);
UINFO("Cloud fitness = %f", fitness);
UINFO("Binary BRIEF bytes = %d", briefBytes);
UINFO("Binary FAST threshold = %f", fastThr);
UINFO("Binary LSH = %f", useLSH?"true":"false");
QApplication app(argc, argv);
rtabmap::CameraOpenni camera("", 0, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
rtabmap::Odometry * odom = 0;
if(odomType == 0)
@@ -344,7 +485,11 @@ int main (int argc, char * argv[])
maxDepth,
linearUpdate,
angularUpdate,
resetCountdown);
resetCountdown,
briefBytes,
fastThr,
true,
!useLSH);
}
else // ICP
{
@@ -361,17 +506,20 @@ int main (int argc, char * argv[])
resetCountdown);
}
rtabmap::OdometryThread odomThread(odom);
rtabmap::OdometryViewer odomViewer(100, 2, 0.0);
rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0);
UEventsManager::addHandler(&odomThread);
UEventsManager::addHandler(&odomViewer);
odomViewer.setWindowTitle("Odometry viewer");
odomViewer.setMinimumWidth(500);
odomViewer.setMinimumHeight(300);
odomViewer.showMaximized();
odomViewer.setMinimumWidth(800);
odomViewer.setMinimumHeight(500);
odomViewer.showNormal();
app.processEvents();
if(inputDatabase.size())
{
rtabmap::DBReader camera(inputDatabase, rate, true, sec);
if(camera.init())
{
odomThread.start();
@@ -382,6 +530,21 @@ int main (int argc, char * argv[])
camera.kill();
odomThread.join(true);
}
}
else
{
rtabmap::CameraOpenni camera("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
if(camera.init())
{
odomThread.start();
camera.start();
app.exec();
camera.kill();
odomThread.join(true);
}
}
return 0;
}

4
utilite/src/UEventsManager.cpp
View File

@@ -223,9 +223,13 @@ void UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
UEventsHandler * handler = *it;
handlersMutex_.unlock();
// Don't process event if the handler is the same as the sender
if(handler != sender)
{
// To be able to add/remove an handler in a handleEvent call (without a deadlock)
// @see _addHandler(), _removeHandler()
handler->handleEvent(event);
}
handlersMutex_.lock();
}