mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 09:30: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( EurocDataset )
|
||||||
ADD_SUBDIRECTORY( Recovery )
|
ADD_SUBDIRECTORY( Recovery )
|
||||||
ADD_SUBDIRECTORY( Reprocess )
|
ADD_SUBDIRECTORY( Reprocess )
|
||||||
|
ADD_SUBDIRECTORY( DetectMoreLoopClosures )
|
||||||
|
|
||||||
IF(OPENCV_NONFREE_FOUND)
|
IF(OPENCV_NONFREE_FOUND)
|
||||||
ADD_SUBDIRECTORY( VocabularyComparison )
|
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