Commit db1af1e5 by Dominik Schlegel

source copy

parent 0be8d8c5
cmake_minimum_required(VERSION 2.8.3)
project(srrg_gslam)
SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
#set(CMAKE_BUILD_TYPE Debug)
set(CMAKE_BUILD_TYPE Release)
#ds DBoW2 configuration
#set(DLIB_INCLUDE_DIR "$ENV{HOME}/source/libraries/DLib/include")
#set(DLIB_LIBRARY "$ENV{HOME}/source/libraries/DLib/build/release/libDLib.so")
#set(DBOW2_INCLUDE_DIR "$ENV{HOME}/source/libraries/DBoW2/include")
#set(DBOW2_LIBRARY "$ENV{HOME}/source/libraries/DBoW2/build/release/libDBoW2.so")
#ds OpenCV: we do not want to use the kinect opencv(3) package
if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
#ds use our custom opencv
set(OpenCV_DIR "$ENV{HOME}/source/libraries/opencv/build/release")
find_package(OpenCV REQUIRED)
message("using custom OpenCV ${OpenCV_VERSION} (${OpenCV_DIR})")
else()
#ds regular find (TODO enforce custom opencv installation?)
find_package(OpenCV REQUIRED)
message("using system OpenCV ${OpenCV_VERSION} (${OpenCV_DIR})")
endif()
find_package(Eigen3 REQUIRED)
find_package(QGLViewer REQUIRED)
#ds check if qt5 is available - lock variable to false if qt4 should be used
set(SRRG_GSLAM_USE_QT_5 false)
find_package(Qt5Widgets)
if(${Qt5Widgets_FOUND})
#ds only enable qt5 if ros kinetic is present
if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
set(SRRG_GSLAM_USE_QT_5 true)
endif()
endif()
#ds depending on found qt installation
if(${SRRG_GSLAM_USE_QT_5})
message("using Qt 5")
find_package(Qt5 COMPONENTS Xml OpenGL Widgets REQUIRED)
else()
message("using Qt 4")
find_package(Qt4 REQUIRED)
endif()
find_package(G2O REQUIRED)
find_package(SuiteSparse REQUIRED)
find_package(Cholmod REQUIRED)
#ds find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
cv_bridge
srrg_core
srrg_gl_helpers
srrg_core_viewers
srrg_hbst
srrg_core_map
roscpp
sensor_msgs
tf
)
################################################
## Declare ROS messages, services and actions ##
################################################
###########
## Build ##
###########
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --std=c++11 -Wall -O0 -g -fPIC -fstack-check")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --std=c++11 -Wall -O3 -DNDEBUG -fPIC")
## Specify additional locations of header files
include_directories(
${EIGEN3_INCLUDE_DIR}
${G2O_INCLUDE_DIR}
${CSPARSE_INCLUDE_DIR}
${OpenCV_INCLUDE_DIRS}
${QGLVIEWER_INCLUDE_DIR}
${CHOLMOD_INCLUDE_DIR}
# ${DLIB_INCLUDE_DIR}
# ${DBOW2_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
src
)
#ds specific includes: qt
if(${SRRG_GSLAM_USE_QT_5})
include_directories(${Qt5Xml_INCLUDE_DIRS} ${Qt5OpenGL_INCLUDE_DIRS} ${Qt5Widgets_INCLUDE_DIRS})
else()
include_directories(${QT_INCLUDES})
endif()
#ds set sources
add_subdirectory(src)
#############
## Install ##
#############
#############
## Testing ##
#############
# Cholmod lib usually requires linking to a blas and lapack library.
# It is up to the user of this module to find a BLAS and link to it.
if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
set(CHOLMOD_FIND_QUIETLY TRUE)
endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
find_path(CHOLMOD_INCLUDE_DIR
NAMES
cholmod.h
PATHS
$ENV{CHOLMODDIR}
${INCLUDE_INSTALL_DIR}
PATH_SUFFIXES
suitesparse
ufsparse
)
find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY})
if(CHOLMOD_LIBRARIES)
get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (AMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (COLAMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (CAMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (CCOLAMD_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
else ()
set(CHOLMOD_LIBRARIES FALSE)
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (CHOLMOD_METIS_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
endif ()
endif(CHOLMOD_LIBRARIES)
if(CHOLMOD_LIBRARIES)
find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
if (SUITESPARSECONFIG_LIBRARY)
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSECONFIG_LIBRARY})
endif ()
endif(CHOLMOD_LIBRARIES)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES)
mark_as_advanced(CHOLMOD_LIBRARIES)
# Find the header files
SET(G2O_LOCAL_DIR ${PROJECT_SOURCE_DIR}/../g2o)
SET(G2O_DEEPER_LOCAL_DIR ${PROJECT_SOURCE_DIR}/../../g2o)
SET(G2O_EXTERNAL_DIR ${PROJECT_SOURCE_DIR}/../../external/g2o_wrapper/g2o)
message(STATUS "Searching for g2o in " $ENV{G2O_ROOT})
FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
${G2O_LOCAL_DIR}
${G2O_DEEPER_LOCAL_DIR}
${G2O_EXTERNAL_DIR}
$ENV{G2O_ROOT}/include
$ENV{G2O_ROOT}
/usr/local/include
/usr/include
/opt/local/include
/sw/local/include
/sw/include
NO_DEFAULT_PATH
)
# Macro to unify finding both the debug and release versions of the
# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
# macro.
MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
FIND_LIBRARY("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
${G2O_LOCAL_DIR}/lib/Debug
${G2O_LOCAL_DIR}/lib
${G2O_DEEPER_LOCAL_DIR}/lib/Debug
${G2O_DEEPER_LOCAL_DIR}/lib
${G2O_EXTERNAL_DIR}/lib/Debug
${G2O_EXTERNAL_DIR}/lib
${G2O_ROOT}/lib/Debug
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Debug
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
FIND_LIBRARY("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
)
FIND_LIBRARY(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
${G2O_LOCAL_DIR}/lib/Release
${G2O_LOCAL_DIR}/lib
${G2O_DEEPER_LOCAL_DIR}/lib/Release
${G2O_DEEPER_LOCAL_DIR}/lib
${G2O_EXTERNAL_DIR}/lib/Release
${G2O_EXTERNAL_DIR}/lib
${G2O_ROOT}/lib/Release
${G2O_ROOT}/lib
$ENV{G2O_ROOT}/lib/Release
$ENV{G2O_ROOT}/lib
NO_DEFAULT_PATH
)
FIND_LIBRARY(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
PATHS
~/Library/Frameworks
/Library/Frameworks
/usr/local/lib
/usr/local/lib64
/usr/lib
/usr/lib64
/opt/local/lib
/sw/local/lib
/sw/lib
)
IF(NOT ${MYLIBRARY}_DEBUG)
IF(MYLIBRARY)
SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
ENDIF(MYLIBRARY)
ENDIF( NOT ${MYLIBRARY}_DEBUG)
ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
# Find the core elements
FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
FIND_G2O_LIBRARY(G2O_OPENGL_HELPER_LIBRARY opengl_helper)
FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
FIND_G2O_LIBRARY(G2O_HIERARCHICAL_LIBRARY hierarchical)
# Find the CLI library
FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
# Find the pluggable solvers
FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
# Find the predefined types
FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D_ADDONS types_slam2d_addons)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons)
# G2O solvers declared found if we found at least one solver
SET(G2O_SOLVERS_FOUND "NO")
IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
SET(G2O_SOLVERS_FOUND "YES")
ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
# 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)
SET(G2O_FOUND "YES")
ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
#ds check if qt5 is available - otherwise fallback to qt4
find_package(Qt5Xml)
# Need to find both Qt4 and QGLViewer if the QQL support is to be built
if(${Qt5Xml_FOUND})
find_package(Qt5 COMPONENTS Xml OpenGL Gui)
else()
find_package(Qt4 COMPONENTS QtXml QtOpenGL QtGui)
endif()
find_path(QGLVIEWER_INCLUDE_DIR qglviewer.h
/usr/include/QGLViewer
/opt/local/include/QGLViewer
/usr/local/include/QGLViewer
/sw/include/QGLViewer
)
if(${Qt5Xml_FOUND})
find_library(QGLVIEWER_LIBRARY NAMES qglviewer-qt5 QGLViewer
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
else()
find_library(QGLVIEWER_LIBRARY NAMES qglviewer-qt4 QGLViewer
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
endif()
if(QGLVIEWER_INCLUDE_DIR AND QGLVIEWER_LIBRARY)
set(QGLVIEWER_FOUND TRUE)
else(QGLVIEWER_INCLUDE_DIR AND QGLVIEWER_LIBRARY)
set(QGLVIEWER_FOUND FALSE)
endif(QGLVIEWER_INCLUDE_DIR AND QGLVIEWER_LIBRARY)
FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h
PATHS
${SUITE_SPARSE_ROOT}/include
/usr/include/suitesparse
/usr/include/ufsparse
/opt/local/include/ufsparse
/usr/local/include/ufsparse
/sw/include/ufsparse
)
FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod
PATHS
${SUITE_SPARSE_ROOT}/lib
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd
PATHS
${SUITE_SPARSE_ROOT}/lib
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(CAMD_LIBRARY NAMES camd
PATHS
${SUITE_SPARSE_ROOT}/lib
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
# Different platforms seemingly require linking against different sets of libraries
IF(CYGWIN)
FIND_PACKAGE(PkgConfig)
FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
PKG_CHECK_MODULES(LAPACK lapack REQUIRED)
SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES})
# MacPorts build of the SparseSuite requires linking against extra libraries
ELSEIF(APPLE)
FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
FIND_LIBRARY(METIS_LIBRARY NAMES metis
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate")
ELSE(APPLE)
SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY})
ENDIF(CYGWIN)
IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
SET(CHOLMOD_FOUND TRUE)
ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
SET(CHOLMOD_FOUND FALSE)
ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
# Look for csparse; note the difference in the directory specifications!
FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
PATHS
/usr/include/suitesparse
/usr/include
/opt/local/include
/usr/local/include
/sw/include
/usr/include/ufsparse
/opt/local/include/ufsparse
/usr/local/include/ufsparse
/sw/include/ufsparse
)
FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
PATHS
/usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
)
IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
SET(CSPARSE_FOUND TRUE)
ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
SET(CSPARSE_FOUND FALSE)
ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
Dominik Schlegel
Mirco Colosi
Giorgio Grisetti
<?xml version="1.0"?>
<package>
<name>srrg_gslam</name>
<version>0.0.0</version>
<description>The gslam package</description>
<maintainer email="giorgio@todo.todo">giorgio</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>thin_state_publisher</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>tf</build_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>thin_state_publisher</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>tf</run_depend>
</package>
add_subdirectory(core)
add_subdirectory(executables)
add_subdirectory(types)
add_subdirectory(ui)
add_subdirectory(utilities)
add_subdirectory(unittests)
add_library(srrg_gslam_core_library
gt_mapper.cpp
gt_relocalizer.cpp
gt_tracker.cpp
tracker_svi.cpp
)
target_link_libraries(srrg_gslam_core_library
srrg_gslam_utilities_library
srrg_gslam_ui_library
# ${DLIB_LIBRARY}
# ${DBOW2_LIBRARY}
)
#include "gt_mapper.h"
namespace gslam {
Mapper::Mapper(): _optimizer(Optimizer::getOptimizer()) {
LOG_INFO("Mapper::Mapper", "constructed");
}
Mapper::~Mapper(){
LOG_INFO("Mapper::Mapper", "destroying");
LOG_INFO("Mapper::Mapper", "destroyed");
}
void Mapper::optimize(TrackingContext* context_) {
CHRONOMETER_START(overall)
optimizePoses(context_);
optimizeLandmarks(context_);
CHRONOMETER_STOP(overall)
}
// optimizes all landmarks by computing rudely the average. no g2o
void Mapper::optimizeLandmarks(TrackingContext* context_){
//ds reset all active landmark positions (redundant calls)
for (const KeyFrame* keyframe: context_->keyframes()) {
for (LandmarkItem* item: keyframe->items()) {
item->landmark()->resetCoordinates();
}
}
//ds update all active landmark positions
for (const KeyFrame* keyframe: context_->keyframes()) {
for (LandmarkItem* item: keyframe->items()) {
//ds buffer current landmark
Landmark* landmark = item->landmark();
assert(landmark);
//ds compute new coordinates according to keyframe position
const PointCoordinates& coordinates_in_world = keyframe->robotToWorld()*item->spatials();
const Matrix3& information(Matrix3::Identity());
landmark->updateCoordinates(coordinates_in_world, information);
landmark->setIsOptimized(true);
landmark->setIsClosed(true);
}
}
}
void Mapper::optimizePoses(TrackingContext* context_) {
//ds compute string for file naming
//const std::string identifier = std::to_string(_id_last_optimization)+ "-" +std::to_string(context_->keyframes().back()->index());
//const std::string file_pose_raw = "g2o/graph_"+ identifier +"_pose_raw.g2o";
//const std::string file_pose_optimized = "g2o/graph_"+ identifier +"_pose_optimized.g2o";
//const std::string file_full_raw = "g2o/graph_"+ identifier +"_full_raw.g2o";
//const std::string file_full_ptimized = "g2o/graph_"+ identifier +"_full_optimized.g2o";
//const std::string file_full_optimized_clean = "g2o/graph_"+ identifier +"_full_optimized_clean.g2o";
_id_last_optimization = context_->keyframes().back()->index();
_optimizer->clear();
_optimizer->clearParameters();
//ds add the camera as parameter
g2o::ParameterCamera* parameter_camera = new g2o::ParameterCamera();
parameter_camera->setId(0);
parameter_camera->setOffset(context_->currentKeyframe()->camera()->cameraToRobot().cast<double>());
parameter_camera->setKcam(static_cast<double>(context_->currentKeyframe()->camera()->cameraMatrix()(0,0)),
static_cast<double>(context_->currentKeyframe()->camera()->cameraMatrix()(1,1)),
static_cast<double>(context_->currentKeyframe()->camera()->cameraMatrix()(0,2)),
static_cast<double>(context_->currentKeyframe()->camera()->cameraMatrix()(1,2)));
_optimizer->addParameter(parameter_camera);
g2o::ParameterSE3Offset* camera_offset = new g2o::ParameterSE3Offset();
camera_offset->setId(1);
camera_offset->setOffset(Eigen::Isometry3d::Identity());
_optimizer->addParameter(camera_offset);
//ds vertex linking
g2o::VertexSE3* vertex_keyframe_previous = 0;
TransformMatrix3D world_to_frame_previous(TransformMatrix3D::Identity());
//ds added measurements
KeyFramePtrVector keyframes_in_graph(0);
//ds pose measurements
for (KeyFrame* keyframe: context_->keyframes()) {
//ds add the pose to g2o
g2o::VertexSE3* vertex_keyframe = new g2o::VertexSE3;
vertex_keyframe->setId(keyframe->index());
vertex_keyframe->setEstimate(keyframe->robotToWorld().cast<double>());
_optimizer->addVertex(vertex_keyframe);
//ds if previous pose is not set (first frame)
if (0 == vertex_keyframe_previous) {
vertex_keyframe->setFixed(true);
} else {
_optimizer->addEdge(Optimizer::getPoseEdge(_optimizer,
vertex_keyframe->id(),
vertex_keyframe_previous->id(),
world_to_frame_previous*keyframe->robotToWorld()));
}
//ds update previous
vertex_keyframe_previous = vertex_keyframe;
world_to_frame_previous = keyframe->worldToRobot();
keyframes_in_graph.push_back(keyframe);
}
//ds closure checking
ClosureBuffer closure_buffer;
LoopClosureChecker closure_checker;
Count number_of_closures_checked = 0;
Count number_of_closures_added = 0;
//ds pose measurements: check for closures now as all id's are in the graph
for (const KeyFrame* keyframe_query: context_->keyframes()) {
//ds candidates for the current pose
OptimizableGraph::EdgeSet closure_candidates;
for (const KeyFrame::KeyFrameCorrespondence& keyframe_correspondence: keyframe_query->closures()) {
const KeyFrame* keyframe_reference = keyframe_correspondence.keyframe;
const TransformMatrix3D transform_query_to_reference = keyframe_correspondence.relation.transform;
closure_candidates.insert(Optimizer::getClosureEdge(_optimizer,
keyframe_query->index(),
keyframe_reference->index(),
transform_query_to_reference));
//ds compute required transform delta
const TransformMatrix3D transform_query_to_reference_current = keyframe_reference->worldToRobot()*keyframe_query->robotToWorld();
const Vector3 translation_delta = transform_query_to_reference.translation()-transform_query_to_reference_current.translation();
//ds check threshold
if (4.0 < translation_delta.norm()) {
std::cerr << "Mapper::optimizePoses|WARNING: adding large impact closure: " << keyframe_query->index() << " > " << keyframe_reference->index() << " translation L1: " << translation_delta.norm() << std::endl;
}
//ds retrieve closure edge
g2o::EdgeSE3* edge_closure = Optimizer::getClosureEdge(_optimizer,
keyframe_query->index(),
keyframe_reference->index(),
transform_query_to_reference);
_optimizer->addEdge(edge_closure);
++number_of_closures_added;
++number_of_closures_checked;
}
#ifdef ENABLE_CLOSURE_CHECKER
//ds add closures for this pose
if (0 < closure_candidates.size()) {
closure_buffer.addEdgeSet(closure_candidates);
closure_buffer.addVertex(_optimizer->vertex(keyframe_query->index()));
}
//ds check buffer state
if (closure_buffer.checkList(_closure_checker_window)) {
//ds determine valid closures
closure_checker.init(closure_buffer.vertices(), closure_buffer.edgeSet(), static_cast<double>(_closure_checker_threshold));
closure_checker.check();
//ds there were closures with inliers
if (0 < closure_checker.inliers()) {
//ds add the inliers to the graph
for (LoopClosureChecker::EdgeDoubleMap::iterator edge_closure = closure_checker.closures( ).begin(); edge_closure != closure_checker.closures( ).end( ); ++edge_closure){
if (_closure_checker_threshold > edge_closure->second) {
_optimizer->addEdge(edge_closure->first);
++number_of_closures_added;
}
}
}
//ds always clear buffer
closure_buffer.removeEdgeSet(closure_buffer.edgeSet());