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

Added OptimizerCeres class (Optimizer/Strategy=3)

Browse Source
This commit is contained in:
matlabbe
2019-08-17 03:55:04 -04:00
parent 38044040a6
commit 536f001479
29 changed files with 1982 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

220
CMakeLists.txt
View File

@@ -172,6 +172,7 @@ option(WITH_DC1394 "Include dc1394 support" ON)
option(WITH_G2O "Include g2o support" ON) option(WITH_G2O "Include g2o support" ON)
option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_GTSAM "Include GTSAM support" ON)
option(WITH_TORO "Include TORO support" ON) option(WITH_TORO "Include TORO support" ON)
option(WITH_CERES "Include Ceres support" ON)
option(WITH_VERTIGO "Include Vertigo support" ON) option(WITH_VERTIGO "Include Vertigo support" ON)
option(WITH_CVSBA "Include cvsba support" ON) option(WITH_CVSBA "Include cvsba support" ON)
option(WITH_POINTMATCHER "Include libpointmatcher support" ON) option(WITH_POINTMATCHER "Include libpointmatcher support" ON)
@@ -501,6 +502,11 @@ IF(WITH_OKVIS)
ENDIF(okvis_FOUND) ENDIF(okvis_FOUND)
ENDIF(WITH_OKVIS) ENDIF(WITH_OKVIS)
# If built with okvis, we found already ceres above
IF(NOT okvis_FOUND AND WITH_CERES)
FIND_PACKAGE(Ceres QUIET)
ENDIF(NOT okvis_FOUND AND WITH_CERES)
IF(WITH_MSCKF_VIO) IF(WITH_MSCKF_VIO)
FIND_PACKAGE(msckf_vio QUIET) FIND_PACKAGE(msckf_vio QUIET)
IF(msckf_vio_FOUND) IF(msckf_vio_FOUND)
@@ -554,6 +560,7 @@ IF(loam_velodyne_FOUND)
ENDIF() ENDIF()
ELSEIF(G2O_FOUND OR ELSEIF(G2O_FOUND OR
GTSAM_FOUND OR GTSAM_FOUND OR
CERES_FOUND OR
ZED_FOUND OR ZED_FOUND OR
ANDROID OR ANDROID OR
RealSense_FOUND OR RealSense_FOUND OR
@@ -638,6 +645,9 @@ IF(NOT GTSAM_FOUND)
ELSE() ELSE()
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES}) SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
ENDIF() ENDIF()
IF(NOT CERES_FOUND)
SET(CERES "//")
ENDIF(NOT CERES_FOUND)
IF(NOT WITH_TORO) IF(NOT WITH_TORO)
SET(TORO "//") SET(TORO "//")
ENDIF(NOT WITH_TORO) ENDIF(NOT WITH_TORO)
@@ -966,10 +976,12 @@ ENDIF(APPLE OR WIN32)
MESSAGE(STATUS " CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}") MESSAGE(STATUS " CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
MESSAGE(STATUS " FLANN_KDTREE_MEM_OPT = ${FLANN_KDTREE_MEM_OPT}") MESSAGE(STATUS " FLANN_KDTREE_MEM_OPT = ${FLANN_KDTREE_MEM_OPT}")
MESSAGE(STATUS " PCL_DEFINITIONS = ${PCL_DEFINITIONS}") MESSAGE(STATUS " PCL_DEFINITIONS = ${PCL_DEFINITIONS}")
MESSAGE(STATUS " PCL_VERSION = ${PCL_VERSION}")
IF(PCL_COMPILE_OPTIONS) IF(PCL_COMPILE_OPTIONS)
MESSAGE(STATUS " PCL_COMPILE_OPTIONS = ${PCL_COMPILE_OPTIONS}") MESSAGE(STATUS " PCL_COMPILE_OPTIONS = ${PCL_COMPILE_OPTIONS}")
ENDIF(PCL_COMPILE_OPTIONS) ENDIF(PCL_COMPILE_OPTIONS)
MESSAGE(STATUS "")
MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :") MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
IF(OpenCV_FOUND) IF(OpenCV_FOUND)
IF(OpenCV_VERSION_MAJOR EQUAL 2) IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -987,12 +999,17 @@ IF(OpenCV_FOUND)
ENDIF() ENDIF()
ENDIF(OpenCV_FOUND) ENDIF(OpenCV_FOUND)
IF(FastCV_FOUND) IF(QT4_FOUND)
MESSAGE(STATUS " With FastCV = YES (License: Apache v2)") MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)")
ELSEIF(NOT WITH_FASTCV) MESSAGE(STATUS " With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION} = YES (License: BSD)")
MESSAGE(STATUS " With FastCV = NO (WITH_FASTCV=OFF)") ELSEIF(Qt5_FOUND)
MESSAGE(STATUS " With Qt5 = YES (License: Open Source or Commercial)")
MESSAGE(STATUS " With VTK ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION} = YES (License: BSD)")
ELSEIF(NOT WITH_QT)
MESSAGE(STATUS " With Qt = NO (WITH_QT=OFF)")
ELSE() ELSE()
MESSAGE(STATUS " With FastCV = NO (FastCV not found)") MESSAGE(STATUS " With Qt = NO (Qt not found)")
ENDIF() ENDIF()
IF(Sqlite3_FOUND) IF(Sqlite3_FOUND)
@@ -1013,6 +1030,100 @@ ELSE()
MESSAGE(STATUS " With Madgwick = NO (WITH_MADGWICK=OFF)") MESSAGE(STATUS " With Madgwick = NO (WITH_MADGWICK=OFF)")
ENDIF() ENDIF()
IF(FastCV_FOUND)
MESSAGE(STATUS " With FastCV = YES (License: Apache v2)")
ELSEIF(NOT WITH_FASTCV)
MESSAGE(STATUS " With FastCV = NO (WITH_FASTCV=OFF)")
ELSE()
MESSAGE(STATUS " With FastCV = NO (FastCV not found)")
ENDIF()
MESSAGE(STATUS "")
MESSAGE(STATUS " Solvers:")
IF(WITH_TORO)
MESSAGE(STATUS " With TORO = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])")
ELSE()
MESSAGE(STATUS " With TORO = NO (WITH_TORO=OFF)")
ENDIF()
IF(G2O_FOUND)
MESSAGE(STATUS " *With g2o = YES (License: BSD)")
ELSEIF(NOT WITH_G2O)
MESSAGE(STATUS " *With g2o = NO (WITH_G2O=OFF)")
ELSE()
MESSAGE(STATUS " *With g2o = NO (g2o not found)")
ENDIF()
IF(GTSAM_FOUND)
MESSAGE(STATUS " *With GTSAM = YES (License: BSD)")
ELSEIF(NOT WITH_GTSAM)
MESSAGE(STATUS " *With GTSAM = NO (WITH_GTSAM=OFF)")
ELSE()
MESSAGE(STATUS " *With GTSAM = NO (GTSAM not found)")
ENDIF()
IF(CERES_FOUND)
MESSAGE(STATUS " *With Ceres = YES (License: BSD)")
ELSEIF(NOT WITH_CERES)
MESSAGE(STATUS " *With Ceres = NO (WITH_CERES=OFF)")
ELSE()
MESSAGE(STATUS " *With Ceres = NO (Ceres not found)")
ENDIF()
IF(G2O_FOUND OR GTSAM_FOUND)
IF(WITH_VERTIGO)
MESSAGE(STATUS " With VERTIGO = YES (License: GPLv3)")
ELSE()
MESSAGE(STATUS " With VERTIGO = NO (WITH_VERTIGO=OFF)")
ENDIF()
ELSE()
MESSAGE(STATUS " With VERTIGO = NO (GTSAM or g2o required)")
ENDIF()
IF(cvsba_FOUND)
MESSAGE(STATUS " With cvsba = YES (License: GPLv2)")
ELSEIF(NOT WITH_CVSBA)
MESSAGE(STATUS " With cvsba = NO (WITH_CVSBA=OFF)")
ELSE()
MESSAGE(STATUS " With cvsba = NO (cvsba not found)")
ENDIF()
IF(libpointmatcher_FOUND)
MESSAGE(STATUS " *With libpointmatcher = YES (License: BSD)")
ELSEIF(NOT WITH_POINTMATCHER)
MESSAGE(STATUS " *With libpointmatcher = NO (WITH_POINTMATCHER=OFF)")
ELSE()
MESSAGE(STATUS " *With libpointmatcher = NO (libpointmatcher not found)")
ENDIF()
MESSAGE(STATUS "")
MESSAGE(STATUS " Reconstruction Approaches:")
IF(OCTOMAP_FOUND)
MESSAGE(STATUS " With OCTOMAP = YES (License: BSD)")
ELSEIF(NOT WITH_OCTOMAP)
MESSAGE(STATUS " With OCTOMAP = NO (WITH_OCTOMAP=OFF)")
ELSE()
MESSAGE(STATUS " With OCTOMAP = NO (octomap not found)")
ENDIF()
IF(CPUTSDF_FOUND)
MESSAGE(STATUS " With CPUTSDF = YES (License: BSD)")
ELSEIF(NOT WITH_CPUTSDF)
MESSAGE(STATUS " With CPUTSDF = NO (WITH_CPUTSDF=OFF)")
ELSE()
MESSAGE(STATUS " With CPUTSDF = NO (CPUTSDF not found)")
ENDIF()
IF(open_chisel_FOUND)
MESSAGE(STATUS " With OpenChisel = YES (License: ???)")
ELSEIF(NOT WITH_OPENCHISEL)
MESSAGE(STATUS " With OpenChisel = NO (WITH_OPENCHISEL=OFF)")
ELSE()
MESSAGE(STATUS " With OpenChisel = NO (open_chisel not found)")
ENDIF()
MESSAGE(STATUS "")
MESSAGE(STATUS " Camera Drivers:")
IF(Freenect_FOUND) IF(Freenect_FOUND)
MESSAGE(STATUS " With Freenect = YES (License: Apache v2 and/or GPLv2)") MESSAGE(STATUS " With Freenect = YES (License: Apache v2 and/or GPLv2)")
ELSEIF(NOT WITH_FREENECT) ELSEIF(NOT WITH_FREENECT)
@@ -1069,62 +1180,6 @@ ELSE()
MESSAGE(STATUS " With FlyCapture2/Triclops = NO (Point Grey SDK not found)") MESSAGE(STATUS " With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
ENDIF() ENDIF()
IF(WITH_TORO)
MESSAGE(STATUS " With TORO = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])")
ELSE()
MESSAGE(STATUS " With TORO = NO (WITH_TORO=OFF)")
ENDIF()
IF(G2O_FOUND)
MESSAGE(STATUS " *With g2o = YES (License: BSD)")
ELSEIF(NOT WITH_G2O)
MESSAGE(STATUS " *With g2o = NO (WITH_G2O=OFF)")
ELSE()
MESSAGE(STATUS " *With g2o = NO (g2o not found)")
ENDIF()
IF(GTSAM_FOUND)
MESSAGE(STATUS " *With GTSAM = YES (License: BSD)")
ELSEIF(NOT WITH_GTSAM)
MESSAGE(STATUS " *With GTSAM = NO (WITH_GTSAM=OFF)")
ELSE()
MESSAGE(STATUS " *With GTSAM = NO (GTSAM not found)")
ENDIF()
IF(G2O_FOUND OR GTSAM_FOUND)
IF(WITH_VERTIGO)
MESSAGE(STATUS " With VERTIGO = YES (License: GPLv3)")
ELSE()
MESSAGE(STATUS " With VERTIGO = NO (WITH_VERTIGO=OFF)")
ENDIF()
ELSE()
MESSAGE(STATUS " With VERTIGO = NO (GTSAM or g2o required)")
ENDIF()
IF(cvsba_FOUND)
MESSAGE(STATUS " With cvsba = YES (License: GPLv2)")
ELSEIF(NOT WITH_CVSBA)
MESSAGE(STATUS " With cvsba = NO (WITH_CVSBA=OFF)")
ELSE()
MESSAGE(STATUS " With cvsba = NO (cvsba not found)")
ENDIF()
IF(libpointmatcher_FOUND)
MESSAGE(STATUS " *With libpointmatcher = YES (License: BSD)")
ELSEIF(NOT WITH_POINTMATCHER)
MESSAGE(STATUS " *With libpointmatcher = NO (WITH_POINTMATCHER=OFF)")
ELSE()
MESSAGE(STATUS " *With libpointmatcher = NO (libpointmatcher not found)")
ENDIF()
IF(loam_velodyne_FOUND)
MESSAGE(STATUS " With loam_velodyne = YES (License: BSD)")
ELSEIF(NOT WITH_LOAM)
MESSAGE(STATUS " With loam_velodyne = NO (WITH_LOAM=OFF)")
ELSE()
MESSAGE(STATUS " With loam_velodyne = NO (loam_velodyne not found)")
ENDIF()
IF(ZED_FOUND) IF(ZED_FOUND)
IF(CUDA_FOUND) IF(CUDA_FOUND)
MESSAGE(STATUS " With ZED = YES (With CUDA)") MESSAGE(STATUS " With ZED = YES (With CUDA)")
@@ -1160,28 +1215,14 @@ ELSE()
MESSAGE(STATUS " With RealSense2 = NO (librealsense2 not found)") MESSAGE(STATUS " With RealSense2 = NO (librealsense2 not found)")
ENDIF() ENDIF()
IF(OCTOMAP_FOUND) MESSAGE(STATUS "")
MESSAGE(STATUS " With OCTOMAP = YES (License: BSD)") MESSAGE(STATUS " Odometry Approaches:")
ELSEIF(NOT WITH_OCTOMAP) IF(loam_velodyne_FOUND)
MESSAGE(STATUS " With OCTOMAP = NO (WITH_OCTOMAP=OFF)") MESSAGE(STATUS " With loam_velodyne = YES (License: BSD)")
ELSEIF(NOT WITH_LOAM)
MESSAGE(STATUS " With loam_velodyne = NO (WITH_LOAM=OFF)")
ELSE() ELSE()
MESSAGE(STATUS " With OCTOMAP = NO (octomap not found)") MESSAGE(STATUS " With loam_velodyne = NO (loam_velodyne not found)")
ENDIF()
IF(CPUTSDF_FOUND)
MESSAGE(STATUS " With CPUTSDF = YES (License: BSD)")
ELSEIF(NOT WITH_CPUTSDF)
MESSAGE(STATUS " With CPUTSDF = NO (WITH_CPUTSDF=OFF)")
ELSE()
MESSAGE(STATUS " With CPUTSDF = NO (CPUTSDF not found)")
ENDIF()
IF(open_chisel_FOUND)
MESSAGE(STATUS " With OpenChisel = YES (License: ???)")
ELSEIF(NOT WITH_OPENCHISEL)
MESSAGE(STATUS " With OpenChisel = NO (WITH_OPENCHISEL=OFF)")
ELSE()
MESSAGE(STATUS " With OpenChisel = NO (open_chisel not found)")
ENDIF() ENDIF()
IF(libfovis_FOUND) IF(libfovis_FOUND)
@@ -1242,23 +1283,14 @@ ELSE()
MESSAGE(STATUS " With ORB_SLAM2 = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)") MESSAGE(STATUS " With ORB_SLAM2 = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
ENDIF() ENDIF()
IF(QT4_FOUND)
MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)")
ELSEIF(Qt5_FOUND)
MESSAGE(STATUS " With Qt5 = YES (License: Open Source or Commercial)")
ELSEIF(NOT WITH_QT)
MESSAGE(STATUS " With Qt = NO (WITH_QT=OFF)")
ELSE()
MESSAGE(STATUS " With Qt = NO (Qt not found)")
ENDIF()
MESSAGE(STATUS "--------------------------------------------") MESSAGE(STATUS "--------------------------------------------")
IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO) IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of these options: MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of these options:
g2o (https://github.com/RainerKuemmerle/g2o) g2o (https://github.com/RainerKuemmerle/g2o)
GTSAM (https://collab.cc.gatech.edu/borg/gtsam) GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
Ceres (http://ceres-solver.org)
set -DWITH_TORO=ON") set -DWITH_TORO=ON")
ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO) ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO AND NOT CERES_FOUND)
# vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : # vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :

1
Version.h.in
View File

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@G2O@#define RTABMAP_G2O @G2O@#define RTABMAP_G2O
@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
@GTSAM@#define RTABMAP_GTSAM @GTSAM@#define RTABMAP_GTSAM
@CERES@#define RTABMAP_CERES
@VERTIGO@#define RTABMAP_VERTIGO @VERTIGO@#define RTABMAP_VERTIGO
@OPENCV3@#define RTABMAP_OPENCV3 @OPENCV3@#define RTABMAP_OPENCV3
@OPENNI2@#define RTABMAP_OPENNI2 @OPENNI2@#define RTABMAP_OPENNI2

3
corelib/include/rtabmap/core/Optimizer.h
View File

@@ -62,7 +62,8 @@ public:
kTypeTORO = 0, kTypeTORO = 0,
kTypeG2O = 1, kTypeG2O = 1,
kTypeGTSAM = 2, kTypeGTSAM = 2,
kTypeCVSBA = 3 kTypeCeres = 3,
kTypeCVSBA = 4
}; };
static bool isAvailable(Optimizer::Type type); static bool isAvailable(Optimizer::Type type);
static Optimizer * create(const ParametersMap & parameters); static Optimizer * create(const ParametersMap & parameters);

22
corelib/include/rtabmap/core/Parameters.h
View File

@@ -374,25 +374,31 @@ class RTABMAP_EXP Parameters
// Graph optimization // Graph optimization
#ifdef RTABMAP_GTSAM #ifdef RTABMAP_GTSAM
RTABMAP_PARAM(Optimizer, Strategy, int, 2, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM."); RTABMAP_PARAM(Optimizer, Strategy, int, 2, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations."); RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.00001, "Stop optimizing when the error improvement is less than this value."); RTABMAP_PARAM(Optimizer, Epsilon, double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
#else #else
#ifdef RTABMAP_G2O #ifdef RTABMAP_G2O
RTABMAP_PARAM(Optimizer, Strategy, int, 1, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM."); RTABMAP_PARAM(Optimizer, Strategy, int, 1, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations."); RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.0, "Stop optimizing when the error improvement is less than this value."); RTABMAP_PARAM(Optimizer, Epsilon, double, 0.0, "Stop optimizing when the error improvement is less than this value.");
#else #else
RTABMAP_PARAM(Optimizer, Strategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM."); #ifdef RTABMAP_CERES
RTABMAP_PARAM(Optimizer, Strategy, int, 3, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.000001, "Stop optimizing when the error improvement is less than this value.");
#else
RTABMAP_PARAM(Optimizer, Strategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 100, "Optimization iterations."); RTABMAP_PARAM(Optimizer, Iterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.00001, "Stop optimizing when the error improvement is less than this value."); RTABMAP_PARAM(Optimizer, Epsilon, double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
#endif #endif
#endif
#endif #endif
RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str())); RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(Optimizer, PriorsIgnored, bool, true, "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this."); RTABMAP_PARAM(Optimizer, PriorsIgnored, bool, true, "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
RTABMAP_PARAM(Optimizer, LandmarksIgnored, bool, false, "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this."); RTABMAP_PARAM(Optimizer, LandmarksIgnored, bool, false, "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
RTABMAP_PARAM(Optimizer, GravitySigma, float, 0.0, uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with GTSAM optimization strategy (see %s).", kOptimizerStrategy().c_str())); RTABMAP_PARAM(Optimizer, GravitySigma, float, 0.0, uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
#ifdef RTABMAP_ORB_SLAM2 #ifdef RTABMAP_ORB_SLAM2
RTABMAP_PARAM(g2o, Solver, int, 3, "0=csparse 1=pcg 2=cholmod 3=Eigen"); RTABMAP_PARAM(g2o, Solver, int, 3, "0=csparse 1=pcg 2=cholmod 3=Eigen");
@@ -436,9 +442,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str()); RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
RTABMAP_PARAM(OdomF2M, ValidDepthRatio, float, 0.75, "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map."); RTABMAP_PARAM(OdomF2M, ValidDepthRatio, float, 0.75, "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2) #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba."); RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#else #else
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba."); RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#endif #endif
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map)."); RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
@@ -599,9 +605,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2) #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba."); RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#else #else
RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba."); RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#endif #endif
// ICP registration parameters // ICP registration parameters

75
corelib/include/rtabmap/core/optimizer/OptimizerCeres.h Normal file
View File

@@ -0,0 +1,75 @@
/*
Copyright (c) 2010-2019, 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.
*/
#ifndef OPTIMIZERCERES_H_
#define OPTIMIZERCERES_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Optimizer.h>
namespace rtabmap {
class RTABMAP_EXP OptimizerCeres : public Optimizer
{
public:
static bool available();
public:
OptimizerCeres(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
OptimizerCeres(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~OptimizerCeres() {}
virtual Type type() const {return kTypeCeres;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
virtual std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);
};
} /* namespace rtabmap */
#endif /* OPTIMIZERCERES_H_ */

12
corelib/src/CMakeLists.txt
View File

@@ -69,6 +69,7 @@ SET(SRC_FILES
optimizer/OptimizerG2O.cpp optimizer/OptimizerG2O.cpp
optimizer/OptimizerGTSAM.cpp optimizer/OptimizerGTSAM.cpp
optimizer/OptimizerCVSBA.cpp optimizer/OptimizerCVSBA.cpp
optimizer/OptimizerCeres.cpp
Registration.cpp Registration.cpp
RegistrationIcp.cpp RegistrationIcp.cpp
@@ -337,6 +338,17 @@ IF(cvsba_FOUND)
) )
ENDIF(cvsba_FOUND) ENDIF(cvsba_FOUND)
IF(CERES_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${CERES_LIBRARIES}
)
ENDIF(CERES_FOUND)
IF(libpointmatcher_FOUND) IF(libpointmatcher_FOUND)
SET(INCLUDE_DIRS SET(INCLUDE_DIRS
${INCLUDE_DIRS} ${INCLUDE_DIRS}

48
corelib/src/Optimizer.cpp
View File

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/optimizer/OptimizerG2O.h> #include <rtabmap/core/optimizer/OptimizerG2O.h>
#include <rtabmap/core/optimizer/OptimizerGTSAM.h> #include <rtabmap/core/optimizer/OptimizerGTSAM.h>
#include <rtabmap/core/optimizer/OptimizerCVSBA.h> #include <rtabmap/core/optimizer/OptimizerCVSBA.h>
#include <rtabmap/core/optimizer/OptimizerCeres.h>
namespace rtabmap { namespace rtabmap {
@@ -61,6 +62,10 @@ bool Optimizer::isAvailable(Optimizer::Type type)
{ {
return OptimizerTORO::available(); return OptimizerTORO::available();
} }
else if(type == Optimizer::kTypeCeres)
{
return OptimizerCeres::available();
}
return false; return false;
} }
@@ -73,7 +78,7 @@ Optimizer * Optimizer::create(const ParametersMap & parameters)
Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parameters) Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parameters)
{ {
UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available(), UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available() || OptimizerCeres::available(),
"RTAB-Map is not built with any graph optimization approach!"); "RTAB-Map is not built with any graph optimization approach!");
if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO) if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO)
@@ -88,6 +93,11 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
UWARN("TORO optimizer not available. g2o will be used instead."); UWARN("TORO optimizer not available. g2o will be used instead.");
type = Optimizer::kTypeG2O; type = Optimizer::kTypeG2O;
} }
else if(OptimizerCeres::available())
{
UWARN("TORO optimizer not available. ceres will be used instead.");
type = Optimizer::kTypeCeres;
}
} }
if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O) if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
{ {
@@ -101,6 +111,11 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
UWARN("g2o optimizer not available. GTSAM will be used instead."); UWARN("g2o optimizer not available. GTSAM will be used instead.");
type = Optimizer::kTypeGTSAM; type = Optimizer::kTypeGTSAM;
} }
else if(OptimizerCeres::available())
{
UWARN("g2o optimizer not available. ceres will be used instead.");
type = Optimizer::kTypeCeres;
}
} }
if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM) if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
{ {
@@ -114,6 +129,11 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
UWARN("GTSAM optimizer not available. g2o will be used instead."); UWARN("GTSAM optimizer not available. g2o will be used instead.");
type = Optimizer::kTypeG2O; type = Optimizer::kTypeG2O;
} }
else if(OptimizerCeres::available())
{
UWARN("GTSAM optimizer not available. ceres will be used instead.");
type = Optimizer::kTypeCeres;
}
} }
if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA) if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
{ {
@@ -132,6 +152,29 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
UWARN("CVSBA optimizer not available. g2o will be used instead."); UWARN("CVSBA optimizer not available. g2o will be used instead.");
type = Optimizer::kTypeG2O; type = Optimizer::kTypeG2O;
} }
else if(OptimizerCeres::available())
{
UWARN("CVSBA optimizer not available. ceres will be used instead.");
type = Optimizer::kTypeCeres;
}
}
if(!OptimizerCeres::available() && type == Optimizer::kTypeCeres)
{
if(OptimizerGTSAM::available())
{
UWARN("Ceres optimizer not available. gtsam will be used instead.");
type = Optimizer::kTypeGTSAM;
}
else if(OptimizerG2O::available())
{
UWARN("Ceres optimizer not available. g2o will be used instead.");
type = Optimizer::kTypeG2O;
}
else if(OptimizerTORO::available())
{
UWARN("Ceres optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
} }
Optimizer * optimizer = 0; Optimizer * optimizer = 0;
switch(type) switch(type)
@@ -145,6 +188,9 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
case Optimizer::kTypeCVSBA: case Optimizer::kTypeCVSBA:
optimizer = new OptimizerCVSBA(parameters); optimizer = new OptimizerCVSBA(parameters);
break; break;
case Optimizer::kTypeCeres:
optimizer = new OptimizerCeres(parameters);
break;
case Optimizer::kTypeTORO: case Optimizer::kTypeTORO:
default: default:
optimizer = new OptimizerTORO(parameters); optimizer = new OptimizerTORO(parameters);

6
corelib/src/Parameters.cpp
View File

@@ -647,6 +647,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else #else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With Ceres:";
#ifdef RTABMAP_CERES
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif #endif
str = "With OpenNI2:"; str = "With OpenNI2:";
#ifdef RTABMAP_OPENNI2 #ifdef RTABMAP_OPENNI2

2
corelib/src/RegistrationVis.cpp
View File

@@ -1435,7 +1435,7 @@ Transform RegistrationVis::computeTransformationImpl(
toSignature.sensorData().cameraModels().size() <= 1) toSignature.sensorData().cameraModels().size() <= 1)
{ {
UDEBUG("Refine with bundle adjustment"); UDEBUG("Refine with bundle adjustment");
Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters); Optimizer * sba = Optimizer::create(_bundleAdjustment==3?Optimizer::kTypeCeres:_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::multimap<int, Link> links; std::multimap<int, Link> links;

5
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -97,11 +97,12 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
if(bundleAdjustment_ > 0) if(bundleAdjustment_ > 0)
{ {
if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) || if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
(bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA))) (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA)) ||
(bundleAdjustment_==3 && Optimizer::isAvailable(Optimizer::kTypeCeres)))
{ {
// disable bundle in RegistrationVis as we do it already here // disable bundle in RegistrationVis as we do it already here
uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0")); uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
sba_ = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters); sba_ = Optimizer::create(bundleAdjustment_==3?Optimizer::kTypeCeres:bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters);
} }
else else
{ {

511
corelib/src/optimizer/OptimizerCeres.cpp Normal file
View File

@@ -0,0 +1,511 @@
/*
Copyright (c) 2010-2019, 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/Graph.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <set>
#include <rtabmap/core/optimizer/OptimizerCeres.h>
#ifdef RTABMAP_CERES
#include <ceres/ceres.h>
#include <ceres/local_parameterization.h>
#include "ceres/pose_graph_2d/types.h"
#include "ceres/pose_graph_2d/pose_graph_2d_error_term.h"
#include "ceres/pose_graph_2d/angle_local_parameterization.h"
#include "ceres/pose_graph_3d/types.h"
#include "ceres/pose_graph_3d/pose_graph_3d_error_term.h"
#include "ceres/bundle/BAProblem.h"
#include "ceres/bundle/snavely_reprojection_error.h"
#if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
#include "ceres/pose_graph_3d/eigen_quaternion_parameterization.h"
#endif
#endif
namespace rtabmap {
bool OptimizerCeres::available()
{
#ifdef RTABMAP_CERES
return true;
#else
return false;
#endif
}
std::map<int, Transform> OptimizerCeres::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
double * finalError,
int * iterationsDone)
{
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_CERES
UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
//Build problem
ceres::Problem problem;
std::map<int, ceres::examples::Pose2d> poses2d;
ceres::examples::MapOfPoses poses3d;
UDEBUG("fill poses to Ceres...");
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
ceres::examples::Pose2d p;
p.x = iter->second.x();
p.y = iter->second.y();
p.yaw_radians = ceres::examples::NormalizeAngle(iter->second.theta());
poses2d.insert(std::make_pair(iter->first, p));
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
ceres::examples::Pose3d p;
p.p.x() = iter->second.x();
p.p.y() = iter->second.y();
p.p.z() = iter->second.z();
p.q = iter->second.getQuaterniond();
poses3d.insert(std::make_pair(iter->first, p));
}
}
}
ceres::LossFunction* loss_function = NULL;
ceres::LocalParameterization* angle_local_parameterization = NULL;
ceres::LocalParameterization* quaternion_local_parameterization = NULL;
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->second.from();
int id2 = iter->second.to();
if(id1 != id2 && id1 > 0 && id2 > 0)
{
UASSERT(poses.find(id1) != poses.end() && poses.find(id2) != poses.end());
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
float yaw_radians = ceres::examples::NormalizeAngle(iter->second.transform().theta());
const Eigen::Matrix3d sqrt_information = information.llt().matrixL();
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function = ceres::examples::PoseGraph2dErrorTerm::Create(
iter->second.transform().x(),
iter->second.transform().y(),
yaw_radians,
sqrt_information);
std::map<int, ceres::examples::Pose2d>::iterator pose_begin_iter = poses2d.find(id1);
std::map<int, ceres::examples::Pose2d>::iterator pose_end_iter = poses2d.find(id2);
problem.AddResidualBlock(
cost_function, loss_function,
&pose_begin_iter->second.x, &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
&pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians);
if(angle_local_parameterization == NULL)
{
angle_local_parameterization = ceres::examples::AngleLocalParameterization::Create();
}
problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);
problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
}
else
{
ceres::examples::MapOfPoses::iterator pose_begin_iter = poses3d.find(id1);
ceres::examples::MapOfPoses::iterator pose_end_iter = poses3d.find(id2);
ceres::examples::Constraint3d constraint;
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
ceres::examples::Pose3d t;
t.p.x() = iter->second.transform().x();
t.p.y() = iter->second.transform().y();
t.p.z() = iter->second.transform().z();
t.q = iter->second.transform().getQuaterniond();
const Eigen::Matrix<double, 6, 6> sqrt_information = information.llt().matrixL();
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function = ceres::examples::PoseGraph3dErrorTerm::Create(t, sqrt_information);
problem.AddResidualBlock(cost_function, loss_function,
pose_begin_iter->second.p.data(), pose_begin_iter->second.q.coeffs().data(),
pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data());
if(quaternion_local_parameterization == NULL)
{
quaternion_local_parameterization = new ceres::EigenQuaternionParameterization;
}
problem.SetParameterization(pose_begin_iter->second.q.coeffs().data(), quaternion_local_parameterization);
problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization);
}
}
//else // not supporting pose prior and landmarks
}
if(isSlam2d())
{
// The pose graph optimization problem has three DOFs that are not fully
// constrained. This is typically referred to as gauge freedom. You can apply
// a rigid body transformation to all the nodes and the optimization problem
// will still have the exact same cost. The Levenberg-Marquardt algorithm has
// internal damping which mitigate this issue, but it is better to properly
// constrain the gauge freedom. This can be done by setting one of the poses
// as constant so the optimizer cannot change it.
std::map<int, ceres::examples::Pose2d>::iterator pose_start_iter = rootId>0?poses2d.find(rootId):poses2d.begin();
UASSERT(pose_start_iter != poses2d.end());
problem.SetParameterBlockConstant(&pose_start_iter->second.x);
problem.SetParameterBlockConstant(&pose_start_iter->second.y);
problem.SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}
else
{
// The pose graph optimization problem has six DOFs that are not fully
// constrained. This is typically referred to as gauge freedom. You can apply
// a rigid body transformation to all the nodes and the optimization problem
// will still have the exact same cost. The Levenberg-Marquardt algorithm has
// internal damping which mitigates this issue, but it is better to properly
// constrain the gauge freedom. This can be done by setting one of the poses
// as constant so the optimizer cannot change it.
ceres::examples::MapOfPoses::iterator pose_start_iter = rootId>0?poses3d.find(rootId):poses3d.begin();
UASSERT(pose_start_iter != poses3d.end());
problem.SetParameterBlockConstant(pose_start_iter->second.p.data());
problem.SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
UINFO("Ceres optimizing begin (iterations=%d)", iterations());
ceres::Solver::Options options;
options.max_num_iterations = iterations();
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.function_tolerance = this->epsilon();
ceres::Solver::Summary summary;
UTimer timer;
ceres::Solve(options, &problem, &summary);
if(ULogger::level() == ULogger::kDebug)
{
UDEBUG("Ceres Report:");
std::cout << summary.FullReport() << '\n';
}
if(!summary.IsSolutionUsable())
{
UWARN("ceres: Could not find a usable solution, aborting optimization!");
return optimizedPoses;
}
if(finalError)
{
*finalError = summary.final_cost;
}
if(iterationsDone)
{
*iterationsDone = summary.iterations.size();
}
UINFO("Ceres optimizing end (%d iterations done, error=%f, time = %f s)", (int)summary.iterations.size(), summary.final_cost, timer.ticks());
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
if(isSlam2d())
{
const std::map<int, ceres::examples::Pose2d>::iterator & pter = poses2d.find(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(pter->second.x, pter->second.y, iter->second.z(), roll, pitch, pter->second.yaw_radians);
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
else
{
const std::map<int, ceres::examples::Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, ceres::examples::Pose3d> > >::
iterator& pter = poses3d.find(iter->first);
Transform newPose(pter->second.p.x(), pter->second.p.y(), pter->second.p.z(), pter->second.q.x(), pter->second.q.y(), pter->second.q.z(), pter->second.q.w());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
// Ceres doesn't compute marginals...
}
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
#else
UERROR("Not built with Ceres support!");
#endif
return optimizedPoses;
}
std::map<int, Transform> OptimizerCeres::optimizeBA(
int rootId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
std::set<int> * outliers)
{
#ifdef RTABMAP_CERES
// run sba optimization
std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
ceres::BAProblem baProblem;
baProblem.num_cameras_ = poses.size();
baProblem.num_points_ = points3DMap.size();
baProblem.num_observations_ = 0;
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
iter!=wordReferences.end();
++iter)
{
baProblem.num_observations_ += iter->second.size();
}
baProblem.point_index_ = new int[baProblem.num_observations_];
baProblem.camera_index_ = new int[baProblem.num_observations_];
baProblem.observations_ = new double[4 * baProblem.num_observations_];
baProblem.cameras_ = new double[6 * baProblem.num_cameras_];
baProblem.points_ = new double[3 * baProblem.num_points_];
// Each camera is a set of 6 parameters: R and t. The rotation R is specified as a Rodrigues' vector.
int oi=0;
int camIndex=0;
std::map<int, int> camIdToIndex;
for(std::map<int, Transform>::const_iterator iter=poses.begin();
iter!=poses.end();
++iter)
{
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
const Transform & t = (iter->second * iterModel->second.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
(double)t.r31(), (double)t.r32(), (double)t.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
UASSERT(oi+6 <= baProblem.num_cameras_*6);
baProblem.cameras_[oi++] = rvec.at<double>(0,0);
baProblem.cameras_[oi++] = rvec.at<double>(0,1);
baProblem.cameras_[oi++] = rvec.at<double>(0,2);
baProblem.cameras_[oi++] = t.x();
baProblem.cameras_[oi++] = t.y();
baProblem.cameras_[oi++] = t.z();
camIdToIndex.insert(std::make_pair(iter->first, camIndex++));
}
UASSERT(oi == baProblem.num_cameras_*6);
oi=0;
int pointIndex=0;
std::map<int, int> pointIdToIndex;
for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
{
UASSERT(oi+3 <= baProblem.num_points_*3);
baProblem.points_[oi++] = kter->second.x;
baProblem.points_[oi++] = kter->second.y;
baProblem.points_[oi++] = kter->second.z;
pointIdToIndex.insert(std::make_pair(kter->first, pointIndex++));
}
UASSERT(oi == baProblem.num_points_*3);
oi = 0;
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
iter!=wordReferences.end();
++iter)
{
for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin();
jter!=iter->second.end();
++jter)
{
std::map<int, CameraModel>::const_iterator iterModel = models.find(jter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
baProblem.camera_index_[oi] = camIdToIndex.at(jter->first);
baProblem.point_index_[oi] = pointIdToIndex.at(iter->first);
baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second.cx();
baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second.cy();
baProblem.observations_[4*oi+2] = iterModel->second.fx();
baProblem.observations_[4*oi+3] = iterModel->second.fy();
++oi;
}
}
UASSERT(oi == baProblem.num_observations_);
// Build problem
const double* observations = baProblem.observations();
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
ceres::Problem problem;
for (int i = 0; i < baProblem.num_observations(); ++i) {
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
ceres::SnavelyReprojectionError::Create(
observations[4 * i], //u
observations[4 * i + 1], //v
observations[4 * i + 2], //fx
observations[4 * i + 3]); //fy
ceres::LossFunction* loss_function = new ceres::HuberLoss(8.0);
problem.AddResidualBlock(cost_function,
loss_function,
baProblem.mutable_camera_for_observation(i),
baProblem.mutable_point_for_observation(i));
}
// SBA
// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.max_num_iterations = iterations();
//options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.function_tolerance = this->epsilon();
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
if(ULogger::level() == ULogger::kDebug)
{
UDEBUG("Ceres report:");
std::cout << summary.FullReport() << "\n";
}
if(!summary.IsSolutionUsable())
{
UWARN("ceres: Could not find a usable solution, aborting optimization!");
return poses;
}
//update poses
std::map<int, Transform> newPoses = poses;
oi=0;
for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
{
cv::Mat rvec = (cv::Mat_<double>(1,3) <<
baProblem.cameras_[oi], baProblem.cameras_[oi+1], baProblem.cameras_[oi+2]);
cv::Mat R;
cv::Rodrigues(rvec, R);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), baProblem.cameras_[oi+3],
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), baProblem.cameras_[oi+4],
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), baProblem.cameras_[oi+5]);
oi+=6;
if(this->isSlam2d())
{
t = (models.at(iter->first).localTransform() * t).inverse();
t = iter->second.inverse() * t;
iter->second *= t.to3DoF();
}
else
{
iter->second = (models.at(iter->first).localTransform() * t).inverse();
}
}
//update 3D points
oi = 0;
for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
{
kter->second.x = baProblem.points_[oi++];
kter->second.y = baProblem.points_[oi++];
kter->second.z = baProblem.points_[oi++];
}
return newPoses;
#else
UERROR("RTAB-Map is not built with ceres!");
return std::map<int, Transform>();
#endif
}
} /* namespace rtabmap */

6
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -1372,7 +1372,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// negative root means that all other poses should be fixed instead of the root // negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId)); vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s", /*UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first, iter->first,
vCam->fixed()?1:0, vCam->fixed()?1:0,
iterModel->second.fx(), iterModel->second.fx(),
@@ -1381,7 +1381,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
iterModel->second.cy(), iterModel->second.cy(),
iterModel->second.Tx(), iterModel->second.Tx(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_, iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
camPose.prettyPrint().c_str()); camPose.prettyPrint().c_str());*/
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str()); UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
} }
@@ -1721,7 +1721,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// remove model local transform // remove model local transform
t *= models.at(iter->first).localTransform().inverse(); t *= models.at(iter->first).localTransform().inverse();
UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str()); //UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull()) if(t.isNull())
{ {
UERROR("Optimized pose %d is null!?!?", iter->first); UERROR("Optimized pose %d is null!?!?", iter->first);

52
corelib/src/optimizer/ceres/bundle/BAProblem.h Normal file
View File

@@ -0,0 +1,52 @@
/*
* BAProblem.h
*
* Created on: Aug 16, 2019
* Author: mathieu
*/
#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BAPROBLEM_H_
#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BAPROBLEM_H_
namespace ceres {
class BAProblem {
public:
BAProblem() :
num_cameras_(0),
num_points_(0),
num_observations_(0),
point_index_(NULL),
camera_index_(NULL),
observations_(NULL),
cameras_(NULL),
points_(NULL)
{}
~BAProblem() {
delete[] point_index_;
delete[] camera_index_;
delete[] observations_;
delete[] cameras_;
delete[] points_;
}
int num_observations() const { return num_observations_;}
const double* observations() const { return observations_;}
double* mutable_camera_for_observation(int i) {
return cameras_ + camera_index_[i] * 6;
}
double* mutable_point_for_observation(int i) {
return points_ + point_index_[i] * 3;
}
int num_cameras_;
int num_points_;
int num_observations_;
int* point_index_;
int* camera_index_;
double* observations_;
double* cameras_;
double* points_;
};
}
#endif /* CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BAPROBLEM_H_ */

69
corelib/src/optimizer/ceres/bundle/snavely_reprojection_error.h Normal file
View File

@@ -0,0 +1,69 @@
/*
* snavely_reprojection_error.h
*
* Created on: Aug 16, 2019
* Author: mathieu
*/
#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_
#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include <rtabmap/utilite/ULogger.h>
namespace ceres {
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 6 parameters: 3 for rotation, 3 for translation. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y, double fx, double fy)
: observed_x(observed_x), observed_y(observed_y), fx(fx), fy(fy) {}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion.
T xp = p[0] / p[2];
T yp = p[1] / p[2];
// Compute final projected point position.
T predicted_x = fx * xp;
T predicted_y = fy * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - observed_x;
residuals[1] = predicted_y - observed_y;
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y,
const double fx,
const double fy) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 6, 3>(
new SnavelyReprojectionError(observed_x, observed_y, fx, fy)));
}
double observed_x;
double observed_y;
double fx;
double fy;
};
}
#endif /* CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_ */

46
corelib/src/optimizer/ceres/pose_graph_2d/README.md Normal file
View File

@@ -0,0 +1,46 @@
Pose Graph 2D
----------------
The Simultaneous Localization and Mapping (SLAM) problem consists of building a
map of an unknown environment while simultaneously localizing against this
map. The main difficulty of this problem stems from not having any additional
external aiding information such as GPS. SLAM has been considered one of the
fundamental challenges of robotics. A pose graph optimization problem is one
example of a SLAM problem.
This package defines the necessary Ceres cost functions needed to model the
2-dimensional pose graph optimization problem as well as a binary to build and
solve the problem. The cost functions are shown for instruction purposes and can
be speed up by using analytical derivatives which take longer to implement.
Running
-----------
This package includes an executable `pose_graph_2d` that will read a problem
definition file. This executable can work with any 2D problem definition that
uses the g2o format. It would be relatively straightforward to implement a new
reader for a different format such as TORO or others. `pose_graph_2d` will print
the Ceres solver full summary and then output to disk the original and optimized
poses (`poses_original.txt` and `poses_optimized.txt`, respectively) of the
robot in the following format:
```
pose_id x y yaw_radians
pose_id x y yaw_radians
pose_id x y yaw_radians
...
```
where `pose_id` is the corresponding integer ID from the file definition. Note,
the file will be sorted in ascending order for the `pose_id`.
The executable `pose_graph_2d` has one flag `--input` which is the path to the
problem definition. To run the executable,
```
/path/to/bin/pose_graph_2d --input /path/to/dataset/dataset.g2o
```
A python script is provided to visualize the resulting output files.
```
/path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
```

63
corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h Normal file
View File

@@ -0,0 +1,63 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 Google Inc. 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 OWNER 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.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
#include "ceres/local_parameterization.h"
#include "normalize_angle.h"
namespace ceres {
namespace examples {
// Defines a local parameterization for updating the angle to be constrained in
// [-pi to pi).
class AngleLocalParameterization {
public:
template <typename T>
bool operator()(const T* theta_radians, const T* delta_theta_radians,
T* theta_radians_plus_delta) const {
*theta_radians_plus_delta =
NormalizeAngle(*theta_radians + *delta_theta_radians);
return true;
}
static ceres::LocalParameterization* Create() {
return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
1, 1>);
}
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_

67
corelib/src/optimizer/ceres/pose_graph_2d/normalize_angle.h Normal file
View File

@@ -0,0 +1,67 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 Google Inc. 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 OWNER 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.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
#include <cmath>
#include "ceres/ceres.h"
namespace ceres {
#if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
inline double floor (double x) { return std::floor(x); }
// The floor function should be used with extreme care as this operation will
// result in a zero derivative which provides no information to the solver.
//
// floor(a + h) ~= floor(a) + 0
template <typename T, int N> inline
Jet<T, N> floor(const Jet<T, N>& f) {
return Jet<T, N>(floor(f.a));
}
#endif
namespace examples {
// Normalizes the angle in radians between [-pi and pi).
template <typename T>
inline T NormalizeAngle(const T& angle_radians) {
// Use ceres::floor because it is specialized for double and Jet types.
T two_pi(2.0 * M_PI);
return angle_radians -
two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
}
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_

112
corelib/src/optimizer/ceres/pose_graph_2d/pose_graph_2d_error_term.h Normal file
View File

@@ -0,0 +1,112 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 Google Inc. 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 OWNER 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.
//
// Author: vitus@google.com (Michael Vitus)
//
// Cost function for a 2D pose graph formulation.
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
#include "Eigen/Core"
namespace ceres {
namespace examples {
template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
const T cos_yaw = ceres::cos(yaw_radians);
const T sin_yaw = ceres::sin(yaw_radians);
Eigen::Matrix<T, 2, 2> rotation;
rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
return rotation;
}
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement.
//
// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
//
// where r_a is the rotation matrix that rotates a vector represented in frame A
// into the global frame, and Normalize(*) ensures the angles are in the range
// [-pi, pi).
class PoseGraph2dErrorTerm {
public:
PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information)
: p_ab_(x_ab, y_ab),
yaw_ab_radians_(yaw_ab_radians),
sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a,
const T* const x_b, const T* const y_b, const T* const yaw_b,
T* residuals_ptr) const {
const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr);
residuals_map.template head<2>() =
RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) -
p_ab_.cast<T>();
residuals_map(2) = ceres::examples::NormalizeAngle(
(*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
// Scale the residuals by the square root information matrix to account for
// the measurement uncertainty.
residuals_map = sqrt_information_.template cast<T>() * residuals_map;
return true;
}
static ceres::CostFunction* Create(double x_ab, double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information) {
return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1,
1, 1>(new PoseGraph2dErrorTerm(
x_ab, y_ab, yaw_ab_radians, sqrt_information)));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The position of B relative to A in the A frame.
const Eigen::Vector2d p_ab_;
// The orientation of frame B relative to frame A.
const double yaw_ab_radians_;
// The inverse square root of the measurement covariance matrix.
const Eigen::Matrix3d sqrt_information_;
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_

105
corelib/src/optimizer/ceres/pose_graph_2d/types.h Normal file
View File

@@ -0,0 +1,105 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 Google Inc. 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 OWNER 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.
//
// Author: vitus@google.com (Michael Vitus)
//
// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
// the graph has a unique integer ID with a position and orientation. There are
// delta transformation constraints between two vertices.
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
#include <fstream>
#include "Eigen/Core"
#include "normalize_angle.h"
namespace ceres {
namespace examples {
// The state for each vertex in the pose graph.
struct Pose2d {
double x;
double y;
double yaw_radians;
// The name of the data type in the g2o file format.
static std::string name() {
return "VERTEX_SE2";
}
};
inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
input >> pose.x >> pose.y >> pose.yaw_radians;
// Normalize the angle between -pi to pi.
pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
return input;
}
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
struct Constraint2d {
int id_begin;
int id_end;
double x;
double y;
double yaw_radians;
// The inverse of the covariance matrix for the measurement. The order of the
// entries are x, y, and yaw.
Eigen::Matrix3d information;
// The name of the data type in the g2o file format.
static std::string name() {
return "EDGE_SE2";
}
};
inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
constraint.y >> constraint.yaw_radians >>
constraint.information(0, 0) >> constraint.information(0, 1) >>
constraint.information(0, 2) >> constraint.information(1, 1) >>
constraint.information(1, 2) >> constraint.information(2, 2);
// Set the lower triangular part of the information matrix.
constraint.information(1, 0) = constraint.information(0, 1);
constraint.information(2, 0) = constraint.information(0, 2);
constraint.information(2, 1) = constraint.information(1, 2);
// Normalize the angle between -pi to pi.
constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
return input;
}
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_

54
corelib/src/optimizer/ceres/pose_graph_3d/README.md Normal file
View File

@@ -0,0 +1,54 @@
Pose Graph 3D
----------------
The Simultaneous Localization and Mapping (SLAM) problem consists of building a
map of an unknown environment while simultaneously localizing against this
map. The main difficulty of this problem stems from not having any additional
external aiding information such as GPS. SLAM has been considered one of the
fundamental challenges of robotics. A pose graph optimization problem is one
example of a SLAM problem.
The example also illustrates how to use Eigen's geometry module with Ceres'
automatic differentiation functionality. To represent the orientation, we will
use Eigen's quaternion which uses the Hamiltonian convention but has different
element ordering as compared with Ceres's rotation representation. Specifically
they differ by whether the scalar component q_w is first or last; the element
order for Ceres's quaternion is [q_w, q_x, q_y, q_z] where as Eigen's quaternion
is [q_x, q_y, q_z, q_w].
This package defines the necessary Ceres cost functions needed to model the
3-dimensional pose graph optimization problem as well as a binary to build and
solve the problem. The cost functions are shown for instruction purposes and can
be speed up by using analytical derivatives which take longer to implement.
Running
-----------
This package includes an executable `pose_graph_3d` that will read a problem
definition file. This executable can work with any 3D problem definition that
uses the g2o format with quaternions used for the orientation representation. It
would be relatively straightforward to implement a new reader for a different
format such as TORO or others. `pose_graph_3d` will print the Ceres solver full
summary and then output to disk the original and optimized poses
(`poses_original.txt` and `poses_optimized.txt`, respectively) of the robot in
the following format:
```
pose_id x y z q_x q_y q_z q_w
pose_id x y z q_x q_y q_z q_w
pose_id x y z q_x q_y q_z q_w
...
```
where `pose_id` is the corresponding integer ID from the file definition. Note,
the file will be sorted in ascending order for the `pose_id`.
The executable `pose_graph_3d` has one flag `--input` which is the path to the
problem definition. To run the executable,
```
/path/to/bin/pose_graph_3d --input /path/to/dataset/dataset.g2o
```
A script is provided to visualize the resulting output files. There is also an
option to enable equal axes using ```--axes_equal```.
```
/path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
```

90
corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h Normal file
View File

@@ -0,0 +1,90 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 Google Inc. 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 OWNER 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.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
#ifndef CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
#define CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
#include "ceres/local_parameterization.h"
namespace ceres {
// Implements the quaternion local parameterization for Eigen's representation
// of the quaternion. Eigen uses a different internal memory layout for the
// elements of the quaternion than what is commonly used. Specifically, Eigen
// stores the elements in memory as [x, y, z, w] where the real part is last
// whereas it is typically stored first. Note, when creating an Eigen quaternion
// through the constructor the elements are accepted in w, x, y, z order. Since
// Ceres operates on parameter blocks which are raw double pointers this
// difference is important and requires a different parameterization.
//
// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
// with * being the quaternion multiplication operator.
class EigenQuaternionParameterization : public ceres::LocalParameterization {
public:
virtual ~EigenQuaternionParameterization() {}
virtual bool Plus(const double* x_ptr,
const double* delta,
double* x_plus_delta_ptr) const
{
Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr);
Eigen::Map<const Eigen::Quaterniond> x(x_ptr);
const double norm_delta =
sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
if (norm_delta > 0.0) {
const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
// Note, in the constructor w is first.
Eigen::Quaterniond delta_q(cos(norm_delta),
sin_delta_by_delta * delta[0],
sin_delta_by_delta * delta[1],
sin_delta_by_delta * delta[2]);
x_plus_delta = delta_q * x;
} else {
x_plus_delta = x;
}
return true;
}
virtual bool ComputeJacobian(const double* x,
double* jacobian) const{
jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT
jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT
jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT
jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT
return true;
}
virtual int GlobalSize() const { return 4; }
virtual int LocalSize() const { return 3; }
};
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_

131
corelib/src/optimizer/ceres/pose_graph_3d/pose_graph_3d_error_term.h Normal file
View File

@@ -0,0 +1,131 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 Google Inc. 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 OWNER 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.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
#include "Eigen/Core"
#include "ceres/autodiff_cost_function.h"
#include "types.h"
namespace ceres {
namespace examples {
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement. We have two poses x_a
// and x_b. Through sensor measurements we can measure the transformation of
// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
// between the current estimate of the poses and the measurement.
//
// In this formulation, we have chosen to represent the rigid transformation as
// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
// [x, y, z, w].
// The estimated measurement is:
// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
// [ q_ab ] [ q_a^{-1] * q_b ]
//
// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
// quaternion. Now we can compute an error metric between the estimated and
// measurement transformation. For the orientation error, we will use the
// standard multiplicative error resulting in:
//
// error = [ p_ab - \hat{p}_ab ]
// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
//
// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
// the measurement has an uncertainty associated with how accurate it is, we
// will weight the errors by the square root of the measurement information
// matrix:
//
// residuals = I^{1/2) * error
// where I is the information matrix which is the inverse of the covariance.
class PoseGraph3dErrorTerm {
public:
PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information)
: t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
const T* const p_b_ptr, const T* const q_b_ptr,
T* residuals_ptr) const {
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_a(p_a_ptr);
Eigen::Map<const Eigen::Quaternion<T> > q_a(q_a_ptr);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_b(p_b_ptr);
Eigen::Map<const Eigen::Quaternion<T> > q_b(q_b_ptr);
// Compute the relative transformation between the two frames.
Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
// Represent the displacement between the two frames in the A frame.
Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
// Compute the error between the two orientation estimates.
Eigen::Quaternion<T> delta_q =
t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
// Compute the residuals.
// [ position ] [ delta_p ]
// [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
residuals.template block<3, 1>(0, 0) =
p_ab_estimated - t_ab_measured_.p.template cast<T>();
residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
// Scale the residuals by the measurement uncertainty.
residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
return true;
}
static ceres::CostFunction* Create(
const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information) {
return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The measurement for the position of B relative to A in the A frame.
const Pose3d t_ab_measured_;
// The square root of the measurement information matrix.
const Eigen::Matrix<double, 6, 6> sqrt_information_;
};
} // namespace examples
} // namespace ceres
#endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_

114
corelib/src/optimizer/ceres/pose_graph_3d/types.h Normal file
View File

@@ -0,0 +1,114 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 Google Inc. 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 OWNER 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.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef EXAMPLES_CERES_TYPES_H_
#define EXAMPLES_CERES_TYPES_H_
#include <istream>
#include <map>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "Eigen/Geometry"
namespace ceres {
namespace examples {
struct Pose3d {
Eigen::Vector3d p;
Eigen::Quaterniond q;
// The name of the data type in the g2o file format.
static std::string name() {
return "VERTEX_SE3:QUAT";
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::istream& operator>>(std::istream& input, Pose3d& pose) {
input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >>
pose.q.y() >> pose.q.z() >> pose.q.w();
// Normalize the quaternion to account for precision loss due to
// serialization.
pose.q.normalize();
return input;
}
typedef std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >
MapOfPoses;
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
struct Constraint3d {
int id_begin;
int id_end;
// The transformation that represents the pose of the end frame E w.r.t. the
// begin frame B. In other words, it transforms a vector in the E frame to
// the B frame.
Pose3d t_be;
// The inverse of the covariance matrix for the measurement. The order of the
// entries are x, y, z, delta orientation.
Eigen::Matrix<double, 6, 6> information;
// The name of the data type in the g2o file format.
static std::string name() {
return "EDGE_SE3:QUAT";
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
Pose3d& t_be = constraint.t_be;
input >> constraint.id_begin >> constraint.id_end >> t_be;
for (int i = 0; i < 6 && input.good(); ++i) {
for (int j = i; j < 6 && input.good(); ++j) {
input >> constraint.information(i, j);
if (i != j) {
constraint.information(j, i) = constraint.information(i, j);
}
}
}
return input;
}
typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d> >
VectorOfConstraints;
} // namespace examples
} // namespace ceres
#endif // EXAMPLES_CERES_TYPES_H_

2
guilib/src/AboutDialog.cpp
View File

@@ -118,6 +118,8 @@ AboutDialog::AboutDialog(QWidget * parent) :
_ui->label_gtsam_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?true:false); _ui->label_gtsam_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?true:false);
_ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No"); _ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No");
_ui->label_cvsba_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?true:false); _ui->label_cvsba_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?true:false);
_ui->label_ceres->setText(Optimizer::isAvailable(Optimizer::kTypeCeres)?"Yes":"No");
_ui->label_ceres_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCeres)?true:false);
#ifdef RTABMAP_POINTMATCHER #ifdef RTABMAP_POINTMATCHER
_ui->label_libpointmatcher->setText("Yes"); _ui->label_libpointmatcher->setText("Yes");

57
guilib/src/PostProcessingDialog.cpp
View File

@@ -40,20 +40,30 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
_ui = new Ui_PostProcessingDialog(); _ui = new Ui_PostProcessingDialog();
_ui->setupUi(this); _ui->setupUi(this);
if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O)) if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) &&
!Optimizer::isAvailable(Optimizer::kTypeG2O) &&
!Optimizer::isAvailable(Optimizer::kTypeCeres))
{ {
_ui->sba->setEnabled(false); _ui->sba->setEnabled(false);
_ui->sba->setChecked(false); _ui->sba->setChecked(false);
} }
else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA)) else
{ {
_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1); if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
_ui->comboBox_sbaType->setCurrentIndex(0); {
} _ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
else if(!Optimizer::isAvailable(Optimizer::kTypeG2O)) _ui->comboBox_sbaType->setCurrentIndex(0);
{ }
_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1); if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
_ui->comboBox_sbaType->setCurrentIndex(1); {
_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
_ui->comboBox_sbaType->setCurrentIndex(1);
}
if(!Optimizer::isAvailable(Optimizer::kTypeCeres))
{
_ui->comboBox_sbaType->setItemData(2, 0, Qt::UserRole - 1);
_ui->comboBox_sbaType->setCurrentIndex(1);
}
} }
restoreDefaults(); restoreDefaults();
@@ -191,7 +201,19 @@ void PostProcessingDialog::restoreDefaults()
setRefineLoopClosureLinks(false); setRefineLoopClosureLinks(false);
setSBA(false); setSBA(false);
setSBAIterations(20); setSBAIterations(20);
setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O); Optimizer::Type sbaType = Optimizer::kTypeG2O; // g2o
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
if(Optimizer::isAvailable(Optimizer::kTypeCVSBA))
{
sbaType = Optimizer::kTypeCVSBA;
}
else if(Optimizer::isAvailable(Optimizer::kTypeCeres))
{
sbaType = Optimizer::kTypeCeres;
}
}
setSBAType(sbaType);
setSBAVariance(1.0); setSBAVariance(1.0);
setSBARematchFeatures(true); setSBARematchFeatures(true);
} }
@@ -244,7 +266,7 @@ bool PostProcessingDialog::isRefineLoopClosureLinks() const
bool PostProcessingDialog::isSBA() const bool PostProcessingDialog::isSBA() const
{ {
return _ui->sba->isChecked(); return _ui->sba->isEnabled() && _ui->sba->isChecked();
} }
int PostProcessingDialog::sbaIterations() const int PostProcessingDialog::sbaIterations() const
@@ -257,7 +279,7 @@ double PostProcessingDialog::sbaVariance() const
} }
Optimizer::Type PostProcessingDialog::sbaType() const Optimizer::Type PostProcessingDialog::sbaType() const
{ {
return _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA; return _ui->comboBox_sbaType->currentIndex()==2?Optimizer::kTypeCeres:_ui->comboBox_sbaType->currentIndex()==1?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O;
} }
bool PostProcessingDialog::sbaRematchFeatures() const bool PostProcessingDialog::sbaRematchFeatures() const
{ {
@@ -299,7 +321,10 @@ void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
} }
void PostProcessingDialog::setSBA(bool on) void PostProcessingDialog::setSBA(bool on)
{ {
_ui->sba->setChecked((Optimizer::isAvailable(Optimizer::kTypeCVSBA) || Optimizer::isAvailable(Optimizer::kTypeG2O)) && on); _ui->sba->setChecked((
Optimizer::isAvailable(Optimizer::kTypeCVSBA) ||
Optimizer::isAvailable(Optimizer::kTypeG2O) ||
Optimizer::isAvailable(Optimizer::kTypeCeres)) && on);
} }
void PostProcessingDialog::setSBAIterations(int iterations) void PostProcessingDialog::setSBAIterations(int iterations)
{ {
@@ -311,7 +336,11 @@ void PostProcessingDialog::setSBAVariance(double variance)
} }
void PostProcessingDialog::setSBAType(Optimizer::Type type) void PostProcessingDialog::setSBAType(Optimizer::Type type)
{ {
if(type == Optimizer::kTypeCVSBA) if(type == Optimizer::kTypeCeres)
{
_ui->comboBox_sbaType->setCurrentIndex(2);
}
else if(type == Optimizer::kTypeCVSBA)
{ {
_ui->comboBox_sbaType->setCurrentIndex(1); _ui->comboBox_sbaType->setCurrentIndex(1);
} }

20
guilib/src/PreferencesDialog.cpp
View File

@@ -262,6 +262,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_f2m_bundleStrategy->setItemData(2, 0, Qt::UserRole - 1); _ui->odom_f2m_bundleStrategy->setItemData(2, 0, Qt::UserRole - 1);
_ui->loopClosure_bundle->setItemData(2, 0, Qt::UserRole - 1); _ui->loopClosure_bundle->setItemData(2, 0, Qt::UserRole - 1);
} }
if(!Optimizer::isAvailable(Optimizer::kTypeCeres))
{
_ui->graphOptimization_type->setItemData(3, 0, Qt::UserRole - 1);
_ui->odom_f2m_bundleStrategy->setItemData(3, 0, Qt::UserRole - 1);
_ui->loopClosure_bundle->setItemData(3, 0, Qt::UserRole - 1);
}
#ifdef RTABMAP_VERTIGO #ifdef RTABMAP_VERTIGO
if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM)) if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
#endif #endif
@@ -2943,6 +2949,13 @@ bool PreferencesDialog::validateForm()
"with cvsba. Bundle adjustment is disabled.")); "with cvsba. Bundle adjustment is disabled."));
_ui->loopClosure_bundle->setCurrentIndex(0); _ui->loopClosure_bundle->setCurrentIndex(0);
} }
if(_ui->loopClosure_bundle->currentIndex() == 3 && !Optimizer::isAvailable(Optimizer::kTypeCeres))
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Selected visual registration bundle adjustment optimization strategy (Ceres) is not available. RTAB-Map is not built "
"with cERES. Bundle adjustment is disabled."));
_ui->loopClosure_bundle->setCurrentIndex(0);
}
// Local bundle adjustment strategy for odometry F2M // Local bundle adjustment strategy for odometry F2M
if(_ui->odom_f2m_bundleStrategy->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O)) if(_ui->odom_f2m_bundleStrategy->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
@@ -2959,6 +2972,13 @@ bool PreferencesDialog::validateForm()
"with cvsba. Bundle adjustment is disabled.")); "with cvsba. Bundle adjustment is disabled."));
_ui->odom_f2m_bundleStrategy->setCurrentIndex(0); _ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
} }
if(_ui->odom_f2m_bundleStrategy->currentIndex() == 3 && !Optimizer::isAvailable(Optimizer::kTypeCeres))
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Selected odometry local bundle adjustment optimization strategy (Ceres) is not available. RTAB-Map is not built "
"with Ceres. Bundle adjustment is disabled."));
_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
}
// verify that Robust and Reject threshold are not set at the same time // verify that Robust and Reject threshold are not set at the same time
if(_ui->graphOptimization_robust->isChecked() && _ui->graphOptimization_maxError->value()>0.0) if(_ui->graphOptimization_robust->isChecked() && _ui->graphOptimization_maxError->value()>0.0)

349
guilib/src/ui/aboutDialog.ui
View File

@@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; }
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>-243</y>
<width>597</width> <width>597</width>
<height>751</height> <height>774</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_2"> <layout class="QVBoxLayout" name="verticalLayout_2">
@@ -185,7 +185,27 @@ p, li { white-space: pre-wrap; }
</property> </property>
<item> <item>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1"> <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
<item row="25" column="2"> <item row="14" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>With stereo Zed :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="26" column="0">
<widget class="QLabel" name="label_25">
<property name="text">
<string>With FOVIS :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="26" column="2">
<widget class="QLabel" name="label_fovis_license"> <widget class="QLabel" name="label_fovis_license">
<property name="text"> <property name="text">
<string>GPLv2</string> <string>GPLv2</string>
@@ -231,7 +251,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="29" column="0"> <item row="30" column="0">
<widget class="QLabel" name="label_35"> <widget class="QLabel" name="label_35">
<property name="text"> <property name="text">
<string>With OKVIS :</string> <string>With OKVIS :</string>
@@ -241,7 +261,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="27" column="1"> <item row="28" column="1">
<widget class="QLabel" name="label_dvo"> <widget class="QLabel" name="label_dvo">
<property name="text"> <property name="text">
<string/> <string/>
@@ -287,7 +307,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="29" column="1"> <item row="30" column="1">
<widget class="QLabel" name="label_okvis"> <widget class="QLabel" name="label_okvis">
<property name="text"> <property name="text">
<string/> <string/>
@@ -300,7 +320,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="31" column="0"> <item row="32" column="0">
<widget class="QLabel" name="label_34"> <widget class="QLabel" name="label_34">
<property name="text"> <property name="text">
<string>With MSCKF :</string> <string>With MSCKF :</string>
@@ -310,7 +330,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="22" column="1"> <item row="23" column="1">
<widget class="QLabel" name="label_octomap"> <widget class="QLabel" name="label_octomap">
<property name="text"> <property name="text">
<string/> <string/>
@@ -323,7 +343,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="31" column="1"> <item row="32" column="1">
<widget class="QLabel" name="label_msckf"> <widget class="QLabel" name="label_msckf">
<property name="text"> <property name="text">
<string/> <string/>
@@ -336,6 +356,16 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="8" column="2">
<widget class="QLabel" name="label_openni2_license">
<property name="text">
<string>Apache v2</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="2"> <item row="2" column="2">
<widget class="QLabel" name="label_pcl_license"> <widget class="QLabel" name="label_pcl_license">
<property name="text"> <property name="text">
@@ -369,7 +399,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="23" column="0"> <item row="24" column="0">
<widget class="QLabel" name="label_24"> <widget class="QLabel" name="label_24">
<property name="text"> <property name="text">
<string>With CPU-TSDF :</string> <string>With CPU-TSDF :</string>
@@ -389,7 +419,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="26" column="0"> <item row="27" column="0">
<widget class="QLabel" name="label_26"> <widget class="QLabel" name="label_26">
<property name="text"> <property name="text">
<string>With Viso2 :</string> <string>With Viso2 :</string>
@@ -399,7 +429,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="28" column="0"> <item row="29" column="0">
<widget class="QLabel" name="label_28"> <widget class="QLabel" name="label_28">
<property name="text"> <property name="text">
<string>With ORB SLAM 2 :</string> <string>With ORB SLAM 2 :</string>
@@ -419,27 +449,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="25" column="0"> <item row="29" column="1">
<widget class="QLabel" name="label_25">
<property name="text">
<string>With FOVIS :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>With stereo Zed :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="28" column="1">
<widget class="QLabel" name="label_orbslam2"> <widget class="QLabel" name="label_orbslam2">
<property name="text"> <property name="text">
<string/> <string/>
@@ -462,7 +472,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="26" column="2"> <item row="27" column="2">
<widget class="QLabel" name="label_viso2_license"> <widget class="QLabel" name="label_viso2_license">
<property name="text"> <property name="text">
<string>GPLv3</string> <string>GPLv3</string>
@@ -482,6 +492,16 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="23" column="0">
<widget class="QLabel" name="label_20">
<property name="text">
<string>With Octomap :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="17" column="0"> <item row="17" column="0">
<widget class="QLabel" name="label_77"> <widget class="QLabel" name="label_77">
<property name="text"> <property name="text">
@@ -492,7 +512,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="25" column="1"> <item row="26" column="1">
<widget class="QLabel" name="label_fovis"> <widget class="QLabel" name="label_fovis">
<property name="text"> <property name="text">
<string/> <string/>
@@ -505,17 +525,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="22" column="0"> <item row="23" column="2">
<widget class="QLabel" name="label_20">
<property name="text">
<string>With Octomap :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="22" column="2">
<widget class="QLabel" name="label_octomap_license"> <widget class="QLabel" name="label_octomap_license">
<property name="text"> <property name="text">
<string>BSD</string> <string>BSD</string>
@@ -525,19 +535,6 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="13" column="1">
<widget class="QLabel" name="label_flycapture2">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="0"> <item row="9" column="0">
<widget class="QLabel" name="label_15"> <widget class="QLabel" name="label_15">
<property name="text"> <property name="text">
@@ -548,8 +545,8 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="23" column="1"> <item row="13" column="1">
<widget class="QLabel" name="label_cputsdf"> <widget class="QLabel" name="label_flycapture2">
<property name="text"> <property name="text">
<string/> <string/>
</property> </property>
@@ -581,6 +578,19 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="24" column="1">
<widget class="QLabel" name="label_cputsdf">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="10" column="0"> <item row="10" column="0">
<widget class="QLabel" name="label_22"> <widget class="QLabel" name="label_22">
<property name="text"> <property name="text">
@@ -601,6 +611,16 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="18" column="0">
<widget class="QLabel" name="label_14">
<property name="text">
<string>With g2o :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="20" column="1"> <item row="20" column="1">
<widget class="QLabel" name="label_cvsba"> <widget class="QLabel" name="label_cvsba">
<property name="text"> <property name="text">
@@ -614,10 +634,10 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="18" column="0"> <item row="7" column="2">
<widget class="QLabel" name="label_14"> <widget class="QLabel" name="label_freenect_license">
<property name="text"> <property name="text">
<string>With g2o :</string> <string>Apache v2 and/or GPLv2</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -650,10 +670,10 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="2"> <item row="19" column="0">
<widget class="QLabel" name="label_freenect_license"> <widget class="QLabel" name="label_19">
<property name="text"> <property name="text">
<string>Apache v2 and/or GPLv2</string> <string>With GTSAM :</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -673,26 +693,6 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="19" column="0">
<widget class="QLabel" name="label_19">
<property name="text">
<string>With GTSAM :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QLabel" name="label_openni2_license">
<property name="text">
<string>Apache v2</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="2"> <item row="1" column="2">
<widget class="QLabel" name="label_opencv_license"> <widget class="QLabel" name="label_opencv_license">
<property name="text"> <property name="text">
@@ -703,6 +703,16 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>With Freenect :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="1"> <item row="7" column="1">
<widget class="QLabel" name="label_freenect"> <widget class="QLabel" name="label_freenect">
<property name="text"> <property name="text">
@@ -726,10 +736,10 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="0"> <item row="22" column="0">
<widget class="QLabel" name="label_12"> <widget class="QLabel" name="label_29">
<property name="text"> <property name="text">
<string>With Freenect :</string> <string>With libpointmatcher :</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -762,16 +772,6 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="21" column="0">
<widget class="QLabel" name="label_29">
<property name="text">
<string>With libpointmatcher :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="1"> <item row="9" column="1">
<widget class="QLabel" name="label_freenect2"> <widget class="QLabel" name="label_freenect2">
<property name="text"> <property name="text">
@@ -785,19 +785,6 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="10" column="1">
<widget class="QLabel" name="label_realsense">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="11" column="0"> <item row="11" column="0">
<widget class="QLabel" name="label_33"> <widget class="QLabel" name="label_33">
<property name="text"> <property name="text">
@@ -808,7 +795,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="31" column="2"> <item row="32" column="2">
<widget class="QLabel" name="label_msckf_license"> <widget class="QLabel" name="label_msckf_license">
<property name="text"> <property name="text">
<string>Penn Software License</string> <string>Penn Software License</string>
@@ -818,18 +805,8 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="27" column="2"> <item row="10" column="1">
<widget class="QLabel" name="label_dvo_license"> <widget class="QLabel" name="label_realsense">
<property name="text">
<string>GPLv3</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_orboctree">
<property name="text"> <property name="text">
<string/> <string/>
</property> </property>
@@ -841,7 +818,17 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="30" column="0"> <item row="28" column="2">
<widget class="QLabel" name="label_dvo_license">
<property name="text">
<string>GPLv3</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="31" column="0">
<widget class="QLabel" name="label_36"> <widget class="QLabel" name="label_36">
<property name="text"> <property name="text">
<string>With loam_velodyne :</string> <string>With loam_velodyne :</string>
@@ -861,8 +848,8 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="24" column="1"> <item row="5" column="1">
<widget class="QLabel" name="label_openchisel"> <widget class="QLabel" name="label_orboctree">
<property name="text"> <property name="text">
<string/> <string/>
</property> </property>
@@ -874,8 +861,8 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="21" column="1"> <item row="25" column="1">
<widget class="QLabel" name="label_libpointmatcher"> <widget class="QLabel" name="label_openchisel">
<property name="text"> <property name="text">
<string/> <string/>
</property> </property>
@@ -897,7 +884,20 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="30" column="1"> <item row="22" column="1">
<widget class="QLabel" name="label_libpointmatcher">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="31" column="1">
<widget class="QLabel" name="label_loam"> <widget class="QLabel" name="label_loam">
<property name="text"> <property name="text">
<string/> <string/>
@@ -920,7 +920,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="27" column="0"> <item row="28" column="0">
<widget class="QLabel" name="label_27"> <widget class="QLabel" name="label_27">
<property name="text"> <property name="text">
<string>With DVO :</string> <string>With DVO :</string>
@@ -930,7 +930,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="26" column="1"> <item row="27" column="1">
<widget class="QLabel" name="label_viso2"> <widget class="QLabel" name="label_viso2">
<property name="text"> <property name="text">
<string/> <string/>
@@ -943,6 +943,26 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="29" column="2">
<widget class="QLabel" name="label_orbslam2_license">
<property name="text">
<string>GPLv3</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="25" column="0">
<widget class="QLabel" name="label_30">
<property name="text">
<string>With OpenChisel :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0"> <item row="0" column="0">
<widget class="QLabel" name="label_32"> <widget class="QLabel" name="label_32">
<property name="text"> <property name="text">
@@ -956,26 +976,6 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="28" column="2">
<widget class="QLabel" name="label_orbslam2_license">
<property name="text">
<string>GPLv3</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="24" column="0">
<widget class="QLabel" name="label_30">
<property name="text">
<string>With OpenChisel :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="17" column="1"> <item row="17" column="1">
<widget class="QLabel" name="label_toro"> <widget class="QLabel" name="label_toro">
<property name="text"> <property name="text">
@@ -999,7 +999,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="29" column="2"> <item row="30" column="2">
<widget class="QLabel" name="label_okvis_license"> <widget class="QLabel" name="label_okvis_license">
<property name="text"> <property name="text">
<string>BSD</string> <string>BSD</string>
@@ -1009,7 +1009,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="30" column="2"> <item row="31" column="2">
<widget class="QLabel" name="label_loam_license"> <widget class="QLabel" name="label_loam_license">
<property name="text"> <property name="text">
<string>BSD</string> <string>BSD</string>
@@ -1039,7 +1039,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="21" column="2"> <item row="22" column="2">
<widget class="QLabel" name="label_libpointmatcher_license"> <widget class="QLabel" name="label_libpointmatcher_license">
<property name="text"> <property name="text">
<string>BSD</string> <string>BSD</string>
@@ -1049,7 +1049,7 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="23" column="2"> <item row="24" column="2">
<widget class="QLabel" name="label_cputsdf_license"> <widget class="QLabel" name="label_cputsdf_license">
<property name="text"> <property name="text">
<string>BSD</string> <string>BSD</string>
@@ -1191,6 +1191,39 @@ p, li { white-space: pre-wrap; }
</property> </property>
</widget> </widget>
</item> </item>
<item row="21" column="0">
<widget class="QLabel" name="label_40">
<property name="text">
<string>With Ceres :</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="21" column="2">
<widget class="QLabel" name="label_ceres_license">
<property name="text">
<string>BSD</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="21" column="1">
<widget class="QLabel" name="label_ceres">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout> </layout>
</item> </item>
</layout> </layout>

5
guilib/src/ui/postProcessingDialog.ui
View File

@@ -292,6 +292,11 @@
<string>cvsba</string> <string>cvsba</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>Ceres</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="3" column="1"> <item row="3" column="1">

19
guilib/src/ui/preferencesDialog.ui
View File

@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>29</number> <number>16</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -10354,6 +10354,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>GTSAM</string> <string>GTSAM</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>Ceres</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="0" column="1"> <item row="0" column="1">
@@ -12703,7 +12708,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QStackedWidget" name="stackedWidget_odometryType"> <widget class="QStackedWidget" name="stackedWidget_odometryType">
<property name="currentIndex"> <property name="currentIndex">
<number>9</number> <number>0</number>
</property> </property>
<widget class="QWidget" name="page_52"> <widget class="QWidget" name="page_52">
<layout class="QVBoxLayout" name="verticalLayout_77"> <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -12885,6 +12890,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>cvsba</string> <string>cvsba</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>Ceres</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="4" column="1"> <item row="4" column="1">
@@ -16569,6 +16579,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>cvsba</string> <string>cvsba</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>Ceres</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="6" column="1"> <item row="6" column="1">