Showing preview only (996K chars total). Download the full file or copy to clipboard to get everything.
Repository: XunshanMan/Object-oriented-SLAM
Branch: master
Commit: cb931afab8a6
Files: 232
Total size: 928.0 KB
Directory structure:
gitextract__e5hoirx/
├── CMakeLists.txt
├── Example/
│ ├── dataset/
│ │ └── cabinet/
│ │ ├── associate.py
│ │ ├── associate.txt
│ │ ├── associateGroundtruth.txt
│ │ ├── bbox/
│ │ │ ├── 1341841278.8427.txt
│ │ │ ├── 1341841279.5107.txt
│ │ │ ├── 1341841280.1825.txt
│ │ │ ├── 1341841280.8507.txt
│ │ │ ├── 1341841281.5546.txt
│ │ │ ├── 1341841282.2265.txt
│ │ │ ├── 1341841282.8987.txt
│ │ │ ├── 1341841283.5665.txt
│ │ │ ├── 1341841284.2388.txt
│ │ │ ├── 1341841284.9067.txt
│ │ │ ├── 1341841285.6124.txt
│ │ │ ├── 1341841286.2827.txt
│ │ │ ├── 1341841286.9506.txt
│ │ │ ├── 1341841287.6227.txt
│ │ │ ├── 1341841288.2946.txt
│ │ │ ├── 1341841288.9627.txt
│ │ │ ├── 1341841289.6346.txt
│ │ │ ├── 1341841290.303.txt
│ │ │ ├── 1341841290.9745.txt
│ │ │ ├── 1341841291.6426.txt
│ │ │ ├── 1341841292.3147.txt
│ │ │ ├── 1341841292.9866.txt
│ │ │ ├── 1341841293.6548.txt
│ │ │ ├── 1341841294.3266.txt
│ │ │ ├── 1341841294.9952.txt
│ │ │ ├── 1341841295.6666.txt
│ │ │ ├── 1341841296.3348.txt
│ │ │ ├── 1341841297.0065.txt
│ │ │ ├── 1341841297.6786.txt
│ │ │ ├── 1341841298.3466.txt
│ │ │ ├── 1341841299.0185.txt
│ │ │ ├── 1341841299.6866.txt
│ │ │ ├── 1341841300.3656.txt
│ │ │ ├── 1341841301.0336.txt
│ │ │ ├── 1341841301.7057.txt
│ │ │ ├── 1341841302.3776.txt
│ │ │ ├── 1341841303.0456.txt
│ │ │ ├── 1341841303.7106.txt
│ │ │ ├── 1341841304.3787.txt
│ │ │ ├── 1341841305.0506.txt
│ │ │ ├── 1341841305.7227.txt
│ │ │ ├── 1341841306.3907.txt
│ │ │ ├── 1341841307.0627.txt
│ │ │ ├── 1341841307.7306.txt
│ │ │ ├── 1341841308.4099.txt
│ │ │ ├── 1341841309.1095.txt
│ │ │ ├── 1341841309.7816.txt
│ │ │ ├── 1341841310.4468.txt
│ │ │ ├── 1341841311.1507.txt
│ │ │ ├── 1341841311.8186.txt
│ │ │ ├── 1341841312.4905.txt
│ │ │ ├── 1341841313.1696.txt
│ │ │ ├── 1341841313.8695.txt
│ │ │ ├── 1341841314.5706.txt
│ │ │ ├── 1341841315.2456.txt
│ │ │ ├── 1341841315.9108.txt
│ │ │ ├── 1341841316.5786.txt
│ │ │ └── 1341841317.2506.txt
│ │ ├── depth.txt
│ │ ├── groundtruth.txt
│ │ └── rgb.txt
│ ├── interface/
│ │ └── rgbd.cpp
│ └── param/
│ └── TUM3.yaml
├── LICENSE
├── README.md
├── Thirdparty/
│ └── g2o/
│ ├── CMakeLists.txt
│ ├── README.txt
│ ├── cmake_modules/
│ │ ├── FindBLAS.cmake
│ │ ├── FindEigen3.cmake
│ │ └── FindLAPACK.cmake
│ ├── config.h
│ ├── config.h.in
│ ├── g2o/
│ │ ├── core/
│ │ │ ├── base_binary_edge.h
│ │ │ ├── base_binary_edge.hpp
│ │ │ ├── base_edge.h
│ │ │ ├── base_multi_edge.h
│ │ │ ├── base_multi_edge.hpp
│ │ │ ├── base_unary_edge.h
│ │ │ ├── base_unary_edge.hpp
│ │ │ ├── base_vertex.h
│ │ │ ├── base_vertex.hpp
│ │ │ ├── batch_stats.cpp
│ │ │ ├── batch_stats.h
│ │ │ ├── block_solver.h
│ │ │ ├── block_solver.hpp
│ │ │ ├── cache.cpp
│ │ │ ├── cache.h
│ │ │ ├── creators.h
│ │ │ ├── eigen_types.h
│ │ │ ├── estimate_propagator.cpp
│ │ │ ├── estimate_propagator.h
│ │ │ ├── factory.cpp
│ │ │ ├── factory.h
│ │ │ ├── hyper_dijkstra.cpp
│ │ │ ├── hyper_dijkstra.h
│ │ │ ├── hyper_graph.cpp
│ │ │ ├── hyper_graph.h
│ │ │ ├── hyper_graph_action.cpp
│ │ │ ├── hyper_graph_action.h
│ │ │ ├── jacobian_workspace.cpp
│ │ │ ├── jacobian_workspace.h
│ │ │ ├── linear_solver.h
│ │ │ ├── marginal_covariance_cholesky.cpp
│ │ │ ├── marginal_covariance_cholesky.h
│ │ │ ├── matrix_operations.h
│ │ │ ├── matrix_structure.cpp
│ │ │ ├── matrix_structure.h
│ │ │ ├── openmp_mutex.h
│ │ │ ├── optimizable_graph.cpp
│ │ │ ├── optimizable_graph.h
│ │ │ ├── optimization_algorithm.cpp
│ │ │ ├── optimization_algorithm.h
│ │ │ ├── optimization_algorithm_dogleg.cpp
│ │ │ ├── optimization_algorithm_dogleg.h
│ │ │ ├── optimization_algorithm_factory.cpp
│ │ │ ├── optimization_algorithm_factory.h
│ │ │ ├── optimization_algorithm_gauss_newton.cpp
│ │ │ ├── optimization_algorithm_gauss_newton.h
│ │ │ ├── optimization_algorithm_levenberg.cpp
│ │ │ ├── optimization_algorithm_levenberg.h
│ │ │ ├── optimization_algorithm_property.h
│ │ │ ├── optimization_algorithm_with_hessian.cpp
│ │ │ ├── optimization_algorithm_with_hessian.h
│ │ │ ├── parameter.cpp
│ │ │ ├── parameter.h
│ │ │ ├── parameter_container.cpp
│ │ │ ├── parameter_container.h
│ │ │ ├── robust_kernel.cpp
│ │ │ ├── robust_kernel.h
│ │ │ ├── robust_kernel_factory.cpp
│ │ │ ├── robust_kernel_factory.h
│ │ │ ├── robust_kernel_impl.cpp
│ │ │ ├── robust_kernel_impl.h
│ │ │ ├── solver.cpp
│ │ │ ├── solver.h
│ │ │ ├── sparse_block_matrix.h
│ │ │ ├── sparse_block_matrix.hpp
│ │ │ ├── sparse_block_matrix_ccs.h
│ │ │ ├── sparse_block_matrix_diagonal.h
│ │ │ ├── sparse_block_matrix_test.cpp
│ │ │ ├── sparse_optimizer.cpp
│ │ │ └── sparse_optimizer.h
│ │ ├── solvers/
│ │ │ ├── linear_solver_dense.h
│ │ │ └── linear_solver_eigen.h
│ │ ├── stuff/
│ │ │ ├── color_macros.h
│ │ │ ├── macros.h
│ │ │ ├── misc.h
│ │ │ ├── os_specific.c
│ │ │ ├── os_specific.h
│ │ │ ├── property.cpp
│ │ │ ├── property.h
│ │ │ ├── string_tools.cpp
│ │ │ ├── string_tools.h
│ │ │ ├── timeutil.cpp
│ │ │ └── timeutil.h
│ │ └── types/
│ │ ├── CMakeLists.txt
│ │ ├── se3_ops.h
│ │ ├── se3_ops.hpp
│ │ ├── se3quat.h
│ │ ├── sim3.h
│ │ ├── types_sba.cpp
│ │ ├── types_sba.h
│ │ ├── types_seven_dof_expmap.cpp
│ │ ├── types_seven_dof_expmap.h
│ │ ├── types_six_dof_expmap.cpp
│ │ └── types_six_dof_expmap.h
│ └── license-bsd.txt
├── cmake_modules/
│ └── FindEigen3.cmake
├── include/
│ ├── core/
│ │ ├── BasicEllipsoidEdges.h
│ │ ├── DataAssociation.h
│ │ ├── Ellipsoid.h
│ │ ├── Frame.h
│ │ ├── FrameDrawer.h
│ │ ├── Geometry.h
│ │ ├── Initializer.h
│ │ ├── Map.h
│ │ ├── MapDrawer.h
│ │ ├── Optimizer.h
│ │ ├── Plane.h
│ │ ├── System.h
│ │ ├── Tracking.h
│ │ └── Viewer.h
│ └── utils/
│ ├── dataprocess_utils.h
│ └── matrix_utils.h
├── install_g2o.sh
└── src/
├── Polygon/
│ ├── CMakeLists.txt
│ ├── Polygon.cpp
│ └── Polygon.hpp
├── config/
│ ├── CMakeLists.txt
│ ├── Config.cpp
│ └── Config.h
├── core/
│ ├── BasicEllipsoidEdges.cpp
│ ├── DataAssociation.cpp
│ ├── Ellipsoid.cpp
│ ├── Frame.cpp
│ ├── FrameDrawer.cpp
│ ├── Geometry.cpp
│ ├── Initializer.cpp
│ ├── Map.cpp
│ ├── MapDrawer.cpp
│ ├── Optimizer.cpp
│ ├── Plane.cpp
│ ├── System.cpp
│ ├── Tracking.cpp
│ └── Viewer.cpp
├── dense_builder/
│ ├── CMakeLists.txt
│ ├── builder.cpp
│ └── builder.h
├── pca/
│ ├── CMakeLists.txt
│ ├── EllipsoidExtractor.cpp
│ └── EllipsoidExtractor.h
├── plane/
│ ├── CMakeLists.txt
│ ├── PlaneExtractor.cpp
│ └── PlaneExtractor.h
├── symmetry/
│ ├── BorderExtractor.cpp
│ ├── BorderExtractor.h
│ ├── CMakeLists.txt
│ ├── PointCloudFilter.cpp
│ ├── PointCloudFilter.h
│ ├── Symmetry.cpp
│ ├── Symmetry.h
│ ├── SymmetrySolver.cpp
│ └── SymmetrySolver.h
├── tum_rgbd/
│ ├── CMakeLists.txt
│ ├── io.cpp
│ └── io.h
└── utils/
├── dataprocess_utils.cpp
└── matrix_utils.cpp
================================================
FILE CONTENTS
================================================
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8)
project(EllipsoidSLAM)
SET(CMAKE_BUILD_TYPE Release)
# SET(CMAKE_BUILD_TYPE Debug)
MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
# Compile with C14
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
# set no warnings
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated")
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
# For Opencv, check environment first
if(NOT DEFINED ENV{OpenCV_DIR})
message("not defined environment variable: OpenCV_DIR")
find_package(OpenCV REQUIRED)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
endif()
else()
message("Environment variable OpenCV_DIR: " $ENV{OpenCV_DIR})
set(OpenCV_DIR $ENV{OpenCV_DIR})
find_package(OpenCV)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV not found.")
endif()
endif()
find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
add_library(utils SHARED
src/utils/matrix_utils.cpp
src/utils/dataprocess_utils.cpp
)
target_link_libraries(utils
${OpenCV_LIBS}
)
add_library(${PROJECT_NAME} SHARED
src/core/Ellipsoid.cpp
src/core/Map.cpp
src/core/MapDrawer.cpp
src/core/Viewer.cpp
src/core/Initializer.cpp
src/core/Geometry.cpp
src/core/System.cpp
src/core/Tracking.cpp
src/core/FrameDrawer.cpp
src/core/Optimizer.cpp
src/core/Frame.cpp
src/core/Plane.cpp
src/core/DataAssociation.cpp
src/core/BasicEllipsoidEdges.cpp
)
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
${Pangolin_LIBRARIES}
utils
Config
symmetry
EllipsoidExtractor
PlaneExtractor
dense_builder
Polygon
)
FIND_PACKAGE( PCL REQUIRED )
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") # use this in Ubuntu 16.04
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS} )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
# add modules
add_subdirectory(src/tum_rgbd)
add_subdirectory(src/dense_builder)
add_subdirectory(src/symmetry)
add_subdirectory(src/config)
add_subdirectory(src/pca)
add_subdirectory(src/plane)
add_subdirectory(src/Polygon)
# interface
add_executable(rgbd
./Example/interface/rgbd.cpp
)
target_link_libraries(rgbd
tum_rgbd
boost_system
EllipsoidSLAM
${PCL_LIBRARIES}
)
================================================
FILE: Example/dataset/cabinet/associate.py
================================================
#!/usr/bin/python
# Software License Agreement (BSD License)
#
# Copyright (c) 2013, Juergen Sturm, TUM
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of TUM nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Requirements:
# sudo apt-get install python-argparse
"""
The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.
For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
"""
import argparse
import sys
import os
import numpy
def read_file_list(filename):
"""
Reads a trajectory from a text file.
File format:
The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.
Input:
filename -- File name
Output:
dict -- dictionary of (stamp,data) tuples
"""
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
return dict(list)
def associate(first_list, second_list,offset,max_difference):
"""
Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
to find the closest match for every input tuple.
Input:
first_list -- first dictionary of (stamp,data) tuples
second_list -- second dictionary of (stamp,data) tuples
offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
max_difference -- search radius for candidate generation
Output:
matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
"""
first_keys = first_list.keys()
second_keys = second_list.keys()
potential_matches = [(abs(a - (b + offset)), a, b)
for a in first_keys
for b in second_keys
if abs(a - (b + offset)) < max_difference]
potential_matches.sort()
matches = []
for diff, a, b in potential_matches:
if a in first_keys and b in second_keys:
first_keys.remove(a)
second_keys.remove(b)
matches.append((a, b))
matches.sort()
return matches
if __name__ == '__main__':
# parse command line
parser = argparse.ArgumentParser(description='''
This script takes two data files with timestamps and associates them
''')
parser.add_argument('first_file', help='first text file (format: timestamp data)')
parser.add_argument('second_file', help='second text file (format: timestamp data)')
parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
args = parser.parse_args()
first_list = read_file_list(args.first_file)
second_list = read_file_list(args.second_file)
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
if args.first_only:
for a,b in matches:
print("%f %s"%(a," ".join(first_list[a])))
else:
for a,b in matches:
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
================================================
FILE: Example/dataset/cabinet/associate.txt
================================================
1341841278.842700 rgb/1341841278.8427.jpg 1341841278.842700 depth/1341841278.8427.png
1341841279.510700 rgb/1341841279.5107.jpg 1341841279.510700 depth/1341841279.5107.png
1341841280.182500 rgb/1341841280.1825.jpg 1341841280.182500 depth/1341841280.1825.png
1341841280.850700 rgb/1341841280.8507.jpg 1341841280.850700 depth/1341841280.8507.png
1341841281.554600 rgb/1341841281.5546.jpg 1341841281.554600 depth/1341841281.5546.png
1341841282.226500 rgb/1341841282.2265.jpg 1341841282.226500 depth/1341841282.2265.png
1341841282.898700 rgb/1341841282.8987.jpg 1341841282.898700 depth/1341841282.8987.png
1341841283.566500 rgb/1341841283.5665.jpg 1341841283.566500 depth/1341841283.5665.png
1341841284.238800 rgb/1341841284.2388.jpg 1341841284.238800 depth/1341841284.2388.png
1341841284.906700 rgb/1341841284.9067.jpg 1341841284.906700 depth/1341841284.9067.png
1341841285.612400 rgb/1341841285.6124.jpg 1341841285.612400 depth/1341841285.6124.png
1341841286.282700 rgb/1341841286.2827.jpg 1341841286.282700 depth/1341841286.2827.png
1341841286.950600 rgb/1341841286.9506.jpg 1341841286.950600 depth/1341841286.9506.png
1341841287.622700 rgb/1341841287.6227.jpg 1341841287.622700 depth/1341841287.6227.png
1341841288.294600 rgb/1341841288.2946.jpg 1341841288.294600 depth/1341841288.2946.png
1341841288.962700 rgb/1341841288.9627.jpg 1341841288.962700 depth/1341841288.9627.png
1341841289.634600 rgb/1341841289.6346.jpg 1341841289.634600 depth/1341841289.6346.png
1341841290.303000 rgb/1341841290.303.jpg 1341841290.303000 depth/1341841290.303.png
1341841290.974500 rgb/1341841290.9745.jpg 1341841290.974500 depth/1341841290.9745.png
1341841291.642600 rgb/1341841291.6426.jpg 1341841291.642600 depth/1341841291.6426.png
1341841292.314700 rgb/1341841292.3147.jpg 1341841292.314700 depth/1341841292.3147.png
1341841292.986600 rgb/1341841292.9866.jpg 1341841292.986600 depth/1341841292.9866.png
1341841293.654800 rgb/1341841293.6548.jpg 1341841293.654800 depth/1341841293.6548.png
1341841294.326600 rgb/1341841294.3266.jpg 1341841294.326600 depth/1341841294.3266.png
1341841294.995200 rgb/1341841294.9952.jpg 1341841294.995200 depth/1341841294.9952.png
1341841295.666600 rgb/1341841295.6666.jpg 1341841295.666600 depth/1341841295.6666.png
1341841296.334800 rgb/1341841296.3348.jpg 1341841296.334800 depth/1341841296.3348.png
1341841297.006500 rgb/1341841297.0065.jpg 1341841297.006500 depth/1341841297.0065.png
1341841297.678600 rgb/1341841297.6786.jpg 1341841297.678600 depth/1341841297.6786.png
1341841298.346600 rgb/1341841298.3466.jpg 1341841298.346600 depth/1341841298.3466.png
1341841299.018500 rgb/1341841299.0185.jpg 1341841299.018500 depth/1341841299.0185.png
1341841299.686600 rgb/1341841299.6866.jpg 1341841299.686600 depth/1341841299.6866.png
1341841300.365600 rgb/1341841300.3656.jpg 1341841300.365600 depth/1341841300.3656.png
1341841301.033600 rgb/1341841301.0336.jpg 1341841301.033600 depth/1341841301.0336.png
1341841301.705700 rgb/1341841301.7057.jpg 1341841301.705700 depth/1341841301.7057.png
1341841302.377600 rgb/1341841302.3776.jpg 1341841302.377600 depth/1341841302.3776.png
1341841303.045600 rgb/1341841303.0456.jpg 1341841303.045600 depth/1341841303.0456.png
1341841303.710600 rgb/1341841303.7106.jpg 1341841303.710600 depth/1341841303.7106.png
1341841304.378700 rgb/1341841304.3787.jpg 1341841304.378700 depth/1341841304.3787.png
1341841305.050600 rgb/1341841305.0506.jpg 1341841305.050600 depth/1341841305.0506.png
1341841305.722700 rgb/1341841305.7227.jpg 1341841305.722700 depth/1341841305.7227.png
1341841306.390700 rgb/1341841306.3907.jpg 1341841306.390700 depth/1341841306.3907.png
1341841307.062700 rgb/1341841307.0627.jpg 1341841307.062700 depth/1341841307.0627.png
1341841307.730600 rgb/1341841307.7306.jpg 1341841307.730600 depth/1341841307.7306.png
1341841308.409900 rgb/1341841308.4099.jpg 1341841308.409900 depth/1341841308.4099.png
1341841309.109500 rgb/1341841309.1095.jpg 1341841309.109500 depth/1341841309.1095.png
1341841309.781600 rgb/1341841309.7816.jpg 1341841309.781600 depth/1341841309.7816.png
1341841310.446800 rgb/1341841310.4468.jpg 1341841310.446800 depth/1341841310.4468.png
1341841311.150700 rgb/1341841311.1507.jpg 1341841311.150700 depth/1341841311.1507.png
1341841311.818600 rgb/1341841311.8186.jpg 1341841311.818600 depth/1341841311.8186.png
1341841312.490500 rgb/1341841312.4905.jpg 1341841312.490500 depth/1341841312.4905.png
1341841313.169600 rgb/1341841313.1696.jpg 1341841313.169600 depth/1341841313.1696.png
1341841313.869500 rgb/1341841313.8695.jpg 1341841313.869500 depth/1341841313.8695.png
1341841314.570600 rgb/1341841314.5706.jpg 1341841314.570600 depth/1341841314.5706.png
1341841315.245600 rgb/1341841315.2456.jpg 1341841315.245600 depth/1341841315.2456.png
1341841315.910800 rgb/1341841315.9108.jpg 1341841315.910800 depth/1341841315.9108.png
1341841316.578600 rgb/1341841316.5786.jpg 1341841316.578600 depth/1341841316.5786.png
1341841317.250600 rgb/1341841317.2506.jpg 1341841317.250600 depth/1341841317.2506.png
================================================
FILE: Example/dataset/cabinet/associateGroundtruth.txt
================================================
1341841278.842700 rgb/1341841278.8427.jpg 1341841278.842700 -2.5508 0.9872 1.1019 -0.4871 0.7673 -0.3519 0.2239
1341841279.510700 rgb/1341841279.5107.jpg 1341841279.510700 -2.4691 1.0757 1.0996 -0.4454 0.7908 -0.3627 0.2114
1341841280.182500 rgb/1341841280.1825.jpg 1341841280.182500 -2.3319 1.1799 1.1107 -0.4077 0.8121 -0.3749 0.1837
1341841280.850700 rgb/1341841280.8507.jpg 1341841280.850700 -2.1921 1.3039 1.112 -0.3546 0.8399 -0.3802 0.1557
1341841281.554600 rgb/1341841281.5546.jpg 1341841281.554600 -2.0579 1.405 1.0916 -0.2848 0.8704 -0.3827 0.1221
1341841282.226500 rgb/1341841282.2265.jpg 1341841282.226500 -1.8585 1.4244 1.0901 -0.2096 0.8898 -0.3963 0.0858
1341841282.898700 rgb/1341841282.8987.jpg 1341841282.898700 -1.6681 1.4397 1.0889 -0.1669 0.8981 -0.4036 0.0515
1341841283.566500 rgb/1341841283.5665.jpg 1341841283.566500 -1.4768 1.494 1.0823 -0.0381 0.9119 -0.408 -0.024
1341841284.238800 rgb/1341841284.2388.jpg 1341841284.238800 -1.3308 1.5641 1.0771 -0.0058 -0.9108 0.4085 0.0598
1341841284.906700 rgb/1341841284.9067.jpg 1341841284.906700 -1.1279 1.5557 1.0799 -0.0682 -0.913 0.3959 0.0705
1341841285.612400 rgb/1341841285.6124.jpg 1341841285.612400 -0.9343 1.4993 1.0839 -0.1445 -0.897 0.4038 0.107
1341841286.282700 rgb/1341841286.2827.jpg 1341841286.282700 -0.7291 1.4183 1.0731 -0.1904 -0.8912 0.3889 0.1349
1341841286.950600 rgb/1341841286.9506.jpg 1341841286.950600 -0.6052 1.3015 1.0644 -0.2859 -0.8628 0.375 0.1821
1341841287.622700 rgb/1341841287.6227.jpg 1341841287.622700 -0.5046 1.1646 1.0717 -0.3173 -0.8491 0.3709 0.2018
1341841288.294600 rgb/1341841288.2946.jpg 1341841288.294600 -0.371 1.0006 1.0469 -0.3852 -0.8263 0.3359 0.2366
1341841288.962700 rgb/1341841288.9627.jpg 1341841288.962700 -0.2798 0.8379 1.041 -0.4218 -0.801 0.3441 0.2491
1341841289.634600 rgb/1341841289.6346.jpg 1341841289.634600 -0.244 0.6645 1.037 -0.4767 -0.7642 0.339 0.2717
1341841290.303000 rgb/1341841290.303.jpg 1341841290.303000 -0.2237 0.5276 1.0291 -0.5267 -0.7302 0.3317 0.2818
1341841290.974500 rgb/1341841290.9745.jpg 1341841290.974500 -0.1652 0.3855 0.992 -0.5928 -0.6755 0.2919 0.3272
1341841291.642600 rgb/1341841291.6426.jpg 1341841291.642600 -0.1583 0.2306 0.9804 -0.5968 -0.6637 0.304 0.333
1341841292.314700 rgb/1341841292.3147.jpg 1341841292.314700 -0.2461 0.138 0.9661 -0.6592 -0.5475 0.3345 0.3922
1341841292.986600 rgb/1341841292.9866.jpg 1341841292.986600 -0.2727 0.0571 0.9256 -0.6528 -0.5484 0.304 0.425
1341841293.654800 rgb/1341841293.6548.jpg 1341841293.654800 -0.2643 -0.0883 0.8924 -0.6995 -0.4827 0.2912 0.4392
1341841294.326600 rgb/1341841294.3266.jpg 1341841294.326600 -0.2652 -0.1506 0.9245 -0.7059 -0.5037 0.2472 0.4323
1341841294.995200 rgb/1341841294.9952.jpg 1341841294.995200 -0.3045 -0.2513 1.0231 -0.7576 -0.4739 0.1949 0.4043
1341841295.666600 rgb/1341841295.6666.jpg 1341841295.666600 -0.3214 -0.3483 1.0935 -0.7881 -0.4131 0.205 0.4077
1341841296.334800 rgb/1341841296.3348.jpg 1341841296.334800 -0.355 -0.5434 1.1132 -0.8137 -0.3519 0.2025 0.4159
1341841297.006500 rgb/1341841297.0065.jpg 1341841297.006500 -0.4216 -0.632 1.0842 -0.8338 -0.3215 0.1805 0.4109
1341841297.678600 rgb/1341841297.6786.jpg 1341841297.678600 -0.5709 -0.7039 1.0905 -0.853 -0.2736 0.1487 0.4189
1341841298.346600 rgb/1341841298.3466.jpg 1341841298.346600 -0.6784 -0.7764 1.0892 -0.8701 -0.2298 0.1137 0.421
1341841299.018500 rgb/1341841299.0185.jpg 1341841299.018500 -0.8697 -0.8985 1.0975 -0.8758 -0.1786 0.1105 0.4346
1341841299.686600 rgb/1341841299.6866.jpg 1341841299.686600 -1.0402 -0.9193 1.0879 -0.8837 -0.1347 0.0856 0.44
1341841300.365600 rgb/1341841300.3656.jpg 1341841300.365600 -1.2054 -0.9138 1.0942 -0.8959 -0.0775 0.0593 0.4334
1341841301.033600 rgb/1341841301.0336.jpg 1341841301.033600 -1.3841 -0.9139 1.1004 -0.9012 -0.034 0.0297 0.431
1341841301.705700 rgb/1341841301.7057.jpg 1341841301.705700 -1.5678 -0.8897 1.09 -0.9056 0.0124 0.0063 0.4239
1341841302.377600 rgb/1341841302.3776.jpg 1341841302.377600 -1.7028 -0.7873 1.097 -0.8994 0.0622 -0.0177 0.4323
1341841303.045600 rgb/1341841303.0456.jpg 1341841303.045600 -1.8151 -0.7055 1.0994 -0.8989 0.1032 -0.0413 0.4238
1341841303.710600 rgb/1341841303.7106.jpg 1341841303.710600 -1.98 -0.6541 1.1132 -0.8913 0.16 -0.067 0.4189
1341841304.378700 rgb/1341841304.3787.jpg 1341841304.378700 -2.1474 -0.6164 1.1051 -0.8818 0.2145 -0.0843 0.4114
1341841305.050600 rgb/1341841305.0506.jpg 1341841305.050600 -2.2441 -0.5275 1.1066 -0.8624 0.2864 -0.0937 0.4067
1341841305.722700 rgb/1341841305.7227.jpg 1341841305.722700 -2.3441 -0.4434 1.1156 -0.8676 0.2986 -0.1047 0.3837
1341841306.390700 rgb/1341841306.3907.jpg 1341841306.390700 -2.3844 -0.3738 1.1478 -0.8363 0.3529 -0.154 0.3903
1341841307.062700 rgb/1341841307.0627.jpg 1341841307.062700 -2.3806 -0.3684 1.1422 -0.8237 0.4174 -0.1444 0.3556
1341841307.730600 rgb/1341841307.7306.jpg 1341841307.730600 -2.4697 -0.2827 1.1886 -0.8146 0.3928 -0.1827 0.3856
1341841308.409900 rgb/1341841308.4099.jpg 1341841308.409900 -2.5874 -0.1709 1.1961 -0.8269 0.3908 -0.1601 0.3712
1341841309.109500 rgb/1341841309.1095.jpg 1341841309.109500 -2.6213 -0.0986 1.2251 -0.8072 0.4179 -0.1686 0.3812
1341841309.781600 rgb/1341841309.7816.jpg 1341841309.781600 -2.6543 -0.0312 1.2287 -0.7813 0.4828 -0.1931 0.3452
1341841310.446800 rgb/1341841310.4468.jpg 1341841310.446800 -2.7015 0.0672 1.2179 -0.7367 0.5665 -0.2032 0.3084
1341841311.150700 rgb/1341841311.1507.jpg 1341841311.150700 -2.7779 0.1718 1.2415 -0.7324 0.5586 -0.2273 0.316
1341841311.818600 rgb/1341841311.8186.jpg 1341841311.818600 -2.7911 0.2175 1.2397 -0.7031 0.5976 -0.2545 0.2893
1341841312.490500 rgb/1341841312.4905.jpg 1341841312.490500 -2.8112 0.3029 1.2555 -0.6862 0.6212 -0.2554 0.2794
1341841313.169600 rgb/1341841313.1696.jpg 1341841313.169600 -2.8302 0.4497 1.2677 -0.6376 0.6605 -0.2868 0.2738
1341841313.869500 rgb/1341841313.8695.jpg 1341841313.869500 -2.8575 0.5893 1.2733 -0.6207 0.6707 -0.3104 0.2617
1341841314.570600 rgb/1341841314.5706.jpg 1341841314.570600 -2.8391 0.6635 1.2827 -0.5653 0.7276 -0.3093 0.2352
1341841315.245600 rgb/1341841315.2456.jpg 1341841315.245600 -2.8206 0.7592 1.2867 -0.5539 0.7424 -0.3032 0.2237
1341841315.910800 rgb/1341841315.9108.jpg 1341841315.910800 -2.7594 0.9179 1.3087 -0.5035 0.7696 -0.3156 0.2336
1341841316.578600 rgb/1341841316.5786.jpg 1341841316.578600 -2.7223 1.0345 1.3228 -0.4835 0.7802 -0.3255 0.2271
1341841317.250600 rgb/1341841317.2506.jpg 1341841317.250600 -2.7302 1.051 1.3149 -0.4803 0.7867 -0.332 0.2003
================================================
FILE: Example/dataset/cabinet/bbox/1341841278.8427.txt
================================================
0 175 24 560 397 28 0.42 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841279.5107.txt
================================================
0 201 40 542 426 28 0.32 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841280.1825.txt
================================================
0 208 54 559 425 28 0.54 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841280.8507.txt
================================================
0 182 56 587 420 28 0.54 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841281.5546.txt
================================================
0 180 41 585 388 28 0.57 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841282.2265.txt
================================================
0 173 57 569 418 28 0.6 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841282.8987.txt
================================================
0 170 55 603 425 28 0.44 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841283.5665.txt
================================================
0 127 52 585 379 28 0.16 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841284.2388.txt
================================================
0 156 49 551 348 28 0.23 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841284.9067.txt
================================================
0 218 56 557 326 28 0.18 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841285.6124.txt
================================================
0 204 62 576 341 28 0.29 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841286.2827.txt
================================================
0 231 51 594 341 28 0.52 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841286.9506.txt
================================================
0 172 40 537 348 28 0.28 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841287.6227.txt
================================================
0 206 49 553 378 28 0.48 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841288.2946.txt
================================================
0 188 28 574 359 28 0.45 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841288.9627.txt
================================================
0 236 40 605 376 28 0.51 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841289.6346.txt
================================================
0 273 57 551 362 28 0.52 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841290.303.txt
================================================
0 291 62 535 352 28 0.28 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841290.9745.txt
================================================
0 260 52 493 344 28 0.58 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841291.6426.txt
================================================
0 312 59 558 351 28 0.33 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841292.3147.txt
================================================
================================================
FILE: Example/dataset/cabinet/bbox/1341841292.9866.txt
================================================
================================================
FILE: Example/dataset/cabinet/bbox/1341841293.6548.txt
================================================
================================================
FILE: Example/dataset/cabinet/bbox/1341841294.3266.txt
================================================
================================================
FILE: Example/dataset/cabinet/bbox/1341841294.9952.txt
================================================
0 168 30 490 359 28 0.28 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841295.6666.txt
================================================
0 162 49 469 371 28 0.31 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841296.3348.txt
================================================
0 173 51 488 368 28 0.42 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841297.0065.txt
================================================
0 164 33 484 326 28 0.25 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841297.6786.txt
================================================
0 142 28 465 336 28 0.48 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841298.3466.txt
================================================
0 109 40 453 321 28 0.36 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841299.0185.txt
================================================
0 151 50 466 313 28 0.28 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841299.6866.txt
================================================
0 153 74 466 317 28 0.28 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841300.3656.txt
================================================
0 131 84 457 309 28 0.23 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841301.0336.txt
================================================
================================================
FILE: Example/dataset/cabinet/bbox/1341841301.7057.txt
================================================
0 151 55 524 295 28 0.2 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841302.3776.txt
================================================
================================================
FILE: Example/dataset/cabinet/bbox/1341841303.0456.txt
================================================
0 155 67 522 347 28 0.24 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841303.7106.txt
================================================
0 163 61 547 364 28 0.39 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841304.3787.txt
================================================
0 175 45 551 368 28 0.61 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841305.0506.txt
================================================
0 131 46 541 365 28 0.7 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841305.7227.txt
================================================
0 185 11 579 347 28 0.72 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841306.3907.txt
================================================
0 132 26 516 402 28 0.69 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841307.0627.txt
================================================
0 85 1 465 359 28 0.63 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841307.7306.txt
================================================
0 144 47 477 429 28 0.53 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841308.4099.txt
================================================
0 200 34 571 387 28 0.4 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841309.1095.txt
================================================
0 221 45 585 421 28 0.29 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841309.7816.txt
================================================
0 159 21 512 399 28 0.33 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841310.4468.txt
================================================
0 122 12 440 335 28 0.38 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841311.1507.txt
================================================
0 202 36 458 357 28 0.25 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841311.8186.txt
================================================
0 148 25 402 329 28 0.21 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841312.4905.txt
================================================
0 157 33 410 321 28 0.25 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841313.1696.txt
================================================
0 193 40 408 358 28 0.22 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841313.8695.txt
================================================
0 215 47 425 356 28 0.18 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841314.5706.txt
================================================
================================================
FILE: Example/dataset/cabinet/bbox/1341841315.2456.txt
================================================
0 204 27 420 322 28 0.18 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841315.9108.txt
================================================
0 192 26 482 383 28 0.5 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841316.5786.txt
================================================
0 212 30 497 391 28 0.33 0
================================================
FILE: Example/dataset/cabinet/bbox/1341841317.2506.txt
================================================
0 185 7 473 382 28 0.34 0
================================================
FILE: Example/dataset/cabinet/depth.txt
================================================
1341841278.8427 depth/1341841278.8427.png
1341841279.5107 depth/1341841279.5107.png
1341841280.1825 depth/1341841280.1825.png
1341841280.8507 depth/1341841280.8507.png
1341841281.5546 depth/1341841281.5546.png
1341841282.2265 depth/1341841282.2265.png
1341841282.8987 depth/1341841282.8987.png
1341841283.5665 depth/1341841283.5665.png
1341841284.2388 depth/1341841284.2388.png
1341841284.9067 depth/1341841284.9067.png
1341841285.6124 depth/1341841285.6124.png
1341841286.2827 depth/1341841286.2827.png
1341841286.9506 depth/1341841286.9506.png
1341841287.6227 depth/1341841287.6227.png
1341841288.2946 depth/1341841288.2946.png
1341841288.9627 depth/1341841288.9627.png
1341841289.6346 depth/1341841289.6346.png
1341841290.303 depth/1341841290.303.png
1341841290.9745 depth/1341841290.9745.png
1341841291.6426 depth/1341841291.6426.png
1341841292.3147 depth/1341841292.3147.png
1341841292.9866 depth/1341841292.9866.png
1341841293.6548 depth/1341841293.6548.png
1341841294.3266 depth/1341841294.3266.png
1341841294.9952 depth/1341841294.9952.png
1341841295.6666 depth/1341841295.6666.png
1341841296.3348 depth/1341841296.3348.png
1341841297.0065 depth/1341841297.0065.png
1341841297.6786 depth/1341841297.6786.png
1341841298.3466 depth/1341841298.3466.png
1341841299.0185 depth/1341841299.0185.png
1341841299.6866 depth/1341841299.6866.png
1341841300.3656 depth/1341841300.3656.png
1341841301.0336 depth/1341841301.0336.png
1341841301.7057 depth/1341841301.7057.png
1341841302.3776 depth/1341841302.3776.png
1341841303.0456 depth/1341841303.0456.png
1341841303.7106 depth/1341841303.7106.png
1341841304.3787 depth/1341841304.3787.png
1341841305.0506 depth/1341841305.0506.png
1341841305.7227 depth/1341841305.7227.png
1341841306.3907 depth/1341841306.3907.png
1341841307.0627 depth/1341841307.0627.png
1341841307.7306 depth/1341841307.7306.png
1341841308.4099 depth/1341841308.4099.png
1341841309.1095 depth/1341841309.1095.png
1341841309.7816 depth/1341841309.7816.png
1341841310.4468 depth/1341841310.4468.png
1341841311.1507 depth/1341841311.1507.png
1341841311.8186 depth/1341841311.8186.png
1341841312.4905 depth/1341841312.4905.png
1341841313.1696 depth/1341841313.1696.png
1341841313.8695 depth/1341841313.8695.png
1341841314.5706 depth/1341841314.5706.png
1341841315.2456 depth/1341841315.2456.png
1341841315.9108 depth/1341841315.9108.png
1341841316.5786 depth/1341841316.5786.png
1341841317.2506 depth/1341841317.2506.png
================================================
FILE: Example/dataset/cabinet/groundtruth.txt
================================================
1341841278.8427 -2.5508 0.9872 1.1019 -0.4871 0.7673 -0.3519 0.2239
1341841279.5107 -2.4691 1.0757 1.0996 -0.4454 0.7908 -0.3627 0.2114
1341841280.1825 -2.3319 1.1799 1.1107 -0.4077 0.8121 -0.3749 0.1837
1341841280.8507 -2.1921 1.3039 1.112 -0.3546 0.8399 -0.3802 0.1557
1341841281.5546 -2.0579 1.405 1.0916 -0.2848 0.8704 -0.3827 0.1221
1341841282.2265 -1.8585 1.4244 1.0901 -0.2096 0.8898 -0.3963 0.0858
1341841282.8987 -1.6681 1.4397 1.0889 -0.1669 0.8981 -0.4036 0.0515
1341841283.5665 -1.4768 1.494 1.0823 -0.0381 0.9119 -0.408 -0.024
1341841284.2388 -1.3308 1.5641 1.0771 -0.0058 -0.9108 0.4085 0.0598
1341841284.9067 -1.1279 1.5557 1.0799 -0.0682 -0.913 0.3959 0.0705
1341841285.6124 -0.9343 1.4993 1.0839 -0.1445 -0.897 0.4038 0.107
1341841286.2827 -0.7291 1.4183 1.0731 -0.1904 -0.8912 0.3889 0.1349
1341841286.9506 -0.6052 1.3015 1.0644 -0.2859 -0.8628 0.375 0.1821
1341841287.6227 -0.5046 1.1646 1.0717 -0.3173 -0.8491 0.3709 0.2018
1341841288.2946 -0.371 1.0006 1.0469 -0.3852 -0.8263 0.3359 0.2366
1341841288.9627 -0.2798 0.8379 1.041 -0.4218 -0.801 0.3441 0.2491
1341841289.6346 -0.244 0.6645 1.037 -0.4767 -0.7642 0.339 0.2717
1341841290.303 -0.2237 0.5276 1.0291 -0.5267 -0.7302 0.3317 0.2818
1341841290.9745 -0.1652 0.3855 0.992 -0.5928 -0.6755 0.2919 0.3272
1341841291.6426 -0.1583 0.2306 0.9804 -0.5968 -0.6637 0.304 0.333
1341841292.3147 -0.2461 0.138 0.9661 -0.6592 -0.5475 0.3345 0.3922
1341841292.9866 -0.2727 0.0571 0.9256 -0.6528 -0.5484 0.304 0.425
1341841293.6548 -0.2643 -0.0883 0.8924 -0.6995 -0.4827 0.2912 0.4392
1341841294.3266 -0.2652 -0.1506 0.9245 -0.7059 -0.5037 0.2472 0.4323
1341841294.9952 -0.3045 -0.2513 1.0231 -0.7576 -0.4739 0.1949 0.4043
1341841295.6666 -0.3214 -0.3483 1.0935 -0.7881 -0.4131 0.205 0.4077
1341841296.3348 -0.355 -0.5434 1.1132 -0.8137 -0.3519 0.2025 0.4159
1341841297.0065 -0.4216 -0.632 1.0842 -0.8338 -0.3215 0.1805 0.4109
1341841297.6786 -0.5709 -0.7039 1.0905 -0.853 -0.2736 0.1487 0.4189
1341841298.3466 -0.6784 -0.7764 1.0892 -0.8701 -0.2298 0.1137 0.421
1341841299.0185 -0.8697 -0.8985 1.0975 -0.8758 -0.1786 0.1105 0.4346
1341841299.6866 -1.0402 -0.9193 1.0879 -0.8837 -0.1347 0.0856 0.44
1341841300.3656 -1.2054 -0.9138 1.0942 -0.8959 -0.0775 0.0593 0.4334
1341841301.0336 -1.3841 -0.9139 1.1004 -0.9012 -0.034 0.0297 0.431
1341841301.7057 -1.5678 -0.8897 1.09 -0.9056 0.0124 0.0063 0.4239
1341841302.3776 -1.7028 -0.7873 1.097 -0.8994 0.0622 -0.0177 0.4323
1341841303.0456 -1.8151 -0.7055 1.0994 -0.8989 0.1032 -0.0413 0.4238
1341841303.7106 -1.98 -0.6541 1.1132 -0.8913 0.16 -0.067 0.4189
1341841304.3787 -2.1474 -0.6164 1.1051 -0.8818 0.2145 -0.0843 0.4114
1341841305.0506 -2.2441 -0.5275 1.1066 -0.8624 0.2864 -0.0937 0.4067
1341841305.7227 -2.3441 -0.4434 1.1156 -0.8676 0.2986 -0.1047 0.3837
1341841306.3907 -2.3844 -0.3738 1.1478 -0.8363 0.3529 -0.154 0.3903
1341841307.0627 -2.3806 -0.3684 1.1422 -0.8237 0.4174 -0.1444 0.3556
1341841307.7306 -2.4697 -0.2827 1.1886 -0.8146 0.3928 -0.1827 0.3856
1341841308.4099 -2.5874 -0.1709 1.1961 -0.8269 0.3908 -0.1601 0.3712
1341841309.1095 -2.6213 -0.0986 1.2251 -0.8072 0.4179 -0.1686 0.3812
1341841309.7816 -2.6543 -0.0312 1.2287 -0.7813 0.4828 -0.1931 0.3452
1341841310.4468 -2.7015 0.0672 1.2179 -0.7367 0.5665 -0.2032 0.3084
1341841311.1507 -2.7779 0.1718 1.2415 -0.7324 0.5586 -0.2273 0.316
1341841311.8186 -2.7911 0.2175 1.2397 -0.7031 0.5976 -0.2545 0.2893
1341841312.4905 -2.8112 0.3029 1.2555 -0.6862 0.6212 -0.2554 0.2794
1341841313.1696 -2.8302 0.4497 1.2677 -0.6376 0.6605 -0.2868 0.2738
1341841313.8695 -2.8575 0.5893 1.2733 -0.6207 0.6707 -0.3104 0.2617
1341841314.5706 -2.8391 0.6635 1.2827 -0.5653 0.7276 -0.3093 0.2352
1341841315.2456 -2.8206 0.7592 1.2867 -0.5539 0.7424 -0.3032 0.2237
1341841315.9108 -2.7594 0.9179 1.3087 -0.5035 0.7696 -0.3156 0.2336
1341841316.5786 -2.7223 1.0345 1.3228 -0.4835 0.7802 -0.3255 0.2271
1341841317.2506 -2.7302 1.051 1.3149 -0.4803 0.7867 -0.332 0.2003
================================================
FILE: Example/dataset/cabinet/rgb.txt
================================================
1341841278.8427 rgb/1341841278.8427.jpg
1341841279.5107 rgb/1341841279.5107.jpg
1341841280.1825 rgb/1341841280.1825.jpg
1341841280.8507 rgb/1341841280.8507.jpg
1341841281.5546 rgb/1341841281.5546.jpg
1341841282.2265 rgb/1341841282.2265.jpg
1341841282.8987 rgb/1341841282.8987.jpg
1341841283.5665 rgb/1341841283.5665.jpg
1341841284.2388 rgb/1341841284.2388.jpg
1341841284.9067 rgb/1341841284.9067.jpg
1341841285.6124 rgb/1341841285.6124.jpg
1341841286.2827 rgb/1341841286.2827.jpg
1341841286.9506 rgb/1341841286.9506.jpg
1341841287.6227 rgb/1341841287.6227.jpg
1341841288.2946 rgb/1341841288.2946.jpg
1341841288.9627 rgb/1341841288.9627.jpg
1341841289.6346 rgb/1341841289.6346.jpg
1341841290.303 rgb/1341841290.303.jpg
1341841290.9745 rgb/1341841290.9745.jpg
1341841291.6426 rgb/1341841291.6426.jpg
1341841292.3147 rgb/1341841292.3147.jpg
1341841292.9866 rgb/1341841292.9866.jpg
1341841293.6548 rgb/1341841293.6548.jpg
1341841294.3266 rgb/1341841294.3266.jpg
1341841294.9952 rgb/1341841294.9952.jpg
1341841295.6666 rgb/1341841295.6666.jpg
1341841296.3348 rgb/1341841296.3348.jpg
1341841297.0065 rgb/1341841297.0065.jpg
1341841297.6786 rgb/1341841297.6786.jpg
1341841298.3466 rgb/1341841298.3466.jpg
1341841299.0185 rgb/1341841299.0185.jpg
1341841299.6866 rgb/1341841299.6866.jpg
1341841300.3656 rgb/1341841300.3656.jpg
1341841301.0336 rgb/1341841301.0336.jpg
1341841301.7057 rgb/1341841301.7057.jpg
1341841302.3776 rgb/1341841302.3776.jpg
1341841303.0456 rgb/1341841303.0456.jpg
1341841303.7106 rgb/1341841303.7106.jpg
1341841304.3787 rgb/1341841304.3787.jpg
1341841305.0506 rgb/1341841305.0506.jpg
1341841305.7227 rgb/1341841305.7227.jpg
1341841306.3907 rgb/1341841306.3907.jpg
1341841307.0627 rgb/1341841307.0627.jpg
1341841307.7306 rgb/1341841307.7306.jpg
1341841308.4099 rgb/1341841308.4099.jpg
1341841309.1095 rgb/1341841309.1095.jpg
1341841309.7816 rgb/1341841309.7816.jpg
1341841310.4468 rgb/1341841310.4468.jpg
1341841311.1507 rgb/1341841311.1507.jpg
1341841311.8186 rgb/1341841311.8186.jpg
1341841312.4905 rgb/1341841312.4905.jpg
1341841313.1696 rgb/1341841313.1696.jpg
1341841313.8695 rgb/1341841313.8695.jpg
1341841314.5706 rgb/1341841314.5706.jpg
1341841315.2456 rgb/1341841315.2456.jpg
1341841315.9108 rgb/1341841315.9108.jpg
1341841316.5786 rgb/1341841316.5786.jpg
1341841317.2506 rgb/1341841317.2506.jpg
================================================
FILE: Example/interface/rgbd.cpp
================================================
#include "core/Initializer.h"
#include "core/Geometry.h"
#include "utils/dataprocess_utils.h"
#include "utils/matrix_utils.h"
#include <Eigen/Core>
#include "include/core/Viewer.h"
#include "include/core/MapDrawer.h"
#include "include/core/Map.h"
#include <thread>
#include <string>
#include "include/core/Ellipsoid.h"
#include "src/tum_rgbd/io.h"
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudPCL;
#include "src/config/Config.h"
using namespace std;
using namespace Eigen;
int main(int argc,char* argv[]) {
if( argc != 3)
{
std::cout << "usage: " << argv[0] << " path_to_settings path_to_dataset" << std::endl;
return 1;
}
string strSettingPath = string(argv[1]);
string dataset_path(argv[2]);
string strDetectionDir = dataset_path + "bbox/";
std::cout << "- settings file: " << strSettingPath << std::endl;
std::cout << "- dataset_path: " << dataset_path << std::endl;
std::cout << "- strDetectionDir: " << strDetectionDir << std::endl;
TUMRGBD::Dataset loader;
loader.loadDataset(dataset_path);
loader.loadDetectionDir(strDetectionDir);
EllipsoidSLAM::System SLAM(strSettingPath, true);
cv::Mat rgb,depth;
VectorXd pose;
while(!loader.empty())
{
bool valid = loader.readFrame(rgb,depth,pose);
int current_id = loader.getCurrentID();
Eigen::MatrixXd detMat = loader.getDetectionMat();
double timestamp = loader.GetCurrentTimestamp();
if(valid)
{
std::cout << "*****************************" << std::endl;
std::cout << "Press [ENTER] to continue ... " << std::endl;
std::cout << "*****************************" << std::endl;
getchar();
SLAM.TrackWithObjects(timestamp, pose, detMat, depth, rgb, true); // Process frame.
std::cout << std::endl;
}
std::cout << " -> " << loader.getCurrentID() << "/" << loader.getTotalNum() << std::endl;
}
std::cout << "Finished all data." << std::endl;
// save objects
string output_path("./objects.txt");
SLAM.SaveObjectsToFile(output_path);
SLAM.getTracker()->SaveObjectHistory("./object_history.txt");
cout << "Use Ctrl+C to quit." << endl;
while(1);
cout << "End." << endl;
return 0;
}
================================================
FILE: Example/param/TUM3.yaml
================================================
%YAML:1.0
Dataset.Type: "TUM"
# ------------------------------------
# Single-frame ellipsoid estimation
# ------------------------------------
# Pointcloud segmentation parameters
EllipsoidExtraction.Euclidean.ClusterTolerance: 0.02
EllipsoidExtraction.Euclidean.MinClusterSize: 100
EllipsoidExtraction.Euclidean.CenterDis: 0.5
# weights of 3d, 2d edges in optimization
Optimizer.Edges.3DEllipsoid.Scale: 10000
Optimizer.Edges.2D.Scale: 1
# ------------------------------
# Symmetry Estimation
# ------------------------------
# Whether open symmetry estimation
EllipsoidExtraction.Symmetry.Open: 1
# Downsample grid size. Extremely influence the time and efficiency.
EllipsoidExtraction.Symmetry.GridSize: 0.1
# Parameters for symmetry probability
SymmetrySolver.Sigma: 0.1
# ----------------------------------
# Symmetry types of semantic labels
# ----------------------------------
# Please see the definitions in the function:
# EllipsoidExtractor::LoadSymmetryPrior()
# -------------------------------
# Ground Plane Extraction
# -------------------------------
Plane.MinSize: 200
Plane.AngleThreshold: 5
Plane.DistanceThreshold: 0.1
# Whether use the normal of the groundplane to constrain the ellipsoid
Optimizer.Edges.GravityPrior.Open: 1
Optimizer.Edges.GravityPrior.Scale: 100
# ------------------------------
# Other Parameters
# ------------------------------
# Bounding box filter
Measurement.Border.Pixels: 10
Measurement.LengthLimit.Pixels: 0
# A observation will only be valid when the robot has a change of view.
# Close it to consider every observations.
Tracking.KeyFrameCheck.Close: 1
#--------------------------------------------------------------------------------------------
# Camera Parameters.
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 535.4
Camera.fy: 539.2
Camera.cx: 320.1
Camera.cy: 247.6
Camera.width: 640
Camera.height: 480
Camera.scale: 5000.0
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
Viewer.CameraSize: 0.1
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
================================================
FILE: LICENSE
================================================
BSD 3-Clause License
Copyright (c) 2020, Liao Ziwei
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
================================================
FILE: README.md
================================================
# EllipsoidSLAM
## Update
### Aug 22, 2021
* Now support Ubuntu 20.04 and OpenCV 4.2
* Fix bugs for crushes
## Introduction
We propose a sparse object-level SLAM using Quadrics and Symmetry Properties for indoor environments. The algorithm is specially designed for mobile robots mounting an RGB-D camera. The algorithm takes bounding boxes generated from object detection and also the point cloud from the RGB-D frame to estimate the pose and occupy space of objects. Since ellipsoids are taken as the object representation, we name it EllipsoidSLAM.
We have released a C++ implementation and a demo trajectory. We need to point out that this code is only a basic demo:
* The core modules of Groundplane Extraction, Ellipsoid Estimation, and Symmetry Estimation are basic versions. They may not have full performance. Please see the paper for the complete framework.
* By default, only mapping is supported. If you want, it's possible to make simple changes to the Optimizer to enable the SLAM mode.
## Author
Ziwei Liao et al., Robotics Institute, School of Mechanical Engineering & Automation, Beihang University, Beijing, China (Email: liaoziwei{at}buaa.edu.cn)
## Related Paper
Please cite the following papers when you found this code useful for your research. Welcome to read our new paper [2], which proposes two RGB-D observation models for quadrics, and introduces an automatic data association method.
[1] Ziwei Liao, Wei Wang, Xianyu Qi, Xiaoyu Zhang, Lin Xue, Jianzhen Jiao, Ran Wei, Object-oriented SLAM using Quadrics and Symmetry Properties for Indoor Environments. arXiv 2020. [[pdf]](https://arxiv.org/abs/2004.05303
) [[Video]](https://www.youtube.com/watch?v=u9zRBp4TPIs
)
[2] Liao, Z.; Wang, W.; Qi, X.; Zhang, X. RGB-D Object SLAM Using Quadrics for Indoor Environments. Sensors 2020, 20, 5150. [[pdf]](https://www.mdpi.com/1424-8220/20/18/5150/pdf)
## Codes
### Dependencies
The code has been tested on Ubuntu 16.04/18.04. The main dependencies are:
* OpenCV (4.X recommended)
* PCL 1.7+
* Pangolin
* g2o
For g2o, a modified version with SE3 transformation has been attached in the code. Use one simple command to compile it:
$ sh install_g2o.sh
### Build
These commands will automatically generate the Core module and an interface for RGB-D dataset:
$ mkdir build
$ cd build
$ cmake ..
$ make -j
if there occurs a linking problem, add the lib directory to the environment variable:
export LD_LIBRARY_PATH={YOUR_SOUCE_CODE_PATH}/lib:$LD_LIBRARY_PATH
## Demos
### TUM-Cabinet
The codes contain a trajectory of fr3_cabinet, which belongs to the TUM-RGB-D dataset. The bounding boxes are generated by YOLO. Just use one command to run the demo:
#### Run
./build/rgbd ./Example/param/TUM3.yaml ./Example/dataset/cabinet/
Please press Enter in the console to see the result of every frame. A visualization tool based on Pangolin is offered to visualize the point cloud, the symmetry planes, the ground plane, and the ellipsoids.
### Your Own Dataset
First, please run object detector like YOLO to generate bounding boxes and store the result as text files, with each line containing:
* id x1 y1 x2 y2 label probability instance
where, (x1,y1), (x2,y2) are the top-left, bottom-right corners. Multiple objects are supported, however, you need to manually specify the data association in [instance].
Second, keep the directory structure the same as the demo, then run the RGB-D interface. For the best effect, you may need to check the ground plane extraction and the point cloud segmentation.
## Notes
* All the important parameters could be adjusted in the .yaml file. See the comments in the file for details.
* The code is released under the BSD license. Feel free to adjust it as you like for research. Please cite our paper in your publications if you feel it helpful.
* The code referred to several open-source SLAM codes, thanks to their great work: [ORBSLAM](https://github.com/raulmur/ORB_SLAM2), [CubeSLAM](https://github.com/shichaoy/cube_slam).
* If you have any further questions, feel free to contact the author: liaoziwei{at}buaa.edu.cn
================================================
FILE: Thirdparty/g2o/CMakeLists.txt
================================================
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") # add by me. synced with flags in my cmakelist.txt
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)
# For building the CHOLMOD / CSPARSE solvers
FIND_PACKAGE(BLAS REQUIRED)
FIND_PACKAGE(LAPACK REQUIRED)
# 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 #comment by me
# SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native")
# SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native")
# 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_sba.cpp
g2o/types/types_six_dof_expmap.h
g2o/types/types_six_dof_expmap.cpp
g2o/types/types_seven_dof_expmap.h
g2o/types/types_seven_dof_expmap.cpp
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/eigen_types.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/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
)
================================================
FILE: Thirdparty/g2o/README.txt
================================================
Modified g2o version
See the original g2o library at: https://github.com/RainerKuemmerle/g2o
All files included in this g2o version are BSD, see license-bsd.txt
================================================
FILE: Thirdparty/g2o/cmake_modules/FindBLAS.cmake
================================================
# Find BLAS library
#
# This module finds an installed library that implements the BLAS
# linear-algebra interface (see http://www.netlib.org/blas/).
# The list of libraries searched for is mainly taken
# from the autoconf macro file, acx_blas.m4 (distributed at
# http://ac-archive.sourceforge.net/ac-archive/acx_blas.html).
#
# This module sets the following variables:
# BLAS_FOUND - set to true if a library implementing the BLAS interface
# is found
# BLAS_INCLUDE_DIR - Directories containing the BLAS header files
# BLAS_DEFINITIONS - Compilation options to use BLAS
# BLAS_LINKER_FLAGS - Linker flags to use BLAS (excluding -l
# and -L).
# BLAS_LIBRARIES_DIR - Directories containing the BLAS libraries.
# May be null if BLAS_LIBRARIES contains libraries name using full path.
# BLAS_LIBRARIES - List of libraries to link against BLAS interface.
# May be null if the compiler supports auto-link (e.g. VC++).
# BLAS_USE_FILE - The name of the cmake module to include to compile
# applications or libraries using BLAS.
#
# This module was modified by CGAL team:
# - find libraries for a C++ compiler, instead of Fortran
# - added BLAS_INCLUDE_DIR, BLAS_DEFINITIONS and BLAS_LIBRARIES_DIR
# - removed BLAS95_LIBRARIES
include(CheckFunctionExists)
# This macro checks for the existence of the combination of fortran libraries
# given by _list. If the combination is found, this macro checks (using the
# check_function_exists macro) whether can link against that library
# combination using the name of a routine given by _name using the linker
# flags given by _flags. If the combination of libraries is found and passes
# the link test, LIBRARIES is set to the list of complete library paths that
# have been found and DEFINITIONS to the required definitions.
# Otherwise, LIBRARIES is set to FALSE.
# N.B. _prefix is the prefix applied to the names of all cached variables that
# are generated internally and marked advanced by this macro.
macro(check_fortran_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _path)
#message("DEBUG: check_fortran_libraries(${_list} in ${_path})")
# Check for the existence of the libraries given by _list
set(_libraries_found TRUE)
set(_libraries_work FALSE)
set(${DEFINITIONS} "")
set(${LIBRARIES} "")
set(_combined_name)
foreach(_library ${_list})
set(_combined_name ${_combined_name}_${_library})
if(_libraries_found)
# search first in ${_path}
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS ${_path} NO_DEFAULT_PATH
)
# if not found, search in environment variables and system
if ( WIN32 )
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS ENV LIB
)
elseif ( APPLE )
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
)
else ()
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
)
endif()
mark_as_advanced(${_prefix}_${_library}_LIBRARY)
set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
endif(_libraries_found)
endforeach(_library ${_list})
if(_libraries_found)
set(_libraries_found ${${LIBRARIES}})
endif()
# Test this combination of libraries with the Fortran/f2c interface.
# We test the Fortran interface first as it is well standardized.
if(_libraries_found AND NOT _libraries_work)
set(${DEFINITIONS} "-D${_prefix}_USE_F2C")
set(${LIBRARIES} ${_libraries_found})
# Some C++ linkers require the f2c library to link with Fortran libraries.
# I do not know which ones, thus I just add the f2c library if it is available.
find_package( F2C QUIET )
if ( F2C_FOUND )
set(${DEFINITIONS} ${${DEFINITIONS}} ${F2C_DEFINITIONS})
set(${LIBRARIES} ${${LIBRARIES}} ${F2C_LIBRARIES})
endif()
set(CMAKE_REQUIRED_DEFINITIONS ${${DEFINITIONS}})
set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}})
#message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
#message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
# Check if function exists with f2c calling convention (ie a trailing underscore)
check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
set(CMAKE_REQUIRED_DEFINITIONS} "")
set(CMAKE_REQUIRED_LIBRARIES "")
mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
endif(_libraries_found AND NOT _libraries_work)
# If not found, test this combination of libraries with a C interface.
# A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
if(_libraries_found AND NOT _libraries_work)
set(${DEFINITIONS} "")
set(${LIBRARIES} ${_libraries_found})
set(CMAKE_REQUIRED_DEFINITIONS "")
set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}})
#message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
set(CMAKE_REQUIRED_LIBRARIES "")
mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
endif(_libraries_found AND NOT _libraries_work)
# on failure
if(NOT _libraries_work)
set(${DEFINITIONS} "")
set(${LIBRARIES} FALSE)
endif()
#message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
#message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
endmacro(check_fortran_libraries)
#
# main
#
# Is it already configured?
if (BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
set(BLAS_FOUND TRUE)
else()
# reset variables
set( BLAS_INCLUDE_DIR "" )
set( BLAS_DEFINITIONS "" )
set( BLAS_LINKER_FLAGS "" )
set( BLAS_LIBRARIES "" )
set( BLAS_LIBRARIES_DIR "" )
#
# If Unix, search for BLAS function in possible libraries
#
# BLAS in ATLAS library? (http://math-atlas.sourceforge.net/)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"cblas;f77blas;atlas"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
# BLAS in PhiPACK libraries? (requires generic BLAS lib, too)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"sgemm;dgemm;blas"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
# BLAS in Alpha CXML library?
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"cxml"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
# BLAS in Alpha DXML library? (now called CXML, see above)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"dxml"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
# BLAS in Sun Performance library?
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
"-xlic_lib=sunperf"
"sunperf;sunmath"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
if(BLAS_LIBRARIES)
# Extra linker flag
set(BLAS_LINKER_FLAGS "-xlic_lib=sunperf")
endif()
endif()
# BLAS in SCSL library? (SGI/Cray Scientific Library)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"scsl"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
# BLAS in SGIMATH library?
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"complib.sgimath"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
# BLAS in IBM ESSL library? (requires generic BLAS lib, too)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"essl;blas"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
#BLAS in intel mkl 10 library? (em64t 64bit)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"mkl_intel_lp64;mkl_intel_thread;mkl_core;guide;pthread"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
### windows version of intel mkl 10?
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
SGEMM
""
"mkl_c_dll;mkl_intel_thread_dll;mkl_core_dll;libguide40"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
#older versions of intel mkl libs
# BLAS in intel mkl library? (shared)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"mkl;guide;pthread"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
#BLAS in intel mkl library? (static, 32bit)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"mkl_ia32;guide;pthread"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
#BLAS in intel mkl library? (static, em64t 64bit)
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"mkl_em64t;guide;pthread"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
#BLAS in acml library?
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"acml"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
# Apple BLAS library?
if(NOT BLAS_LIBRARIES)
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"Accelerate"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
if ( NOT BLAS_LIBRARIES )
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"vecLib"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif ( NOT BLAS_LIBRARIES )
# Generic BLAS library?
# This configuration *must* be the last try as this library is notably slow.
if ( NOT BLAS_LIBRARIES )
check_fortran_libraries(
BLAS_DEFINITIONS
BLAS_LIBRARIES
BLAS
sgemm
""
"blas"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
)
endif()
if(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
set(BLAS_FOUND TRUE)
else()
set(BLAS_FOUND FALSE)
endif()
if(NOT BLAS_FIND_QUIETLY)
if(BLAS_FOUND)
message(STATUS "A library with BLAS API found.")
else(BLAS_FOUND)
if(BLAS_FIND_REQUIRED)
message(FATAL_ERROR "A required library with BLAS API not found. Please specify library location.")
else()
message(STATUS "A library with BLAS API not found. Please specify library location.")
endif()
endif(BLAS_FOUND)
endif(NOT BLAS_FIND_QUIETLY)
# Add variables to cache
set( BLAS_INCLUDE_DIR "${BLAS_INCLUDE_DIR}"
CACHE PATH "Directories containing the BLAS header files" FORCE )
set( BLAS_DEFINITIONS "${BLAS_DEFINITIONS}"
CACHE STRING "Compilation options to use BLAS" FORCE )
set( BLAS_LINKER_FLAGS "${BLAS_LINKER_FLAGS}"
CACHE STRING "Linker flags to use BLAS" FORCE )
set( BLAS_LIBRARIES "${BLAS_LIBRARIES}"
CACHE FILEPATH "BLAS libraries name" FORCE )
set( BLAS_LIBRARIES_DIR "${BLAS_LIBRARIES_DIR}"
CACHE PATH "Directories containing the BLAS libraries" FORCE )
#message("DEBUG: BLAS_INCLUDE_DIR = ${BLAS_INCLUDE_DIR}")
#message("DEBUG: BLAS_DEFINITIONS = ${BLAS_DEFINITIONS}")
#message("DEBUG: BLAS_LINKER_FLAGS = ${BLAS_LINKER_FLAGS}")
#message("DEBUG: BLAS_LIBRARIES = ${BLAS_LIBRARIES}")
#message("DEBUG: BLAS_LIBRARIES_DIR = ${BLAS_LIBRARIES_DIR}")
#message("DEBUG: BLAS_FOUND = ${BLAS_FOUND}")
endif(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
================================================
FILE: Thirdparty/g2o/cmake_modules/FindEigen3.cmake
================================================
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
# specific additional paths for some OS
if (WIN32)
set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
endif(WIN32)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${EIGEN_ADDITIONAL_SEARCH_PATHS}
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN3_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)
================================================
FILE: Thirdparty/g2o/cmake_modules/FindLAPACK.cmake
================================================
# Find LAPACK library
#
# This module finds an installed library that implements the LAPACK
# linear-algebra interface (see http://www.netlib.org/lapack/).
# The approach follows mostly that taken for the autoconf macro file, acx_lapack.m4
# (distributed at http://ac-archive.sourceforge.net/ac-archive/acx_lapack.html).
#
# This module sets the following variables:
# LAPACK_FOUND - set to true if a library implementing the LAPACK interface
# is found
# LAPACK_INCLUDE_DIR - Directories containing the LAPACK header files
# LAPACK_DEFINITIONS - Compilation options to use LAPACK
# LAPACK_LINKER_FLAGS - Linker flags to use LAPACK (excluding -l
# and -L).
# LAPACK_LIBRARIES_DIR - Directories containing the LAPACK libraries.
# May be null if LAPACK_LIBRARIES contains libraries name using full path.
# LAPACK_LIBRARIES - List of libraries to link against LAPACK interface.
# May be null if the compiler supports auto-link (e.g. VC++).
# LAPACK_USE_FILE - The name of the cmake module to include to compile
# applications or libraries using LAPACK.
#
# This module was modified by CGAL team:
# - find libraries for a C++ compiler, instead of Fortran
# - added LAPACK_INCLUDE_DIR, LAPACK_DEFINITIONS and LAPACK_LIBRARIES_DIR
# - removed LAPACK95_LIBRARIES
include(CheckFunctionExists)
# This macro checks for the existence of the combination of fortran libraries
# given by _list. If the combination is found, this macro checks (using the
# check_function_exists macro) whether can link against that library
# combination using the name of a routine given by _name using the linker
# flags given by _flags. If the combination of libraries is found and passes
# the link test, LIBRARIES is set to the list of complete library paths that
# have been found and DEFINITIONS to the required definitions.
# Otherwise, LIBRARIES is set to FALSE.
# N.B. _prefix is the prefix applied to the names of all cached variables that
# are generated internally and marked advanced by this macro.
macro(check_lapack_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _blas _path)
#message("DEBUG: check_lapack_libraries(${_list} in ${_path} with ${_blas})")
# Check for the existence of the libraries given by _list
set(_libraries_found TRUE)
set(_libraries_work FALSE)
set(${DEFINITIONS} "")
set(${LIBRARIES} "")
set(_combined_name)
foreach(_library ${_list})
set(_combined_name ${_combined_name}_${_library})
if(_libraries_found)
# search first in ${_path}
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS ${_path} NO_DEFAULT_PATH
)
# if not found, search in environment variables and system
if ( WIN32 )
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS ENV LIB
)
elseif ( APPLE )
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
)
else ()
find_library(${_prefix}_${_library}_LIBRARY
NAMES ${_library}
PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
)
endif()
mark_as_advanced(${_prefix}_${_library}_LIBRARY)
set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
endif(_libraries_found)
endforeach(_library ${_list})
if(_libraries_found)
set(_libraries_found ${${LIBRARIES}})
endif()
# Test this combination of libraries with the Fortran/f2c interface.
# We test the Fortran interface first as it is well standardized.
if(_libraries_found AND NOT _libraries_work)
set(${DEFINITIONS} "-D${_prefix}_USE_F2C")
set(${LIBRARIES} ${_libraries_found})
# Some C++ linkers require the f2c library to link with Fortran libraries.
# I do not know which ones, thus I just add the f2c library if it is available.
find_package( F2C QUIET )
if ( F2C_FOUND )
set(${DEFINITIONS} ${${DEFINITIONS}} ${F2C_DEFINITIONS})
set(${LIBRARIES} ${${LIBRARIES}} ${F2C_LIBRARIES})
endif()
set(CMAKE_REQUIRED_DEFINITIONS ${${DEFINITIONS}})
set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}} ${_blas})
#message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
#message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
# Check if function exists with f2c calling convention (ie a trailing underscore)
check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
set(CMAKE_REQUIRED_DEFINITIONS} "")
set(CMAKE_REQUIRED_LIBRARIES "")
mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
endif(_libraries_found AND NOT _libraries_work)
# If not found, test this combination of libraries with a C interface.
# A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
if(_libraries_found AND NOT _libraries_work)
set(${DEFINITIONS} "")
set(${LIBRARIES} ${_libraries_found})
set(CMAKE_REQUIRED_DEFINITIONS "")
set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}} ${_blas})
#message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
set(CMAKE_REQUIRED_LIBRARIES "")
mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
endif(_libraries_found AND NOT _libraries_work)
# on failure
if(NOT _libraries_work)
set(${DEFINITIONS} "")
set(${LIBRARIES} FALSE)
endif()
#message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
#message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
endmacro(check_lapack_libraries)
#
# main
#
# LAPACK requires BLAS
if(LAPACK_FIND_QUIETLY OR NOT LAPACK_FIND_REQUIRED)
find_package(BLAS)
else()
find_package(BLAS REQUIRED)
endif()
if (NOT BLAS_FOUND)
message(STATUS "LAPACK requires BLAS.")
set(LAPACK_FOUND FALSE)
# Is it already configured?
elseif (LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
set(LAPACK_FOUND TRUE)
else()
# reset variables
set( LAPACK_INCLUDE_DIR "" )
set( LAPACK_DEFINITIONS "" )
set( LAPACK_LINKER_FLAGS "" ) # unused (yet)
set( LAPACK_LIBRARIES "" )
set( LAPACK_LIBRARIES_DIR "" )
#
# If Unix, search for LAPACK function in possible libraries
#
#intel mkl lapack?
if(NOT LAPACK_LIBRARIES)
check_lapack_libraries(
LAPACK_DEFINITIONS
LAPACK_LIBRARIES
LAPACK
cheev
""
"mkl_lapack"
"${BLAS_LIBRARIES}"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
)
endif()
#acml lapack?
if(NOT LAPACK_LIBRARIES)
check_lapack_libraries(
LAPACK_DEFINITIONS
LAPACK_LIBRARIES
LAPACK
cheev
""
"acml"
"${BLAS_LIBRARIES}"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
)
endif()
# Apple LAPACK library?
if(NOT LAPACK_LIBRARIES)
check_lapack_libraries(
LAPACK_DEFINITIONS
LAPACK_LIBRARIES
LAPACK
cheev
""
"Accelerate"
"${BLAS_LIBRARIES}"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
)
endif()
if ( NOT LAPACK_LIBRARIES )
check_lapack_libraries(
LAPACK_DEFINITIONS
LAPACK_LIBRARIES
LAPACK
cheev
""
"vecLib"
"${BLAS_LIBRARIES}"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
)
endif ( NOT LAPACK_LIBRARIES )
# Generic LAPACK library?
# This configuration *must* be the last try as this library is notably slow.
if ( NOT LAPACK_LIBRARIES )
check_lapack_libraries(
LAPACK_DEFINITIONS
LAPACK_LIBRARIES
LAPACK
cheev
""
"lapack"
"${BLAS_LIBRARIES}"
"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
)
endif()
if(LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
set(LAPACK_FOUND TRUE)
else()
set(LAPACK_FOUND FALSE)
endif()
if(NOT LAPACK_FIND_QUIETLY)
if(LAPACK_FOUND)
message(STATUS "A library with LAPACK API found.")
else(LAPACK_FOUND)
if(LAPACK_FIND_REQUIRED)
message(FATAL_ERROR "A required library with LAPACK API not found. Please specify library location.")
else()
message(STATUS "A library with LAPACK API not found. Please specify library location.")
endif()
endif(LAPACK_FOUND)
endif(NOT LAPACK_FIND_QUIETLY)
# Add variables to cache
set( LAPACK_INCLUDE_DIR "${LAPACK_INCLUDE_DIR}"
CACHE PATH "Directories containing the LAPACK header files" FORCE )
set( LAPACK_DEFINITIONS "${LAPACK_DEFINITIONS}"
CACHE STRING "Compilation options to use LAPACK" FORCE )
set( LAPACK_LINKER_FLAGS "${LAPACK_LINKER_FLAGS}"
CACHE STRING "Linker flags to use LAPACK" FORCE )
set( LAPACK_LIBRARIES "${LAPACK_LIBRARIES}"
CACHE FILEPATH "LAPACK libraries name" FORCE )
set( LAPACK_LIBRARIES_DIR "${LAPACK_LIBRARIES_DIR}"
CACHE PATH "Directories containing the LAPACK libraries" FORCE )
#message("DEBUG: LAPACK_INCLUDE_DIR = ${LAPACK_INCLUDE_DIR}")
#message("DEBUG: LAPACK_DEFINITIONS = ${LAPACK_DEFINITIONS}")
#message("DEBUG: LAPACK_LINKER_FLAGS = ${LAPACK_LINKER_FLAGS}")
#message("DEBUG: LAPACK_LIBRARIES = ${LAPACK_LIBRARIES}")
#message("DEBUG: LAPACK_LIBRARIES_DIR = ${LAPACK_LIBRARIES_DIR}")
#message("DEBUG: LAPACK_FOUND = ${LAPACK_FOUND}")
endif(NOT BLAS_FOUND)
================================================
FILE: Thirdparty/g2o/config.h
================================================
#ifndef G2O_CONFIG_H
#define G2O_CONFIG_H
/* #undef G2O_OPENMP */
/* #undef G2O_SHARED_LIBS */
// 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
================================================
FILE: Thirdparty/g2o/config.h.in
================================================
#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
================================================
FILE: Thirdparty/g2o/g2o/core/base_binary_edge.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/base_binary_edge.hpp
================================================
// 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;
}
================================================
FILE: Thirdparty/g2o/g2o/core/base_edge.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/base_multi_edge.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/base_multi_edge.hpp
================================================
// 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
}
}
}
================================================
FILE: Thirdparty/g2o/g2o/core/base_unary_edge.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/base_unary_edge.hpp
================================================
// 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;
}
================================================
FILE: Thirdparty/g2o/g2o/core/base_vertex.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/base_vertex.hpp
================================================
// 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);
}
================================================
FILE: Thirdparty/g2o/g2o/core/batch_stats.cpp
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/batch_stats.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/block_solver.h
================================================
// 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;
typedef BlockSolver< BlockSolverTraits<6, 9> > BlockSolver_6_9;
// 2Dof landmarks 3Dof poses
typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;
} // end namespace
#include "block_solver.hpp"
#endif
================================================
FILE: Thirdparty/g2o/g2o/core/block_solver.hpp
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/cache.cpp
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/cache.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/creators.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/eigen_types.h
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/estimate_propagator.cpp
================================================
// 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
================================================
FILE: Thirdparty/g2o/g2o/core/estimate_propagator.h
================================================
// 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<doub
gitextract__e5hoirx/
├── CMakeLists.txt
├── Example/
│ ├── dataset/
│ │ └── cabinet/
│ │ ├── associate.py
│ │ ├── associate.txt
│ │ ├── associateGroundtruth.txt
│ │ ├── bbox/
│ │ │ ├── 1341841278.8427.txt
│ │ │ ├── 1341841279.5107.txt
│ │ │ ├── 1341841280.1825.txt
│ │ │ ├── 1341841280.8507.txt
│ │ │ ├── 1341841281.5546.txt
│ │ │ ├── 1341841282.2265.txt
│ │ │ ├── 1341841282.8987.txt
│ │ │ ├── 1341841283.5665.txt
│ │ │ ├── 1341841284.2388.txt
│ │ │ ├── 1341841284.9067.txt
│ │ │ ├── 1341841285.6124.txt
│ │ │ ├── 1341841286.2827.txt
│ │ │ ├── 1341841286.9506.txt
│ │ │ ├── 1341841287.6227.txt
│ │ │ ├── 1341841288.2946.txt
│ │ │ ├── 1341841288.9627.txt
│ │ │ ├── 1341841289.6346.txt
│ │ │ ├── 1341841290.303.txt
│ │ │ ├── 1341841290.9745.txt
│ │ │ ├── 1341841291.6426.txt
│ │ │ ├── 1341841292.3147.txt
│ │ │ ├── 1341841292.9866.txt
│ │ │ ├── 1341841293.6548.txt
│ │ │ ├── 1341841294.3266.txt
│ │ │ ├── 1341841294.9952.txt
│ │ │ ├── 1341841295.6666.txt
│ │ │ ├── 1341841296.3348.txt
│ │ │ ├── 1341841297.0065.txt
│ │ │ ├── 1341841297.6786.txt
│ │ │ ├── 1341841298.3466.txt
│ │ │ ├── 1341841299.0185.txt
│ │ │ ├── 1341841299.6866.txt
│ │ │ ├── 1341841300.3656.txt
│ │ │ ├── 1341841301.0336.txt
│ │ │ ├── 1341841301.7057.txt
│ │ │ ├── 1341841302.3776.txt
│ │ │ ├── 1341841303.0456.txt
│ │ │ ├── 1341841303.7106.txt
│ │ │ ├── 1341841304.3787.txt
│ │ │ ├── 1341841305.0506.txt
│ │ │ ├── 1341841305.7227.txt
│ │ │ ├── 1341841306.3907.txt
│ │ │ ├── 1341841307.0627.txt
│ │ │ ├── 1341841307.7306.txt
│ │ │ ├── 1341841308.4099.txt
│ │ │ ├── 1341841309.1095.txt
│ │ │ ├── 1341841309.7816.txt
│ │ │ ├── 1341841310.4468.txt
│ │ │ ├── 1341841311.1507.txt
│ │ │ ├── 1341841311.8186.txt
│ │ │ ├── 1341841312.4905.txt
│ │ │ ├── 1341841313.1696.txt
│ │ │ ├── 1341841313.8695.txt
│ │ │ ├── 1341841314.5706.txt
│ │ │ ├── 1341841315.2456.txt
│ │ │ ├── 1341841315.9108.txt
│ │ │ ├── 1341841316.5786.txt
│ │ │ └── 1341841317.2506.txt
│ │ ├── depth.txt
│ │ ├── groundtruth.txt
│ │ └── rgb.txt
│ ├── interface/
│ │ └── rgbd.cpp
│ └── param/
│ └── TUM3.yaml
├── LICENSE
├── README.md
├── Thirdparty/
│ └── g2o/
│ ├── CMakeLists.txt
│ ├── README.txt
│ ├── cmake_modules/
│ │ ├── FindBLAS.cmake
│ │ ├── FindEigen3.cmake
│ │ └── FindLAPACK.cmake
│ ├── config.h
│ ├── config.h.in
│ ├── g2o/
│ │ ├── core/
│ │ │ ├── base_binary_edge.h
│ │ │ ├── base_binary_edge.hpp
│ │ │ ├── base_edge.h
│ │ │ ├── base_multi_edge.h
│ │ │ ├── base_multi_edge.hpp
│ │ │ ├── base_unary_edge.h
│ │ │ ├── base_unary_edge.hpp
│ │ │ ├── base_vertex.h
│ │ │ ├── base_vertex.hpp
│ │ │ ├── batch_stats.cpp
│ │ │ ├── batch_stats.h
│ │ │ ├── block_solver.h
│ │ │ ├── block_solver.hpp
│ │ │ ├── cache.cpp
│ │ │ ├── cache.h
│ │ │ ├── creators.h
│ │ │ ├── eigen_types.h
│ │ │ ├── estimate_propagator.cpp
│ │ │ ├── estimate_propagator.h
│ │ │ ├── factory.cpp
│ │ │ ├── factory.h
│ │ │ ├── hyper_dijkstra.cpp
│ │ │ ├── hyper_dijkstra.h
│ │ │ ├── hyper_graph.cpp
│ │ │ ├── hyper_graph.h
│ │ │ ├── hyper_graph_action.cpp
│ │ │ ├── hyper_graph_action.h
│ │ │ ├── jacobian_workspace.cpp
│ │ │ ├── jacobian_workspace.h
│ │ │ ├── linear_solver.h
│ │ │ ├── marginal_covariance_cholesky.cpp
│ │ │ ├── marginal_covariance_cholesky.h
│ │ │ ├── matrix_operations.h
│ │ │ ├── matrix_structure.cpp
│ │ │ ├── matrix_structure.h
│ │ │ ├── openmp_mutex.h
│ │ │ ├── optimizable_graph.cpp
│ │ │ ├── optimizable_graph.h
│ │ │ ├── optimization_algorithm.cpp
│ │ │ ├── optimization_algorithm.h
│ │ │ ├── optimization_algorithm_dogleg.cpp
│ │ │ ├── optimization_algorithm_dogleg.h
│ │ │ ├── optimization_algorithm_factory.cpp
│ │ │ ├── optimization_algorithm_factory.h
│ │ │ ├── optimization_algorithm_gauss_newton.cpp
│ │ │ ├── optimization_algorithm_gauss_newton.h
│ │ │ ├── optimization_algorithm_levenberg.cpp
│ │ │ ├── optimization_algorithm_levenberg.h
│ │ │ ├── optimization_algorithm_property.h
│ │ │ ├── optimization_algorithm_with_hessian.cpp
│ │ │ ├── optimization_algorithm_with_hessian.h
│ │ │ ├── parameter.cpp
│ │ │ ├── parameter.h
│ │ │ ├── parameter_container.cpp
│ │ │ ├── parameter_container.h
│ │ │ ├── robust_kernel.cpp
│ │ │ ├── robust_kernel.h
│ │ │ ├── robust_kernel_factory.cpp
│ │ │ ├── robust_kernel_factory.h
│ │ │ ├── robust_kernel_impl.cpp
│ │ │ ├── robust_kernel_impl.h
│ │ │ ├── solver.cpp
│ │ │ ├── solver.h
│ │ │ ├── sparse_block_matrix.h
│ │ │ ├── sparse_block_matrix.hpp
│ │ │ ├── sparse_block_matrix_ccs.h
│ │ │ ├── sparse_block_matrix_diagonal.h
│ │ │ ├── sparse_block_matrix_test.cpp
│ │ │ ├── sparse_optimizer.cpp
│ │ │ └── sparse_optimizer.h
│ │ ├── solvers/
│ │ │ ├── linear_solver_dense.h
│ │ │ └── linear_solver_eigen.h
│ │ ├── stuff/
│ │ │ ├── color_macros.h
│ │ │ ├── macros.h
│ │ │ ├── misc.h
│ │ │ ├── os_specific.c
│ │ │ ├── os_specific.h
│ │ │ ├── property.cpp
│ │ │ ├── property.h
│ │ │ ├── string_tools.cpp
│ │ │ ├── string_tools.h
│ │ │ ├── timeutil.cpp
│ │ │ └── timeutil.h
│ │ └── types/
│ │ ├── CMakeLists.txt
│ │ ├── se3_ops.h
│ │ ├── se3_ops.hpp
│ │ ├── se3quat.h
│ │ ├── sim3.h
│ │ ├── types_sba.cpp
│ │ ├── types_sba.h
│ │ ├── types_seven_dof_expmap.cpp
│ │ ├── types_seven_dof_expmap.h
│ │ ├── types_six_dof_expmap.cpp
│ │ └── types_six_dof_expmap.h
│ └── license-bsd.txt
├── cmake_modules/
│ └── FindEigen3.cmake
├── include/
│ ├── core/
│ │ ├── BasicEllipsoidEdges.h
│ │ ├── DataAssociation.h
│ │ ├── Ellipsoid.h
│ │ ├── Frame.h
│ │ ├── FrameDrawer.h
│ │ ├── Geometry.h
│ │ ├── Initializer.h
│ │ ├── Map.h
│ │ ├── MapDrawer.h
│ │ ├── Optimizer.h
│ │ ├── Plane.h
│ │ ├── System.h
│ │ ├── Tracking.h
│ │ └── Viewer.h
│ └── utils/
│ ├── dataprocess_utils.h
│ └── matrix_utils.h
├── install_g2o.sh
└── src/
├── Polygon/
│ ├── CMakeLists.txt
│ ├── Polygon.cpp
│ └── Polygon.hpp
├── config/
│ ├── CMakeLists.txt
│ ├── Config.cpp
│ └── Config.h
├── core/
│ ├── BasicEllipsoidEdges.cpp
│ ├── DataAssociation.cpp
│ ├── Ellipsoid.cpp
│ ├── Frame.cpp
│ ├── FrameDrawer.cpp
│ ├── Geometry.cpp
│ ├── Initializer.cpp
│ ├── Map.cpp
│ ├── MapDrawer.cpp
│ ├── Optimizer.cpp
│ ├── Plane.cpp
│ ├── System.cpp
│ ├── Tracking.cpp
│ └── Viewer.cpp
├── dense_builder/
│ ├── CMakeLists.txt
│ ├── builder.cpp
│ └── builder.h
├── pca/
│ ├── CMakeLists.txt
│ ├── EllipsoidExtractor.cpp
│ └── EllipsoidExtractor.h
├── plane/
│ ├── CMakeLists.txt
│ ├── PlaneExtractor.cpp
│ └── PlaneExtractor.h
├── symmetry/
│ ├── BorderExtractor.cpp
│ ├── BorderExtractor.h
│ ├── CMakeLists.txt
│ ├── PointCloudFilter.cpp
│ ├── PointCloudFilter.h
│ ├── Symmetry.cpp
│ ├── Symmetry.h
│ ├── SymmetrySolver.cpp
│ └── SymmetrySolver.h
├── tum_rgbd/
│ ├── CMakeLists.txt
│ ├── io.cpp
│ └── io.h
└── utils/
├── dataprocess_utils.cpp
└── matrix_utils.cpp
SYMBOL INDEX (392 symbols across 137 files)
FILE: Example/dataset/cabinet/associate.py
function read_file_list (line 49) | def read_file_list(filename):
function associate (line 71) | def associate(first_list, second_list,offset,max_difference):
FILE: Example/interface/rgbd.cpp
function main (line 28) | int main(int argc,char* argv[]) {
FILE: Thirdparty/g2o/g2o/core/base_binary_edge.h
function namespace (line 37) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/base_edge.h
function namespace (line 37) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/base_multi_edge.hpp
type internal (line 27) | namespace internal {
function computeUpperTriangleIndex (line 28) | inline int computeUpperTriangleIndex(int i, int j)
FILE: Thirdparty/g2o/g2o/core/base_unary_edge.h
function namespace (line 38) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/base_vertex.h
function namespace (line 40) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/batch_stats.cpp
type g2o (line 30) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/batch_stats.h
function namespace (line 34) | namespace g2o {
type std (line 80) | typedef std::vector<G2OBatchStatistics> BatchStatisticsContainer;
FILE: Thirdparty/g2o/g2o/core/block_solver.h
function namespace (line 37) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/block_solver.hpp
type g2o (line 36) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/cache.cpp
type g2o (line 33) | namespace g2o {
function OptimizableGraph (line 65) | OptimizableGraph* Cache::graph() {
function CacheContainer (line 71) | CacheContainer* Cache::container() {
function ParameterVector (line 75) | ParameterVector& Cache::parameters() {
function Cache (line 95) | Cache* Cache::installDependency(const std::string& type_, const std::v...
function Cache (line 122) | Cache* CacheContainer::findCache(const Cache::CacheKey& key) {
function Cache (line 129) | Cache* CacheContainer::createCache(const Cache::CacheKey& key){
function OptimizableGraph (line 157) | OptimizableGraph* CacheContainer::graph(){
FILE: Thirdparty/g2o/g2o/core/cache.h
function namespace (line 34) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/creators.h
function namespace (line 35) | namespace g2o
FILE: Thirdparty/g2o/g2o/core/eigen_types.h
function namespace (line 33) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/estimate_propagator.cpp
type g2o (line 40) | namespace g2o {
type FrontierLevelCmp (line 43) | struct FrontierLevelCmp {
FILE: Thirdparty/g2o/g2o/core/estimate_propagator.h
function namespace (line 43) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/factory.cpp
type g2o (line 41) | namespace g2o {
function Factory (line 61) | Factory* Factory::instance()
FILE: Thirdparty/g2o/g2o/core/factory.h
function namespace (line 42) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp
type g2o (line 35) | namespace g2o{
FILE: Thirdparty/g2o/g2o/core/hyper_dijkstra.h
function namespace (line 36) | namespace g2o{
type std (line 70) | typedef std::map<HyperGraph::Vertex*, AdjacencyMapEntry> AdjacencyMap;
function HyperGraph (line 74) | HyperGraph* graph() {return _graph;}
type UniformCostFunction (line 107) | struct UniformCostFunction
FILE: Thirdparty/g2o/g2o/core/hyper_graph.cpp
type g2o (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/hyper_graph.h
function namespace (line 47) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/hyper_graph_action.cpp
type g2o (line 34) | namespace g2o {
function HyperGraphAction (line 53) | HyperGraphAction* HyperGraphAction::operator()(const HyperGraph*, Para...
function HyperGraphElementAction (line 73) | HyperGraphElementAction* HyperGraphElementAction::operator()(HyperGrap...
function HyperGraphElementAction (line 78) | HyperGraphElementAction* HyperGraphElementAction::operator()(const Hyp...
function HyperGraphElementAction (line 99) | HyperGraphElementAction* HyperGraphElementActionCollection::operator()...
function HyperGraphElementAction (line 109) | HyperGraphElementAction* HyperGraphElementActionCollection::operator()...
function HyperGraphActionLibrary (line 145) | HyperGraphActionLibrary* HyperGraphActionLibrary::instance()
function HyperGraphElementAction (line 166) | HyperGraphElementAction* HyperGraphActionLibrary::actionByName(const s...
function applyAction (line 253) | void applyAction(HyperGraph* graph, HyperGraphElementAction* action, H...
FILE: Thirdparty/g2o/g2o/core/hyper_graph_action.h
function namespace (line 43) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/jacobian_workspace.cpp
type g2o (line 35) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/jacobian_workspace.h
function namespace (line 38) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/linear_solver.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp
type g2o (line 33) | namespace g2o {
type MatrixElem (line 35) | struct MatrixElem
method MatrixElem (line 38) | MatrixElem(int r_, int c_) : r(r_), c(c_) {}
FILE: Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h
function namespace (line 43) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/matrix_operations.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/matrix_structure.cpp
type g2o (line 35) | namespace g2o {
type ColSort (line 37) | struct ColSort
FILE: Thirdparty/g2o/g2o/core/matrix_structure.h
function namespace (line 31) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/openmp_mutex.h
function namespace (line 38) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimizable_graph.cpp
type g2o (line 49) | namespace g2o {
function CacheContainer (line 70) | CacheContainer* OptimizableGraph::Vertex::cacheContainer(){
function OptimizableGraph (line 143) | OptimizableGraph* OptimizableGraph::Edge::graph(){
function OptimizableGraph (line 152) | const OptimizableGraph* OptimizableGraph::Edge::graph() const{
FILE: Thirdparty/g2o/g2o/core/optimizable_graph.h
function namespace (line 45) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm.cpp
type g2o (line 31) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm.h
function namespace (line 39) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp
type g2o (line 39) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp
type g2o (line 35) | namespace g2o {
function OptimizationAlgorithmFactory (line 54) | OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::instance()
function OptimizationAlgorithm (line 84) | OptimizationAlgorithm* OptimizationAlgorithmFactory::construct(const s...
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h
function namespace (line 42) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp
type g2o (line 39) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp
type g2o (line 41) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_property.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp
type g2o (line 36) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/parameter.cpp
type g2o (line 29) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/parameter.h
function namespace (line 34) | namespace g2o {
type std (line 52) | typedef std::vector<Parameter*> ParameterVector;
FILE: Thirdparty/g2o/g2o/core/parameter_container.cpp
type g2o (line 38) | namespace g2o {
function Parameter (line 70) | Parameter* ParameterContainer::getParameter(int id) {
function Parameter (line 77) | Parameter* ParameterContainer::detachParameter(int id){
FILE: Thirdparty/g2o/g2o/core/parameter_container.h
function namespace (line 34) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/robust_kernel.cpp
type g2o (line 29) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/robust_kernel.h
function namespace (line 38) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp
type g2o (line 34) | namespace g2o {
function RobustKernelFactory (line 50) | RobustKernelFactory* RobustKernelFactory::instance()
function RobustKernel (line 80) | RobustKernel* RobustKernelFactory::construct(const std::string& tag) c...
function AbstractRobustKernelCreator (line 89) | AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::s...
FILE: Thirdparty/g2o/g2o/core/robust_kernel_factory.h
function namespace (line 37) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp
type g2o (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/robust_kernel_impl.h
function namespace (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/solver.cpp
type g2o (line 32) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/solver.h
function namespace (line 35) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix.h
function namespace (line 43) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp
type g2o (line 27) | namespace g2o {
type TripletEntry (line 31) | struct TripletEntry
method TripletEntry (line 35) | TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {}
type TripletColSort (line 37) | struct TripletColSort
type CmpPairFirst (line 46) | struct CmpPairFirst {
FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
function namespace (line 43) | namespace g2o {
type std (line 226) | typedef std::tr1::unordered_map<int, MatrixType*> SparseColumn;
function rowsOfBlock (line 233) | int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBloc...
function colsOfBlock (line 236) | int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBloc...
function rowBaseOfBlock (line 239) | int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
function colBaseOfBlock (line 242) | int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h
function namespace (line 37) | namespace g2o {
FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp
function main (line 46) | int main (int argc, char** argv){
FILE: Thirdparty/g2o/g2o/core/sparse_optimizer.cpp
type g2o (line 46) | namespace g2o{
FILE: Thirdparty/g2o/g2o/core/sparse_optimizer.h
function namespace (line 38) | namespace g2o {
FILE: Thirdparty/g2o/g2o/solvers/linear_solver_dense.h
function namespace (line 40) | namespace g2o {
FILE: Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h
function namespace (line 42) | namespace g2o {
FILE: Thirdparty/g2o/g2o/stuff/misc.h
function namespace (line 46) | namespace g2o {
type ForceLinker (line 196) | struct ForceLinker
FILE: Thirdparty/g2o/g2o/stuff/os_specific.c
function vasprintf (line 31) | int vasprintf(char** strp, const char* fmt, va_list ap)
FILE: Thirdparty/g2o/g2o/stuff/property.cpp
type g2o (line 37) | namespace g2o {
FILE: Thirdparty/g2o/g2o/stuff/property.h
function class (line 38) | class BaseProperty {
FILE: Thirdparty/g2o/g2o/stuff/string_tools.cpp
type g2o (line 44) | namespace g2o {
function trim (line 48) | std::string trim(const std::string& s)
function trimLeft (line 59) | std::string trimLeft(const std::string& s)
function trimRight (line 70) | std::string trimRight(const std::string& s)
function strToLower (line 81) | std::string strToLower(const std::string& s)
function strToUpper (line 88) | std::string strToUpper(const std::string& s)
function formatString (line 95) | std::string formatString(const char* fmt, ...)
function strPrintf (line 112) | int strPrintf(std::string& str, const char* fmt, ...)
function strExpandFilename (line 124) | std::string strExpandFilename(const std::string& filename)
function strSplit (line 143) | std::vector<std::string> strSplit(const std::string& str, const std::s...
function strStartsWith (line 158) | bool strStartsWith(const std::string& s, const std::string& start)
function strEndsWith (line 165) | bool strEndsWith(const std::string& s, const std::string& end)
function readLine (line 172) | int readLine(std::istream& is, std::stringstream& currentLine)
FILE: Thirdparty/g2o/g2o/stuff/string_tools.h
function namespace (line 37) | namespace g2o {
FILE: Thirdparty/g2o/g2o/stuff/timeutil.cpp
type g2o (line 39) | namespace g2o {
type timezone (line 48) | struct timezone
function gettimeofday (line 54) | int gettimeofday(struct timeval *tv, struct timezone *tz)
function get_monotonic_time (line 113) | double get_monotonic_time()
FILE: Thirdparty/g2o/g2o/stuff/timeutil.h
function namespace (line 78) | namespace g2o {
FILE: Thirdparty/g2o/g2o/types/se3_ops.h
function namespace (line 33) | namespace g2o {
FILE: Thirdparty/g2o/g2o/types/se3_ops.hpp
function Matrix3d (line 28) | Matrix3d skew(const Vector3d&v)
function Vector3d (line 41) | Vector3d deltaR(const Matrix3d& R)
function Vector2d (line 50) | Vector2d project(const Vector3d& v)
function Vector3d (line 58) | Vector3d project(const Vector4d& v)
function Vector3d (line 67) | Vector3d unproject(const Vector2d& v)
function Vector4d (line 76) | Vector4d unproject(const Vector3d& v)
function zyx_euler_to_quat (line 86) | Eigen::Quaterniond zyx_euler_to_quat(const double &roll, const double &p...
function quat_to_euler_zyx (line 101) | void quat_to_euler_zyx(const Eigen::Quaterniond q, double& roll, double&...
FILE: Thirdparty/g2o/g2o/types/se3quat.h
function namespace (line 35) | namespace g2o {
function Vector3d (line 268) | Vector3d map(const Vector3d & xyz) const
function SE3Quat (line 274) | static SE3Quat exp(const Vector6d & update) // doesn't multiply into cu...
function normalizeRotation (line 345) | void normalizeRotation(){
FILE: Thirdparty/g2o/g2o/types/sim3.h
function namespace (line 33) | namespace g2o
function Vector3d (line 140) | Vector3d map (const Vector3d& xyz) const {
function Vector7d (line 144) | Vector7d log() const
function const (line 232) | double operator[](int i) const
function Vector3d (line 275) | inline Vector3d& translation() {return t;}
function Quaterniond (line 279) | inline Quaterniond& rotation() {return r;}
function scale (line 283) | inline double& scale() {return s;}
FILE: Thirdparty/g2o/g2o/types/types_sba.cpp
type g2o (line 30) | namespace g2o {
FILE: Thirdparty/g2o/g2o/types/types_sba.h
function namespace (line 35) | namespace g2o {
FILE: Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp
type g2o (line 29) | namespace g2o {
FILE: Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h
function virtual (line 56) | virtual void setToOriginImpl() {
function virtual (line 60) | virtual void oplusImpl(const double* update_)
function Vector2d (line 74) | Vector2d cam_map1(const Vector2d & v) const
function Vector2d (line 82) | Vector2d cam_map2(const Vector2d & v) const
function virtual (line 105) | virtual bool write(std::ostream& os) const;
function virtual (line 116) | virtual double initialEstimatePossible(const OptimizableGraph::VertexSet...
function virtual (line 117) | virtual void initialEstimate(const OptimizableGraph::VertexSet& from, Op...
function computeError (line 138) | void computeError()
function computeError (line 160) | void computeError()
FILE: Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp
type g2o (line 32) | namespace g2o {
function Vector2d (line 37) | Vector2d project2d(const Vector3d& v) {
function Vector3d (line 44) | Vector3d unproject2d(const Vector2d& v) {
function Vector2d (line 186) | Vector2d EdgeSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz) co...
function Vector3d (line 195) | Vector3d EdgeStereoSE3ProjectXYZ::cam_project(const Vector3d & trans_x...
function Vector2d (line 335) | Vector2d EdgeSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans...
function Vector3d (line 344) | Vector3d EdgeStereoSE3ProjectXYZOnlyPose::cam_project(const Vector3d &...
FILE: Thirdparty/g2o/g2o/types/types_six_dof_expmap.h
function namespace (line 45) | namespace g2o {
FILE: include/core/BasicEllipsoidEdges.h
function namespace (line 8) | namespace g2o
FILE: include/core/DataAssociation.h
function namespace (line 8) | namespace EllipsoidSLAM
FILE: include/core/Ellipsoid.h
type Eigen (line 11) | typedef Eigen::Matrix<double, 9, 1> Vector9d;
type Eigen (line 12) | typedef Eigen::Matrix<double, 9, 9> Matrix9d;
type Eigen (line 13) | typedef Eigen::Matrix<double, 5, 5> Matrix5d;
type Eigen (line 14) | typedef Eigen::Matrix<double, 3, 8> Matrix38d;
type Eigen (line 15) | typedef Eigen::Matrix<double, 10, 1> Vector10d;
type Eigen (line 16) | typedef Eigen::Matrix<double, 6, 1> Vector6d;
type Eigen (line 17) | typedef Eigen::Matrix<double, 5, 1> Vector5d;
type Eigen (line 18) | typedef Eigen::Matrix<double, 2, 1> Vector2d;
type Eigen (line 19) | typedef Eigen::Matrix<double, 4, 1> Vector4d;
type Eigen (line 20) | typedef Eigen::Matrix<double, 6, 6> Matrix6d;
function namespace (line 22) | namespace g2o
FILE: include/core/Frame.h
function namespace (line 8) | namespace EllipsoidSLAM{
FILE: include/core/FrameDrawer.h
function namespace (line 7) | namespace EllipsoidSLAM{
FILE: include/core/Geometry.h
function namespace (line 13) | namespace EllipsoidSLAM
FILE: include/core/Initializer.h
function namespace (line 12) | namespace EllipsoidSLAM
FILE: include/core/Map.h
function namespace (line 14) | namespace EllipsoidSLAM
FILE: include/core/MapDrawer.h
function namespace (line 13) | namespace EllipsoidSLAM{
FILE: include/core/Optimizer.h
function namespace (line 10) | namespace EllipsoidSLAM {
FILE: include/core/Plane.h
function namespace (line 11) | namespace g2o
FILE: include/core/System.h
function namespace (line 15) | namespace EllipsoidSLAM{
FILE: include/core/Tracking.h
function namespace (line 18) | namespace EllipsoidSLAM{
FILE: include/core/Viewer.h
type MenuStruct (line 15) | struct MenuStruct
function namespace (line 23) | namespace EllipsoidSLAM
FILE: src/Polygon/Polygon.cpp
type EllipsoidSLAM (line 3) | namespace EllipsoidSLAM
function distPoint (line 5) | float distPoint(cv::Point2d v,cv::Point2d w) {
function segementIntersection (line 9) | bool segementIntersection(cv::Point2d p0,cv::Point2d p1,cv::Point2d p2...
function pointInPolygon (line 49) | bool pointInPolygon(cv::Point2d p,const cv::Point2d * points,int n) {
function computeArea (line 62) | float computeArea(const cv::Point2d * pt,int n ) {
type PointAngle (line 72) | struct PointAngle{
function comp_point_with_angle (line 90) | static int comp_point_with_angle(const void * a, const void * b) {
function intersectPolygon (line 121) | void intersectPolygon( const cv::Point2d * poly0, int n0,const cv::Poi...
function intersectPolygon (line 150) | void intersectPolygon( const Polygon & poly0, const Polygon & poly1, P...
function cross (line 157) | static inline int cross(const cv::Point2d* a,const cv::Point2d* b) {
function line_sect (line 167) | static int line_sect(const cv::Point2d* x0,const cv::Point2d* x1,const...
function left_of (line 181) | static int left_of(const cv::Point2d* a,const cv::Point2d* b,const cv:...
function poly_edge_clip (line 190) | static void poly_edge_clip(const Polygon* sub,const cv::Point2d* x0,c...
function poly_winding (line 214) | static int poly_winding(const Polygon* p) {
function intersectPolygonSHPC (line 218) | void intersectPolygonSHPC(const Polygon * sub,const Polygon* clip,Poly...
function intersectPolygonSHPC (line 239) | void intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Poly...
FILE: src/Polygon/Polygon.hpp
type EllipsoidSLAM (line 13) | namespace EllipsoidSLAM
type Polygon (line 22) | struct Polygon {
method Polygon (line 26) | Polygon(int n_ = 0 ) { assert(n_>= 0 && n_ < MAX_POINT_POLYGON); n =...
method clear (line 29) | void clear() { n = 0; }
method add (line 30) | void add(const cv::Point2d &p) {if(n < MAX_POINT_POLYGON) pt[n++] = p;}
method push_back (line 31) | void push_back(const cv::Point2d &p) {add(p);}
method size (line 32) | int size() const { return n;}
FILE: src/config/Config.cpp
type EllipsoidSLAM (line 4) | namespace EllipsoidSLAM{
FILE: src/config/Config.h
function namespace (line 13) | namespace EllipsoidSLAM {
FILE: src/core/BasicEllipsoidEdges.cpp
type g2o (line 3) | namespace g2o
function Vector4d (line 90) | Vector4d EdgeSE3EllipsoidProj::getProject(){
FILE: src/core/DataAssociation.cpp
type EllipsoidSLAM (line 6) | namespace EllipsoidSLAM
FILE: src/core/Ellipsoid.cpp
type g2o (line 9) | namespace g2o
function Vector3d (line 31) | const Vector3d& ellipsoid::translation() const {return pose.translatio...
function ellipsoid (line 38) | ellipsoid ellipsoid::exp_update(const Vector9d& update)
function ellipsoid (line 50) | ellipsoid ellipsoid::exp_update_XYZABC(const Vector6d& update)
function Vector9d (line 63) | Vector9d ellipsoid::ellipsoid_log_error_9dof(const ellipsoid& newone) ...
function ellipsoid (line 75) | ellipsoid ellipsoid::rotate_ellipsoid(double yaw_angle) const // to de...
function Vector9d (line 92) | Vector9d ellipsoid::min_log_error_9dof(const ellipsoid& newone, bool p...
function ellipsoid (line 120) | ellipsoid ellipsoid::transform_from(const SE3Quat& Twc) const{
function ellipsoid (line 132) | ellipsoid ellipsoid::transform_to(const SE3Quat& Twc) const{
function Vector9d (line 144) | Vector9d ellipsoid::toMinimalVector() const{
function Vector10d (line 152) | Vector10d ellipsoid::toVector() const{
function Matrix4d (line 159) | Matrix4d ellipsoid::similarityTransform() const
function ellipsoid (line 185) | const ellipsoid& ellipsoid::operator=(const g2o::ellipsoid &e) {
function Vector2d (line 195) | Vector2d ellipsoid::projectCenterIntoImagePoint(const SE3Quat& campose...
function Vector5d (line 209) | Vector5d ellipsoid::projectOntoImageEllipse(const SE3Quat& campose_cw,...
function Vector4d (line 252) | Vector4d ellipsoid::getBoundingBoxFromEllipse(Vector5d &ellipse) const
function Matrix3Xd (line 276) | Matrix3Xd ellipsoid::generateProjectionMatrix(const SE3Quat& campose_c...
function Matrix4d (line 290) | Matrix4d ellipsoid::generateQuadric() const
function Vector4d (line 303) | Vector4d ellipsoid::getBoundingBoxFromProjection(const SE3Quat& campos...
function Vector3d (line 309) | Vector3d ellipsoid::getColor(){
function Vector4d (line 313) | Vector4d ellipsoid::getColorWithAlpha(){
function OutputPolygon (line 379) | void OutputPolygon(EllipsoidSLAM::Polygon& polygon, double resolution)
function Matrix3Xd (line 472) | Matrix3Xd ellipsoid::compute3D_BoxCorner() const
function Matrix2Xd (line 482) | Matrix2Xd ellipsoid::projectOntoImageBoxCorner(const SE3Quat& campose_...
function Vector4d (line 491) | Vector4d ellipsoid::projectOntoImageRect(const SE3Quat& campose_cw, co...
function Vector4d (line 500) | Vector4d ellipsoid::projectOntoImageBbox(const SE3Quat& campose_cw, co...
FILE: src/core/Frame.cpp
type EllipsoidSLAM (line 3) | namespace EllipsoidSLAM
FILE: src/core/FrameDrawer.cpp
type EllipsoidSLAM (line 7) | namespace EllipsoidSLAM
FILE: src/core/Geometry.cpp
type EllipsoidSLAM (line 4) | namespace EllipsoidSLAM
function PointCloud (line 8) | PointCloud getPointCloud(cv::Mat &depth, cv::Mat &rgb, Eigen::VectorXd...
function PointCloud (line 44) | PointCloud* transformPointCloud(PointCloud *pPoints_local, g2o::SE3Qua...
function transformPointCloudSelf (line 68) | void transformPointCloudSelf(PointCloud *pPoints_local, g2o::SE3Quat* ...
function PointCloud (line 84) | PointCloud loadPointsToPointVector(Eigen::MatrixXd &pMat){
function SetPointCloudProperty (line 107) | void SetPointCloudProperty(PointCloud* pCloud, uchar r, uchar g, uchar...
function Matrix3d (line 119) | Matrix3d getCalibFromCamera(camera_intrinsic &camera)
function SavePointCloudToTxt (line 129) | void SavePointCloudToTxt(const string& path, PointCloud* pCloud){
function Vector3d (line 143) | Vector3d GetPointcloudCenter(PointCloud* pCloud)
function Vector3d (line 160) | Vector3d TransformPoint(Vector3d &point, const Eigen::Matrix4d &T)
FILE: src/core/Initializer.cpp
type EllipsoidSLAM (line 10) | namespace EllipsoidSLAM
function cmp (line 18) | bool cmp(pair<int, double>a, pair<int, double>b)
function MatrixXd (line 58) | MatrixXd Initializer::getPlanesHomo(MatrixXd &pose_mat, MatrixXd &dete...
function Matrix3Xd (line 93) | Matrix3Xd Initializer::generateProjectionMatrix(const SE3Quat& campose...
function MatrixXd (line 107) | MatrixXd Initializer::fromDetectionsToLines(VectorXd &detections) {
function MatrixXd (line 147) | MatrixXd Initializer::getVectorFromPlanesHomo(MatrixXd &planes) {
function Matrix4d (line 166) | Matrix4d Initializer::getQStarFromVectors(MatrixXd &planeVecs) {
FILE: src/core/Map.cpp
type EllipsoidSLAM (line 5) | namespace EllipsoidSLAM
FILE: src/core/MapDrawer.cpp
type EllipsoidSLAM (line 9) | namespace EllipsoidSLAM
function drawAxisNormal (line 12) | void drawAxisNormal()
FILE: src/core/Optimizer.cpp
type EllipsoidSLAM (line 17) | namespace EllipsoidSLAM
function isInImage (line 20) | bool isInImage(Eigen::Vector2d& uv, int rows, int cols){
function checkVisibility (line 35) | bool checkVisibility(g2o::EdgeSE3EllipsoidProj *edge, g2o::VertexSE3Ex...
FILE: src/core/Plane.cpp
type g2o (line 3) | namespace g2o
function plane (line 21) | plane plane::exp_update(const Vector3d& update)
function plane (line 34) | plane plane::exp_update2DOF(const Vector2d& update)
function plane (line 71) | const plane& plane::operator=(const plane& p){
function Vector3d (line 146) | Vector3d plane::GetLineFromCenterAngle(const Vector2d center, double a...
function Vector4d (line 168) | Vector4d plane::LineToPlane(const Vector3d line)
FILE: src/core/System.cpp
type EllipsoidSLAM (line 6) | namespace EllipsoidSLAM
function Map (line 55) | Map* System::getMap() {
function MapDrawer (line 59) | MapDrawer* System::getMapDrawer() {
function FrameDrawer (line 63) | FrameDrawer* System::getFrameDrawer() {
function Viewer (line 67) | Viewer* System::getViewer() {
function Tracking (line 71) | Tracking* System::getTracker() {
FILE: src/core/Tracking.cpp
type EllipsoidSLAM (line 7) | namespace EllipsoidSLAM
function outputObjectObservations (line 9) | void outputObjectObservations(std::map<int, Observations> &mmObjectObs...
function AddSegCloudsToQuadricStorage (line 259) | void AddSegCloudsToQuadricStorage(std::vector<pcl::PointCloud<pcl::Poi...
FILE: src/core/Viewer.cpp
type EllipsoidSLAM (line 6) | namespace EllipsoidSLAM {
FILE: src/dense_builder/builder.h
type pcl (line 11) | typedef pcl::PointXYZRGB PointT;
type pcl (line 12) | typedef pcl::PointCloud<PointT> PointCloudPCL;
type Camera (line 19) | struct Camera
function class (line 27) | class Builder
FILE: src/pca/EllipsoidExtractor.cpp
type EllipsoidSLAM (line 31) | namespace EllipsoidSLAM
function compare_cloud_with_z (line 34) | bool compare_cloud_with_z(EllipsoidSLAM::PointXYZRGB &p1, EllipsoidSLA...
function PCAResult (line 170) | PCAResult EllipsoidExtractor::ProcessPCA(pcl::PointCloud<PointType>::P...
function PCAResult (line 495) | PCAResult EllipsoidExtractor::ProcessPCANormalized(EllipsoidSLAM::Poin...
function SymmetryOutputData (line 533) | SymmetryOutputData EllipsoidExtractor::GetSymmetryOutputData()
FILE: src/pca/EllipsoidExtractor.h
type pcl (line 25) | typedef pcl::PointXYZ PointType;
type pcl (line 26) | typedef pcl::Normal NormalType;
function namespace (line 28) | namespace EllipsoidSLAM
FILE: src/plane/PlaneExtractor.cpp
type EllipsoidSLAM (line 7) | namespace EllipsoidSLAM
function compare_func_plane_dis (line 9) | bool compare_func_plane_dis(g2o::plane* &p1, g2o::plane* &p2)
function compare_func_plane_size (line 17) | bool compare_func_plane_size(std::pair<g2o::plane*, int> &p1, std::pai...
FILE: src/plane/PlaneExtractor.h
type pcl (line 26) | typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudPCL;
function namespace (line 29) | namespace EllipsoidSLAM
FILE: src/symmetry/BorderExtractor.cpp
type EllipsoidSLAM (line 3) | namespace EllipsoidSLAM
FILE: src/symmetry/BorderExtractor.h
type pcl (line 20) | typedef pcl::PointXYZ PointType;
function namespace (line 22) | namespace EllipsoidSLAM
FILE: src/symmetry/PointCloudFilter.cpp
function Vector2d (line 3) | Vector2d getXYCenterOfPointCloud(EllipsoidSLAM::PointCloud* pPoints)
function getPointCloudInRect (line 21) | EllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, cv::Mat &r...
function getPointCloudInRect (line 58) | EllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, const Vect...
function filterGround (line 63) | void filterGround(EllipsoidSLAM::PointCloud** ppCloud)
function outputCloud (line 78) | void outputCloud(EllipsoidSLAM::PointCloud *pCloud, int num )
function pclToQuadricPointCloud (line 92) | EllipsoidSLAM::PointCloud pclToQuadricPointCloud(PointCloudPCL::Ptr &pCl...
function QuadricPointCloudToPcl (line 133) | PointCloudPCL::Ptr QuadricPointCloudToPcl(EllipsoidSLAM::PointCloud &cloud)
function QuadricPointCloudToPclXYZ (line 155) | pcl::PointCloud<pcl::PointXYZ>::Ptr QuadricPointCloudToPclXYZ(EllipsoidS...
function pclXYZToQuadricPointCloud (line 175) | EllipsoidSLAM::PointCloud pclXYZToQuadricPointCloud(pcl::PointCloud<pcl:...
function DownSamplePointCloud (line 209) | void
function DownSamplePointCloudOnly (line 249) | void DownSamplePointCloudOnly(EllipsoidSLAM::PointCloud& cloudIn, Ellips...
function FiltOutliers (line 263) | void FiltOutliers(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::Poi...
function FiltPointsInBox (line 278) | void FiltPointsInBox(EllipsoidSLAM::PointCloud* pPoints_global, Ellipsoi...
function CombinePointCloud (line 303) | void CombinePointCloud(EllipsoidSLAM::PointCloud *p1, EllipsoidSLAM::Poi...
FILE: src/symmetry/PointCloudFilter.h
type pcl (line 13) | typedef pcl::PointXYZRGB PointT;
type pcl (line 14) | typedef pcl::PointCloud<PointT> PointCloudPCL;
FILE: src/symmetry/Symmetry.cpp
type EllipsoidSLAM (line 7) | namespace EllipsoidSLAM
function comp_func_mapPlane (line 10) | bool comp_func_mapPlane(const pair<double,g2o::plane*>& t1, const pair...
function comp_func_mapProbData (line 15) | bool comp_func_mapProbData(const pair<double,SymmetrySolverData>& t1, ...
function SaveSymmetryResultToText (line 20) | void SaveSymmetryResultToText(std::vector<pair<double, SymmetrySolverD...
function GenerateInitPlanes (line 44) | std::vector<g2o::plane*> GenerateInitPlanes(int symType = 1)
function SymmetrySolverData (line 88) | SymmetrySolverData Symmetry::estimateSymmetry(Vector4d &bbox, PointClo...
function calculateProjZ (line 137) | double calculateProjZ(double f, double d, double xi, double yi){
FILE: src/symmetry/Symmetry.h
function namespace (line 14) | namespace EllipsoidSLAM {
FILE: src/symmetry/SymmetrySolver.cpp
function isInRange (line 22) | bool isInRange(int x, int y, int range_x, int range_x2, int range_y, int...
function isInFrustrum (line 30) | bool isInFrustrum(int x, int y, int range_x, int range_y)
function checkValidPoint (line 35) | bool checkValidPoint(EllipsoidSLAM::PointXYZRGB &p)
function PointCloud (line 168) | PointCloud* SymmetrySolver::GetSymmetryPointCloud(PointCloud* pCloud, g2...
function Vector4d (line 193) | Vector4d SymmetrySolver::GetSymmetryPointOfPlane(Vector4d& point, g2o::p...
function SymmetrySolverData (line 217) | SymmetrySolverData SymmetrySolver::OptimizeSymmetryPlane(Vector4d &bbox,...
function SymmetrySolverData (line 279) | SymmetrySolverData SymmetrySolver::OptimizeSymmetryDualPlane(Vector4d &b...
function SymmetrySolverData (line 343) | SymmetrySolverData SymmetrySolver::getResult()
type g2o (line 355) | namespace g2o
FILE: src/symmetry/SymmetrySolver.h
function class (line 16) | class SymmetrySolverData
function class (line 34) | class SymmetrySolver
function namespace (line 89) | namespace g2o
FILE: src/tum_rgbd/io.cpp
type TUMRGBD (line 11) | namespace TUMRGBD
function AssociateWithNumber (line 97) | map<string, string>::const_iterator AssociateWithNumber(const map<stri...
function AssociateWithNumber (line 111) | map<string, VectorXd>::const_iterator AssociateWithNumber(const map<st...
function VectorXd (line 412) | VectorXd Dataset::calibratePose(VectorXd& pose)
FILE: src/tum_rgbd/io.h
function namespace (line 18) | namespace TUMRGBD
FILE: src/utils/dataprocess_utils.cpp
function compare_func_stringasdouble (line 25) | bool compare_func_stringasdouble(string &s1, string &s2)
function GetFileNamesUnderDir (line 32) | void GetFileNamesUnderDir(string path,vector<string>& filenames)
function string (line 49) | string splitFileNameFromFullDir(string &s, bool bare)
function sortFileNames (line 66) | void sortFileNames(vector<string>& filenames, vector<string>& filenamesS...
function readDataFromFile (line 72) | Eigen::MatrixXd readDataFromFile(const char* fileName, bool dropFirstline){
function saveMatToFile (line 105) | bool saveMatToFile(Eigen::MatrixXd &matIn, const char* fileName){
function readStringFromFile (line 129) | std::vector<std::vector<string>> readStringFromFile(const char* fileName...
function calibrateMeasurement (line 150) | bool calibrateMeasurement(Vector4d &measure , int rows, int cols, int co...
FILE: src/utils/matrix_utils.cpp
function Matrix4d (line 17) | Matrix4d getTransformFromVector(VectorXd& pose)
function addVecToMatirx (line 32) | void addVecToMatirx(Eigen::MatrixXd &mat, Eigen::VectorXd &vec)
function zyx_euler_to_quat (line 41) | Eigen::Quaternion<T> zyx_euler_to_quat(const T &roll, const T &pitch, co...
function quat_to_euler_zyx (line 60) | void quat_to_euler_zyx(const Eigen::Quaternion<T>& q, T& roll, T& pitch,...
function rot_to_euler_zyx (line 76) | void rot_to_euler_zyx(const Eigen::Matrix<T,3,3>& R, T& roll, T& pitch, ...
function euler_zyx_to_rot (line 102) | Eigen::Matrix<T,3,3> euler_zyx_to_rot(const T& roll,const T& pitch,const...
function real_to_homo_coord (line 122) | Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> real_to_homo_coord(cons...
function real_to_homo_coord (line 136) | void real_to_homo_coord(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dy...
function real_to_homo_coord_vec (line 148) | Eigen::Matrix<T, Eigen::Dynamic, 1> real_to_homo_coord_vec(const Eigen::...
function homo_to_real_coord (line 162) | Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> homo_to_real_coord(cons...
function homo_to_real_coord (line 174) | void homo_to_real_coord(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dy...
function homo_to_real_coord_vec (line 184) | Eigen::Matrix<T, Eigen::Dynamic, 1> homo_to_real_coord_vec(const Eigen::...
function fast_RemoveRow (line 197) | void fast_RemoveRow(MatrixXd& matrix,int rowToRemove, int& total_line_nu...
function vert_stack_m (line 203) | void vert_stack_m(const MatrixXd &a_in, const MatrixXd &b_in, MatrixXd &...
function vert_stack_m_self (line 211) | void vert_stack_m_self(MatrixXf &a_in, const MatrixXf &b_in)
function read_all_number_txt (line 222) | bool read_all_number_txt(const std::string txt_file_name, Eigen::Matrix<...
function read_obj_detection_txt (line 260) | bool read_obj_detection_txt(const std::string txt_file_name, Eigen::Matr...
function read_obj_detection2_txt (line 297) | bool read_obj_detection2_txt(const std::string txt_file_name, Eigen::Mat...
function sort_indexes (line 338) | void sort_indexes(const Eigen::VectorXd &vec, std::vector<int>& idx, int...
function sort_indexes (line 343) | void sort_indexes(const Eigen::VectorXd &vec, std::vector<int>& idx)
function T (line 351) | T normalize_to_pi(T angle)
function print_vector (line 365) | void print_vector(const std::vector<T>& vec)
function linespace (line 375) | void linespace(T starting, T ending, T step, std::vector<T>& res) {
Condensed preview — 232 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (997K chars).
[
{
"path": "CMakeLists.txt",
"chars": 2768,
"preview": "cmake_minimum_required(VERSION 2.8)\nproject(EllipsoidSLAM)\n\nSET(CMAKE_BUILD_TYPE Release)\n# SET(CMAKE_BUILD_TYPE Debug)\n"
},
{
"path": "Example/dataset/cabinet/associate.py",
"chars": 5330,
"preview": "#!/usr/bin/python\n# Software License Agreement (BSD License)\n#\n# Copyright (c) 2013, Juergen Sturm, TUM\n# All rights res"
},
{
"path": "Example/dataset/cabinet/associate.txt",
"chars": 4986,
"preview": "1341841278.842700 rgb/1341841278.8427.jpg 1341841278.842700 depth/1341841278.8427.png\n1341841279.510700 rgb/1341841279.5"
},
{
"path": "Example/dataset/cabinet/associateGroundtruth.txt",
"chars": 6478,
"preview": "1341841278.842700 rgb/1341841278.8427.jpg 1341841278.842700 -2.5508 0.9872 1.1019 -0.4871 0.7673 -0.3519 0.2239\n13418412"
},
{
"path": "Example/dataset/cabinet/bbox/1341841278.8427.txt",
"chars": 27,
"preview": "0 175 24 560 397 28 0.42 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841279.5107.txt",
"chars": 27,
"preview": "0 201 40 542 426 28 0.32 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841280.1825.txt",
"chars": 27,
"preview": "0 208 54 559 425 28 0.54 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841280.8507.txt",
"chars": 27,
"preview": "0 182 56 587 420 28 0.54 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841281.5546.txt",
"chars": 27,
"preview": "0 180 41 585 388 28 0.57 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841282.2265.txt",
"chars": 26,
"preview": "0 173 57 569 418 28 0.6 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841282.8987.txt",
"chars": 27,
"preview": "0 170 55 603 425 28 0.44 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841283.5665.txt",
"chars": 27,
"preview": "0 127 52 585 379 28 0.16 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841284.2388.txt",
"chars": 27,
"preview": "0 156 49 551 348 28 0.23 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841284.9067.txt",
"chars": 27,
"preview": "0 218 56 557 326 28 0.18 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841285.6124.txt",
"chars": 27,
"preview": "0 204 62 576 341 28 0.29 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841286.2827.txt",
"chars": 27,
"preview": "0 231 51 594 341 28 0.52 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841286.9506.txt",
"chars": 27,
"preview": "0 172 40 537 348 28 0.28 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841287.6227.txt",
"chars": 27,
"preview": "0 206 49 553 378 28 0.48 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841288.2946.txt",
"chars": 27,
"preview": "0 188 28 574 359 28 0.45 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841288.9627.txt",
"chars": 27,
"preview": "0 236 40 605 376 28 0.51 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841289.6346.txt",
"chars": 27,
"preview": "0 273 57 551 362 28 0.52 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841290.303.txt",
"chars": 27,
"preview": "0 291 62 535 352 28 0.28 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841290.9745.txt",
"chars": 27,
"preview": "0 260 52 493 344 28 0.58 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841291.6426.txt",
"chars": 27,
"preview": "0 312 59 558 351 28 0.33 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841292.3147.txt",
"chars": 0,
"preview": ""
},
{
"path": "Example/dataset/cabinet/bbox/1341841292.9866.txt",
"chars": 0,
"preview": ""
},
{
"path": "Example/dataset/cabinet/bbox/1341841293.6548.txt",
"chars": 0,
"preview": ""
},
{
"path": "Example/dataset/cabinet/bbox/1341841294.3266.txt",
"chars": 0,
"preview": ""
},
{
"path": "Example/dataset/cabinet/bbox/1341841294.9952.txt",
"chars": 27,
"preview": "0 168 30 490 359 28 0.28 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841295.6666.txt",
"chars": 27,
"preview": "0 162 49 469 371 28 0.31 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841296.3348.txt",
"chars": 27,
"preview": "0 173 51 488 368 28 0.42 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841297.0065.txt",
"chars": 27,
"preview": "0 164 33 484 326 28 0.25 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841297.6786.txt",
"chars": 27,
"preview": "0 142 28 465 336 28 0.48 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841298.3466.txt",
"chars": 27,
"preview": "0 109 40 453 321 28 0.36 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841299.0185.txt",
"chars": 27,
"preview": "0 151 50 466 313 28 0.28 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841299.6866.txt",
"chars": 27,
"preview": "0 153 74 466 317 28 0.28 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841300.3656.txt",
"chars": 27,
"preview": "0 131 84 457 309 28 0.23 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841301.0336.txt",
"chars": 0,
"preview": ""
},
{
"path": "Example/dataset/cabinet/bbox/1341841301.7057.txt",
"chars": 26,
"preview": "0 151 55 524 295 28 0.2 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841302.3776.txt",
"chars": 0,
"preview": ""
},
{
"path": "Example/dataset/cabinet/bbox/1341841303.0456.txt",
"chars": 27,
"preview": "0 155 67 522 347 28 0.24 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841303.7106.txt",
"chars": 27,
"preview": "0 163 61 547 364 28 0.39 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841304.3787.txt",
"chars": 27,
"preview": "0 175 45 551 368 28 0.61 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841305.0506.txt",
"chars": 26,
"preview": "0 131 46 541 365 28 0.7 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841305.7227.txt",
"chars": 27,
"preview": "0 185 11 579 347 28 0.72 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841306.3907.txt",
"chars": 27,
"preview": "0 132 26 516 402 28 0.69 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841307.0627.txt",
"chars": 25,
"preview": "0 85 1 465 359 28 0.63 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841307.7306.txt",
"chars": 27,
"preview": "0 144 47 477 429 28 0.53 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841308.4099.txt",
"chars": 26,
"preview": "0 200 34 571 387 28 0.4 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841309.1095.txt",
"chars": 27,
"preview": "0 221 45 585 421 28 0.29 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841309.7816.txt",
"chars": 27,
"preview": "0 159 21 512 399 28 0.33 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841310.4468.txt",
"chars": 27,
"preview": "0 122 12 440 335 28 0.38 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841311.1507.txt",
"chars": 27,
"preview": "0 202 36 458 357 28 0.25 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841311.8186.txt",
"chars": 27,
"preview": "0 148 25 402 329 28 0.21 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841312.4905.txt",
"chars": 27,
"preview": "0 157 33 410 321 28 0.25 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841313.1696.txt",
"chars": 27,
"preview": "0 193 40 408 358 28 0.22 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841313.8695.txt",
"chars": 27,
"preview": "0 215 47 425 356 28 0.18 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841314.5706.txt",
"chars": 0,
"preview": ""
},
{
"path": "Example/dataset/cabinet/bbox/1341841315.2456.txt",
"chars": 27,
"preview": "0 204 27 420 322 28 0.18 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841315.9108.txt",
"chars": 26,
"preview": "0 192 26 482 383 28 0.5 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841316.5786.txt",
"chars": 27,
"preview": "0 212 30 497 391 28 0.33 0\n"
},
{
"path": "Example/dataset/cabinet/bbox/1341841317.2506.txt",
"chars": 26,
"preview": "0 185 7 473 382 28 0.34 0\n"
},
{
"path": "Example/dataset/cabinet/depth.txt",
"chars": 2434,
"preview": "1341841278.8427 depth/1341841278.8427.png\n1341841279.5107 depth/1341841279.5107.png\n1341841280.1825 depth/1341841280.182"
},
{
"path": "Example/dataset/cabinet/groundtruth.txt",
"chars": 3926,
"preview": "1341841278.8427 -2.5508 0.9872 1.1019 -0.4871 0.7673 -0.3519 0.2239\n1341841279.5107 -2.4691 1.0757 1.0996 -0.4454 0.7908"
},
{
"path": "Example/dataset/cabinet/rgb.txt",
"chars": 2318,
"preview": "1341841278.8427 rgb/1341841278.8427.jpg\n1341841279.5107 rgb/1341841279.5107.jpg\n1341841280.1825 rgb/1341841280.1825.jpg\n"
},
{
"path": "Example/interface/rgbd.cpp",
"chars": 2407,
"preview": "#include \"core/Initializer.h\"\n#include \"core/Geometry.h\"\n#include \"utils/dataprocess_utils.h\"\n#include \"utils/matrix_uti"
},
{
"path": "Example/param/TUM3.yaml",
"chars": 2469,
"preview": "%YAML:1.0\nDataset.Type: \"TUM\"\n\n# ------------------------------------\n# Single-frame ellipsoid estimation\n# -----------"
},
{
"path": "LICENSE",
"chars": 1518,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2020, Liao Ziwei\nAll rights reserved.\n\nRedistribution and use in source and binary f"
},
{
"path": "README.md",
"chars": 4159,
"preview": "# EllipsoidSLAM\n\n## Update\n### Aug 22, 2021\n* Now support Ubuntu 20.04 and OpenCV 4.2\n* Fix bugs for crushes\n\n## Introdu"
},
{
"path": "Thirdparty/g2o/CMakeLists.txt",
"chars": 5849,
"preview": "CMAKE_MINIMUM_REQUIRED(VERSION 2.6)\nset(CMAKE_CXX_FLAGS \"-std=c++11 ${CMAKE_CXX_FLAGS}\") # add by me. synced with flags"
},
{
"path": "Thirdparty/g2o/README.txt",
"chars": 161,
"preview": "Modified g2o version\nSee the original g2o library at: https://github.com/RainerKuemmerle/g2o\nAll files included in this "
},
{
"path": "Thirdparty/g2o/cmake_modules/FindBLAS.cmake",
"chars": 12988,
"preview": "# Find BLAS library\n#\n# This module finds an installed library that implements the BLAS\n# linear-algebra interface (see "
},
{
"path": "Thirdparty/g2o/cmake_modules/FindEigen3.cmake",
"chars": 3249,
"preview": "# - Try to find Eigen3 lib\n#\n# This module supports requiring a minimum version, e.g. you can do\n# find_package(Eigen3"
},
{
"path": "Thirdparty/g2o/cmake_modules/FindLAPACK.cmake",
"chars": 9909,
"preview": "# Find LAPACK library\n#\n# This module finds an installed library that implements the LAPACK\n# linear-algebra interface ("
},
{
"path": "Thirdparty/g2o/config.h",
"chars": 380,
"preview": "#ifndef G2O_CONFIG_H\n#define G2O_CONFIG_H\n\n/* #undef G2O_OPENMP */\n/* #undef G2O_SHARED_LIBS */\n\n// give a warning if Ei"
},
{
"path": "Thirdparty/g2o/config.h.in",
"chars": 384,
"preview": "#ifndef G2O_CONFIG_H\n#define G2O_CONFIG_H\n\n#cmakedefine G2O_OPENMP 1\n#cmakedefine G2O_SHARED_LIBS 1\n\n// give a warning i"
},
{
"path": "Thirdparty/g2o/g2o/core/base_binary_edge.h",
"chars": 4621,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/base_binary_edge.hpp",
"chars": 7868,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/base_edge.h",
"chars": 3912,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/base_multi_edge.h",
"chars": 3921,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/base_multi_edge.hpp",
"chars": 7378,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/base_unary_edge.h",
"chars": 3603,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/base_unary_edge.hpp",
"chars": 4571,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/base_vertex.h",
"chars": 4545,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/base_vertex.hpp",
"chars": 2152,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/batch_stats.cpp",
"chars": 3360,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/batch_stats.h",
"chars": 3782,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/block_solver.h",
"chars": 7595,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/block_solver.hpp",
"chars": 22001,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/cache.cpp",
"chars": 5242,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/cache.h",
"chars": 4952,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/creators.h",
"chars": 2656,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/eigen_types.h",
"chars": 4167,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/estimate_propagator.cpp",
"chars": 10141,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/estimate_propagator.h",
"chars": 6395,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/factory.cpp",
"chars": 6420,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/factory.h",
"chars": 5930,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp",
"chars": 9489,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/hyper_dijkstra.h",
"chars": 4579,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/hyper_graph.cpp",
"chars": 4368,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/hyper_graph.h",
"chars": 8135,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/hyper_graph_action.cpp",
"chars": 8960,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/hyper_graph_action.h",
"chars": 7930,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/jacobian_workspace.cpp",
"chars": 3376,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/jacobian_workspace.h",
"chars": 3321,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/linear_solver.h",
"chars": 3811,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp",
"chars": 7345,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h",
"chars": 4172,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/matrix_operations.h",
"chars": 3358,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/matrix_structure.cpp",
"chars": 3566,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/matrix_structure.h",
"chars": 2581,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/openmp_mutex.h",
"chars": 2916,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimizable_graph.cpp",
"chars": 27537,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/optimizable_graph.h",
"chars": 25597,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All righ"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm.cpp",
"chars": 2283,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm.h",
"chars": 4355,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp",
"chars": 8625,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h",
"chars": 3493,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp",
"chars": 4892,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h",
"chars": 6192,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp",
"chars": 3491,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h",
"chars": 2177,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp",
"chars": 6704,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h",
"chars": 3815,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_property.h",
"chars": 2593,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp",
"chars": 3537,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h",
"chars": 2731,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/parameter.cpp",
"chars": 1587,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/parameter.h",
"chars": 2166,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/parameter_container.cpp",
"chars": 4459,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/parameter_container.h",
"chars": 2827,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/robust_kernel.cpp",
"chars": 1670,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/robust_kernel.h",
"chars": 2835,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp",
"chars": 3401,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/robust_kernel_factory.h",
"chars": 4749,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp",
"chars": 4973,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/robust_kernel_impl.h",
"chars": 4678,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/solver.cpp",
"chars": 2610,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/solver.h",
"chars": 5444,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/sparse_block_matrix.h",
"chars": 9514,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp",
"chars": 25490,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h",
"chars": 10801,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h",
"chars": 4184,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp",
"chars": 3192,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/sparse_optimizer.cpp",
"chars": 20717,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/core/sparse_optimizer.h",
"chars": 12602,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h",
"chars": 3946,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// Copyright (C) 2012 R. Kümmerle\n// All rights re"
},
{
"path": "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h",
"chars": 9099,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/color_macros.h",
"chars": 2572,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/macros.h",
"chars": 4360,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/misc.h",
"chars": 4207,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/os_specific.c",
"chars": 2043,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/os_specific.h",
"chars": 1882,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/property.cpp",
"chars": 3410,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/property.h",
"chars": 5023,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/string_tools.cpp",
"chars": 5037,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/string_tools.h",
"chars": 4767,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/timeutil.cpp",
"chars": 3722,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/stuff/timeutil.h",
"chars": 3690,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n"
},
{
"path": "Thirdparty/g2o/g2o/types/CMakeLists.txt",
"chars": 319,
"preview": "ADD_LIBRARY(types ${G2O_LIB_TYPE}\n types_sba.h\n types_six_dof_expmap.h\n types_sba.cpp\n types_six_dof_expmap.cpp\n ty"
},
{
"path": "Thirdparty/g2o/g2o/types/se3_ops.h",
"chars": 2061,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/g2o/types/se3_ops.hpp",
"chars": 3218,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/g2o/types/se3quat.h",
"chars": 10452,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/g2o/types/sim3.h",
"chars": 6958,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/g2o/types/types_sba.cpp",
"chars": 1905,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 Kurt Konolige\n// All rights reserved.\n//\n// Redistribution and"
},
{
"path": "Thirdparty/g2o/g2o/types/types_sba.h",
"chars": 2045,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 Kurt Konolige\n// All rights reserved.\n//\n// Redistribution and"
},
{
"path": "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp",
"chars": 6226,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h",
"chars": 5648,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp",
"chars": 11485,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h",
"chars": 8656,
"preview": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and u"
},
{
"path": "Thirdparty/g2o/license-bsd.txt",
"chars": 1401,
"preview": "g2o - General Graph Optimization\nCopyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat,\nKurt Konolige, a"
},
{
"path": "cmake_modules/FindEigen3.cmake",
"chars": 3249,
"preview": "# - Try to find Eigen3 lib\n#\n# This module supports requiring a minimum version, e.g. you can do\n# find_package(Eigen3"
},
{
"path": "include/core/BasicEllipsoidEdges.h",
"chars": 2024,
"preview": "#include \"include/core/Ellipsoid.h\"\n#include \"Thirdparty/g2o/g2o/core/base_multi_edge.h\"\n#include \"Thirdparty/g2o/g2o/ty"
},
{
"path": "include/core/DataAssociation.h",
"chars": 621,
"preview": "#ifndef ELLIPSOIDSLAM_DATAASSOCIATION_H\n#define ELLIPSOIDSLAM_DATAASSOCIATION_H\n\n#include <core/Map.h>\n#include <core/Fr"
},
{
"path": "include/core/Ellipsoid.h",
"chars": 5053,
"preview": "#pragma once\n\n#include \"Thirdparty/g2o/g2o/core/base_multi_edge.h\"\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_exp"
},
{
"path": "include/core/Frame.h",
"chars": 1319,
"preview": "#ifndef ELLIPSOIDSLAM_FRAME_H\n#define ELLIPSOIDSLAM_FRAME_H\n\n#include <opencv2/opencv.hpp>\n#include <vector>\n#include \"E"
},
{
"path": "include/core/FrameDrawer.h",
"chars": 678,
"preview": "#ifndef ELLIPSOIDSLAM_FRAMEDRAWER_H\n#define ELLIPSOIDSLAM_FRAMEDRAWER_H\n\n#include \"Map.h\"\n#include \"Tracking.h\"\n\nnamespa"
},
{
"path": "include/core/Geometry.h",
"chars": 1447,
"preview": "#ifndef ELLIPSOIDSLAM_GEOMETRY_H\n#define ELLIPSOIDSLAM_GEOMETRY_H\n\n#include <vector>\n#include <opencv2/opencv.hpp>\n#incl"
},
{
"path": "include/core/Initializer.h",
"chars": 2618,
"preview": "#ifndef ELLIPSOIDSLAM_INITIALIZER_H\n#define ELLIPSOIDSLAM_INITIALIZER_H\n\n#include \"Ellipsoid.h\"\n#include \"Frame.h\"\n\n#inc"
},
{
"path": "include/core/Map.h",
"chars": 2114,
"preview": "#ifndef ELLIPSOIDSLAM_MAP_H\n#define ELLIPSOIDSLAM_MAP_H\n\n#include \"Ellipsoid.h\"\n#include \"Geometry.h\"\n#include \"Plane.h\""
},
{
"path": "include/core/MapDrawer.h",
"chars": 1526,
"preview": "#ifndef ELLIPSOIDSLAM_MAPDRAWER_H\n#define ELLIPSOIDSLAM_MAPDRAWER_H\n\n#include \"Map.h\"\n#include<pangolin/pangolin.h>\n\n#in"
},
{
"path": "include/core/Optimizer.h",
"chars": 920,
"preview": "#ifndef ELLIPSOIDSLAM_OPTIMIZER_H\n#define ELLIPSOIDSLAM_OPTIMIZER_H\n\n#include \"Frame.h\"\n#include \"Map.h\"\n#include \"Initi"
},
{
"path": "include/core/Plane.h",
"chars": 3630,
"preview": "#pragma once\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n#include <Eigen/Dense>\n\n#include \"include/utils/matrix_ut"
},
{
"path": "include/core/System.h",
"chars": 1610,
"preview": "#ifndef ELLIPSOIDSLAM_SYSTEM_H\n#define ELLIPSOIDSLAM_SYSTEM_H\n\n#include \"Frame.h\"\n#include \"Map.h\"\n#include \"MapDrawer.h"
},
{
"path": "include/core/Tracking.h",
"chars": 3722,
"preview": "#ifndef ELLIPSOIDSLAM_TRACKING_H\n#define ELLIPSOIDSLAM_TRACKING_H\n\n#include \"System.h\"\n#include \"FrameDrawer.h\"\n#include"
},
{
"path": "include/core/Viewer.h",
"chars": 1719,
"preview": "#ifndef ELLIPSOIDSLAM_VIEWER_H\n#define ELLIPSOIDSLAM_VIEWER_H\n\n#include <opencv2/opencv.hpp>\n#include <string>\n\n#include"
},
{
"path": "include/utils/dataprocess_utils.h",
"chars": 1018,
"preview": "#ifndef ELLIPSOIDSLAM_DATAPROCESS_UTILS_H\n#define ELLIPSOIDSLAM_DATAPROCESS_UTILS_H\n\n// Eigen\n#include <Eigen/Core>\n#inc"
},
{
"path": "include/utils/matrix_utils.h",
"chars": 3555,
"preview": "#ifndef ELLIPSOIDSLAM_MATRIX_UTILS_H\n#define ELLIPSOIDSLAM_MATRIX_UTILS_H\n\n#include <vector>\n// Eigen\n#include <Eigen/Co"
},
{
"path": "install_g2o.sh",
"chars": 97,
"preview": "#!/bin/bash\n\ncd Thirdparty/g2o\nmkdir build\ncd build\ncmake .. -DCMAKE_BUILD_TYPE=Release\nmake -j2\n"
},
{
"path": "src/Polygon/CMakeLists.txt",
"chars": 87,
"preview": "add_library(Polygon SHARED\n Polygon.cpp)\ntarget_link_libraries(Polygon ${OpenCV_LIBS})\n"
},
{
"path": "src/Polygon/Polygon.cpp",
"chars": 6217,
"preview": "#include \"Polygon.hpp\"\n\nnamespace EllipsoidSLAM\n{\nfloat distPoint(cv::Point2d v,cv::Point2d w) { \n\treturn sqrtf((v.x - w"
},
{
"path": "src/Polygon/Polygon.hpp",
"chars": 1725,
"preview": "/*\n*\tThis file is dependent on github open-source. All rights are perserved by original authors.\n* \tWeb: https://github."
},
{
"path": "src/config/CMakeLists.txt",
"chars": 107,
"preview": "\nadd_library(Config SHARED\n Config.cpp\n)\n\ntarget_link_libraries(Config\n ${OpenCV_LIBS}\n )\n"
},
{
"path": "src/config/Config.cpp",
"chars": 1112,
"preview": "\n#include \"Config.h\"\n\nnamespace EllipsoidSLAM{\n void Config::SetParameterFile( const std::string& filename )\n {\n "
},
{
"path": "src/config/Config.h",
"chars": 2076,
"preview": "// A parameter file supporting global dynamic changes\n\n#ifndef ELLIPSOIDSLAM_CONFIG_H\n#define ELLIPSOIDSLAM_CONFIG_H\n\n#i"
},
{
"path": "src/core/BasicEllipsoidEdges.cpp",
"chars": 4701,
"preview": "#include \"include/core/BasicEllipsoidEdges.h\"\n\nnamespace g2o\n{\n// ---- VertexEllipsoid\n void VertexEllipsoid::setToOr"
},
{
"path": "src/core/DataAssociation.cpp",
"chars": 5063,
"preview": "#include \"core/DataAssociation.h\"\n\n#include <iostream>\n#include <map>\n\nnamespace EllipsoidSLAM\n{\n\n DataAssociationSol"
},
{
"path": "src/core/Ellipsoid.cpp",
"chars": 18796,
"preview": "#include \"include/core/Ellipsoid.h\"\n\n#include \"src/Polygon/Polygon.hpp\"\n\n#include <Eigen/Core>\n#include <Eigen/SVD>\n#inc"
},
{
"path": "src/core/Frame.cpp",
"chars": 882,
"preview": "#include <include/core/Frame.h>\n\nnamespace EllipsoidSLAM\n{\n int Frame::total_frame=0;\n\n Frame::Frame(double timest"
},
{
"path": "src/core/FrameDrawer.cpp",
"chars": 3898,
"preview": "#include \"include/core/FrameDrawer.h\"\n#include \"src/config/Config.h\"\n#include \"utils/dataprocess_utils.h\"\n\nusing namespa"
}
]
// ... and 32 more files (download for full content)
About this extraction
This page contains the full source code of the XunshanMan/Object-oriented-SLAM GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 232 files (928.0 KB), approximately 256.5k tokens, and a symbol index with 392 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.