mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
New tool: rtabmap-detectMoreLoopClosures
This commit is contained in:
matlabbe
@@ -10,6 +10,7 @@ ADD_SUBDIRECTORY( RgbdDataset )
|
||||
ADD_SUBDIRECTORY( EurocDataset )
|
||||
ADD_SUBDIRECTORY( Recovery )
|
||||
ADD_SUBDIRECTORY( Reprocess )
|
||||
ADD_SUBDIRECTORY( DetectMoreLoopClosures )
|
||||
|
||||
IF(OPENCV_NONFREE_FOUND)
|
||||
ADD_SUBDIRECTORY( VocabularyComparison )
|
||||
|
||||
40
tools/DetectMoreLoopClosures/CMakeLists.txt
Normal file
40
tools/DetectMoreLoopClosures/CMakeLists.txt
Normal file
@@ -0,0 +1,40 @@
|
||||
|
||||
SET(RTABMap_INCLUDE_DIRS
|
||||
${PROJECT_SOURCE_DIR}/utilite/include
|
||||
${PROJECT_SOURCE_DIR}/corelib/include
|
||||
)
|
||||
SET(RTABMap_LIBRARIES
|
||||
rtabmap_core
|
||||
rtabmap_utilite
|
||||
)
|
||||
|
||||
if(POLICY CMP0020)
|
||||
cmake_policy(SET CMP0020 OLD)
|
||||
endif()
|
||||
|
||||
SET(INCLUDE_DIRS
|
||||
${RTABMap_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
SET(LIBRARIES
|
||||
${RTABMap_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
|
||||
|
||||
ADD_EXECUTABLE(detectMoreLoopClosures main.cpp)
|
||||
|
||||
TARGET_LINK_LIBRARIES(detectMoreLoopClosures ${LIBRARIES})
|
||||
|
||||
SET_TARGET_PROPERTIES( detectMoreLoopClosures
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-detectMoreLoopClosures)
|
||||
|
||||
INSTALL(TARGETS detectMoreLoopClosures
|
||||
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
|
||||
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
|
||||
|
||||
|
||||
192
tools/DetectMoreLoopClosures/main.cpp
Normal file
192
tools/DetectMoreLoopClosures/main.cpp
Normal file
@@ -0,0 +1,192 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <rtabmap/core/DBDriver.h>
|
||||
#include <rtabmap/core/Rtabmap.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/util3d_surface.h>
|
||||
#include <rtabmap/utilite/UMath.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <pcl/filters/filter.h>
|
||||
#include <pcl/io/ply_io.h>
|
||||
#include <pcl/io/obj_io.h>
|
||||
#include <pcl/common/common.h>
|
||||
#include <pcl/surface/poisson.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
void showUsage()
|
||||
{
|
||||
printf("\nUsage:\n"
|
||||
"rtabmap-detectMoreLoopClosures [options] database.db\n"
|
||||
"Options:\n"
|
||||
" -r # Cluster radius (default 1 m).\n"
|
||||
" -a # Cluster angle (default 30 deg).\n"
|
||||
" -i # Iterations (default 1).\n"
|
||||
" --intra Add only intra-session loop closures.\n"
|
||||
" --inter Add only inter-session loop closures.\n"
|
||||
"\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
class PrintProgressState : public ProgressState
|
||||
{
|
||||
public:
|
||||
virtual bool callback(const std::string & msg) const
|
||||
{
|
||||
if(!msg.empty())
|
||||
printf("%s \n", msg.c_str());
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
ULogger::setLevel(ULogger::kError);
|
||||
|
||||
if(argc < 2)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
|
||||
float clusterRadius = 1.0f;
|
||||
float clusterAngle = CV_PI/6.0f;
|
||||
int iterations = 1;
|
||||
bool intraSession = false;
|
||||
bool interSession = false;
|
||||
for(int i=1; i<argc-1; ++i)
|
||||
{
|
||||
if(std::strcmp(argv[i], "--intra") == 0)
|
||||
{
|
||||
intraSession = true;
|
||||
if(interSession)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "--inter") == 0)
|
||||
{
|
||||
interSession = true;
|
||||
if(intraSession)
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "-r") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i<argc-1)
|
||||
{
|
||||
clusterRadius = uStr2Float(argv[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "-a") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i<argc-1)
|
||||
{
|
||||
clusterAngle = uStr2Float(argv[i])*CV_PI/180.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
else if(std::strcmp(argv[i], "-i") == 0)
|
||||
{
|
||||
++i;
|
||||
if(i<argc-1)
|
||||
{
|
||||
iterations = uStr2Int(argv[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
showUsage();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::string dbPath = argv[argc-1];
|
||||
|
||||
printf("\nDatabase: %s\n", dbPath.c_str());
|
||||
printf("Cluster radius = %f m\n", clusterRadius);
|
||||
printf("Cluster angle = %f deg\n", clusterAngle*180.0f/CV_PI);
|
||||
if(intraSession)
|
||||
{
|
||||
printf("Intra-session only\n");
|
||||
}
|
||||
else if(interSession)
|
||||
{
|
||||
printf("Inter-session only\n");
|
||||
}
|
||||
|
||||
if(!intraSession && !interSession)
|
||||
{
|
||||
intraSession = true;
|
||||
interSession = true;
|
||||
}
|
||||
|
||||
// Get parameters
|
||||
ParametersMap parameters;
|
||||
DBDriver * driver = DBDriver::create();
|
||||
if(driver->openConnection(dbPath))
|
||||
{
|
||||
parameters = driver->getLastParameters();
|
||||
driver->closeConnection(false);
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Cannot open database %s!", dbPath.c_str());
|
||||
}
|
||||
delete driver;
|
||||
|
||||
// Get the global optimized map
|
||||
Rtabmap rtabmap;
|
||||
printf("Initialization...\n");
|
||||
rtabmap.init(parameters, dbPath);
|
||||
|
||||
PrintProgressState progress;
|
||||
printf("Detecting...\n");
|
||||
int detected = rtabmap.detectMoreLoopClosures(clusterRadius, clusterAngle, iterations, intraSession, interSession, &progress);
|
||||
if(detected < 0)
|
||||
{
|
||||
printf("Loop closure detection failed!\n");
|
||||
}
|
||||
|
||||
rtabmap.close();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user