mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-01 17:10:26 +08:00
Merged pcl_integration branch to trunk
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
34
tools/OdometryViewer/CMakeLists.txt
Normal file
34
tools/OdometryViewer/CMakeLists.txt
Normal file
@@ -0,0 +1,34 @@
|
||||
|
||||
SET(SRC_FILES
|
||||
main.cpp
|
||||
)
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${PROJECT_SOURCE_DIR}/corelib/include
|
||||
${PROJECT_SOURCE_DIR}/utilite/include
|
||||
${PROJECT_SOURCE_DIR}/guilib/include
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
INCLUDE(${QT_USE_FILE})
|
||||
|
||||
SET(LIBRARIES
|
||||
${PCL_LIBRARIES}
|
||||
${QT_LIBRARIES}
|
||||
)
|
||||
|
||||
add_definitions(${PCL_DEFINITIONS})
|
||||
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
IF(MINGW)
|
||||
ADD_EXECUTABLE(odometryViewer WIN32 ${SRC_FILES})
|
||||
ELSE()
|
||||
ADD_EXECUTABLE(odometryViewer ${SRC_FILES})
|
||||
ENDIF()
|
||||
TARGET_LINK_LIBRARIES(odometryViewer rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( odometryViewer
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-odometryViewer)
|
||||
|
||||
387
tools/OdometryViewer/main.cpp
Normal file
387
tools/OdometryViewer/main.cpp
Normal file
@@ -0,0 +1,387 @@
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UEventsManager.h>
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
#include <rtabmap/gui/OdometryViewer.h>
|
||||
#include <rtabmap/core/CameraOpenni.h>
|
||||
#include <QtGui/QApplication.h>
|
||||
|
||||
void showUsage()
|
||||
{
|
||||
printf("\nUsage:\n"
|
||||
"odometryViewer [options]\n"
|
||||
"Options:\n"
|
||||
" -bow # Use bag-of-words odometry (default 0): 0=SURF, 1=SIFT\n"
|
||||
" -bin Use binary odometry (FAST+BRIEF)\n"
|
||||
" -icp Use ICP odometry\n"
|
||||
"\n"
|
||||
" -in #.# Inliers maximum distance (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"
|
||||
" -i # RANSAC/ICP iterations (default 100)\n"
|
||||
" -lu # Linear update (default 0.0 m)\n"
|
||||
" -au # Angular update (default 0.0 radian)\n"
|
||||
" -reset # Reset countdown (default 0 = disabled)\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"
|
||||
" -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 -icp -in 0.05 -i 30 ICP example\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int main (int argc, char * argv[])
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kInfo);
|
||||
|
||||
// parse arguments
|
||||
int odomType = 0; // 0=bow 1=bin 2=ICP
|
||||
int bowType = 0;
|
||||
float distance = 0.005;
|
||||
int maxWords = 0;
|
||||
int minInliers = 20;
|
||||
float maxDepth = 5.0f;
|
||||
int iterations = 100;
|
||||
float linearUpdate = 0.0f;
|
||||
float angularUpdate = 0.0f;
|
||||
int resetCountdown = 0;
|
||||
int decimation = 4;
|
||||
float voxel = 0.005;
|
||||
int samples = 10000;
|
||||
float fitness = 0.01f;
|
||||
|
||||
for(int i=1; i<argc; ++i)
|
||||
{
|
||||
if(strcmp(argv[i], "-bow") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
bowType = std::atoi(argv[i]);
|
||||
odomType = 0;
|
||||
if(bowType < 0 || bowType > 1)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-in") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
distance = std::atof(argv[i]);
|
||||
if(distance <= 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-max") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
maxWords = std::atoi(argv[i]);
|
||||
if(maxWords < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-min") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
minInliers = std::atoi(argv[i]);
|
||||
if(minInliers < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-depth") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
maxDepth = std::atof(argv[i]);
|
||||
if(maxDepth < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-i") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
iterations = std::atoi(argv[i]);
|
||||
if(iterations <= 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-lu") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
linearUpdate = std::atof(argv[i]);
|
||||
if(linearUpdate < 0.0f)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-au") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
angularUpdate = std::atof(argv[i]);
|
||||
if(angularUpdate < 0.0f)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-reset") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
resetCountdown = std::atoi(argv[i]);
|
||||
if(resetCountdown < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-d") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
decimation = std::atoi(argv[i]);
|
||||
if(decimation < 1)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-v") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
voxel = std::atof(argv[i]);
|
||||
if(voxel < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-s") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
samples = std::atoi(argv[i]);
|
||||
if(samples < 0)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-f") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i < argc)
|
||||
{
|
||||
fitness = std::atof(argv[i]);
|
||||
if(fitness < 0.0f)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-bin") == 0)
|
||||
{
|
||||
odomType = 1;
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-icp") == 0)
|
||||
{
|
||||
odomType = 2;
|
||||
continue;
|
||||
}
|
||||
if(strcmp(argv[i], "-debug") == 0)
|
||||
{
|
||||
ULogger::setLevel(ULogger::kDebug);
|
||||
continue;
|
||||
}
|
||||
|
||||
printf("Unrecognized option : %s\n", argv[i]);
|
||||
showUsage();
|
||||
}
|
||||
|
||||
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);
|
||||
UINFO("Min inliers = %d", minInliers);
|
||||
UINFO("RANSAC/ICP iterations = %d", iterations);
|
||||
UINFO("Max depth = %f", maxDepth);
|
||||
UINFO("Linear update = %f", linearUpdate);
|
||||
UINFO("Angular update = %f", angularUpdate);
|
||||
UINFO("Reset odometry coutdown = %d", resetCountdown);
|
||||
UINFO("Cloud decimation = %d", decimation);
|
||||
UINFO("Cloud voxel size = %f", voxel);
|
||||
UINFO("Cloud samples = %d", samples);
|
||||
UINFO("Cloud fitness = %f", fitness);
|
||||
|
||||
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)
|
||||
{
|
||||
odom = new rtabmap::OdometryBOW(
|
||||
bowType,
|
||||
distance,
|
||||
maxWords,
|
||||
minInliers,
|
||||
iterations,
|
||||
maxDepth,
|
||||
linearUpdate,
|
||||
angularUpdate,
|
||||
resetCountdown);
|
||||
}
|
||||
else if(odomType == 1)
|
||||
{
|
||||
odom = new rtabmap::OdometryBinary(
|
||||
distance,
|
||||
maxWords,
|
||||
minInliers,
|
||||
iterations,
|
||||
maxDepth,
|
||||
linearUpdate,
|
||||
angularUpdate,
|
||||
resetCountdown);
|
||||
}
|
||||
else // ICP
|
||||
{
|
||||
odom = new rtabmap::OdometryICP(
|
||||
decimation,
|
||||
voxel,
|
||||
samples,
|
||||
distance,
|
||||
iterations,
|
||||
fitness,
|
||||
maxDepth,
|
||||
linearUpdate,
|
||||
angularUpdate,
|
||||
resetCountdown);
|
||||
}
|
||||
rtabmap::OdometryThread odomThread(odom);
|
||||
rtabmap::OdometryViewer odomViewer(100, 2, 0.0);
|
||||
UEventsManager::addHandler(&odomThread);
|
||||
UEventsManager::addHandler(&odomViewer);
|
||||
|
||||
odomViewer.setWindowTitle("Odometry viewer");
|
||||
odomViewer.setMinimumWidth(500);
|
||||
odomViewer.setMinimumHeight(300);
|
||||
odomViewer.showMaximized();
|
||||
|
||||
app.processEvents();
|
||||
|
||||
if(camera.init())
|
||||
{
|
||||
odomThread.start();
|
||||
camera.start();
|
||||
|
||||
app.exec();
|
||||
|
||||
camera.kill();
|
||||
odomThread.join(true);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user