mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-08 12:30:20 +08:00
Merge branch 'g2o_prior' of https://github.com/dronecrew/rtabmap into dronecrew-g2o_prior
This commit is contained in:
matlabbe
@@ -1,6 +1,8 @@
|
||||
/lib
|
||||
.DS_Store
|
||||
.settings/language.settings.xml
|
||||
.idea/
|
||||
cmake-build-debug/
|
||||
app/android/.classpath
|
||||
app/android/.project
|
||||
app/android/AndroidManifest.xml
|
||||
|
||||
+70
-64
@@ -46,25 +46,25 @@ IF(NOT ANDROID)
|
||||
ENDIF(NOT ANDROID)
|
||||
|
||||
IF(WIN32 AND NOT MINGW)
|
||||
ADD_DEFINITIONS("-DNOMINMAX")
|
||||
ADD_DEFINITIONS("-DNOMINMAX")
|
||||
ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
|
||||
ELSE ()
|
||||
ADD_DEFINITIONS( "-Wall" )
|
||||
ADD_DEFINITIONS("-Wno-unknown-pragmas")
|
||||
ADD_DEFINITIONS( "-Wall" )
|
||||
ADD_DEFINITIONS("-Wno-unknown-pragmas")
|
||||
ENDIF()
|
||||
|
||||
if(POLICY CMP0020)
|
||||
cmake_policy(SET CMP0020 OLD)
|
||||
cmake_policy(SET CMP0020 OLD)
|
||||
endif()
|
||||
if(POLICY CMP0043)
|
||||
cmake_policy(SET CMP0043 OLD)
|
||||
cmake_policy(SET CMP0043 OLD)
|
||||
endif()
|
||||
|
||||
IF(MINGW)
|
||||
# Hide the --enable-auto-import warning
|
||||
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")
|
||||
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
|
||||
@@ -125,7 +125,7 @@ set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
|
||||
|
||||
####### BUILD OPTIONS #######
|
||||
IF(APPLE)
|
||||
OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
|
||||
OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
|
||||
ENDIF(APPLE)
|
||||
OPTION(BUILD_APP "Build main application" ON)
|
||||
OPTION(BUILD_TOOLS "Build tools" ON)
|
||||
@@ -142,6 +142,7 @@ option(WITH_FREENECT2 "Include Freenect2 support" ON)
|
||||
option(WITH_OPENNI2 "Include OpenNI2 support" ON)
|
||||
option(WITH_DC1394 "Include dc1394 support" ON)
|
||||
option(WITH_G2O "Include g2o support" ON)
|
||||
option(WITH_G2O_UNARY "Include g2o unary support" ON)
|
||||
option(WITH_GTSAM "Include GTSAM support" ON)
|
||||
option(WITH_TORO "Include TORO support" ON)
|
||||
option(WITH_VERTIGO "Include Vertigo support" ON)
|
||||
@@ -232,18 +233,18 @@ ENDIF(WITH_FREENECT)
|
||||
IF(WITH_FREENECT2)
|
||||
FIND_PACKAGE(freenect2 QUIET)
|
||||
IF(freenect2_FOUND)
|
||||
IF(NOT freenect2_INCLUDE_DIRS)
|
||||
SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
|
||||
ENDIF(NOT freenect2_INCLUDE_DIRS)
|
||||
IF(NOT freenect2_INCLUDE_DIRS)
|
||||
SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
|
||||
ENDIF(NOT freenect2_INCLUDE_DIRS)
|
||||
MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}")
|
||||
# Explicitly link to OpenCL (in case of CUDA installed)
|
||||
FIND_PACKAGE(OpenCL QUIET)
|
||||
IF(OpenCL_FOUND)
|
||||
SET(freenect2_LIBRARIES
|
||||
${OpenCL_LIBRARIES}
|
||||
${freenect2_LIBRARIES}
|
||||
)
|
||||
ENDIF(OpenCL_FOUND)
|
||||
FIND_PACKAGE(OpenCL QUIET)
|
||||
IF(OpenCL_FOUND)
|
||||
SET(freenect2_LIBRARIES
|
||||
${OpenCL_LIBRARIES}
|
||||
${freenect2_LIBRARIES}
|
||||
)
|
||||
ENDIF(OpenCL_FOUND)
|
||||
ENDIF(freenect2_FOUND)
|
||||
ENDIF(WITH_FREENECT2)
|
||||
|
||||
@@ -281,47 +282,47 @@ IF(WITH_FLYCAPTURE2)
|
||||
ENDIF(WITH_FLYCAPTURE2)
|
||||
|
||||
IF(WITH_CVSBA)
|
||||
FIND_PACKAGE(cvsba QUIET)
|
||||
FIND_PACKAGE(cvsba QUIET)
|
||||
IF(cvsba_FOUND)
|
||||
MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
|
||||
ENDIF(cvsba_FOUND)
|
||||
ENDIF(WITH_CVSBA)
|
||||
|
||||
IF(WITH_ZED)
|
||||
IF(WIN32) # Windows
|
||||
SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
|
||||
if (CMAKE_CL_64) # 64 bits
|
||||
SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
|
||||
else(CMAKE_CL_64) # 32 bits
|
||||
message("32bits compilation is no more available with CUDA7.0")
|
||||
endif(CMAKE_CL_64)
|
||||
SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
|
||||
IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
|
||||
SET(ZED_FOUND TRUE)
|
||||
LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
|
||||
ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
|
||||
ELSE() # Linux
|
||||
find_package(ZED 1 QUIET)
|
||||
ENDIF(WIN32)
|
||||
|
||||
IF(ZED_FOUND)
|
||||
IF(WIN32) # Windows
|
||||
SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
|
||||
if (CMAKE_CL_64) # 64 bits
|
||||
SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
|
||||
else(CMAKE_CL_64) # 32 bits
|
||||
message("32bits compilation is no more available with CUDA7.0")
|
||||
endif(CMAKE_CL_64)
|
||||
SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
|
||||
IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
|
||||
SET(ZED_FOUND TRUE)
|
||||
LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
|
||||
ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
|
||||
ELSE() # Linux
|
||||
find_package(ZED 1 QUIET)
|
||||
ENDIF(WIN32)
|
||||
|
||||
IF(ZED_FOUND)
|
||||
MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}")
|
||||
## look for CUDA
|
||||
find_package(CUDA)
|
||||
IF(CUDA_FOUND)
|
||||
MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
|
||||
ELSE()
|
||||
MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
|
||||
ENDIF()
|
||||
## look for CUDA
|
||||
find_package(CUDA)
|
||||
IF(CUDA_FOUND)
|
||||
MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
|
||||
ELSE()
|
||||
MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
|
||||
ENDIF()
|
||||
ENDIF(ZED_FOUND)
|
||||
ENDIF(WITH_ZED)
|
||||
|
||||
IF(WITH_REALSENSE)
|
||||
IF(WITH_REALSENSE_SLAM)
|
||||
FIND_PACKAGE(RealSense QUIET COMPONENTS slam)
|
||||
ELSE()
|
||||
FIND_PACKAGE(RealSense QUIET)
|
||||
ENDIF()
|
||||
FIND_PACKAGE(RealSense QUIET COMPONENTS slam)
|
||||
ELSE()
|
||||
FIND_PACKAGE(RealSense QUIET)
|
||||
ENDIF()
|
||||
IF(RealSense_FOUND)
|
||||
MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}")
|
||||
ENDIF(RealSense_FOUND)
|
||||
@@ -331,14 +332,14 @@ IF(WITH_REALSENSE)
|
||||
ENDIF(WITH_REALSENSE)
|
||||
|
||||
IF(WITH_OCTOMAP)
|
||||
FIND_PACKAGE(OCTOMAP QUIET)
|
||||
FIND_PACKAGE(OCTOMAP QUIET)
|
||||
IF(OCTOMAP_FOUND)
|
||||
MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
|
||||
ENDIF(OCTOMAP_FOUND)
|
||||
ENDIF(WITH_OCTOMAP)
|
||||
|
||||
IF(WITH_CPUTSDF)
|
||||
FIND_PACKAGE(CPUTSDF QUIET)
|
||||
FIND_PACKAGE(CPUTSDF QUIET)
|
||||
IF(CPUTSDF_FOUND)
|
||||
MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}")
|
||||
ENDIF(CPUTSDF_FOUND)
|
||||
@@ -348,15 +349,15 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
|
||||
#Newest versions require std11
|
||||
IF(NOT MSVC)
|
||||
include(CheckCXXCompilerFlag)
|
||||
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
|
||||
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
|
||||
IF(COMPILER_SUPPORTS_CXX11)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
|
||||
ELSEIF(COMPILER_SUPPORTS_CXX0X)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
|
||||
ELSE()
|
||||
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
|
||||
ENDIF()
|
||||
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
|
||||
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
|
||||
IF(COMPILER_SUPPORTS_CXX11)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
|
||||
ELSEIF(COMPILER_SUPPORTS_CXX0X)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
|
||||
ELSE()
|
||||
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
|
||||
ENDIF()
|
||||
ENDIF()
|
||||
ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
|
||||
|
||||
@@ -392,7 +393,7 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
|
||||
|
||||
# CONF_DEPENDENCIES contains only dependencies not required by the headers
|
||||
SET(CONF_DEPENDENCIES
|
||||
${ZLIB_LIBRARIES}
|
||||
${ZLIB_LIBRARIES}
|
||||
)
|
||||
IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
|
||||
SET(NONFREE "//")
|
||||
@@ -402,6 +403,9 @@ IF(NOT G2O_FOUND)
|
||||
ELSE()
|
||||
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
|
||||
ENDIF()
|
||||
IF(NOT WITH_G2O_UNARY)
|
||||
SET(G2O_UNARY "//")
|
||||
ENDIF()
|
||||
IF(NOT GTSAM_FOUND)
|
||||
SET(GTSAM "//")
|
||||
ELSE()
|
||||
@@ -520,8 +524,8 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$
|
||||
|
||||
# ... for the build tree
|
||||
set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include"
|
||||
"${PROJECT_SOURCE_DIR}/guilib/include"
|
||||
"${PROJECT_SOURCE_DIR}/utilite/include")
|
||||
"${PROJECT_SOURCE_DIR}/guilib/include"
|
||||
"${PROJECT_SOURCE_DIR}/utilite/include")
|
||||
set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
|
||||
IF(QT4_FOUND OR Qt5_FOUND)
|
||||
set(CONF_WITH_GUI ON)
|
||||
@@ -646,8 +650,8 @@ MESSAGE(STATUS " BUILD_APP = ${BUILD_APP}")
|
||||
MESSAGE(STATUS " BUILD_TOOLS = ${BUILD_TOOLS}")
|
||||
MESSAGE(STATUS " BUILD_EXAMPLES = ${BUILD_EXAMPLES}")
|
||||
IF(NOT WIN32)
|
||||
# see comment above for the BUILD_SHARED_LIBS option on Windows
|
||||
MESSAGE(STATUS " BUILD_SHARED_LIBS = ${BUILD_SHARED_LIBS}")
|
||||
# see comment above for the BUILD_SHARED_LIBS option on Windows
|
||||
MESSAGE(STATUS " BUILD_SHARED_LIBS = ${BUILD_SHARED_LIBS}")
|
||||
ENDIF(NOT WIN32)
|
||||
IF(APPLE)
|
||||
MESSAGE(STATUS " BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
|
||||
@@ -807,3 +811,5 @@ MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of th
|
||||
GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
|
||||
set -DWITH_TORO=ON")
|
||||
ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
|
||||
|
||||
# vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :
|
||||
|
||||
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@NONFREE@#define RTABMAP_NONFREE
|
||||
@TORO@#define RTABMAP_TORO
|
||||
@G2O@#define RTABMAP_G2O
|
||||
@G2O_UNARY@#define RTABMAP_G2O_UNARY
|
||||
@GTSAM@#define RTABMAP_GTSAM
|
||||
@VERTIGO@#define RTABMAP_VERTIGO
|
||||
@OPENCV3@#define RTABMAP_OPENCV3
|
||||
|
||||
@@ -47,6 +47,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "g2o/core/optimization_algorithm_levenberg.h"
|
||||
#include "g2o/core/linear_solver.h"
|
||||
#include "g2o/types/sba/types_sba.h"
|
||||
#include "g2o/types/slam2d/types_slam2d.h"
|
||||
#include "g2o/types/slam3d/types_slam3d.h"
|
||||
#include "g2o/core/robust_kernel_impl.h"
|
||||
#ifdef G2O_HAVE_CSPARSE
|
||||
#include "g2o/solvers/csparse/linear_solver_csparse.h"
|
||||
@@ -56,10 +58,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
|
||||
#endif
|
||||
#include "g2o/solvers/eigen/linear_solver_eigen.h"
|
||||
#include "g2o/types/slam3d/vertex_se3.h"
|
||||
#include "g2o/types/slam3d/edge_se3.h"
|
||||
#include "g2o/types/slam2d/vertex_se2.h"
|
||||
#include "g2o/types/slam2d/edge_se2.h"
|
||||
|
||||
enum {
|
||||
PARAM_OFFSET=0,
|
||||
};
|
||||
|
||||
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
|
||||
typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
|
||||
@@ -165,6 +167,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
|
||||
|
||||
g2o::SparseOptimizer optimizer;
|
||||
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
|
||||
g2o::ParameterSE3Offset* odomOffset = new g2o::ParameterSE3Offset();
|
||||
odomOffset->setId(PARAM_OFFSET);
|
||||
optimizer.addParameter(odomOffset);
|
||||
|
||||
SlamBlockSolver * blockSolver = 0;
|
||||
|
||||
@@ -377,6 +382,20 @@ std::map<int, Transform> OptimizerG2O::optimize(
|
||||
e->setInformation(information);
|
||||
edge = e;
|
||||
}
|
||||
#ifdef RTABMAP_G2O_UNARY
|
||||
{
|
||||
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
|
||||
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
|
||||
priorEdge->setVertex(0, v2);
|
||||
priorEdge->setMeasurement(v2->estimate());
|
||||
priorEdge->setParameterId(0, PARAM_OFFSET);
|
||||
priorEdge->setInformation(information);
|
||||
UWARN("adding edge prior");
|
||||
if (!optimizer.addEdge(priorEdge)) {
|
||||
UERROR("Map: Failed adding unary edge for %d, skipping", id2);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
if (!optimizer.addEdge(edge))
|
||||
@@ -384,6 +403,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
|
||||
delete edge;
|
||||
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
UDEBUG("Initial optimization...");
|
||||
|
||||
Reference in New Issue
Block a user