ml/ORB_SLAM3
Archived
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

raw

Browse Source
This commit is contained in:
PodmogilnyjIvan
2021-12-03 03:34:31 -08:00
commit ff4acf84be
542 changed files with 136810 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

178
Thirdparty/g2o/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,178 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(CMAKE_LEGACY_CYGWIN_WIN32 0)
PROJECT(g2o)
SET(g2o_C_FLAGS)
SET(g2o_CXX_FLAGS)
# default built type
IF(NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE Release)
ENDIF()
MESSAGE(STATUS "BUILD TYPE:" ${CMAKE_BUILD_TYPE})
SET (G2O_LIB_TYPE SHARED)
# There seems to be an issue with MSVC8
# see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=83
if(MSVC90)
add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY=1)
message(STATUS "Disabling memory alignment for MSVC8")
endif(MSVC90)
# Set the output directory for the build executables and libraries
IF(WIN32)
SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH "Target for the libraries")
ELSE(WIN32)
SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH "Target for the libraries")
ENDIF(WIN32)
SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${g2o_RUNTIME_OUTPUT_DIRECTORY})
# Set search directory for looking for our custom CMake scripts to
# look for SuiteSparse, QGLViewer, and Eigen3.
LIST(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules)
# Detect OS and define macros appropriately
IF(UNIX)
ADD_DEFINITIONS(-DUNIX)
MESSAGE(STATUS "Compiling on Unix")
ENDIF(UNIX)
# Eigen library parallelise itself, though, presumably due to performance issues
# OPENMP is experimental. We experienced some slowdown with it
FIND_PACKAGE(OpenMP)
SET(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)")
IF(OPENMP_FOUND AND G2O_USE_OPENMP)
SET (G2O_OPENMP 1)
SET(g2o_C_FLAGS "${g2o_C_FLAGS} ${OpenMP_C_FLAGS}")
SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}")
MESSAGE(STATUS "Compiling with OpenMP support")
ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP)
# Compiler specific options for gcc
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native")
SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native")
# SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3")
# SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3")
# activate warnings !!!
SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W")
SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -W")
# specifying compiler flags
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}")
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}")
# Find Eigen3
SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE})
FIND_PACKAGE(Eigen3 3.1.0 REQUIRED)
IF(EIGEN3_FOUND)
SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3")
ELSE(EIGEN3_FOUND)
SET(G2O_EIGEN3_INCLUDE "" CACHE PATH "Directory of Eigen3")
ENDIF(EIGEN3_FOUND)
# Generate config.h
SET(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}")
configure_file(config.h.in ${g2o_SOURCE_DIR}/config.h)
# Set up the top-level include directories
INCLUDE_DIRECTORIES(
${g2o_SOURCE_DIR}/core
${g2o_SOURCE_DIR}/types
${g2o_SOURCE_DIR}/stuff
${G2O_EIGEN3_INCLUDE})
# Include the subdirectories
ADD_LIBRARY(g2o ${G2O_LIB_TYPE}
#types
g2o/types/types_sba.h
g2o/types/types_six_dof_expmap.h
g2o/types/types_sba.cpp
g2o/types/types_six_dof_expmap.cpp
g2o/types/types_seven_dof_expmap.cpp
g2o/types/types_seven_dof_expmap.h
g2o/types/se3quat.h
g2o/types/se3_ops.h
g2o/types/se3_ops.hpp
#core
g2o/core/base_edge.h
g2o/core/base_binary_edge.h
g2o/core/hyper_graph_action.cpp
g2o/core/base_binary_edge.hpp
g2o/core/hyper_graph_action.h
g2o/core/base_multi_edge.h
g2o/core/hyper_graph.cpp
g2o/core/base_multi_edge.hpp
g2o/core/hyper_graph.h
g2o/core/base_unary_edge.h
g2o/core/linear_solver.h
g2o/core/base_unary_edge.hpp
g2o/core/marginal_covariance_cholesky.cpp
g2o/core/base_vertex.h
g2o/core/marginal_covariance_cholesky.h
g2o/core/base_vertex.hpp
g2o/core/matrix_structure.cpp
g2o/core/batch_stats.cpp
g2o/core/matrix_structure.h
g2o/core/batch_stats.h
g2o/core/openmp_mutex.h
g2o/core/block_solver.h
g2o/core/block_solver.hpp
g2o/core/parameter.cpp
g2o/core/parameter.h
g2o/core/cache.cpp
g2o/core/cache.h
g2o/core/optimizable_graph.cpp
g2o/core/optimizable_graph.h
g2o/core/solver.cpp
g2o/core/solver.h
g2o/core/creators.h
g2o/core/optimization_algorithm_factory.cpp
g2o/core/estimate_propagator.cpp
g2o/core/optimization_algorithm_factory.h
g2o/core/estimate_propagator.h
g2o/core/factory.cpp
g2o/core/optimization_algorithm_property.h
g2o/core/factory.h
g2o/core/sparse_block_matrix.h
g2o/core/sparse_optimizer.cpp
g2o/core/sparse_block_matrix.hpp
g2o/core/sparse_optimizer.h
g2o/core/hyper_dijkstra.cpp
g2o/core/hyper_dijkstra.h
g2o/core/parameter_container.cpp
g2o/core/parameter_container.h
g2o/core/optimization_algorithm.cpp
g2o/core/optimization_algorithm.h
g2o/core/optimization_algorithm_with_hessian.cpp
g2o/core/optimization_algorithm_with_hessian.h
g2o/core/optimization_algorithm_levenberg.cpp
g2o/core/optimization_algorithm_levenberg.h
g2o/core/optimization_algorithm_gauss_newton.cpp
g2o/core/optimization_algorithm_gauss_newton.h
g2o/core/jacobian_workspace.cpp
g2o/core/jacobian_workspace.h
g2o/core/robust_kernel.cpp
g2o/core/robust_kernel.h
g2o/core/robust_kernel_factory.cpp
g2o/core/robust_kernel_factory.h
g2o/core/robust_kernel_impl.cpp
g2o/core/robust_kernel_impl.h
#stuff
g2o/stuff/string_tools.h
g2o/stuff/color_macros.h
g2o/stuff/macros.h
g2o/stuff/timeutil.cpp
g2o/stuff/misc.h
g2o/stuff/timeutil.h
g2o/stuff/os_specific.c
g2o/stuff/os_specific.h
g2o/stuff/string_tools.cpp
g2o/stuff/property.cpp
g2o/stuff/property.h
)

3
Thirdparty/g2o/README.txt vendored Normal file
View File

@@ -0,0 +1,3 @@
You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2).
See the original g2o library at: https://github.com/RainerKuemmerle/g2o
All files included in this g2o version are BSD, see license-bsd.txt

13
Thirdparty/g2o/config.h.in vendored Normal file
View File

@@ -0,0 +1,13 @@
#ifndef G2O_CONFIG_H
#define G2O_CONFIG_H
#cmakedefine G2O_OPENMP 1
#cmakedefine G2O_SHARED_LIBS 1
// give a warning if Eigen defaults to row-major matrices.
// We internally assume column-major matrices throughout the code.
#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
# error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)"
#endif
#endif

119
Thirdparty/g2o/g2o/core/base_binary_edge.h vendored Normal file
View File

@@ -0,0 +1,119 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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 G2O_BASE_BINARY_EDGE_H
#define G2O_BASE_BINARY_EDGE_H
#include <iostream>
#include <limits>
#include "base_edge.h"
#include "robust_kernel.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseBinaryEdge : public BaseEdge<D, E>
{
public:
typedef VertexXi VertexXiType;
typedef VertexXj VertexXjType;
static const int Di = VertexXiType::Dimension;
static const int Dj = VertexXjType::Dimension;
static const int Dimension = BaseEdge<D, E>::Dimension;
typedef typename BaseEdge<D,E>::Measurement Measurement;
typedef typename Matrix<double, D, Di>::AlignedMapType JacobianXiOplusType;
typedef typename Matrix<double, D, Dj>::AlignedMapType JacobianXjOplusType;
typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
typedef typename BaseEdge<D,E>::InformationType InformationType;
typedef Eigen::Map<Matrix<double, Di, Dj>, Matrix<double, Di, Dj>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
typedef Eigen::Map<Matrix<double, Dj, Di>, Matrix<double, Dj, Di>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType;
BaseBinaryEdge() : BaseEdge<D,E>(),
_hessianRowMajor(false),
_hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps
_hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension),
_jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj)
{
_vertices.resize(2);
}
virtual OptimizableGraph::Vertex* createFrom();
virtual OptimizableGraph::Vertex* createTo();
virtual void resize(size_t size);
virtual bool allVerticesFixed() const;
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
/**
* Linearizes the oplus operator in the vertex, and stores
* the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
*/
virtual void linearizeOplus();
//! returns the result of the linearization in the manifold space for the node xi
const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
//! returns the result of the linearization in the manifold space for the node xj
const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;}
virtual void constructQuadraticForm() ;
virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
using BaseEdge<D,E>::resize;
using BaseEdge<D,E>::computeError;
protected:
using BaseEdge<D,E>::_measurement;
using BaseEdge<D,E>::_information;
using BaseEdge<D,E>::_error;
using BaseEdge<D,E>::_vertices;
using BaseEdge<D,E>::_dimension;
bool _hessianRowMajor;
HessianBlockType _hessian;
HessianBlockTransposedType _hessianTransposed;
JacobianXiOplusType _jacobianOplusXi;
JacobianXjOplusType _jacobianOplusXj;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_binary_edge.hpp"
} // end namespace g2o
#endif

218
Thirdparty/g2o/g2o/core/base_binary_edge.hpp vendored Normal file
View File

@@ -0,0 +1,218 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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.
template <int D, typename E, typename VertexXiType, typename VertexXjType>
OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createFrom(){
return new VertexXiType();
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createTo(){
return new VertexXjType();
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::resize(size_t size)
{
if (size != 2) {
std::cerr << "WARNING, attempting to resize binary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
}
BaseEdge<D, E>::resize(size);
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
bool BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::allVerticesFixed() const
{
return (static_cast<const VertexXiType*> (_vertices[0])->fixed() &&
static_cast<const VertexXjType*> (_vertices[1])->fixed());
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::constructQuadraticForm()
{
VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
VertexXjType* to = static_cast<VertexXjType*>(_vertices[1]);
// get the Jacobian of the nodes in the manifold domain
const JacobianXiOplusType& A = jacobianOplusXi();
const JacobianXjOplusType& B = jacobianOplusXj();
bool fromNotFixed = !(from->fixed());
bool toNotFixed = !(to->fixed());
if (fromNotFixed || toNotFixed) {
#ifdef G2O_OPENMP
from->lockQuadraticForm();
to->lockQuadraticForm();
#endif
const InformationType& omega = _information;
Matrix<double, D, 1> omega_r = - omega * _error;
if (this->robustKernel() == 0) {
if (fromNotFixed) {
Matrix<double, VertexXiType::Dimension, D> AtO = A.transpose() * omega;
from->b().noalias() += A.transpose() * omega_r;
from->A().noalias() += AtO*A;
if (toNotFixed ) {
if (_hessianRowMajor) // we have to write to the block as transposed
_hessianTransposed.noalias() += B.transpose() * AtO.transpose();
else
_hessian.noalias() += AtO * B;
}
}
if (toNotFixed) {
to->b().noalias() += B.transpose() * omega_r;
to->A().noalias() += B.transpose() * omega * B;
}
} else { // robust (weighted) error according to some kernel
double error = this->chi2();
Eigen::Vector3d rho;
this->robustKernel()->robustify(error, rho);
InformationType weightedOmega = this->robustInformation(rho);
//std::cout << PVAR(rho.transpose()) << std::endl;
//std::cout << PVAR(weightedOmega) << std::endl;
omega_r *= rho[1];
if (fromNotFixed) {
from->b().noalias() += A.transpose() * omega_r;
from->A().noalias() += A.transpose() * weightedOmega * A;
if (toNotFixed ) {
if (_hessianRowMajor) // we have to write to the block as transposed
_hessianTransposed.noalias() += B.transpose() * weightedOmega * A;
else
_hessian.noalias() += A.transpose() * weightedOmega * B;
}
}
if (toNotFixed) {
to->b().noalias() += B.transpose() * omega_r;
to->A().noalias() += B.transpose() * weightedOmega * B;
}
}
#ifdef G2O_OPENMP
to->unlockQuadraticForm();
from->unlockQuadraticForm();
#endif
}
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
{
new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di);
new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj);
linearizeOplus();
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus()
{
VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
VertexXjType* vj = static_cast<VertexXjType*>(_vertices[1]);
bool iNotFixed = !(vi->fixed());
bool jNotFixed = !(vj->fixed());
if (!iNotFixed && !jNotFixed)
return;
#ifdef G2O_OPENMP
vi->lockQuadraticForm();
vj->lockQuadraticForm();
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector errorBak;
ErrorVector errorBeforeNumeric = _error;
if (iNotFixed) {
//Xi - estimate the jacobian numerically
double add_vi[VertexXiType::Dimension];
std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
// add small step along the unit vector in each dimension
for (int d = 0; d < VertexXiType::Dimension; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
errorBak = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
errorBak -= _error;
vi->pop();
add_vi[d] = 0.0;
_jacobianOplusXi.col(d) = scalar * errorBak;
} // end dimension
}
if (jNotFixed) {
//Xj - estimate the jacobian numerically
double add_vj[VertexXjType::Dimension];
std::fill(add_vj, add_vj + VertexXjType::Dimension, 0.0);
// add small step along the unit vector in each dimension
for (int d = 0; d < VertexXjType::Dimension; ++d) {
vj->push();
add_vj[d] = delta;
vj->oplus(add_vj);
computeError();
errorBak = _error;
vj->pop();
vj->push();
add_vj[d] = -delta;
vj->oplus(add_vj);
computeError();
errorBak -= _error;
vj->pop();
add_vj[d] = 0.0;
_jacobianOplusXj.col(d) = scalar * errorBak;
}
} // end dimension
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
vj->unlockQuadraticForm();
vi->unlockQuadraticForm();
#endif
}
template <int D, typename E, typename VertexXiType, typename VertexXjType>
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
{
(void) i; (void) j;
//assert(i == 0 && j == 1);
if (rowMajor) {
new (&_hessianTransposed) HessianBlockTransposedType(d, VertexXjType::Dimension, VertexXiType::Dimension);
} else {
new (&_hessian) HessianBlockType(d, VertexXiType::Dimension, VertexXjType::Dimension);
}
_hessianRowMajor = rowMajor;
}

110
Thirdparty/g2o/g2o/core/base_edge.h vendored Normal file
View File

@@ -0,0 +1,110 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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 G2O_BASE_EDGE_H
#define G2O_BASE_EDGE_H
#include <iostream>
#include <limits>
#include <Eigen/Core>
#include "optimizable_graph.h"
namespace g2o {
using namespace Eigen;
template <int D, typename E>
class BaseEdge : public OptimizableGraph::Edge
{
public:
static const int Dimension = D;
typedef E Measurement;
typedef Matrix<double, D, 1> ErrorVector;
typedef Matrix<double, D, D> InformationType;
BaseEdge() : OptimizableGraph::Edge()
{
_dimension = D;
}
virtual ~BaseEdge() {}
virtual double chi2() const
{
return _error.dot(information()*_error);
}
virtual const double* errorData() const { return _error.data();}
virtual double* errorData() { return _error.data();}
const ErrorVector& error() const { return _error;}
ErrorVector& error() { return _error;}
//! information matrix of the constraint
const InformationType& information() const { return _information;}
InformationType& information() { return _information;}
void setInformation(const InformationType& information) { _information = information;}
virtual const double* informationData() const { return _information.data();}
virtual double* informationData() { return _information.data();}
//! accessor functions for the measurement represented by the edge
const Measurement& measurement() const { return _measurement;}
virtual void setMeasurement(const Measurement& m) { _measurement = m;}
virtual int rank() const {return _dimension;}
virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
{
std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl;
}
protected:
Measurement _measurement;
InformationType _information;
ErrorVector _error;
/**
* calculate the robust information matrix by updating the information matrix of the error
*/
InformationType robustInformation(const Eigen::Vector3d& rho)
{
InformationType result = rho[1] * _information;
//ErrorVector weightedErrror = _information * _error;
//result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());
return result;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // end namespace g2o
#endif

113
Thirdparty/g2o/g2o/core/base_multi_edge.h vendored Normal file
View File

@@ -0,0 +1,113 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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 G2O_BASE_MULTI_EDGE_H
#define G2O_BASE_MULTI_EDGE_H
#include <iostream>
#include <iomanip>
#include <limits>
#include <Eigen/StdVector>
#include "base_edge.h"
#include "robust_kernel.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
/**
* \brief base class to represent an edge connecting an arbitrary number of nodes
*
* D - Dimension of the measurement
* E - type to represent the measurement
*/
template <int D, typename E>
class BaseMultiEdge : public BaseEdge<D,E>
{
public:
/**
* \brief helper for mapping the Hessian memory of the upper triangular block
*/
struct HessianHelper {
Eigen::Map<MatrixXd> matrix; ///< the mapped memory
bool transposed; ///< the block has to be transposed
HessianHelper() : matrix(0, 0, 0), transposed(false) {}
};
public:
static const int Dimension = BaseEdge<D,E>::Dimension;
typedef typename BaseEdge<D,E>::Measurement Measurement;
typedef MatrixXd::MapType JacobianType;
typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
typedef typename BaseEdge<D,E>::InformationType InformationType;
typedef Eigen::Map<MatrixXd, MatrixXd::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
BaseMultiEdge() : BaseEdge<D,E>()
{
}
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
/**
* Linearizes the oplus operator in the vertex, and stores
* the result in temporary variable vector _jacobianOplus
*/
virtual void linearizeOplus();
virtual void resize(size_t size);
virtual bool allVerticesFixed() const;
virtual void constructQuadraticForm() ;
virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
using BaseEdge<D,E>::computeError;
protected:
using BaseEdge<D,E>::_measurement;
using BaseEdge<D,E>::_information;
using BaseEdge<D,E>::_error;
using BaseEdge<D,E>::_vertices;
using BaseEdge<D,E>::_dimension;
std::vector<HessianHelper> _hessian;
std::vector<JacobianType, aligned_allocator<JacobianType> > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus)
void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_multi_edge.hpp"
} // end namespace g2o
#endif

222
Thirdparty/g2o/g2o/core/base_multi_edge.hpp vendored Normal file
View File

@@ -0,0 +1,222 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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.
namespace internal {
inline int computeUpperTriangleIndex(int i, int j)
{
int elemsUpToCol = ((j-1) * j) / 2;
return elemsUpToCol + i;
}
}
template <int D, typename E>
void BaseMultiEdge<D, E>::constructQuadraticForm()
{
if (this->robustKernel()) {
double error = this->chi2();
Eigen::Vector3d rho;
this->robustKernel()->robustify(error, rho);
Matrix<double, D, 1> omega_r = - _information * _error;
omega_r *= rho[1];
computeQuadraticForm(this->robustInformation(rho), omega_r);
} else {
computeQuadraticForm(_information, - _information * _error);
}
}
template <int D, typename E>
void BaseMultiEdge<D, E>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
{
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
assert(v->dimension() >= 0);
new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension());
}
linearizeOplus();
}
template <int D, typename E>
void BaseMultiEdge<D, E>::linearizeOplus()
{
#ifdef G2O_OPENMP
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->lockQuadraticForm();
}
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector errorBak;
ErrorVector errorBeforeNumeric = _error;
for (size_t i = 0; i < _vertices.size(); ++i) {
//Xi - estimate the jacobian numerically
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
if (vi->fixed())
continue;
const int vi_dim = vi->dimension();
assert(vi_dim >= 0);
#ifdef _MSC_VER
double* add_vi = new double[vi_dim];
#else
double add_vi[vi_dim];
#endif
std::fill(add_vi, add_vi + vi_dim, 0.0);
assert(_dimension >= 0);
assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && "jacobian cache dimension does not match");
_jacobianOplus[i].resize(_dimension, vi_dim);
// add small step along the unit vector in each dimension
for (int d = 0; d < vi_dim; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
errorBak = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
errorBak -= _error;
vi->pop();
add_vi[d] = 0.0;
_jacobianOplus[i].col(d) = scalar * errorBak;
} // end dimension
#ifdef _MSC_VER
delete[] add_vi;
#endif
}
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->unlockQuadraticForm();
}
#endif
}
template <int D, typename E>
void BaseMultiEdge<D, E>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
{
int idx = internal::computeUpperTriangleIndex(i, j);
assert(idx < (int)_hessian.size());
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(i));
OptimizableGraph::Vertex* vj = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(j));
assert(vi->dimension() >= 0);
assert(vj->dimension() >= 0);
HessianHelper& h = _hessian[idx];
if (rowMajor) {
if (h.matrix.data() != d || h.transposed != rowMajor)
new (&h.matrix) HessianBlockType(d, vj->dimension(), vi->dimension());
} else {
if (h.matrix.data() != d || h.transposed != rowMajor)
new (&h.matrix) HessianBlockType(d, vi->dimension(), vj->dimension());
}
h.transposed = rowMajor;
}
template <int D, typename E>
void BaseMultiEdge<D, E>::resize(size_t size)
{
BaseEdge<D,E>::resize(size);
int n = (int)_vertices.size();
int maxIdx = (n * (n-1))/2;
assert(maxIdx >= 0);
_hessian.resize(maxIdx);
_jacobianOplus.resize(size, JacobianType(0,0,0));
}
template <int D, typename E>
bool BaseMultiEdge<D, E>::allVerticesFixed() const
{
for (size_t i = 0; i < _vertices.size(); ++i) {
if (!static_cast<const OptimizableGraph::Vertex*> (_vertices[i])->fixed()) {
return false;
}
}
return true;
}
template <int D, typename E>
void BaseMultiEdge<D, E>::computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError)
{
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
bool istatus = !(from->fixed());
if (istatus) {
const MatrixXd& A = _jacobianOplus[i];
MatrixXd AtO = A.transpose() * omega;
int fromDim = from->dimension();
assert(fromDim >= 0);
Eigen::Map<MatrixXd> fromMap(from->hessianData(), fromDim, fromDim);
Eigen::Map<VectorXd> fromB(from->bData(), fromDim);
// ii block in the hessian
#ifdef G2O_OPENMP
from->lockQuadraticForm();
#endif
fromMap.noalias() += AtO * A;
fromB.noalias() += A.transpose() * weightedError;
// compute the off-diagonal blocks ij for all j
for (size_t j = i+1; j < _vertices.size(); ++j) {
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(_vertices[j]);
#ifdef G2O_OPENMP
to->lockQuadraticForm();
#endif
bool jstatus = !(to->fixed());
if (jstatus) {
const MatrixXd& B = _jacobianOplus[j];
int idx = internal::computeUpperTriangleIndex(i, j);
assert(idx < (int)_hessian.size());
HessianHelper& hhelper = _hessian[idx];
if (hhelper.transposed) { // we have to write to the block as transposed
hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
} else {
hhelper.matrix.noalias() += AtO * B;
}
}
#ifdef G2O_OPENMP
to->unlockQuadraticForm();
#endif
}
#ifdef G2O_OPENMP
from->unlockQuadraticForm();
#endif
}
}
}

100
Thirdparty/g2o/g2o/core/base_unary_edge.h vendored Normal file
View File

@@ -0,0 +1,100 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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 G2O_BASE_UNARY_EDGE_H
#define G2O_BASE_UNARY_EDGE_H
#include <iostream>
#include <cassert>
#include <limits>
#include "base_edge.h"
#include "robust_kernel.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
template <int D, typename E, typename VertexXi>
class BaseUnaryEdge : public BaseEdge<D,E>
{
public:
static const int Dimension = BaseEdge<D, E>::Dimension;
typedef typename BaseEdge<D,E>::Measurement Measurement;
typedef VertexXi VertexXiType;
typedef typename Matrix<double, D, VertexXiType::Dimension>::AlignedMapType JacobianXiOplusType;
typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
typedef typename BaseEdge<D,E>::InformationType InformationType;
BaseUnaryEdge() : BaseEdge<D,E>(),
_jacobianOplusXi(0, D, VertexXiType::Dimension)
{
_vertices.resize(1);
}
virtual void resize(size_t size);
virtual bool allVerticesFixed() const;
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
/**
* Linearizes the oplus operator in the vertex, and stores
* the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
*/
virtual void linearizeOplus();
//! returns the result of the linearization in the manifold space for the node xi
const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
virtual void constructQuadraticForm();
virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to);
virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");}
using BaseEdge<D,E>::resize;
using BaseEdge<D,E>::computeError;
protected:
using BaseEdge<D,E>::_measurement;
using BaseEdge<D,E>::_information;
using BaseEdge<D,E>::_error;
using BaseEdge<D,E>::_vertices;
using BaseEdge<D,E>::_dimension;
JacobianXiOplusType _jacobianOplusXi;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_unary_edge.hpp"
} // end namespace g2o
#endif

129
Thirdparty/g2o/g2o/core/base_unary_edge.hpp vendored Normal file
View File

@@ -0,0 +1,129 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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.
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::resize(size_t size)
{
if (size != 1) {
std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
}
BaseEdge<D, E>::resize(size);
}
template <int D, typename E, typename VertexXiType>
bool BaseUnaryEdge<D, E, VertexXiType>::allVerticesFixed() const
{
return static_cast<const VertexXiType*> (_vertices[0])->fixed();
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::constructQuadraticForm()
{
VertexXiType* from=static_cast<VertexXiType*>(_vertices[0]);
// chain rule to get the Jacobian of the nodes in the manifold domain
const JacobianXiOplusType& A = jacobianOplusXi();
const InformationType& omega = _information;
bool istatus = !from->fixed();
if (istatus) {
#ifdef G2O_OPENMP
from->lockQuadraticForm();
#endif
if (this->robustKernel()) {
double error = this->chi2();
Eigen::Vector3d rho;
this->robustKernel()->robustify(error, rho);
InformationType weightedOmega = this->robustInformation(rho);
from->b().noalias() -= rho[1] * A.transpose() * omega * _error;
from->A().noalias() += A.transpose() * weightedOmega * A;
} else {
from->b().noalias() -= A.transpose() * omega * _error;
from->A().noalias() += A.transpose() * omega * A;
}
#ifdef G2O_OPENMP
from->unlockQuadraticForm();
#endif
}
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
{
new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension);
linearizeOplus();
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus()
{
//Xi - estimate the jacobian numerically
VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
if (vi->fixed())
return;
#ifdef G2O_OPENMP
vi->lockQuadraticForm();
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector error1;
ErrorVector errorBeforeNumeric = _error;
double add_vi[VertexXiType::Dimension];
std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
// add small step along the unit vector in each dimension
for (int d = 0; d < VertexXiType::Dimension; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
error1 = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
vi->pop();
add_vi[d] = 0.0;
_jacobianOplusXi.col(d) = scalar * (error1 - _error);
} // end dimension
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
vi->unlockQuadraticForm();
#endif
}
template <int D, typename E, typename VertexXiType>
void BaseUnaryEdge<D, E, VertexXiType>::initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
{
std::cerr << __PRETTY_FUNCTION__ << " is not implemented, please give implementation in your derived class" << std::endl;
}

120
Thirdparty/g2o/g2o/core/base_vertex.h vendored Normal file
View File

@@ -0,0 +1,120 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_BASE_VERTEX_H
#define G2O_BASE_VERTEX_H
#include "optimizable_graph.h"
#include "creators.h"
#include "../stuff/macros.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/StdVector>
#include <stack>
namespace g2o {
using namespace Eigen;
/**
* \brief Templatized BaseVertex
*
* Templatized BaseVertex
* D : minimal dimension of the vertex, e.g., 3 for rotation in 3D
* T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
*/
template <int D, typename T>
class BaseVertex : public OptimizableGraph::Vertex {
public:
typedef T EstimateType;
typedef std::stack<EstimateType,
std::vector<EstimateType, Eigen::aligned_allocator<EstimateType> > >
BackupStackType;
static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space
typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
public:
BaseVertex();
virtual const double& hessian(int i, int j) const { assert(i<D && j<D); return _hessian(i,j);}
virtual double& hessian(int i, int j) { assert(i<D && j<D); return _hessian(i,j);}
virtual double hessianDeterminant() const {return _hessian.determinant();}
virtual double* hessianData() { return const_cast<double*>(_hessian.data());}
virtual void mapHessianMemory(double* d);
virtual int copyB(double* b_) const {
memcpy(b_, _b.data(), Dimension * sizeof(double));
return Dimension;
}
virtual const double& b(int i) const { assert(i < D); return _b(i);}
virtual double& b(int i) { assert(i < D); return _b(i);}
virtual double* bData() { return _b.data();}
virtual void clearQuadraticForm();
//! updates the current vertex with the direct solution x += H_ii\b_ii
//! @returns the determinant of the inverted hessian
virtual double solveDirect(double lambda=0);
//! return right hand side b of the constructed linear system
Matrix<double, D, 1>& b() { return _b;}
const Matrix<double, D, 1>& b() const { return _b;}
//! return the hessian block associated with the vertex
HessianBlockType& A() { return _hessian;}
const HessianBlockType& A() const { return _hessian;}
virtual void push() { _backup.push(_estimate);}
virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}
virtual int stackSize() const {return _backup.size();}
//! return the current estimate of the vertex
const EstimateType& estimate() const { return _estimate;}
//! set the estimate for the vertex also calls updateCache()
void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}
protected:
HessianBlockType _hessian;
Matrix<double, D, 1> _b;
EstimateType _estimate;
BackupStackType _backup;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
#include "base_vertex.hpp"
} // end namespace g2o
#endif

55
Thirdparty/g2o/g2o/core/base_vertex.hpp vendored Normal file
View File

@@ -0,0 +1,55 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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.
template <int D, typename T>
BaseVertex<D, T>::BaseVertex() :
OptimizableGraph::Vertex(),
_hessian(0, D, D)
{
_dimension = D;
}
template <int D, typename T>
double BaseVertex<D, T>::solveDirect(double lambda) {
Matrix <double, D, D> tempA=_hessian + Matrix <double, D, D>::Identity()*lambda;
double det=tempA.determinant();
if (g2o_isnan(det) || det < std::numeric_limits<double>::epsilon())
return det;
Matrix <double, D, 1> dx=tempA.llt().solve(_b);
oplus(&dx[0]);
return det;
}
template <int D, typename T>
void BaseVertex<D, T>::clearQuadraticForm() {
_b.setZero();
}
template <int D, typename T>
void BaseVertex<D, T>::mapHessianMemory(double* d)
{
new (&_hessian) HessianBlockType(d, D, D);
}

90
Thirdparty/g2o/g2o/core/batch_stats.cpp vendored Normal file
View File

@@ -0,0 +1,90 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "batch_stats.h"
#include <cstring>
namespace g2o {
using namespace std;
G2OBatchStatistics* G2OBatchStatistics::_globalStats=0;
#ifndef PTHING
#define PTHING(s) \
#s << "= " << (st.s) << "\t "
#endif
G2OBatchStatistics::G2OBatchStatistics(){
// zero all.
memset (this, 0, sizeof(G2OBatchStatistics));
// set the iteration to -1 to show that it isn't valid
iteration = -1;
}
std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st)
{
os << PTHING(iteration);
os << PTHING( numVertices ); // how many vertices are involved
os << PTHING( numEdges ); // hoe many edges
os << PTHING( chi2 ); // total chi2
/** timings **/
// nonlinear part
os << PTHING( timeResiduals );
os << PTHING( timeLinearize ); // jacobians
os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph
// block_solver (constructs Ax=b, plus maybe schur);
os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done);
// linear solver (computes Ax=b); );
os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done);
os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done);
os << PTHING( timeLinearSolution ); // total time for solving Ax=b
os << PTHING( iterationsLinearSolver ); // iterations of PCG
os << PTHING( timeUpdate ); // oplus
os << PTHING( timeIteration ); // total time );
os << PTHING( levenbergIterations );
os << PTHING( timeLinearSolver);
os << PTHING(hessianDimension);
os << PTHING(hessianPoseDimension);
os << PTHING(hessianLandmarkDimension);
os << PTHING(choleskyNNZ);
os << PTHING(timeMarginals);
return os;
};
void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b)
{
_globalStats = b;
}
} // end namespace

83
Thirdparty/g2o/g2o/core/batch_stats.h vendored Normal file
View File

@@ -0,0 +1,83 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_BATCH_STATS_H_
#define G2O_BATCH_STATS_H_
#include <iostream>
#include <vector>
namespace g2o {
/**
* \brief statistics about the optimization
*/
struct G2OBatchStatistics {
G2OBatchStatistics();
int iteration; ///< which iteration
int numVertices; ///< how many vertices are involved
int numEdges; ///< how many edges
double chi2; ///< total chi2
/** timings **/
// nonlinear part
double timeResiduals; ///< residuals
double timeLinearize; ///< jacobians
double timeQuadraticForm; ///< construct the quadratic form in the graph
int levenbergIterations; ///< number of iterations performed by LM
// block_solver (constructs Ax=b, plus maybe schur)
double timeSchurComplement; ///< compute schur complement (0 if not done)
// linear solver (computes Ax=b);
double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done)
double timeNumericDecomposition; ///< numeric decomposition (0 if not done)
double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur)
double timeLinearSolver; ///< time for solving, excluding Schur setup
int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky)
double timeUpdate; ///< time to apply the update
double timeIteration; ///< total time;
double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances
// information about the Hessian matrix
size_t hessianDimension; ///< rows / cols of the Hessian
size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur
size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur
size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor
static G2OBatchStatistics* globalStats() {return _globalStats;}
static void setGlobalStats(G2OBatchStatistics* b);
protected:
static G2OBatchStatistics* _globalStats;
};
std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&);
typedef std::vector<G2OBatchStatistics> BatchStatisticsContainer;
}
#endif

193
Thirdparty/g2o/g2o/core/block_solver.h vendored Normal file
View File

@@ -0,0 +1,193 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_BLOCK_SOLVER_H
#define G2O_BLOCK_SOLVER_H
#include <Eigen/Core>
#include "solver.h"
#include "linear_solver.h"
#include "sparse_block_matrix.h"
#include "sparse_block_matrix_diagonal.h"
#include "openmp_mutex.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
/**
* \brief traits to summarize the properties of the fixed size optimization problem
*/
template <int _PoseDim, int _LandmarkDim>
struct BlockSolverTraits
{
static const int PoseDim = _PoseDim;
static const int LandmarkDim = _LandmarkDim;
typedef Matrix<double, PoseDim, PoseDim> PoseMatrixType;
typedef Matrix<double, LandmarkDim, LandmarkDim> LandmarkMatrixType;
typedef Matrix<double, PoseDim, LandmarkDim> PoseLandmarkMatrixType;
typedef Matrix<double, PoseDim, 1> PoseVectorType;
typedef Matrix<double, LandmarkDim, 1> LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};
/**
* \brief traits to summarize the properties of the dynamic size optimization problem
*/
template <>
struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>
{
static const int PoseDim = Eigen::Dynamic;
static const int LandmarkDim = Eigen::Dynamic;
typedef MatrixXd PoseMatrixType;
typedef MatrixXd LandmarkMatrixType;
typedef MatrixXd PoseLandmarkMatrixType;
typedef VectorXd PoseVectorType;
typedef VectorXd LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};
/**
* \brief base for the block solvers with some basic function interfaces
*/
class BlockSolverBase : public Solver
{
public:
virtual ~BlockSolverBase() {}
/**
* compute dest = H * src
*/
virtual void multiplyHessian(double* dest, const double* src) const = 0;
};
/**
* \brief Implementation of a solver operating on the blocks of the Hessian
*/
template <typename Traits>
class BlockSolver: public BlockSolverBase {
public:
static const int PoseDim = Traits::PoseDim;
static const int LandmarkDim = Traits::LandmarkDim;
typedef typename Traits::PoseMatrixType PoseMatrixType;
typedef typename Traits::LandmarkMatrixType LandmarkMatrixType;
typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
typedef typename Traits::PoseVectorType PoseVectorType;
typedef typename Traits::LandmarkVectorType LandmarkVectorType;
typedef typename Traits::PoseHessianType PoseHessianType;
typedef typename Traits::LandmarkHessianType LandmarkHessianType;
typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
typedef typename Traits::LinearSolverType LinearSolverType;
public:
/**
* allocate a block solver ontop of the underlying linear solver.
* NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer
* in its destructor.
*/
BlockSolver(LinearSolverType* linearSolver);
~BlockSolver();
virtual bool init(SparseOptimizer* optmizer, bool online = false);
virtual bool buildStructure(bool zeroBlocks = false);
virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
virtual bool buildSystem();
virtual bool solve();
virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
virtual bool setLambda(double lambda, bool backup = false);
virtual void restoreDiagonal();
virtual bool supportsSchur() {return true;}
virtual bool schur() { return _doSchur;}
virtual void setSchur(bool s) { _doSchur = s;}
LinearSolver<PoseMatrixType>* linearSolver() const { return _linearSolver;}
virtual void setWriteDebug(bool writeDebug);
virtual bool writeDebug() const {return _linearSolver->writeDebug();}
virtual bool saveHessian(const std::string& fileName) const;
virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}
protected:
void resize(int* blockPoseIndices, int numPoseBlocks,
int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);
void deallocate();
SparseBlockMatrix<PoseMatrixType>* _Hpp;
SparseBlockMatrix<LandmarkMatrixType>* _Hll;
SparseBlockMatrix<PoseLandmarkMatrixType>* _Hpl;
SparseBlockMatrix<PoseMatrixType>* _Hschur;
SparseBlockMatrixDiagonal<LandmarkMatrixType>* _DInvSchur;
SparseBlockMatrixCCS<PoseLandmarkMatrixType>* _HplCCS;
SparseBlockMatrixCCS<PoseMatrixType>* _HschurTransposedCCS;
LinearSolver<PoseMatrixType>* _linearSolver;
std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;
# ifdef G2O_OPENMP
std::vector<OpenMPMutex> _coefficientsMutex;
# endif
bool _doSchur;
double* _coefficients;
double* _bschur;
int _numPoses, _numLandmarks;
int _sizePoses, _sizeLandmarks;
};
//variable size solver
typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > BlockSolverX;
// solver for BA/3D SLAM
typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;
// solver fo BA with scale
typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;
// 2Dof landmarks 3Dof poses
typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;
} // end namespace
#include "block_solver.hpp"
#endif

634
Thirdparty/g2o/g2o/core/block_solver.hpp vendored Normal file
View File

@@ -0,0 +1,634 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "sparse_optimizer.h"
#include <Eigen/LU>
#include <fstream>
#include <iomanip>
#include "../stuff/timeutil.h"
#include "../stuff/macros.h"
#include "../stuff/misc.h"
namespace g2o {
using namespace std;
using namespace Eigen;
template <typename Traits>
BlockSolver<Traits>::BlockSolver(LinearSolverType* linearSolver) :
BlockSolverBase(),
_linearSolver(linearSolver)
{
// workspace
_Hpp=0;
_Hll=0;
_Hpl=0;
_HplCCS = 0;
_HschurTransposedCCS = 0;
_Hschur=0;
_DInvSchur=0;
_coefficients=0;
_bschur = 0;
_xSize=0;
_numPoses=0;
_numLandmarks=0;
_sizePoses=0;
_sizeLandmarks=0;
_doSchur=true;
}
template <typename Traits>
void BlockSolver<Traits>::resize(int* blockPoseIndices, int numPoseBlocks,
int* blockLandmarkIndices, int numLandmarkBlocks,
int s)
{
deallocate();
resizeVector(s);
if (_doSchur) {
// the following two are only used in schur
assert(_sizePoses > 0 && "allocating with wrong size");
_coefficients = new double [s];
_bschur = new double[_sizePoses];
}
_Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
if (_doSchur) {
_Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
_Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);
_DInvSchur = new SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll->colBlockIndices());
_Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);
_HplCCS = new SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
_HschurTransposedCCS = new SparseBlockMatrixCCS<PoseMatrixType>(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
#ifdef G2O_OPENMP
_coefficientsMutex.resize(numPoseBlocks);
#endif
}
}
template <typename Traits>
void BlockSolver<Traits>::deallocate()
{
if (_Hpp){
delete _Hpp;
_Hpp=0;
}
if (_Hll){
delete _Hll;
_Hll=0;
}
if (_Hpl){
delete _Hpl;
_Hpl = 0;
}
if (_Hschur){
delete _Hschur;
_Hschur=0;
}
if (_DInvSchur){
delete _DInvSchur;
_DInvSchur=0;
}
if (_coefficients) {
delete[] _coefficients;
_coefficients = 0;
}
if (_bschur) {
delete[] _bschur;
_bschur = 0;
}
if (_HplCCS) {
delete _HplCCS;
_HplCCS = 0;
}
if (_HschurTransposedCCS) {
delete _HschurTransposedCCS;
_HschurTransposedCCS = 0;
}
}
template <typename Traits>
BlockSolver<Traits>::~BlockSolver()
{
delete _linearSolver;
deallocate();
}
template <typename Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
{
assert(_optimizer);
size_t sparseDim = 0;
_numPoses=0;
_numLandmarks=0;
_sizePoses=0;
_sizeLandmarks=0;
int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
blockPoseIndices[_numPoses]=_sizePoses;
++_numPoses;
} else {
v->setColInHessian(_sizeLandmarks);
_sizeLandmarks+=dim;
blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
++_numLandmarks;
}
sparseDim += dim;
}
resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
delete[] blockLandmarkIndices;
delete[] blockPoseIndices;
// allocate the diagonal on Hpp and Hll
int poseIdx = 0;
int landmarkIdx = 0;
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
if (! v->marginalized()){
//assert(poseIdx == v->hessianIndex());
PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++poseIdx;
} else {
LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++landmarkIdx;
}
}
assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
// temporary structures for building the pattern of the Schur complement
SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
if (_doSchur) {
schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
}
// here we assume that the landmark indices start after the pose ones
// create the structure in Hpp, Hll and in Hpl
for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
OptimizableGraph::Edge* e = *it;
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
if (ind1 == -1)
continue;
int indexV1Bak = ind1;
for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
ind1 = indexV1Bak;
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
swap(ind1, ind2);
}
if (! v1->marginalized() && !v2->marginalized()){
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
if (_Hschur) {// assume this is only needed in case we solve with the schur complement
schurMatrixLookup->addBlock(ind1, ind2);
}
} else if (v1->marginalized() && v2->marginalized()){
// RAINER hmm.... should we ever reach this here????
LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
} else {
if (v1->marginalized()){
PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
} else {
PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
}
}
}
}
}
if (! _doSchur)
return true;
_DInvSchur->diagonal().resize(landmarkIdx);
_Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
if (v->marginalized()){
const HyperGraph::EdgeSet& vedges=v->edges();
for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
for (size_t i=0; i<(*it1)->vertices().size(); ++i)
{
OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
if (v1->hessianIndex()==-1 || v1==v)
continue;
for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
for (size_t j=0; j<(*it2)->vertices().size(); ++j)
{
OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
if (v2->hessianIndex()==-1 || v2==v)
continue;
int i1=v1->hessianIndex();
int i2=v2->hessianIndex();
if (i1<=i2) {
schurMatrixLookup->addBlock(i1, i2);
}
}
}
}
}
}
}
_Hschur->takePatternFromHash(*schurMatrixLookup);
delete schurMatrixLookup;
_Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);
return true;
}
template <typename Traits>
bool BlockSolver<Traits>::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
{
for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
_Hpp->rowBlockIndices().push_back(_sizePoses);
_Hpp->colBlockIndices().push_back(_sizePoses);
_Hpp->blockCols().push_back(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
++_numPoses;
int ind = v->hessianIndex();
PoseMatrixType* m = _Hpp->block(ind, ind, true);
v->mapHessianMemory(m->data());
} else {
std::cerr << "updateStructure(): Schur not supported" << std::endl;
abort();
}
}
resizeVector(_sizePoses + _sizeLandmarks);
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
int indexV1Bak = ind1;
if (ind1 == -1)
continue;
for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
ind1 = indexV1Bak;
bool transposedBlock = ind1 > ind2;
if (transposedBlock) // make sure, we allocate the upper triangular block
swap(ind1, ind2);
if (! v1->marginalized() && !v2->marginalized()) {
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
} else {
std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
}
}
}
}
return true;
}
template <typename Traits>
bool BlockSolver<Traits>::solve(){
//cerr << __PRETTY_FUNCTION__ << endl;
if (! _doSchur){
double t=get_monotonic_time();
bool ok = _linearSolver->solve(*_Hpp, _x, _b);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
}
return ok;
}
// schur thing
// backup the coefficient matrix
double t=get_monotonic_time();
// _Hschur = _Hpp, but keeping the pattern of _Hschur
_Hschur->clear();
_Hpp->add(_Hschur);
//_DInvSchur->clear();
memset (_coefficients, 0, _sizePoses*sizeof(double));
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
// calculate inverse block for the landmark
const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
assert (D && D->rows()==D->cols() && "Error in landmark matrix");
LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
Dinv = D->inverse();
LandmarkVectorType db(D->rows());
for (int j=0; j<D->rows(); ++j) {
db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
}
db=Dinv*db;
assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
it_outer != landmarkColumn.end(); ++it_outer) {
int i1 = it_outer->row;
const PoseLandmarkMatrixType* Bi = it_outer->block;
assert(Bi);
PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
# ifdef G2O_OPENMP
ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
# endif
Bb.noalias() += (*Bi)*db;
assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
for (; it_inner != landmarkColumn.end(); ++it_inner) {
int i2 = it_inner->row;
const PoseLandmarkMatrixType* Bj = it_inner->block;
assert(Bj);
while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
++targetColumnIt;
assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
assert(Hi1i2);
(*Hi1i2).noalias() -= BDinv*Bj->transpose();
}
}
}
//cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl;
// _bschur = _b for calling solver, and not touching _b
memcpy(_bschur, _b, _sizePoses * sizeof(double));
for (int i=0; i<_sizePoses; ++i){
_bschur[i]-=_coefficients[i];
}
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats){
globalStats->timeSchurComplement = get_monotonic_time() - t;
}
t=get_monotonic_time();
bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianPoseDimension = _Hpp->cols();
globalStats->hessianLandmarkDimension = _Hll->cols();
globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
}
//cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl;
if (! solvedPoses)
return false;
// _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
// solution;
double* xp = _x;
double* cp = _coefficients;
double* xl=_x+_sizePoses;
double* cl=_coefficients + _sizePoses;
double* bl=_b+_sizePoses;
// cp = -xp
for (int i=0; i<_sizePoses; ++i)
cp[i]=-xp[i];
// cl = bl
memcpy(cl,bl,_sizeLandmarks*sizeof(double));
// cl = bl - Bt * xp
//Bt->multiply(cl, cp);
_HplCCS->rightMultiply(cl, cp);
// xl = Dinv * cl
memset(xl,0, _sizeLandmarks*sizeof(double));
_DInvSchur->multiply(xl,cl);
//_DInvSchur->rightMultiply(xl,cl);
//cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl;
return true;
}
template <typename Traits>
bool BlockSolver<Traits>::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
{
double t = get_monotonic_time();
bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeMarginals = get_monotonic_time() - t;
}
return ok;
}
template <typename Traits>
bool BlockSolver<Traits>::buildSystem()
{
// clear b vector
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
assert(v);
v->clearQuadraticForm();
}
_Hpp->clear();
if (_doSchur) {
_Hll->clear();
_Hpl->clear();
}
// resetting the terms for the pairwise constraints
// built up the current system by storing the Hessian blocks in the edges and vertices
# ifndef G2O_OPENMP
// no threading, we do not need to copy the workspace
JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
# else
// if running with threads need to produce copies of the workspace for each thread
JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
# endif
for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
e->constructQuadraticForm();
# ifndef NDEBUG
for (size_t i = 0; i < e->vertices().size(); ++i) {
const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
if (! v->fixed()) {
bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
if (hasANan) {
cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << endl;
break;
}
}
}
# endif
}
// flush the current system in a sparse block matrix
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
int iBase = v->colInHessian();
if (v->marginalized())
iBase+=_sizePoses;
v->copyB(_b+iBase);
}
return 0;
}
template <typename Traits>
bool BlockSolver<Traits>::setLambda(double lambda, bool backup)
{
if (backup) {
_diagonalBackupPose.resize(_numPoses);
_diagonalBackupLandmark.resize(_numLandmarks);
}
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numPoses > 100)
# endif
for (int i = 0; i < _numPoses; ++i) {
PoseMatrixType *b=_Hpp->block(i,i);
if (backup)
_diagonalBackupPose[i] = b->diagonal();
b->diagonal().array() += lambda;
}
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numLandmarks > 100)
# endif
for (int i = 0; i < _numLandmarks; ++i) {
LandmarkMatrixType *b=_Hll->block(i,i);
if (backup)
_diagonalBackupLandmark[i] = b->diagonal();
b->diagonal().array() += lambda;
}
return true;
}
template <typename Traits>
void BlockSolver<Traits>::restoreDiagonal()
{
assert((int) _diagonalBackupPose.size() == _numPoses && "Mismatch in dimensions");
assert((int) _diagonalBackupLandmark.size() == _numLandmarks && "Mismatch in dimensions");
for (int i = 0; i < _numPoses; ++i) {
PoseMatrixType *b=_Hpp->block(i,i);
b->diagonal() = _diagonalBackupPose[i];
}
for (int i = 0; i < _numLandmarks; ++i) {
LandmarkMatrixType *b=_Hll->block(i,i);
b->diagonal() = _diagonalBackupLandmark[i];
}
}
template <typename Traits>
bool BlockSolver<Traits>::init(SparseOptimizer* optimizer, bool online)
{
_optimizer = optimizer;
if (! online) {
if (_Hpp)
_Hpp->clear();
if (_Hpl)
_Hpl->clear();
if (_Hll)
_Hll->clear();
}
_linearSolver->init();
return true;
}
template <typename Traits>
void BlockSolver<Traits>::setWriteDebug(bool writeDebug)
{
_linearSolver->setWriteDebug(writeDebug);
}
template <typename Traits>
bool BlockSolver<Traits>::saveHessian(const std::string& fileName) const
{
return _Hpp->writeOctave(fileName.c_str(), true);
}
} // end namespace

183
Thirdparty/g2o/g2o/core/cache.cpp vendored Normal file
View File

@@ -0,0 +1,183 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard
// 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.
//
// 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 "cache.h"
#include "optimizable_graph.h"
#include "factory.h"
#include <iostream>
namespace g2o {
using namespace std;
Cache::CacheKey::CacheKey() :
_type(), _parameters()
{
}
Cache::CacheKey::CacheKey(const std::string& type_, const ParameterVector& parameters_) :
_type(type_), _parameters(parameters_)
{
}
Cache::Cache(CacheContainer* container_, const ParameterVector& parameters_) :
_updateNeeded(true), _parameters(parameters_), _container(container_)
{
}
bool Cache::CacheKey::operator<(const Cache::CacheKey& c) const{
if (_type < c._type)
return true;
return std::lexicographical_compare (_parameters.begin( ), _parameters.end( ),
c._parameters.begin( ), c._parameters.end( ) );
}
OptimizableGraph::Vertex* Cache::vertex() {
if (container() )
return container()->vertex();
return 0;
}
OptimizableGraph* Cache::graph() {
if (container())
return container()->graph();
return 0;
}
CacheContainer* Cache::container() {
return _container;
}
ParameterVector& Cache::parameters() {
return _parameters;
}
Cache::CacheKey Cache::key() const {
Factory* factory=Factory::instance();
return CacheKey(factory->tag(this), _parameters);
};
void Cache::update(){
if (! _updateNeeded)
return;
for(std::vector<Cache*>::iterator it=_parentCaches.begin(); it!=_parentCaches.end(); it++){
(*it)->update();
}
updateImpl();
_updateNeeded=false;
}
Cache* Cache::installDependency(const std::string& type_, const std::vector<int>& parameterIndices){
ParameterVector pv(parameterIndices.size());
for (size_t i=0; i<parameterIndices.size(); i++){
if (parameterIndices[i]<0 || parameterIndices[i] >=(int)_parameters.size())
return 0;
pv[i]=_parameters[ parameterIndices[i] ];
}
CacheKey k(type_, pv);
if (!container())
return 0;
Cache* c=container()->findCache(k);
if (!c) {
c = container()->createCache(k);
}
if (c)
_parentCaches.push_back(c);
return c;
}
bool Cache::resolveDependancies(){
return true;
}
CacheContainer::CacheContainer(OptimizableGraph::Vertex* vertex_) {
_vertex = vertex_;
}
Cache* CacheContainer::findCache(const Cache::CacheKey& key) {
iterator it=find(key);
if (it==end())
return 0;
return it->second;
}
Cache* CacheContainer::createCache(const Cache::CacheKey& key){
Factory* f = Factory::instance();
HyperGraph::HyperGraphElement* e = f->construct(key.type());
if (!e) {
cerr << __PRETTY_FUNCTION__ << endl;
cerr << "fatal error in creating cache of type " << key.type() << endl;
return 0;
}
Cache* c = dynamic_cast<Cache*>(e);
if (! c){
cerr << __PRETTY_FUNCTION__ << endl;
cerr << "fatal error in creating cache of type " << key.type() << endl;
return 0;
}
c->_container = this;
c->_parameters = key._parameters;
if (c->resolveDependancies()){
insert(make_pair(key,c));
c->update();
return c;
}
return 0;
}
OptimizableGraph::Vertex* CacheContainer::vertex() {
return _vertex;
}
OptimizableGraph* CacheContainer::graph(){
if (_vertex)
return _vertex->graph();
return 0;
}
void CacheContainer::update() {
for (iterator it=begin(); it!=end(); it++){
(it->second)->update();
}
_updateNeeded=false;
}
void CacheContainer::setUpdateNeeded(bool needUpdate) {
_updateNeeded=needUpdate;
for (iterator it=begin(); it!=end(); ++it){
(it->second)->_updateNeeded = needUpdate;
}
}
CacheContainer::~CacheContainer(){
for (iterator it=begin(); it!=end(); ++it){
delete (it->second);
}
}
} // end namespace

140
Thirdparty/g2o/g2o/core/cache.h vendored Normal file
View File

@@ -0,0 +1,140 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_CACHE_HH_
#define G2O_CACHE_HH_
#include <map>
#include "optimizable_graph.h"
namespace g2o {
class CacheContainer;
class Cache: public HyperGraph::HyperGraphElement
{
public:
friend class CacheContainer;
class CacheKey
{
public:
friend class CacheContainer;
CacheKey();
CacheKey(const std::string& type_, const ParameterVector& parameters_);
bool operator<(const CacheKey& c) const;
const std::string& type() const { return _type;}
const ParameterVector& parameters() const { return _parameters;}
protected:
std::string _type;
ParameterVector _parameters;
};
Cache(CacheContainer* container_ = 0, const ParameterVector& parameters_ = ParameterVector());
CacheKey key() const;
OptimizableGraph::Vertex* vertex();
OptimizableGraph* graph();
CacheContainer* container();
ParameterVector& parameters();
void update();
virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_CACHE;}
protected:
//! redefine this to do the update
virtual void updateImpl() = 0;
/**
* this function installs and satisfies a cache
* @param type_: the typename of the dependency
* @param parameterIndices: a vector containing the indices if the parameters
* in _parameters that will be used to assemble the Key of the cache being created
* For example if I have a cache of type C2, having parameters "A, B, and C",
* and it depends on a cache of type C1 that depends on the parameters A and C,
* the parameterIndices should contain "0, 2", since they are the positions in the
* parameter vector of C2 of the parameters needed to construct C1.
* @returns the newly created cache
*/
Cache* installDependency(const std::string& type_, const std::vector<int>& parameterIndices);
/**
* Function to be called from a cache that has dependencies. It just invokes a
* sequence of installDependency().
* Although the caches returned are stored in the _parentCache vector,
* it is better that you redefine your own cache member variables, for better readability
*/
virtual bool resolveDependancies();
bool _updateNeeded;
ParameterVector _parameters;
std::vector<Cache*> _parentCaches;
CacheContainer* _container;
};
class CacheContainer: public std::map<Cache::CacheKey, Cache*>
{
public:
CacheContainer(OptimizableGraph::Vertex* vertex_);
virtual ~CacheContainer();
OptimizableGraph::Vertex* vertex();
OptimizableGraph* graph();
Cache* findCache(const Cache::CacheKey& key);
Cache* createCache(const Cache::CacheKey& key);
void setUpdateNeeded(bool needUpdate=true);
void update();
protected:
OptimizableGraph::Vertex* _vertex;
bool _updateNeeded;
};
template <typename CacheType>
void OptimizableGraph::Edge::resolveCache(CacheType*& cache,
OptimizableGraph::Vertex* v,
const std::string& type_,
const ParameterVector& parameters_)
{
cache = 0;
CacheContainer* container= v->cacheContainer();
Cache::CacheKey key(type_, parameters_);
Cache* c = container->findCache(key);
if (!c) {
c = container->createCache(key);
}
if (c) {
cache = dynamic_cast<CacheType*>(c);
}
}
} // end namespace
#endif

75
Thirdparty/g2o/g2o/core/creators.h vendored Normal file
View File

@@ -0,0 +1,75 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_CREATORS_H
#define G2O_CREATORS_H
#include "hyper_graph.h"
#include <string>
#include <typeinfo>
namespace g2o
{
/**
* \brief Abstract interface for allocating HyperGraphElement
*/
class AbstractHyperGraphElementCreator
{
public:
/**
* create a hyper graph element. Has to implemented in derived class.
*/
virtual HyperGraph::HyperGraphElement* construct() = 0;
/**
* name of the class to be created. Has to implemented in derived class.
*/
virtual const std::string& name() const = 0;
virtual ~AbstractHyperGraphElementCreator() { }
};
/**
* \brief templatized creator class which creates graph elements
*/
template <typename T>
class HyperGraphElementCreator : public AbstractHyperGraphElementCreator
{
public:
HyperGraphElementCreator() : _name(typeid(T).name()) {}
#if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC
__attribute__((force_align_arg_pointer))
#endif
HyperGraph::HyperGraphElement* construct() { return new T;}
virtual const std::string& name() const { return _name;}
protected:
std::string _name;
};
} // end namespace
#endif

73
Thirdparty/g2o/g2o/core/eigen_types.h vendored Normal file
View File

@@ -0,0 +1,73 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_EIGEN_TYPES_H
#define G2O_EIGEN_TYPES_H
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace g2o {
typedef Eigen::Matrix<int,2,1,Eigen::ColMajor> Vector2I;
typedef Eigen::Matrix<int,3,1,Eigen::ColMajor> Vector3I;
typedef Eigen::Matrix<int,4,1,Eigen::ColMajor> Vector4I;
typedef Eigen::Matrix<int,Eigen::Dynamic,1,Eigen::ColMajor> VectorXI;
typedef Eigen::Matrix<float,2,1,Eigen::ColMajor> Vector2F;
typedef Eigen::Matrix<float,3,1,Eigen::ColMajor> Vector3F;
typedef Eigen::Matrix<float,4,1,Eigen::ColMajor> Vector4F;
typedef Eigen::Matrix<float,Eigen::Dynamic,1,Eigen::ColMajor> VectorXF;
typedef Eigen::Matrix<double,2,1,Eigen::ColMajor> Vector2D;
typedef Eigen::Matrix<double,3,1,Eigen::ColMajor> Vector3D;
typedef Eigen::Matrix<double,4,1,Eigen::ColMajor> Vector4D;
typedef Eigen::Matrix<double,Eigen::Dynamic,1,Eigen::ColMajor> VectorXD;
typedef Eigen::Matrix<int,2,2,Eigen::ColMajor> Matrix2I;
typedef Eigen::Matrix<int,3,3,Eigen::ColMajor> Matrix3I;
typedef Eigen::Matrix<int,4,4,Eigen::ColMajor> Matrix4I;
typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXI;
typedef Eigen::Matrix<float,2,2,Eigen::ColMajor> Matrix2F;
typedef Eigen::Matrix<float,3,3,Eigen::ColMajor> Matrix3F;
typedef Eigen::Matrix<float,4,4,Eigen::ColMajor> Matrix4F;
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXF;
typedef Eigen::Matrix<double,2,2,Eigen::ColMajor> Matrix2D;
typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> Matrix3D;
typedef Eigen::Matrix<double,4,4,Eigen::ColMajor> Matrix4D;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXD;
typedef Eigen::Transform<double,2,Eigen::Isometry,Eigen::ColMajor> Isometry2D;
typedef Eigen::Transform<double,3,Eigen::Isometry,Eigen::ColMajor> Isometry3D;
typedef Eigen::Transform<double,2,Eigen::Affine,Eigen::ColMajor> Affine2D;
typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Affine3D;
} // end namespace g2o
#endif

267
Thirdparty/g2o/g2o/core/estimate_propagator.cpp vendored Normal file
View File

@@ -0,0 +1,267 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "estimate_propagator.h"
#include <queue>
#include <vector>
#include <cassert>
#include <iostream>
#include <algorithm>
#include <fstream>
//#define DEBUG_ESTIMATE_PROPAGATOR
using namespace std;
namespace g2o {
# ifdef DEBUG_ESTIMATE_PROPAGATOR
struct FrontierLevelCmp {
bool operator()(EstimatePropagator::AdjacencyMapEntry* e1, EstimatePropagator::AdjacencyMapEntry* e2) const
{
return e1->frontierLevel() < e2->frontierLevel();
}
};
# endif
EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry()
{
reset();
}
void EstimatePropagator::AdjacencyMapEntry::reset()
{
_child = 0;
_parent.clear();
_edge = 0;
_distance = numeric_limits<double>::max();
_frontierLevel = -1;
inQueue = false;
}
EstimatePropagator::EstimatePropagator(OptimizableGraph* g): _graph(g)
{
for (OptimizableGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it){
AdjacencyMapEntry entry;
entry._child = static_cast<OptimizableGraph::Vertex*>(it->second);
_adjacencyMap.insert(make_pair(entry.child(), entry));
}
}
void EstimatePropagator::reset()
{
for (OptimizableGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); ++it){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
AdjacencyMap::iterator at = _adjacencyMap.find(v);
assert(at != _adjacencyMap.end());
at->second.reset();
}
_visited.clear();
}
void EstimatePropagator::propagate(OptimizableGraph::Vertex* v,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action,
double maxDistance,
double maxEdgeCost)
{
OptimizableGraph::VertexSet vset;
vset.insert(v);
propagate(vset, cost, action, maxDistance, maxEdgeCost);
}
void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action,
double maxDistance,
double maxEdgeCost)
{
reset();
PriorityQueue frontier;
for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
AdjacencyMap::iterator it = _adjacencyMap.find(v);
assert(it != _adjacencyMap.end());
it->second._distance = 0.;
it->second._parent.clear();
it->second._frontierLevel = 0;
frontier.push(&it->second);
}
while(! frontier.empty()){
AdjacencyMapEntry* entry = frontier.pop();
OptimizableGraph::Vertex* u = entry->child();
double uDistance = entry->distance();
//cerr << "uDistance " << uDistance << endl;
// initialize the vertex
if (entry->_frontierLevel > 0) {
action(entry->edge(), entry->parent(), u);
}
/* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
while (et != u->edges().end()){
OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
++et;
int maxFrontier = -1;
OptimizableGraph::VertexSet initializedVertices;
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
if (ot->second._distance != numeric_limits<double>::max()) {
initializedVertices.insert(z);
maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
}
}
assert(maxFrontier >= 0);
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
if (z == u)
continue;
size_t wasInitialized = initializedVertices.erase(z);
double edgeDistance = cost(edge, initializedVertices, z);
if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
double zDistance = uDistance + edgeDistance;
//cerr << z->id() << " " << zDistance << endl;
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
assert(ot!=_adjacencyMap.end());
if (zDistance < ot->second.distance() && zDistance < maxDistance){
//if (ot->second.inQueue)
//cerr << "Updating" << endl;
ot->second._distance = zDistance;
ot->second._parent = initializedVertices;
ot->second._edge = edge;
ot->second._frontierLevel = maxFrontier + 1;
frontier.push(&ot->second);
}
}
if (wasInitialized > 0)
initializedVertices.insert(z);
}
}
}
// writing debug information like cost for reaching each vertex and the parent used to initialize
#ifdef DEBUG_ESTIMATE_PROPAGATOR
cerr << "Writing cost.dat" << endl;
ofstream costStream("cost.dat");
for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
HyperGraph::Vertex* u = it->second.child();
costStream << "vertex " << u->id() << " cost " << it->second._distance << endl;
}
cerr << "Writing init.dat" << endl;
ofstream initStream("init.dat");
vector<AdjacencyMapEntry*> frontierLevels;
for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
if (it->second._frontierLevel > 0)
frontierLevels.push_back(&it->second);
}
sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
AdjacencyMapEntry* entry = *it;
OptimizableGraph::Vertex* to = entry->child();
initStream << "calling init level = " << entry->_frontierLevel << "\t (";
for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
initStream << " " << (*pit)->id();
}
initStream << " ) -> " << to->id() << endl;
}
#endif
}
void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry)
{
assert(entry != NULL);
if (entry->inQueue) {
assert(entry->queueIt->second == entry);
erase(entry->queueIt);
}
entry->queueIt = insert(std::make_pair(entry->distance(), entry));
assert(entry->queueIt != end());
entry->inQueue = true;
}
EstimatePropagator::AdjacencyMapEntry* EstimatePropagator::PriorityQueue::pop()
{
assert(!empty());
iterator it = begin();
AdjacencyMapEntry* entry = it->second;
erase(it);
assert(entry != NULL);
entry->queueIt = end();
entry->inQueue = false;
return entry;
}
EstimatePropagatorCost::EstimatePropagatorCost (SparseOptimizer* graph) :
_graph(graph)
{
}
double EstimatePropagatorCost::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const
{
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
if (it == _graph->activeEdges().end()) // it has to be an active edge
return std::numeric_limits<double>::max();
return e->initialEstimatePossible(from, to);
}
EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(SparseOptimizer* graph) :
EstimatePropagatorCost(graph)
{
}
double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const
{
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
OptimizableGraph::Vertex* from = dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph
return std::numeric_limits<double>::max();
SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
if (it == _graph->activeEdges().end()) // it has to be an active edge
return std::numeric_limits<double>::max();
return e->initialEstimatePossible(from_, to);
}
} // end namespace

175
Thirdparty/g2o/g2o/core/estimate_propagator.h vendored Normal file
View File

@@ -0,0 +1,175 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_ESTIMATE_PROPAGATOR_H
#define G2O_ESTIMATE_PROPAGATOR_H
#include "optimizable_graph.h"
#include "sparse_optimizer.h"
#include <map>
#include <set>
#include <limits>
#ifdef _MSC_VER
#include <unordered_map>
#else
#include <tr1/unordered_map>
#endif
namespace g2o {
/**
* \brief cost for traversing along active edges in the optimizer
*
* You may derive an own one, if necessary. The default is to return initialEstimatePossible(from, to) for the edge.
*/
class EstimatePropagatorCost {
public:
EstimatePropagatorCost (SparseOptimizer* graph);
virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const;
virtual const char* name() const { return "spanning tree";}
protected:
SparseOptimizer* _graph;
};
/**
* \brief cost for traversing only odometry edges.
*
* Initialize your graph along odometry edges. An odometry edge is assumed to connect vertices
* whose IDs only differs by one.
*/
class EstimatePropagatorCostOdometry : public EstimatePropagatorCost {
public:
EstimatePropagatorCostOdometry(SparseOptimizer* graph);
virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const;
virtual const char* name() const { return "odometry";}
};
/**
* \brief propagation of an initial guess
*/
class EstimatePropagator {
public:
/**
* \brief Applying the action for propagating.
*
* You may derive an own one, if necessary. The default is to call initialEstimate(from, to) for the edge.
*/
struct PropagateAction {
virtual void operator()(OptimizableGraph::Edge* e, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) const
{
if (! to->fixed())
e->initialEstimate(from, to);
}
};
typedef EstimatePropagatorCost PropagateCost;
class AdjacencyMapEntry;
/**
* \brief priority queue for AdjacencyMapEntry
*/
class PriorityQueue : public std::multimap<double, AdjacencyMapEntry*> {
public:
void push(AdjacencyMapEntry* entry);
AdjacencyMapEntry* pop();
};
/**
* \brief data structure for loopuk during Dijkstra
*/
class AdjacencyMapEntry {
public:
friend class EstimatePropagator;
friend class PriorityQueue;
AdjacencyMapEntry();
void reset();
OptimizableGraph::Vertex* child() const {return _child;}
const OptimizableGraph::VertexSet& parent() const {return _parent;}
OptimizableGraph::Edge* edge() const {return _edge;}
double distance() const {return _distance;}
int frontierLevel() const { return _frontierLevel;}
protected:
OptimizableGraph::Vertex* _child;
OptimizableGraph::VertexSet _parent;
OptimizableGraph::Edge* _edge;
double _distance;
int _frontierLevel;
private: // for PriorityQueue
bool inQueue;
PriorityQueue::iterator queueIt;
};
/**
* \brief hash function for a vertex
*/
class VertexIDHashFunction {
public:
size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}
};
typedef std::tr1::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> AdjacencyMap;
public:
EstimatePropagator(OptimizableGraph* g);
OptimizableGraph::VertexSet& visited() {return _visited; }
AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
OptimizableGraph* graph() {return _graph;}
/**
* propagate an initial guess starting from v. The function computes a spanning tree
* whereas the cost for each edge is determined by calling cost() and the action applied to
* each vertex is action().
*/
void propagate(OptimizableGraph::Vertex* v,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action = PropagateAction(),
double maxDistance=std::numeric_limits<double>::max(),
double maxEdgeCost=std::numeric_limits<double>::max());
/**
* same as above but starting to propagate from a set of vertices instead of just a single one.
*/
void propagate(OptimizableGraph::VertexSet& vset,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action = PropagateAction(),
double maxDistance=std::numeric_limits<double>::max(),
double maxEdgeCost=std::numeric_limits<double>::max());
protected:
void reset();
AdjacencyMap _adjacencyMap;
OptimizableGraph::VertexSet _visited;
OptimizableGraph* _graph;
};
}
#endif

217
Thirdparty/g2o/g2o/core/factory.cpp vendored Normal file
View File

@@ -0,0 +1,217 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "factory.h"
#include "creators.h"
#include "parameter.h"
#include "cache.h"
#include "optimizable_graph.h"
#include "../stuff/color_macros.h"
#include <iostream>
#include <typeinfo>
#include <cassert>
using namespace std;
namespace g2o {
Factory* Factory::factoryInstance = 0;
Factory::Factory()
{
}
Factory::~Factory()
{
# ifdef G2O_DEBUG_FACTORY
cerr << "# Factory destroying " << (void*)this << endl;
# endif
for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {
delete it->second->creator;
}
_creator.clear();
_tagLookup.clear();
}
Factory* Factory::instance()
{
if (factoryInstance == 0) {
factoryInstance = new Factory;
# ifdef G2O_DEBUG_FACTORY
cerr << "# Factory allocated " << (void*)factoryInstance << endl;
# endif
}
return factoryInstance;
}
void Factory::registerType(const std::string& tag, AbstractHyperGraphElementCreator* c)
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end()) {
cerr << "FACTORY WARNING: Overwriting Vertex tag " << tag << endl;
assert(0);
}
TagLookup::const_iterator tagIt = _tagLookup.find(c->name());
if (tagIt != _tagLookup.end()) {
cerr << "FACTORY WARNING: Registering same class for two tags " << c->name() << endl;
assert(0);
}
CreatorInformation* ci = new CreatorInformation();
ci->creator = c;
#ifdef G2O_DEBUG_FACTORY
cerr << "# Factory " << (void*)this << " constructing type " << tag << " ";
#endif
// construct an element once to figure out its type
HyperGraph::HyperGraphElement* element = c->construct();
ci->elementTypeBit = element->elementType();
#ifdef G2O_DEBUG_FACTORY
cerr << "done." << endl;
cerr << "# Factory " << (void*)this << " registering " << tag;
cerr << " " << (void*) c << " ";
switch (element->elementType()) {
case HyperGraph::HGET_VERTEX:
cerr << " -> Vertex";
break;
case HyperGraph::HGET_EDGE:
cerr << " -> Edge";
break;
case HyperGraph::HGET_PARAMETER:
cerr << " -> Parameter";
break;
case HyperGraph::HGET_CACHE:
cerr << " -> Cache";
break;
case HyperGraph::HGET_DATA:
cerr << " -> Data";
break;
default:
assert(0 && "Unknown element type occured, fix elementTypes");
break;
}
cerr << endl;
#endif
_creator[tag] = ci;
_tagLookup[c->name()] = tag;
delete element;
}
void Factory::unregisterType(const std::string& tag)
{
// Look for the tag
CreatorMap::iterator tagPosition = _creator.find(tag);
if (tagPosition != _creator.end()) {
AbstractHyperGraphElementCreator* c = tagPosition->second->creator;
// If we found it, remove the creator from the tag lookup map
TagLookup::iterator classPosition = _tagLookup.find(c->name());
if (classPosition != _tagLookup.end())
{
_tagLookup.erase(classPosition);
}
_creator.erase(tagPosition);
}
}
HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag) const
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end()) {
//cerr << "tag " << tag << " -> " << (void*) foundIt->second->creator << " " << foundIt->second->creator->name() << endl;
return foundIt->second->creator->construct();
}
return 0;
}
const std::string& Factory::tag(const HyperGraph::HyperGraphElement* e) const
{
static std::string emptyStr("");
TagLookup::const_iterator foundIt = _tagLookup.find(typeid(*e).name());
if (foundIt != _tagLookup.end())
return foundIt->second;
return emptyStr;
}
void Factory::fillKnownTypes(std::vector<std::string>& types) const
{
types.clear();
for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
types.push_back(it->first);
}
bool Factory::knowsTag(const std::string& tag, int* elementType) const
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt == _creator.end()) {
if (elementType)
*elementType = -1;
return false;
}
if (elementType)
*elementType = foundIt->second->elementTypeBit;
return true;
}
void Factory::destroy()
{
delete factoryInstance;
factoryInstance = 0;
}
void Factory::printRegisteredTypes(std::ostream& os, bool comment) const
{
if (comment)
os << "# ";
os << "types:" << endl;
for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
if (comment)
os << "#";
cerr << "\t" << it->first << endl;
}
}
HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const
{
if (elemsToConstruct.none()) {
return construct(tag);
}
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end() && foundIt->second->elementTypeBit >= 0 && elemsToConstruct.test(foundIt->second->elementTypeBit)) {
return foundIt->second->creator->construct();
}
return 0;
}
} // end namespace

179
Thirdparty/g2o/g2o/core/factory.h vendored Normal file
View File

@@ -0,0 +1,179 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_FACTORY_H
#define G2O_FACTORY_H
#include "../../config.h"
#include "../stuff/misc.h"
#include "hyper_graph.h"
#include "creators.h"
#include <string>
#include <map>
#include <iostream>
// define to get some verbose output
//#define G2O_DEBUG_FACTORY
namespace g2o {
class AbstractHyperGraphElementCreator;
/**
* \brief create vertices and edges based on TAGs in, for example, a file
*/
class Factory
{
public:
//! return the instance
static Factory* instance();
//! free the instance
static void destroy();
/**
* register a tag for a specific creator
*/
void registerType(const std::string& tag, AbstractHyperGraphElementCreator* c);
/**
* unregister a tag for a specific creator
*/
void unregisterType(const std::string& tag);
/**
* construct a graph element based on its tag
*/
HyperGraph::HyperGraphElement* construct(const std::string& tag) const;
/**
* construct a graph element based on its tag, but only if it's type (a bitmask) matches. A bitmask without any
* bit set will construct any item. Otherwise a bit has to be set to allow construction of a graph element.
*/
HyperGraph::HyperGraphElement* construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const;
/**
* return whether the factory knows this tag or not
*/
bool knowsTag(const std::string& tag, int* elementType = 0) const;
//! return the TAG given a vertex
const std::string& tag(const HyperGraph::HyperGraphElement* v) const;
/**
* get a list of all known types
*/
void fillKnownTypes(std::vector<std::string>& types) const;
/**
* print a list of the known registered types to the given stream
*/
void printRegisteredTypes(std::ostream& os, bool comment = false) const;
protected:
class CreatorInformation
{
public:
AbstractHyperGraphElementCreator* creator;
int elementTypeBit;
CreatorInformation()
{
creator = 0;
elementTypeBit = -1;
}
~CreatorInformation()
{
std::cout << "Deleting " << (void*) creator << std::endl;
delete creator;
}
};
typedef std::map<std::string, CreatorInformation*> CreatorMap;
typedef std::map<std::string, std::string> TagLookup;
Factory();
~Factory();
CreatorMap _creator; ///< look-up map for the existing creators
TagLookup _tagLookup; ///< reverse look-up, class name to tag
private:
static Factory* factoryInstance;
};
template<typename T>
class RegisterTypeProxy
{
public:
RegisterTypeProxy(const std::string& name) : _name(name)
{
#ifdef G2O_DEBUG_FACTORY
std::cout << __FUNCTION__ << ": Registering " << _name << " of type " << typeid(T).name() << std::endl;
#endif
Factory::instance()->registerType(_name, new HyperGraphElementCreator<T>());
}
~RegisterTypeProxy()
{
#ifdef G2O_DEBUG_FACTORY
std::cout << __FUNCTION__ << ": Unregistering " << _name << " of type " << typeid(T).name() << std::endl;
#endif
Factory::instance()->unregisterType(_name);
}
private:
std::string _name;
};
#if defined _MSC_VER && defined G2O_SHARED_LIBS
# define G2O_FACTORY_EXPORT __declspec(dllexport)
# define G2O_FACTORY_IMPORT __declspec(dllimport)
#else
# define G2O_FACTORY_EXPORT
# define G2O_FACTORY_IMPORT
#endif
// These macros are used to automate registering types and forcing linkage
#define G2O_REGISTER_TYPE(name, classname) \
extern "C" void G2O_FACTORY_EXPORT g2o_type_##classname(void) {} \
static g2o::RegisterTypeProxy<classname> g_type_proxy_##classname(#name);
#define G2O_USE_TYPE_BY_CLASS_NAME(classname) \
extern "C" void G2O_FACTORY_IMPORT g2o_type_##classname(void); \
static g2o::ForceLinker proxy_##classname(g2o_type_##classname);
#define G2O_REGISTER_TYPE_GROUP(typeGroupName) \
extern "C" void G2O_FACTORY_EXPORT g2o_type_group_##typeGroupName(void) {}
#define G2O_USE_TYPE_GROUP(typeGroupName) \
extern "C" void G2O_FACTORY_IMPORT g2o_type_group_##typeGroupName(void); \
static g2o::ForceLinker g2o_force_type_link_##typeGroupName(g2o_type_group_##typeGroupName);
}
#endif

261
Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp vendored Normal file
View File

@@ -0,0 +1,261 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 <queue>
#include <deque>
#include <vector>
#include <assert.h>
#include <iostream>
#include "hyper_dijkstra.h"
#include "../stuff/macros.h"
namespace g2o{
using namespace std;
double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e){
(void) v;
(void) vParent;
(void) e;
return std::numeric_limits<double>::max();
}
double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance){
if (distance==-1)
return perform (v,vParent,e);
return std::numeric_limits<double>::max();
}
HyperDijkstra::AdjacencyMapEntry::AdjacencyMapEntry(HyperGraph::Vertex* child_, HyperGraph::Vertex* parent_,
HyperGraph::Edge* edge_, double distance_)
{
_child=child_;
_parent=parent_;
_edge=edge_;
_distance=distance_;
}
HyperDijkstra::HyperDijkstra(HyperGraph* g): _graph(g)
{
for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){
AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());
_adjacencyMap.insert(make_pair(entry.child(), entry));
}
}
void HyperDijkstra::reset()
{
for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){
AdjacencyMap::iterator at=_adjacencyMap.find(*it);
assert(at!=_adjacencyMap.end());
at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());
}
_visited.clear();
}
bool operator<(const HyperDijkstra::AdjacencyMapEntry& a, const HyperDijkstra::AdjacencyMapEntry& b)
{
return a.distance()>b.distance();
}
void HyperDijkstra::shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost,
double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost)
{
reset();
std::priority_queue< AdjacencyMapEntry > frontier;
for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
HyperGraph::Vertex* v=*vit;
assert(v!=0);
AdjacencyMap::iterator it=_adjacencyMap.find(v);
if (it == _adjacencyMap.end()) {
cerr << __PRETTY_FUNCTION__ << "Vertex " << v->id() << " is not in the adjacency map" << endl;
}
assert(it!=_adjacencyMap.end());
it->second._distance=0.;
it->second._parent=0;
frontier.push(it->second);
}
while(! frontier.empty()){
AdjacencyMapEntry entry=frontier.top();
frontier.pop();
HyperGraph::Vertex* u=entry.child();
AdjacencyMap::iterator ut=_adjacencyMap.find(u);
if (ut == _adjacencyMap.end()) {
cerr << __PRETTY_FUNCTION__ << "Vertex " << u->id() << " is not in the adjacency map" << endl;
}
assert(ut!=_adjacencyMap.end());
double uDistance=ut->second.distance();
std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult;
HyperGraph::EdgeSet::iterator et=u->edges().begin();
while (et != u->edges().end()){
HyperGraph::Edge* edge=*et;
++et;
if (directed && edge->vertex(0) != u)
continue;
for (size_t i = 0; i < edge->vertices().size(); ++i) {
HyperGraph::Vertex* z = edge->vertex(i);
if (z == u)
continue;
double edgeDistance=(*cost)(edge, u, z);
if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost)
continue;
double zDistance=uDistance+edgeDistance;
//cerr << z->id() << " " << zDistance << endl;
AdjacencyMap::iterator ot=_adjacencyMap.find(z);
assert(ot!=_adjacencyMap.end());
if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
ot->second._distance=zDistance;
ot->second._parent=u;
ot->second._edge=edge;
frontier.push(ot->second);
}
}
}
}
}
void HyperDijkstra::shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance,
double comparisonConditioner, bool directed, double maxEdgeCost)
{
HyperGraph::VertexSet vset;
vset.insert(v);
shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost);
}
void HyperDijkstra::computeTree(AdjacencyMap& amap)
{
for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
AdjacencyMapEntry& entry(it->second);
entry._children.clear();
}
for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
AdjacencyMapEntry& entry(it->second);
HyperGraph::Vertex* parent=entry.parent();
if (!parent){
continue;
}
HyperGraph::Vertex* v=entry.child();
assert (v==it->first);
AdjacencyMap::iterator pt=amap.find(parent);
assert(pt!=amap.end());
pt->second._children.insert(v);
}
}
void HyperDijkstra::visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance)
{
typedef std::deque<HyperGraph::Vertex*> Deque;
Deque q;
// scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them.
for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
AdjacencyMapEntry& entry(it->second);
if (! entry.parent()) {
action->perform(it->first,0,0);
q.push_back(it->first);
}
}
//std::cerr << "q.size()" << q.size() << endl;
int count=0;
while (! q.empty()){
HyperGraph::Vertex* parent=q.front();
q.pop_front();
++count;
AdjacencyMap::iterator parentIt=amap.find(parent);
if (parentIt==amap.end()) {
continue;
}
//cerr << "parent= " << parent << " parent id= " << parent->id() << "\t children id =";
HyperGraph::VertexSet& childs(parentIt->second.children());
for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){
HyperGraph::Vertex* child=*childsIt;
//cerr << child->id();
AdjacencyMap::iterator adjacencyIt=amap.find(child);
assert (adjacencyIt!=amap.end());
HyperGraph::Edge* edge=adjacencyIt->second.edge();
assert(adjacencyIt->first==child);
assert(adjacencyIt->second.child()==child);
assert(adjacencyIt->second.parent()==parent);
if (! useDistance) {
action->perform(child, parent, edge);
} else {
action->perform(child, parent, edge, adjacencyIt->second.distance());
}
q.push_back(child);
}
//cerr << endl;
}
}
void HyperDijkstra::connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited,
HyperGraph::VertexSet& startingSet,
HyperGraph* g, HyperGraph::Vertex* v,
HyperDijkstra::CostFunction* cost, double distance,
double comparisonConditioner, double maxEdgeCost)
{
typedef std::queue<HyperGraph::Vertex*> VertexDeque;
visited.clear();
connected.clear();
VertexDeque frontier;
HyperDijkstra dv(g);
connected.insert(v);
frontier.push(v);
while (! frontier.empty()){
HyperGraph::Vertex* v0=frontier.front();
frontier.pop();
dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost);
for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){
visited.insert(*it);
if (startingSet.find(*it)==startingSet.end())
continue;
std::pair<HyperGraph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
if (insertOutcome.second){ // the node was not in the connectedSet;
frontier.push(dynamic_cast<HyperGraph::Vertex*>(*it));
}
}
}
}
double UniformCostFunction::operator () (HyperGraph::Edge* /*edge*/, HyperGraph::Vertex* /*from*/, HyperGraph::Vertex* /*to*/)
{
return 1.;
}
};

112
Thirdparty/g2o/g2o/core/hyper_dijkstra.h vendored Normal file
View File

@@ -0,0 +1,112 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_AIS_GENERAL_DIJKSTRA_HH
#define G2O_AIS_GENERAL_DIJKSTRA_HH
#include <map>
#include <set>
#include <limits>
#include "hyper_graph.h"
namespace g2o{
struct HyperDijkstra{
struct CostFunction {
virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0;
virtual ~CostFunction() { }
};
struct TreeAction {
virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e);
virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance);
};
struct AdjacencyMapEntry{
friend struct HyperDijkstra;
AdjacencyMapEntry(HyperGraph::Vertex* _child=0,
HyperGraph::Vertex* _parent=0,
HyperGraph::Edge* _edge=0,
double _distance=std::numeric_limits<double>::max());
HyperGraph::Vertex* child() const {return _child;}
HyperGraph::Vertex* parent() const {return _parent;}
HyperGraph::Edge* edge() const {return _edge;}
double distance() const {return _distance;}
HyperGraph::VertexSet& children() {return _children;}
const HyperGraph::VertexSet& children() const {return _children;}
protected:
HyperGraph::Vertex* _child;
HyperGraph::Vertex* _parent;
HyperGraph::Edge* _edge;
double _distance;
HyperGraph::VertexSet _children;
};
typedef std::map<HyperGraph::Vertex*, AdjacencyMapEntry> AdjacencyMap;
HyperDijkstra(HyperGraph* g);
HyperGraph::VertexSet& visited() {return _visited; }
AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
HyperGraph* graph() {return _graph;}
void shortestPaths(HyperGraph::Vertex* v,
HyperDijkstra::CostFunction* cost,
double maxDistance=std::numeric_limits< double >::max(),
double comparisonConditioner=1e-3,
bool directed=false,
double maxEdgeCost=std::numeric_limits< double >::max());
void shortestPaths(HyperGraph::VertexSet& vset,
HyperDijkstra::CostFunction* cost,
double maxDistance=std::numeric_limits< double >::max(),
double comparisonConditioner=1e-3,
bool directed=false,
double maxEdgeCost=std::numeric_limits< double >::max());
static void computeTree(AdjacencyMap& amap);
static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false);
static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited,
HyperGraph::VertexSet& startingSet,
HyperGraph* g, HyperGraph::Vertex* v,
HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner,
double maxEdgeCost=std::numeric_limits< double >::max() );
protected:
void reset();
AdjacencyMap _adjacencyMap;
HyperGraph::VertexSet _visited;
HyperGraph* _graph;
};
struct UniformCostFunction: public HyperDijkstra::CostFunction {
virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to);
};
}
#endif

166
Thirdparty/g2o/g2o/core/hyper_graph.cpp vendored Normal file
View File

@@ -0,0 +1,166 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "hyper_graph.h"
#include <assert.h>
#include <queue>
namespace g2o {
HyperGraph::Vertex::Vertex(int id) : _id(id)
{
}
HyperGraph::Vertex::~Vertex()
{
}
HyperGraph::Edge::Edge(int id) : _id(id)
{
}
HyperGraph::Edge::~Edge()
{
}
void HyperGraph::Edge::resize(size_t size)
{
_vertices.resize(size, 0);
}
void HyperGraph::Edge::setId(int id)
{
_id = id;
}
HyperGraph::Vertex* HyperGraph::vertex(int id)
{
VertexIDMap::iterator it=_vertices.find(id);
if (it==_vertices.end())
return 0;
return it->second;
}
const HyperGraph::Vertex* HyperGraph::vertex(int id) const
{
VertexIDMap::const_iterator it=_vertices.find(id);
if (it==_vertices.end())
return 0;
return it->second;
}
bool HyperGraph::addVertex(Vertex* v)
{
Vertex* vn=vertex(v->id());
if (vn)
return false;
_vertices.insert( std::make_pair(v->id(),v) );
return true;
}
/**
* changes the id of a vertex already in the graph, and updates the bookkeeping
@ returns false if the vertex is not in the graph;
*/
bool HyperGraph::changeId(Vertex* v, int newId){
Vertex* v2 = vertex(v->id());
if (v != v2)
return false;
_vertices.erase(v->id());
v->setId(newId);
_vertices.insert(std::make_pair(v->id(), v));
return true;
}
bool HyperGraph::addEdge(Edge* e)
{
std::pair<EdgeSet::iterator, bool> result = _edges.insert(e);
if (! result.second)
return false;
for (std::vector<Vertex*>::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
Vertex* v = *it;
v->edges().insert(e);
}
return true;
}
bool HyperGraph::removeVertex(Vertex* v)
{
VertexIDMap::iterator it=_vertices.find(v->id());
if (it==_vertices.end())
return false;
assert(it->second==v);
//remove all edges which are entering or leaving v;
EdgeSet tmp(v->edges());
for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){
if (!removeEdge(*it)){
assert(0);
}
}
_vertices.erase(it);
delete v;
return true;
}
bool HyperGraph::removeEdge(Edge* e)
{
EdgeSet::iterator it = _edges.find(e);
if (it == _edges.end())
return false;
_edges.erase(it);
for (std::vector<Vertex*>::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
Vertex* v = *vit;
it = v->edges().find(e);
assert(it!=v->edges().end());
v->edges().erase(it);
}
delete e;
return true;
}
HyperGraph::HyperGraph()
{
}
void HyperGraph::clear()
{
for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it)
delete (it->second);
for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it)
delete (*it);
_vertices.clear();
_edges.clear();
}
HyperGraph::~HyperGraph()
{
clear();
}
} // end namespace

220
Thirdparty/g2o/g2o/core/hyper_graph.h vendored Normal file
View File

@@ -0,0 +1,220 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_AIS_HYPER_GRAPH_HH
#define G2O_AIS_HYPER_GRAPH_HH
#include <map>
#include <set>
#include <bitset>
#include <cassert>
#include <vector>
#include <limits>
#include <cstddef>
#ifdef _MSC_VER
#include <unordered_map>
#else
#include <tr1/unordered_map>
#endif
/** @addtogroup graph */
//@{
namespace g2o {
/**
Class that models a directed Hyper-Graph. An hyper graph is a graph where an edge
can connect one or more nodes. Both Vertices and Edges of an hyoper graph
derive from the same class HyperGraphElement, thus one can implement generic algorithms
that operate transparently on edges or vertices (see HyperGraphAction).
The vertices are uniquely identified by an int id, while the edges are
identfied by their pointers.
*/
class HyperGraph
{
public:
/**
* \brief enum of all the types we have in our graphs
*/
enum HyperGraphElementType {
HGET_VERTEX,
HGET_EDGE,
HGET_PARAMETER,
HGET_CACHE,
HGET_DATA,
HGET_NUM_ELEMS // keep as last elem
};
typedef std::bitset<HyperGraph::HGET_NUM_ELEMS> GraphElemBitset;
class Vertex;
class Edge;
/**
* base hyper graph element, specialized in vertex and edge
*/
struct HyperGraphElement {
virtual ~HyperGraphElement() {}
/**
* returns the type of the graph element, see HyperGraphElementType
*/
virtual HyperGraphElementType elementType() const = 0;
};
typedef std::set<Edge*> EdgeSet;
typedef std::set<Vertex*> VertexSet;
typedef std::unordered_map<int, Vertex*> VertexIDMap;
typedef std::vector<Vertex*> VertexContainer;
//! abstract Vertex, your types must derive from that one
class Vertex : public HyperGraphElement {
public:
//! creates a vertex having an ID specified by the argument
explicit Vertex(int id=-1);
virtual ~Vertex();
//! returns the id
int id() const {return _id;}
virtual void setId( int newId) { _id=newId; }
//! returns the set of hyper-edges that are leaving/entering in this vertex
const EdgeSet& edges() const {return _edges;}
//! returns the set of hyper-edges that are leaving/entering in this vertex
EdgeSet& edges() {return _edges;}
virtual HyperGraphElementType elementType() const { return HGET_VERTEX;}
protected:
int _id;
EdgeSet _edges;
};
/**
* Abstract Edge class. Your nice edge classes should inherit from that one.
* An hyper-edge has pointers to the vertices it connects and stores them in a vector.
*/
class Edge : public HyperGraphElement {
public:
//! creates and empty edge with no vertices
explicit Edge(int id = -1);
virtual ~Edge();
/**
* resizes the number of vertices connected by this edge
*/
virtual void resize(size_t size);
/**
returns the vector of pointers to the vertices connected by the hyper-edge.
*/
const VertexContainer& vertices() const { return _vertices;}
/**
returns the vector of pointers to the vertices connected by the hyper-edge.
*/
VertexContainer& vertices() { return _vertices;}
/**
returns the pointer to the ith vertex connected to the hyper-edge.
*/
const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
/**
returns the pointer to the ith vertex connected to the hyper-edge.
*/
Vertex* vertex(size_t i) { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
/**
set the ith vertex on the hyper-edge to the pointer supplied
*/
void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}
int id() const {return _id;}
void setId(int id);
virtual HyperGraphElementType elementType() const { return HGET_EDGE;}
protected:
VertexContainer _vertices;
int _id; ///< unique id
};
public:
//! constructs an empty hyper graph
HyperGraph();
//! destroys the hyper-graph and all the vertices of the graph
virtual ~HyperGraph();
//! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
Vertex* vertex(int id);
//! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
const Vertex* vertex(int id) const;
//! removes a vertex from the graph. Returns true on success (vertex was present)
virtual bool removeVertex(Vertex* v);
//! removes a vertex from the graph. Returns true on success (edge was present)
virtual bool removeEdge(Edge* e);
//! clears the graph and empties all structures.
virtual void clear();
//! @returns the map <i>id -> vertex</i> where the vertices are stored
const VertexIDMap& vertices() const {return _vertices;}
//! @returns the map <i>id -> vertex</i> where the vertices are stored
VertexIDMap& vertices() {return _vertices;}
//! @returns the set of edges of the hyper graph
const EdgeSet& edges() const {return _edges;}
//! @returns the set of edges of the hyper graph
EdgeSet& edges() {return _edges;}
/**
* adds a vertex to the graph. The id of the vertex should be set before
* invoking this function. the function fails if another vertex
* with the same id is already in the graph.
* returns true, on success, or false on failure.
*/
virtual bool addVertex(Vertex* v);
/**
* Adds an edge to the graph. If the edge is already in the graph, it
* does nothing and returns false. Otherwise it returns true.
*/
virtual bool addEdge(Edge* e);
/**
* changes the id of a vertex already in the graph, and updates the bookkeeping
@ returns false if the vertex is not in the graph;
*/
virtual bool changeId(Vertex* v, int newId);
protected:
VertexIDMap _vertices;
EdgeSet _edges;
private:
// Disable the copy constructor and assignment operator
HyperGraph(const HyperGraph&) { }
HyperGraph& operator= (const HyperGraph&) { return *this; }
};
} // end namespace
//@}
#endif

268
Thirdparty/g2o/g2o/core/hyper_graph_action.cpp vendored Normal file
View File

@@ -0,0 +1,268 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "hyper_graph_action.h"
#include "optimizable_graph.h"
#include "../stuff/macros.h"
#include <iostream>
namespace g2o {
using namespace std;
HyperGraphActionLibrary* HyperGraphActionLibrary::actionLibInstance = 0;
HyperGraphAction::Parameters::~Parameters()
{
}
HyperGraphAction::ParametersIteration::ParametersIteration(int iter) :
HyperGraphAction::Parameters(),
iteration(iter)
{
}
HyperGraphAction::~HyperGraphAction()
{
}
HyperGraphAction* HyperGraphAction::operator()(const HyperGraph*, Parameters*)
{
return 0;
}
HyperGraphElementAction::Parameters::~Parameters()
{
}
HyperGraphElementAction::HyperGraphElementAction(const std::string& typeName_)
{
_typeName = typeName_;
}
void HyperGraphElementAction::setTypeName(const std::string& typeName_)
{
_typeName = typeName_;
}
HyperGraphElementAction* HyperGraphElementAction::operator()(HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )
{
return 0;
}
HyperGraphElementAction* HyperGraphElementAction::operator()(const HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )
{
return 0;
}
HyperGraphElementAction::~HyperGraphElementAction()
{
}
HyperGraphElementActionCollection::HyperGraphElementActionCollection(const std::string& name_)
{
_name = name_;
}
HyperGraphElementActionCollection::~HyperGraphElementActionCollection()
{
for (ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {
delete it->second;
}
}
HyperGraphElementAction* HyperGraphElementActionCollection::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)
{
ActionMap::iterator it=_actionMap.find(typeid(*element).name());
//cerr << typeid(*element).name() << endl;
if (it==_actionMap.end())
return 0;
HyperGraphElementAction* action=it->second;
return (*action)(element, params);
}
HyperGraphElementAction* HyperGraphElementActionCollection::operator()(const HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)
{
ActionMap::iterator it=_actionMap.find(typeid(*element).name());
if (it==_actionMap.end())
return 0;
HyperGraphElementAction* action=it->second;
return (*action)(element, params);
}
bool HyperGraphElementActionCollection::registerAction(HyperGraphElementAction* action)
{
# ifdef G2O_DEBUG_ACTIONLIB
cerr << __PRETTY_FUNCTION__ << " " << action->name() << " " << action->typeName() << endl;
# endif
if (action->name()!=name()){
cerr << __PRETTY_FUNCTION__ << ": invalid attempt to register an action in a collection with a different name " << name() << " " << action->name() << endl;
}
_actionMap.insert(make_pair ( action->typeName(), action) );
return true;
}
bool HyperGraphElementActionCollection::unregisterAction(HyperGraphElementAction* action)
{
for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {
if (it->second == action){
_actionMap.erase(it);
return true;
}
}
return false;
}
HyperGraphActionLibrary::HyperGraphActionLibrary()
{
}
HyperGraphActionLibrary* HyperGraphActionLibrary::instance()
{
if (actionLibInstance == 0) {
actionLibInstance = new HyperGraphActionLibrary;
}
return actionLibInstance;
}
void HyperGraphActionLibrary::destroy()
{
delete actionLibInstance;
actionLibInstance = 0;
}
HyperGraphActionLibrary::~HyperGraphActionLibrary()
{
for (HyperGraphElementAction::ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {
delete it->second;
}
}
HyperGraphElementAction* HyperGraphActionLibrary::actionByName(const std::string& name)
{
HyperGraphElementAction::ActionMap::iterator it=_actionMap.find(name);
if (it!=_actionMap.end())
return it->second;
return 0;
}
bool HyperGraphActionLibrary::registerAction(HyperGraphElementAction* action)
{
HyperGraphElementAction* oldAction = actionByName(action->name());
HyperGraphElementActionCollection* collection = 0;
if (oldAction) {
collection = dynamic_cast<HyperGraphElementActionCollection*>(oldAction);
if (! collection) {
cerr << __PRETTY_FUNCTION__ << ": fatal error, a collection is not at the first level in the library" << endl;
return 0;
}
}
if (! collection) {
#ifdef G2O_DEBUG_ACTIONLIB
cerr << __PRETTY_FUNCTION__ << ": creating collection for \"" << action->name() << "\"" << endl;
#endif
collection = new HyperGraphElementActionCollection(action->name());
_actionMap.insert(make_pair(action->name(), collection));
}
return collection->registerAction(action);
}
bool HyperGraphActionLibrary::unregisterAction(HyperGraphElementAction* action)
{
list<HyperGraphElementActionCollection*> collectionDeleteList;
// Search all the collections and delete the registered actions; if a collection becomes empty, schedule it for deletion; note that we can't delete the collections as we go because this will screw up the state of the iterators
for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {
HyperGraphElementActionCollection* collection = dynamic_cast<HyperGraphElementActionCollection*> (it->second);
if (collection != 0) {
collection->unregisterAction(action);
if (collection->actionMap().size() == 0) {
collectionDeleteList.push_back(collection);
}
}
}
// Delete any empty action collections
for (list<HyperGraphElementActionCollection*>::iterator itc = collectionDeleteList.begin(); itc != collectionDeleteList.end(); ++itc) {
//cout << "Deleting collection " << (*itc)->name() << endl;
_actionMap.erase((*itc)->name());
}
return true;
}
WriteGnuplotAction::WriteGnuplotAction(const std::string& typeName_)
: HyperGraphElementAction(typeName_)
{
_name="writeGnuplot";
}
DrawAction::Parameters::Parameters(){
}
DrawAction::DrawAction(const std::string& typeName_)
: HyperGraphElementAction(typeName_)
{
_name="draw";
_previousParams = (Parameters*)0x42;
refreshPropertyPtrs(0);
}
bool DrawAction::refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_){
if (_previousParams == params_)
return false;
DrawAction::Parameters* p=dynamic_cast<DrawAction::Parameters*>(params_);
if (! p){
_previousParams = 0;
_show = 0;
_showId = 0;
} else {
_previousParams = p;
_show = p->makeProperty<BoolProperty>(_typeName+"::SHOW", true);
_showId = p->makeProperty<BoolProperty>(_typeName+"::SHOW_ID", false);
}
return true;
}
void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* params, const std::string& typeName)
{
for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin();
it!=graph->vertices().end(); ++it){
if ( typeName.empty() || typeid(*it->second).name()==typeName){
(*action)(it->second, params);
}
}
for (HyperGraph::EdgeSet::iterator it=graph->edges().begin();
it!=graph->edges().end(); ++it){
if ( typeName.empty() || typeid(**it).name()==typeName)
(*action)(*it, params);
}
}
} // end namespace

222
Thirdparty/g2o/g2o/core/hyper_graph_action.h vendored Normal file
View File

@@ -0,0 +1,222 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_HYPER_GRAPH_ACTION_H
#define G2O_HYPER_GRAPH_ACTION_H
#include "hyper_graph.h"
#include "../stuff/property.h"
#include <typeinfo>
#include <iosfwd>
#include <set>
#include <string>
#include <iostream>
// define to get verbose output
//#define G2O_DEBUG_ACTIONLIB
namespace g2o {
/**
* \brief Abstract action that operates on an entire graph
*/
class HyperGraphAction {
public:
class Parameters {
public:
virtual ~Parameters();
};
class ParametersIteration : public Parameters {
public:
explicit ParametersIteration(int iter);
int iteration;
};
virtual ~HyperGraphAction();
/**
* re-implement to carry out an action given the graph
*/
virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0);
};
/**
* \brief Abstract action that operates on a graph entity
*/
class HyperGraphElementAction{
public:
struct Parameters{
virtual ~Parameters();
};
typedef std::map<std::string, HyperGraphElementAction*> ActionMap;
//! an action should be instantiated with the typeid.name of the graph element
//! on which it operates
HyperGraphElementAction(const std::string& typeName_="");
//! redefine this to do the action stuff. If successful, the action returns a pointer to itself
virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);
//! redefine this to do the action stuff. If successful, the action returns a pointer to itself
virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);
//! destroyed actions release the memory
virtual ~HyperGraphElementAction();
//! returns the typeid name of the action
const std::string& typeName() const { return _typeName;}
//! returns the name of an action, e.g "draw"
const std::string& name() const{ return _name;}
//! sets the type on which an action has to operate
void setTypeName(const std::string& typeName_);
protected:
std::string _typeName;
std::string _name;
};
/**
* \brief collection of actions
*
* collection of actions calls contains homogeneous actions operating on different types
* all collected actions have the same name and should have the same functionality
*/
class HyperGraphElementActionCollection: public HyperGraphElementAction{
public:
//! constructor. name_ is the name of the action e.g.draw).
HyperGraphElementActionCollection(const std::string& name_);
//! destructor: it deletes all actions in the pool.
virtual ~HyperGraphElementActionCollection();
//! calling functions, they return a pointer to the instance of action in actionMap
//! that was active on element
virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);
virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);
ActionMap& actionMap() {return _actionMap;}
//! inserts an action in the pool. The action should have the same name of the container.
//! returns false on failure (the container has a different name than the action);
bool registerAction(HyperGraphElementAction* action);
bool unregisterAction(HyperGraphElementAction* action);
protected:
ActionMap _actionMap;
};
/**
* \brief library of actions, indexed by the action name;
*
* library of actions, indexed by the action name;
* one can use ti to register a collection of actions
*/
class HyperGraphActionLibrary{
public:
//! return the single instance of the HyperGraphActionLibrary
static HyperGraphActionLibrary* instance();
//! free the instance
static void destroy();
// returns a pointer to a collection indexed by name
HyperGraphElementAction* actionByName(const std::string& name);
// registers a basic action in the pool. If necessary a container is created
bool registerAction(HyperGraphElementAction* action);
bool unregisterAction(HyperGraphElementAction* action);
inline HyperGraphElementAction::ActionMap& actionMap() {return _actionMap;}
protected:
HyperGraphActionLibrary();
~HyperGraphActionLibrary();
HyperGraphElementAction::ActionMap _actionMap;
private:
static HyperGraphActionLibrary* actionLibInstance;
};
/**
* apply an action to all the elements of the graph.
*/
void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* parameters=0, const std::string& typeName="");
/**
* brief write into gnuplot
*/
class WriteGnuplotAction: public HyperGraphElementAction{
public:
struct Parameters: public HyperGraphElementAction::Parameters{
std::ostream* os;
};
WriteGnuplotAction(const std::string& typeName_);
};
/**
* \brief draw actions
*/
class DrawAction : public HyperGraphElementAction{
public:
class Parameters: public HyperGraphElementAction::Parameters, public PropertyMap{
public:
Parameters();
};
DrawAction(const std::string& typeName_);
protected:
virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_);
Parameters* _previousParams;
BoolProperty* _show;
BoolProperty* _showId;
};
template<typename T> class RegisterActionProxy
{
public:
RegisterActionProxy()
{
#ifdef G2O_DEBUG_ACTIONLIB
std::cout << __FUNCTION__ << ": Registering action of type " << typeid(T).name() << std::endl;
#endif
_action = new T();
HyperGraphActionLibrary::instance()->registerAction(_action);
}
~RegisterActionProxy()
{
#ifdef G2O_DEBUG_ACTIONLIB
std::cout << __FUNCTION__ << ": Unregistering action of type " << typeid(T).name() << std::endl;
#endif
HyperGraphActionLibrary::instance()->unregisterAction(_action);
delete _action;
}
private:
HyperGraphElementAction* _action;
};
#define G2O_REGISTER_ACTION(classname) \
extern "C" void g2o_action_##classname(void) {} \
static g2o::RegisterActionProxy<classname> g_action_proxy_##classname;
};
#endif

89
Thirdparty/g2o/g2o/core/jacobian_workspace.cpp vendored Normal file
View File

@@ -0,0 +1,89 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "jacobian_workspace.h"
#include <cmath>
#include "optimizable_graph.h"
using namespace std;
namespace g2o {
JacobianWorkspace::JacobianWorkspace() :
_maxNumVertices(-1), _maxDimension(-1)
{
}
JacobianWorkspace::~JacobianWorkspace()
{
}
bool JacobianWorkspace::allocate()
{
//cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
if (_maxNumVertices <=0 || _maxDimension <= 0)
return false;
_workspace.resize(_maxNumVertices);
for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) {
it->resize(_maxDimension);
it->setZero();
}
return true;
}
void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_)
{
const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(e_);
int errorDimension = e->dimension();
int numVertices = e->vertices().size();
int maxDimensionForEdge = -1;
for (int i = 0; i < numVertices; ++i) {
const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
assert(v && "Edge has no vertex assigned");
maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge);
}
_maxNumVertices = max(numVertices, _maxNumVertices);
_maxDimension = max(maxDimensionForEdge, _maxDimension);
//cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
}
void JacobianWorkspace::updateSize(const OptimizableGraph& graph)
{
for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) {
const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
updateSize(e);
}
}
void JacobianWorkspace::updateSize(int numVertices, int dimension)
{
_maxNumVertices = max(numVertices, _maxNumVertices);
_maxDimension = max(dimension, _maxDimension);
}
} // end namespace

96
Thirdparty/g2o/g2o/core/jacobian_workspace.h vendored Normal file
View File

@@ -0,0 +1,96 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 JACOBIAN_WORKSPACE_H
#define JACOBIAN_WORKSPACE_H
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <vector>
#include <cassert>
#include "hyper_graph.h"
namespace g2o {
struct OptimizableGraph;
/**
* \brief provide memory workspace for computing the Jacobians
*
* The workspace is used by an OptimizableGraph to provide temporary memory
* for computing the Jacobian of the error functions.
* Before calling linearizeOplus on an edge, the workspace needs to be allocated
* by calling allocate().
*/
class JacobianWorkspace
{
public:
typedef std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > WorkspaceVector;
public:
JacobianWorkspace();
~JacobianWorkspace();
/**
* allocate the workspace
*/
bool allocate();
/**
* update the maximum required workspace needed by taking into account this edge
*/
void updateSize(const HyperGraph::Edge* e);
/**
* update the required workspace by looking at a full graph
*/
void updateSize(const OptimizableGraph& graph);
/**
* manually update with the given parameters
*/
void updateSize(int numVertices, int dimension);
/**
* return the workspace for a vertex in an edge
*/
double* workspaceForVertex(int vertexIndex)
{
assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds");
return _workspace[vertexIndex].data();
}
protected:
WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians
int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge
int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian
};
} // end namespace
#endif

109
Thirdparty/g2o/g2o/core/linear_solver.h vendored Normal file
View File

@@ -0,0 +1,109 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_LINEAR_SOLVER_H
#define G2O_LINEAR_SOLVER_H
#include "sparse_block_matrix.h"
#include "sparse_block_matrix_ccs.h"
namespace g2o {
/**
* \brief basic solver for Ax = b
*
* basic solver for Ax = b which has to reimplemented for different linear algebra libraries.
* A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit.
*/
template <typename MatrixType>
class LinearSolver
{
public:
LinearSolver() {};
virtual ~LinearSolver() {}
/**
* init for operating on matrices with a different non-zero pattern like before
*/
virtual bool init() = 0;
/**
* Assumes that A is the same matrix for several calls.
* Among other assumptions, the non-zero pattern does not change!
* If the matrix changes call init() before.
* solve system Ax = b, x and b have to allocated beforehand!!
*/
virtual bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b) = 0;
/**
* Inverts the diagonal blocks of A
* @returns false if not defined.
*/
virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix<MatrixType>& A) { (void)blocks; (void) A; return false; }
/**
* Inverts the a block pattern of A in spinv
* @returns false if not defined.
*/
virtual bool solvePattern(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices, const SparseBlockMatrix<MatrixType>& A){
(void) spinv;
(void) blockIndices;
(void) A;
return false;
}
//! write a debug dump of the system matrix if it is not PSD in solve
virtual bool writeDebug() const { return false;}
virtual void setWriteDebug(bool) {}
};
/**
* \brief Solver with faster iterating structure for the linear matrix
*/
template <typename MatrixType>
class LinearSolverCCS : public LinearSolver<MatrixType>
{
public:
LinearSolverCCS() : LinearSolver<MatrixType>(), _ccsMatrix(0) {}
~LinearSolverCCS()
{
delete _ccsMatrix;
}
protected:
SparseBlockMatrixCCS<MatrixType>* _ccsMatrix;
void initMatrixStructure(const SparseBlockMatrix<MatrixType>& A)
{
delete _ccsMatrix;
_ccsMatrix = new SparseBlockMatrixCCS<MatrixType>(A.rowBlockIndices(), A.colBlockIndices());
A.fillSparseBlockMatrixCCS(*_ccsMatrix);
}
};
} // end namespace
#endif

222
Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp vendored Normal file
View File

@@ -0,0 +1,222 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "marginal_covariance_cholesky.h"
#include <algorithm>
#include <cassert>
using namespace std;
namespace g2o {
struct MatrixElem
{
int r, c;
MatrixElem(int r_, int c_) : r(r_), c(c_) {}
bool operator<(const MatrixElem& other) const
{
return c > other.c || (c == other.c && r > other.r);
}
};
MarginalCovarianceCholesky::MarginalCovarianceCholesky() :
_n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0)
{
}
MarginalCovarianceCholesky::~MarginalCovarianceCholesky()
{
}
void MarginalCovarianceCholesky::setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv)
{
_n = n;
_Ap = Lp;
_Ai = Li;
_Ax = Lx;
_perm = permInv;
// pre-compute reciprocal values of the diagonal of L
_diag.resize(n);
for (int r = 0; r < n; ++r) {
const int& sc = _Ap[r]; // L is lower triangular, thus the first elem in the column is the diagonal entry
assert(r == _Ai[sc] && "Error in CCS storage of L");
_diag[r] = 1.0 / _Ax[sc];
}
}
double MarginalCovarianceCholesky::computeEntry(int r, int c)
{
assert(r <= c);
int idx = computeIndex(r, c);
LookupMap::const_iterator foundIt = _map.find(idx);
if (foundIt != _map.end()) {
return foundIt->second;
}
// compute the summation over column r
double s = 0.;
const int& sc = _Ap[r];
const int& ec = _Ap[r+1];
for (int j = sc+1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal
const int& rr = _Ai[j];
double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr);
s += val * _Ax[j];
}
double result;
if (r == c) {
const double& diagElem = _diag[r];
result = diagElem * (diagElem - s);
} else {
result = -s * _diag[r];
}
_map[idx] = result;
return result;
}
void MarginalCovarianceCholesky::computeCovariance(double** covBlocks, const std::vector<int>& blockIndices)
{
_map.clear();
int base = 0;
vector<MatrixElem> elemsToCompute;
for (size_t i = 0; i < blockIndices.size(); ++i) {
int nbase = blockIndices[i];
int vdim = nbase - base;
for (int rr = 0; rr < vdim; ++rr)
for (int cc = rr; cc < vdim; ++cc) {
int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
int c = _perm ? _perm[cc + base] : cc + base;
if (r > c) // make sure it's still upper triangular after applying the permutation
swap(r, c);
elemsToCompute.push_back(MatrixElem(r, c));
}
base = nbase;
}
// sort the elems to reduce the recursive calls
sort(elemsToCompute.begin(), elemsToCompute.end());
// compute the inverse elements we need
for (size_t i = 0; i < elemsToCompute.size(); ++i) {
const MatrixElem& me = elemsToCompute[i];
computeEntry(me.r, me.c);
}
// set the marginal covariance for the vertices, by writing to the blocks memory
base = 0;
for (size_t i = 0; i < blockIndices.size(); ++i) {
int nbase = blockIndices[i];
int vdim = nbase - base;
double* cov = covBlocks[i];
for (int rr = 0; rr < vdim; ++rr)
for (int cc = rr; cc < vdim; ++cc) {
int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
int c = _perm ? _perm[cc + base] : cc + base;
if (r > c) // upper triangle
swap(r, c);
int idx = computeIndex(r, c);
LookupMap::const_iterator foundIt = _map.find(idx);
assert(foundIt != _map.end());
cov[rr*vdim + cc] = foundIt->second;
if (rr != cc)
cov[cc*vdim + rr] = foundIt->second;
}
base = nbase;
}
}
void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices)
{
// allocate the sparse
spinv = SparseBlockMatrix<MatrixXd>(&rowBlockIndices[0],
&rowBlockIndices[0],
rowBlockIndices.size(),
rowBlockIndices.size(), true);
_map.clear();
vector<MatrixElem> elemsToCompute;
for (size_t i = 0; i < blockIndices.size(); ++i) {
int blockRow=blockIndices[i].first;
int blockCol=blockIndices[i].second;
assert(blockRow>=0);
assert(blockRow < (int)rowBlockIndices.size());
assert(blockCol>=0);
assert(blockCol < (int)rowBlockIndices.size());
int rowBase=spinv.rowBaseOfBlock(blockRow);
int colBase=spinv.colBaseOfBlock(blockCol);
MatrixXd *block=spinv.block(blockRow, blockCol, true);
assert(block);
for (int iRow=0; iRow<block->rows(); ++iRow)
for (int iCol=0; iCol<block->cols(); ++iCol){
int rr=rowBase+iRow;
int cc=colBase+iCol;
int r = _perm ? _perm[rr] : rr; // apply permutation
int c = _perm ? _perm[cc] : cc;
if (r > c)
swap(r, c);
elemsToCompute.push_back(MatrixElem(r, c));
}
}
// sort the elems to reduce the number of recursive calls
sort(elemsToCompute.begin(), elemsToCompute.end());
// compute the inverse elements we need
for (size_t i = 0; i < elemsToCompute.size(); ++i) {
const MatrixElem& me = elemsToCompute[i];
computeEntry(me.r, me.c);
}
// set the marginal covariance
for (size_t i = 0; i < blockIndices.size(); ++i) {
int blockRow=blockIndices[i].first;
int blockCol=blockIndices[i].second;
int rowBase=spinv.rowBaseOfBlock(blockRow);
int colBase=spinv.colBaseOfBlock(blockCol);
MatrixXd *block=spinv.block(blockRow, blockCol);
assert(block);
for (int iRow=0; iRow<block->rows(); ++iRow)
for (int iCol=0; iCol<block->cols(); ++iCol){
int rr=rowBase+iRow;
int cc=colBase+iCol;
int r = _perm ? _perm[rr] : rr; // apply permutation
int c = _perm ? _perm[cc] : cc;
if (r > c)
swap(r, c);
int idx = computeIndex(r, c);
LookupMap::const_iterator foundIt = _map.find(idx);
assert(foundIt != _map.end());
(*block)(iRow, iCol) = foundIt->second;
}
}
}
} // end namespace

103
Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h vendored Normal file
View File

@@ -0,0 +1,103 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_MARGINAL_COVARIANCE_CHOLESKY_H
#define G2O_MARGINAL_COVARIANCE_CHOLESKY_H
#include "optimizable_graph.h"
#include "sparse_block_matrix.h"
#include <cassert>
#include <vector>
#ifdef _MSC_VER
#include <unordered_map>
#else
#include <tr1/unordered_map>
#endif
namespace g2o {
/**
* \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor)
*/
class MarginalCovarianceCholesky {
protected:
/**
* hash struct for storing the matrix elements needed to compute the covariance
*/
typedef std::tr1::unordered_map<int, double> LookupMap;
public:
MarginalCovarianceCholesky();
~MarginalCovarianceCholesky();
/**
* compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to
* be provided by the caller).
*/
void computeCovariance(double** covBlocks, const std::vector<int>& blockIndices);
/**
* compute the marginal cov for the given block indices, write the result in spinv).
*/
void computeCovariance(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices);
/**
* set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in.
* permInv might be 0, will then not permute the entries.
*
* The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers
* are owned by the caller, MarginalCovarianceCholesky does not free the pointers.
*/
void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv);
protected:
// information about the cholesky factor (lower triangle)
int _n; ///< L is an n X n matrix
int* _Ap; ///< column pointer of the CCS storage
int* _Ai; ///< row indices of the CCS storage
double* _Ax; ///< values of the cholesky factor
int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in
LookupMap _map; ///< hash look up table for the already computed entries
std::vector<double> _diag; ///< cache 1 / H_ii to avoid recalculations
//! compute the index used for hashing
int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;}
/**
* compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular.
* May issue recursive calls to itself to compute the missing values.
*/
double computeEntry(int r, int c);
};
}
#endif

74
Thirdparty/g2o/g2o/core/matrix_operations.h vendored Normal file
View File

@@ -0,0 +1,74 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_CORE_MATRIX_OPERATIONS_H
#define G2O_CORE_MATRIX_OPERATIONS_H
#include <Eigen/Core>
namespace g2o {
namespace internal {
template<typename MatrixType>
inline void axpy(const MatrixType& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
{
y.segment<MatrixType::RowsAtCompileTime>(yoff) += A * x.segment<MatrixType::ColsAtCompileTime>(xoff);
}
template<int t>
inline void axpy(const Eigen::Matrix<double, Eigen::Dynamic, t>& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
{
y.segment(yoff, A.rows()) += A * x.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(xoff);
}
template<>
inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
{
y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols());
}
template<typename MatrixType>
inline void atxpy(const MatrixType& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
{
y.segment<MatrixType::ColsAtCompileTime>(yoff) += A.transpose() * x.segment<MatrixType::RowsAtCompileTime>(xoff);
}
template<int t>
inline void atxpy(const Eigen::Matrix<double, Eigen::Dynamic, t>& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
{
y.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows());
}
template<>
inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
{
y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows());
}
} // end namespace internal
} // end namespace g2o
#endif

125
Thirdparty/g2o/g2o/core/matrix_structure.cpp vendored Normal file
View File

@@ -0,0 +1,125 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "matrix_structure.h"
#include <string>
#include <vector>
#include <fstream>
#include <algorithm>
using namespace std;
namespace g2o {
struct ColSort
{
bool operator()(const pair<int, int>& e1, const pair<int, int>& e2) const
{
return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first);
}
};
MatrixStructure::MatrixStructure() :
n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0)
{
}
MatrixStructure::~MatrixStructure()
{
free();
}
void MatrixStructure::alloc(int n_, int nz)
{
if (n == 0) {
maxN = n = n_;
maxNz = nz;
Ap = new int[maxN + 1];
Aii = new int[maxNz];
}
else {
n = n_;
if (maxNz < nz) {
maxNz = 2 * nz;
delete[] Aii;
Aii = new int[maxNz];
}
if (maxN < n) {
maxN = 2 * n;
delete[] Ap;
Ap = new int[maxN + 1];
}
}
}
void MatrixStructure::free()
{
n = 0;
m = 0;
maxN = 0;
maxNz = 0;
delete[] Aii; Aii = 0;
delete[] Ap; Ap = 0;
}
bool MatrixStructure::write(const char* filename) const
{
const int& cols = n;
const int& rows = m;
string name = filename;
std::string::size_type lastDot = name.find_last_of('.');
if (lastDot != std::string::npos)
name = name.substr(0, lastDot);
vector<pair<int, int> > entries;
for (int i=0; i < cols; ++i) {
const int& rbeg = Ap[i];
const int& rend = Ap[i+1];
for (int j = rbeg; j < rend; ++j) {
entries.push_back(make_pair(Aii[j], i));
if (Aii[j] != i)
entries.push_back(make_pair(i, Aii[j]));
}
}
sort(entries.begin(), entries.end(), ColSort());
std::ofstream fout(filename);
fout << "# name: " << name << std::endl;
fout << "# type: sparse matrix" << std::endl;
fout << "# nnz: " << entries.size() << std::endl;
fout << "# rows: " << rows << std::endl;
fout << "# columns: " << cols << std::endl;
for (vector<pair<int, int> >::const_iterator it = entries.begin(); it != entries.end(); ++it) {
const pair<int, int>& entry = *it;
fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0
}
return fout.good();
}
} // end namespace

69
Thirdparty/g2o/g2o/core/matrix_structure.h vendored Normal file
View File

@@ -0,0 +1,69 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_MATRIX_STRUCTURE_H
#define G2O_MATRIX_STRUCTURE_H
namespace g2o {
/**
* \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix)
*/
class MatrixStructure
{
public:
MatrixStructure();
~MatrixStructure();
/**
* allocate space for the Matrix Structure. You may call this on an already allocated struct, it will
* then reallocate the memory + additional space (double the required space).
*/
void alloc(int n_, int nz);
void free();
/**
* Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix)
*/
bool write(const char* filename) const;
int n; ///< A is m-by-n. n must be >= 0.
int m; ///< A is m-by-n. m must be >= 0.
int* Ap; ///< column pointers for A, of size n+1
int* Aii; ///< row indices of A, of size nz = Ap [n]
//! max number of non-zeros blocks
int nzMax() const { return maxNz;}
protected:
int maxN; ///< size of the allocated memory
int maxNz; ///< size of the allocated memory
};
} // end namespace
#endif

97
Thirdparty/g2o/g2o/core/openmp_mutex.h vendored Normal file
View File

@@ -0,0 +1,97 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OPENMP_MUTEX
#define G2O_OPENMP_MUTEX
#include "../../config.h"
#ifdef G2O_OPENMP
#include <omp.h>
#else
#include <cassert>
#endif
namespace g2o {
#ifdef G2O_OPENMP
/**
* \brief Mutex realized via OpenMP
*/
class OpenMPMutex
{
public:
OpenMPMutex() { omp_init_lock(&_lock); }
~OpenMPMutex() { omp_destroy_lock(&_lock); }
void lock() { omp_set_lock(&_lock); }
void unlock() { omp_unset_lock(&_lock); }
protected:
omp_lock_t _lock;
};
#else
/*
* dummy which does nothing in case we don't have OpenMP support.
* In debug mode, the mutex allows to verify the correct lock and unlock behavior
*/
class OpenMPMutex
{
public:
#ifdef NDEBUG
OpenMPMutex() {}
#else
OpenMPMutex() : _cnt(0) {}
#endif
~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");}
void lock() { assert(++_cnt == 1 && "Locking already locked mutex");}
void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");}
protected:
#ifndef NDEBUG
char _cnt;
#endif
};
#endif
/**
* \brief lock a mutex within a scope
*/
class ScopedOpenMPMutex
{
public:
explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); }
~ScopedOpenMPMutex() { _mutex->unlock(); }
private:
OpenMPMutex* const _mutex;
ScopedOpenMPMutex(const ScopedOpenMPMutex&);
void operator=(const ScopedOpenMPMutex&);
};
}
#endif

910
Thirdparty/g2o/g2o/core/optimizable_graph.cpp vendored Normal file
View File

@@ -0,0 +1,910 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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 "optimizable_graph.h"
#include <cassert>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <algorithm>
#include <Eigen/Dense>
#include "estimate_propagator.h"
#include "factory.h"
#include "optimization_algorithm_property.h"
#include "hyper_graph_action.h"
#include "cache.h"
#include "robust_kernel.h"
#include "../stuff/macros.h"
#include "../stuff/color_macros.h"
#include "../stuff/string_tools.h"
#include "../stuff/misc.h"
namespace g2o {
using namespace std;
OptimizableGraph::Data::Data(){
_next = 0;
}
OptimizableGraph::Data::~Data(){
if (_next)
delete _next;
}
OptimizableGraph::Vertex::Vertex() :
HyperGraph::Vertex(),
_graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false),
_colInHessian(-1), _cacheContainer(0)
{
}
CacheContainer* OptimizableGraph::Vertex::cacheContainer(){
if (! _cacheContainer)
_cacheContainer = new CacheContainer(this);
return _cacheContainer;
}
void OptimizableGraph::Vertex::updateCache(){
if (_cacheContainer){
_cacheContainer->setUpdateNeeded();
_cacheContainer->update();
}
}
OptimizableGraph::Vertex::~Vertex()
{
if (_cacheContainer)
delete (_cacheContainer);
if (_userData)
delete _userData;
}
OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const
{
return 0;
}
bool OptimizableGraph::Vertex::setEstimateData(const double* v)
{
bool ret = setEstimateDataImpl(v);
updateCache();
return ret;
}
bool OptimizableGraph::Vertex::getEstimateData(double *) const
{
return false;
}
int OptimizableGraph::Vertex::estimateDimension() const
{
return -1;
}
bool OptimizableGraph::Vertex::setMinimalEstimateData(const double* v)
{
bool ret = setMinimalEstimateDataImpl(v);
updateCache();
return ret;
}
bool OptimizableGraph::Vertex::getMinimalEstimateData(double *) const
{
return false;
}
int OptimizableGraph::Vertex::minimalEstimateDimension() const
{
return -1;
}
OptimizableGraph::Edge::Edge() :
HyperGraph::Edge(),
_dimension(-1), _level(0), _robustKernel(0)
{
}
OptimizableGraph::Edge::~Edge()
{
delete _robustKernel;
}
OptimizableGraph* OptimizableGraph::Edge::graph(){
if (! _vertices.size())
return 0;
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)_vertices[0];
if (!v)
return 0;
return v->graph();
}
const OptimizableGraph* OptimizableGraph::Edge::graph() const{
if (! _vertices.size())
return 0;
const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) _vertices[0];
if (!v)
return 0;
return v->graph();
}
bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){
if ((int)_parameters.size()<=argNum)
return false;
if (argNum<0)
return false;
*_parameters[argNum] = 0;
_parameterIds[argNum] = paramId;
return true;
}
bool OptimizableGraph::Edge::resolveParameters() {
if (!graph()) {
cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl;
return false;
}
assert (_parameters.size() == _parameterIds.size());
//cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl;
for (size_t i=0; i<_parameters.size(); i++){
int index = _parameterIds[i];
*_parameters[i] = graph()->parameter(index);
if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){
cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << endl;
}
if (!*_parameters[i]) {
cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << endl;
return false;
}
}
return true;
}
void OptimizableGraph::Edge::setRobustKernel(RobustKernel* ptr)
{
if (_robustKernel)
delete _robustKernel;
_robustKernel = ptr;
}
bool OptimizableGraph::Edge::resolveCaches() {
return true;
}
bool OptimizableGraph::Edge::setMeasurementData(const double *)
{
return false;
}
bool OptimizableGraph::Edge::getMeasurementData(double *) const
{
return false;
}
int OptimizableGraph::Edge::measurementDimension() const
{
return -1;
}
bool OptimizableGraph::Edge::setMeasurementFromState(){
return false;
}
OptimizableGraph::Edge* OptimizableGraph::Edge::clone() const
{
// TODO
return 0;
}
OptimizableGraph::OptimizableGraph()
{
_nextEdgeId = 0; _edge_has_id = false;
_graphActions.resize(AT_NUM_ELEMENTS);
}
OptimizableGraph::~OptimizableGraph()
{
clear();
clearParameters();
}
bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData)
{
Vertex* inserted = vertex(v->id());
if (inserted) {
cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl;
assert(0 && "Vertex with this ID already contained in the graph");
return false;
}
OptimizableGraph::Vertex* ov=dynamic_cast<OptimizableGraph::Vertex*>(v);
assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex");
if (ov->_graph != 0 && ov->_graph != this) {
cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl;
assert(0 && "Vertex already registered with another graph");
return false;
}
if (userData)
ov->setUserData(userData);
ov->_graph=this;
return HyperGraph::addVertex(v);
}
bool OptimizableGraph::addEdge(HyperGraph::Edge* e_)
{
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(e_);
assert(e && "Edge does not inherit from OptimizableGraph::Edge");
if (! e)
return false;
bool eresult = HyperGraph::addEdge(e);
if (! eresult)
return false;
e->_internalId = _nextEdgeId++;
if (! e->resolveParameters()){
cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl;
return false;
}
if (! e->resolveCaches()){
cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl;
return false;
}
_jacobianWorkspace.updateSize(e);
return true;
}
int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;}
double OptimizableGraph::chi2() const
{
double chi = 0.0;
for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); ++it) {
const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
chi += e->chi2();
}
return chi;
}
void OptimizableGraph::push()
{
for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
v->push();
}
}
void OptimizableGraph::pop()
{
for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
v->pop();
}
}
void OptimizableGraph::discardTop()
{
for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
v->discardTop();
}
}
void OptimizableGraph::push(HyperGraph::VertexSet& vset)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->push();
}
}
void OptimizableGraph::pop(HyperGraph::VertexSet& vset)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->pop();
}
}
void OptimizableGraph::discardTop(HyperGraph::VertexSet& vset)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->discardTop();
}
}
void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->setFixed(fixed);
}
}
bool OptimizableGraph::load(istream& is, bool createEdges)
{
// scna for the paramers in the whole file
if (!_parameters.read(is,&_renamedTypesLookup))
return false;
#ifndef NDEBUG
cerr << "Loaded " << _parameters.size() << " parameters" << endl;
#endif
is.clear();
is.seekg(ios_base::beg);
set<string> warnedUnknownTypes;
stringstream currentLine;
string token;
Factory* factory = Factory::instance();
HyperGraph::GraphElemBitset elemBitset;
elemBitset[HyperGraph::HGET_PARAMETER] = 1;
elemBitset.flip();
Vertex* previousVertex = 0;
Data* previousData = 0;
while (1) {
int bytesRead = readLine(is, currentLine);
if (bytesRead == -1)
break;
currentLine >> token;
//cerr << "Token=" << token << endl;
if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
continue;
// handle commands encoded in the file
bool handledCommand = false;
if (token == "FIX") {
handledCommand = true;
int id;
while (currentLine >> id) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(vertex(id));
if (v) {
# ifndef NDEBUG
cerr << "Fixing vertex " << v->id() << endl;
# endif
v->setFixed(true);
} else {
cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl;
}
}
}
if (handledCommand)
continue;
// do the mapping to an internal type if it matches
if (_renamedTypesLookup.size() > 0) {
map<string, string>::const_iterator foundIt = _renamedTypesLookup.find(token);
if (foundIt != _renamedTypesLookup.end()) {
token = foundIt->second;
}
}
if (! factory->knowsTag(token)) {
if (warnedUnknownTypes.count(token) != 1) {
warnedUnknownTypes.insert(token);
cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl;
}
continue;
}
HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
if (dynamic_cast<Vertex*>(element)) { // it's a vertex type
//cerr << "it is a vertex" << endl;
previousData = 0;
Vertex* v = static_cast<Vertex*>(element);
int id;
currentLine >> id;
bool r = v->read(currentLine);
if (! r)
cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl;
v->setId(id);
if (!addVertex(v)) {
cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl;
delete v;
} else {
previousVertex = v;
}
}
else if (dynamic_cast<Edge*>(element)) {
//cerr << "it is an edge" << endl;
previousData = 0;
Edge* e = static_cast<Edge*>(element);
int numV = e->vertices().size();
if (_edge_has_id){
int id;
currentLine >> id;
e->setId(id);
}
//cerr << PVAR(token) << " " << PVAR(numV) << endl;
if (numV == 2) { // it's a pairwise / binary edge type which we handle in a special way
int id1, id2;
currentLine >> id1 >> id2;
Vertex* from = vertex(id1);
Vertex* to = vertex(id2);
int doInit=0;
if ((!from || !to) ) {
if (! createEdges) {
cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token << " " << id1 << " <-> " << id2 << endl;
delete e;
} else {
if (! from) {
from=e->createFrom();
from->setId(id1);
addVertex(from);
doInit=2;
}
if (! to) {
to=e->createTo();
to->setId(id2);
addVertex(to);
doInit=1;
}
}
}
if (from && to) {
e->setVertex(0, from);
e->setVertex(1, to);
e->read(currentLine);
if (!addEdge(e)) {
cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token << " " << id1 << " <-> " << id2 << endl;
delete e;
} else {
switch (doInit){
case 1:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
e->initialEstimate(fromSet, to);
break;
}
case 2:
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
e->initialEstimate(toSet, from);
break;
}
default:;
}
}
}
}
else {
vector<int> ids;
ids.resize(numV);
for (int l = 0; l < numV; ++l)
currentLine >> ids[l];
bool vertsOkay = true;
for (int l = 0; l < numV; ++l) {
e->setVertex(l, vertex(ids[l]));
if (e->vertex(l) == 0) {
vertsOkay = false;
break;
}
}
if (! vertsOkay) {
cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token;
for (int l = 0; l < numV; ++l) {
if (l > 0)
cerr << " <->";
cerr << " " << ids[l];
}
delete e;
} else {
bool r = e->read(currentLine);
if (!r || !addEdge(e)) {
cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token;
for (int l = 0; l < numV; ++l) {
if (l > 0)
cerr << " <->";
cerr << " " << ids[l];
}
delete e;
}
}
}
} else if (dynamic_cast<Data*>(element)) { // reading in the data packet for the vertex
//cerr << "read data packet " << token << " vertex " << previousVertex->id() << endl;
Data* d = static_cast<Data*>(element);
bool r = d->read(currentLine);
if (! r) {
cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for vertex " << previousVertex->id() << endl;
delete d;
previousData = 0;
} else if (previousData){
//cerr << "chaining" << endl;
previousData->setNext(d);
previousData = d;
//cerr << "done" << endl;
} else if (previousVertex){
//cerr << "embedding in vertex" << endl;
previousVertex->setUserData(d);
previousData = d;
previousVertex = 0;
//cerr << "done" << endl;
} else {
cerr << __PRETTY_FUNCTION__ << ": got data element, but no vertex available" << endl;
delete d;
previousData = 0;
}
}
} // while read line
return true;
}
bool OptimizableGraph::load(const char* filename, bool createEdges)
{
ifstream ifs(filename);
if (!ifs) {
cerr << __PRETTY_FUNCTION__ << " unable to open file " << filename << endl;
return false;
}
return load(ifs, createEdges);
}
bool OptimizableGraph::save(const char* filename, int level) const
{
ofstream ofs(filename);
if (!ofs)
return false;
return save(ofs, level);
}
bool OptimizableGraph::save(ostream& os, int level) const
{
if (! _parameters.write(os))
return false;
set<Vertex*, VertexIDCompare> verticesToSave;
for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
if (e->level() == level) {
for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
verticesToSave.insert(static_cast<OptimizableGraph::Vertex*>(*it));
}
}
}
for (set<Vertex*, VertexIDCompare>::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){
OptimizableGraph::Vertex* v = *it;
saveVertex(os, v);
}
EdgeContainer edgesToSave;
for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
const OptimizableGraph::Edge* e = dynamic_cast<const OptimizableGraph::Edge*>(*it);
if (e->level() == level)
edgesToSave.push_back(const_cast<Edge*>(e));
}
sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare());
for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) {
OptimizableGraph::Edge* e = *it;
saveEdge(os, e);
}
return os.good();
}
bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::VertexSet& vset, int level)
{
if (! _parameters.write(os))
return false;
for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
saveVertex(os, v);
}
for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
if (e->level() != level)
continue;
bool verticesInEdge = true;
for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
if (vset.find(*it) == vset.end()) {
verticesInEdge = false;
break;
}
}
if (! verticesInEdge)
continue;
saveEdge(os, e);
}
return os.good();
}
bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset)
{
if (!_parameters.write(os))
return false;
std::set<OptimizableGraph::Vertex*> vset;
for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
HyperGraph::Edge* e = *it;
for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
vset.insert(v);
}
}
for (std::set<OptimizableGraph::Vertex*>::const_iterator it=vset.begin(); it!=vset.end(); ++it){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
saveVertex(os, v);
}
for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
saveEdge(os, e);
}
return os.good();
}
void OptimizableGraph::addGraph(OptimizableGraph* g){
for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second);
if (vertex(v->id()))
continue;
OptimizableGraph::Vertex* v2=v->clone();
v2->edges().clear();
v2->setHessianIndex(-1);
addVertex(v2);
}
for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){
OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it);
OptimizableGraph::Edge* en = e->clone();
en->resize(e->vertices().size());
int cnt = 0;
for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) vertex((*it)->id());
assert(v);
en->setVertex(cnt++, v);
}
addEdge(en);
}
}
int OptimizableGraph::maxDimension() const{
int maxDim=0;
for (HyperGraph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); ++it){
const OptimizableGraph::Vertex* v= static_cast< const OptimizableGraph::Vertex*>(it->second);
maxDim = (std::max)(maxDim, v->dimension());
}
return maxDim;
}
void OptimizableGraph::setRenamedTypesFromString(const std::string& types)
{
Factory* factory = Factory::instance();
vector<string> typesMap = strSplit(types, ",");
for (size_t i = 0; i < typesMap.size(); ++i) {
vector<string> m = strSplit(typesMap[i], "=");
if (m.size() != 2) {
cerr << __PRETTY_FUNCTION__ << ": unable to extract type map from " << typesMap[i] << endl;
continue;
}
string typeInFile = trim(m[0]);
string loadedType = trim(m[1]);
if (! factory->knowsTag(loadedType)) {
cerr << __PRETTY_FUNCTION__ << ": unknown type " << loadedType << endl;
continue;
}
_renamedTypesLookup[typeInFile] = loadedType;
}
cerr << "# load look up table" << endl;
for (std::map<std::string, std::string>::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) {
cerr << "#\t" << it->first << " -> " << it->second << endl;
}
}
bool OptimizableGraph::isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims_) const
{
std::set<int> auxDims;
if (vertDims_.size() == 0) {
auxDims = dimensions();
}
const set<int>& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;
bool suitableSolver = true;
if (vertDims.size() == 2) {
if (solverProperty.requiresMarginalize) {
suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1;
}
else {
suitableSolver = solverProperty.poseDim == -1;
}
} else if (vertDims.size() == 1) {
suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1;
} else {
suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize;
}
return suitableSolver;
}
std::set<int> OptimizableGraph::dimensions() const
{
std::set<int> auxDims;
for (VertexIDMap::const_iterator it = vertices().begin(); it != vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
auxDims.insert(v->dimension());
}
return auxDims;
}
void OptimizableGraph::preIteration(int iter)
{
HyperGraphActionSet& actions = _graphActions[AT_PREITERATION];
if (actions.size() > 0) {
HyperGraphAction::ParametersIteration params(iter);
for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
(*(*it))(this, &params);
}
}
}
void OptimizableGraph::postIteration(int iter)
{
HyperGraphActionSet& actions = _graphActions[AT_POSTITERATION];
if (actions.size() > 0) {
HyperGraphAction::ParametersIteration params(iter);
for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
(*(*it))(this, &params);
}
}
}
bool OptimizableGraph::addPostIterationAction(HyperGraphAction* action)
{
std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_POSTITERATION].insert(action);
return insertResult.second;
}
bool OptimizableGraph::addPreIterationAction(HyperGraphAction* action)
{
std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_PREITERATION].insert(action);
return insertResult.second;
}
bool OptimizableGraph::removePreIterationAction(HyperGraphAction* action)
{
return _graphActions[AT_PREITERATION].erase(action) > 0;
}
bool OptimizableGraph::removePostIterationAction(HyperGraphAction* action)
{
return _graphActions[AT_POSTITERATION].erase(action) > 0;
}
bool OptimizableGraph::saveVertex(std::ostream& os, OptimizableGraph::Vertex* v) const
{
Factory* factory = Factory::instance();
string tag = factory->tag(v);
if (tag.size() > 0) {
os << tag << " " << v->id() << " ";
v->write(os);
os << endl;
Data* d=v->userData();
while (d) { // write the data packet for the vertex
tag = factory->tag(d);
if (tag.size() > 0) {
os << tag << " ";
d->write(os);
os << endl;
}
d=d->next();
}
if (v->fixed()) {
os << "FIX " << v->id() << endl;
}
return os.good();
}
return false;
}
bool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) const
{
Factory* factory = Factory::instance();
string tag = factory->tag(e);
if (tag.size() > 0) {
os << tag << " ";
if (_edge_has_id)
os << e->id() << " ";
for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
os << v->id() << " ";
}
e->write(os);
os << endl;
return os.good();
}
return false;
}
void OptimizableGraph::clearParameters()
{
_parameters.clear();
}
bool OptimizableGraph::verifyInformationMatrices(bool verbose) const
{
bool allEdgeOk = true;
SelfAdjointEigenSolver<MatrixXd> eigenSolver;
for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension());
// test on symmetry
bool isSymmetric = information.transpose() == information;
bool okay = isSymmetric;
if (isSymmetric) {
// compute the eigenvalues
eigenSolver.compute(information, Eigen::EigenvaluesOnly);
bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;
okay = okay && isSPD;
}
allEdgeOk = allEdgeOk && okay;
if (! okay) {
if (verbose) {
if (! isSymmetric)
cerr << "Information Matrix for an edge is not symmetric:";
else
cerr << "Information Matrix for an edge is not SPD:";
for (size_t i = 0; i < e->vertices().size(); ++i)
cerr << " " << e->vertex(i)->id();
if (isSymmetric)
cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose();
cerr << endl;
}
}
}
return allEdgeOk;
}
bool OptimizableGraph::initMultiThreading()
{
# if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0)
Eigen::initParallel();
# endif
return true;
}
} // end namespace

688
Thirdparty/g2o/g2o/core/optimizable_graph.h vendored Normal file
View File

@@ -0,0 +1,688 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// 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.
//
// 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 G2O_AIS_OPTIMIZABLE_GRAPH_HH_
#define G2O_AIS_OPTIMIZABLE_GRAPH_HH_
#include <set>
#include <iostream>
#include <list>
#include <limits>
#include <cmath>
#include <typeinfo>
#include "openmp_mutex.h"
#include "hyper_graph.h"
#include "parameter.h"
#include "parameter_container.h"
#include "jacobian_workspace.h"
#include "../stuff/macros.h"
namespace g2o {
class HyperGraphAction;
struct OptimizationAlgorithmProperty;
class Cache;
class CacheContainer;
class RobustKernel;
/**
@addtogroup g2o
*/
/**
This is an abstract class that represents one optimization
problem. It specializes the general graph to contain special
vertices and edges. The vertices represent parameters that can
be optimized, while the edges represent constraints. This class
also provides basic functionalities to handle the backup/restore
of portions of the vertices.
*/
struct OptimizableGraph : public HyperGraph {
enum ActionType {
AT_PREITERATION, AT_POSTITERATION,
AT_NUM_ELEMENTS, // keep as last element
};
typedef std::set<HyperGraphAction*> HyperGraphActionSet;
// forward declarations
class Vertex;
class Edge;
/**
* \brief data packet for a vertex. Extend this class to store in the vertices
* the potential additional information you need (e.g. images, laser scans, ...).
*/
class Data : public HyperGraph::HyperGraphElement
{
friend struct OptimizableGraph;
public:
virtual ~Data();
Data();
//! read the data from a stream
virtual bool read(std::istream& is) = 0;
//! write the data to a stream
virtual bool write(std::ostream& os) const = 0;
virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;}
const Data* next() const {return _next;}
Data* next() {return _next;}
void setNext(Data* next_) { _next = next_; }
protected:
Data* _next; // linked list of multiple data;
};
/**
* \brief order vertices based on their ID
*/
struct VertexIDCompare {
bool operator() (const Vertex* v1, const Vertex* v2) const
{
return v1->id() < v2->id();
}
};
/**
* \brief order edges based on the internal ID, which is assigned to the edge in addEdge()
*/
struct EdgeIDCompare {
bool operator() (const Edge* e1, const Edge* e2) const
{
return e1->internalId() < e2->internalId();
}
};
//! vector container for vertices
typedef std::vector<OptimizableGraph::Vertex*> VertexContainer;
//! vector container for edges
typedef std::vector<OptimizableGraph::Edge*> EdgeContainer;
/**
* \brief A general case Vertex for optimization
*/
class Vertex : public HyperGraph::Vertex {
private:
friend struct OptimizableGraph;
public:
Vertex();
//! returns a deep copy of the current vertex
virtual Vertex* clone() const ;
//! the user data associated with this vertex
const Data* userData() const { return _userData; }
Data* userData() { return _userData; }
void setUserData(Data* obs) { _userData = obs;}
void addUserData(Data* obs) {
if (obs) {
obs->setNext(_userData);
_userData=obs;
}
}
virtual ~Vertex();
//! sets the node to the origin (used in the multilevel stuff)
void setToOrigin() { setToOriginImpl(); updateCache();}
//! get the element from the hessian matrix
virtual const double& hessian(int i, int j) const = 0;
virtual double& hessian(int i, int j) = 0;
virtual double hessianDeterminant() const = 0;
virtual double* hessianData() = 0;
/** maps the internal matrix to some external memory location */
virtual void mapHessianMemory(double* d) = 0;
/**
* copies the b vector in the array b_
* @return the number of elements copied
*/
virtual int copyB(double* b_) const = 0;
//! get the b vector element
virtual const double& b(int i) const = 0;
virtual double& b(int i) = 0;
//! return a pointer to the b vector associated with this vertex
virtual double* bData() = 0;
/**
* set the b vector part of this vertex to zero
*/
virtual void clearQuadraticForm() = 0;
/**
* updates the current vertex with the direct solution x += H_ii\b_ii
* @return the determinant of the inverted hessian
*/
virtual double solveDirect(double lambda=0) = 0;
/**
* sets the initial estimate from an array of double
* Implement setEstimateDataImpl()
* @return true on success
*/
bool setEstimateData(const double* estimate);
/**
* sets the initial estimate from an array of double
* Implement setEstimateDataImpl()
* @return true on success
*/
bool setEstimateData(const std::vector<double>& estimate) {
#ifndef NDEBUG
int dim = estimateDimension();
assert((dim == -1) || (estimate.size() == std::size_t(dim)));
#endif
return setEstimateData(&estimate[0]);
};
/**
* writes the estimater to an array of double
* @returns true on success
*/
virtual bool getEstimateData(double* estimate) const;
/**
* writes the estimater to an array of double
* @returns true on success
*/
virtual bool getEstimateData(std::vector<double>& estimate) const {
int dim = estimateDimension();
if (dim < 0)
return false;
estimate.resize(dim);
return getEstimateData(&estimate[0]);
};
/**
* returns the dimension of the extended representation used by get/setEstimate(double*)
* -1 if it is not supported
*/
virtual int estimateDimension() const;
/**
* sets the initial estimate from an array of double.
* Implement setMinimalEstimateDataImpl()
* @return true on success
*/
bool setMinimalEstimateData(const double* estimate);
/**
* sets the initial estimate from an array of double.
* Implement setMinimalEstimateDataImpl()
* @return true on success
*/
bool setMinimalEstimateData(const std::vector<double>& estimate) {
#ifndef NDEBUG
int dim = minimalEstimateDimension();
assert((dim == -1) || (estimate.size() == std::size_t(dim)));
#endif
return setMinimalEstimateData(&estimate[0]);
};
/**
* writes the estimate to an array of double
* @returns true on success
*/
virtual bool getMinimalEstimateData(double* estimate) const ;
/**
* writes the estimate to an array of double
* @returns true on success
*/
virtual bool getMinimalEstimateData(std::vector<double>& estimate) const {
int dim = minimalEstimateDimension();
if (dim < 0)
return false;
estimate.resize(dim);
return getMinimalEstimateData(&estimate[0]);
};
/**
* returns the dimension of the extended representation used by get/setEstimate(double*)
* -1 if it is not supported
*/
virtual int minimalEstimateDimension() const;
//! backup the position of the vertex to a stack
virtual void push() = 0;
//! restore the position of the vertex by retrieving the position from the stack
virtual void pop() = 0;
//! pop the last element from the stack, without restoring the current estimate
virtual void discardTop() = 0;
//! return the stack size
virtual int stackSize() const = 0;
/**
* Update the position of the node from the parameters in v.
* Depends on the implementation of oplusImpl in derived classes to actually carry
* out the update.
* Will also call updateCache() to update the caches of depending on the vertex.
*/
void oplus(const double* v)
{
oplusImpl(v);
updateCache();
}
//! temporary index of this node in the parameter vector obtained from linearization
int hessianIndex() const { return _hessianIndex;}
int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex();}
//! set the temporary index of the vertex in the parameter blocks
void setHessianIndex(int ti) { _hessianIndex = ti;}
void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti);}
//! true => this node is fixed during the optimization
bool fixed() const {return _fixed;}
//! true => this node should be considered fixed during the optimization
void setFixed(bool fixed) { _fixed = fixed;}
//! true => this node is marginalized out during the optimization
bool marginalized() const {return _marginalized;}
//! true => this node should be marginalized out during the optimization
void setMarginalized(bool marginalized) { _marginalized = marginalized;}
//! dimension of the estimated state belonging to this node
int dimension() const { return _dimension;}
//! sets the id of the node in the graph be sure that the graph keeps consistent after changing the id
virtual void setId(int id) {_id = id;}
//! set the row of this vertex in the Hessian
void setColInHessian(int c) { _colInHessian = c;}
//! get the row of this vertex in the Hessian
int colInHessian() const {return _colInHessian;}
const OptimizableGraph* graph() const {return _graph;}
OptimizableGraph* graph() {return _graph;}
/**
* lock for the block of the hessian and the b vector associated with this vertex, to avoid
* race-conditions if multi-threaded.
*/
void lockQuadraticForm() { _quadraticFormMutex.lock();}
/**
* unlock the block of the hessian and the b vector associated with this vertex
*/
void unlockQuadraticForm() { _quadraticFormMutex.unlock();}
//! read the vertex from a stream, i.e., the internal state of the vertex
virtual bool read(std::istream& is) = 0;
//! write the vertex to a stream
virtual bool write(std::ostream& os) const = 0;
virtual void updateCache();
CacheContainer* cacheContainer();
protected:
OptimizableGraph* _graph;
Data* _userData;
int _hessianIndex;
bool _fixed;
bool _marginalized;
int _dimension;
int _colInHessian;
OpenMPMutex _quadraticFormMutex;
CacheContainer* _cacheContainer;
/**
* update the position of the node from the parameters in v.
* Implement in your class!
*/
virtual void oplusImpl(const double* v) = 0;
//! sets the node to the origin (used in the multilevel stuff)
virtual void setToOriginImpl() = 0;
/**
* writes the estimater to an array of double
* @returns true on success
*/
virtual bool setEstimateDataImpl(const double* ) { return false;}
/**
* sets the initial estimate from an array of double
* @return true on success
*/
virtual bool setMinimalEstimateDataImpl(const double* ) { return false;}
};
class Edge: public HyperGraph::Edge {
private:
friend struct OptimizableGraph;
public:
Edge();
virtual ~Edge();
virtual Edge* clone() const;
// indicates if all vertices are fixed
virtual bool allVerticesFixed() const = 0;
// computes the error of the edge and stores it in an internal structure
virtual void computeError() = 0;
//! sets the measurement from an array of double
//! @returns true on success
virtual bool setMeasurementData(const double* m);
//! writes the measurement to an array of double
//! @returns true on success
virtual bool getMeasurementData(double* m) const;
//! returns the dimension of the measurement in the extended representation which is used
//! by get/setMeasurement;
virtual int measurementDimension() const;
/**
* sets the estimate to have a zero error, based on the current value of the state variables
* returns false if not supported.
*/
virtual bool setMeasurementFromState();
//! if NOT NULL, error of this edge will be robustifed with the kernel
RobustKernel* robustKernel() const { return _robustKernel;}
/**
* specify the robust kernel to be used in this edge
*/
void setRobustKernel(RobustKernel* ptr);
//! returns the error vector cached after calling the computeError;
virtual const double* errorData() const = 0;
virtual double* errorData() = 0;
//! returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd>
virtual const double* informationData() const = 0;
virtual double* informationData() = 0;
//! computes the chi2 based on the cached error value, only valid after computeError has been called.
virtual double chi2() const = 0;
/**
* Linearizes the constraint in the edge.
* Makes side effect on the vertices of the graph by changing
* the parameter vector b and the hessian blocks ii and jj.
* The off diagoinal block is accesed via _hessian.
*/
virtual void constructQuadraticForm() = 0;
/**
* maps the internal matrix to some external memory location,
* you need to provide the memory before calling constructQuadraticForm
* @param d the memory location to which we map
* @param i index of the vertex i
* @param j index of the vertex j (j > i, upper triangular fashion)
* @param rowMajor if true, will write in rowMajor order to the block. Since EIGEN is columnMajor by default, this results in writing the transposed
*/
virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0;
/**
* Linearizes the constraint in the edge in the manifold space, and store
* the result in the given workspace
*/
virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0;
/** set the estimate of the to vertex, based on the estimate of the from vertices in the edge. */
virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) = 0;
/**
* override in your class if it's possible to initialize the vertices in certain combinations.
* The return value may correspond to the cost for initiliaizng the vertex but should be positive if
* the initialization is possible and negative if not possible.
*/
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) from; (void) to; return -1.;}
//! returns the level of the edge
int level() const { return _level;}
//! sets the level of the edge
void setLevel(int l) { _level=l;}
//! returns the dimensions of the error function
int dimension() const { return _dimension;}
virtual Vertex* createFrom() {return 0;}
virtual Vertex* createTo() {return 0;}
//! read the vertex from a stream, i.e., the internal state of the vertex
virtual bool read(std::istream& is) = 0;
//! write the vertex to a stream
virtual bool write(std::ostream& os) const = 0;
//! the internal ID of the edge
long long internalId() const { return _internalId;}
OptimizableGraph* graph();
const OptimizableGraph* graph() const;
bool setParameterId(int argNum, int paramId);
inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);}
inline size_t numParameters() const {return _parameters.size();}
inline void resizeParameters(size_t newSize) {
_parameters.resize(newSize, 0);
_parameterIds.resize(newSize, -1);
_parameterTypes.resize(newSize, typeid(void*).name());
}
protected:
int _dimension;
int _level;
RobustKernel* _robustKernel;
long long _internalId;
std::vector<int> _cacheIds;
template <typename ParameterType>
bool installParameter(ParameterType*& p, size_t argNo, int paramId=-1){
if (argNo>=_parameters.size())
return false;
_parameterIds[argNo] = paramId;
_parameters[argNo] = (Parameter**)&p;
_parameterTypes[argNo] = typeid(ParameterType).name();
return true;
}
template <typename CacheType>
void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*,
const std::string& _type,
const ParameterVector& parameters);
bool resolveParameters();
virtual bool resolveCaches();
std::vector<std::string> _parameterTypes;
std::vector<Parameter**> _parameters;
std::vector<int> _parameterIds;
};
//! returns the vertex number <i>id</i> appropriately casted
inline Vertex* vertex(int id) { return reinterpret_cast<Vertex*>(HyperGraph::vertex(id));}
//! returns the vertex number <i>id</i> appropriately casted
inline const Vertex* vertex (int id) const{ return reinterpret_cast<const Vertex*>(HyperGraph::vertex(id));}
//! empty constructor
OptimizableGraph();
virtual ~OptimizableGraph();
//! adds all edges and vertices of the graph <i>g</i> to this graph.
void addGraph(OptimizableGraph* g);
/**
* adds a new vertex. The new vertex is then "taken".
* @return false if a vertex with the same id as v is already in the graph, true otherwise.
*/
virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);
virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);}
/**
* adds a new edge.
* The edge should point to the vertices that it is connecting (setFrom/setTo).
* @return false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise.
*/
virtual bool addEdge(HyperGraph::Edge* e);
//! returns the chi2 of the current configuration
double chi2() const;
//! return the maximum dimension of all vertices in the graph
int maxDimension() const;
/**
* iterates over all vertices and returns a set of all the vertex dimensions in the graph
*/
std::set<int> dimensions() const;
/**
* carry out n iterations
* @return the number of performed iterations
*/
virtual int optimize(int iterations, bool online=false);
//! called at the beginning of an iteration (argument is the number of the iteration)
virtual void preIteration(int);
//! called at the end of an iteration (argument is the number of the iteration)
virtual void postIteration(int);
//! add an action to be executed before each iteration
bool addPreIterationAction(HyperGraphAction* action);
//! add an action to be executed after each iteration
bool addPostIterationAction(HyperGraphAction* action);
//! remove an action that should no longer be execured before each iteration
bool removePreIterationAction(HyperGraphAction* action);
//! remove an action that should no longer be execured after each iteration
bool removePostIterationAction(HyperGraphAction* action);
//! push the estimate of all variables onto a stack
virtual void push();
//! pop (restore) the estimate of all variables from the stack
virtual void pop();
//! discard the last backup of the estimate for all variables by removing it from the stack
virtual void discardTop();
//! load the graph from a stream. Uses the Factory singleton for creating the vertices and edges.
virtual bool load(std::istream& is, bool createEdges=true);
bool load(const char* filename, bool createEdges=true);
//! save the graph to a stream. Again uses the Factory system.
virtual bool save(std::ostream& os, int level = 0) const;
//! function provided for convenience, see save() above
bool save(const char* filename, int level = 0) const;
//! save a subgraph to a stream. Again uses the Factory system.
bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0);
//! save a subgraph to a stream. Again uses the Factory system.
bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset);
//! push the estimate of a subset of the variables onto a stack
virtual void push(HyperGraph::VertexSet& vset);
//! pop (restore) the estimate a subset of the variables from the stack
virtual void pop(HyperGraph::VertexSet& vset);
//! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate
virtual void discardTop(HyperGraph::VertexSet& vset);
//! fixes/releases a set of vertices
virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed);
/**
* set the renamed types lookup from a string, format is for example:
* VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP
* This will change the occurance of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP
*/
void setRenamedTypesFromString(const std::string& types);
/**
* test whether a solver is suitable for optimizing this graph.
* @param solverProperty the solver property to evaluate.
* @param vertDims should equal to the set returned by dimensions() to avoid re-evaluating.
*/
bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims = std::set<int>()) const;
/**
* remove the parameters of the graph
*/
virtual void clearParameters();
bool addParameter(Parameter* p) {
return _parameters.addParameter(p);
}
Parameter* parameter(int id) {
return _parameters.getParameter(id);
}
/**
* verify that all the information of the edges are semi positive definite, i.e.,
* all Eigenvalues are >= 0.
* @param verbose output edges with not PSD information matrix on cerr
* @return true if all edges have PSD information matrix
*/
bool verifyInformationMatrices(bool verbose = false) const;
// helper functions to save an individual vertex
bool saveVertex(std::ostream& os, Vertex* v) const;
// helper functions to save an individual edge
bool saveEdge(std::ostream& os, Edge* e) const;
//! the workspace for storing the Jacobians of the graph
JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace;}
const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace;}
/**
* Eigen starting from version 3.1 requires that we call an initialize
* function, if we perform calls to Eigen from several threads.
* Currently, this function calls Eigen::initParallel if g2o is compiled
* with OpenMP support and Eigen's version is at least 3.1
*/
static bool initMultiThreading();
protected:
std::map<std::string, std::string> _renamedTypesLookup;
long long _nextEdgeId;
std::vector<HyperGraphActionSet> _graphActions;
// do not watch this. To be removed soon, or integrated in a nice way
bool _edge_has_id;
ParameterContainer _parameters;
JacobianWorkspace _jacobianWorkspace;
};
/**
@}
*/
} // end namespace
#endif

62
Thirdparty/g2o/g2o/core/optimization_algorithm.cpp vendored Normal file
View File

@@ -0,0 +1,62 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "optimization_algorithm.h"
using namespace std;
namespace g2o {
OptimizationAlgorithm::OptimizationAlgorithm() :
_optimizer(0)
{
}
OptimizationAlgorithm::~OptimizationAlgorithm()
{
}
void OptimizationAlgorithm::printProperties(std::ostream& os) const
{
os << "------------- Algorithm Properties -------------" << endl;
for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) {
BaseProperty* p = it->second;
os << it->first << "\t" << p->toString() << endl;
}
os << "------------------------------------------------" << endl;
}
bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString)
{
return _properties.updateMapFromString(propString);
}
void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer)
{
_optimizer = optimizer;
}
} // end namespace

117
Thirdparty/g2o/g2o/core/optimization_algorithm.h vendored Normal file
View File

@@ -0,0 +1,117 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OPTIMIZATION_ALGORITHM_H
#define G2O_OPTIMIZATION_ALGORITHM_H
#include <vector>
#include <utility>
#include <iosfwd>
#include "../stuff/property.h"
#include "hyper_graph.h"
#include "sparse_block_matrix.h"
namespace g2o {
class SparseOptimizer;
/**
* \brief Generic interface for a non-linear solver operating on a graph
*/
class OptimizationAlgorithm
{
public:
enum SolverResult {Terminate=2, OK=1, Fail=-1};
OptimizationAlgorithm();
virtual ~OptimizationAlgorithm();
/**
* initialize the solver, called once before the first call to solve()
*/
virtual bool init(bool online = false) = 0;
/**
* Solve one iteration. The SparseOptimizer running on-top will call this
* for the given number of iterations.
* @param iteration indicates the current iteration
*/
virtual SolverResult solve(int iteration, bool online = false) = 0;
/**
* computes the block diagonal elements of the pattern specified in the input
* and stores them in given SparseBlockMatrix.
* If your solver does not support computing the marginals, return false.
*/
virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices) = 0;
/**
* update the structures for online processing
*/
virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges) = 0;
/**
* called by the optimizer if verbose. re-implement, if you want to print something
*/
virtual void printVerbose(std::ostream& os) const {(void) os;};
public:
//! return the optimizer operating on
const SparseOptimizer* optimizer() const { return _optimizer;}
SparseOptimizer* optimizer() { return _optimizer;}
/**
* specify on which optimizer the solver should work on
*/
void setOptimizer(SparseOptimizer* optimizer);
//! return the properties of the solver
const PropertyMap& properties() const { return _properties;}
/**
* update the properties from a string, see PropertyMap::updateMapFromString()
*/
bool updatePropertiesFromString(const std::string& propString);
/**
* print the properties to a stream in a human readable fashion
*/
void printProperties(std::ostream& os) const;
protected:
SparseOptimizer* _optimizer; ///< the optimizer the solver is working on
PropertyMap _properties; ///< the properties of your solver, use this to store the parameters of your solver
private:
// Disable the copy constructor and assignment operator
OptimizationAlgorithm(const OptimizationAlgorithm&) { }
OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; }
};
} // end namespace
#endif

229
Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp vendored Normal file
View File

@@ -0,0 +1,229 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "optimization_algorithm_dogleg.h"
#include <iostream>
#include "../stuff/timeutil.h"
#include "block_solver.h"
#include "sparse_optimizer.h"
#include "solver.h"
#include "batch_stats.h"
using namespace std;
namespace g2o {
OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(BlockSolverBase* solver) :
OptimizationAlgorithmWithHessian(solver)
{
_userDeltaInit = _properties.makeProperty<Property<double> >("initialDelta", 1e4);
_maxTrialsAfterFailure = _properties.makeProperty<Property<int> >("maxTrialsAfterFailure", 100);
_initialLambda = _properties.makeProperty<Property<double> >("initialLambda", 1e-7);
_lamdbaFactor = _properties.makeProperty<Property<double> >("lambdaFactor", 10.);
_delta = _userDeltaInit->value();
_lastStep = STEP_UNDEFINED;
_wasPDInAllIterations = true;
}
OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg()
{
}
OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
assert(dynamic_cast<BlockSolverBase*>(_solver) && "underlying linear solver is not a block solver");
BlockSolverBase* blockSolver = static_cast<BlockSolverBase*>(_solver);
if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
bool ok = _solver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
// init some members to the current size of the problem
_hsd.resize(_solver->vectorSize());
_hdl.resize(_solver->vectorSize());
_auxVector.resize(_solver->vectorSize());
_delta = _userDeltaInit->value();
_currentLambda = _initialLambda->value();
_wasPDInAllIterations = true;
}
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
t=get_monotonic_time();
}
double currentChi = _optimizer->activeRobustChi2();
_solver->buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
}
Eigen::VectorXd::ConstMapType b(_solver->b(), _solver->vectorSize());
// compute alpha
_auxVector.setZero();
blockSolver->multiplyHessian(_auxVector.data(), _solver->b());
double bNormSquared = b.squaredNorm();
double alpha = bNormSquared / _auxVector.dot(b);
_hsd = alpha * b;
double hsdNorm = _hsd.norm();
double hgnNorm = -1.;
bool solvedGaussNewton = false;
bool goodStep = false;
int& numTries = _lastNumTries;
numTries = 0;
do {
++numTries;
if (! solvedGaussNewton) {
const double minLambda = 1e-12;
const double maxLambda = 1e3;
solvedGaussNewton = true;
// apply a damping factor to enforce positive definite Hessian, if the matrix appeared
// to be not positive definite in at least one iteration before.
// We apply a damping factor to obtain a PD matrix.
bool solverOk = false;
while(!solverOk) {
if (! _wasPDInAllIterations)
_solver->setLambda(_currentLambda, true); // add _currentLambda to the diagonal
solverOk = _solver->solve();
if (! _wasPDInAllIterations)
_solver->restoreDiagonal();
_wasPDInAllIterations = _wasPDInAllIterations && solverOk;
if (! _wasPDInAllIterations) {
// simple strategy to control the damping factor
if (solverOk) {
_currentLambda = std::max(minLambda, _currentLambda / (0.5 * _lamdbaFactor->value()));
} else {
_currentLambda *= _lamdbaFactor->value();
if (_currentLambda > maxLambda) {
_currentLambda = maxLambda;
return Fail;
}
}
}
}
if (!solverOk) {
return Fail;
}
hgnNorm = Eigen::VectorXd::ConstMapType(_solver->x(), _solver->vectorSize()).norm();
}
Eigen::VectorXd::ConstMapType hgn(_solver->x(), _solver->vectorSize());
assert(hgnNorm >= 0. && "Norm of the GN step is not computed");
if (hgnNorm < _delta) {
_hdl = hgn;
_lastStep = STEP_GN;
}
else if (hsdNorm > _delta) {
_hdl = _delta / hsdNorm * _hsd;
_lastStep = STEP_SD;
} else {
_auxVector = hgn - _hsd; // b - a
double c = _hsd.dot(_auxVector);
double bmaSquaredNorm = _auxVector.squaredNorm();
double beta;
if (c <= 0.)
beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm;
else {
double hsdSqrNorm = _hsd.squaredNorm();
beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm)));
}
assert(beta > 0. && beta < 1 && "Error while computing beta");
_hdl = _hsd + beta * (hgn - _hsd);
_lastStep = STEP_DL;
assert(_hdl.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region");
}
// compute the linear gain
_auxVector.setZero();
blockSolver->multiplyHessian(_auxVector.data(), _hdl.data());
double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl));
// apply the update and see what happens
_optimizer->push();
_optimizer->update(_hdl.data());
_optimizer->computeActiveErrors();
double newChi = _optimizer-> activeRobustChi2();
double nonLinearGain = currentChi - newChi;
if (fabs(linearGain) < 1e-12)
linearGain = 1e-12;
double rho = nonLinearGain / linearGain;
//cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " << PVAR(rho) << endl;
if (rho > 0) { // step is good and will be accepted
_optimizer->discardTop();
goodStep = true;
} else { // recover previous state
_optimizer->pop();
}
// update trust region based on the step quality
if (rho > 0.75)
_delta = std::max(_delta, 3. * _hdl.norm());
else if (rho < 0.25)
_delta *= 0.5;
} while (!goodStep && numTries < _maxTrialsAfterFailure->value());
if (numTries == _maxTrialsAfterFailure->value() || !goodStep)
return Terminate;
return OK;
}
void OptimizationAlgorithmDogleg::printVerbose(std::ostream& os) const
{
os
<< "\t Delta= " << _delta
<< "\t step= " << stepType2Str(_lastStep)
<< "\t tries= " << _lastNumTries;
if (! _wasPDInAllIterations)
os << "\t lambda= " << _currentLambda;
}
const char* OptimizationAlgorithmDogleg::stepType2Str(int stepType)
{
switch (stepType) {
case STEP_SD: return "Descent";
case STEP_GN: return "GN";
case STEP_DL: return "Dogleg";
default: return "Undefined";
}
}
} // end namespace

89
Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h vendored Normal file
View File

@@ -0,0 +1,89 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
#define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
#include "optimization_algorithm_with_hessian.h"
namespace g2o {
class BlockSolverBase;
/**
* \brief Implementation of Powell's Dogleg Algorithm
*/
class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian
{
public:
/** \brief type of the step to take */
enum {
STEP_UNDEFINED,
STEP_SD, STEP_GN, STEP_DL
};
public:
/**
* construct the Dogleg algorithm, which will use the given Solver for solving the
* linearized system.
*/
explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver);
virtual ~OptimizationAlgorithmDogleg();
virtual SolverResult solve(int iteration, bool online = false);
virtual void printVerbose(std::ostream& os) const;
//! return the type of the last step taken by the algorithm
int lastStep() const { return _lastStep;}
//! return the diameter of the trust region
double trustRegion() const { return _delta;}
//! convert the type into an integer
static const char* stepType2Str(int stepType);
protected:
// parameters
Property<int>* _maxTrialsAfterFailure;
Property<double>* _userDeltaInit;
// damping to enforce positive definite matrix
Property<double>* _initialLambda;
Property<double>* _lamdbaFactor;
Eigen::VectorXd _hsd; ///< steepest decent step
Eigen::VectorXd _hdl; ///< final dogleg step
Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff
double _currentLambda; ///< the damping factor to force positive definite matrix
double _delta; ///< trust region
int _lastStep; ///< type of the step taken by the algorithm
bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping
int _lastNumTries;
};
} // end namespace
#endif

137
Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp vendored Normal file
View File

@@ -0,0 +1,137 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "optimization_algorithm_factory.h"
#include <iostream>
#include <typeinfo>
#include <cassert>
using namespace std;
namespace g2o {
AbstractOptimizationAlgorithmCreator::AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p) :
_property(p)
{
}
OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::factoryInstance = 0;
OptimizationAlgorithmFactory::OptimizationAlgorithmFactory()
{
}
OptimizationAlgorithmFactory::~OptimizationAlgorithmFactory()
{
for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it)
delete *it;
}
OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::instance()
{
if (factoryInstance == 0) {
factoryInstance = new OptimizationAlgorithmFactory;
}
return factoryInstance;
}
void OptimizationAlgorithmFactory::registerSolver(AbstractOptimizationAlgorithmCreator* c)
{
const string& name = c->property().name;
CreatorList::iterator foundIt = findSolver(name);
if (foundIt != _creator.end()) {
_creator.erase(foundIt);
cerr << "SOLVER FACTORY WARNING: Overwriting Solver creator " << name << endl;
assert(0);
}
_creator.push_back(c);
}
void OptimizationAlgorithmFactory::unregisterSolver(AbstractOptimizationAlgorithmCreator* c)
{
const string& name = c->property().name;
CreatorList::iterator foundIt = findSolver(name);
if (foundIt != _creator.end()) {
delete *foundIt;
_creator.erase(foundIt);
}
}
OptimizationAlgorithm* OptimizationAlgorithmFactory::construct(const std::string& name, OptimizationAlgorithmProperty& solverProperty) const
{
CreatorList::const_iterator foundIt = findSolver(name);
if (foundIt != _creator.end()) {
solverProperty = (*foundIt)->property();
return (*foundIt)->construct();
}
cerr << "SOLVER FACTORY WARNING: Unable to create solver " << name << endl;
return 0;
}
void OptimizationAlgorithmFactory::destroy()
{
delete factoryInstance;
factoryInstance = 0;
}
void OptimizationAlgorithmFactory::listSolvers(std::ostream& os) const
{
size_t solverNameColumnLength = 0;
for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
solverNameColumnLength = std::max(solverNameColumnLength, (*it)->property().name.size());
solverNameColumnLength += 4;
for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
const OptimizationAlgorithmProperty& sp = (*it)->property();
os << sp.name;
for (size_t i = sp.name.size(); i < solverNameColumnLength; ++i)
os << ' ';
os << sp.desc << endl;
}
}
OptimizationAlgorithmFactory::CreatorList::const_iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) const
{
for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
const OptimizationAlgorithmProperty& sp = (*it)->property();
if (sp.name == name)
return it;
}
return _creator.end();
}
OptimizationAlgorithmFactory::CreatorList::iterator OptimizationAlgorithmFactory::findSolver(const std::string& name)
{
for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) {
const OptimizationAlgorithmProperty& sp = (*it)->property();
if (sp.name == name)
return it;
}
return _creator.end();
}
} // end namespace

167
Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h vendored Normal file
View File

@@ -0,0 +1,167 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OPTMIZATION_ALGORITHM_PROPERTY_H
#define G2O_OPTMIZATION_ALGORITHM_PROPERTY_H
#include "../../config.h"
#include "../stuff/misc.h"
#include "optimization_algorithm_property.h"
#include <list>
#include <iostream>
#include <typeinfo>
// define to get some verbose output
//#define G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
namespace g2o {
// forward decl
class OptimizationAlgorithm;
class SparseOptimizer;
/**
* \brief base for allocating an optimization algorithm
*
* Allocating a solver for a given optimizer. The method construct() has to be
* implemented in your derived class to allocate the desired solver.
*/
class AbstractOptimizationAlgorithmCreator
{
public:
AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p);
virtual ~AbstractOptimizationAlgorithmCreator() { }
//! allocate a solver operating on optimizer, re-implement for your creator
virtual OptimizationAlgorithm* construct() = 0;
//! return the properties of the solver
const OptimizationAlgorithmProperty& property() const { return _property;}
protected:
OptimizationAlgorithmProperty _property;
};
/**
* \brief create solvers based on their short name
*
* Factory to allocate solvers based on their short name.
* The Factory is implemented as a sigleton and the single
* instance can be accessed via the instance() function.
*/
class OptimizationAlgorithmFactory
{
public:
typedef std::list<AbstractOptimizationAlgorithmCreator*> CreatorList;
//! return the instance
static OptimizationAlgorithmFactory* instance();
//! free the instance
static void destroy();
/**
* register a specific creator for allocating a solver
*/
void registerSolver(AbstractOptimizationAlgorithmCreator* c);
/**
* unregister a specific creator for allocating a solver
*/
void unregisterSolver(AbstractOptimizationAlgorithmCreator* c);
/**
* construct a solver based on its name, e.g., var, fix3_2_cholmod
*/
OptimizationAlgorithm* construct(const std::string& tag, OptimizationAlgorithmProperty& solverProperty) const;
//! list the known solvers into a stream
void listSolvers(std::ostream& os) const;
//! return the underlying list of creators
const CreatorList& creatorList() const { return _creator;}
protected:
OptimizationAlgorithmFactory();
~OptimizationAlgorithmFactory();
CreatorList _creator;
CreatorList::const_iterator findSolver(const std::string& name) const;
CreatorList::iterator findSolver(const std::string& name);
private:
static OptimizationAlgorithmFactory* factoryInstance;
};
class RegisterOptimizationAlgorithmProxy
{
public:
RegisterOptimizationAlgorithmProxy(AbstractOptimizationAlgorithmCreator* c)
{
_creator = c;
#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
std::cout << __FUNCTION__ << ": Registering " << _creator->property().name << " of type " << typeid(*_creator).name() << std::endl;
#endif
OptimizationAlgorithmFactory::instance()->registerSolver(c);
}
~RegisterOptimizationAlgorithmProxy()
{
#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
std::cout << __FUNCTION__ << ": Unregistering " << _creator->property().name << std::endl;
#endif
OptimizationAlgorithmFactory::instance()->unregisterSolver(_creator);
}
private:
AbstractOptimizationAlgorithmCreator* _creator;
};
}
#if defined _MSC_VER && defined G2O_SHARED_LIBS
# define G2O_OAF_EXPORT __declspec(dllexport)
# define G2O_OAF_IMPORT __declspec(dllimport)
#else
# define G2O_OAF_EXPORT
# define G2O_OAF_IMPORT
#endif
#define G2O_REGISTER_OPTIMIZATION_LIBRARY(libraryname) \
extern "C" void G2O_OAF_EXPORT g2o_optimization_library_##libraryname(void) {}
#define G2O_USE_OPTIMIZATION_LIBRARY(libraryname) \
extern "C" void G2O_OAF_IMPORT g2o_optimization_library_##libraryname(void); \
static g2o::ForceLinker g2o_force_optimization_algorithm_library_##libraryname(g2o_optimization_library_##libraryname);
#define G2O_REGISTER_OPTIMIZATION_ALGORITHM(optimizername, instance) \
extern "C" void G2O_OAF_EXPORT g2o_optimization_algorithm_##optimizername(void) {} \
static g2o::RegisterOptimizationAlgorithmProxy g_optimization_algorithm_proxy_##optimizername(instance);
#define G2O_USE_OPTIMIZATION_ALGORITHM(optimizername) \
extern "C" void G2O_OAF_IMPORT g2o_optimization_algorithm_##optimizername(void); \
static g2o::ForceLinker g2o_force_optimization_algorithm_link_##optimizername(g2o_optimization_algorithm_##optimizername);
#endif

101
Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp vendored Normal file
View File

@@ -0,0 +1,101 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "optimization_algorithm_gauss_newton.h"
#include <iostream>
#include "../stuff/timeutil.h"
#include "../stuff/macros.h"
#include "solver.h"
#include "batch_stats.h"
#include "sparse_optimizer.h"
using namespace std;
namespace g2o {
OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) :
OptimizationAlgorithmWithHessian(solver)
{
}
OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton()
{
}
OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
bool ok = true;
//here so that correct component for max-mixtures can be computed before the build structure
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
}
if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
ok = _solver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
}
t=get_monotonic_time();
_solver->buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
t=get_monotonic_time();
}
ok = _solver->solve();
if (globalStats) {
globalStats->timeLinearSolution = get_monotonic_time()-t;
t=get_monotonic_time();
}
_optimizer->update(_solver->x());
if (globalStats) {
globalStats->timeUpdate = get_monotonic_time()-t;
}
if (ok)
return OK;
else
return Fail;
}
void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const
{
os
<< "\t schur= " << _solver->schur();
}
} // end namespace

54
Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h vendored Normal file
View File

@@ -0,0 +1,54 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H
#define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H
#include "optimization_algorithm_with_hessian.h"
namespace g2o {
/**
* \brief Implementation of the Gauss Newton Algorithm
*/
class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian
{
public:
/**
* construct the Gauss Newton algorithm, which use the given Solver for solving the
* linearized system.
*/
explicit OptimizationAlgorithmGaussNewton(Solver* solver);
virtual ~OptimizationAlgorithmGaussNewton();
virtual SolverResult solve(int iteration, bool online = false);
virtual void printVerbose(std::ostream& os) const;
};
} // end namespace
#endif

214
Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp vendored Normal file
View File

@@ -0,0 +1,214 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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.
// Modified Raul Mur Artal (2014)
// - Stop criterium (solve function)
#include "optimization_algorithm_levenberg.h"
#include <iostream>
#include "../stuff/timeutil.h"
#include "sparse_optimizer.h"
#include "solver.h"
#include "batch_stats.h"
using namespace std;
namespace g2o {
OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Solver* solver) :
OptimizationAlgorithmWithHessian(solver)
{
_currentLambda = -1.;
_tau = 1e-5; // Carlos: originally 1e-5
_goodStepUpperScale = 2./3.;
_goodStepLowerScale = 1./3.;
_userLambdaInit = _properties.makeProperty<Property<double> >("initialLambda", 0.);
_maxTrialsAfterFailure = _properties.makeProperty<Property<int> >("maxTrialsAfterFailure", 10); // Carlos: Originally 10 iterations
_ni=2.;
_levenbergIterations = 0;
_nBad = 0;
}
OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg()
{
}
OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
bool ok = _solver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
}
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
t=get_monotonic_time();
}
double currentChi = _optimizer->activeRobustChi2();
double tempChi=currentChi;
double iniChi = currentChi;
_solver->buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
}
// core part of the Levenbarg algorithm
if (iteration == 0) {
_currentLambda = computeLambdaInit();
_ni = 2;
_nBad = 0;
}
double rho=0;
int& qmax = _levenbergIterations;
qmax = 0;
do {
_optimizer->push();
if (globalStats) {
globalStats->levenbergIterations++;
t=get_monotonic_time();
}
// update the diagonal of the system matrix
_solver->setLambda(_currentLambda, true);
bool ok2 = _solver->solve();
if (globalStats) {
globalStats->timeLinearSolution+=get_monotonic_time()-t;
t=get_monotonic_time();
}
_optimizer->update(_solver->x());
if (globalStats) {
globalStats->timeUpdate = get_monotonic_time()-t;
}
// restore the diagonal
_solver->restoreDiagonal();
_optimizer->computeActiveErrors();
tempChi = _optimizer->activeRobustChi2();
// cout << "tempChi: " << tempChi << endl;
if (! ok2)
tempChi=std::numeric_limits<double>::max();
rho = (currentChi-tempChi);
double scale = computeScale();
scale += 1e-3; // make sure it's non-zero :)
rho /= scale;
if (rho>0 && g2o_isfinite(tempChi)){ // last step was good
double alpha = 1.-pow((2*rho-1),3);
// crop lambda between minimum and maximum factors
alpha = (std::min)(alpha, _goodStepUpperScale);
double scaleFactor = (std::max)(_goodStepLowerScale, alpha);
_currentLambda *= scaleFactor;
_ni = 2;
currentChi=tempChi;
_optimizer->discardTop();
} else {
_currentLambda*=_ni;
_ni*=2;
_optimizer->pop(); // restore the last state before trying to optimize
}
qmax++;
} while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate());
if (qmax == _maxTrialsAfterFailure->value() || rho==0)
{
// cout << "qmax = " << qmax << " rho = " << rho << endl;
return Terminate;
}
//Stop criterium (Raul)
if((iniChi-currentChi)*1e3<iniChi)
_nBad++;
else
_nBad=0;
if(_nBad>=3)
{
return Terminate;
}
return OK;
}
double OptimizationAlgorithmLevenberg::computeLambdaInit() const
{
if (_userLambdaInit->value() > 0)
return _userLambdaInit->value();
double maxDiagonal=0.;
for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k];
assert(v);
int dim = v->dimension();
for (int j = 0; j < dim; ++j){
maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal);
}
}
return _tau*maxDiagonal;
}
double OptimizationAlgorithmLevenberg::computeScale() const
{
double scale = 0.;
for (size_t j=0; j < _solver->vectorSize(); j++){
scale += _solver->x()[j] * (_currentLambda * _solver->x()[j] + _solver->b()[j]);
}
return scale;
}
void OptimizationAlgorithmLevenberg::setMaxTrialsAfterFailure(int max_trials)
{
_maxTrialsAfterFailure->setValue(max_trials);
}
void OptimizationAlgorithmLevenberg::setUserLambdaInit(double lambda)
{
_userLambdaInit->setValue(lambda);
}
void OptimizationAlgorithmLevenberg::printVerbose(std::ostream& os) const
{
os
<< "\t schur= " << _solver->schur()
<< "\t lambda= " << FIXED(_currentLambda)
<< "\t levenbergIter= " << _levenbergIterations;
}
} // end namespace

92
Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h vendored Normal file
View File

@@ -0,0 +1,92 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_SOLVER_LEVENBERG_H
#define G2O_SOLVER_LEVENBERG_H
#include "optimization_algorithm_with_hessian.h"
namespace g2o {
/**
* \brief Implementation of the Levenberg Algorithm
*/
class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian
{
public:
/**
* construct the Levenberg algorithm, which will use the given Solver for solving the
* linearized system.
*/
explicit OptimizationAlgorithmLevenberg(Solver* solver);
virtual ~OptimizationAlgorithmLevenberg();
virtual SolverResult solve(int iteration, bool online = false);
virtual void printVerbose(std::ostream& os) const;
//! return the currently used damping factor
double currentLambda() const { return _currentLambda;}
//! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt
void setMaxTrialsAfterFailure(int max_trials);
//! get the number of inner iterations for Levenberg-Marquardt
int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();}
//! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda
double userLambdaInit() {return _userLambdaInit->value();}
//! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value
void setUserLambdaInit(double lambda);
//! return the number of levenberg iterations performed in the last round
int levenbergIteration() { return _levenbergIterations;}
protected:
// Levenberg parameters
Property<int>* _maxTrialsAfterFailure;
Property<double>* _userLambdaInit;
double _currentLambda;
double _tau;
double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step
double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step
double _ni;
int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step
//RAUL
int _nBad;
/**
* helper for Levenberg, this function computes the initial damping factor, if the user did not
* specify an own value, see setUserLambdaInit()
*/
double computeLambdaInit() const;
double computeScale() const;
};
} // end namespace
#endif

57
Thirdparty/g2o/g2o/core/optimization_algorithm_property.h vendored Normal file
View File

@@ -0,0 +1,57 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H
#define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H
#include <string>
namespace g2o {
/**
* \brief describe the properties of a solver
*/
struct OptimizationAlgorithmProperty
{
std::string name; ///< name of the solver, e.g., var
std::string desc; ///< short description of the solver
std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG"
bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks
int poseDim; ///< dimension of the pose vertices (-1 if variable)
int landmarkDim; ///< dimension of the landmar vertices (-1 if variable)
OptimizationAlgorithmProperty() :
name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1)
{
}
OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) :
name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_)
{
}
};
} // end namespace
#endif

101
Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp vendored Normal file
View File

@@ -0,0 +1,101 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "optimization_algorithm_with_hessian.h"
#include "solver.h"
#include "optimizable_graph.h"
#include "sparse_optimizer.h"
#include <iostream>
using namespace std;
namespace g2o {
OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) :
OptimizationAlgorithm(),
_solver(solver)
{
_writeDebug = _properties.makeProperty<Property<bool> >("writeDebug", true);
}
OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian()
{
delete _solver;
}
bool OptimizationAlgorithmWithHessian::init(bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver && "Solver not set");
_solver->setWriteDebug(_writeDebug->value());
bool useSchur=false;
for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) {
OptimizableGraph::Vertex* v= *it;
if (v->marginalized()){
useSchur=true;
break;
}
}
if (useSchur){
if (_solver->supportsSchur())
_solver->setSchur(true);
} else {
if (_solver->supportsSchur())
_solver->setSchur(false);
}
bool initState = _solver->init(_optimizer, online);
return initState;
}
bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
{
return _solver ? _solver->computeMarginals(spinv, blockIndices) : false;
}
bool OptimizationAlgorithmWithHessian::buildLinearStructure()
{
return _solver ? _solver->buildStructure() : false;
}
void OptimizationAlgorithmWithHessian::updateLinearSystem()
{
if (_solver)
_solver->buildSystem();
}
bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
{
return _solver ? _solver->updateStructure(vset, edges) : false;
}
void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug)
{
_writeDebug->setValue(writeDebug);
}
} // end namespace

72
Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h vendored Normal file
View File

@@ -0,0 +1,72 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H
#define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H
#include "optimization_algorithm.h"
namespace g2o {
class Solver;
/**
* \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg
*/
class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm
{
public:
explicit OptimizationAlgorithmWithHessian(Solver* solver);
virtual ~OptimizationAlgorithmWithHessian();
virtual bool init(bool online = false);
virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
virtual bool buildLinearStructure();
virtual void updateLinearSystem();
virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
//! return the underlying solver used to solve the linear system
Solver* solver() { return _solver;}
/**
* write debug output of the Hessian if system is not positive definite
*/
virtual void setWriteDebug(bool writeDebug);
virtual bool writeDebug() const { return _writeDebug->value();}
protected:
Solver* _solver;
Property<bool>* _writeDebug;
};
}// end namespace
#endif

40
Thirdparty/g2o/g2o/core/parameter.cpp vendored Normal file
View File

@@ -0,0 +1,40 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "parameter.h"
namespace g2o {
Parameter::Parameter() : _id(-1)
{
}
void Parameter::setId(int id_)
{
_id = id_;
}
} // end namespace

56
Thirdparty/g2o/g2o/core/parameter.h vendored Normal file
View File

@@ -0,0 +1,56 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_GRAPH_PARAMETER_HH_
#define G2O_GRAPH_PARAMETER_HH_
#include <iosfwd>
#include "hyper_graph.h"
namespace g2o {
class Parameter : public HyperGraph::HyperGraphElement
{
public:
Parameter();
virtual ~Parameter() {};
//! read the data from a stream
virtual bool read(std::istream& is) = 0;
//! write the data to a stream
virtual bool write(std::ostream& os) const = 0;
int id() const {return _id;}
void setId(int id_);
virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;}
protected:
int _id;
};
typedef std::vector<Parameter*> ParameterVector;
} // end namespace
#endif

142
Thirdparty/g2o/g2o/core/parameter_container.cpp vendored Normal file
View File

@@ -0,0 +1,142 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "parameter_container.h"
#include <iostream>
#include "factory.h"
#include "parameter.h"
#include "../stuff/macros.h"
#include "../stuff/color_macros.h"
#include "../stuff/string_tools.h"
namespace g2o {
using namespace std;
ParameterContainer::ParameterContainer(bool isMainStorage_) :
_isMainStorage(isMainStorage_)
{
}
void ParameterContainer::clear() {
if (!_isMainStorage)
return;
for (iterator it = begin(); it!=end(); it++){
delete it->second;
}
BaseClass::clear();
}
ParameterContainer::~ParameterContainer(){
clear();
}
bool ParameterContainer::addParameter(Parameter* p){
if (p->id()<0)
return false;
iterator it=find(p->id());
if (it!=end())
return false;
insert(make_pair(p->id(), p));
return true;
}
Parameter* ParameterContainer::getParameter(int id) {
iterator it=find(id);
if (it==end())
return 0;
return it->second;
}
Parameter* ParameterContainer::detachParameter(int id){
iterator it=find(id);
if (it==end())
return 0;
Parameter* p=it->second;
erase(it);
return p;
}
bool ParameterContainer::write(std::ostream& os) const{
Factory* factory = Factory::instance();
for (const_iterator it=begin(); it!=end(); it++){
os << factory->tag(it->second) << " ";
os << it->second->id() << " ";
it->second->write(os);
os << endl;
}
return true;
}
bool ParameterContainer::read(std::istream& is, const std::map<std::string, std::string>* _renamedTypesLookup){
stringstream currentLine;
string token;
Factory* factory = Factory::instance();
HyperGraph::GraphElemBitset elemBitset;
elemBitset[HyperGraph::HGET_PARAMETER] = 1;
while (1) {
int bytesRead = readLine(is, currentLine);
if (bytesRead == -1)
break;
currentLine >> token;
if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
continue;
if (_renamedTypesLookup && _renamedTypesLookup->size()>0){
map<string, string>::const_iterator foundIt = _renamedTypesLookup->find(token);
if (foundIt != _renamedTypesLookup->end()) {
token = foundIt->second;
}
}
HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
if (! element) // not a parameter or otherwise unknown tag
continue;
assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param");
Parameter* p = static_cast<Parameter*>(element);
int pid;
currentLine >> pid;
p->setId(pid);
bool r = p->read(currentLine);
if (! r) {
cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl;
delete p;
} else {
if (! addParameter(p) ){
cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl;
}
}
} // while read line
return true;
}
} // end namespace

74
Thirdparty/g2o/g2o/core/parameter_container.h vendored Normal file
View File

@@ -0,0 +1,74 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_GRAPH_PARAMETER_CONTAINER_HH_
#define G2O_GRAPH_PARAMETER_CONTAINER_HH_
#include <iosfwd>
#include <map>
#include <string>
namespace g2o {
class Parameter;
/**
* \brief map id to parameters
*/
class ParameterContainer : protected std::map<int, Parameter*>
{
public:
typedef std::map<int, Parameter*> BaseClass;
/**
* create a container for the parameters.
* @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor
*/
ParameterContainer(bool isMainStorage_=true);
virtual ~ParameterContainer();
//! add parameter to the container
bool addParameter(Parameter* p);
//! return a parameter based on its ID
Parameter* getParameter(int id);
//! remove a parameter from the container, i.e., the user now owns the pointer
Parameter* detachParameter(int id);
//! read parameters from a stream
virtual bool read(std::istream& is, const std::map<std::string, std::string>* renamedMap =0);
//! write the data to a stream
virtual bool write(std::ostream& os) const;
bool isMainStorage() const {return _isMainStorage;}
void clear();
// stuff of the base class that should re-appear
using BaseClass::size;
protected:
bool _isMainStorage;
};
} // end namespace
#endif

46
Thirdparty/g2o/g2o/core/robust_kernel.cpp vendored Normal file
View File

@@ -0,0 +1,46 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "robust_kernel.h"
namespace g2o {
RobustKernel::RobustKernel() :
_delta(1.)
{
}
RobustKernel::RobustKernel(double delta) :
_delta(delta)
{
}
void RobustKernel::setDelta(double delta)
{
_delta = delta;
}
} // end namespace g2o

81
Thirdparty/g2o/g2o/core/robust_kernel.h vendored Normal file
View File

@@ -0,0 +1,81 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_ROBUST_KERNEL_H
#define G2O_ROBUST_KERNEL_H
#ifdef _MSC_VER
#include <memory>
#else
#include <tr1/memory>
#endif
#include <Eigen/Core>
namespace g2o {
/**
* \brief base for all robust cost functions
*
* Note in all the implementations for the other cost functions the e in the
* funtions corresponds to the sqaured errors, i.e., the robustification
* functions gets passed the squared error.
*
* e.g. the robustified least squares function is
*
* chi^2 = sum_{e} rho( e^T Omega e )
*/
class RobustKernel
{
public:
RobustKernel();
explicit RobustKernel(double delta);
virtual ~RobustKernel() {}
/**
* compute the scaling factor for a error:
* The error is e^T Omega e
* The output rho is
* rho[0]: The actual scaled error value
* rho[1]: First derivative of the scaling function
* rho[2]: Second derivative of the scaling function
*/
virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0;
/**
* set the window size of the error. A squared error above delta^2 is considered
* as outlier in the data.
*/
virtual void setDelta(double delta);
double delta() const { return _delta;}
protected:
double _delta;
};
typedef std::shared_ptr<RobustKernel> RobustKernelPtr;
} // end namespace g2o
#endif

111
Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp vendored Normal file
View File

@@ -0,0 +1,111 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "robust_kernel_factory.h"
#include "robust_kernel.h"
#include <cassert>
using namespace std;
namespace g2o {
RobustKernelFactory* RobustKernelFactory::factoryInstance = 0;
RobustKernelFactory::RobustKernelFactory()
{
}
RobustKernelFactory::~RobustKernelFactory()
{
for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {
delete it->second;
}
_creator.clear();
}
RobustKernelFactory* RobustKernelFactory::instance()
{
if (factoryInstance == 0) {
factoryInstance = new RobustKernelFactory;
}
return factoryInstance;
}
void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c)
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end()) {
cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl;
assert(0);
}
_creator[tag] = c;
}
void RobustKernelFactory::unregisterType(const std::string& tag)
{
CreatorMap::iterator tagPosition = _creator.find(tag);
if (tagPosition != _creator.end()) {
AbstractRobustKernelCreator* c = tagPosition->second;
delete c;
_creator.erase(tagPosition);
}
}
RobustKernel* RobustKernelFactory::construct(const std::string& tag) const
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end()) {
return foundIt->second->construct();
}
return 0;
}
AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const
{
CreatorMap::const_iterator foundIt = _creator.find(tag);
if (foundIt != _creator.end()) {
return foundIt->second;
}
return 0;
}
void RobustKernelFactory::fillKnownKernels(std::vector<std::string>& types) const
{
types.clear();
for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
types.push_back(it->first);
}
void RobustKernelFactory::destroy()
{
delete factoryInstance;
factoryInstance = 0;
}
} // end namespace

151
Thirdparty/g2o/g2o/core/robust_kernel_factory.h vendored Normal file
View File

@@ -0,0 +1,151 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_ROBUST_KERNEL_FACTORY_H
#define G2O_ROBUST_KERNEL_FACTORY_H
#include "../stuff/misc.h"
#include <string>
#include <map>
#include <vector>
#include <iostream>
namespace g2o {
class RobustKernel;
/**
* \brief Abstract interface for allocating a robust kernel
*/
class AbstractRobustKernelCreator
{
public:
/**
* create a hyper graph element. Has to implemented in derived class.
*/
virtual RobustKernel* construct() = 0;
virtual ~AbstractRobustKernelCreator() { }
};
/**
* \brief templatized creator class which creates graph elements
*/
template <typename T>
class RobustKernelCreator : public AbstractRobustKernelCreator
{
public:
RobustKernel* construct() { return new T;}
};
/**
* \brief create robust kernels based on their human readable name
*/
class RobustKernelFactory
{
public:
//! return the instance
static RobustKernelFactory* instance();
//! free the instance
static void destroy();
/**
* register a tag for a specific creator
*/
void registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c);
/**
* unregister a tag for a specific creator
*/
void unregisterType(const std::string& tag);
/**
* construct a robust kernel based on its tag
*/
RobustKernel* construct(const std::string& tag) const;
/**
* return the creator for a specific tag
*/
AbstractRobustKernelCreator* creator(const std::string& tag) const;
/**
* get a list of all known robust kernels
*/
void fillKnownKernels(std::vector<std::string>& types) const;
protected:
typedef std::map<std::string, AbstractRobustKernelCreator*> CreatorMap;
RobustKernelFactory();
~RobustKernelFactory();
CreatorMap _creator; ///< look-up map for the existing creators
private:
static RobustKernelFactory* factoryInstance;
};
template<typename T>
class RegisterRobustKernelProxy
{
public:
RegisterRobustKernelProxy(const std::string& name) : _name(name)
{
RobustKernelFactory::instance()->registerRobustKernel(_name, new RobustKernelCreator<T>());
}
~RegisterRobustKernelProxy()
{
RobustKernelFactory::instance()->unregisterType(_name);
}
private:
std::string _name;
};
#if defined _MSC_VER && defined G2O_SHARED_LIBS
# define G2O_ROBUST_KERNEL_FACTORY_EXPORT __declspec(dllexport)
# define G2O_ROBUST_KERNEL_FACTORY_IMPORT __declspec(dllimport)
#else
# define G2O_ROBUST_KERNEL_FACTORY_EXPORT
# define G2O_ROBUST_KERNEL_FACTORY_IMPORT
#endif
// These macros are used to automate registering of robust kernels and forcing linkage
#define G2O_REGISTER_ROBUST_KERNEL(name, classname) \
extern "C" void G2O_ROBUST_KERNEL_FACTORY_EXPORT g2o_robust_kernel_##classname(void) {} \
static g2o::RegisterRobustKernelProxy<classname> g_robust_kernel_proxy_##classname(#name);
#define G2O_USE_ROBUST_KERNEL(classname) \
extern "C" void G2O_ROBUST_KERNEL_FACTORY_IMPORT g2o_robust_kernel_##classname(void); \
static g2o::TypeFunctionProxy proxy_##classname(g2o_robust_kernel_##classname);
} // end namespace g2o
#endif

173
Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp vendored Normal file
View File

@@ -0,0 +1,173 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "robust_kernel_impl.h"
#include "robust_kernel_factory.h"
#include <cmath>
namespace g2o {
RobustKernelScaleDelta::RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta) :
RobustKernel(delta),
_kernel(kernel)
{
}
RobustKernelScaleDelta::RobustKernelScaleDelta(double delta) :
RobustKernel(delta)
{
}
void RobustKernelScaleDelta::setKernel(const RobustKernelPtr& ptr)
{
_kernel = ptr;
}
void RobustKernelScaleDelta::robustify(double error, Eigen::Vector3d& rho) const
{
if (_kernel.get()) {
double dsqr = _delta * _delta;
double dsqrReci = 1. / dsqr;
_kernel->robustify(dsqrReci * error, rho);
rho[0] *= dsqr;
rho[2] *= dsqrReci;
} else { // no robustification
rho[0] = error;
rho[1] = 1.;
rho[2] = 0.;
}
}
void RobustKernelHuber::setDelta(double delta)
{
dsqr = delta*delta;
_delta = delta;
}
void RobustKernelHuber::setDeltaSqr(const double &delta, const double &deltaSqr)
{
dsqr = deltaSqr;
_delta = delta;
}
void RobustKernelHuber::robustify(double e, Eigen::Vector3d& rho) const
{
//dsqr = _delta * _delta;
if (e <= dsqr) { // inlier
rho[0] = e;
rho[1] = 1.;
rho[2] = 0.;
} else { // outlier
double sqrte = sqrt(e); // absolut value of the error
rho[0] = 2*sqrte*_delta - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2
rho[1] = _delta / sqrte; // rho'(e) = delta / sqrt(e)
rho[2] = - 0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e
}
}
void RobustKernelTukey::setDeltaSqr(const double &deltaSqr, const double &inv)
{
_deltaSqr = deltaSqr;
_invDeltaSqr = inv;
}
void RobustKernelTukey::robustify(double e, Eigen::Vector3d& rho) const
{
if (e <= _deltaSqr) { // inlier
double factor = e*_invDeltaSqr;
double d = 1-factor;
double dd = d*d;
rho[0] = _deltaSqr*(1-dd*d);
rho[1] = 3*dd;
rho[2] = -6*_invDeltaSqr*d;
} else { // outlier
rho[0] = _deltaSqr; // rho(e) = delta^2
rho[1] = 0.;
rho[2] = 0.;
}
}
void RobustKernelPseudoHuber::robustify(double e2, Eigen::Vector3d& rho) const
{
double dsqr = _delta * _delta;
double dsqrReci = 1. / dsqr;
double aux1 = dsqrReci * e2 + 1.0;
double aux2 = sqrt(aux1);
rho[0] = 2 * dsqr * (aux2 - 1);
rho[1] = 1. / aux2;
rho[2] = -0.5 * dsqrReci * rho[1] / aux1;
}
void RobustKernelCauchy::robustify(double e2, Eigen::Vector3d& rho) const
{
double dsqr = _delta * _delta;
double dsqrReci = 1. / dsqr;
double aux = dsqrReci * e2 + 1.0;
rho[0] = dsqr * log(aux);
rho[1] = 1. / aux;
rho[2] = -dsqrReci * std::pow(rho[1], 2);
}
void RobustKernelSaturated::robustify(double e2, Eigen::Vector3d& rho) const
{
double dsqr = _delta * _delta;
if (e2 <= dsqr) { // inlier
rho[0] = e2;
rho[1] = 1.;
rho[2] = 0.;
} else { // outlier
rho[0] = dsqr;
rho[1] = 0.;
rho[2] = 0.;
}
}
//delta is used as $phi$
void RobustKernelDCS::robustify(double e2, Eigen::Vector3d& rho) const
{
const double& phi = _delta;
double scale = (2.0*phi)/(phi+e2);
if(scale>=1.0)
scale = 1.0;
rho[0] = scale*e2*scale;
rho[1] = (scale*scale);
rho[2] = 0;
}
// register the kernel to their factory
G2O_REGISTER_ROBUST_KERNEL(Huber, RobustKernelHuber)
G2O_REGISTER_ROBUST_KERNEL(Tukey, RobustKernelTukey)
G2O_REGISTER_ROBUST_KERNEL(PseudoHuber, RobustKernelPseudoHuber)
G2O_REGISTER_ROBUST_KERNEL(Cauchy, RobustKernelCauchy)
G2O_REGISTER_ROBUST_KERNEL(Saturated, RobustKernelSaturated)
G2O_REGISTER_ROBUST_KERNEL(DCS, RobustKernelDCS)
} // end namespace g2o

167
Thirdparty/g2o/g2o/core/robust_kernel_impl.h vendored Normal file
View File

@@ -0,0 +1,167 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_ROBUST_KERNEL_IMPL_H
#define G2O_ROBUST_KERNEL_IMPL_H
#include "robust_kernel.h"
namespace g2o {
/**
* \brief scale a robust kernel to another delta (window size)
*
* Scales a robust kernel to another window size. Useful, in case if
* one implements a kernel which only is designed for a fixed window
* size.
*/
class RobustKernelScaleDelta : public RobustKernel
{
public:
/**
* construct the scaled kernel ontop of another kernel which might be shared accross
* several scaled kernels
*/
explicit RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta = 1.);
explicit RobustKernelScaleDelta(double delta = 1.);
//! return the underlying kernel
const RobustKernelPtr kernel() const { return _kernel;}
//! use another kernel for the underlying operation
void setKernel(const RobustKernelPtr& ptr);
void robustify(double error, Eigen::Vector3d& rho) const;
protected:
RobustKernelPtr _kernel;
};
/**
* \brief Huber Cost Function
*
* Loss function as described by Huber
* See http://en.wikipedia.org/wiki/Huber_loss_function
*
* If e^(1/2) < d
* rho(e) = e
*
* else
*
* 1/2 2
* rho(e) = 2 d e - d
*/
class RobustKernelHuber : public RobustKernel
{
public:
virtual void setDelta(double delta);
virtual void setDeltaSqr(const double &delta, const double &deltaSqr);
virtual void robustify(double e2, Eigen::Vector3d& rho) const;
private:
float dsqr;
};
/**
* \brief Tukey Cost Function
*
*
* If e^(1/2) < d
* rho(e) = delta2(1-(1-e/delta2)^3)
*
* else
*
* rho(e) = delta2
*/
class RobustKernelTukey : public RobustKernel
{
public:
virtual void setDeltaSqr(const double &deltaSqr, const double &inv);
virtual void robustify(double e2, Eigen::Vector3d& rho) const;
private:
float _deltaSqr;
float _invDeltaSqr;
};
/**
* \brief Pseudo Huber Cost Function
*
* The smooth pseudo huber cost function:
* See http://en.wikipedia.org/wiki/Huber_loss_function
*
* 2 e
* 2 d (sqrt(-- + 1) - 1)
* 2
* d
*/
class RobustKernelPseudoHuber : public RobustKernel
{
public:
virtual void robustify(double e2, Eigen::Vector3d& rho) const;
};
/**
* \brief Cauchy cost function
*
* 2 e
* d log(-- + 1)
* 2
* d
*/
class RobustKernelCauchy : public RobustKernel
{
public:
virtual void robustify(double e2, Eigen::Vector3d& rho) const;
};
/**
* \brief Saturated cost function.
*
* The error is at most delta^2
*/
class RobustKernelSaturated : public RobustKernel
{
public:
virtual void robustify(double e2, Eigen::Vector3d& rho) const;
};
/**
* \brief Dynamic covariance scaling - DCS
*
* See paper Robust Map Optimization from Agarwal et al. ICRA 2013
*
* delta is used as $phi$
*/
class RobustKernelDCS : public RobustKernel
{
public:
virtual void robustify(double e2, Eigen::Vector3d& rho) const;
};
} // end namespace g2o
#endif

87
Thirdparty/g2o/g2o/core/solver.cpp vendored Normal file
View File

@@ -0,0 +1,87 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "solver.h"
#include <cstring>
#include <algorithm>
namespace g2o {
Solver::Solver() :
_optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0),
_isLevenberg(false), _additionalVectorSpace(0)
{
}
Solver::~Solver()
{
delete[] _x;
delete[] _b;
}
void Solver::resizeVector(size_t sx)
{
size_t oldSize = _xSize;
_xSize = sx;
sx += _additionalVectorSpace; // allocate some additional space if requested
if (_maxXSize < sx) {
_maxXSize = 2*sx;
delete[] _x;
_x = new double[_maxXSize];
#ifndef NDEBUG
memset(_x, 0, _maxXSize * sizeof(double));
#endif
if (_b) { // backup the former b, might still be needed for online processing
memcpy(_x, _b, oldSize * sizeof(double));
delete[] _b;
_b = new double[_maxXSize];
std::swap(_b, _x);
} else {
_b = new double[_maxXSize];
#ifndef NDEBUG
memset(_b, 0, _maxXSize * sizeof(double));
#endif
}
}
}
void Solver::setOptimizer(SparseOptimizer* optimizer)
{
_optimizer = optimizer;
}
void Solver::setLevenberg(bool levenberg)
{
_isLevenberg = levenberg;
}
void Solver::setAdditionalVectorSpace(size_t s)
{
_additionalVectorSpace = s;
}
} // end namespace

151
Thirdparty/g2o/g2o/core/solver.h vendored Normal file
View File

@@ -0,0 +1,151 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_SOLVER_H
#define G2O_SOLVER_H
#include "hyper_graph.h"
#include "batch_stats.h"
#include "sparse_block_matrix.h"
#include <cstddef>
namespace g2o {
class SparseOptimizer;
/**
* \brief Generic interface for a sparse solver operating on a graph which solves one iteration of the linearized objective function
*/
class Solver
{
public:
Solver();
virtual ~Solver();
public:
/**
* initialize the solver, called once before the first iteration
*/
virtual bool init(SparseOptimizer* optimizer, bool online = false) = 0;
/**
* build the structure of the system
*/
virtual bool buildStructure(bool zeroBlocks = false) = 0;
/**
* update the structures for online processing
*/
virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges) = 0;
/**
* build the current system
*/
virtual bool buildSystem() = 0;
/**
* solve Ax = b
*/
virtual bool solve() = 0;
/**
* computes the block diagonal elements of the pattern specified in the input
* and stores them in given SparseBlockMatrix
*/
virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices) = 0;
/**
* update the system while performing Levenberg, i.e., modifying the diagonal
* components of A by doing += lambda along the main diagonal of the Matrix.
* Note that this function may be called with a positive and a negative lambda.
* The latter is used to undo a former modification.
* If backup is true, then the solver should store a backup of the diagonal, which
* can be restored by restoreDiagonal()
*/
virtual bool setLambda(double lambda, bool backup = false) = 0;
/**
* restore a previosly made backup of the diagonal
*/
virtual void restoreDiagonal() = 0;
//! return x, the solution vector
double* x() { return _x;}
const double* x() const { return _x;}
//! return b, the right hand side of the system
double* b() { return _b;}
const double* b() const { return _b;}
//! return the size of the solution vector (x) and b
size_t vectorSize() const { return _xSize;}
//! the optimizer (graph) on which the solver works
SparseOptimizer* optimizer() const { return _optimizer;}
void setOptimizer(SparseOptimizer* optimizer);
//! the system is Levenberg-Marquardt
bool levenberg() const { return _isLevenberg;}
void setLevenberg(bool levenberg);
/**
* does this solver support the Schur complement for solving a system consisting of poses and
* landmarks. Re-implemement in a derived solver, if your solver supports it.
*/
virtual bool supportsSchur() {return false;}
//! should the solver perform the schur complement or not
virtual bool schur()=0;
virtual void setSchur(bool s)=0;
size_t additionalVectorSpace() const { return _additionalVectorSpace;}
void setAdditionalVectorSpace(size_t s);
/**
* write debug output of the Hessian if system is not positive definite
*/
virtual void setWriteDebug(bool) = 0;
virtual bool writeDebug() const = 0;
//! write the hessian to disk using the specified file name
virtual bool saveHessian(const std::string& /*fileName*/) const = 0;
protected:
SparseOptimizer* _optimizer;
double* _x;
double* _b;
size_t _xSize, _maxXSize;
bool _isLevenberg; ///< the system we gonna solve is a Levenberg-Marquardt system
size_t _additionalVectorSpace;
void resizeVector(size_t sx);
private:
// Disable the copy constructor and assignment operator
Solver(const Solver&) { }
Solver& operator= (const Solver&) { return *this; }
};
} // end namespace
#endif

231
Thirdparty/g2o/g2o/core/sparse_block_matrix.h vendored Normal file
View File

@@ -0,0 +1,231 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_SPARSE_BLOCK_MATRIX_
#define G2O_SPARSE_BLOCK_MATRIX_
#include <map>
#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <cassert>
#include <Eigen/Core>
#include "sparse_block_matrix_ccs.h"
#include "matrix_structure.h"
#include "matrix_operations.h"
#include "../../config.h"
namespace g2o {
using namespace Eigen;
/**
* \brief Sparse matrix which uses blocks
*
* Template class that specifies a sparse block matrix. A block
* matrix is a sparse matrix made of dense blocks. These blocks
* cannot have a random pattern, but follow a (variable) grid
* structure. This structure is specified by a partition of the rows
* and the columns of the matrix. The blocks are represented by the
* Eigen::Matrix structure, thus they can be statically or dynamically
* allocated. For efficiency reasons it is convenient to allocate them
* statically, when possible. A static block matrix has all blocks of
* the same size, and the size of the block is specified by the
* template argument. If this is not the case, and you have different
* block sizes than you have to use a dynamic-block matrix (default
* template argument).
*/
template <class MatrixType = MatrixXd >
class SparseBlockMatrix {
public:
//! this is the type of the elementary block, it is an Eigen::Matrix.
typedef MatrixType SparseMatrixBlock;
//! columns of the matrix
inline int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
//! rows of the matrix
inline int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
typedef std::map<int, SparseMatrixBlock*> IntBlockMap;
/**
* constructs a sparse block matrix having a specific layout
* @param rbi: array of int containing the row layout of the blocks.
* the component i of the array should contain the index of the first row of the block i+1.
* @param rbi: array of int containing the column layout of the blocks.
* the component i of the array should contain the index of the first col of the block i+1.
* @param rb: number of row blocks
* @param cb: number of col blocks
* @param hasStorage: set it to true if the matrix "owns" the blocks, thus it deletes it on destruction.
* if false the matrix is only a "view" over an existing structure.
*/
SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true);
SparseBlockMatrix();
~SparseBlockMatrix();
//! this zeroes all the blocks. If dealloc=true the blocks are removed from memory
void clear(bool dealloc=false) ;
//! returns the block at location r,c. if alloc=true he block is created if it does not exist
SparseMatrixBlock* block(int r, int c, bool alloc=false);
//! returns the block at location r,c
const SparseMatrixBlock* block(int r, int c) const;
//! how many rows does the block at block-row r has?
inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
//! how many cols does the block at block-col c has?
inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
//! where does the row at block-row r starts?
inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
//! where does the col at block-col r starts?
inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
//! number of non-zero elements
size_t nonZeros() const;
//! number of allocated blocks
size_t nonZeroBlocks() const;
//! deep copy of a sparse-block-matrix;
SparseBlockMatrix* clone() const ;
/**
* returns a view or a copy of the block matrix
* @param rmin: starting block row
* @param rmax: ending block row
* @param cmin: starting block col
* @param cmax: ending block col
* @param alloc: if true it makes a deep copy, if false it creates a view.
*/
SparseBlockMatrix* slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const;
//! transposes a block matrix, The transposed type should match the argument false on failure
template <class MatrixTransposedType>
bool transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const;
//! adds the current matrix to the destination
bool add(SparseBlockMatrix<MatrixType>*& dest) const ;
//! dest = (*this) * M
template <class MatrixResultType, class MatrixFactorType>
bool multiply(SparseBlockMatrix<MatrixResultType> *& dest, const SparseBlockMatrix<MatrixFactorType>* M) const;
//! dest = (*this) * src
void multiply(double*& dest, const double* src) const;
/**
* compute dest = (*this) * src
* However, assuming that this is a symmetric matrix where only the upper triangle is stored
*/
void multiplySymmetricUpperTriangle(double*& dest, const double* src) const;
//! dest = M * (*this)
void rightMultiply(double*& dest, const double* src) const;
//! *this *= a
void scale( double a);
/**
* writes in dest a block permutaton specified by pinv.
* @param pinv: array such that new_block[i] = old_block[pinv[i]]
*/
bool symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool onlyUpper=false) const;
/**
* fill the CCS arrays of a matrix, arrays have to be allocated beforehand
*/
int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const;
/**
* fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes
* the values and assumes that column and row structures have already been written.
*/
int fillCCS(double* Cx, bool upperTriangle = false) const;
//! exports the non zero blocks in the structure matrix ms
void fillBlockStructure(MatrixStructure& ms) const;
//! the block matrices per block-column
const std::vector<IntBlockMap>& blockCols() const { return _blockCols;}
std::vector<IntBlockMap>& blockCols() { return _blockCols;}
//! indices of the row blocks
const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
std::vector<int>& rowBlockIndices() { return _rowBlockIndices;}
//! indices of the column blocks
const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
std::vector<int>& colBlockIndices() { return _colBlockIndices;}
/**
* write the content of this matrix to a stream loadable by Octave
* @param upperTriangle does this matrix store only the upper triangular blocks
*/
bool writeOctave(const char* filename, bool upperTriangle = true) const;
/**
* copy into CCS structure
* @return number of processed blocks, -1 on error
*/
int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
/**
* copy as transposed into a CCS structure
* @return number of processed blocks, -1 on error
*/
int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
/**
* take over the memory and matrix pattern from a hash matrix.
* The structure of the hash matrix will be cleared.
*/
void takePatternFromHash(SparseBlockMatrixHashMap<MatrixType>& hashMatrix);
protected:
std::vector<int> _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
std::vector<int> _colBlockIndices; ///< vector of the indices of the blocks along the cols
//! array of maps of blocks. The index of the array represent a block column of the matrix
//! and the block column is stored as a map row_block -> matrix_block_ptr.
std::vector <IntBlockMap> _blockCols;
bool _hasStorage;
};
template < class MatrixType >
std::ostream& operator << (std::ostream&, const SparseBlockMatrix<MatrixType>& m);
typedef SparseBlockMatrix<MatrixXd> SparseBlockMatrixXd;
} //end namespace
#include "sparse_block_matrix.hpp"
#endif

657
Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp vendored Normal file
View File

@@ -0,0 +1,657 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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.
namespace g2o {
using namespace Eigen;
namespace {
struct TripletEntry
{
int r, c;
double x;
TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {}
};
struct TripletColSort
{
bool operator()(const TripletEntry& e1, const TripletEntry& e2) const
{
return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r);
}
};
/** Helper class to sort pair based on first elem */
template<class T1, class T2, class Pred = std::less<T1> >
struct CmpPairFirst {
bool operator()(const std::pair<T1,T2>& left, const std::pair<T1,T2>& right) {
return Pred()(left.first, right.first);
}
};
}
template <class MatrixType>
SparseBlockMatrix<MatrixType>::SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage):
_rowBlockIndices(rbi,rbi+rb),
_colBlockIndices(cbi,cbi+cb),
_blockCols(cb), _hasStorage(hasStorage)
{
}
template <class MatrixType>
SparseBlockMatrix<MatrixType>::SparseBlockMatrix( ):
_blockCols(0), _hasStorage(true)
{
}
template <class MatrixType>
void SparseBlockMatrix<MatrixType>::clear(bool dealloc) {
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_blockCols.size() > 100)
# endif
for (int i=0; i < static_cast<int>(_blockCols.size()); ++i) {
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
if (_hasStorage && dealloc)
delete b;
else
b->setZero();
}
if (_hasStorage && dealloc)
_blockCols[i].clear();
}
}
template <class MatrixType>
SparseBlockMatrix<MatrixType>::~SparseBlockMatrix(){
if (_hasStorage)
clear(true);
}
template <class MatrixType>
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* SparseBlockMatrix<MatrixType>::block(int r, int c, bool alloc) {
typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator it =_blockCols[c].find(r);
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* _block=0;
if (it==_blockCols[c].end()){
if (!_hasStorage && ! alloc )
return 0;
else {
int rb=rowsOfBlock(r);
int cb=colsOfBlock(c);
_block=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(rb,cb);
_block->setZero();
std::pair < typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator, bool> result
=_blockCols[c].insert(std::make_pair(r,_block)); (void) result;
assert (result.second);
}
} else {
_block=it->second;
}
return _block;
}
template <class MatrixType>
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* SparseBlockMatrix<MatrixType>::block(int r, int c) const {
typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it =_blockCols[c].find(r);
if (it==_blockCols[c].end())
return 0;
return it->second;
}
template <class MatrixType>
SparseBlockMatrix<MatrixType>* SparseBlockMatrix<MatrixType>::clone() const {
SparseBlockMatrix* ret= new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
for (size_t i=0; i<_blockCols.size(); ++i){
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(*it->second);
ret->_blockCols[i].insert(std::make_pair(it->first, b));
}
}
ret->_hasStorage=true;
return ret;
}
template <class MatrixType>
template <class MatrixTransposedType>
bool SparseBlockMatrix<MatrixType>::transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const {
if (! dest){
dest=new SparseBlockMatrix<MatrixTransposedType>(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size());
} else {
if (! dest->_hasStorage)
return false;
if(_rowBlockIndices.size()!=dest->_colBlockIndices.size())
return false;
if (_colBlockIndices.size()!=dest->_rowBlockIndices.size())
return false;
for (size_t i=0; i<_rowBlockIndices.size(); ++i){
if(_rowBlockIndices[i]!=dest->_colBlockIndices[i])
return false;
}
for (size_t i=0; i<_colBlockIndices.size(); ++i){
if(_colBlockIndices[i]!=dest->_rowBlockIndices[i])
return false;
}
}
for (size_t i=0; i<_blockCols.size(); ++i){
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
typename SparseBlockMatrix<MatrixTransposedType>::SparseMatrixBlock* d=dest->block(i,it->first,true);
*d = s->transpose();
}
}
return true;
}
template <class MatrixType>
bool SparseBlockMatrix<MatrixType>::add(SparseBlockMatrix*& dest) const {
if (! dest){
dest=new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
} else {
if (! dest->_hasStorage)
return false;
if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size())
return false;
if (_colBlockIndices.size()!=dest->_colBlockIndices.size())
return false;
for (size_t i=0; i<_rowBlockIndices.size(); ++i){
if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i])
return false;
}
for (size_t i=0; i<_colBlockIndices.size(); ++i){
if(_colBlockIndices[i]!=dest->_colBlockIndices[i])
return false;
}
}
for (size_t i=0; i<_blockCols.size(); ++i){
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* d=dest->block(it->first,i,true);
(*d)+=*s;
}
}
return true;
}
template <class MatrixType>
template < class MatrixResultType, class MatrixFactorType >
bool SparseBlockMatrix<MatrixType>::multiply(SparseBlockMatrix<MatrixResultType>*& dest, const SparseBlockMatrix<MatrixFactorType> * M) const {
// sanity check
if (_colBlockIndices.size()!=M->_rowBlockIndices.size())
return false;
for (size_t i=0; i<_colBlockIndices.size(); ++i){
if (_colBlockIndices[i]!=M->_rowBlockIndices[i])
return false;
}
if (! dest) {
dest=new SparseBlockMatrix<MatrixResultType>(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() );
}
if (! dest->_hasStorage)
return false;
for (size_t i=0; i<M->_blockCols.size(); ++i){
for (typename SparseBlockMatrix<MatrixFactorType>::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){
// look for a non-zero block in a row of column it
int colM=i;
const typename SparseBlockMatrix<MatrixFactorType>::SparseMatrixBlock *b=it->second;
typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator rbt=_blockCols[it->first].begin();
while(rbt!=_blockCols[it->first].end()){
//int colA=it->first;
int rowA=rbt->first;
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock *a=rbt->second;
typename SparseBlockMatrix<MatrixResultType>::SparseMatrixBlock *c=dest->block(rowA,colM,true);
assert (c->rows()==a->rows());
assert (c->cols()==b->cols());
++rbt;
(*c)+=(*a)*(*b);
}
}
}
return false;
}
template <class MatrixType>
void SparseBlockMatrix<MatrixType>::multiply(double*& dest, const double* src) const {
if (! dest){
dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
}
// map the memory by Eigen
Eigen::Map<VectorXd> destVec(dest, rows());
const Eigen::Map<const VectorXd> srcVec(src, cols());
for (size_t i=0; i<_blockCols.size(); ++i){
int srcOffset = i ? _colBlockIndices[i-1] : 0;
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;
// destVec += *a * srcVec (according to the sub-vector parts)
internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
}
}
}
template <class MatrixType>
void SparseBlockMatrix<MatrixType>::multiplySymmetricUpperTriangle(double*& dest, const double* src) const
{
if (! dest){
dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
}
// map the memory by Eigen
Eigen::Map<VectorXd> destVec(dest, rows());
const Eigen::Map<const VectorXd> srcVec(src, cols());
for (size_t i=0; i<_blockCols.size(); ++i){
int srcOffset = colBaseOfBlock(i);
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
int destOffset = rowBaseOfBlock(it->first);
if (destOffset > srcOffset) // only upper triangle
break;
// destVec += *a * srcVec (according to the sub-vector parts)
internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
if (destOffset < srcOffset)
internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset);
}
}
}
template <class MatrixType>
void SparseBlockMatrix<MatrixType>::rightMultiply(double*& dest, const double* src) const {
int destSize=cols();
if (! dest){
dest=new double [ destSize ];
memset(dest,0, destSize*sizeof(double));
}
// map the memory by Eigen
Eigen::Map<VectorXd> destVec(dest, destSize);
Eigen::Map<const VectorXd> srcVec(src, rows());
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
int destOffset = colBaseOfBlock(i);
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin();
it!=_blockCols[i].end();
++it){
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
int srcOffset = rowBaseOfBlock(it->first);
// destVec += *a.transpose() * srcVec (according to the sub-vector parts)
internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
}
}
}
template <class MatrixType>
void SparseBlockMatrix<MatrixType>::scale(double a_) {
for (size_t i=0; i<_blockCols.size(); ++i){
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
*a *= a_;
}
}
}
template <class MatrixType>
SparseBlockMatrix<MatrixType>* SparseBlockMatrix<MatrixType>::slice(int rmin, int rmax, int cmin, int cmax, bool alloc) const {
int m=rmax-rmin;
int n=cmax-cmin;
int rowIdx [m];
rowIdx[0] = rowsOfBlock(rmin);
for (int i=1; i<m; ++i){
rowIdx[i]=rowIdx[i-1]+rowsOfBlock(rmin+i);
}
int colIdx [n];
colIdx[0] = colsOfBlock(cmin);
for (int i=1; i<n; ++i){
colIdx[i]=colIdx[i-1]+colsOfBlock(cmin+i);
}
typename SparseBlockMatrix<MatrixType>::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);
for (int i=0; i<n; ++i){
int mc=cmin+i;
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){
if (it->first >= rmin && it->first < rmax){
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock (* (it->second) ) : it->second;
s->_blockCols[i].insert(std::make_pair(it->first-rmin, b));
}
}
}
s->_hasStorage=alloc;
return s;
}
template <class MatrixType>
size_t SparseBlockMatrix<MatrixType>::nonZeroBlocks() const {
size_t count=0;
for (size_t i=0; i<_blockCols.size(); ++i)
count+=_blockCols[i].size();
return count;
}
template <class MatrixType>
size_t SparseBlockMatrix<MatrixType>::nonZeros() const{
if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) {
size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime;
return nnz;
} else {
size_t count=0;
for (size_t i=0; i<_blockCols.size(); ++i){
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
count += a->cols()*a->rows();
}
}
return count;
}
}
template <class MatrixType>
std::ostream& operator << (std::ostream& os, const SparseBlockMatrix<MatrixType>& m){
os << "RBI: " << m.rowBlockIndices().size();
for (size_t i=0; i<m.rowBlockIndices().size(); ++i)
os << " " << m.rowBlockIndices()[i];
os << std::endl;
os << "CBI: " << m.colBlockIndices().size();
for (size_t i=0; i<m.colBlockIndices().size(); ++i)
os << " " << m.colBlockIndices()[i];
os << std::endl;
for (size_t i=0; i<m.blockCols().size(); ++i){
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
os << "BLOCK: " << it->first << " " << i << std::endl;
os << *b << std::endl;
}
}
return os;
}
template <class MatrixType>
bool SparseBlockMatrix<MatrixType>::symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool upperTriangle) const{
// compute the permuted version of the new row/column layout
size_t n=_rowBlockIndices.size();
// computed the block sizes
int blockSizes[_rowBlockIndices.size()];
blockSizes[0]=_rowBlockIndices[0];
for (size_t i=1; i<n; ++i){
blockSizes[i]=_rowBlockIndices[i]-_rowBlockIndices[i-1];
}
// permute them
int pBlockIndices[_rowBlockIndices.size()];
for (size_t i=0; i<n; ++i){
pBlockIndices[pinv[i]]=blockSizes[i];
}
for (size_t i=1; i<n; ++i){
pBlockIndices[i]+=pBlockIndices[i-1];
}
// allocate C, or check the structure;
if (! dest){
dest=new SparseBlockMatrix(pBlockIndices, pBlockIndices, n, n);
} else {
if (dest->_rowBlockIndices.size()!=n)
return false;
if (dest->_colBlockIndices.size()!=n)
return false;
for (size_t i=0; i<n; ++i){
if (dest->_rowBlockIndices[i]!=pBlockIndices[i])
return false;
if (dest->_colBlockIndices[i]!=pBlockIndices[i])
return false;
}
dest->clear();
}
// now ready to permute the columns
for (size_t i=0; i<n; ++i){
//cerr << PVAR(i) << " ";
int pi=pinv[i];
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin();
it!=_blockCols[i].end(); ++it){
int pj=pinv[it->first];
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=0;
if (! upperTriangle || pj<=pi) {
b=dest->block(pj,pi,true);
assert(b->cols()==s->cols());
assert(b->rows()==s->rows());
*b=*s;
} else {
b=dest->block(pi,pj,true);
assert(b);
assert(b->rows()==s->cols());
assert(b->cols()==s->rows());
*b=s->transpose();
}
}
//cerr << endl;
// within each row,
}
return true;
}
template <class MatrixType>
int SparseBlockMatrix<MatrixType>::fillCCS(double* Cx, bool upperTriangle) const
{
assert(Cx && "Target destination is NULL");
double* CxStart = Cx;
for (size_t i=0; i<_blockCols.size(); ++i){
int cstart=i ? _colBlockIndices[i-1] : 0;
int csize=colsOfBlock(i);
for (int c=0; c<csize; ++c) {
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
int elemsToCopy = b->rows();
if (upperTriangle && rstart == cstart)
elemsToCopy = c + 1;
memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
Cx += elemsToCopy;
}
}
}
return Cx - CxStart;
}
template <class MatrixType>
int SparseBlockMatrix<MatrixType>::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const
{
assert(Cp && Ci && Cx && "Target destination is NULL");
int nz=0;
for (size_t i=0; i<_blockCols.size(); ++i){
int cstart=i ? _colBlockIndices[i-1] : 0;
int csize=colsOfBlock(i);
for (int c=0; c<csize; ++c) {
*Cp=nz;
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
int elemsToCopy = b->rows();
if (upperTriangle && rstart == cstart)
elemsToCopy = c + 1;
for (int r=0; r<elemsToCopy; ++r){
*Cx++ = (*b)(r,c);
*Ci++ = rstart++;
++nz;
}
}
++Cp;
}
}
*Cp=nz;
return nz;
}
template <class MatrixType>
void SparseBlockMatrix<MatrixType>::fillBlockStructure(MatrixStructure& ms) const
{
int n = _colBlockIndices.size();
int nzMax = (int)nonZeroBlocks();
ms.alloc(n, nzMax);
ms.m = _rowBlockIndices.size();
int nz = 0;
int* Cp = ms.Ap;
int* Ci = ms.Aii;
for (int i = 0; i < static_cast<int>(_blockCols.size()); ++i){
*Cp = nz;
const int& c = i;
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
const int& r = it->first;
if (r <= c) {
*Ci++ = r;
++nz;
}
}
Cp++;
}
*Cp=nz;
assert(nz <= nzMax);
}
template <class MatrixType>
bool SparseBlockMatrix<MatrixType>::writeOctave(const char* filename, bool upperTriangle) const
{
std::string name = filename;
std::string::size_type lastDot = name.find_last_of('.');
if (lastDot != std::string::npos)
name = name.substr(0, lastDot);
std::vector<TripletEntry> entries;
for (size_t i = 0; i<_blockCols.size(); ++i){
const int& c = i;
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
const int& r = it->first;
const MatrixType& m = *(it->second);
for (int cc = 0; cc < m.cols(); ++cc)
for (int rr = 0; rr < m.rows(); ++rr) {
int aux_r = rowBaseOfBlock(r) + rr;
int aux_c = colBaseOfBlock(c) + cc;
entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc)));
if (upperTriangle && r != c) {
entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc)));
}
}
}
}
int nz = entries.size();
std::sort(entries.begin(), entries.end(), TripletColSort());
std::ofstream fout(filename);
fout << "# name: " << name << std::endl;
fout << "# type: sparse matrix" << std::endl;
fout << "# nnz: " << nz << std::endl;
fout << "# rows: " << rows() << std::endl;
fout << "# columns: " << cols() << std::endl;
fout << std::setprecision(9) << std::fixed << std::endl;
for (std::vector<TripletEntry>::const_iterator it = entries.begin(); it != entries.end(); ++it) {
const TripletEntry& entry = *it;
fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl;
}
return fout.good();
}
template <class MatrixType>
int SparseBlockMatrix<MatrixType>::fillSparseBlockMatrixCCS(SparseBlockMatrixCCS<MatrixType>& blockCCS) const
{
blockCCS.blockCols().resize(blockCols().size());
int numblocks = 0;
for (size_t i = 0; i < blockCols().size(); ++i) {
const IntBlockMap& row = blockCols()[i];
typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[i];
dest.clear();
dest.reserve(row.size());
for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(it->first, it->second));
++numblocks;
}
}
return numblocks;
}
template <class MatrixType>
int SparseBlockMatrix<MatrixType>::fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS<MatrixType>& blockCCS) const
{
blockCCS.blockCols().clear();
blockCCS.blockCols().resize(_rowBlockIndices.size());
int numblocks = 0;
for (size_t i = 0; i < blockCols().size(); ++i) {
const IntBlockMap& row = blockCols()[i];
for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[it->first];
dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(i, it->second));
++numblocks;
}
}
return numblocks;
}
template <class MatrixType>
void SparseBlockMatrix<MatrixType>::takePatternFromHash(SparseBlockMatrixHashMap<MatrixType>& hashMatrix)
{
// sort the sparse columns and add them to the map structures by
// exploiting that we are inserting a sorted structure
typedef std::pair<int, MatrixType*> SparseColumnPair;
typedef typename SparseBlockMatrixHashMap<MatrixType>::SparseColumn HashSparseColumn;
for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) {
// prepare a temporary vector for sorting
HashSparseColumn& column = hashMatrix.blockCols()[i];
if (column.size() == 0)
continue;
std::vector<SparseColumnPair> sparseRowSorted; // temporary structure
sparseRowSorted.reserve(column.size());
for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it)
sparseRowSorted.push_back(*it);
std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst<int, MatrixType*>());
// try to free some memory early
HashSparseColumn aux;
swap(aux, column);
// now insert sorted vector to the std::map structure
IntBlockMap& destColumnMap = blockCols()[i];
destColumnMap.insert(sparseRowSorted[0]);
for (size_t j = 1; j < sparseRowSorted.size(); ++j) {
typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator hint = destColumnMap.end();
--hint; // cppreference says the element goes after the hint (until C++11)
destColumnMap.insert(hint, sparseRowSorted[j]);
}
}
}
}// end namespace

282
Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h vendored Normal file
View File

@@ -0,0 +1,282 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_SPARSE_BLOCK_MATRIX_CCS_H
#define G2O_SPARSE_BLOCK_MATRIX_CCS_H
#include <vector>
#include <cassert>
#include <Eigen/Core>
#include "../../config.h"
#include "matrix_operations.h"
#ifdef _MSC_VER
#include <unordered_map>
#else
#include <tr1/unordered_map>
#endif
namespace g2o {
/**
* \brief Sparse matrix which uses blocks
*
* This class is used as a const view on a SparseBlockMatrix
* which allows a faster iteration over the elements of the
* matrix.
*/
template <class MatrixType>
class SparseBlockMatrixCCS
{
public:
//! this is the type of the elementary block, it is an Eigen::Matrix.
typedef MatrixType SparseMatrixBlock;
//! columns of the matrix
int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
//! rows of the matrix
int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
/**
* \brief A block within a column
*/
struct RowBlock
{
int row; ///< row of the block
MatrixType* block; ///< matrix pointer for the block
RowBlock() : row(-1), block(0) {}
RowBlock(int r, MatrixType* b) : row(r), block(b) {}
bool operator<(const RowBlock& other) const { return row < other.row;}
};
typedef std::vector<RowBlock> SparseColumn;
SparseBlockMatrixCCS(const std::vector<int>& rowIndices, const std::vector<int>& colIndices) :
_rowBlockIndices(rowIndices), _colBlockIndices(colIndices)
{}
//! how many rows does the block at block-row r has?
int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
//! how many cols does the block at block-col c has?
int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
//! where does the row at block-row r start?
int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
//! where does the col at block-col r start?
int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
//! the block matrices per block-column
const std::vector<SparseColumn>& blockCols() const { return _blockCols;}
std::vector<SparseColumn>& blockCols() { return _blockCols;}
//! indices of the row blocks
const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
//! indices of the column blocks
const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
void rightMultiply(double*& dest, const double* src) const
{
int destSize=cols();
if (! dest){
dest=new double [ destSize ];
memset(dest,0, destSize*sizeof(double));
}
// map the memory by Eigen
Eigen::Map<Eigen::VectorXd> destVec(dest, destSize);
Eigen::Map<const Eigen::VectorXd> srcVec(src, rows());
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
int destOffset = colBaseOfBlock(i);
for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
const SparseMatrixBlock* a = it->block;
int srcOffset = rowBaseOfBlock(it->row);
// destVec += *a.transpose() * srcVec (according to the sub-vector parts)
internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
}
}
}
/**
* sort the blocks in each column
*/
void sortColumns()
{
for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
std::sort(_blockCols[i].begin(), _blockCols[i].end());
}
}
/**
* fill the CCS arrays of a matrix, arrays have to be allocated beforehand
*/
int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const
{
assert(Cp && Ci && Cx && "Target destination is NULL");
int nz=0;
for (size_t i=0; i<_blockCols.size(); ++i){
int cstart=i ? _colBlockIndices[i-1] : 0;
int csize=colsOfBlock(i);
for (int c=0; c<csize; ++c) {
*Cp=nz;
for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
const SparseMatrixBlock* b=it->block;
int rstart=it->row ? _rowBlockIndices[it->row-1] : 0;
int elemsToCopy = b->rows();
if (upperTriangle && rstart == cstart)
elemsToCopy = c + 1;
for (int r=0; r<elemsToCopy; ++r){
*Cx++ = (*b)(r,c);
*Ci++ = rstart++;
++nz;
}
}
++Cp;
}
}
*Cp=nz;
return nz;
}
/**
* fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes
* the values and assumes that column and row structures have already been written.
*/
int fillCCS(double* Cx, bool upperTriangle = false) const
{
assert(Cx && "Target destination is NULL");
double* CxStart = Cx;
int cstart = 0;
for (size_t i=0; i<_blockCols.size(); ++i){
int csize = _colBlockIndices[i] - cstart;
for (int c=0; c<csize; ++c) {
for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
const SparseMatrixBlock* b = it->block;
int rstart = it->row ? _rowBlockIndices[it->row-1] : 0;
int elemsToCopy = b->rows();
if (upperTriangle && rstart == cstart)
elemsToCopy = c + 1;
memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
Cx += elemsToCopy;
}
}
cstart = _colBlockIndices[i];
}
return Cx - CxStart;
}
protected:
const std::vector<int>& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
const std::vector<int>& _colBlockIndices; ///< vector of the indices of the blocks along the cols
std::vector<SparseColumn> _blockCols; ///< the matrices stored in CCS order
};
/**
* \brief Sparse matrix which uses blocks based on hash structures
*
* This class is used to construct the pattern of a sparse block matrix
*/
template <class MatrixType>
class SparseBlockMatrixHashMap
{
public:
//! this is the type of the elementary block, it is an Eigen::Matrix.
typedef MatrixType SparseMatrixBlock;
//! columns of the matrix
int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
//! rows of the matrix
int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
typedef std::tr1::unordered_map<int, MatrixType*> SparseColumn;
SparseBlockMatrixHashMap(const std::vector<int>& rowIndices, const std::vector<int>& colIndices) :
_rowBlockIndices(rowIndices), _colBlockIndices(colIndices)
{}
//! how many rows does the block at block-row r has?
int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
//! how many cols does the block at block-col c has?
int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
//! where does the row at block-row r start?
int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
//! where does the col at block-col r start?
int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
//! the block matrices per block-column
const std::vector<SparseColumn>& blockCols() const { return _blockCols;}
std::vector<SparseColumn>& blockCols() { return _blockCols;}
//! indices of the row blocks
const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
//! indices of the column blocks
const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
/**
* add a block to the pattern, return a pointer to the added block
*/
MatrixType* addBlock(int r, int c, bool zeroBlock = false)
{
assert(c <(int)_blockCols.size() && "accessing column which is not available");
SparseColumn& sparseColumn = _blockCols[c];
typename SparseColumn::iterator foundIt = sparseColumn.find(r);
if (foundIt == sparseColumn.end()) {
int rb = rowsOfBlock(r);
int cb = colsOfBlock(c);
MatrixType* m = new MatrixType(rb, cb);
if (zeroBlock)
m->setZero();
sparseColumn[r] = m;
return m;
}
return foundIt->second;
}
protected:
const std::vector<int>& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
const std::vector<int>& _colBlockIndices; ///< vector of the indices of the blocks along the cols
std::vector<SparseColumn> _blockCols; ///< the matrices stored in CCS order
};
} //end namespace
#endif

108
Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h vendored Normal file
View File

@@ -0,0 +1,108 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
#define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
#include <vector>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include "../../config.h"
#include "matrix_operations.h"
namespace g2o {
/**
* \brief Sparse matrix which uses blocks on the diagonal
*
* This class is used as a const view on a SparseBlockMatrix
* which allows a faster iteration over the elements of the
* matrix.
*/
template <class MatrixType>
class SparseBlockMatrixDiagonal
{
public:
//! this is the type of the elementary block, it is an Eigen::Matrix.
typedef MatrixType SparseMatrixBlock;
//! columns of the matrix
int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
//! rows of the matrix
int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
typedef std::vector<MatrixType, Eigen::aligned_allocator<MatrixType> > DiagonalVector;
SparseBlockMatrixDiagonal(const std::vector<int>& blockIndices) :
_blockIndices(blockIndices)
{}
//! how many rows/cols does the block at block-row / block-column r has?
inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; }
//! where does the row /col at block-row / block-column r starts?
inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; }
//! the block matrices per block-column
const DiagonalVector& diagonal() const { return _diagonal;}
DiagonalVector& diagonal() { return _diagonal;}
//! indices of the row blocks
const std::vector<int>& blockIndices() const { return _blockIndices;}
void multiply(double*& dest, const double* src) const
{
int destSize=cols();
if (! dest) {
dest=new double[destSize];
memset(dest,0, destSize*sizeof(double));
}
// map the memory by Eigen
Eigen::Map<Eigen::VectorXd> destVec(dest, destSize);
Eigen::Map<const Eigen::VectorXd> srcVec(src, rows());
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int i=0; i < static_cast<int>(_diagonal.size()); ++i){
int destOffset = baseOfBlock(i);
int srcOffset = destOffset;
const SparseMatrixBlock& A = _diagonal[i];
// destVec += *A.transpose() * srcVec (according to the sub-vector parts)
internal::axpy(A, srcVec, srcOffset, destVec, destOffset);
}
}
protected:
const std::vector<int>& _blockIndices; ///< vector of the indices of the blocks along the diagonal
DiagonalVector _diagonal;
};
} //end namespace
#endif

107
Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp vendored Normal file
View File

@@ -0,0 +1,107 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "sparse_block_matrix.h"
#include <iostream>
using namespace std;
using namespace g2o;
using namespace Eigen;
typedef SparseBlockMatrix< MatrixXd >
SparseBlockMatrixX;
std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) {
for (int i=0; i<m.rows(); ++i){
for (int j=0; j<m.cols(); ++j)
cerr << m(i,j) << " ";
cerr << endl;
}
return os;
}
int main (int argc, char** argv){
int rcol[] = {3,6,8,12};
int ccol[] = {2,4,13};
cerr << "creation" << endl;
SparseBlockMatrixX* M=new SparseBlockMatrixX(rcol, ccol, 4,3);
cerr << "block access" << endl;
SparseBlockMatrixX::SparseMatrixBlock* b=M->block(0,0, true);
cerr << b->rows() << " " << b->cols() << endl;
for (int i=0; i<b->rows(); ++i)
for (int j=0; j<b->cols(); ++j){
(*b)(i,j)=i*b->cols()+j;
}
cerr << "block access 2" << endl;
b=M->block(0,2, true);
cerr << b->rows() << " " << b->cols() << endl;
for (int i=0; i<b->rows(); ++i)
for (int j=0; j<b->cols(); ++j){
(*b)(i,j)=i*b->cols()+j;
}
b=M->block(3,2, true);
cerr << b->rows() << " " << b->cols() << endl;
for (int i=0; i<b->rows(); ++i)
for (int j=0; j<b->cols(); ++j){
(*b)(i,j)=i*b->cols()+j;
}
cerr << *M << endl;
cerr << "SUM" << endl;
SparseBlockMatrixX* Ms=0;
M->add(Ms);
M->add(Ms);
cerr << *Ms;
SparseBlockMatrixX* Mt=0;
M->transpose(Mt);
cerr << *Mt << endl;
SparseBlockMatrixX* Mp=0;
M->multiply(Mp, Mt);
cerr << *Mp << endl;
int iperm[]={3,2,1,0};
SparseBlockMatrixX* PMp=0;
Mp->symmPermutation(PMp,iperm, false);
cerr << *PMp << endl;
PMp->clear(true);
Mp->block(3,0)->fill(0.);
Mp->symmPermutation(PMp,iperm, true);
cerr << *PMp << endl;
}

615
Thirdparty/g2o/g2o/core/sparse_optimizer.cpp vendored Normal file
View File

@@ -0,0 +1,615 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "sparse_optimizer.h"
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <iterator>
#include <cassert>
#include <algorithm>
#include "estimate_propagator.h"
#include "optimization_algorithm.h"
#include "batch_stats.h"
#include "hyper_graph_action.h"
#include "robust_kernel.h"
#include "../stuff/timeutil.h"
#include "../stuff/macros.h"
#include "../stuff/misc.h"
#include "../../config.h"
namespace g2o{
using namespace std;
SparseOptimizer::SparseOptimizer() :
_forceStopFlag(0), _verbose(false), _algorithm(0), _computeBatchStatistics(false)
{
_graphActions.resize(AT_NUM_ELEMENTS);
}
SparseOptimizer::~SparseOptimizer(){
delete _algorithm;
G2OBatchStatistics::setGlobalStats(0);
}
void SparseOptimizer::computeActiveErrors()
{
// call the callbacks in case there is something registered
HyperGraphActionSet& actions = _graphActions[AT_COMPUTEACTIVERROR];
if (actions.size() > 0) {
for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it)
(*(*it))(this);
}
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_activeEdges.size() > 50)
# endif
for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
OptimizableGraph::Edge* e = _activeEdges[k];
e->computeError();
}
# ifndef NDEBUG
for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
OptimizableGraph::Edge* e = _activeEdges[k];
bool hasNan = arrayHasNaN(e->errorData(), e->dimension());
if (hasNan) {
cerr << "computeActiveErrors(): found NaN in error for edge " << e << endl;
}
}
# endif
}
double SparseOptimizer::activeChi2( ) const
{
double chi = 0.0;
for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
const OptimizableGraph::Edge* e = *it;
chi += e->chi2();
}
return chi;
}
double SparseOptimizer::activeRobustChi2() const
{
Eigen::Vector3d rho;
double chi = 0.0;
for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
const OptimizableGraph::Edge* e = *it;
if (e->robustKernel()) {
e->robustKernel()->robustify(e->chi2(), rho);
chi += rho[0];
}
else
chi += e->chi2();
}
return chi;
}
OptimizableGraph::Vertex* SparseOptimizer::findGauge(){
if (vertices().empty())
return 0;
int maxDim=0;
for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
maxDim=std::max(maxDim,v->dimension());
}
OptimizableGraph::Vertex* rut=0;
for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension()==maxDim){
rut=v;
break;
}
}
return rut;
}
bool SparseOptimizer::gaugeFreedom()
{
if (vertices().empty())
return false;
int maxDim=0;
for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
maxDim = std::max(maxDim,v->dimension());
}
for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() == maxDim) {
// test for fixed vertex
if (v->fixed()) {
return false;
}
// test for full dimension prior
for (HyperGraph::EdgeSet::const_iterator eit = v->edges().begin(); eit != v->edges().end(); ++eit) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*eit);
if (e->vertices().size() == 1 && e->dimension() == maxDim)
return false;
}
}
}
return true;
}
bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){
if (! vlist.size()){
_ivMap.clear();
return false;
}
_ivMap.resize(vlist.size());
size_t i = 0;
for (int k=0; k<2; k++)
for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){
OptimizableGraph::Vertex* v = *it;
if (! v->fixed()){
if (static_cast<int>(v->marginalized()) == k){
v->setHessianIndex(i);
_ivMap[i]=v;
i++;
}
}
else {
v->setHessianIndex(-1);
}
}
_ivMap.resize(i);
return true;
}
void SparseOptimizer::clearIndexMapping(){
for (size_t i=0; i<_ivMap.size(); ++i){
_ivMap[i]->setHessianIndex(-1);
_ivMap[i]=0;
}
}
bool SparseOptimizer::initializeOptimization(int level){
HyperGraph::VertexSet vset;
for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it)
vset.insert(it->second);
return initializeOptimization(vset,level);
}
bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
if (edges().size() == 0) {
cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
return false;
}
bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
clearIndexMapping();
_activeVertices.clear();
_activeVertices.reserve(vset.size());
_activeEdges.clear();
set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
// count if there are edges in that level. If not remove from the pool
int levelEdges=0;
for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
OptimizableGraph::Edge* e=reinterpret_cast<OptimizableGraph::Edge*>(*it);
if (level < 0 || e->level() == level) {
bool allVerticesOK = true;
for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
if (vset.find(*vit) == vset.end()) {
allVerticesOK = false;
break;
}
}
if (allVerticesOK && !e->allVerticesFixed()) {
auxEdgeSet.insert(e);
levelEdges++;
}
}
}
if (levelEdges){
_activeVertices.push_back(v);
// test for NANs in the current estimate if we are debugging
# ifndef NDEBUG
int estimateDim = v->estimateDimension();
if (estimateDim > 0) {
Eigen::VectorXd estimateData(estimateDim);
if (v->getEstimateData(estimateData.data()) == true) {
int k;
bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
if (hasNan)
cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
}
}
# endif
}
}
_activeEdges.reserve(auxEdgeSet.size());
for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
_activeEdges.push_back(*it);
sortVectorContainers();
return buildIndexMapping(_activeVertices);
}
bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset){
bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
clearIndexMapping();
_activeVertices.clear();
_activeEdges.clear();
_activeEdges.reserve(eset.size());
set<Vertex*> auxVertexSet; // temporary structure to avoid duplicates
for (HyperGraph::EdgeSet::iterator it=eset.begin(); it!=eset.end(); ++it){
OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it);
for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
auxVertexSet.insert(static_cast<OptimizableGraph::Vertex*>(*vit));
}
_activeEdges.push_back(reinterpret_cast<OptimizableGraph::Edge*>(*it));
}
_activeVertices.reserve(auxVertexSet.size());
for (set<Vertex*>::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it)
_activeVertices.push_back(*it);
sortVectorContainers();
return buildIndexMapping(_activeVertices);
}
void SparseOptimizer::setToOrigin(){
for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
v->setToOrigin();
}
}
void SparseOptimizer::computeInitialGuess()
{
EstimatePropagator::PropagateCost costFunction(this);
computeInitialGuess(costFunction);
}
void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction)
{
OptimizableGraph::VertexSet emptySet;
std::set<Vertex*> backupVertices;
HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization
for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
OptimizableGraph::Edge* e = *it;
for (size_t i = 0; i < e->vertices().size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertex(i));
if (v->fixed())
fixedVertices.insert(v);
else { // check for having a prior which is able to fully initialize a vertex
for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) {
OptimizableGraph::Edge* vedge = static_cast<OptimizableGraph::Edge*>(*vedgeIt);
if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) {
//cerr << "Initialize with prior for " << v->id() << endl;
vedge->initialEstimate(emptySet, v);
fixedVertices.insert(v);
}
}
}
if (v->hessianIndex() == -1) {
std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v);
if (foundIt == backupVertices.end()) {
v->push();
backupVertices.insert(v);
}
}
}
}
EstimatePropagator estimatePropagator(this);
estimatePropagator.propagate(fixedVertices, costFunction);
// restoring the vertices that should not be initialized
for (std::set<Vertex*>::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) {
Vertex* v = *it;
v->pop();
}
if (verbose()) {
computeActiveErrors();
cerr << "iteration= -1\t chi2= " << activeChi2()
<< "\t time= 0.0"
<< "\t cumTime= 0.0"
<< "\t (using initial guess from " << costFunction.name() << ")" << endl;
}
}
int SparseOptimizer::optimize(int iterations, bool online)
{
if (_ivMap.size() == 0) {
cerr << __PRETTY_FUNCTION__ << ": 0 vertices to optimize, maybe forgot to call initializeOptimization()" << endl;
return -1;
}
int cjIterations=0;
double cumTime=0;
bool ok=true;
ok = _algorithm->init(online);
if (! ok) {
cerr << __PRETTY_FUNCTION__ << " Error while initializing" << endl;
return -1;
}
_batchStatistics.clear();
if (_computeBatchStatistics)
_batchStatistics.resize(iterations);
OptimizationAlgorithm::SolverResult result = OptimizationAlgorithm::OK;
for (int i=0; i<iterations && ! terminate() && ok; i++){
preIteration(i);
if (_computeBatchStatistics) {
G2OBatchStatistics& cstat = _batchStatistics[i];
G2OBatchStatistics::setGlobalStats(&cstat);
cstat.iteration = i;
cstat.numEdges = _activeEdges.size();
cstat.numVertices = _activeVertices.size();
}
double ts = get_monotonic_time();
result = _algorithm->solve(i, online);
ok = ( result == OptimizationAlgorithm::OK );
bool errorComputed = false;
if (_computeBatchStatistics) {
computeActiveErrors();
errorComputed = true;
_batchStatistics[i].chi2 = activeRobustChi2();
_batchStatistics[i].timeIteration = get_monotonic_time()-ts;
}
if (verbose()){
double dts = get_monotonic_time()-ts;
cumTime += dts;
if (! errorComputed)
computeActiveErrors();
cerr << "iteration= " << i
<< "\t chi2= " << FIXED(activeRobustChi2())
<< "\t time= " << dts
<< "\t cumTime= " << cumTime
<< "\t edges= " << _activeEdges.size();
_algorithm->printVerbose(cerr);
cerr << endl;
}
++cjIterations;
postIteration(i);
}
if (result == OptimizationAlgorithm::Fail) {
return 0;
}
return cjIterations;
}
void SparseOptimizer::update(const double* update)
{
// update the graph by calling oplus on the vertices
for (size_t i=0; i < _ivMap.size(); ++i) {
OptimizableGraph::Vertex* v= _ivMap[i];
#ifndef NDEBUG
bool hasNan = arrayHasNaN(update, v->dimension());
if (hasNan)
cerr << __PRETTY_FUNCTION__ << ": Update contains a nan for vertex " << v->id() << endl;
#endif
v->oplus(update);
update += v->dimension();
}
}
void SparseOptimizer::setComputeBatchStatistics(bool computeBatchStatistics)
{
if ((_computeBatchStatistics == true) && (computeBatchStatistics == false)) {
G2OBatchStatistics::setGlobalStats(0);
_batchStatistics.clear();
}
_computeBatchStatistics = computeBatchStatistics;
}
bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)
{
std::vector<HyperGraph::Vertex*> newVertices;
newVertices.reserve(vset.size());
_activeVertices.reserve(_activeVertices.size() + vset.size());
_activeEdges.reserve(_activeEdges.size() + eset.size());
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
if (!e->allVerticesFixed()) _activeEdges.push_back(e);
}
// update the index mapping
size_t next = _ivMap.size();
for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(*it);
if (! v->fixed()){
if (! v->marginalized()){
v->setHessianIndex(next);
_ivMap.push_back(v);
newVertices.push_back(v);
_activeVertices.push_back(v);
next++;
}
else // not supported right now
abort();
}
else {
v->setHessianIndex(-1);
}
}
//if (newVertices.size() != vset.size())
//cerr << __PRETTY_FUNCTION__ << ": something went wrong " << PVAR(vset.size()) << " " << PVAR(newVertices.size()) << endl;
return _algorithm->updateStructure(newVertices, eset);
}
void SparseOptimizer::sortVectorContainers()
{
// sort vector structures to get deterministic ordering based on IDs
sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare());
sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare());
}
void SparseOptimizer::clear() {
_ivMap.clear();
_activeVertices.clear();
_activeEdges.clear();
OptimizableGraph::clear();
}
SparseOptimizer::VertexContainer::const_iterator SparseOptimizer::findActiveVertex(const OptimizableGraph::Vertex* v) const
{
VertexContainer::const_iterator lower = lower_bound(_activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare());
if (lower == _activeVertices.end())
return _activeVertices.end();
if ((*lower) == v)
return lower;
return _activeVertices.end();
}
SparseOptimizer::EdgeContainer::const_iterator SparseOptimizer::findActiveEdge(const OptimizableGraph::Edge* e) const
{
EdgeContainer::const_iterator lower = lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare());
if (lower == _activeEdges.end())
return _activeEdges.end();
if ((*lower) == e)
return lower;
return _activeEdges.end();
}
void SparseOptimizer::push(SparseOptimizer::VertexContainer& vlist)
{
for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
(*it)->push();
}
void SparseOptimizer::pop(SparseOptimizer::VertexContainer& vlist)
{
for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
(*it)->pop();
}
void SparseOptimizer::push(HyperGraph::VertexSet& vlist)
{
for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) {
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
if (v)
v->push();
else
cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl;
}
}
void SparseOptimizer::pop(HyperGraph::VertexSet& vlist)
{
for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*> (*it);
if (v)
v->pop();
else
cerr << __FUNCTION__ << ": FATAL POP SET" << endl;
}
}
void SparseOptimizer::discardTop(SparseOptimizer::VertexContainer& vlist)
{
for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
(*it)->discardTop();
}
void SparseOptimizer::setVerbose(bool verbose)
{
_verbose = verbose;
}
void SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
{
if (_algorithm) // reset the optimizer for the formerly used solver
_algorithm->setOptimizer(0);
_algorithm = algorithm;
if (_algorithm)
_algorithm->setOptimizer(this);
}
bool SparseOptimizer::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices){
return _algorithm->computeMarginals(spinv, blockIndices);
}
void SparseOptimizer::setForceStopFlag(bool* flag)
{
_forceStopFlag=flag;
}
bool SparseOptimizer::removeVertex(HyperGraph::Vertex* v)
{
OptimizableGraph::Vertex* vv = static_cast<OptimizableGraph::Vertex*>(v);
if (vv->hessianIndex() >= 0) {
clearIndexMapping();
_ivMap.clear();
}
return HyperGraph::removeVertex(v);
}
bool SparseOptimizer::addComputeErrorAction(HyperGraphAction* action)
{
std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_COMPUTEACTIVERROR].insert(action);
return insertResult.second;
}
bool SparseOptimizer::removeComputeErrorAction(HyperGraphAction* action)
{
return _graphActions[AT_COMPUTEACTIVERROR].erase(action) > 0;
}
void SparseOptimizer::push()
{
push(_activeVertices);
}
void SparseOptimizer::pop()
{
pop(_activeVertices);
}
void SparseOptimizer::discardTop()
{
discardTop(_activeVertices);
}
} // end namespace

312
Thirdparty/g2o/g2o/core/sparse_optimizer.h vendored Normal file
View File

@@ -0,0 +1,312 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_GRAPH_OPTIMIZER_CHOL_H_
#define G2O_GRAPH_OPTIMIZER_CHOL_H_
#include "../stuff/macros.h"
#include "optimizable_graph.h"
#include "sparse_block_matrix.h"
#include "batch_stats.h"
#include <map>
namespace g2o {
// forward declaration
class ActivePathCostFunction;
class OptimizationAlgorithm;
class EstimatePropagatorCost;
class SparseOptimizer : public OptimizableGraph {
public:
enum {
AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS,
AT_NUM_ELEMENTS, // keep as last element
};
friend class ActivePathCostFunction;
// Attention: _solver & _statistics is own by SparseOptimizer and will be
// deleted in its destructor.
SparseOptimizer();
virtual ~SparseOptimizer();
// new interface for the optimizer
// the old functions will be dropped
/**
* Initializes the structures for optimizing a portion of the graph specified by a subset of edges.
* Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the
* schur complement or to set as fixed during the optimization.
* @param eset: the subgraph to be optimized.
* @returns false if somethings goes wrong
*/
virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);
/**
* Initializes the structures for optimizing a portion of the graph specified by a subset of vertices.
* Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the
* schur complement or to set as fixed during the optimization.
* @param vset: the subgraph to be optimized.
* @param level: is the level (in multilevel optimization)
* @returns false if somethings goes wrong
*/
virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0);
/**
* Initializes the structures for optimizing the whole graph.
* Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the
* schur complement or to set as fixed during the optimization.
* @param level: is the level (in multilevel optimization)
* @returns false if somethings goes wrong
*/
virtual bool initializeOptimization(int level=0);
/**
* HACK updating the internal structures for online processing
*/
virtual bool updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset);
/**
* Propagates an initial guess from the vertex specified as origin.
* It should be called after initializeOptimization(...), as it relies on the _activeVertices/_edges structures.
* It constructs a set of trees starting from the nodes in the graph which are fixed and eligible for preforming optimization.
* The trees are constructed by utlizing a cost-function specified.
* @param costFunction: the cost function used
* @patam maxDistance: the distance where to stop the search
*/
virtual void computeInitialGuess();
/**
* Same as above but using a specific propagator
*/
virtual void computeInitialGuess(EstimatePropagatorCost& propagator);
/**
* sets all vertices to their origin.
*/
virtual void setToOrigin();
/**
* starts one optimization run given the current configuration of the graph,
* and the current settings stored in the class instance.
* It can be called only after initializeOptimization
*/
int optimize(int iterations, bool online = false);
/**
* computes the blocks of the inverse of the specified pattern.
* the pattern is given via pairs <row, col> of the blocks in the hessian
* @param blockIndices: the pattern
* @param spinv: the sparse block matrix with the result
* @returns false if the operation is not supported by the solver
*/
bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
/**
* computes the inverse of the specified vertex.
* @param vertex: the vertex whose state is to be marginalised
* @param spinv: the sparse block matrix with the result
* @returns false if the operation is not supported by the solver
*/
bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const Vertex* vertex) {
if (vertex->hessianIndex() < 0) {
return false;
}
std::vector<std::pair<int, int> > index;
index.push_back(std::pair<int, int>(vertex->hessianIndex(), vertex->hessianIndex()));
return computeMarginals(spinv, index);
}
/**
* computes the inverse of the set specified vertices, assembled into a single covariance matrix.
* @param vertex: the pattern
* @param spinv: the sparse block matrix with the result
* @returns false if the operation is not supported by the solver
*/
bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const VertexContainer& vertices) {
std::vector<std::pair<int, int> > indices;
for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
indices.push_back(std::pair<int, int>((*it)->hessianIndex(),(*it)->hessianIndex()));
}
return computeMarginals(spinv, indices);
}
//! finds a gauge in the graph to remove the undefined dof.
// The gauge should be fixed() and then the optimization can work (if no additional dof are in
// the system. The default implementation returns a node with maximum dimension.
virtual Vertex* findGauge();
bool gaugeFreedom();
/**returns the cached chi2 of the active portion of the graph*/
double activeChi2() const;
/**
* returns the cached chi2 of the active portion of the graph.
* In contrast to activeChi2() this functions considers the weighting
* of the error according to the robustification of the error functions.
*/
double activeRobustChi2() const;
//! verbose information during optimization
bool verbose() const {return _verbose;}
void setVerbose(bool verbose);
/**
* sets a variable checked at every iteration to force a user stop. The iteration exits when the variable is true;
*/
void setForceStopFlag(bool* flag);
bool* forceStopFlag() const { return _forceStopFlag;};
//! if external stop flag is given, return its state. False otherwise
bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; }
//! the index mapping of the vertices
const VertexContainer& indexMapping() const {return _ivMap;}
//! the vertices active in the current optimization
const VertexContainer& activeVertices() const { return _activeVertices;}
//! the edges active in the current optimization
const EdgeContainer& activeEdges() const { return _activeEdges;}
/**
* Remove a vertex. If the vertex is contained in the currently active set
* of vertices, then the internal temporary structures are cleaned, e.g., the index
* mapping is erased. In case you need the index mapping for manipulating the
* graph, you have to store it in your own copy.
*/
virtual bool removeVertex(HyperGraph::Vertex* v);
/**
* search for an edge in _activeVertices and return the iterator pointing to it
* getActiveVertices().end() if not found
*/
VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const;
/**
* search for an edge in _activeEdges and return the iterator pointing to it
* getActiveEdges().end() if not found
*/
EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const;
//! the solver used by the optimizer
const OptimizationAlgorithm* algorithm() const { return _algorithm;}
OptimizationAlgorithm* solver() { return _algorithm;}
void setAlgorithm(OptimizationAlgorithm* algorithm);
//! push the estimate of a subset of the variables onto a stack
void push(SparseOptimizer::VertexContainer& vlist);
//! push the estimate of a subset of the variables onto a stack
void push(HyperGraph::VertexSet& vlist);
//! push all the active vertices onto a stack
void push();
//! pop (restore) the estimate a subset of the variables from the stack
void pop(SparseOptimizer::VertexContainer& vlist);
//! pop (restore) the estimate a subset of the variables from the stack
void pop(HyperGraph::VertexSet& vlist);
//! pop (restore) the estimate of the active vertices from the stack
void pop();
//! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate
void discardTop(SparseOptimizer::VertexContainer& vlist);
//! same as above, but for the active vertices
void discardTop();
using OptimizableGraph::discardTop;
/**
* clears the graph, and polishes some intermediate structures
* Note that this only removes nodes / edges. Parameters can be removed
* with clearParameters().
*/
virtual void clear();
/**
* computes the error vectors of all edges in the activeSet, and caches them
*/
void computeActiveErrors();
/**
* Linearizes the system by computing the Jacobians for the nodes
* and edges in the graph
*/
G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem())
{
// nothing needed, linearization is now done inside the solver
}
/**
* update the estimate of the active vertices
* @param update: the double vector containing the stacked
* elements of the increments on the vertices.
*/
void update(const double* update);
/**
returns the set of batch statistics about the optimisation
*/
const BatchStatisticsContainer& batchStatistics() const { return _batchStatistics;}
/**
returns the set of batch statistics about the optimisation
*/
BatchStatisticsContainer& batchStatistics() { return _batchStatistics;}
void setComputeBatchStatistics(bool computeBatchStatistics);
bool computeBatchStatistics() const { return _computeBatchStatistics;}
/**** callbacks ****/
//! add an action to be executed before the error vectors are computed
bool addComputeErrorAction(HyperGraphAction* action);
//! remove an action that should no longer be execured before computing the error vectors
bool removeComputeErrorAction(HyperGraphAction* action);
protected:
bool* _forceStopFlag;
bool _verbose;
VertexContainer _ivMap;
VertexContainer _activeVertices; ///< sorted according to VertexIDCompare
EdgeContainer _activeEdges; ///< sorted according to EdgeIDCompare
void sortVectorContainers();
OptimizationAlgorithm* _algorithm;
/**
* builds the mapping of the active vertices to the (block) row / column in the Hessian
*/
bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist);
void clearIndexMapping();
BatchStatisticsContainer _batchStatistics; ///< global statistics of the optimizer, e.g., timing, num-non-zeros
bool _computeBatchStatistics;
};
} // end namespace
#endif

125
Thirdparty/g2o/g2o/solvers/linear_solver_dense.h vendored Normal file
View File

@@ -0,0 +1,125 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// Copyright (C) 2012 R. Kümmerle
// 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.
//
// 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 G2O_LINEAR_SOLVER_DENSE_H
#define G2O_LINEAR_SOLVER_DENSE_H
#include "../core/linear_solver.h"
#include "../core/batch_stats.h"
#include <vector>
#include <utility>
#include<Eigen/Core>
#include<Eigen/Cholesky>
namespace g2o {
/**
* \brief linear solver using dense cholesky decomposition
*/
template <typename MatrixType>
class LinearSolverDense : public LinearSolver<MatrixType>
{
public:
LinearSolverDense() :
LinearSolver<MatrixType>(),
_reset(true)
{
}
virtual ~LinearSolverDense()
{
}
virtual bool init()
{
_reset = true;
return true;
}
bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b)
{
int n = A.cols();
int m = A.cols();
Eigen::MatrixXd& H = _H;
if (H.cols() != n) {
H.resize(n, m);
_reset = true;
}
if (_reset) {
_reset = false;
H.setZero();
}
// copy the sparse block matrix into a dense matrix
int c_idx = 0;
for (size_t i = 0; i < A.blockCols().size(); ++i) {
int c_size = A.colsOfBlock(i);
assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices");
const typename SparseBlockMatrix<MatrixType>::IntBlockMap& col = A.blockCols()[i];
if (col.size() > 0) {
typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it;
for (it = col.begin(); it != col.end(); ++it) {
int r_idx = A.rowBaseOfBlock(it->first);
// only the upper triangular block is processed
if (it->first <= (int)i) {
int r_size = A.rowsOfBlock(it->first);
H.block(r_idx, c_idx, r_size, c_size) = *(it->second);
if (r_idx != c_idx) // write the lower triangular block
H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose();
}
}
}
c_idx += c_size;
}
// solving via Cholesky decomposition
Eigen::VectorXd::MapType xvec(x, m);
Eigen::VectorXd::ConstMapType bvec(b, n);
_cholesky.compute(H);
if (_cholesky.isPositive()) {
xvec = _cholesky.solve(bvec);
return true;
}
return false;
}
protected:
bool _reset;
Eigen::MatrixXd _H;
Eigen::LDLT<Eigen::MatrixXd> _cholesky;
};
}// end namespace
#endif

237
Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h vendored Normal file
View File

@@ -0,0 +1,237 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_LINEAR_SOLVER_EIGEN_H
#define G2O_LINEAR_SOLVER_EIGEN_H
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include "../core/linear_solver.h"
#include "../core/batch_stats.h"
#include "../stuff/timeutil.h"
#include "../core/eigen_types.h"
#include <iostream>
#include <vector>
namespace g2o {
/**
* \brief linear solver which uses the sparse Cholesky solver from Eigen
*
* Has no dependencies except Eigen. Hence, should compile almost everywhere
* without to much issues. Performance should be similar to CSparse, I guess.
*/
template <typename MatrixType>
class LinearSolverEigen: public LinearSolver<MatrixType>
{
public:
typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseMatrix;
typedef Eigen::Triplet<double> Triplet;
typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> PermutationMatrix;
/**
* \brief Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering
*/
class CholeskyDecomposition : public Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>
{
public:
CholeskyDecomposition() : Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>() {}
using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered;
void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation)
{
m_Pinv = permutation;
m_P = permutation.inverse();
int size = a.cols();
SparseMatrix ap(size, size);
ap.selfadjointView<Eigen::Upper>() = a.selfadjointView<UpLo>().twistedBy(m_P);
analyzePattern_preordered(ap, true);
}
};
public:
LinearSolverEigen() :
LinearSolver<MatrixType>(),
_init(true), _blockOrdering(false), _writeDebug(false)
{
}
virtual ~LinearSolverEigen()
{
}
virtual bool init()
{
_init = true;
return true;
}
bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b)
{
if (_init)
_sparseMatrix.resize(A.rows(), A.cols());
fillSparseMatrix(A, !_init);
if (_init) // compute the symbolic composition once
computeSymbolicDecomposition(A);
_init = false;
double t=get_monotonic_time();
_cholesky.factorize(_sparseMatrix);
if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite
if (_writeDebug) {
std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
A.writeOctave("debug.txt");
}
return false;
}
// Solving the system
VectorXD::MapType xx(x, _sparseMatrix.cols());
VectorXD::ConstMapType bb(b, _sparseMatrix.cols());
xx = _cholesky.solve(bb);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeNumericDecomposition = get_monotonic_time() - t;
globalStats->choleskyNNZ = _cholesky.matrixL().nestedExpression().nonZeros() + _sparseMatrix.cols(); // the elements of D
}
return true;
}
//! do the AMD ordering on the blocks or on the scalar matrix
bool blockOrdering() const { return _blockOrdering;}
void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering;}
//! write a debug dump of the system matrix if it is not SPD in solve
virtual bool writeDebug() const { return _writeDebug;}
virtual void setWriteDebug(bool b) { _writeDebug = b;}
protected:
bool _init;
bool _blockOrdering;
bool _writeDebug;
SparseMatrix _sparseMatrix;
CholeskyDecomposition _cholesky;
/**
* compute the symbolic decompostion of the matrix only once.
* Since A has the same pattern in all the iterations, we only
* compute the fill-in reducing ordering once and re-use for all
* the following iterations.
*/
void computeSymbolicDecomposition(const SparseBlockMatrix<MatrixType>& A)
{
double t=get_monotonic_time();
if (! _blockOrdering) {
_cholesky.analyzePattern(_sparseMatrix);
} else {
// block ordering with the Eigen Interface
// This is really ugly currently, as it calls internal functions from Eigen
// and modifies the SparseMatrix class
Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic> blockP;
{
// prepare a block structure matrix for calling AMD
std::vector<Triplet> triplets;
for (size_t c = 0; c < A.blockCols().size(); ++c){
const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
const int& r = it->first;
if (r > static_cast<int>(c)) // only upper triangle
break;
triplets.push_back(Triplet(r, c, 0.));
}
}
// call the AMD ordering on the block matrix.
// Relies on Eigen's internal stuff, probably bad idea
SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size());
auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end());
typename CholeskyDecomposition::CholMatrixType C;
C = auxBlockMatrix.selfadjointView<Eigen::Upper>();
Eigen::internal::minimum_degree_ordering(C, blockP);
}
int rows = A.rows();
assert(rows == A.cols() && "Matrix A is not square");
// Adapt the block permutation to the scalar matrix
PermutationMatrix scalarP;
scalarP.resize(rows);
int scalarIdx = 0;
for (int i = 0; i < blockP.size(); ++i) {
const int& p = blockP.indices()(i);
int base = A.colBaseOfBlock(p);
int nCols = A.colsOfBlock(p);
for (int j = 0; j < nCols; ++j)
scalarP.indices()(scalarIdx++) = base++;
}
assert(scalarIdx == rows && "did not completely fill the permutation matrix");
// analyze with the scalar permutation
_cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP);
}
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats)
globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;
}
void fillSparseMatrix(const SparseBlockMatrix<MatrixType>& A, bool onlyValues)
{
if (onlyValues) {
A.fillCCS(_sparseMatrix.valuePtr(), true);
} else {
// create from triplet structure
std::vector<Triplet> triplets;
triplets.reserve(A.nonZeros());
for (size_t c = 0; c < A.blockCols().size(); ++c) {
int colBaseOfBlock = A.colBaseOfBlock(c);
const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
int rowBaseOfBlock = A.rowBaseOfBlock(it->first);
const MatrixType& m = *(it->second);
for (int cc = 0; cc < m.cols(); ++cc) {
int aux_c = colBaseOfBlock + cc;
for (int rr = 0; rr < m.rows(); ++rr) {
int aux_r = rowBaseOfBlock + rr;
if (aux_r > aux_c)
break;
triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc)));
}
}
}
}
_sparseMatrix.setFromTriplets(triplets.begin(), triplets.end());
}
}
};
} // end namespace
#endif

65
Thirdparty/g2o/g2o/stuff/color_macros.h vendored Normal file
View File

@@ -0,0 +1,65 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_COLOR_MACROS_H
#define G2O_COLOR_MACROS_H
// font attributes
#define FT_BOLD "\033[1m"
#define FT_UNDERLINE "\033[4m"
//background color
#define BG_BLACK "\033[40m"
#define BG_RED "\033[41m"
#define BG_GREEN "\033[42m"
#define BG_YELLOW "\033[43m"
#define BG_LIGHTBLUE "\033[44m"
#define BG_MAGENTA "\033[45m"
#define BG_BLUE "\033[46m"
#define BG_WHITE "\033[47m"
// font color
#define CL_BLACK(s) "\033[30m" << s << "\033[0m"
#define CL_RED(s) "\033[31m" << s << "\033[0m"
#define CL_GREEN(s) "\033[32m" << s << "\033[0m"
#define CL_YELLOW(s) "\033[33m" << s << "\033[0m"
#define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m"
#define CL_MAGENTA(s) "\033[35m" << s << "\033[0m"
#define CL_BLUE(s) "\033[36m" << s << "\033[0m"
#define CL_WHITE(s) "\033[37m" << s << "\033[0m"
#define FG_BLACK "\033[30m"
#define FG_RED "\033[31m"
#define FG_GREEN "\033[32m"
#define FG_YELLOW "\033[33m"
#define FG_LIGHTBLUE "\033[34m"
#define FG_MAGENTA "\033[35m"
#define FG_BLUE "\033[36m"
#define FG_WHITE "\033[37m"
#define FG_NORM "\033[0m"
#endif

134
Thirdparty/g2o/g2o/stuff/macros.h vendored Normal file
View File

@@ -0,0 +1,134 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_MACROS_H
#define G2O_MACROS_H
#ifndef DEG2RAD
#define DEG2RAD(x) ((x) * 0.01745329251994329575)
#endif
#ifndef RAD2DEG
#define RAD2DEG(x) ((x) * 57.29577951308232087721)
#endif
// GCC the one and only
#if defined(__GNUC__)
# define G2O_ATTRIBUTE_CONSTRUCTOR(func) \
static void func(void)__attribute__ ((constructor)); \
static void func(void)
# define G2O_ATTRIBUTE_UNUSED __attribute__((unused))
# define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2)))
# define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3)))
# define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning))
# define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated))
#ifdef ANDROID
# define g2o_isnan(x) isnan(x)
# define g2o_isinf(x) isinf(x)
# define g2o_isfinite(x) isfinite(x)
#else
# define g2o_isnan(x) std::isnan(x)
# define g2o_isinf(x) std::isinf(x)
# define g2o_isfinite(x) std::isfinite(x)
#endif
// MSVC on Windows
#elif defined _MSC_VER
# define __PRETTY_FUNCTION__ __FUNCTION__
/**
Modified by Mark Pupilli from:
"Initializer/finalizer sample for MSVC and GCC.
2010 Joe Lowe. Released into the public domain."
"For MSVC, places a ptr to the function in the user initializer section (.CRT$XCU), basically the same thing the compiler does for the constructor calls for static C++ objects. For GCC, uses a constructor attribute."
(As posted on Stack OVerflow)
*/
# define G2O_ATTRIBUTE_CONSTRUCTOR(f) \
__pragma(section(".CRT$XCU",read)) \
static void __cdecl f(void); \
__declspec(allocate(".CRT$XCU")) void (__cdecl*f##_)(void) = f; \
static void __cdecl f(void)
# define G2O_ATTRIBUTE_UNUSED
# define G2O_ATTRIBUTE_FORMAT12
# define G2O_ATTRIBUTE_FORMAT23
# define G2O_ATTRIBUTE_WARNING(func) func
# define G2O_ATTRIBUTE_DEPRECATED(func) func
#include <float.h>
# define g2o_isnan(x) _isnan(x)
# define g2o_isinf(x) (_finite(x) == 0)
# define g2o_isfinite(x) (_finite(x) != 0)
// unknown compiler
#else
# ifndef __PRETTY_FUNCTION__
# define __PRETTY_FUNCTION__ ""
# endif
# define G2O_ATTRIBUTE_CONSTRUCTOR(func) func
# define G2O_ATTRIBUTE_UNUSED
# define G2O_ATTRIBUTE_FORMAT12
# define G2O_ATTRIBUTE_FORMAT23
# define G2O_ATTRIBUTE_WARNING(func) func
# define G2O_ATTRIBUTE_DEPRECATED(func) func
#include <math.h>
#define g2o_isnan(x) isnan(x)
#define g2o_isinf(x) isinf(x)
#define g2o_isfinite(x) isfinite(x)
#endif
// some macros that are only useful for c++
#ifdef __cplusplus
#define G2O_FSKIP_LINE(f) \
{char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);}
#ifndef PVAR
#define PVAR(s) \
#s << " = " << (s) << std::flush
#endif
#ifndef PVARA
#define PVARA(s) \
#s << " = " << RAD2DEG(s) << "deg" << std::flush
#endif
#ifndef FIXED
#define FIXED(s) \
std::fixed << s << std::resetiosflags(std::ios_base::fixed)
#endif
#endif // __cplusplus
#endif

206
Thirdparty/g2o/g2o/stuff/misc.h vendored Normal file
View File

@@ -0,0 +1,206 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_STUFF_MISC_H
#define G2O_STUFF_MISC_H
#include "macros.h"
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
/** @addtogroup utils **/
// @{
/** \file misc.h
* \brief some general case utility functions
*
* This file specifies some general case utility functions
**/
namespace g2o {
/**
* return the square value
*/
template <typename T>
inline T square(T x)
{
return x*x;
}
/**
* return the hypot of x and y
*/
template <typename T>
inline T hypot(T x, T y)
{
return (T) (sqrt(x*x + y*y));
}
/**
* return the squared hypot of x and y
*/
template <typename T>
inline T hypot_sqr(T x, T y)
{
return x*x + y*y;
}
/**
* convert from degree to radian
*/
inline double deg2rad(double degree)
{
return degree * 0.01745329251994329576;
}
/**
* convert from radian to degree
*/
inline double rad2deg(double rad)
{
return rad * 57.29577951308232087721;
}
/**
* normalize the angle
*/
inline double normalize_theta(double theta)
{
if (theta >= -M_PI && theta < M_PI)
return theta;
double multiplier = floor(theta / (2*M_PI));
theta = theta - multiplier*2*M_PI;
if (theta >= M_PI)
theta -= 2*M_PI;
if (theta < -M_PI)
theta += 2*M_PI;
return theta;
}
/**
* inverse of an angle, i.e., +180 degree
*/
inline double inverse_theta(double th)
{
return normalize_theta(th + M_PI);
}
/**
* average two angles
*/
inline double average_angle(double theta1, double theta2)
{
double x, y;
x = cos(theta1) + cos(theta2);
y = sin(theta1) + sin(theta2);
if(x == 0 && y == 0)
return 0;
else
return std::atan2(y, x);
}
/**
* sign function.
* @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0
*/
template <typename T>
inline int sign(T x)
{
if (x > 0)
return 1;
else if (x < 0)
return -1;
else
return 0;
}
/**
* clamp x to the interval [l, u]
*/
template <typename T>
inline T clamp(T l, T x, T u)
{
if (x < l)
return l;
if (x > u)
return u;
return x;
}
/**
* wrap x to be in the interval [l, u]
*/
template <typename T>
inline T wrap(T l, T x, T u)
{
T intervalWidth = u - l;
while (x < l)
x += intervalWidth;
while (x > u)
x -= intervalWidth;
return x;
}
/**
* tests whether there is a NaN in the array
*/
inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0)
{
for (int i = 0; i < size; ++i)
if (g2o_isnan(array[i])) {
if (nanIndex)
*nanIndex = i;
return true;
}
return false;
}
/**
* The following two functions are used to force linkage with static libraries.
*/
extern "C"
{
typedef void (* ForceLinkFunction) (void);
}
struct ForceLinker
{
ForceLinker(ForceLinkFunction function) { (function)(); }
};
} // end namespace
// @}
#endif

64
Thirdparty/g2o/g2o/stuff/os_specific.c vendored Normal file
View File

@@ -0,0 +1,64 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "os_specific.h"
#ifdef WINDOWS
int vasprintf(char** strp, const char* fmt, va_list ap)
{
int n;
int size = 100;
char* p;
char* np;
if ((p = (char*)malloc(size * sizeof(char))) == NULL)
return -1;
while (1) {
#ifdef _MSC_VER
n = vsnprintf_s(p, size, size - 1, fmt, ap);
#else
n = vsnprintf(p, size, fmt, ap);
#endif
if (n > -1 && n < size) {
*strp = p;
return n;
}
if (n > -1)
size = n+1;
else
size *= 2;
if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) {
free(p);
return -1;
} else
p = np;
}
}
#endif

56
Thirdparty/g2o/g2o/stuff/os_specific.h vendored Normal file
View File

@@ -0,0 +1,56 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_OS_SPECIFIC_HH_
#define G2O_OS_SPECIFIC_HH_
#ifdef WINDOWS
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#ifndef _WINDOWS
#include <sys/time.h>
#endif
#define drand48() ((double) rand()/(double)RAND_MAX)
#ifdef __cplusplus
extern "C" {
#endif
int vasprintf(char** strp, const char* fmt, va_list ap);
#ifdef __cplusplus
}
#endif
#endif
#ifdef UNIX
#include <sys/time.h>
// nothing to do on real operating systems
#endif
#endif

105
Thirdparty/g2o/g2o/stuff/property.cpp vendored Normal file
View File

@@ -0,0 +1,105 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "property.h"
#include <vector>
#include <iostream>
#include "macros.h"
#include "string_tools.h"
using namespace std;
namespace g2o {
BaseProperty::BaseProperty(const std::string name_) :_name(name_){
}
BaseProperty::~BaseProperty(){}
bool PropertyMap::addProperty(BaseProperty* p) {
std::pair<PropertyMapIterator,bool> result = insert(make_pair(p->name(), p));
return result.second;
}
bool PropertyMap::eraseProperty(const std::string& name) {
PropertyMapIterator it=find(name);
if (it==end())
return false;
delete it->second;
erase(it);
return true;
}
PropertyMap::~PropertyMap() {
for (PropertyMapIterator it=begin(); it!=end(); it++){
if (it->second)
delete it->second;
}
}
bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value)
{
PropertyMapIterator it = find(name);
if (it == end())
return false;
it->second->fromString(value);
return true;
}
void PropertyMap::writeToCSV(std::ostream& os) const
{
for (PropertyMapConstIterator it=begin(); it!=end(); it++){
BaseProperty* p =it->second;
os << p->name() << ", ";
}
os << std::endl;
for (PropertyMapConstIterator it=begin(); it!=end(); it++){
BaseProperty* p =it->second;
os << p->toString() << ", ";
}
os << std::endl;
}
bool PropertyMap::updateMapFromString(const std::string& values)
{
bool status = true;
vector<string> valuesMap = strSplit(values, ",");
for (size_t i = 0; i < valuesMap.size(); ++i) {
vector<string> m = strSplit(valuesMap[i], "=");
if (m.size() != 2) {
cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl;
continue;
}
string name = trim(m[0]);
string value = trim(m[1]);
status = status && updatePropertyFromString(name, value);
}
return status;
}
} // end namespace

158
Thirdparty/g2o/g2o/stuff/property.h vendored Normal file
View File

@@ -0,0 +1,158 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_PROPERTY_H_
#define G2O_PROPERTY_H_
#include <string>
#include <map>
#include <sstream>
#include "string_tools.h"
namespace g2o {
class BaseProperty {
public:
BaseProperty(const std::string name_);
virtual ~BaseProperty();
const std::string& name() {return _name;}
virtual std::string toString() const = 0;
virtual bool fromString(const std::string& s) = 0;
protected:
std::string _name;
};
template <typename T>
class Property: public BaseProperty {
public:
typedef T ValueType;
Property(const std::string& name_): BaseProperty(name_){}
Property(const std::string& name_, const T& v): BaseProperty(name_), _value(v){}
void setValue(const T& v) {_value = v; }
const T& value() const {return _value;}
virtual std::string toString() const
{
std::stringstream sstr;
sstr << _value;
return sstr.str();
}
virtual bool fromString(const std::string& s)
{
bool status = convertString(s, _value);
return status;
}
protected:
T _value;
};
/**
* \brief a collection of properties mapping from name to the property itself
*/
class PropertyMap : protected std::map<std::string, BaseProperty*>
{
public:
typedef std::map<std::string, BaseProperty*> BaseClass;
typedef BaseClass::iterator PropertyMapIterator;
typedef BaseClass::const_iterator PropertyMapConstIterator;
~PropertyMap();
/**
* add a property to the map
*/
bool addProperty(BaseProperty* p);
/**
* remove a property from the map
*/
bool eraseProperty(const std::string& name_);
/**
* return a property by its name
*/
template <typename P>
P* getProperty(const std::string& name_)
{
PropertyMapIterator it=find(name_);
if (it==end())
return 0;
return dynamic_cast<P*>(it->second);
}
template <typename P>
const P* getProperty(const std::string& name_) const
{
PropertyMapConstIterator it=find(name_);
if (it==end())
return 0;
return dynamic_cast<P*>(it->second);
}
/**
* create a property and insert it
*/
template <typename P>
P* makeProperty(const std::string& name_, const typename P::ValueType& v)
{
PropertyMapIterator it=find(name_);
if (it==end()){
P* p=new P(name_, v);
addProperty(p);
return p;
} else
return dynamic_cast<P*>(it->second);
}
/**
* update a specfic property with a new value
* @return true if the params is stored and update was carried out
*/
bool updatePropertyFromString(const std::string& name, const std::string& value);
/**
* update the map based on a name=value string, e.g., name1=val1,name2=val2
* @return true, if it was possible to update all parameters
*/
bool updateMapFromString(const std::string& values);
void writeToCSV(std::ostream& os) const;
using BaseClass::size;
using BaseClass::begin;
using BaseClass::end;
using BaseClass::iterator;
using BaseClass::const_iterator;
};
typedef Property<int> IntProperty;
typedef Property<bool> BoolProperty;
typedef Property<float> FloatProperty;
typedef Property<double> DoubleProperty;
typedef Property<std::string> StringProperty;
} // end namespace
#endif

200
Thirdparty/g2o/g2o/stuff/string_tools.cpp vendored Normal file
View File

@@ -0,0 +1,200 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "string_tools.h"
#include "os_specific.h"
#include "macros.h"
#include <stdio.h>
#include <cctype>
#include <string>
#include <cstdarg>
#include <cstring>
#include <algorithm>
#include <cstdio>
#include <iostream>
#include <iterator>
#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID)
#include <wordexp.h>
#endif
#include "os_specific.c"
namespace g2o {
using namespace std;
int vasprintf(char** strp, const char* fmt, va_list ap) {
va_list ap2;
va_copy(ap2, ap);
char tmp[1];
int size = vsnprintf(tmp, 1, fmt, ap2);
if (size <= 0) return size;
va_end(ap2);
size += 1;
*strp = (char*)malloc(size * sizeof(char));
return vsnprintf(*strp, size, fmt, ap);
}
std::string trim(const std::string& s)
{
if(s.length() == 0)
return s;
string::size_type b = s.find_first_not_of(" \t\n");
string::size_type e = s.find_last_not_of(" \t\n");
if(b == string::npos)
return "";
return std::string(s, b, e - b + 1);
}
std::string trimLeft(const std::string& s)
{
if(s.length() == 0)
return s;
string::size_type b = s.find_first_not_of(" \t\n");
string::size_type e = s.length() - 1;
if(b == string::npos)
return "";
return std::string(s, b, e - b + 1);
}
std::string trimRight(const std::string& s)
{
if(s.length() == 0)
return s;
string::size_type b = 0;
string::size_type e = s.find_last_not_of(" \t\n");
if(b == string::npos)
return "";
return std::string(s, b, e - b + 1);
}
std::string strToLower(const std::string& s)
{
string ret;
std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::tolower);
return ret;
}
std::string strToUpper(const std::string& s)
{
string ret;
std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::toupper);
return ret;
}
std::string formatString(const char* fmt, ...)
{
char* auxPtr = NULL;
va_list arg_list;
va_start(arg_list, fmt);
int numChar = vasprintf(&auxPtr, fmt, arg_list);
va_end(arg_list);
string retString;
if (numChar != -1)
retString = auxPtr;
else {
cerr << __PRETTY_FUNCTION__ << ": Error while allocating memory" << endl;
}
free(auxPtr);
return retString;
}
int strPrintf(std::string& str, const char* fmt, ...)
{
char* auxPtr = NULL;
va_list arg_list;
va_start(arg_list, fmt);
int numChars = vasprintf(&auxPtr, fmt, arg_list);
va_end(arg_list);
str = auxPtr;
free(auxPtr);
return numChars;
}
std::string strExpandFilename(const std::string& filename)
{
#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID)
string result = filename;
wordexp_t p;
wordexp(filename.c_str(), &p, 0);
if(p.we_wordc > 0) {
result = p.we_wordv[0];
}
wordfree(&p);
return result;
#else
(void) filename;
std::cerr << "WARNING: " << __PRETTY_FUNCTION__ << " not implemented" << std::endl;
return std::string();
#endif
}
std::vector<std::string> strSplit(const std::string& str, const std::string& delimiters)
{
std::vector<std::string> tokens;
string::size_type lastPos = 0;
string::size_type pos = 0;
do {
pos = str.find_first_of(delimiters, lastPos);
tokens.push_back(str.substr(lastPos, pos - lastPos));
lastPos = pos + 1;
} while (string::npos != pos);
return tokens;
}
bool strStartsWith(const std::string& s, const std::string& start)
{
if (s.size() < start.size())
return false;
return equal(start.begin(), start.end(), s.begin());
}
bool strEndsWith(const std::string& s, const std::string& end)
{
if (s.size() < end.size())
return false;
return equal(end.rbegin(), end.rend(), s.rbegin());
}
int readLine(std::istream& is, std::stringstream& currentLine)
{
if (is.eof())
return -1;
currentLine.str("");
currentLine.clear();
is.get(*currentLine.rdbuf());
if (is.fail()) // fail is set on empty lines
is.clear();
G2O_FSKIP_LINE(is); // read \n not read by get()
return static_cast<int>(currentLine.str().size());
}
} // end namespace

176
Thirdparty/g2o/g2o/stuff/string_tools.h vendored Normal file
View File

@@ -0,0 +1,176 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_STRING_TOOLS_H
#define G2O_STRING_TOOLS_H
#include <string>
#include <sstream>
#include <cstdlib>
#include <vector>
#include "macros.h"
namespace g2o {
/** @addtogroup utils **/
// @{
/** \file stringTools.h
* \brief utility functions for handling strings
*/
/**
* remove whitespaces from the start/end of a string
*/
std::string trim(const std::string& s);
/**
* remove whitespaces from the left side of the string
*/
std::string trimLeft(const std::string& s);
/**
* remove whitespaced from the right side of the string
*/
std::string trimRight(const std::string& s);
/**
* convert the string to lower case
*/
std::string strToLower(const std::string& s);
/**
* convert a string to upper case
*/
std::string strToUpper(const std::string& s);
/**
* read integer values (seperated by spaces) from a string and store
* them in the given OutputIterator.
*/
template <typename OutputIterator>
OutputIterator readInts(const char* str, OutputIterator out)
{
char* cl = (char*)str;
char* cle = cl;
while (1) {
int id = strtol(cl, &cle, 10);
if (cl == cle)
break;
*out++ = id;
cl = cle;
}
return out;
}
/**
* read float values (seperated by spaces) from a string and store
* them in the given OutputIterator.
*/
template <typename OutputIterator>
OutputIterator readFloats(const char* str, OutputIterator out)
{
char* cl = (char*)str;
char* cle = cl;
while (1) {
double val = strtod(cl, &cle);
if (cl == cle)
break;
*out++ = val;
cl = cle;
}
return out;
}
/**
* format a string and return a std::string.
* Format is just like printf, see man 3 printf
*/
std::string formatString(const char* fmt, ...) G2O_ATTRIBUTE_FORMAT12;
/**
* replacement function for sprintf which fills a std::string instead of a char*
*/
int strPrintf(std::string& str, const char* fmt, ...) G2O_ATTRIBUTE_FORMAT23;
/**
* convert a string into an other type.
*/
template<typename T>
bool convertString(const std::string& s, T& x, bool failIfLeftoverChars = true)
{
std::istringstream i(s);
char c;
if (!(i >> x) || (failIfLeftoverChars && i.get(c)))
return false;
return true;
}
/**
* convert a string into an other type.
* Return the converted value. Throw error if parsing is wrong.
*/
template<typename T>
T stringToType(const std::string& s, bool failIfLeftoverChars = true)
{
T x;
convertString(s, x, failIfLeftoverChars);
return x;
}
/**
* return true, if str starts with substr
*/
bool strStartsWith(const std::string & str, const std::string& substr);
/**
* return true, if str ends with substr
*/
bool strEndsWith(const std::string & str, const std::string& substr);
/**
* expand the given filename like a posix shell, e.g., ~ $CARMEN_HOME and other will get expanded.
* Also command substitution, e.g. `pwd` will give the current directory.
*/
std::string strExpandFilename(const std::string& filename);
/**
* split a string into token based on the characters given in delim
*/
std::vector<std::string> strSplit(const std::string& s, const std::string& delim);
/**
* read a line from is into currentLine.
* @return the number of characters read into currentLine (excluding newline), -1 on eof()
*/
int readLine(std::istream& is, std::stringstream& currentLine);
// @}
} // end namespace
#endif

124
Thirdparty/g2o/g2o/stuff/timeutil.cpp vendored Normal file
View File

@@ -0,0 +1,124 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 "timeutil.h"
#include <iostream>
#ifdef _WINDOWS
#include <time.h>
#include <windows.h>
#endif
#ifdef UNIX
#include <unistd.h>
#endif
namespace g2o {
#ifdef _WINDOWS
#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
#define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64
#else
#define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL
#endif
struct timezone
{
int tz_minuteswest; /* minutes W of Greenwich */
int tz_dsttime; /* type of dst correction */
};
int gettimeofday(struct timeval *tv, struct timezone *tz)
{
// Define a structure to receive the current Windows filetime
FILETIME ft;
// Initialize the present time to 0 and the timezone to UTC
unsigned __int64 tmpres = 0;
static int tzflag = 0;
if (NULL != tv)
{
GetSystemTimeAsFileTime(&ft);
// The GetSystemTimeAsFileTime returns the number of 100 nanosecond
// intervals since Jan 1, 1601 in a structure. Copy the high bits to
// the 64 bit tmpres, shift it left by 32 then or in the low 32 bits.
tmpres |= ft.dwHighDateTime;
tmpres <<= 32;
tmpres |= ft.dwLowDateTime;
// Convert to microseconds by dividing by 10
tmpres /= 10;
// The Unix epoch starts on Jan 1 1970. Need to subtract the difference
// in seconds from Jan 1 1601.
tmpres -= DELTA_EPOCH_IN_MICROSECS;
// Finally change microseconds to seconds and place in the seconds value.
// The modulus picks up the microseconds.
tv->tv_sec = (long)(tmpres / 1000000UL);
tv->tv_usec = (long)(tmpres % 1000000UL);
}
if (NULL != tz) {
if (!tzflag) {
_tzset();
tzflag++;
}
long sec;
int hours;
_get_timezone(&sec);
_get_daylight(&hours);
// Adjust for the timezone west of Greenwich
tz->tz_minuteswest = sec / 60;
tz->tz_dsttime = hours;
}
return 0;
}
#endif
ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {}
ScopeTime::~ScopeTime() {
std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n";
}
double get_monotonic_time()
{
#if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK))
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return ts.tv_sec + ts.tv_nsec*1e-9;
#else
return get_time();
#endif
}
} // end namespace

132
Thirdparty/g2o/g2o/stuff/timeutil.h vendored Normal file
View File

@@ -0,0 +1,132 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// 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 G2O_TIMEUTIL_H
#define G2O_TIMEUTIL_H
#ifdef _WINDOWS
#include <time.h>
#else
#include <sys/time.h>
#endif
#include <string>
/** @addtogroup utils **/
// @{
/** \file timeutil.h
* \brief utility functions for handling time related stuff
*/
/// Executes code, only if secs are gone since last exec.
/// extended version, in which the current time is given, e.g., timestamp of IPC message
#ifndef DO_EVERY_TS
#define DO_EVERY_TS(secs, currentTime, code) \
if (1) {\
static double s_lastDone_ = (currentTime); \
double s_now_ = (currentTime); \
if (s_lastDone_ > s_now_) \
s_lastDone_ = s_now_; \
if (s_now_ - s_lastDone_ > (secs)) { \
code; \
s_lastDone_ = s_now_; \
}\
} else \
(void)0
#endif
/// Executes code, only if secs are gone since last exec.
#ifndef DO_EVERY
#define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code)
#endif
#ifndef MEASURE_TIME
#define MEASURE_TIME(text, code) \
if(1) { \
double _start_time_ = g2o::get_time(); \
code; \
fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \
} else \
(void) 0
#endif
namespace g2o {
#ifdef _WINDOWS
typedef struct timeval {
long tv_sec;
long tv_usec;
} timeval;
int gettimeofday(struct timeval *tv, struct timezone *tz);
#endif
/**
* return the current time in seconds since 1. Jan 1970
*/
inline double get_time()
{
struct timeval ts;
gettimeofday(&ts,0);
return ts.tv_sec + ts.tv_usec*1e-6;
}
/**
* return a monotonic increasing time which basically does not need to
* have a reference point. Consider this for measuring how long some
* code fragments required to execute.
*
* On Linux we call clock_gettime() on other systems we currently
* call get_time().
*/
double get_monotonic_time();
/**
* \brief Class to measure the time spent in a scope
*
* To use this class, e.g. to measure the time spent in a function,
* just create and instance at the beginning of the function.
*/
class ScopeTime {
public:
ScopeTime(const char* title);
~ScopeTime();
private:
std::string _title;
double _startTime;
};
} // end namespace
#ifndef MEASURE_FUNCTION_TIME
#define MEASURE_FUNCTION_TIME \
g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__)
#endif
// @}
#endif

47
Thirdparty/g2o/g2o/types/se3_ops.h vendored Normal file
View File

@@ -0,0 +1,47 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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 G2O_MATH_STUFF
#define G2O_MATH_STUFF
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace g2o {
using namespace Eigen;
inline Matrix3d skew(const Vector3d&v);
inline Vector3d deltaR(const Matrix3d& R);
inline Vector2d project(const Vector3d&);
inline Vector3d project(const Vector4d&);
inline Vector3d unproject(const Vector2d&);
inline Vector4d unproject(const Vector3d&);
#include "se3_ops.hpp"
}
#endif //MATH_STUFF

85
Thirdparty/g2o/g2o/types/se3_ops.hpp vendored Normal file
View File

@@ -0,0 +1,85 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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.
Matrix3d skew(const Vector3d&v)
{
Matrix3d m;
m.fill(0.);
m(0,1) = -v(2);
m(0,2) = v(1);
m(1,2) = -v(0);
m(1,0) = v(2);
m(2,0) = -v(1);
m(2,1) = v(0);
return m;
}
Vector3d deltaR(const Matrix3d& R)
{
Vector3d v;
v(0)=R(2,1)-R(1,2);
v(1)=R(0,2)-R(2,0);
v(2)=R(1,0)-R(0,1);
return v;
}
Vector2d project(const Vector3d& v)
{
Vector2d res;
res(0) = v(0)/v(2);
res(1) = v(1)/v(2);
return res;
}
Vector3d project(const Vector4d& v)
{
Vector3d res;
res(0) = v(0)/v(3);
res(1) = v(1)/v(3);
res(2) = v(2)/v(3);
return res;
}
Vector3d unproject(const Vector2d& v)
{
Vector3d res;
res(0) = v(0);
res(1) = v(1);
res(2) = 1;
return res;
}
Vector4d unproject(const Vector3d& v)
{
Vector4d res;
res(0) = v(0);
res(1) = v(1);
res(2) = v(2);
res(3) = 1;
return res;
}

39
Thirdparty/g2o/g2o/types/se3mat.cpp vendored Normal file
View File

@@ -0,0 +1,39 @@
#include "se3mat.h"
namespace g2o {
void SE3mat::Retract(const Eigen::Vector3d dr, const Eigen::Vector3d &dt)
{
t += R*dt;
R = R*ExpSO3(dr);
}
Eigen::Matrix3d SE3mat::ExpSO3(const Eigen::Vector3d r)
{
Eigen::Matrix3d W;
W << 0, -r[2], r[1],
r[2], 0, -r[0],
-r[1], r[0], 0;
const double theta = r.norm();
if(theta<1e-6)
return Eigen::Matrix3d::Identity() + W + 0.5l*W*W;
else
return Eigen::Matrix3d::Identity() + W*sin(theta)/theta + W*W*(1-cos(theta))/(theta*theta);
}
Eigen::Vector3d SE3mat::LogSO3(const Eigen::Matrix3d R)
{
const double tr = R(0,0)+R(1,1)+R(2,2);
const double theta = acos((tr-1.0l)*0.5l);
Eigen::Vector3d w;
w << R(2,1), R(0,2), R(1,0);
if(theta<1e-6)
return w;
else
return theta*w/sin(theta);
}
} //namespace g2o

47
Thirdparty/g2o/g2o/types/se3mat.h vendored Normal file
View File

@@ -0,0 +1,47 @@
#ifndef SE3mat_H
#define SE3mat_H
#include<eigen3/Eigen/Geometry>
#include<eigen3/Eigen/Core>
namespace g2o {
class SE3mat
{
public:
SE3mat(){
R = Eigen::Matrix3d::Identity();
t.setZero();
}
SE3mat(const Eigen::Matrix3d &R_, const Eigen::Vector3d &t_):R(R_),t(t_){}
void Retract(const Eigen::Vector3d dr, const Eigen::Vector3d &dt);
inline Eigen::Vector3d operator* (const Eigen::Vector3d& v) const {
return R*v + t;
}
inline SE3mat& operator*= (const SE3mat& T2){
t+=R*T2.t;
R*=T2.R;
return *this;
}
inline SE3mat inverse() const{
Eigen::Matrix3d Rt = R.transpose();
return SE3mat(Rt,-Rt*t);
}
protected:
Eigen::Vector3d t;
Eigen::Matrix3d R;
public:
static Eigen::Matrix3d ExpSO3(const Eigen::Vector3d r);
static Eigen::Vector3d LogSO3(const Eigen::Matrix3d R);
};
}//namespace g2o
#endif // SE3mat_H

306
Thirdparty/g2o/g2o/types/se3quat.h vendored Normal file
View File

@@ -0,0 +1,306 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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 G2O_SE3QUAT_H_
#define G2O_SE3QUAT_H_
#include "se3_ops.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace g2o {
using namespace Eigen;
typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 7, 1> Vector7d;
class SE3Quat {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
protected:
Quaterniond _r;
Vector3d _t;
public:
SE3Quat(){
_r.setIdentity();
_t.setZero();
}
SE3Quat(const Matrix3d& R, const Vector3d& t):_r(Quaterniond(R)),_t(t){
normalizeRotation();
}
SE3Quat(const Quaterniond& q, const Vector3d& t):_r(q),_t(t){
normalizeRotation();
}
/**
* templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6d or Map<Vector6d>
*/
template <typename Derived>
explicit SE3Quat(const MatrixBase<Derived>& v)
{
assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match");
if (v.size() == 6) {
for (int i=0; i<3; i++){
_t[i]=v[i];
_r.coeffs()(i)=v[i+3];
}
_r.w() = 0.; // recover the positive w
if (_r.norm()>1.){
_r.normalize();
} else {
double w2=1.-_r.squaredNorm();
_r.w()= (w2<0.) ? 0. : sqrt(w2);
}
}
else if (v.size() == 7) {
int idx = 0;
for (int i=0; i<3; ++i, ++idx)
_t(i) = v(idx);
for (int i=0; i<4; ++i, ++idx)
_r.coeffs()(i) = v(idx);
normalizeRotation();
}
}
inline const Vector3d& translation() const {return _t;}
inline void setTranslation(const Vector3d& t_) {_t = t_;}
inline const Quaterniond& rotation() const {return _r;}
void setRotation(const Quaterniond& r_) {_r=r_;}
inline SE3Quat operator* (const SE3Quat& tr2) const{
SE3Quat result(*this);
result._t += _r*tr2._t;
result._r*=tr2._r;
result.normalizeRotation();
return result;
}
inline SE3Quat& operator*= (const SE3Quat& tr2){
_t+=_r*tr2._t;
_r*=tr2._r;
normalizeRotation();
return *this;
}
inline Vector3d operator* (const Vector3d& v) const {
return _t+_r*v;
}
inline SE3Quat inverse() const{
SE3Quat ret;
ret._r=_r.conjugate();
ret._t=ret._r*(_t*-1.);
return ret;
}
inline double operator [](int i) const {
assert(i<7);
if (i<3)
return _t[i];
return _r.coeffs()[i-3];
}
inline Vector7d toVector() const{
Vector7d v;
v[0]=_t(0);
v[1]=_t(1);
v[2]=_t(2);
v[3]=_r.x();
v[4]=_r.y();
v[5]=_r.z();
v[6]=_r.w();
return v;
}
inline void fromVector(const Vector7d& v){
_r=Quaterniond(v[6], v[3], v[4], v[5]);
_t=Vector3d(v[0], v[1], v[2]);
}
inline Vector6d toMinimalVector() const{
Vector6d v;
v[0]=_t(0);
v[1]=_t(1);
v[2]=_t(2);
v[3]=_r.x();
v[4]=_r.y();
v[5]=_r.z();
return v;
}
inline void fromMinimalVector(const Vector6d& v){
double w = 1.-v[3]*v[3]-v[4]*v[4]-v[5]*v[5];
if (w>0){
_r=Quaterniond(sqrt(w), v[3], v[4], v[5]);
} else {
_r=Quaterniond(0, -v[3], -v[4], -v[5]);
}
_t=Vector3d(v[0], v[1], v[2]);
}
Vector6d log() const {
Vector6d res;
Matrix3d _R = _r.toRotationMatrix();
double d = 0.5*(_R(0,0)+_R(1,1)+_R(2,2)-1);
Vector3d omega;
Vector3d upsilon;
Vector3d dR = deltaR(_R);
Matrix3d V_inv;
if (d>0.99999)
{
omega=0.5*dR;
Matrix3d Omega = skew(omega);
V_inv = Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
}
else
{
double theta = acos(d);
omega = theta/(2*sqrt(1-d*d))*dR;
Matrix3d Omega = skew(omega);
V_inv = ( Matrix3d::Identity() - 0.5*Omega
+ ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );
}
upsilon = V_inv*_t;
for (int i=0; i<3;i++){
res[i]=omega[i];
}
for (int i=0; i<3;i++){
res[i+3]=upsilon[i];
}
return res;
}
Vector3d map(const Vector3d & xyz) const
{
return _r*xyz + _t;
}
static SE3Quat exp(const Vector6d & update)
{
Vector3d omega;
for (int i=0; i<3; i++)
omega[i]=update[i];
Vector3d upsilon;
for (int i=0; i<3; i++)
upsilon[i]=update[i+3];
double theta = omega.norm();
Matrix3d Omega = skew(omega);
Matrix3d R;
Matrix3d V;
if (theta<0.00001)
{
//TODO: CHECK WHETHER THIS IS CORRECT!!!
R = (Matrix3d::Identity() + Omega + Omega*Omega);
V = R;
}
else
{
Matrix3d Omega2 = Omega*Omega;
R = (Matrix3d::Identity()
+ sin(theta)/theta *Omega
+ (1-cos(theta))/(theta*theta)*Omega2);
V = (Matrix3d::Identity()
+ (1-cos(theta))/(theta*theta)*Omega
+ (theta-sin(theta))/(pow(theta,3))*Omega2);
}
return SE3Quat(Quaterniond(R),V*upsilon);
}
Matrix<double, 6, 6> adj() const
{
Matrix3d R = _r.toRotationMatrix();
Matrix<double, 6, 6> res;
res.block(0,0,3,3) = R;
res.block(3,3,3,3) = R;
res.block(3,0,3,3) = skew(_t)*R;
res.block(0,3,3,3) = Matrix3d::Zero(3,3);
return res;
}
Matrix<double,4,4> to_homogeneous_matrix() const
{
Matrix<double,4,4> homogeneous_matrix;
homogeneous_matrix.setIdentity();
homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
homogeneous_matrix.col(3).head(3) = translation();
return homogeneous_matrix;
}
void normalizeRotation(){
if (_r.w()<0){
_r.coeffs() *= -1;
}
_r.normalize();
}
/**
* cast SE3Quat into an Eigen::Isometry3d
*/
operator Eigen::Isometry3d() const
{
Eigen::Isometry3d result = (Eigen::Isometry3d) rotation();
result.translation() = translation();
return result;
}
};
inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3)
{
out_str << se3.to_homogeneous_matrix() << std::endl;
return out_str;
}
} // end namespace
#endif

307
Thirdparty/g2o/g2o/types/sim3.h vendored Normal file
View File

@@ -0,0 +1,307 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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 G2O_SIM_3
#define G2O_SIM_3
#include "se3_ops.h"
#include <Eigen/Geometry>
namespace g2o
{
using namespace Eigen;
typedef Matrix <double, 7, 1> Vector7d;
typedef Matrix <double, 7, 7> Matrix7d;
struct Sim3
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
Quaterniond r;
Vector3d t;
double s;
public:
Sim3()
{
r.setIdentity();
t.fill(0.);
s=1.;
}
Sim3(const Quaterniond & r, const Vector3d & t, double s)
: r(r),t(t),s(s)
{
}
Sim3(const Matrix3d & R, const Vector3d & t, double s)
: r(Quaterniond(R)),t(t),s(s)
{
}
Sim3(const Vector7d & update)
{
Vector3d omega;
for (int i=0; i<3; i++)
omega[i]=update[i];
Vector3d upsilon;
for (int i=0; i<3; i++)
upsilon[i]=update[i+3];
double sigma = update[6];
double theta = omega.norm();
Matrix3d Omega = skew(omega);
s = std::exp(sigma);
Matrix3d Omega2 = Omega*Omega;
Matrix3d I;
I.setIdentity();
Matrix3d R;
double eps = 0.00001;
double A,B,C;
if (fabs(sigma)<eps)
{
C = 1;
if (theta<eps)
{
A = 1./2.;
B = 1./6.;
R = (I + Omega + Omega*Omega);
}
else
{
double theta2= theta*theta;
A = (1-cos(theta))/(theta2);
B = (theta-sin(theta))/(theta2*theta);
R = I + sin(theta)/theta *Omega + (1-cos(theta))/(theta*theta)*Omega2;
}
}
else
{
C=(s-1)/sigma;
if (theta<eps)
{
double sigma2= sigma*sigma;
A = ((sigma-1)*s+1)/sigma2;
B= ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);
R = (I + Omega + Omega2);
}
else
{
R = I + sin(theta)/theta *Omega + (1-cos(theta))/(theta*theta)*Omega2;
double a=s*sin(theta);
double b=s*cos(theta);
double theta2= theta*theta;
double sigma2= sigma*sigma;
double c=theta2+sigma2;
A = (a*sigma+ (1-b)*theta)/(theta*c);
B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
}
}
r = Quaterniond(R);
Matrix3d W = A*Omega + B*Omega2 + C*I;
t = W*upsilon;
}
Vector3d map (const Vector3d& xyz) const {
return s*(r*xyz) + t;
}
Vector7d log() const
{
Vector7d res;
double sigma = std::log(s);
Vector3d omega;
Vector3d upsilon;
Matrix3d R = r.toRotationMatrix();
double d = 0.5*(R(0,0)+R(1,1)+R(2,2)-1);
Matrix3d Omega;
double eps = 0.00001;
Matrix3d I = Matrix3d::Identity();
double A,B,C;
if (fabs(sigma)<eps)
{
C = 1;
if (d>1-eps)
{
omega=0.5*deltaR(R);
Omega = skew(omega);
A = 1./2.;
B = 1./6.;
}
else
{
double theta = acos(d);
double theta2 = theta*theta;
omega = theta/(2*sqrt(1-d*d))*deltaR(R);
Omega = skew(omega);
A = (1-cos(theta))/(theta2);
B = (theta-sin(theta))/(theta2*theta);
}
}
else
{
C=(s-1)/sigma;
if (d>1-eps)
{
double sigma2 = sigma*sigma;
omega=0.5*deltaR(R);
Omega = skew(omega);
A = ((sigma-1)*s+1)/(sigma2);
B = ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);
}
else
{
double theta = acos(d);
omega = theta/(2*sqrt(1-d*d))*deltaR(R);
Omega = skew(omega);
double theta2 = theta*theta;
double a=s*sin(theta);
double b=s*cos(theta);
double c=theta2 + sigma*sigma;
A = (a*sigma+ (1-b)*theta)/(theta*c);
B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
}
}
Matrix3d W = A*Omega + B*Omega*Omega + C*I;
upsilon = W.lu().solve(t);
for (int i=0; i<3; i++)
res[i] = omega[i];
for (int i=0; i<3; i++)
res[i+3] = upsilon[i];
res[6] = sigma;
return res;
}
Sim3 inverse() const
{
return Sim3(r.conjugate(), r.conjugate()*((-1./s)*t), 1./s);
}
double operator[](int i) const
{
assert(i<8);
if (i<4){
return r.coeffs()[i];
}
if (i<7){
return t[i-4];
}
return s;
}
double& operator[](int i)
{
assert(i<8);
if (i<4){
return r.coeffs()[i];
}
if (i<7)
{
return t[i-4];
}
return s;
}
Sim3 operator *(const Sim3& other) const {
Sim3 ret;
ret.r = r*other.r;
ret.t=s*(r*other.t)+t;
ret.s=s*other.s;
return ret;
}
Sim3& operator *=(const Sim3& other){
Sim3 ret=(*this)*other;
*this=ret;
return *this;
}
inline const Vector3d& translation() const {return t;}
inline Vector3d& translation() {return t;}
inline const Quaterniond& rotation() const {return r;}
inline Quaterniond& rotation() {return r;}
inline const double& scale() const {return s;}
inline double& scale() {return s;}
};
inline std::ostream& operator <<(std::ostream& out_str,
const Sim3& sim3)
{
out_str << sim3.rotation().coeffs() << std::endl;
out_str << sim3.translation() << std::endl;
out_str << sim3.scale() << std::endl;
return out_str;
}
} // end namespace
#endif

56
Thirdparty/g2o/g2o/types/types_sba.cpp vendored Normal file
View File

@@ -0,0 +1,56 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 Kurt Konolige
// 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.
//
// 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 "types_sba.h"
#include <iostream>
namespace g2o {
using namespace std;
VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>()
{
}
bool VertexSBAPointXYZ::read(std::istream& is)
{
Vector3d lv;
for (int i=0; i<3; i++)
is >> _estimate[i];
return true;
}
bool VertexSBAPointXYZ::write(std::ostream& os) const
{
Vector3d lv=estimate();
for (int i=0; i<3; i++){
os << lv[i] << " ";
}
return os.good();
}
} // end namespace

61
Thirdparty/g2o/g2o/types/types_sba.h vendored Normal file
View File

@@ -0,0 +1,61 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 Kurt Konolige
// 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.
//
// 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 G2O_SBA_TYPES
#define G2O_SBA_TYPES
#include "../core/base_vertex.h"
#include <Eigen/Geometry>
#include <iostream>
namespace g2o {
/**
* \brief Point vertex, XYZ
*/
class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSBAPointXYZ();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate.fill(0.);
}
virtual void oplusImpl(const double* update)
{
Eigen::Map<const Vector3d> v(update);
_estimate += v;
}
};
} // end namespace
#endif // SBA_TYPES

239
Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp vendored Normal file
View File

@@ -0,0 +1,239 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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 "types_seven_dof_expmap.h"
namespace g2o {
VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, Sim3>()
{
_marginalized=false;
_fix_scale = false;
}
EdgeSim3::EdgeSim3() :
BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>()
{
}
bool VertexSim3Expmap::read(std::istream& is)
{
Vector7d cam2world;
for (int i=0; i<6; i++){
is >> cam2world[i];
}
is >> cam2world[6];
// if (! is) {
// // if the scale is not specified we set it to 1;
// std::cerr << "!s";
// cam2world[6]=0.;
// }
for (int i=0; i<2; i++)
{
is >> _focal_length1[i];
}
for (int i=0; i<2; i++)
{
is >> _principle_point1[i];
}
setEstimate(Sim3(cam2world).inverse());
return true;
}
bool VertexSim3Expmap::write(std::ostream& os) const
{
Sim3 cam2world(estimate().inverse());
Vector7d lv=cam2world.log();
for (int i=0; i<7; i++){
os << lv[i] << " ";
}
for (int i=0; i<2; i++)
{
os << _focal_length1[i] << " ";
}
for (int i=0; i<2; i++)
{
os << _principle_point1[i] << " ";
}
return os.good();
}
bool EdgeSim3::read(std::istream& is)
{
Vector7d v7;
for (int i=0; i<7; i++){
is >> v7[i];
}
Sim3 cam2world(v7);
setMeasurement(cam2world.inverse());
for (int i=0; i<7; i++)
for (int j=i; j<7; j++)
{
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeSim3::write(std::ostream& os) const
{
Sim3 cam2world(measurement().inverse());
Vector7d v7 = cam2world.log();
for (int i=0; i<7; i++)
{
os << v7[i] << " ";
}
for (int i=0; i<7; i++)
for (int j=i; j<7; j++){
os << " " << information()(i,j);
}
return os.good();
}
/**Sim3ProjectXYZ*/
EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() :
BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>()
{
}
bool EdgeSim3ProjectXYZ::read(std::istream& is)
{
for (int i=0; i<2; i++)
{
is >> _measurement[i];
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeSim3ProjectXYZ::write(std::ostream& os) const
{
for (int i=0; i<2; i++){
os << _measurement[i] << " ";
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++){
os << " " << information()(i,j);
}
return os.good();
}
/**InverseSim3ProjectXYZ*/
EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() :
BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>()
{
}
bool EdgeInverseSim3ProjectXYZ::read(std::istream& is)
{
for (int i=0; i<2; i++)
{
is >> _measurement[i];
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const
{
for (int i=0; i<2; i++){
os << _measurement[i] << " ";
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++){
os << " " << information()(i,j);
}
return os.good();
}
// void EdgeSim3ProjectXYZ::linearizeOplus()
// {
// VertexSim3Expmap * vj = static_cast<VertexSim3Expmap *>(_vertices[1]);
// Sim3 T = vj->estimate();
// VertexPointXYZ* vi = static_cast<VertexPointXYZ*>(_vertices[0]);
// Vector3d xyz = vi->estimate();
// Vector3d xyz_trans = T.map(xyz);
// double x = xyz_trans[0];
// double y = xyz_trans[1];
// double z = xyz_trans[2];
// double z_2 = z*z;
// Matrix<double,2,3> tmp;
// tmp(0,0) = _focal_length(0);
// tmp(0,1) = 0;
// tmp(0,2) = -x/z*_focal_length(0);
// tmp(1,0) = 0;
// tmp(1,1) = _focal_length(1);
// tmp(1,2) = -y/z*_focal_length(1);
// _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
// _jacobianOplusXj(0,0) = x*y/z_2 * _focal_length(0);
// _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *_focal_length(0);
// _jacobianOplusXj(0,2) = y/z *_focal_length(0);
// _jacobianOplusXj(0,3) = -1./z *_focal_length(0);
// _jacobianOplusXj(0,4) = 0;
// _jacobianOplusXj(0,5) = x/z_2 *_focal_length(0);
// _jacobianOplusXj(0,6) = 0; // scale is ignored
// _jacobianOplusXj(1,0) = (1+y*y/z_2) *_focal_length(1);
// _jacobianOplusXj(1,1) = -x*y/z_2 *_focal_length(1);
// _jacobianOplusXj(1,2) = -x/z *_focal_length(1);
// _jacobianOplusXj(1,3) = 0;
// _jacobianOplusXj(1,4) = -1./z *_focal_length(1);
// _jacobianOplusXj(1,5) = y/z_2 *_focal_length(1);
// _jacobianOplusXj(1,6) = 0; // scale is ignored
// }
} // end namespace

176
Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h vendored Normal file
View File

@@ -0,0 +1,176 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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.
// Modified by Raúl Mur Artal (2014)
// - Added EdgeInverseSim3ProjectXYZ
// - Modified VertexSim3Expmap to represent relative transformation between two cameras. Includes calibration of both cameras.
#ifndef G2O_SEVEN_DOF_EXPMAP_TYPES
#define G2O_SEVEN_DOF_EXPMAP_TYPES
#include "../core/base_vertex.h"
#include "../core/base_binary_edge.h"
#include "types_six_dof_expmap.h"
#include "sim3.h"
namespace g2o {
using namespace Eigen;
/**
* \brief Sim3 Vertex, (x,y,z,qw,qx,qy,qz)
* the parameterization for the increments constructed is a 7d vector
* (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
*/
class VertexSim3Expmap : public BaseVertex<7, Sim3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSim3Expmap();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate = Sim3();
}
virtual void oplusImpl(const double* update_)
{
Eigen::Map<Vector7d> update(const_cast<double*>(update_));
if (_fix_scale)
update[6] = 0;
Sim3 s(update);
setEstimate(s*estimate());
}
Vector2d _principle_point1, _principle_point2;
Vector2d _focal_length1, _focal_length2;
Vector2d cam_map1(const Vector2d & v) const
{
Vector2d res;
res[0] = v[0]*_focal_length1[0] + _principle_point1[0];
res[1] = v[1]*_focal_length1[1] + _principle_point1[1];
return res;
}
Vector2d cam_map2(const Vector2d & v) const
{
Vector2d res;
res[0] = v[0]*_focal_length2[0] + _principle_point2[0];
res[1] = v[1]*_focal_length2[1] + _principle_point2[1];
return res;
}
bool _fix_scale;
protected:
};
/**
* \brief 7D edge between two Vertex7
*/
class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSim3();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError()
{
const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]);
const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
Sim3 C(_measurement);
Sim3 error_=C*v1->estimate()*v2->estimate().inverse();
_error = error_.log();
}
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}
virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
{
VertexSim3Expmap* v1 = static_cast<VertexSim3Expmap*>(_vertices[0]);
VertexSim3Expmap* v2 = static_cast<VertexSim3Expmap*>(_vertices[1]);
if (from.count(v1) > 0)
v2->setEstimate(measurement()*v1->estimate());
else
v1->setEstimate(measurement().inverse()*v2->estimate());
}
};
/**/
class EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSim3ProjectXYZ();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError()
{
const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
Vector2d obs(_measurement);
_error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));
}
// virtual void linearizeOplus();
};
/**/
class EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeInverseSim3ProjectXYZ();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError()
{
const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
Vector2d obs(_measurement);
_error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate())));
}
// virtual void linearizeOplus();
};
} // end namespace
#endif

407
Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp vendored Normal file
View File

@@ -0,0 +1,407 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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 "types_six_dof_expmap.h"
#include "../core/factory.h"
#include "../stuff/macros.h"
namespace g2o {
using namespace std;
Vector2d project2d(const Vector3d& v) {
Vector2d res;
res(0) = v(0)/v(2);
res(1) = v(1)/v(2);
return res;
}
Vector3d unproject2d(const Vector2d& v) {
Vector3d res;
res(0) = v(0);
res(1) = v(1);
res(2) = 1;
return res;
}
VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() {
}
bool VertexSE3Expmap::read(std::istream& is) {
Vector7d est;
for (int i=0; i<7; i++)
is >> est[i];
SE3Quat cam2world;
cam2world.fromVector(est);
setEstimate(cam2world.inverse());
return true;
}
bool VertexSE3Expmap::write(std::ostream& os) const {
SE3Quat cam2world(estimate().inverse());
for (int i=0; i<7; i++)
os << cam2world[i] << " ";
return os.good();
}
EdgeSE3::EdgeSE3() :
BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>()
{
}
bool EdgeSE3::read(std::istream& is)
{
Vector6d v6;
for (int i=0; i<6; i++){
is >> v6[i];
}
SE3Quat cam2world(v6);
setMeasurement(cam2world.inverse());
for (int i=0; i<6; i++)
for (int j=i; j<6; j++)
{
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeSE3::write(std::ostream& os) const
{
SE3Quat cam2world(measurement().inverse());
Vector6d v6 = cam2world.log();
for (int i=0; i<6; i++)
{
os << v6[i] << " ";
}
for (int i=0; i<6; i++)
for (int j=i; j<6; j++){
os << " " << information()(i,j);
}
return os.good();
}
EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() {
}
bool EdgeSE3ProjectXYZ::read(std::istream& is){
for (int i=0; i<2; i++){
is >> _measurement[i];
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeSE3ProjectXYZ::write(std::ostream& os) const {
for (int i=0; i<2; i++){
os << measurement()[i] << " ";
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++){
os << " " << information()(i,j);
}
return os.good();
}
void EdgeSE3ProjectXYZ::linearizeOplus() {
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z*z;
Matrix<double,2,3> tmp;
tmp(0,0) = fx;
tmp(0,1) = 0;
tmp(0,2) = -x/z*fx;
tmp(1,0) = 0;
tmp(1,1) = fy;
tmp(1,2) = -y/z*fy;
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
_jacobianOplusXj(0,0) = x*y/z_2 *fx;
_jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
_jacobianOplusXj(0,2) = y/z *fx;
_jacobianOplusXj(0,3) = -1./z *fx;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 *fx;
_jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
_jacobianOplusXj(1,1) = -x*y/z_2 *fy;
_jacobianOplusXj(1,2) = -x/z *fy;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *fy;
_jacobianOplusXj(1,5) = y/z_2 *fy;
}
Vector2d EdgeSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz) const{
Vector2d proj = project2d(trans_xyz);
Vector2d res;
res[0] = proj[0]*fx + cx;
res[1] = proj[1]*fy + cy;
return res;
}
Vector3d EdgeStereoSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz, const float &bf) const{
const float invz = 1.0f/trans_xyz[2];
Vector3d res;
res[0] = trans_xyz[0]*invz*fx + cx;
res[1] = trans_xyz[1]*invz*fy + cy;
res[2] = res[0] - bf*invz;
return res;
}
EdgeStereoSE3ProjectXYZ::EdgeStereoSE3ProjectXYZ() : BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>() {
}
bool EdgeStereoSE3ProjectXYZ::read(std::istream& is){
for (int i=0; i<=3; i++){
is >> _measurement[i];
}
for (int i=0; i<=2; i++)
for (int j=i; j<=2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeStereoSE3ProjectXYZ::write(std::ostream& os) const {
for (int i=0; i<=3; i++){
os << measurement()[i] << " ";
}
for (int i=0; i<=2; i++)
for (int j=i; j<=2; j++){
os << " " << information()(i,j);
}
return os.good();
}
void EdgeStereoSE3ProjectXYZ::linearizeOplus() {
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
const Matrix3d R = T.rotation().toRotationMatrix();
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z*z;
_jacobianOplusXi(0,0) = -fx*R(0,0)/z+fx*x*R(2,0)/z_2;
_jacobianOplusXi(0,1) = -fx*R(0,1)/z+fx*x*R(2,1)/z_2;
_jacobianOplusXi(0,2) = -fx*R(0,2)/z+fx*x*R(2,2)/z_2;
_jacobianOplusXi(1,0) = -fy*R(1,0)/z+fy*y*R(2,0)/z_2;
_jacobianOplusXi(1,1) = -fy*R(1,1)/z+fy*y*R(2,1)/z_2;
_jacobianOplusXi(1,2) = -fy*R(1,2)/z+fy*y*R(2,2)/z_2;
_jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*R(2,0)/z_2;
_jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)-bf*R(2,1)/z_2;
_jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*R(2,2)/z_2;
_jacobianOplusXj(0,0) = x*y/z_2 *fx;
_jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
_jacobianOplusXj(0,2) = y/z *fx;
_jacobianOplusXj(0,3) = -1./z *fx;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 *fx;
_jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
_jacobianOplusXj(1,1) = -x*y/z_2 *fy;
_jacobianOplusXj(1,2) = -x/z *fy;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *fy;
_jacobianOplusXj(1,5) = y/z_2 *fy;
_jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2;
_jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2;
_jacobianOplusXj(2,2) = _jacobianOplusXj(0,2);
_jacobianOplusXj(2,3) = _jacobianOplusXj(0,3);
_jacobianOplusXj(2,4) = 0;
_jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2;
}
//Only Pose
bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is){
for (int i=0; i<2; i++){
is >> _measurement[i];
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const {
for (int i=0; i<2; i++){
os << measurement()[i] << " ";
}
for (int i=0; i<2; i++)
for (int j=i; j<2; j++){
os << " " << information()(i,j);
}
return os.good();
}
void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
Vector3d xyz_trans = vi->estimate().map(Xw);
double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;
_jacobianOplusXi(0,0) = x*y*invz_2 *fx;
_jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
_jacobianOplusXi(0,2) = y*invz *fx;
_jacobianOplusXi(0,3) = -invz *fx;
_jacobianOplusXi(0,4) = 0;
_jacobianOplusXi(0,5) = x*invz_2 *fx;
_jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
_jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
_jacobianOplusXi(1,2) = -x*invz *fy;
_jacobianOplusXi(1,3) = 0;
_jacobianOplusXi(1,4) = -invz *fy;
_jacobianOplusXi(1,5) = y*invz_2 *fy;
}
Vector2d EdgeSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{
Vector2d proj = project2d(trans_xyz);
Vector2d res;
res[0] = proj[0]*fx + cx;
res[1] = proj[1]*fy + cy;
return res;
}
Vector3d EdgeStereoSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{
const float invz = 1.0f/trans_xyz[2];
Vector3d res;
res[0] = trans_xyz[0]*invz*fx + cx;
res[1] = trans_xyz[1]*invz*fy + cy;
res[2] = res[0] - bf*invz;
return res;
}
bool EdgeStereoSE3ProjectXYZOnlyPose::read(std::istream& is){
for (int i=0; i<=3; i++){
is >> _measurement[i];
}
for (int i=0; i<=2; i++)
for (int j=i; j<=2; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool EdgeStereoSE3ProjectXYZOnlyPose::write(std::ostream& os) const {
for (int i=0; i<=3; i++){
os << measurement()[i] << " ";
}
for (int i=0; i<=2; i++)
for (int j=i; j<=2; j++){
os << " " << information()(i,j);
}
return os.good();
}
void EdgeStereoSE3ProjectXYZOnlyPose::linearizeOplus() {
VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
Vector3d xyz_trans = vi->estimate().map(Xw);
double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;
_jacobianOplusXi(0,0) = x*y*invz_2 *fx;
_jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
_jacobianOplusXi(0,2) = y*invz *fx;
_jacobianOplusXi(0,3) = -invz *fx;
_jacobianOplusXi(0,4) = 0;
_jacobianOplusXi(0,5) = x*invz_2 *fx;
_jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
_jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
_jacobianOplusXi(1,2) = -x*invz *fy;
_jacobianOplusXi(1,3) = 0;
_jacobianOplusXi(1,4) = -invz *fy;
_jacobianOplusXi(1,5) = y*invz_2 *fy;
_jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*y*invz_2;
_jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)+bf*x*invz_2;
_jacobianOplusXi(2,2) = _jacobianOplusXi(0,2);
_jacobianOplusXi(2,3) = _jacobianOplusXi(0,3);
_jacobianOplusXi(2,4) = 0;
_jacobianOplusXi(2,5) = _jacobianOplusXi(0,5)-bf*invz_2;
}
} // end namespace

242
Thirdparty/g2o/g2o/types/types_six_dof_expmap.h vendored Normal file
View File

@@ -0,0 +1,242 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// 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.
//
// 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.
// Modified by Raúl Mur Artal (2014)
// Added EdgeSE3ProjectXYZ (project using focal_length in x,y directions)
// Modified by Raúl Mur Artal (2016)
// Added EdgeStereoSE3ProjectXYZ (project using focal_length in x,y directions)
// Added EdgeSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)
// Added EdgeStereoSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)
#ifndef G2O_SIX_DOF_TYPES_EXPMAP
#define G2O_SIX_DOF_TYPES_EXPMAP
#include "../core/base_vertex.h"
#include "../core/base_binary_edge.h"
#include "../core/base_unary_edge.h"
#include "se3_ops.h"
#include "se3quat.h"
#include "types_sba.h"
#include <Eigen/Geometry>
namespace g2o {
namespace types_six_dof_expmap {
void init();
}
using namespace Eigen;
typedef Matrix<double, 6, 6> Matrix6d;
/**
* \brief SE3 Vertex parameterized internally with a transformation matrix
and externally with its exponential map
*/
class VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSE3Expmap();
bool read(std::istream& is);
bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate = SE3Quat();
}
virtual void oplusImpl(const double* update_) {
Eigen::Map<const Vector6d> update(update_);
setEstimate(SE3Quat::exp(update)*estimate());
}
};
/**
* \brief 6D edge between two Vertex6
*/
class EdgeSE3 : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError()
{
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
const VertexSE3Expmap* v2 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
SE3Quat C(_measurement);
SE3Quat error_=C*v1->estimate()*v2->estimate().inverse();
_error = error_.log();
}
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}
virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
{
VertexSE3Expmap* v1 = static_cast<VertexSE3Expmap*>(_vertices[0]);
VertexSE3Expmap* v2 = static_cast<VertexSE3Expmap*>(_vertices[1]);
if (from.count(v1) > 0)
v2->setEstimate(measurement()*v1->estimate());
else
v1->setEstimate(measurement().inverse()*v2->estimate());
}
};
class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3ProjectXYZ();
bool read(std::istream& is);
bool write(std::ostream& os) const;
void computeError() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
Vector2d obs(_measurement);
_error = obs-cam_project(v1->estimate().map(v2->estimate()));
}
bool isDepthPositive() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
return (v1->estimate().map(v2->estimate()))(2)>0.0;
}
virtual void linearizeOplus();
Vector2d cam_project(const Vector3d & trans_xyz) const;
double fx, fy, cx, cy;
};
class EdgeStereoSE3ProjectXYZ: public BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeStereoSE3ProjectXYZ();
bool read(std::istream& is);
bool write(std::ostream& os) const;
void computeError() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
Vector3d obs(_measurement);
_error = obs - cam_project(v1->estimate().map(v2->estimate()),bf);
}
bool isDepthPositive() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
return (v1->estimate().map(v2->estimate()))(2)>0.0;
}
virtual void linearizeOplus();
Vector3d cam_project(const Vector3d & trans_xyz, const float &bf) const;
double fx, fy, cx, cy, bf;
};
class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3ProjectXYZOnlyPose(){}
bool read(std::istream& is);
bool write(std::ostream& os) const;
void computeError() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
Vector2d obs(_measurement);
_error = obs-cam_project(v1->estimate().map(Xw));
}
bool isDepthPositive() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
return (v1->estimate().map(Xw))(2)>0.0;
}
virtual void linearizeOplus();
Vector2d cam_project(const Vector3d & trans_xyz) const;
Vector3d Xw;
double fx, fy, cx, cy;
};
class EdgeStereoSE3ProjectXYZOnlyPose: public BaseUnaryEdge<3, Vector3d, VertexSE3Expmap>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeStereoSE3ProjectXYZOnlyPose(){}
bool read(std::istream& is);
bool write(std::ostream& os) const;
void computeError() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
Vector3d obs(_measurement);
_error = obs - cam_project(v1->estimate().map(Xw));
}
bool isDepthPositive() {
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
return (v1->estimate().map(Xw))(2)>0.0;
}
virtual void linearizeOplus();
Vector3d cam_project(const Vector3d & trans_xyz) const;
Vector3d Xw;
double fx, fy, cx, cy, bf;
};
} // end namespace
#endif

27
Thirdparty/g2o/license-bsd.txt vendored Normal file
View File

@@ -0,0 +1,27 @@
g2o - General Graph Optimization
Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat,
Kurt Konolige, and Wolfram Burgard
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.
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.