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

Added support for latest g2o code (with c++11 interface)

Browse Source
This commit is contained in:
matlabbe
2018-02-03 15:43:32 -05:00
parent 8ca3bca810
commit 90ed9cd15c
5 changed files with 130 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+4
View File
@@ -320,6 +320,7 @@ IF(WITH_POINTMATCHER)
ENDIF(libpointmatcher_FOUND)
ENDIF(WITH_POINTMATCHER)
SET(ZED_FOUND FALSE)
IF(WITH_ZED)
IF(WIN32) # Windows
SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
@@ -472,6 +473,9 @@ IF(NOT G2O_FOUND)
SET(G2O "//")
ELSE()
SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
IF(NOT G2O_CPP11)
SET(G2O_CPP_CONF "//")
ENDIF(NOT G2O_CPP11)
ENDIF()
IF(NOT GTSAM_FOUND)
SET(GTSAM "//")
Version.h.in
+1
View File
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@NONFREE@#define RTABMAP_NONFREE
@TORO@#define RTABMAP_TORO
@G2O@#define RTABMAP_G2O
@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
@GTSAM@#define RTABMAP_GTSAM
@VERTIGO@#define RTABMAP_VERTIGO
@OPENCV3@#define RTABMAP_OPENCV3
cmake_modules/FindG2O.cmake
+20 -2
View File
@@ -17,6 +17,14 @@ FIND_LIBRARY(CHOLMOD_LIB cholmod)
FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
PATHS "C:\\Program Files\\g2o\\include")
FIND_FILE(G2O_CONFIG_FILE g2o/config.h
PATHS ${G2O_INCLUDE_DIR}
NO_DEFAULT_PATH)
#ifdef G2O_NUMBER_FORMAT_STR
#define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
#endif
# Macro to unify finding both the debug and release versions of the
# libraries; this is adapted from the rtabmap config
@@ -75,7 +83,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER
# G2O itself declared found if we found the core libraries and at least one solver
SET(G2O_FOUND "NO")
IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
SET(G2O_LIBRARIES
${G2O_CORE_LIBRARY}
@@ -105,5 +113,15 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
${CHOLMOD_LIB})
ENDIF(G2O_SOLVER_CHOLMOD)
FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
IF(${matchres} EQUAL -1)
MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
SET(G2O_CPP11 0)
ELSE()
MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
SET(G2O_CPP11 1)
ENDIF()
SET(G2O_FOUND "YES")
ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
corelib/src/OptimizerG2O.cpp
+99 -20
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <set>
#include <rtabmap/core/Version.h>
#include <rtabmap/core/OptimizerG2O.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_motion_estimation.h>
@@ -82,7 +83,7 @@ typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSpa
typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
#endif
#ifdef RTABMAP_VERTIGO
#if defined(RTABMAP_VERTIGO)
#include "vertigo/g2o/edge_switchPrior.h"
#include "vertigo/g2o/edge_se2Switchable.h"
#include "vertigo/g2o/edge_se3Switchable.h"
@@ -190,6 +191,55 @@ std::map<int, Transform> OptimizerG2O::optimize(
odomOffset->setId(PARAM_OFFSET);
optimizer.addParameter(odomOffset);
#ifdef RTABMAP_G2O_CPP11
std::unique_ptr<SlamBlockSolver> blockSolver;
if(solver_ == 3)
{
//eigen
auto linearSolver = g2o::make_unique<SlamLinearEigenSolver>();
linearSolver->setBlockOrdering(false);
blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
}
#ifdef G2O_HAVE_CHOLMOD
else if(solver_ == 2)
{
//chmold
auto linearSolver = g2o::make_unique<SlamLinearCholmodSolver>();
linearSolver->setBlockOrdering(false);
blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
}
#endif
#ifdef G2O_HAVE_CSPARSE
else if(solver_ == 0)
{
//csparse
auto linearSolver = g2o::make_unique<SlamLinearCSparseSolver>();
linearSolver->setBlockOrdering(false);
blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
}
#endif
else
{
//pcg
auto linearSolver = g2o::make_unique<SlamLinearPCGSolver>();
blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
}
if(optimizer_ == 1)
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(std::move(blockSolver)));
}
else
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)));
}
#else
SlamBlockSolver * blockSolver = 0;
if(solver_ == 3)
@@ -199,26 +249,25 @@ std::map<int, Transform> OptimizerG2O::optimize(
linearSolver->setBlockOrdering(false);
blockSolver = new SlamBlockSolver(linearSolver);
}
#ifdef G2O_HAVE_CHOLMOD
else if(solver_ == 2)
{
#ifdef G2O_HAVE_CHOLMOD
//chmold
SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
linearSolver->setBlockOrdering(false);
blockSolver = new SlamBlockSolver(linearSolver);
#endif
}
#endif
#ifdef G2O_HAVE_CSPARSE
else if(solver_ == 0)
{
#ifdef G2O_HAVE_CSPARSE
//csparse
SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
linearSolver->setBlockOrdering(false);
blockSolver = new SlamBlockSolver(linearSolver);
#endif
}
if(blockSolver == 0)
#endif
else
{
//pcg
SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
@@ -233,7 +282,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
}
#endif
// detect if there is a global pose prior set, if so remove rootId
if(!priorsIgnored())
{
@@ -282,7 +331,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
UDEBUG("fill edges to g2o...");
#if defined(RTABMAP_VERTIGO)
int vertigoVertexId = poses.rbegin()->first+1;
#endif
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->second.from();
@@ -342,7 +393,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
else
{
#ifdef RTABMAP_VERTIGO
#if defined(RTABMAP_VERTIGO)
VertexSwitchLinear * v = 0;
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
@@ -388,7 +439,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
#ifdef RTABMAP_VERTIGO
#if defined(RTABMAP_VERTIGO)
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -433,7 +484,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
constraint = a.linear();
constraint.translation() = a.translation();
#ifdef RTABMAP_VERTIGO
#if defined(RTABMAP_VERTIGO)
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -739,7 +790,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
#ifdef RTABMAP_G2O_CPP11
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
#else
g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
#endif
#ifdef RTABMAP_ORB_SLAM2
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
@@ -747,43 +802,67 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(solver_ == 3)
{
//eigen
#ifdef RTABMAP_G2O_CPP11
linearSolver = g2o::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType> >();
#else
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
#endif
}
#ifdef G2O_HAVE_CHOLMOD
else if(solver_ == 2)
{
#ifdef G2O_HAVE_CHOLMOD
//chmold
#ifdef RTABMAP_G2O_CPP11
linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> >();
#else
linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
#endif
}
#endif
#ifdef G2O_HAVE_CSPARSE
else if(solver_ == 0)
{
#ifdef G2O_HAVE_CSPARSE
//csparse
#ifdef RTABMAP_G2O_CPP11
linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType> >();
#else
linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
#endif
}
if(linearSolver == 0)
#endif
else
{
//pcg
#ifdef RTABMAP_G2O_CPP11
linearSolver = g2o::make_unique<g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType> >();
#else
linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
}
#endif
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
}
#endif // RTABMAP_ORB_SLAM2
#ifndef RTABMAP_ORB_SLAM2
if(optimizer_ == 1)
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr));
#ifdef RTABMAP_G2O_CPP11
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
#else
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(new g2o::BlockSolver_6_3(linearSolver)));
#endif
}
else
#endif
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
#ifdef RTABMAP_G2O_CPP11
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
#else
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new g2o::BlockSolver_6_3(linearSolver)));
#endif
}
UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
{
corelib/src/vertigo/g2o/edge_se3Switchable.cpp
+6 -4
View File
@@ -31,7 +31,7 @@ EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d
// ================================================
bool EdgeSE3Switchable::read(std::istream& is)
{
g2o::Vector7d meas;
/* g2o::Vector7d meas;
for (int i=0; i<7; i++)
is >> meas[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
@@ -44,18 +44,20 @@ bool EdgeSE3Switchable::read(std::istream& is)
if (i!=j)
information()(j,i) = information()(i,j);
}
return true;
return true;*/
return false;
}
// ================================================
bool EdgeSE3Switchable::write(std::ostream& os) const
{
g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
/* g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
for (int i=0; i<7; i++) os << meas[i] << " ";
for (int i = 0; i < 6; ++i)
for (int j = i; j < 6; ++j)
os << " " << information()(i, j);
return os.good();
return os.good();*/
return false;
}
// ================================================