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 #include "include/core/Viewer.h" #include "include/core/MapDrawer.h" #include "include/core/Map.h" #include #include #include "include/core/Ellipsoid.h" #include "src/tum_rgbd/io.h" #include #include typedef pcl::PointXYZRGB PointT; typedef pcl::PointCloud 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 ![](.pics/demo.png) 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, # Copyright (c) 2008, 2009 Gael Guennebaud, # Copyright (c) 2009 Benoit Jacob # 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 #include #include "base_edge.h" #include "robust_kernel.h" #include "../../config.h" namespace g2o { using namespace Eigen; template class BaseBinaryEdge : public BaseEdge { public: typedef VertexXi VertexXiType; typedef VertexXj VertexXjType; static const int Di = VertexXiType::Dimension; static const int Dj = VertexXjType::Dimension; static const int Dimension = BaseEdge::Dimension; typedef typename BaseEdge::Measurement Measurement; typedef typename Matrix::AlignedMapType JacobianXiOplusType; typedef typename Matrix::AlignedMapType JacobianXjOplusType; typedef typename BaseEdge::ErrorVector ErrorVector; typedef typename BaseEdge::InformationType InformationType; typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType; BaseBinaryEdge() : BaseEdge(), _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::resize; using BaseEdge::computeError; protected: using BaseEdge::_measurement; using BaseEdge::_information; using BaseEdge::_error; using BaseEdge::_vertices; using BaseEdge::_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 OptimizableGraph::Vertex* BaseBinaryEdge::createFrom(){ return new VertexXiType(); } template OptimizableGraph::Vertex* BaseBinaryEdge::createTo(){ return new VertexXjType(); } template void BaseBinaryEdge::resize(size_t size) { if (size != 2) { std::cerr << "WARNING, attempting to resize binary edge " << BaseEdge::id() << " to " << size << std::endl; } BaseEdge::resize(size); } template bool BaseBinaryEdge::allVerticesFixed() const { return (static_cast (_vertices[0])->fixed() && static_cast (_vertices[1])->fixed()); } template void BaseBinaryEdge::constructQuadraticForm() { VertexXiType* from = static_cast(_vertices[0]); VertexXjType* to = static_cast(_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 omega_r = - omega * _error; if (this->robustKernel() == 0) { if (fromNotFixed) { Matrix 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 void BaseBinaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) { new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di); new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj); linearizeOplus(); } template void BaseBinaryEdge::linearizeOplus() { VertexXiType* vi = static_cast(_vertices[0]); VertexXjType* vj = static_cast(_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 void BaseBinaryEdge::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 #include #include #include "optimizable_graph.h" namespace g2o { using namespace Eigen; template class BaseEdge : public OptimizableGraph::Edge { public: static const int Dimension = D; typedef E Measurement; typedef Matrix ErrorVector; typedef Matrix 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 #include #include #include #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 class BaseMultiEdge : public BaseEdge { public: /** * \brief helper for mapping the Hessian memory of the upper triangular block */ struct HessianHelper { Eigen::Map 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::Dimension; typedef typename BaseEdge::Measurement Measurement; typedef MatrixXd::MapType JacobianType; typedef typename BaseEdge::ErrorVector ErrorVector; typedef typename BaseEdge::InformationType InformationType; typedef Eigen::Map HessianBlockType; BaseMultiEdge() : BaseEdge() { } 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::computeError; protected: using BaseEdge::_measurement; using BaseEdge::_information; using BaseEdge::_error; using BaseEdge::_vertices; using BaseEdge::_dimension; std::vector _hessian; std::vector > _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 void BaseMultiEdge::constructQuadraticForm() { if (this->robustKernel()) { double error = this->chi2(); Eigen::Vector3d rho; this->robustKernel()->robustify(error, rho); Matrix omega_r = - _information * _error; omega_r *= rho[1]; computeQuadraticForm(this->robustInformation(rho), omega_r); } else { computeQuadraticForm(_information, - _information * _error); } } template void BaseMultiEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) { for (size_t i = 0; i < _vertices.size(); ++i) { OptimizableGraph::Vertex* v = static_cast(_vertices[i]); assert(v->dimension() >= 0); new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension()); } linearizeOplus(); } template void BaseMultiEdge::linearizeOplus() { #ifdef G2O_OPENMP for (size_t i = 0; i < _vertices.size(); ++i) { OptimizableGraph::Vertex* v = static_cast(_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(_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(_vertices[i]); v->unlockQuadraticForm(); } #endif } template void BaseMultiEdge::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(HyperGraph::Edge::vertex(i)); OptimizableGraph::Vertex* vj = static_cast(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 void BaseMultiEdge::resize(size_t size) { BaseEdge::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 bool BaseMultiEdge::allVerticesFixed() const { for (size_t i = 0; i < _vertices.size(); ++i) { if (!static_cast (_vertices[i])->fixed()) { return false; } } return true; } template void BaseMultiEdge::computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError) { for (size_t i = 0; i < _vertices.size(); ++i) { OptimizableGraph::Vertex* from = static_cast(_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 fromMap(from->hessianData(), fromDim, fromDim); Eigen::Map 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(_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 #include #include #include "base_edge.h" #include "robust_kernel.h" #include "../../config.h" namespace g2o { using namespace Eigen; template class BaseUnaryEdge : public BaseEdge { public: static const int Dimension = BaseEdge::Dimension; typedef typename BaseEdge::Measurement Measurement; typedef VertexXi VertexXiType; typedef typename Matrix::AlignedMapType JacobianXiOplusType; typedef typename BaseEdge::ErrorVector ErrorVector; typedef typename BaseEdge::InformationType InformationType; BaseUnaryEdge() : BaseEdge(), _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::resize; using BaseEdge::computeError; protected: using BaseEdge::_measurement; using BaseEdge::_information; using BaseEdge::_error; using BaseEdge::_vertices; using BaseEdge::_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 void BaseUnaryEdge::resize(size_t size) { if (size != 1) { std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge::id() << " to " << size << std::endl; } BaseEdge::resize(size); } template bool BaseUnaryEdge::allVerticesFixed() const { return static_cast (_vertices[0])->fixed(); } template void BaseUnaryEdge::constructQuadraticForm() { VertexXiType* from=static_cast(_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 void BaseUnaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) { new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension); linearizeOplus(); } template void BaseUnaryEdge::linearizeOplus() { //Xi - estimate the jacobian numerically VertexXiType* vi = static_cast(_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 void BaseUnaryEdge::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 #include #include #include #include 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 class BaseVertex : public OptimizableGraph::Vertex { public: typedef T EstimateType; typedef std::stack > > BackupStackType; static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType; public: BaseVertex(); virtual const double& hessian(int i, int j) const { assert(i(_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& b() { return _b;} const Matrix& 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 _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 BaseVertex::BaseVertex() : OptimizableGraph::Vertex(), _hessian(0, D, D) { _dimension = D; } template double BaseVertex::solveDirect(double lambda) { Matrix tempA=_hessian + Matrix ::Identity()*lambda; double det=tempA.determinant(); if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) return det; Matrix dx=tempA.llt().solve(_b); oplus(&dx[0]); return det; } template void BaseVertex::clearQuadraticForm() { _b.setZero(); } template void BaseVertex::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 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 #include 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 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 #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 struct BlockSolverTraits { static const int PoseDim = _PoseDim; static const int LandmarkDim = _LandmarkDim; typedef Matrix PoseMatrixType; typedef Matrix LandmarkMatrixType; typedef Matrix PoseLandmarkMatrixType; typedef Matrix PoseVectorType; typedef Matrix LandmarkVectorType; typedef SparseBlockMatrix PoseHessianType; typedef SparseBlockMatrix LandmarkHessianType; typedef SparseBlockMatrix PoseLandmarkHessianType; typedef LinearSolver LinearSolverType; }; /** * \brief traits to summarize the properties of the dynamic size optimization problem */ template <> struct BlockSolverTraits { 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 PoseHessianType; typedef SparseBlockMatrix LandmarkHessianType; typedef SparseBlockMatrix PoseLandmarkHessianType; typedef LinearSolver 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 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& vset, const HyperGraph::EdgeSet& edges); virtual bool buildSystem(); virtual bool solve(); virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& 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* 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* _Hpp; SparseBlockMatrix* _Hll; SparseBlockMatrix* _Hpl; SparseBlockMatrix* _Hschur; SparseBlockMatrixDiagonal* _DInvSchur; SparseBlockMatrixCCS* _HplCCS; SparseBlockMatrixCCS* _HschurTransposedCCS; LinearSolver* _linearSolver; std::vector > _diagonalBackupPose; std::vector > _diagonalBackupLandmark; # ifdef G2O_OPENMP std::vector _coefficientsMutex; # endif bool _doSchur; double* _coefficients; double* _bschur; int _numPoses, _numLandmarks; int _sizePoses, _sizeLandmarks; }; //variable size solver typedef BlockSolver< BlockSolverTraits > 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 #include #include #include "../stuff/timeutil.h" #include "../stuff/macros.h" #include "../stuff/misc.h" namespace g2o { using namespace std; using namespace Eigen; template BlockSolver::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 void BlockSolver::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(_Hll->colBlockIndices()); _Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks); _HplCCS = new SparseBlockMatrixCCS(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices()); _HschurTransposedCCS = new SparseBlockMatrixCCS(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices()); #ifdef G2O_OPENMP _coefficientsMutex.resize(numPoseBlocks); #endif } } template void BlockSolver::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 BlockSolver::~BlockSolver() { delete _linearSolver; deallocate(); } template bool BlockSolver::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* schurMatrixLookup = 0; if (_doSchur) { schurMatrixLookup = new SparseBlockMatrixHashMap(_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 bool BlockSolver::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) { for (std::vector::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) { OptimizableGraph::Vertex* v = static_cast(*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::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(*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 bool BlockSolver::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(_Hll->blockCols().size()); ++landmarkIndex) { const typename SparseBlockMatrix::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; jrows(); ++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::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex]; for (typename SparseBlockMatrixCCS::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(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds"); typename SparseBlockMatrixCCS::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin(); typename SparseBlockMatrixCCS::RowBlock aux(i1, 0); typename SparseBlockMatrixCCS::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 bool BlockSolver::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& 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 bool BlockSolver::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(_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(_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(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(_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 bool BlockSolver::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 void BlockSolver::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 bool BlockSolver::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 void BlockSolver::setWriteDebug(bool writeDebug) { _linearSolver->setWriteDebug(writeDebug); } template bool BlockSolver::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 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::iterator it=_parentCaches.begin(); it!=_parentCaches.end(); it++){ (*it)->update(); } updateImpl(); _updateNeeded=false; } Cache* Cache::installDependency(const std::string& type_, const std::vector& parameterIndices){ ParameterVector pv(parameterIndices.size()); for (size_t i=0; 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(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 #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& 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 _parentCaches; CacheContainer* _container; }; class CacheContainer: public std::map { 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 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(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 #include 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 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 #include namespace g2o { typedef Eigen::Matrix Vector2I; typedef Eigen::Matrix Vector3I; typedef Eigen::Matrix Vector4I; typedef Eigen::Matrix VectorXI; typedef Eigen::Matrix Vector2F; typedef Eigen::Matrix Vector3F; typedef Eigen::Matrix Vector4F; typedef Eigen::Matrix VectorXF; typedef Eigen::Matrix Vector2D; typedef Eigen::Matrix Vector3D; typedef Eigen::Matrix Vector4D; typedef Eigen::Matrix VectorXD; typedef Eigen::Matrix Matrix2I; typedef Eigen::Matrix Matrix3I; typedef Eigen::Matrix Matrix4I; typedef Eigen::Matrix MatrixXI; typedef Eigen::Matrix Matrix2F; typedef Eigen::Matrix Matrix3F; typedef Eigen::Matrix Matrix4F; typedef Eigen::Matrix MatrixXF; typedef Eigen::Matrix Matrix2D; typedef Eigen::Matrix Matrix3D; typedef Eigen::Matrix Matrix4D; typedef Eigen::Matrix MatrixXD; typedef Eigen::Transform Isometry2D; typedef Eigen::Transform Isometry3D; typedef Eigen::Transform Affine2D; typedef Eigen::Transform 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 #include #include #include #include #include //#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::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(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(*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(*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(*et); ++et; int maxFrontier = -1; OptimizableGraph::VertexSet initializedVertices; for (size_t i = 0; i < edge->vertices().size(); ++i) { OptimizableGraph::Vertex* z = static_cast(edge->vertex(i)); AdjacencyMap::iterator ot = _adjacencyMap.find(z); if (ot->second._distance != numeric_limits::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(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::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 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::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(edge); OptimizableGraph::Vertex* to = dynamic_cast(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::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(edge); OptimizableGraph::Vertex* from = dynamic_cast(*from_.begin()); OptimizableGraph::Vertex* to = dynamic_cast(to_); if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph return std::numeric_limits::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::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 #include #include #ifdef _MSC_VER #include #else #include #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 { 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 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::max(), double maxEdgeCost=std::numeric_limits::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::max(), double maxEdgeCost=std::numeric_limits::max()); protected: void reset(); AdjacencyMap _adjacencyMap; OptimizableGraph::VertexSet _visited; OptimizableGraph* _graph; }; } #endif ================================================ FILE: Thirdparty/g2o/g2o/core/factory.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 "factory.h" #include "creators.h" #include "parameter.h" #include "cache.h" #include "optimizable_graph.h" #include "../stuff/color_macros.h" #include #include #include using namespace std; namespace g2o { Factory* Factory::factoryInstance = 0; Factory::Factory() { } Factory::~Factory() { # ifdef G2O_DEBUG_FACTORY cerr << "# Factory destroying " << (void*)this << endl; # endif for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { delete it->second->creator; } _creator.clear(); _tagLookup.clear(); } Factory* Factory::instance() { if (factoryInstance == 0) { factoryInstance = new Factory; # ifdef G2O_DEBUG_FACTORY cerr << "# Factory allocated " << (void*)factoryInstance << endl; # endif } return factoryInstance; } void Factory::registerType(const std::string& tag, AbstractHyperGraphElementCreator* c) { CreatorMap::const_iterator foundIt = _creator.find(tag); if (foundIt != _creator.end()) { cerr << "FACTORY WARNING: Overwriting Vertex tag " << tag << endl; assert(0); } TagLookup::const_iterator tagIt = _tagLookup.find(c->name()); if (tagIt != _tagLookup.end()) { cerr << "FACTORY WARNING: Registering same class for two tags " << c->name() << endl; assert(0); } CreatorInformation* ci = new CreatorInformation(); ci->creator = c; #ifdef G2O_DEBUG_FACTORY cerr << "# Factory " << (void*)this << " constructing type " << tag << " "; #endif // construct an element once to figure out its type HyperGraph::HyperGraphElement* element = c->construct(); ci->elementTypeBit = element->elementType(); #ifdef G2O_DEBUG_FACTORY cerr << "done." << endl; cerr << "# Factory " << (void*)this << " registering " << tag; cerr << " " << (void*) c << " "; switch (element->elementType()) { case HyperGraph::HGET_VERTEX: cerr << " -> Vertex"; break; case HyperGraph::HGET_EDGE: cerr << " -> Edge"; break; case HyperGraph::HGET_PARAMETER: cerr << " -> Parameter"; break; case HyperGraph::HGET_CACHE: cerr << " -> Cache"; break; case HyperGraph::HGET_DATA: cerr << " -> Data"; break; default: assert(0 && "Unknown element type occured, fix elementTypes"); break; } cerr << endl; #endif _creator[tag] = ci; _tagLookup[c->name()] = tag; delete element; } void Factory::unregisterType(const std::string& tag) { // Look for the tag CreatorMap::iterator tagPosition = _creator.find(tag); if (tagPosition != _creator.end()) { AbstractHyperGraphElementCreator* c = tagPosition->second->creator; // If we found it, remove the creator from the tag lookup map TagLookup::iterator classPosition = _tagLookup.find(c->name()); if (classPosition != _tagLookup.end()) { _tagLookup.erase(classPosition); } _creator.erase(tagPosition); } } HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag) const { CreatorMap::const_iterator foundIt = _creator.find(tag); if (foundIt != _creator.end()) { //cerr << "tag " << tag << " -> " << (void*) foundIt->second->creator << " " << foundIt->second->creator->name() << endl; return foundIt->second->creator->construct(); } return 0; } const std::string& Factory::tag(const HyperGraph::HyperGraphElement* e) const { static std::string emptyStr(""); TagLookup::const_iterator foundIt = _tagLookup.find(typeid(*e).name()); if (foundIt != _tagLookup.end()) return foundIt->second; return emptyStr; } void Factory::fillKnownTypes(std::vector& types) const { types.clear(); for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) types.push_back(it->first); } bool Factory::knowsTag(const std::string& tag, int* elementType) const { CreatorMap::const_iterator foundIt = _creator.find(tag); if (foundIt == _creator.end()) { if (elementType) *elementType = -1; return false; } if (elementType) *elementType = foundIt->second->elementTypeBit; return true; } void Factory::destroy() { delete factoryInstance; factoryInstance = 0; } void Factory::printRegisteredTypes(std::ostream& os, bool comment) const { if (comment) os << "# "; os << "types:" << endl; for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) { if (comment) os << "#"; cerr << "\t" << it->first << endl; } } HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const { if (elemsToConstruct.none()) { return construct(tag); } CreatorMap::const_iterator foundIt = _creator.find(tag); if (foundIt != _creator.end() && foundIt->second->elementTypeBit >= 0 && elemsToConstruct.test(foundIt->second->elementTypeBit)) { return foundIt->second->creator->construct(); } return 0; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/factory.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_FACTORY_H #define G2O_FACTORY_H #include "../../config.h" #include "../stuff/misc.h" #include "hyper_graph.h" #include "creators.h" #include #include #include // define to get some verbose output //#define G2O_DEBUG_FACTORY namespace g2o { class AbstractHyperGraphElementCreator; /** * \brief create vertices and edges based on TAGs in, for example, a file */ class Factory { public: //! return the instance static Factory* instance(); //! free the instance static void destroy(); /** * register a tag for a specific creator */ void registerType(const std::string& tag, AbstractHyperGraphElementCreator* c); /** * unregister a tag for a specific creator */ void unregisterType(const std::string& tag); /** * construct a graph element based on its tag */ HyperGraph::HyperGraphElement* construct(const std::string& tag) const; /** * construct a graph element based on its tag, but only if it's type (a bitmask) matches. A bitmask without any * bit set will construct any item. Otherwise a bit has to be set to allow construction of a graph element. */ HyperGraph::HyperGraphElement* construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const; /** * return whether the factory knows this tag or not */ bool knowsTag(const std::string& tag, int* elementType = 0) const; //! return the TAG given a vertex const std::string& tag(const HyperGraph::HyperGraphElement* v) const; /** * get a list of all known types */ void fillKnownTypes(std::vector& types) const; /** * print a list of the known registered types to the given stream */ void printRegisteredTypes(std::ostream& os, bool comment = false) const; protected: class CreatorInformation { public: AbstractHyperGraphElementCreator* creator; int elementTypeBit; CreatorInformation() { creator = 0; elementTypeBit = -1; } ~CreatorInformation() { std::cout << "Deleting " << (void*) creator << std::endl; delete creator; } }; typedef std::map CreatorMap; typedef std::map TagLookup; Factory(); ~Factory(); CreatorMap _creator; ///< look-up map for the existing creators TagLookup _tagLookup; ///< reverse look-up, class name to tag private: static Factory* factoryInstance; }; template class RegisterTypeProxy { public: RegisterTypeProxy(const std::string& name) : _name(name) { #ifdef G2O_DEBUG_FACTORY std::cout << __FUNCTION__ << ": Registering " << _name << " of type " << typeid(T).name() << std::endl; #endif Factory::instance()->registerType(_name, new HyperGraphElementCreator()); } ~RegisterTypeProxy() { #ifdef G2O_DEBUG_FACTORY std::cout << __FUNCTION__ << ": Unregistering " << _name << " of type " << typeid(T).name() << std::endl; #endif Factory::instance()->unregisterType(_name); } private: std::string _name; }; #if defined _MSC_VER && defined G2O_SHARED_LIBS # define G2O_FACTORY_EXPORT __declspec(dllexport) # define G2O_FACTORY_IMPORT __declspec(dllimport) #else # define G2O_FACTORY_EXPORT # define G2O_FACTORY_IMPORT #endif // These macros are used to automate registering types and forcing linkage #define G2O_REGISTER_TYPE(name, classname) \ extern "C" void G2O_FACTORY_EXPORT g2o_type_##classname(void) {} \ static g2o::RegisterTypeProxy g_type_proxy_##classname(#name); #define G2O_USE_TYPE_BY_CLASS_NAME(classname) \ extern "C" void G2O_FACTORY_IMPORT g2o_type_##classname(void); \ static g2o::ForceLinker proxy_##classname(g2o_type_##classname); #define G2O_REGISTER_TYPE_GROUP(typeGroupName) \ extern "C" void G2O_FACTORY_EXPORT g2o_type_group_##typeGroupName(void) {} #define G2O_USE_TYPE_GROUP(typeGroupName) \ extern "C" void G2O_FACTORY_IMPORT g2o_type_group_##typeGroupName(void); \ static g2o::ForceLinker g2o_force_type_link_##typeGroupName(g2o_type_group_##typeGroupName); } #endif ================================================ FILE: Thirdparty/g2o/g2o/core/hyper_dijkstra.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 #include #include #include #include #include "hyper_dijkstra.h" #include "../stuff/macros.h" namespace g2o{ using namespace std; double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e){ (void) v; (void) vParent; (void) e; return std::numeric_limits::max(); } double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance){ if (distance==-1) return perform (v,vParent,e); return std::numeric_limits::max(); } HyperDijkstra::AdjacencyMapEntry::AdjacencyMapEntry(HyperGraph::Vertex* child_, HyperGraph::Vertex* parent_, HyperGraph::Edge* edge_, double distance_) { _child=child_; _parent=parent_; _edge=edge_; _distance=distance_; } HyperDijkstra::HyperDijkstra(HyperGraph* g): _graph(g) { for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){ AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max()); _adjacencyMap.insert(make_pair(entry.child(), entry)); } } void HyperDijkstra::reset() { for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){ AdjacencyMap::iterator at=_adjacencyMap.find(*it); assert(at!=_adjacencyMap.end()); at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max()); } _visited.clear(); } bool operator<(const HyperDijkstra::AdjacencyMapEntry& a, const HyperDijkstra::AdjacencyMapEntry& b) { return a.distance()>b.distance(); } void HyperDijkstra::shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost, double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost) { reset(); std::priority_queue< AdjacencyMapEntry > frontier; for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){ HyperGraph::Vertex* v=*vit; assert(v!=0); AdjacencyMap::iterator it=_adjacencyMap.find(v); if (it == _adjacencyMap.end()) { cerr << __PRETTY_FUNCTION__ << "Vertex " << v->id() << " is not in the adjacency map" << endl; } assert(it!=_adjacencyMap.end()); it->second._distance=0.; it->second._parent=0; frontier.push(it->second); } while(! frontier.empty()){ AdjacencyMapEntry entry=frontier.top(); frontier.pop(); HyperGraph::Vertex* u=entry.child(); AdjacencyMap::iterator ut=_adjacencyMap.find(u); if (ut == _adjacencyMap.end()) { cerr << __PRETTY_FUNCTION__ << "Vertex " << u->id() << " is not in the adjacency map" << endl; } assert(ut!=_adjacencyMap.end()); double uDistance=ut->second.distance(); std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult; HyperGraph::EdgeSet::iterator et=u->edges().begin(); while (et != u->edges().end()){ HyperGraph::Edge* edge=*et; ++et; if (directed && edge->vertex(0) != u) continue; for (size_t i = 0; i < edge->vertices().size(); ++i) { HyperGraph::Vertex* z = edge->vertex(i); if (z == u) continue; double edgeDistance=(*cost)(edge, u, z); if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost) continue; double zDistance=uDistance+edgeDistance; //cerr << z->id() << " " << zDistance << endl; AdjacencyMap::iterator ot=_adjacencyMap.find(z); assert(ot!=_adjacencyMap.end()); if (zDistance+comparisonConditionersecond.distance() && zDistancesecond._distance=zDistance; ot->second._parent=u; ot->second._edge=edge; frontier.push(ot->second); } } } } } void HyperDijkstra::shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost) { HyperGraph::VertexSet vset; vset.insert(v); shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost); } void HyperDijkstra::computeTree(AdjacencyMap& amap) { for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){ AdjacencyMapEntry& entry(it->second); entry._children.clear(); } for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){ AdjacencyMapEntry& entry(it->second); HyperGraph::Vertex* parent=entry.parent(); if (!parent){ continue; } HyperGraph::Vertex* v=entry.child(); assert (v==it->first); AdjacencyMap::iterator pt=amap.find(parent); assert(pt!=amap.end()); pt->second._children.insert(v); } } void HyperDijkstra::visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance) { typedef std::deque Deque; Deque q; // scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them. for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){ AdjacencyMapEntry& entry(it->second); if (! entry.parent()) { action->perform(it->first,0,0); q.push_back(it->first); } } //std::cerr << "q.size()" << q.size() << endl; int count=0; while (! q.empty()){ HyperGraph::Vertex* parent=q.front(); q.pop_front(); ++count; AdjacencyMap::iterator parentIt=amap.find(parent); if (parentIt==amap.end()) { continue; } //cerr << "parent= " << parent << " parent id= " << parent->id() << "\t children id ="; HyperGraph::VertexSet& childs(parentIt->second.children()); for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){ HyperGraph::Vertex* child=*childsIt; //cerr << child->id(); AdjacencyMap::iterator adjacencyIt=amap.find(child); assert (adjacencyIt!=amap.end()); HyperGraph::Edge* edge=adjacencyIt->second.edge(); assert(adjacencyIt->first==child); assert(adjacencyIt->second.child()==child); assert(adjacencyIt->second.parent()==parent); if (! useDistance) { action->perform(child, parent, edge); } else { action->perform(child, parent, edge, adjacencyIt->second.distance()); } q.push_back(child); } //cerr << endl; } } void HyperDijkstra::connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, HyperGraph::VertexSet& startingSet, HyperGraph* g, HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner, double maxEdgeCost) { typedef std::queue VertexDeque; visited.clear(); connected.clear(); VertexDeque frontier; HyperDijkstra dv(g); connected.insert(v); frontier.push(v); while (! frontier.empty()){ HyperGraph::Vertex* v0=frontier.front(); frontier.pop(); dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost); for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){ visited.insert(*it); if (startingSet.find(*it)==startingSet.end()) continue; std::pair insertOutcome=connected.insert(*it); if (insertOutcome.second){ // the node was not in the connectedSet; frontier.push(dynamic_cast(*it)); } } } } double UniformCostFunction::operator () (HyperGraph::Edge* /*edge*/, HyperGraph::Vertex* /*from*/, HyperGraph::Vertex* /*to*/) { return 1.; } }; ================================================ FILE: Thirdparty/g2o/g2o/core/hyper_dijkstra.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_AIS_GENERAL_DIJKSTRA_HH #define G2O_AIS_GENERAL_DIJKSTRA_HH #include #include #include #include "hyper_graph.h" namespace g2o{ struct HyperDijkstra{ struct CostFunction { virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0; virtual ~CostFunction() { } }; struct TreeAction { virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e); virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance); }; struct AdjacencyMapEntry{ friend struct HyperDijkstra; AdjacencyMapEntry(HyperGraph::Vertex* _child=0, HyperGraph::Vertex* _parent=0, HyperGraph::Edge* _edge=0, double _distance=std::numeric_limits::max()); HyperGraph::Vertex* child() const {return _child;} HyperGraph::Vertex* parent() const {return _parent;} HyperGraph::Edge* edge() const {return _edge;} double distance() const {return _distance;} HyperGraph::VertexSet& children() {return _children;} const HyperGraph::VertexSet& children() const {return _children;} protected: HyperGraph::Vertex* _child; HyperGraph::Vertex* _parent; HyperGraph::Edge* _edge; double _distance; HyperGraph::VertexSet _children; }; typedef std::map AdjacencyMap; HyperDijkstra(HyperGraph* g); HyperGraph::VertexSet& visited() {return _visited; } AdjacencyMap& adjacencyMap() {return _adjacencyMap; } HyperGraph* graph() {return _graph;} void shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance=std::numeric_limits< double >::max(), double comparisonConditioner=1e-3, bool directed=false, double maxEdgeCost=std::numeric_limits< double >::max()); void shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost, double maxDistance=std::numeric_limits< double >::max(), double comparisonConditioner=1e-3, bool directed=false, double maxEdgeCost=std::numeric_limits< double >::max()); static void computeTree(AdjacencyMap& amap); static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false); static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, HyperGraph::VertexSet& startingSet, HyperGraph* g, HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner, double maxEdgeCost=std::numeric_limits< double >::max() ); protected: void reset(); AdjacencyMap _adjacencyMap; HyperGraph::VertexSet _visited; HyperGraph* _graph; }; struct UniformCostFunction: public HyperDijkstra::CostFunction { virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to); }; } #endif ================================================ FILE: Thirdparty/g2o/g2o/core/hyper_graph.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 "hyper_graph.h" #include #include namespace g2o { HyperGraph::Vertex::Vertex(int id) : _id(id) { } HyperGraph::Vertex::~Vertex() { } HyperGraph::Edge::Edge(int id) : _id(id) { } HyperGraph::Edge::~Edge() { } void HyperGraph::Edge::resize(size_t size) { _vertices.resize(size, 0); } void HyperGraph::Edge::setId(int id) { _id = id; } HyperGraph::Vertex* HyperGraph::vertex(int id) { VertexIDMap::iterator it=_vertices.find(id); if (it==_vertices.end()) return 0; return it->second; } const HyperGraph::Vertex* HyperGraph::vertex(int id) const { VertexIDMap::const_iterator it=_vertices.find(id); if (it==_vertices.end()) return 0; return it->second; } bool HyperGraph::addVertex(Vertex* v) { Vertex* vn=vertex(v->id()); if (vn) return false; _vertices.insert( std::make_pair(v->id(),v) ); return true; } /** * changes the id of a vertex already in the graph, and updates the bookkeeping @ returns false if the vertex is not in the graph; */ bool HyperGraph::changeId(Vertex* v, int newId){ Vertex* v2 = vertex(v->id()); if (v != v2) return false; _vertices.erase(v->id()); v->setId(newId); _vertices.insert(std::make_pair(v->id(), v)); return true; } bool HyperGraph::addEdge(Edge* e) { std::pair result = _edges.insert(e); if (! result.second) return false; for (std::vector::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { Vertex* v = *it; v->edges().insert(e); } return true; } bool HyperGraph::removeVertex(Vertex* v) { VertexIDMap::iterator it=_vertices.find(v->id()); if (it==_vertices.end()) return false; assert(it->second==v); //remove all edges which are entering or leaving v; EdgeSet tmp(v->edges()); for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){ if (!removeEdge(*it)){ assert(0); } } _vertices.erase(it); delete v; return true; } bool HyperGraph::removeEdge(Edge* e) { EdgeSet::iterator it = _edges.find(e); if (it == _edges.end()) return false; _edges.erase(it); for (std::vector::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { Vertex* v = *vit; it = v->edges().find(e); assert(it!=v->edges().end()); v->edges().erase(it); } delete e; return true; } HyperGraph::HyperGraph() { } void HyperGraph::clear() { for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) delete (it->second); for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it) delete (*it); _vertices.clear(); _edges.clear(); } HyperGraph::~HyperGraph() { clear(); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/hyper_graph.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_AIS_HYPER_GRAPH_HH #define G2O_AIS_HYPER_GRAPH_HH #include #include #include #include #include #include #include #ifdef _MSC_VER #include #else #include #endif /** @addtogroup graph */ //@{ namespace g2o { /** Class that models a directed Hyper-Graph. An hyper graph is a graph where an edge can connect one or more nodes. Both Vertices and Edges of an hyoper graph derive from the same class HyperGraphElement, thus one can implement generic algorithms that operate transparently on edges or vertices (see HyperGraphAction). The vertices are uniquely identified by an int id, while the edges are identfied by their pointers. */ class HyperGraph { public: /** * \brief enum of all the types we have in our graphs */ enum HyperGraphElementType { HGET_VERTEX, HGET_EDGE, HGET_PARAMETER, HGET_CACHE, HGET_DATA, HGET_NUM_ELEMS // keep as last elem }; typedef std::bitset GraphElemBitset; class Vertex; class Edge; /** * base hyper graph element, specialized in vertex and edge */ struct HyperGraphElement { virtual ~HyperGraphElement() {} /** * returns the type of the graph element, see HyperGraphElementType */ virtual HyperGraphElementType elementType() const = 0; }; typedef std::set EdgeSet; typedef std::set VertexSet; typedef std::tr1::unordered_map VertexIDMap; typedef std::vector VertexContainer; //! abstract Vertex, your types must derive from that one class Vertex : public HyperGraphElement { public: //! creates a vertex having an ID specified by the argument explicit Vertex(int id=-1); virtual ~Vertex(); //! returns the id int id() const {return _id;} virtual void setId( int newId) { _id=newId; } //! returns the set of hyper-edges that are leaving/entering in this vertex const EdgeSet& edges() const {return _edges;} //! returns the set of hyper-edges that are leaving/entering in this vertex EdgeSet& edges() {return _edges;} virtual HyperGraphElementType elementType() const { return HGET_VERTEX;} protected: int _id; EdgeSet _edges; }; /** * Abstract Edge class. Your nice edge classes should inherit from that one. * An hyper-edge has pointers to the vertices it connects and stores them in a vector. */ class Edge : public HyperGraphElement { public: //! creates and empty edge with no vertices explicit Edge(int id = -1); virtual ~Edge(); /** * resizes the number of vertices connected by this edge */ virtual void resize(size_t size); /** returns the vector of pointers to the vertices connected by the hyper-edge. */ const VertexContainer& vertices() const { return _vertices;} /** returns the vector of pointers to the vertices connected by the hyper-edge. */ VertexContainer& vertices() { return _vertices;} /** returns the pointer to the ith vertex connected to the hyper-edge. */ const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];} /** returns the pointer to the ith vertex connected to the hyper-edge. */ Vertex* vertex(size_t i) { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];} /** set the ith vertex on the hyper-edge to the pointer supplied */ void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;} int id() const {return _id;} void setId(int id); virtual HyperGraphElementType elementType() const { return HGET_EDGE;} protected: VertexContainer _vertices; int _id; ///< unique id }; public: //! constructs an empty hyper graph HyperGraph(); //! destroys the hyper-graph and all the vertices of the graph virtual ~HyperGraph(); //! returns a vertex id in the hyper-graph, or 0 if the vertex id is not present Vertex* vertex(int id); //! returns a vertex id in the hyper-graph, or 0 if the vertex id is not present const Vertex* vertex(int id) const; //! removes a vertex from the graph. Returns true on success (vertex was present) virtual bool removeVertex(Vertex* v); //! removes a vertex from the graph. Returns true on success (edge was present) virtual bool removeEdge(Edge* e); //! clears the graph and empties all structures. virtual void clear(); //! @returns the map id -> vertex where the vertices are stored const VertexIDMap& vertices() const {return _vertices;} //! @returns the map id -> vertex where the vertices are stored VertexIDMap& vertices() {return _vertices;} //! @returns the set of edges of the hyper graph const EdgeSet& edges() const {return _edges;} //! @returns the set of edges of the hyper graph EdgeSet& edges() {return _edges;} /** * adds a vertex to the graph. The id of the vertex should be set before * invoking this function. the function fails if another vertex * with the same id is already in the graph. * returns true, on success, or false on failure. */ virtual bool addVertex(Vertex* v); /** * Adds an edge to the graph. If the edge is already in the graph, it * does nothing and returns false. Otherwise it returns true. */ virtual bool addEdge(Edge* e); /** * changes the id of a vertex already in the graph, and updates the bookkeeping @ returns false if the vertex is not in the graph; */ virtual bool changeId(Vertex* v, int newId); protected: VertexIDMap _vertices; EdgeSet _edges; private: // Disable the copy constructor and assignment operator HyperGraph(const HyperGraph&) { } HyperGraph& operator= (const HyperGraph&) { return *this; } }; } // end namespace //@} #endif ================================================ FILE: Thirdparty/g2o/g2o/core/hyper_graph_action.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 "hyper_graph_action.h" #include "optimizable_graph.h" #include "../stuff/macros.h" #include namespace g2o { using namespace std; HyperGraphActionLibrary* HyperGraphActionLibrary::actionLibInstance = 0; HyperGraphAction::Parameters::~Parameters() { } HyperGraphAction::ParametersIteration::ParametersIteration(int iter) : HyperGraphAction::Parameters(), iteration(iter) { } HyperGraphAction::~HyperGraphAction() { } HyperGraphAction* HyperGraphAction::operator()(const HyperGraph*, Parameters*) { return 0; } HyperGraphElementAction::Parameters::~Parameters() { } HyperGraphElementAction::HyperGraphElementAction(const std::string& typeName_) { _typeName = typeName_; } void HyperGraphElementAction::setTypeName(const std::string& typeName_) { _typeName = typeName_; } HyperGraphElementAction* HyperGraphElementAction::operator()(HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* ) { return 0; } HyperGraphElementAction* HyperGraphElementAction::operator()(const HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* ) { return 0; } HyperGraphElementAction::~HyperGraphElementAction() { } HyperGraphElementActionCollection::HyperGraphElementActionCollection(const std::string& name_) { _name = name_; } HyperGraphElementActionCollection::~HyperGraphElementActionCollection() { for (ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) { delete it->second; } } HyperGraphElementAction* HyperGraphElementActionCollection::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params) { ActionMap::iterator it=_actionMap.find(typeid(*element).name()); //cerr << typeid(*element).name() << endl; if (it==_actionMap.end()) return 0; HyperGraphElementAction* action=it->second; return (*action)(element, params); } HyperGraphElementAction* HyperGraphElementActionCollection::operator()(const HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params) { ActionMap::iterator it=_actionMap.find(typeid(*element).name()); if (it==_actionMap.end()) return 0; HyperGraphElementAction* action=it->second; return (*action)(element, params); } bool HyperGraphElementActionCollection::registerAction(HyperGraphElementAction* action) { # ifdef G2O_DEBUG_ACTIONLIB cerr << __PRETTY_FUNCTION__ << " " << action->name() << " " << action->typeName() << endl; # endif if (action->name()!=name()){ cerr << __PRETTY_FUNCTION__ << ": invalid attempt to register an action in a collection with a different name " << name() << " " << action->name() << endl; } _actionMap.insert(make_pair ( action->typeName(), action) ); return true; } bool HyperGraphElementActionCollection::unregisterAction(HyperGraphElementAction* action) { for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) { if (it->second == action){ _actionMap.erase(it); return true; } } return false; } HyperGraphActionLibrary::HyperGraphActionLibrary() { } HyperGraphActionLibrary* HyperGraphActionLibrary::instance() { if (actionLibInstance == 0) { actionLibInstance = new HyperGraphActionLibrary; } return actionLibInstance; } void HyperGraphActionLibrary::destroy() { delete actionLibInstance; actionLibInstance = 0; } HyperGraphActionLibrary::~HyperGraphActionLibrary() { for (HyperGraphElementAction::ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) { delete it->second; } } HyperGraphElementAction* HyperGraphActionLibrary::actionByName(const std::string& name) { HyperGraphElementAction::ActionMap::iterator it=_actionMap.find(name); if (it!=_actionMap.end()) return it->second; return 0; } bool HyperGraphActionLibrary::registerAction(HyperGraphElementAction* action) { HyperGraphElementAction* oldAction = actionByName(action->name()); HyperGraphElementActionCollection* collection = 0; if (oldAction) { collection = dynamic_cast(oldAction); if (! collection) { cerr << __PRETTY_FUNCTION__ << ": fatal error, a collection is not at the first level in the library" << endl; return 0; } } if (! collection) { #ifdef G2O_DEBUG_ACTIONLIB cerr << __PRETTY_FUNCTION__ << ": creating collection for \"" << action->name() << "\"" << endl; #endif collection = new HyperGraphElementActionCollection(action->name()); _actionMap.insert(make_pair(action->name(), collection)); } return collection->registerAction(action); } bool HyperGraphActionLibrary::unregisterAction(HyperGraphElementAction* action) { list collectionDeleteList; // Search all the collections and delete the registered actions; if a collection becomes empty, schedule it for deletion; note that we can't delete the collections as we go because this will screw up the state of the iterators for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) { HyperGraphElementActionCollection* collection = dynamic_cast (it->second); if (collection != 0) { collection->unregisterAction(action); if (collection->actionMap().size() == 0) { collectionDeleteList.push_back(collection); } } } // Delete any empty action collections for (list::iterator itc = collectionDeleteList.begin(); itc != collectionDeleteList.end(); ++itc) { //cout << "Deleting collection " << (*itc)->name() << endl; _actionMap.erase((*itc)->name()); } return true; } WriteGnuplotAction::WriteGnuplotAction(const std::string& typeName_) : HyperGraphElementAction(typeName_) { _name="writeGnuplot"; } DrawAction::Parameters::Parameters(){ } DrawAction::DrawAction(const std::string& typeName_) : HyperGraphElementAction(typeName_) { _name="draw"; _previousParams = (Parameters*)0x42; refreshPropertyPtrs(0); } bool DrawAction::refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_){ if (_previousParams == params_) return false; DrawAction::Parameters* p=dynamic_cast(params_); if (! p){ _previousParams = 0; _show = 0; _showId = 0; } else { _previousParams = p; _show = p->makeProperty(_typeName+"::SHOW", true); _showId = p->makeProperty(_typeName+"::SHOW_ID", false); } return true; } void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* params, const std::string& typeName) { for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin(); it!=graph->vertices().end(); ++it){ if ( typeName.empty() || typeid(*it->second).name()==typeName){ (*action)(it->second, params); } } for (HyperGraph::EdgeSet::iterator it=graph->edges().begin(); it!=graph->edges().end(); ++it){ if ( typeName.empty() || typeid(**it).name()==typeName) (*action)(*it, params); } } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/hyper_graph_action.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_HYPER_GRAPH_ACTION_H #define G2O_HYPER_GRAPH_ACTION_H #include "hyper_graph.h" #include "../stuff/property.h" #include #include #include #include #include // define to get verbose output //#define G2O_DEBUG_ACTIONLIB namespace g2o { /** * \brief Abstract action that operates on an entire graph */ class HyperGraphAction { public: class Parameters { public: virtual ~Parameters(); }; class ParametersIteration : public Parameters { public: explicit ParametersIteration(int iter); int iteration; }; virtual ~HyperGraphAction(); /** * re-implement to carry out an action given the graph */ virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0); }; /** * \brief Abstract action that operates on a graph entity */ class HyperGraphElementAction{ public: struct Parameters{ virtual ~Parameters(); }; typedef std::map ActionMap; //! an action should be instantiated with the typeid.name of the graph element //! on which it operates HyperGraphElementAction(const std::string& typeName_=""); //! redefine this to do the action stuff. If successful, the action returns a pointer to itself virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters); //! redefine this to do the action stuff. If successful, the action returns a pointer to itself virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters); //! destroyed actions release the memory virtual ~HyperGraphElementAction(); //! returns the typeid name of the action const std::string& typeName() const { return _typeName;} //! returns the name of an action, e.g "draw" const std::string& name() const{ return _name;} //! sets the type on which an action has to operate void setTypeName(const std::string& typeName_); protected: std::string _typeName; std::string _name; }; /** * \brief collection of actions * * collection of actions calls contains homogeneous actions operating on different types * all collected actions have the same name and should have the same functionality */ class HyperGraphElementActionCollection: public HyperGraphElementAction{ public: //! constructor. name_ is the name of the action e.g.draw). HyperGraphElementActionCollection(const std::string& name_); //! destructor: it deletes all actions in the pool. virtual ~HyperGraphElementActionCollection(); //! calling functions, they return a pointer to the instance of action in actionMap //! that was active on element virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters); virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters); ActionMap& actionMap() {return _actionMap;} //! inserts an action in the pool. The action should have the same name of the container. //! returns false on failure (the container has a different name than the action); bool registerAction(HyperGraphElementAction* action); bool unregisterAction(HyperGraphElementAction* action); protected: ActionMap _actionMap; }; /** * \brief library of actions, indexed by the action name; * * library of actions, indexed by the action name; * one can use ti to register a collection of actions */ class HyperGraphActionLibrary{ public: //! return the single instance of the HyperGraphActionLibrary static HyperGraphActionLibrary* instance(); //! free the instance static void destroy(); // returns a pointer to a collection indexed by name HyperGraphElementAction* actionByName(const std::string& name); // registers a basic action in the pool. If necessary a container is created bool registerAction(HyperGraphElementAction* action); bool unregisterAction(HyperGraphElementAction* action); inline HyperGraphElementAction::ActionMap& actionMap() {return _actionMap;} protected: HyperGraphActionLibrary(); ~HyperGraphActionLibrary(); HyperGraphElementAction::ActionMap _actionMap; private: static HyperGraphActionLibrary* actionLibInstance; }; /** * apply an action to all the elements of the graph. */ void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* parameters=0, const std::string& typeName=""); /** * brief write into gnuplot */ class WriteGnuplotAction: public HyperGraphElementAction{ public: struct Parameters: public HyperGraphElementAction::Parameters{ std::ostream* os; }; WriteGnuplotAction(const std::string& typeName_); }; /** * \brief draw actions */ class DrawAction : public HyperGraphElementAction{ public: class Parameters: public HyperGraphElementAction::Parameters, public PropertyMap{ public: Parameters(); }; DrawAction(const std::string& typeName_); protected: virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_); Parameters* _previousParams; BoolProperty* _show; BoolProperty* _showId; }; template class RegisterActionProxy { public: RegisterActionProxy() { #ifdef G2O_DEBUG_ACTIONLIB std::cout << __FUNCTION__ << ": Registering action of type " << typeid(T).name() << std::endl; #endif _action = new T(); HyperGraphActionLibrary::instance()->registerAction(_action); } ~RegisterActionProxy() { #ifdef G2O_DEBUG_ACTIONLIB std::cout << __FUNCTION__ << ": Unregistering action of type " << typeid(T).name() << std::endl; #endif HyperGraphActionLibrary::instance()->unregisterAction(_action); delete _action; } private: HyperGraphElementAction* _action; }; #define G2O_REGISTER_ACTION(classname) \ extern "C" void g2o_action_##classname(void) {} \ static g2o::RegisterActionProxy g_action_proxy_##classname; }; #endif ================================================ FILE: Thirdparty/g2o/g2o/core/jacobian_workspace.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 "jacobian_workspace.h" #include #include "optimizable_graph.h" using namespace std; namespace g2o { JacobianWorkspace::JacobianWorkspace() : _maxNumVertices(-1), _maxDimension(-1) { } JacobianWorkspace::~JacobianWorkspace() { } bool JacobianWorkspace::allocate() { //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; if (_maxNumVertices <=0 || _maxDimension <= 0) return false; _workspace.resize(_maxNumVertices); for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) { it->resize(_maxDimension); it->setZero(); } return true; } void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_) { const OptimizableGraph::Edge* e = static_cast(e_); int errorDimension = e->dimension(); int numVertices = e->vertices().size(); int maxDimensionForEdge = -1; for (int i = 0; i < numVertices; ++i) { const OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); assert(v && "Edge has no vertex assigned"); maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge); } _maxNumVertices = max(numVertices, _maxNumVertices); _maxDimension = max(maxDimensionForEdge, _maxDimension); //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl; } void JacobianWorkspace::updateSize(const OptimizableGraph& graph) { for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { const OptimizableGraph::Edge* e = static_cast(*it); updateSize(e); } } void JacobianWorkspace::updateSize(int numVertices, int dimension) { _maxNumVertices = max(numVertices, _maxNumVertices); _maxDimension = max(dimension, _maxDimension); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/jacobian_workspace.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 JACOBIAN_WORKSPACE_H #define JACOBIAN_WORKSPACE_H #include #include #include #include #include "hyper_graph.h" namespace g2o { struct OptimizableGraph; /** * \brief provide memory workspace for computing the Jacobians * * The workspace is used by an OptimizableGraph to provide temporary memory * for computing the Jacobian of the error functions. * Before calling linearizeOplus on an edge, the workspace needs to be allocated * by calling allocate(). */ class JacobianWorkspace { public: typedef std::vector > WorkspaceVector; public: JacobianWorkspace(); ~JacobianWorkspace(); /** * allocate the workspace */ bool allocate(); /** * update the maximum required workspace needed by taking into account this edge */ void updateSize(const HyperGraph::Edge* e); /** * update the required workspace by looking at a full graph */ void updateSize(const OptimizableGraph& graph); /** * manually update with the given parameters */ void updateSize(int numVertices, int dimension); /** * return the workspace for a vertex in an edge */ double* workspaceForVertex(int vertexIndex) { assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds"); return _workspace[vertexIndex].data(); } protected: WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/linear_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_LINEAR_SOLVER_H #define G2O_LINEAR_SOLVER_H #include "sparse_block_matrix.h" #include "sparse_block_matrix_ccs.h" namespace g2o { /** * \brief basic solver for Ax = b * * basic solver for Ax = b which has to reimplemented for different linear algebra libraries. * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit. */ template class LinearSolver { public: LinearSolver() {}; virtual ~LinearSolver() {} /** * init for operating on matrices with a different non-zero pattern like before */ virtual bool init() = 0; /** * Assumes that A is the same matrix for several calls. * Among other assumptions, the non-zero pattern does not change! * If the matrix changes call init() before. * solve system Ax = b, x and b have to allocated beforehand!! */ virtual bool solve(const SparseBlockMatrix& A, double* x, double* b) = 0; /** * Inverts the diagonal blocks of A * @returns false if not defined. */ virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix& A) { (void)blocks; (void) A; return false; } /** * Inverts the a block pattern of A in spinv * @returns false if not defined. */ virtual bool solvePattern(SparseBlockMatrix& spinv, const std::vector >& blockIndices, const SparseBlockMatrix& A){ (void) spinv; (void) blockIndices; (void) A; return false; } //! write a debug dump of the system matrix if it is not PSD in solve virtual bool writeDebug() const { return false;} virtual void setWriteDebug(bool) {} }; /** * \brief Solver with faster iterating structure for the linear matrix */ template class LinearSolverCCS : public LinearSolver { public: LinearSolverCCS() : LinearSolver(), _ccsMatrix(0) {} ~LinearSolverCCS() { delete _ccsMatrix; } protected: SparseBlockMatrixCCS* _ccsMatrix; void initMatrixStructure(const SparseBlockMatrix& A) { delete _ccsMatrix; _ccsMatrix = new SparseBlockMatrixCCS(A.rowBlockIndices(), A.colBlockIndices()); A.fillSparseBlockMatrixCCS(*_ccsMatrix); } }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.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 "marginal_covariance_cholesky.h" #include #include using namespace std; namespace g2o { struct MatrixElem { int r, c; MatrixElem(int r_, int c_) : r(r_), c(c_) {} bool operator<(const MatrixElem& other) const { return c > other.c || (c == other.c && r > other.r); } }; MarginalCovarianceCholesky::MarginalCovarianceCholesky() : _n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0) { } MarginalCovarianceCholesky::~MarginalCovarianceCholesky() { } void MarginalCovarianceCholesky::setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv) { _n = n; _Ap = Lp; _Ai = Li; _Ax = Lx; _perm = permInv; // pre-compute reciprocal values of the diagonal of L _diag.resize(n); for (int r = 0; r < n; ++r) { const int& sc = _Ap[r]; // L is lower triangular, thus the first elem in the column is the diagonal entry assert(r == _Ai[sc] && "Error in CCS storage of L"); _diag[r] = 1.0 / _Ax[sc]; } } double MarginalCovarianceCholesky::computeEntry(int r, int c) { assert(r <= c); int idx = computeIndex(r, c); LookupMap::const_iterator foundIt = _map.find(idx); if (foundIt != _map.end()) { return foundIt->second; } // compute the summation over column r double s = 0.; const int& sc = _Ap[r]; const int& ec = _Ap[r+1]; for (int j = sc+1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal const int& rr = _Ai[j]; double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr); s += val * _Ax[j]; } double result; if (r == c) { const double& diagElem = _diag[r]; result = diagElem * (diagElem - s); } else { result = -s * _diag[r]; } _map[idx] = result; return result; } void MarginalCovarianceCholesky::computeCovariance(double** covBlocks, const std::vector& blockIndices) { _map.clear(); int base = 0; vector elemsToCompute; for (size_t i = 0; i < blockIndices.size(); ++i) { int nbase = blockIndices[i]; int vdim = nbase - base; for (int rr = 0; rr < vdim; ++rr) for (int cc = rr; cc < vdim; ++cc) { int r = _perm ? _perm[rr + base] : rr + base; // apply permutation int c = _perm ? _perm[cc + base] : cc + base; if (r > c) // make sure it's still upper triangular after applying the permutation swap(r, c); elemsToCompute.push_back(MatrixElem(r, c)); } base = nbase; } // sort the elems to reduce the recursive calls sort(elemsToCompute.begin(), elemsToCompute.end()); // compute the inverse elements we need for (size_t i = 0; i < elemsToCompute.size(); ++i) { const MatrixElem& me = elemsToCompute[i]; computeEntry(me.r, me.c); } // set the marginal covariance for the vertices, by writing to the blocks memory base = 0; for (size_t i = 0; i < blockIndices.size(); ++i) { int nbase = blockIndices[i]; int vdim = nbase - base; double* cov = covBlocks[i]; for (int rr = 0; rr < vdim; ++rr) for (int cc = rr; cc < vdim; ++cc) { int r = _perm ? _perm[rr + base] : rr + base; // apply permutation int c = _perm ? _perm[cc + base] : cc + base; if (r > c) // upper triangle swap(r, c); int idx = computeIndex(r, c); LookupMap::const_iterator foundIt = _map.find(idx); assert(foundIt != _map.end()); cov[rr*vdim + cc] = foundIt->second; if (rr != cc) cov[cc*vdim + rr] = foundIt->second; } base = nbase; } } void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices) { // allocate the sparse spinv = SparseBlockMatrix(&rowBlockIndices[0], &rowBlockIndices[0], rowBlockIndices.size(), rowBlockIndices.size(), true); _map.clear(); vector elemsToCompute; for (size_t i = 0; i < blockIndices.size(); ++i) { int blockRow=blockIndices[i].first; int blockCol=blockIndices[i].second; assert(blockRow>=0); assert(blockRow < (int)rowBlockIndices.size()); assert(blockCol>=0); assert(blockCol < (int)rowBlockIndices.size()); int rowBase=spinv.rowBaseOfBlock(blockRow); int colBase=spinv.colBaseOfBlock(blockCol); MatrixXd *block=spinv.block(blockRow, blockCol, true); assert(block); for (int iRow=0; iRowrows(); ++iRow) for (int iCol=0; iColcols(); ++iCol){ int rr=rowBase+iRow; int cc=colBase+iCol; int r = _perm ? _perm[rr] : rr; // apply permutation int c = _perm ? _perm[cc] : cc; if (r > c) swap(r, c); elemsToCompute.push_back(MatrixElem(r, c)); } } // sort the elems to reduce the number of recursive calls sort(elemsToCompute.begin(), elemsToCompute.end()); // compute the inverse elements we need for (size_t i = 0; i < elemsToCompute.size(); ++i) { const MatrixElem& me = elemsToCompute[i]; computeEntry(me.r, me.c); } // set the marginal covariance for (size_t i = 0; i < blockIndices.size(); ++i) { int blockRow=blockIndices[i].first; int blockCol=blockIndices[i].second; int rowBase=spinv.rowBaseOfBlock(blockRow); int colBase=spinv.colBaseOfBlock(blockCol); MatrixXd *block=spinv.block(blockRow, blockCol); assert(block); for (int iRow=0; iRowrows(); ++iRow) for (int iCol=0; iColcols(); ++iCol){ int rr=rowBase+iRow; int cc=colBase+iCol; int r = _perm ? _perm[rr] : rr; // apply permutation int c = _perm ? _perm[cc] : cc; if (r > c) swap(r, c); int idx = computeIndex(r, c); LookupMap::const_iterator foundIt = _map.find(idx); assert(foundIt != _map.end()); (*block)(iRow, iCol) = foundIt->second; } } } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.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_MARGINAL_COVARIANCE_CHOLESKY_H #define G2O_MARGINAL_COVARIANCE_CHOLESKY_H #include "optimizable_graph.h" #include "sparse_block_matrix.h" #include #include #ifdef _MSC_VER #include #else #include #endif namespace g2o { /** * \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor) */ class MarginalCovarianceCholesky { protected: /** * hash struct for storing the matrix elements needed to compute the covariance */ typedef std::tr1::unordered_map LookupMap; public: MarginalCovarianceCholesky(); ~MarginalCovarianceCholesky(); /** * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to * be provided by the caller). */ void computeCovariance(double** covBlocks, const std::vector& blockIndices); /** * compute the marginal cov for the given block indices, write the result in spinv). */ void computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices); /** * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in. * permInv might be 0, will then not permute the entries. * * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers * are owned by the caller, MarginalCovarianceCholesky does not free the pointers. */ void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv); protected: // information about the cholesky factor (lower triangle) int _n; ///< L is an n X n matrix int* _Ap; ///< column pointer of the CCS storage int* _Ai; ///< row indices of the CCS storage double* _Ax; ///< values of the cholesky factor int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in LookupMap _map; ///< hash look up table for the already computed entries std::vector _diag; ///< cache 1 / H_ii to avoid recalculations //! compute the index used for hashing int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;} /** * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular. * May issue recursive calls to itself to compute the missing values. */ double computeEntry(int r, int c); }; } #endif ================================================ FILE: Thirdparty/g2o/g2o/core/matrix_operations.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_CORE_MATRIX_OPERATIONS_H #define G2O_CORE_MATRIX_OPERATIONS_H #include namespace g2o { namespace internal { template inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff) += A * x.segment(xoff); } template inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); } template<> inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); } template inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff) += A.transpose() * x.segment(xoff); } template inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); } template<> inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); } } // end namespace internal } // end namespace g2o #endif ================================================ FILE: Thirdparty/g2o/g2o/core/matrix_structure.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 "matrix_structure.h" #include #include #include #include using namespace std; namespace g2o { struct ColSort { bool operator()(const pair& e1, const pair& e2) const { return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first); } }; MatrixStructure::MatrixStructure() : n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0) { } MatrixStructure::~MatrixStructure() { free(); } void MatrixStructure::alloc(int n_, int nz) { if (n == 0) { maxN = n = n_; maxNz = nz; Ap = new int[maxN + 1]; Aii = new int[maxNz]; } else { n = n_; if (maxNz < nz) { maxNz = 2 * nz; delete[] Aii; Aii = new int[maxNz]; } if (maxN < n) { maxN = 2 * n; delete[] Ap; Ap = new int[maxN + 1]; } } } void MatrixStructure::free() { n = 0; m = 0; maxN = 0; maxNz = 0; delete[] Aii; Aii = 0; delete[] Ap; Ap = 0; } bool MatrixStructure::write(const char* filename) const { const int& cols = n; const int& rows = m; string name = filename; std::string::size_type lastDot = name.find_last_of('.'); if (lastDot != std::string::npos) name = name.substr(0, lastDot); vector > entries; for (int i=0; i < cols; ++i) { const int& rbeg = Ap[i]; const int& rend = Ap[i+1]; for (int j = rbeg; j < rend; ++j) { entries.push_back(make_pair(Aii[j], i)); if (Aii[j] != i) entries.push_back(make_pair(i, Aii[j])); } } sort(entries.begin(), entries.end(), ColSort()); std::ofstream fout(filename); fout << "# name: " << name << std::endl; fout << "# type: sparse matrix" << std::endl; fout << "# nnz: " << entries.size() << std::endl; fout << "# rows: " << rows << std::endl; fout << "# columns: " << cols << std::endl; for (vector >::const_iterator it = entries.begin(); it != entries.end(); ++it) { const pair& entry = *it; fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0 } return fout.good(); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/matrix_structure.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_MATRIX_STRUCTURE_H #define G2O_MATRIX_STRUCTURE_H namespace g2o { /** * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix) */ class MatrixStructure { public: MatrixStructure(); ~MatrixStructure(); /** * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will * then reallocate the memory + additional space (double the required space). */ void alloc(int n_, int nz); void free(); /** * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix) */ bool write(const char* filename) const; int n; ///< A is m-by-n. n must be >= 0. int m; ///< A is m-by-n. m must be >= 0. int* Ap; ///< column pointers for A, of size n+1 int* Aii; ///< row indices of A, of size nz = Ap [n] //! max number of non-zeros blocks int nzMax() const { return maxNz;} protected: int maxN; ///< size of the allocated memory int maxNz; ///< size of the allocated memory }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/openmp_mutex.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_OPENMP_MUTEX #define G2O_OPENMP_MUTEX #include "../../config.h" #ifdef G2O_OPENMP #include #else #include #endif namespace g2o { #ifdef G2O_OPENMP /** * \brief Mutex realized via OpenMP */ class OpenMPMutex { public: OpenMPMutex() { omp_init_lock(&_lock); } ~OpenMPMutex() { omp_destroy_lock(&_lock); } void lock() { omp_set_lock(&_lock); } void unlock() { omp_unset_lock(&_lock); } protected: omp_lock_t _lock; }; #else /* * dummy which does nothing in case we don't have OpenMP support. * In debug mode, the mutex allows to verify the correct lock and unlock behavior */ class OpenMPMutex { public: #ifdef NDEBUG OpenMPMutex() {} #else OpenMPMutex() : _cnt(0) {} #endif ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");} void lock() { assert(++_cnt == 1 && "Locking already locked mutex");} void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");} protected: #ifndef NDEBUG char _cnt; #endif }; #endif /** * \brief lock a mutex within a scope */ class ScopedOpenMPMutex { public: explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); } ~ScopedOpenMPMutex() { _mutex->unlock(); } private: OpenMPMutex* const _mutex; ScopedOpenMPMutex(const ScopedOpenMPMutex&); void operator=(const ScopedOpenMPMutex&); }; } #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimizable_graph.cpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "optimizable_graph.h" #include #include #include #include #include #include #include "estimate_propagator.h" #include "factory.h" #include "optimization_algorithm_property.h" #include "hyper_graph_action.h" #include "cache.h" #include "robust_kernel.h" #include "../stuff/macros.h" #include "../stuff/color_macros.h" #include "../stuff/string_tools.h" #include "../stuff/misc.h" namespace g2o { using namespace std; OptimizableGraph::Data::Data(){ _next = 0; } OptimizableGraph::Data::~Data(){ if (_next) delete _next; } OptimizableGraph::Vertex::Vertex() : HyperGraph::Vertex(), _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false), _colInHessian(-1), _cacheContainer(0) { } CacheContainer* OptimizableGraph::Vertex::cacheContainer(){ if (! _cacheContainer) _cacheContainer = new CacheContainer(this); return _cacheContainer; } void OptimizableGraph::Vertex::updateCache(){ if (_cacheContainer){ _cacheContainer->setUpdateNeeded(); _cacheContainer->update(); } } OptimizableGraph::Vertex::~Vertex() { if (_cacheContainer) delete (_cacheContainer); if (_userData) delete _userData; } OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const { return 0; } bool OptimizableGraph::Vertex::setEstimateData(const double* v) { bool ret = setEstimateDataImpl(v); updateCache(); return ret; } bool OptimizableGraph::Vertex::getEstimateData(double *) const { return false; } int OptimizableGraph::Vertex::estimateDimension() const { return -1; } bool OptimizableGraph::Vertex::setMinimalEstimateData(const double* v) { bool ret = setMinimalEstimateDataImpl(v); updateCache(); return ret; } bool OptimizableGraph::Vertex::getMinimalEstimateData(double *) const { return false; } int OptimizableGraph::Vertex::minimalEstimateDimension() const { return -1; } OptimizableGraph::Edge::Edge() : HyperGraph::Edge(), _dimension(-1), _level(0), _robustKernel(0) { } OptimizableGraph::Edge::~Edge() { delete _robustKernel; } OptimizableGraph* OptimizableGraph::Edge::graph(){ if (! _vertices.size()) return 0; OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)_vertices[0]; if (!v) return 0; return v->graph(); } const OptimizableGraph* OptimizableGraph::Edge::graph() const{ if (! _vertices.size()) return 0; const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) _vertices[0]; if (!v) return 0; return v->graph(); } bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){ if ((int)_parameters.size()<=argNum) return false; if (argNum<0) return false; *_parameters[argNum] = 0; _parameterIds[argNum] = paramId; return true; } bool OptimizableGraph::Edge::resolveParameters() { if (!graph()) { cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl; return false; } assert (_parameters.size() == _parameterIds.size()); //cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl; for (size_t i=0; i<_parameters.size(); i++){ int index = _parameterIds[i]; *_parameters[i] = graph()->parameter(index); if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){ cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << endl; } if (!*_parameters[i]) { cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << endl; return false; } } return true; } void OptimizableGraph::Edge::setRobustKernel(RobustKernel* ptr) { if (_robustKernel) delete _robustKernel; _robustKernel = ptr; } bool OptimizableGraph::Edge::resolveCaches() { return true; } bool OptimizableGraph::Edge::setMeasurementData(const double *) { return false; } bool OptimizableGraph::Edge::getMeasurementData(double *) const { return false; } int OptimizableGraph::Edge::measurementDimension() const { return -1; } bool OptimizableGraph::Edge::setMeasurementFromState(){ return false; } OptimizableGraph::Edge* OptimizableGraph::Edge::clone() const { // TODO return 0; } OptimizableGraph::OptimizableGraph() { _nextEdgeId = 0; _edge_has_id = false; _graphActions.resize(AT_NUM_ELEMENTS); } OptimizableGraph::~OptimizableGraph() { clear(); clearParameters(); } bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData) { Vertex* inserted = vertex(v->id()); if (inserted) { cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl; assert(0 && "Vertex with this ID already contained in the graph"); return false; } OptimizableGraph::Vertex* ov=dynamic_cast(v); assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex"); if (ov->_graph != 0 && ov->_graph != this) { cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl; assert(0 && "Vertex already registered with another graph"); return false; } if (userData) ov->setUserData(userData); ov->_graph=this; return HyperGraph::addVertex(v); } bool OptimizableGraph::addEdge(HyperGraph::Edge* e_) { OptimizableGraph::Edge* e = dynamic_cast(e_); assert(e && "Edge does not inherit from OptimizableGraph::Edge"); if (! e) return false; bool eresult = HyperGraph::addEdge(e); if (! eresult) return false; e->_internalId = _nextEdgeId++; if (! e->resolveParameters()){ cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl; return false; } if (! e->resolveCaches()){ cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl; return false; } _jacobianWorkspace.updateSize(e); return true; } int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;} double OptimizableGraph::chi2() const { double chi = 0.0; for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); ++it) { const OptimizableGraph::Edge* e = static_cast(*it); chi += e->chi2(); } return chi; } void OptimizableGraph::push() { for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) { OptimizableGraph::Vertex* v = static_cast(it->second); v->push(); } } void OptimizableGraph::pop() { for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) { OptimizableGraph::Vertex* v= static_cast(it->second); v->pop(); } } void OptimizableGraph::discardTop() { for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) { OptimizableGraph::Vertex* v= static_cast(it->second); v->discardTop(); } } void OptimizableGraph::push(HyperGraph::VertexSet& vset) { for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { OptimizableGraph::Vertex* v = static_cast(*it); v->push(); } } void OptimizableGraph::pop(HyperGraph::VertexSet& vset) { for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { OptimizableGraph::Vertex* v = static_cast(*it); v->pop(); } } void OptimizableGraph::discardTop(HyperGraph::VertexSet& vset) { for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { OptimizableGraph::Vertex* v = static_cast(*it); v->discardTop(); } } void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed) { for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) { OptimizableGraph::Vertex* v = static_cast(*it); v->setFixed(fixed); } } bool OptimizableGraph::load(istream& is, bool createEdges) { // scna for the paramers in the whole file if (!_parameters.read(is,&_renamedTypesLookup)) return false; #ifndef NDEBUG cerr << "Loaded " << _parameters.size() << " parameters" << endl; #endif is.clear(); is.seekg(ios_base::beg); set warnedUnknownTypes; stringstream currentLine; string token; Factory* factory = Factory::instance(); HyperGraph::GraphElemBitset elemBitset; elemBitset[HyperGraph::HGET_PARAMETER] = 1; elemBitset.flip(); Vertex* previousVertex = 0; Data* previousData = 0; while (1) { int bytesRead = readLine(is, currentLine); if (bytesRead == -1) break; currentLine >> token; //cerr << "Token=" << token << endl; if (bytesRead == 0 || token.size() == 0 || token[0] == '#') continue; // handle commands encoded in the file bool handledCommand = false; if (token == "FIX") { handledCommand = true; int id; while (currentLine >> id) { OptimizableGraph::Vertex* v = static_cast(vertex(id)); if (v) { # ifndef NDEBUG cerr << "Fixing vertex " << v->id() << endl; # endif v->setFixed(true); } else { cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl; } } } if (handledCommand) continue; // do the mapping to an internal type if it matches if (_renamedTypesLookup.size() > 0) { map::const_iterator foundIt = _renamedTypesLookup.find(token); if (foundIt != _renamedTypesLookup.end()) { token = foundIt->second; } } if (! factory->knowsTag(token)) { if (warnedUnknownTypes.count(token) != 1) { warnedUnknownTypes.insert(token); cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl; } continue; } HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset); if (dynamic_cast(element)) { // it's a vertex type //cerr << "it is a vertex" << endl; previousData = 0; Vertex* v = static_cast(element); int id; currentLine >> id; bool r = v->read(currentLine); if (! r) cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl; v->setId(id); if (!addVertex(v)) { cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl; delete v; } else { previousVertex = v; } } else if (dynamic_cast(element)) { //cerr << "it is an edge" << endl; previousData = 0; Edge* e = static_cast(element); int numV = e->vertices().size(); if (_edge_has_id){ int id; currentLine >> id; e->setId(id); } //cerr << PVAR(token) << " " << PVAR(numV) << endl; if (numV == 2) { // it's a pairwise / binary edge type which we handle in a special way int id1, id2; currentLine >> id1 >> id2; Vertex* from = vertex(id1); Vertex* to = vertex(id2); int doInit=0; if ((!from || !to) ) { if (! createEdges) { cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token << " " << id1 << " <-> " << id2 << endl; delete e; } else { if (! from) { from=e->createFrom(); from->setId(id1); addVertex(from); doInit=2; } if (! to) { to=e->createTo(); to->setId(id2); addVertex(to); doInit=1; } } } if (from && to) { e->setVertex(0, from); e->setVertex(1, to); e->read(currentLine); if (!addEdge(e)) { cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token << " " << id1 << " <-> " << id2 << endl; delete e; } else { switch (doInit){ case 1: { HyperGraph::VertexSet fromSet; fromSet.insert(from); e->initialEstimate(fromSet, to); break; } case 2: { HyperGraph::VertexSet toSet; toSet.insert(to); e->initialEstimate(toSet, from); break; } default:; } } } } else { vector ids; ids.resize(numV); for (int l = 0; l < numV; ++l) currentLine >> ids[l]; bool vertsOkay = true; for (int l = 0; l < numV; ++l) { e->setVertex(l, vertex(ids[l])); if (e->vertex(l) == 0) { vertsOkay = false; break; } } if (! vertsOkay) { cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token; for (int l = 0; l < numV; ++l) { if (l > 0) cerr << " <->"; cerr << " " << ids[l]; } delete e; } else { bool r = e->read(currentLine); if (!r || !addEdge(e)) { cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token; for (int l = 0; l < numV; ++l) { if (l > 0) cerr << " <->"; cerr << " " << ids[l]; } delete e; } } } } else if (dynamic_cast(element)) { // reading in the data packet for the vertex //cerr << "read data packet " << token << " vertex " << previousVertex->id() << endl; Data* d = static_cast(element); bool r = d->read(currentLine); if (! r) { cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for vertex " << previousVertex->id() << endl; delete d; previousData = 0; } else if (previousData){ //cerr << "chaining" << endl; previousData->setNext(d); previousData = d; //cerr << "done" << endl; } else if (previousVertex){ //cerr << "embedding in vertex" << endl; previousVertex->setUserData(d); previousData = d; previousVertex = 0; //cerr << "done" << endl; } else { cerr << __PRETTY_FUNCTION__ << ": got data element, but no vertex available" << endl; delete d; previousData = 0; } } } // while read line return true; } bool OptimizableGraph::load(const char* filename, bool createEdges) { ifstream ifs(filename); if (!ifs) { cerr << __PRETTY_FUNCTION__ << " unable to open file " << filename << endl; return false; } return load(ifs, createEdges); } bool OptimizableGraph::save(const char* filename, int level) const { ofstream ofs(filename); if (!ofs) return false; return save(ofs, level); } bool OptimizableGraph::save(ostream& os, int level) const { if (! _parameters.write(os)) return false; set verticesToSave; for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { OptimizableGraph::Edge* e = static_cast(*it); if (e->level() == level) { for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { verticesToSave.insert(static_cast(*it)); } } } for (set::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){ OptimizableGraph::Vertex* v = *it; saveVertex(os, v); } EdgeContainer edgesToSave; for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { const OptimizableGraph::Edge* e = dynamic_cast(*it); if (e->level() == level) edgesToSave.push_back(const_cast(e)); } sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare()); for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) { OptimizableGraph::Edge* e = *it; saveEdge(os, e); } return os.good(); } bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::VertexSet& vset, int level) { if (! _parameters.write(os)) return false; for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){ OptimizableGraph::Vertex* v = dynamic_cast(*it); saveVertex(os, v); } for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it); if (e->level() != level) continue; bool verticesInEdge = true; for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { if (vset.find(*it) == vset.end()) { verticesInEdge = false; break; } } if (! verticesInEdge) continue; saveEdge(os, e); } return os.good(); } bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset) { if (!_parameters.write(os)) return false; std::set vset; for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) { HyperGraph::Edge* e = *it; for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { OptimizableGraph::Vertex* v = static_cast(*it); vset.insert(v); } } for (std::set::const_iterator it=vset.begin(); it!=vset.end(); ++it){ OptimizableGraph::Vertex* v = dynamic_cast(*it); saveVertex(os, v); } for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) { OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it); saveEdge(os, e); } return os.good(); } void OptimizableGraph::addGraph(OptimizableGraph* g){ for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){ OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second); if (vertex(v->id())) continue; OptimizableGraph::Vertex* v2=v->clone(); v2->edges().clear(); v2->setHessianIndex(-1); addVertex(v2); } for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){ OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it); OptimizableGraph::Edge* en = e->clone(); en->resize(e->vertices().size()); int cnt = 0; for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) vertex((*it)->id()); assert(v); en->setVertex(cnt++, v); } addEdge(en); } } int OptimizableGraph::maxDimension() const{ int maxDim=0; for (HyperGraph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); ++it){ const OptimizableGraph::Vertex* v= static_cast< const OptimizableGraph::Vertex*>(it->second); maxDim = (std::max)(maxDim, v->dimension()); } return maxDim; } void OptimizableGraph::setRenamedTypesFromString(const std::string& types) { Factory* factory = Factory::instance(); vector typesMap = strSplit(types, ","); for (size_t i = 0; i < typesMap.size(); ++i) { vector m = strSplit(typesMap[i], "="); if (m.size() != 2) { cerr << __PRETTY_FUNCTION__ << ": unable to extract type map from " << typesMap[i] << endl; continue; } string typeInFile = trim(m[0]); string loadedType = trim(m[1]); if (! factory->knowsTag(loadedType)) { cerr << __PRETTY_FUNCTION__ << ": unknown type " << loadedType << endl; continue; } _renamedTypesLookup[typeInFile] = loadedType; } cerr << "# load look up table" << endl; for (std::map::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) { cerr << "#\t" << it->first << " -> " << it->second << endl; } } bool OptimizableGraph::isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set& vertDims_) const { std::set auxDims; if (vertDims_.size() == 0) { auxDims = dimensions(); } const set& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_; bool suitableSolver = true; if (vertDims.size() == 2) { if (solverProperty.requiresMarginalize) { suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1; } else { suitableSolver = solverProperty.poseDim == -1; } } else if (vertDims.size() == 1) { suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1; } else { suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize; } return suitableSolver; } std::set OptimizableGraph::dimensions() const { std::set auxDims; for (VertexIDMap::const_iterator it = vertices().begin(); it != vertices().end(); ++it) { OptimizableGraph::Vertex* v = static_cast(it->second); auxDims.insert(v->dimension()); } return auxDims; } void OptimizableGraph::preIteration(int iter) { HyperGraphActionSet& actions = _graphActions[AT_PREITERATION]; if (actions.size() > 0) { HyperGraphAction::ParametersIteration params(iter); for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) { (*(*it))(this, ¶ms); } } } void OptimizableGraph::postIteration(int iter) { HyperGraphActionSet& actions = _graphActions[AT_POSTITERATION]; if (actions.size() > 0) { HyperGraphAction::ParametersIteration params(iter); for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) { (*(*it))(this, ¶ms); } } } bool OptimizableGraph::addPostIterationAction(HyperGraphAction* action) { std::pair insertResult = _graphActions[AT_POSTITERATION].insert(action); return insertResult.second; } bool OptimizableGraph::addPreIterationAction(HyperGraphAction* action) { std::pair insertResult = _graphActions[AT_PREITERATION].insert(action); return insertResult.second; } bool OptimizableGraph::removePreIterationAction(HyperGraphAction* action) { return _graphActions[AT_PREITERATION].erase(action) > 0; } bool OptimizableGraph::removePostIterationAction(HyperGraphAction* action) { return _graphActions[AT_POSTITERATION].erase(action) > 0; } bool OptimizableGraph::saveVertex(std::ostream& os, OptimizableGraph::Vertex* v) const { Factory* factory = Factory::instance(); string tag = factory->tag(v); if (tag.size() > 0) { os << tag << " " << v->id() << " "; v->write(os); os << endl; Data* d=v->userData(); while (d) { // write the data packet for the vertex tag = factory->tag(d); if (tag.size() > 0) { os << tag << " "; d->write(os); os << endl; } d=d->next(); } if (v->fixed()) { os << "FIX " << v->id() << endl; } return os.good(); } return false; } bool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) const { Factory* factory = Factory::instance(); string tag = factory->tag(e); if (tag.size() > 0) { os << tag << " "; if (_edge_has_id) os << e->id() << " "; for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) { OptimizableGraph::Vertex* v = static_cast(*it); os << v->id() << " "; } e->write(os); os << endl; return os.good(); } return false; } void OptimizableGraph::clearParameters() { _parameters.clear(); } bool OptimizableGraph::verifyInformationMatrices(bool verbose) const { bool allEdgeOk = true; SelfAdjointEigenSolver eigenSolver; for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) { OptimizableGraph::Edge* e = static_cast(*it); Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension()); // test on symmetry bool isSymmetric = information.transpose() == information; bool okay = isSymmetric; if (isSymmetric) { // compute the eigenvalues eigenSolver.compute(information, Eigen::EigenvaluesOnly); bool isSPD = eigenSolver.eigenvalues()(0) >= 0.; okay = okay && isSPD; } allEdgeOk = allEdgeOk && okay; if (! okay) { if (verbose) { if (! isSymmetric) cerr << "Information Matrix for an edge is not symmetric:"; else cerr << "Information Matrix for an edge is not SPD:"; for (size_t i = 0; i < e->vertices().size(); ++i) cerr << " " << e->vertex(i)->id(); if (isSymmetric) cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose(); cerr << endl; } } } return allEdgeOk; } bool OptimizableGraph::initMultiThreading() { # if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0) Eigen::initParallel(); # endif return true; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/optimizable_graph.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_AIS_OPTIMIZABLE_GRAPH_HH_ #define G2O_AIS_OPTIMIZABLE_GRAPH_HH_ #include #include #include #include #include #include #include "openmp_mutex.h" #include "hyper_graph.h" #include "parameter.h" #include "parameter_container.h" #include "jacobian_workspace.h" #include "../stuff/macros.h" namespace g2o { class HyperGraphAction; struct OptimizationAlgorithmProperty; class Cache; class CacheContainer; class RobustKernel; /** @addtogroup g2o */ /** This is an abstract class that represents one optimization problem. It specializes the general graph to contain special vertices and edges. The vertices represent parameters that can be optimized, while the edges represent constraints. This class also provides basic functionalities to handle the backup/restore of portions of the vertices. */ struct OptimizableGraph : public HyperGraph { enum ActionType { AT_PREITERATION, AT_POSTITERATION, AT_NUM_ELEMENTS, // keep as last element }; typedef std::set HyperGraphActionSet; // forward declarations class Vertex; class Edge; /** * \brief data packet for a vertex. Extend this class to store in the vertices * the potential additional information you need (e.g. images, laser scans, ...). */ class Data : public HyperGraph::HyperGraphElement { friend struct OptimizableGraph; public: virtual ~Data(); Data(); //! read the data from a stream virtual bool read(std::istream& is) = 0; //! write the data to a stream virtual bool write(std::ostream& os) const = 0; virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;} const Data* next() const {return _next;} Data* next() {return _next;} void setNext(Data* next_) { _next = next_; } protected: Data* _next; // linked list of multiple data; }; /** * \brief order vertices based on their ID */ struct VertexIDCompare { bool operator() (const Vertex* v1, const Vertex* v2) const { return v1->id() < v2->id(); } }; /** * \brief order edges based on the internal ID, which is assigned to the edge in addEdge() */ struct EdgeIDCompare { bool operator() (const Edge* e1, const Edge* e2) const { return e1->internalId() < e2->internalId(); } }; //! vector container for vertices typedef std::vector VertexContainer; //! vector container for edges typedef std::vector EdgeContainer; /** * \brief A general case Vertex for optimization */ class Vertex : public HyperGraph::Vertex { private: friend struct OptimizableGraph; public: Vertex(); //! returns a deep copy of the current vertex virtual Vertex* clone() const ; //! the user data associated with this vertex const Data* userData() const { return _userData; } Data* userData() { return _userData; } void setUserData(Data* obs) { _userData = obs;} void addUserData(Data* obs) { if (obs) { obs->setNext(_userData); _userData=obs; } } virtual ~Vertex(); //! sets the node to the origin (used in the multilevel stuff) void setToOrigin() { setToOriginImpl(); updateCache();} //! get the element from the hessian matrix virtual const double& hessian(int i, int j) const = 0; virtual double& hessian(int i, int j) = 0; virtual double hessianDeterminant() const = 0; virtual double* hessianData() = 0; /** maps the internal matrix to some external memory location */ virtual void mapHessianMemory(double* d) = 0; /** * copies the b vector in the array b_ * @return the number of elements copied */ virtual int copyB(double* b_) const = 0; //! get the b vector element virtual const double& b(int i) const = 0; virtual double& b(int i) = 0; //! return a pointer to the b vector associated with this vertex virtual double* bData() = 0; /** * set the b vector part of this vertex to zero */ virtual void clearQuadraticForm() = 0; /** * updates the current vertex with the direct solution x += H_ii\b_ii * @return the determinant of the inverted hessian */ virtual double solveDirect(double lambda=0) = 0; /** * sets the initial estimate from an array of double * Implement setEstimateDataImpl() * @return true on success */ bool setEstimateData(const double* estimate); /** * sets the initial estimate from an array of double * Implement setEstimateDataImpl() * @return true on success */ bool setEstimateData(const std::vector& estimate) { #ifndef NDEBUG int dim = estimateDimension(); assert((dim == -1) || (estimate.size() == std::size_t(dim))); #endif return setEstimateData(&estimate[0]); }; /** * writes the estimater to an array of double * @returns true on success */ virtual bool getEstimateData(double* estimate) const; /** * writes the estimater to an array of double * @returns true on success */ virtual bool getEstimateData(std::vector& estimate) const { int dim = estimateDimension(); if (dim < 0) return false; estimate.resize(dim); return getEstimateData(&estimate[0]); }; /** * returns the dimension of the extended representation used by get/setEstimate(double*) * -1 if it is not supported */ virtual int estimateDimension() const; /** * sets the initial estimate from an array of double. * Implement setMinimalEstimateDataImpl() * @return true on success */ bool setMinimalEstimateData(const double* estimate); /** * sets the initial estimate from an array of double. * Implement setMinimalEstimateDataImpl() * @return true on success */ bool setMinimalEstimateData(const std::vector& estimate) { #ifndef NDEBUG int dim = minimalEstimateDimension(); assert((dim == -1) || (estimate.size() == std::size_t(dim))); #endif return setMinimalEstimateData(&estimate[0]); }; /** * writes the estimate to an array of double * @returns true on success */ virtual bool getMinimalEstimateData(double* estimate) const ; /** * writes the estimate to an array of double * @returns true on success */ virtual bool getMinimalEstimateData(std::vector& estimate) const { int dim = minimalEstimateDimension(); if (dim < 0) return false; estimate.resize(dim); return getMinimalEstimateData(&estimate[0]); }; /** * returns the dimension of the extended representation used by get/setEstimate(double*) * -1 if it is not supported */ virtual int minimalEstimateDimension() const; //! backup the position of the vertex to a stack virtual void push() = 0; //! restore the position of the vertex by retrieving the position from the stack virtual void pop() = 0; //! pop the last element from the stack, without restoring the current estimate virtual void discardTop() = 0; //! return the stack size virtual int stackSize() const = 0; /** * Update the position of the node from the parameters in v. * Depends on the implementation of oplusImpl in derived classes to actually carry * out the update. * Will also call updateCache() to update the caches of depending on the vertex. */ void oplus(const double* v) { oplusImpl(v); updateCache(); } //! temporary index of this node in the parameter vector obtained from linearization int hessianIndex() const { return _hessianIndex;} int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex();} //! set the temporary index of the vertex in the parameter blocks void setHessianIndex(int ti) { _hessianIndex = ti;} void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti);} //! true => this node is fixed during the optimization bool fixed() const {return _fixed;} //! true => this node should be considered fixed during the optimization void setFixed(bool fixed) { _fixed = fixed;} //! true => this node is marginalized out during the optimization bool marginalized() const {return _marginalized;} //! true => this node should be marginalized out during the optimization void setMarginalized(bool marginalized) { _marginalized = marginalized;} //! dimension of the estimated state belonging to this node int dimension() const { return _dimension;} //! sets the id of the node in the graph be sure that the graph keeps consistent after changing the id virtual void setId(int id) {_id = id;} //! set the row of this vertex in the Hessian void setColInHessian(int c) { _colInHessian = c;} //! get the row of this vertex in the Hessian int colInHessian() const {return _colInHessian;} const OptimizableGraph* graph() const {return _graph;} OptimizableGraph* graph() {return _graph;} /** * lock for the block of the hessian and the b vector associated with this vertex, to avoid * race-conditions if multi-threaded. */ void lockQuadraticForm() { _quadraticFormMutex.lock();} /** * unlock the block of the hessian and the b vector associated with this vertex */ void unlockQuadraticForm() { _quadraticFormMutex.unlock();} //! read the vertex from a stream, i.e., the internal state of the vertex virtual bool read(std::istream& is) = 0; //! write the vertex to a stream virtual bool write(std::ostream& os) const = 0; virtual void updateCache(); CacheContainer* cacheContainer(); protected: OptimizableGraph* _graph; Data* _userData; int _hessianIndex; bool _fixed; bool _marginalized; int _dimension; int _colInHessian; OpenMPMutex _quadraticFormMutex; CacheContainer* _cacheContainer; /** * update the position of the node from the parameters in v. * Implement in your class! */ virtual void oplusImpl(const double* v) = 0; //! sets the node to the origin (used in the multilevel stuff) virtual void setToOriginImpl() = 0; /** * writes the estimater to an array of double * @returns true on success */ virtual bool setEstimateDataImpl(const double* ) { return false;} /** * sets the initial estimate from an array of double * @return true on success */ virtual bool setMinimalEstimateDataImpl(const double* ) { return false;} }; class Edge: public HyperGraph::Edge { private: friend struct OptimizableGraph; public: Edge(); virtual ~Edge(); virtual Edge* clone() const; // indicates if all vertices are fixed virtual bool allVerticesFixed() const = 0; // computes the error of the edge and stores it in an internal structure virtual void computeError() = 0; //! sets the measurement from an array of double //! @returns true on success virtual bool setMeasurementData(const double* m); //! writes the measurement to an array of double //! @returns true on success virtual bool getMeasurementData(double* m) const; //! returns the dimension of the measurement in the extended representation which is used //! by get/setMeasurement; virtual int measurementDimension() const; /** * sets the estimate to have a zero error, based on the current value of the state variables * returns false if not supported. */ virtual bool setMeasurementFromState(); //! if NOT NULL, error of this edge will be robustifed with the kernel RobustKernel* robustKernel() const { return _robustKernel;} /** * specify the robust kernel to be used in this edge */ void setRobustKernel(RobustKernel* ptr); //! returns the error vector cached after calling the computeError; virtual const double* errorData() const = 0; virtual double* errorData() = 0; //! returns the memory of the information matrix, usable for example with a Eigen::Map virtual const double* informationData() const = 0; virtual double* informationData() = 0; //! computes the chi2 based on the cached error value, only valid after computeError has been called. virtual double chi2() const = 0; /** * Linearizes the constraint in the edge. * Makes side effect on the vertices of the graph by changing * the parameter vector b and the hessian blocks ii and jj. * The off diagoinal block is accesed via _hessian. */ virtual void constructQuadraticForm() = 0; /** * maps the internal matrix to some external memory location, * you need to provide the memory before calling constructQuadraticForm * @param d the memory location to which we map * @param i index of the vertex i * @param j index of the vertex j (j > i, upper triangular fashion) * @param rowMajor if true, will write in rowMajor order to the block. Since EIGEN is columnMajor by default, this results in writing the transposed */ virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0; /** * Linearizes the constraint in the edge in the manifold space, and store * the result in the given workspace */ virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0; /** set the estimate of the to vertex, based on the estimate of the from vertices in the edge. */ virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) = 0; /** * override in your class if it's possible to initialize the vertices in certain combinations. * The return value may correspond to the cost for initiliaizng the vertex but should be positive if * the initialization is possible and negative if not possible. */ virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) from; (void) to; return -1.;} //! returns the level of the edge int level() const { return _level;} //! sets the level of the edge void setLevel(int l) { _level=l;} //! returns the dimensions of the error function int dimension() const { return _dimension;} virtual Vertex* createFrom() {return 0;} virtual Vertex* createTo() {return 0;} //! read the vertex from a stream, i.e., the internal state of the vertex virtual bool read(std::istream& is) = 0; //! write the vertex to a stream virtual bool write(std::ostream& os) const = 0; //! the internal ID of the edge long long internalId() const { return _internalId;} OptimizableGraph* graph(); const OptimizableGraph* graph() const; bool setParameterId(int argNum, int paramId); inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);} inline size_t numParameters() const {return _parameters.size();} inline void resizeParameters(size_t newSize) { _parameters.resize(newSize, 0); _parameterIds.resize(newSize, -1); _parameterTypes.resize(newSize, typeid(void*).name()); } protected: int _dimension; int _level; RobustKernel* _robustKernel; long long _internalId; std::vector _cacheIds; template bool installParameter(ParameterType*& p, size_t argNo, int paramId=-1){ if (argNo>=_parameters.size()) return false; _parameterIds[argNo] = paramId; _parameters[argNo] = (Parameter**)&p; _parameterTypes[argNo] = typeid(ParameterType).name(); return true; } template void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, const std::string& _type, const ParameterVector& parameters); bool resolveParameters(); virtual bool resolveCaches(); std::vector _parameterTypes; std::vector _parameters; std::vector _parameterIds; }; //! returns the vertex number id appropriately casted inline Vertex* vertex(int id) { return reinterpret_cast(HyperGraph::vertex(id));} //! returns the vertex number id appropriately casted inline const Vertex* vertex (int id) const{ return reinterpret_cast(HyperGraph::vertex(id));} //! empty constructor OptimizableGraph(); virtual ~OptimizableGraph(); //! adds all edges and vertices of the graph g to this graph. void addGraph(OptimizableGraph* g); /** * adds a new vertex. The new vertex is then "taken". * @return false if a vertex with the same id as v is already in the graph, true otherwise. */ virtual bool addVertex(HyperGraph::Vertex* v, Data* userData); virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);} /** * adds a new edge. * The edge should point to the vertices that it is connecting (setFrom/setTo). * @return false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise. */ virtual bool addEdge(HyperGraph::Edge* e); //! returns the chi2 of the current configuration double chi2() const; //! return the maximum dimension of all vertices in the graph int maxDimension() const; /** * iterates over all vertices and returns a set of all the vertex dimensions in the graph */ std::set dimensions() const; /** * carry out n iterations * @return the number of performed iterations */ virtual int optimize(int iterations, bool online=false); //! called at the beginning of an iteration (argument is the number of the iteration) virtual void preIteration(int); //! called at the end of an iteration (argument is the number of the iteration) virtual void postIteration(int); //! add an action to be executed before each iteration bool addPreIterationAction(HyperGraphAction* action); //! add an action to be executed after each iteration bool addPostIterationAction(HyperGraphAction* action); //! remove an action that should no longer be execured before each iteration bool removePreIterationAction(HyperGraphAction* action); //! remove an action that should no longer be execured after each iteration bool removePostIterationAction(HyperGraphAction* action); //! push the estimate of all variables onto a stack virtual void push(); //! pop (restore) the estimate of all variables from the stack virtual void pop(); //! discard the last backup of the estimate for all variables by removing it from the stack virtual void discardTop(); //! load the graph from a stream. Uses the Factory singleton for creating the vertices and edges. virtual bool load(std::istream& is, bool createEdges=true); bool load(const char* filename, bool createEdges=true); //! save the graph to a stream. Again uses the Factory system. virtual bool save(std::ostream& os, int level = 0) const; //! function provided for convenience, see save() above bool save(const char* filename, int level = 0) const; //! save a subgraph to a stream. Again uses the Factory system. bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0); //! save a subgraph to a stream. Again uses the Factory system. bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset); //! push the estimate of a subset of the variables onto a stack virtual void push(HyperGraph::VertexSet& vset); //! pop (restore) the estimate a subset of the variables from the stack virtual void pop(HyperGraph::VertexSet& vset); //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate virtual void discardTop(HyperGraph::VertexSet& vset); //! fixes/releases a set of vertices virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed); /** * set the renamed types lookup from a string, format is for example: * VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP * This will change the occurance of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP */ void setRenamedTypesFromString(const std::string& types); /** * test whether a solver is suitable for optimizing this graph. * @param solverProperty the solver property to evaluate. * @param vertDims should equal to the set returned by dimensions() to avoid re-evaluating. */ bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set& vertDims = std::set()) const; /** * remove the parameters of the graph */ virtual void clearParameters(); bool addParameter(Parameter* p) { return _parameters.addParameter(p); } Parameter* parameter(int id) { return _parameters.getParameter(id); } /** * verify that all the information of the edges are semi positive definite, i.e., * all Eigenvalues are >= 0. * @param verbose output edges with not PSD information matrix on cerr * @return true if all edges have PSD information matrix */ bool verifyInformationMatrices(bool verbose = false) const; // helper functions to save an individual vertex bool saveVertex(std::ostream& os, Vertex* v) const; // helper functions to save an individual edge bool saveEdge(std::ostream& os, Edge* e) const; //! the workspace for storing the Jacobians of the graph JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace;} const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace;} /** * Eigen starting from version 3.1 requires that we call an initialize * function, if we perform calls to Eigen from several threads. * Currently, this function calls Eigen::initParallel if g2o is compiled * with OpenMP support and Eigen's version is at least 3.1 */ static bool initMultiThreading(); protected: std::map _renamedTypesLookup; long long _nextEdgeId; std::vector _graphActions; // do not watch this. To be removed soon, or integrated in a nice way bool _edge_has_id; ParameterContainer _parameters; JacobianWorkspace _jacobianWorkspace; }; /** @} */ } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm.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 "optimization_algorithm.h" using namespace std; namespace g2o { OptimizationAlgorithm::OptimizationAlgorithm() : _optimizer(0) { } OptimizationAlgorithm::~OptimizationAlgorithm() { } void OptimizationAlgorithm::printProperties(std::ostream& os) const { os << "------------- Algorithm Properties -------------" << endl; for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) { BaseProperty* p = it->second; os << it->first << "\t" << p->toString() << endl; } os << "------------------------------------------------" << endl; } bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString) { return _properties.updateMapFromString(propString); } void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer) { _optimizer = optimizer; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm.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_OPTIMIZATION_ALGORITHM_H #define G2O_OPTIMIZATION_ALGORITHM_H #include #include #include #include "../stuff/property.h" #include "hyper_graph.h" #include "sparse_block_matrix.h" namespace g2o { class SparseOptimizer; /** * \brief Generic interface for a non-linear solver operating on a graph */ class OptimizationAlgorithm { public: enum SolverResult {Terminate=2, OK=1, Fail=-1}; OptimizationAlgorithm(); virtual ~OptimizationAlgorithm(); /** * initialize the solver, called once before the first call to solve() */ virtual bool init(bool online = false) = 0; /** * Solve one iteration. The SparseOptimizer running on-top will call this * for the given number of iterations. * @param iteration indicates the current iteration */ virtual SolverResult solve(int iteration, bool online = false) = 0; /** * computes the block diagonal elements of the pattern specified in the input * and stores them in given SparseBlockMatrix. * If your solver does not support computing the marginals, return false. */ virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0; /** * update the structures for online processing */ virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0; /** * called by the optimizer if verbose. re-implement, if you want to print something */ virtual void printVerbose(std::ostream& os) const {(void) os;}; public: //! return the optimizer operating on const SparseOptimizer* optimizer() const { return _optimizer;} SparseOptimizer* optimizer() { return _optimizer;} /** * specify on which optimizer the solver should work on */ void setOptimizer(SparseOptimizer* optimizer); //! return the properties of the solver const PropertyMap& properties() const { return _properties;} /** * update the properties from a string, see PropertyMap::updateMapFromString() */ bool updatePropertiesFromString(const std::string& propString); /** * print the properties to a stream in a human readable fashion */ void printProperties(std::ostream& os) const; protected: SparseOptimizer* _optimizer; ///< the optimizer the solver is working on PropertyMap _properties; ///< the properties of your solver, use this to store the parameters of your solver private: // Disable the copy constructor and assignment operator OptimizationAlgorithm(const OptimizationAlgorithm&) { } OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; } }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.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 "optimization_algorithm_dogleg.h" #include #include "../stuff/timeutil.h" #include "block_solver.h" #include "sparse_optimizer.h" #include "solver.h" #include "batch_stats.h" using namespace std; namespace g2o { OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(BlockSolverBase* solver) : OptimizationAlgorithmWithHessian(solver) { _userDeltaInit = _properties.makeProperty >("initialDelta", 1e4); _maxTrialsAfterFailure = _properties.makeProperty >("maxTrialsAfterFailure", 100); _initialLambda = _properties.makeProperty >("initialLambda", 1e-7); _lamdbaFactor = _properties.makeProperty >("lambdaFactor", 10.); _delta = _userDeltaInit->value(); _lastStep = STEP_UNDEFINED; _wasPDInAllIterations = true; } OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg() { } OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(int iteration, bool online) { assert(_optimizer && "_optimizer not set"); assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); assert(dynamic_cast(_solver) && "underlying linear solver is not a block solver"); BlockSolverBase* blockSolver = static_cast(_solver); if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure bool ok = _solver->buildStructure(); if (! ok) { cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; return OptimizationAlgorithm::Fail; } // init some members to the current size of the problem _hsd.resize(_solver->vectorSize()); _hdl.resize(_solver->vectorSize()); _auxVector.resize(_solver->vectorSize()); _delta = _userDeltaInit->value(); _currentLambda = _initialLambda->value(); _wasPDInAllIterations = true; } double t=get_monotonic_time(); _optimizer->computeActiveErrors(); G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); if (globalStats) { globalStats->timeResiduals = get_monotonic_time()-t; t=get_monotonic_time(); } double currentChi = _optimizer->activeRobustChi2(); _solver->buildSystem(); if (globalStats) { globalStats->timeQuadraticForm = get_monotonic_time()-t; } Eigen::VectorXd::ConstMapType b(_solver->b(), _solver->vectorSize()); // compute alpha _auxVector.setZero(); blockSolver->multiplyHessian(_auxVector.data(), _solver->b()); double bNormSquared = b.squaredNorm(); double alpha = bNormSquared / _auxVector.dot(b); _hsd = alpha * b; double hsdNorm = _hsd.norm(); double hgnNorm = -1.; bool solvedGaussNewton = false; bool goodStep = false; int& numTries = _lastNumTries; numTries = 0; do { ++numTries; if (! solvedGaussNewton) { const double minLambda = 1e-12; const double maxLambda = 1e3; solvedGaussNewton = true; // apply a damping factor to enforce positive definite Hessian, if the matrix appeared // to be not positive definite in at least one iteration before. // We apply a damping factor to obtain a PD matrix. bool solverOk = false; while(!solverOk) { if (! _wasPDInAllIterations) _solver->setLambda(_currentLambda, true); // add _currentLambda to the diagonal solverOk = _solver->solve(); if (! _wasPDInAllIterations) _solver->restoreDiagonal(); _wasPDInAllIterations = _wasPDInAllIterations && solverOk; if (! _wasPDInAllIterations) { // simple strategy to control the damping factor if (solverOk) { _currentLambda = std::max(minLambda, _currentLambda / (0.5 * _lamdbaFactor->value())); } else { _currentLambda *= _lamdbaFactor->value(); if (_currentLambda > maxLambda) { _currentLambda = maxLambda; return Fail; } } } } if (!solverOk) { return Fail; } hgnNorm = Eigen::VectorXd::ConstMapType(_solver->x(), _solver->vectorSize()).norm(); } Eigen::VectorXd::ConstMapType hgn(_solver->x(), _solver->vectorSize()); assert(hgnNorm >= 0. && "Norm of the GN step is not computed"); if (hgnNorm < _delta) { _hdl = hgn; _lastStep = STEP_GN; } else if (hsdNorm > _delta) { _hdl = _delta / hsdNorm * _hsd; _lastStep = STEP_SD; } else { _auxVector = hgn - _hsd; // b - a double c = _hsd.dot(_auxVector); double bmaSquaredNorm = _auxVector.squaredNorm(); double beta; if (c <= 0.) beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm; else { double hsdSqrNorm = _hsd.squaredNorm(); beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm))); } assert(beta > 0. && beta < 1 && "Error while computing beta"); _hdl = _hsd + beta * (hgn - _hsd); _lastStep = STEP_DL; assert(_hdl.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region"); } // compute the linear gain _auxVector.setZero(); blockSolver->multiplyHessian(_auxVector.data(), _hdl.data()); double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl)); // apply the update and see what happens _optimizer->push(); _optimizer->update(_hdl.data()); _optimizer->computeActiveErrors(); double newChi = _optimizer-> activeRobustChi2(); double nonLinearGain = currentChi - newChi; if (fabs(linearGain) < 1e-12) linearGain = 1e-12; double rho = nonLinearGain / linearGain; //cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " << PVAR(rho) << endl; if (rho > 0) { // step is good and will be accepted _optimizer->discardTop(); goodStep = true; } else { // recover previous state _optimizer->pop(); } // update trust region based on the step quality if (rho > 0.75) _delta = std::max(_delta, 3. * _hdl.norm()); else if (rho < 0.25) _delta *= 0.5; } while (!goodStep && numTries < _maxTrialsAfterFailure->value()); if (numTries == _maxTrialsAfterFailure->value() || !goodStep) return Terminate; return OK; } void OptimizationAlgorithmDogleg::printVerbose(std::ostream& os) const { os << "\t Delta= " << _delta << "\t step= " << stepType2Str(_lastStep) << "\t tries= " << _lastNumTries; if (! _wasPDInAllIterations) os << "\t lambda= " << _currentLambda; } const char* OptimizationAlgorithmDogleg::stepType2Str(int stepType) { switch (stepType) { case STEP_SD: return "Descent"; case STEP_GN: return "GN"; case STEP_DL: return "Dogleg"; default: return "Undefined"; } } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.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_OPTIMIZATION_ALGORITHM_DOGLEG_H #define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H #include "optimization_algorithm_with_hessian.h" namespace g2o { class BlockSolverBase; /** * \brief Implementation of Powell's Dogleg Algorithm */ class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian { public: /** \brief type of the step to take */ enum { STEP_UNDEFINED, STEP_SD, STEP_GN, STEP_DL }; public: /** * construct the Dogleg algorithm, which will use the given Solver for solving the * linearized system. */ explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver); virtual ~OptimizationAlgorithmDogleg(); virtual SolverResult solve(int iteration, bool online = false); virtual void printVerbose(std::ostream& os) const; //! return the type of the last step taken by the algorithm int lastStep() const { return _lastStep;} //! return the diameter of the trust region double trustRegion() const { return _delta;} //! convert the type into an integer static const char* stepType2Str(int stepType); protected: // parameters Property* _maxTrialsAfterFailure; Property* _userDeltaInit; // damping to enforce positive definite matrix Property* _initialLambda; Property* _lamdbaFactor; Eigen::VectorXd _hsd; ///< steepest decent step Eigen::VectorXd _hdl; ///< final dogleg step Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff double _currentLambda; ///< the damping factor to force positive definite matrix double _delta; ///< trust region int _lastStep; ///< type of the step taken by the algorithm bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping int _lastNumTries; }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_factory.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 "optimization_algorithm_factory.h" #include #include #include using namespace std; namespace g2o { AbstractOptimizationAlgorithmCreator::AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p) : _property(p) { } OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::factoryInstance = 0; OptimizationAlgorithmFactory::OptimizationAlgorithmFactory() { } OptimizationAlgorithmFactory::~OptimizationAlgorithmFactory() { for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) delete *it; } OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::instance() { if (factoryInstance == 0) { factoryInstance = new OptimizationAlgorithmFactory; } return factoryInstance; } void OptimizationAlgorithmFactory::registerSolver(AbstractOptimizationAlgorithmCreator* c) { const string& name = c->property().name; CreatorList::iterator foundIt = findSolver(name); if (foundIt != _creator.end()) { _creator.erase(foundIt); cerr << "SOLVER FACTORY WARNING: Overwriting Solver creator " << name << endl; assert(0); } _creator.push_back(c); } void OptimizationAlgorithmFactory::unregisterSolver(AbstractOptimizationAlgorithmCreator* c) { const string& name = c->property().name; CreatorList::iterator foundIt = findSolver(name); if (foundIt != _creator.end()) { delete *foundIt; _creator.erase(foundIt); } } OptimizationAlgorithm* OptimizationAlgorithmFactory::construct(const std::string& name, OptimizationAlgorithmProperty& solverProperty) const { CreatorList::const_iterator foundIt = findSolver(name); if (foundIt != _creator.end()) { solverProperty = (*foundIt)->property(); return (*foundIt)->construct(); } cerr << "SOLVER FACTORY WARNING: Unable to create solver " << name << endl; return 0; } void OptimizationAlgorithmFactory::destroy() { delete factoryInstance; factoryInstance = 0; } void OptimizationAlgorithmFactory::listSolvers(std::ostream& os) const { size_t solverNameColumnLength = 0; for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) solverNameColumnLength = std::max(solverNameColumnLength, (*it)->property().name.size()); solverNameColumnLength += 4; for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) { const OptimizationAlgorithmProperty& sp = (*it)->property(); os << sp.name; for (size_t i = sp.name.size(); i < solverNameColumnLength; ++i) os << ' '; os << sp.desc << endl; } } OptimizationAlgorithmFactory::CreatorList::const_iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) const { for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) { const OptimizationAlgorithmProperty& sp = (*it)->property(); if (sp.name == name) return it; } return _creator.end(); } OptimizationAlgorithmFactory::CreatorList::iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) { for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) { const OptimizationAlgorithmProperty& sp = (*it)->property(); if (sp.name == name) return it; } return _creator.end(); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_factory.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_OPTMIZATION_ALGORITHM_PROPERTY_H #define G2O_OPTMIZATION_ALGORITHM_PROPERTY_H #include "../../config.h" #include "../stuff/misc.h" #include "optimization_algorithm_property.h" #include #include #include // define to get some verbose output //#define G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY namespace g2o { // forward decl class OptimizationAlgorithm; class SparseOptimizer; /** * \brief base for allocating an optimization algorithm * * Allocating a solver for a given optimizer. The method construct() has to be * implemented in your derived class to allocate the desired solver. */ class AbstractOptimizationAlgorithmCreator { public: AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p); virtual ~AbstractOptimizationAlgorithmCreator() { } //! allocate a solver operating on optimizer, re-implement for your creator virtual OptimizationAlgorithm* construct() = 0; //! return the properties of the solver const OptimizationAlgorithmProperty& property() const { return _property;} protected: OptimizationAlgorithmProperty _property; }; /** * \brief create solvers based on their short name * * Factory to allocate solvers based on their short name. * The Factory is implemented as a sigleton and the single * instance can be accessed via the instance() function. */ class OptimizationAlgorithmFactory { public: typedef std::list CreatorList; //! return the instance static OptimizationAlgorithmFactory* instance(); //! free the instance static void destroy(); /** * register a specific creator for allocating a solver */ void registerSolver(AbstractOptimizationAlgorithmCreator* c); /** * unregister a specific creator for allocating a solver */ void unregisterSolver(AbstractOptimizationAlgorithmCreator* c); /** * construct a solver based on its name, e.g., var, fix3_2_cholmod */ OptimizationAlgorithm* construct(const std::string& tag, OptimizationAlgorithmProperty& solverProperty) const; //! list the known solvers into a stream void listSolvers(std::ostream& os) const; //! return the underlying list of creators const CreatorList& creatorList() const { return _creator;} protected: OptimizationAlgorithmFactory(); ~OptimizationAlgorithmFactory(); CreatorList _creator; CreatorList::const_iterator findSolver(const std::string& name) const; CreatorList::iterator findSolver(const std::string& name); private: static OptimizationAlgorithmFactory* factoryInstance; }; class RegisterOptimizationAlgorithmProxy { public: RegisterOptimizationAlgorithmProxy(AbstractOptimizationAlgorithmCreator* c) { _creator = c; #ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY std::cout << __FUNCTION__ << ": Registering " << _creator->property().name << " of type " << typeid(*_creator).name() << std::endl; #endif OptimizationAlgorithmFactory::instance()->registerSolver(c); } ~RegisterOptimizationAlgorithmProxy() { #ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY std::cout << __FUNCTION__ << ": Unregistering " << _creator->property().name << std::endl; #endif OptimizationAlgorithmFactory::instance()->unregisterSolver(_creator); } private: AbstractOptimizationAlgorithmCreator* _creator; }; } #if defined _MSC_VER && defined G2O_SHARED_LIBS # define G2O_OAF_EXPORT __declspec(dllexport) # define G2O_OAF_IMPORT __declspec(dllimport) #else # define G2O_OAF_EXPORT # define G2O_OAF_IMPORT #endif #define G2O_REGISTER_OPTIMIZATION_LIBRARY(libraryname) \ extern "C" void G2O_OAF_EXPORT g2o_optimization_library_##libraryname(void) {} #define G2O_USE_OPTIMIZATION_LIBRARY(libraryname) \ extern "C" void G2O_OAF_IMPORT g2o_optimization_library_##libraryname(void); \ static g2o::ForceLinker g2o_force_optimization_algorithm_library_##libraryname(g2o_optimization_library_##libraryname); #define G2O_REGISTER_OPTIMIZATION_ALGORITHM(optimizername, instance) \ extern "C" void G2O_OAF_EXPORT g2o_optimization_algorithm_##optimizername(void) {} \ static g2o::RegisterOptimizationAlgorithmProxy g_optimization_algorithm_proxy_##optimizername(instance); #define G2O_USE_OPTIMIZATION_ALGORITHM(optimizername) \ extern "C" void G2O_OAF_IMPORT g2o_optimization_algorithm_##optimizername(void); \ static g2o::ForceLinker g2o_force_optimization_algorithm_link_##optimizername(g2o_optimization_algorithm_##optimizername); #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.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 "optimization_algorithm_gauss_newton.h" #include #include "../stuff/timeutil.h" #include "../stuff/macros.h" #include "solver.h" #include "batch_stats.h" #include "sparse_optimizer.h" using namespace std; namespace g2o { OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) : OptimizationAlgorithmWithHessian(solver) { } OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton() { } OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online) { assert(_optimizer && "_optimizer not set"); assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); bool ok = true; //here so that correct component for max-mixtures can be computed before the build structure double t=get_monotonic_time(); _optimizer->computeActiveErrors(); G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); if (globalStats) { globalStats->timeResiduals = get_monotonic_time()-t; } if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure ok = _solver->buildStructure(); if (! ok) { cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; return OptimizationAlgorithm::Fail; } } t=get_monotonic_time(); _solver->buildSystem(); if (globalStats) { globalStats->timeQuadraticForm = get_monotonic_time()-t; t=get_monotonic_time(); } ok = _solver->solve(); if (globalStats) { globalStats->timeLinearSolution = get_monotonic_time()-t; t=get_monotonic_time(); } _optimizer->update(_solver->x()); if (globalStats) { globalStats->timeUpdate = get_monotonic_time()-t; } if (ok) return OK; else return Fail; } void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const { os << "\t schur= " << _solver->schur(); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.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_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H #define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H #include "optimization_algorithm_with_hessian.h" namespace g2o { /** * \brief Implementation of the Gauss Newton Algorithm */ class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian { public: /** * construct the Gauss Newton algorithm, which use the given Solver for solving the * linearized system. */ explicit OptimizationAlgorithmGaussNewton(Solver* solver); virtual ~OptimizationAlgorithmGaussNewton(); virtual SolverResult solve(int iteration, bool online = false); virtual void printVerbose(std::ostream& os) const; }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.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. // Modified Raul Mur Artal (2014) // - Stop criterium (solve function) #include "optimization_algorithm_levenberg.h" #include #include "../stuff/timeutil.h" #include "sparse_optimizer.h" #include "solver.h" #include "batch_stats.h" using namespace std; namespace g2o { OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Solver* solver) : OptimizationAlgorithmWithHessian(solver) { _currentLambda = -1.; _tau = 1e-5; _goodStepUpperScale = 2./3.; _goodStepLowerScale = 1./3.; _userLambdaInit = _properties.makeProperty >("initialLambda", 0.); _maxTrialsAfterFailure = _properties.makeProperty >("maxTrialsAfterFailure", 10); _ni=2.; _levenbergIterations = 0; _nBad = 0; } OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg() { } OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online) { assert(_optimizer && "_optimizer not set"); assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph"); if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure bool ok = _solver->buildStructure(); if (! ok) { cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl; return OptimizationAlgorithm::Fail; } } double t=get_monotonic_time(); _optimizer->computeActiveErrors(); G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); if (globalStats) { globalStats->timeResiduals = get_monotonic_time()-t; t=get_monotonic_time(); } double currentChi = _optimizer->activeRobustChi2(); double tempChi=currentChi; double iniChi = currentChi; _solver->buildSystem(); if (globalStats) { globalStats->timeQuadraticForm = get_monotonic_time()-t; } // core part of the Levenbarg algorithm if (iteration == 0) { _currentLambda = computeLambdaInit(); _ni = 2; _nBad = 0; } double rho=0; int& qmax = _levenbergIterations; qmax = 0; do { _optimizer->push(); if (globalStats) { globalStats->levenbergIterations++; t=get_monotonic_time(); } // update the diagonal of the system matrix _solver->setLambda(_currentLambda, true); bool ok2 = _solver->solve(); if (globalStats) { globalStats->timeLinearSolution+=get_monotonic_time()-t; t=get_monotonic_time(); } _optimizer->update(_solver->x()); if (globalStats) { globalStats->timeUpdate = get_monotonic_time()-t; } // restore the diagonal _solver->restoreDiagonal(); _optimizer->computeActiveErrors(); tempChi = _optimizer->activeRobustChi2(); if (! ok2) tempChi=std::numeric_limits::max(); rho = (currentChi-tempChi); double scale = computeScale(); scale += 1e-3; // make sure it's non-zero :) rho /= scale; if (rho>0 && g2o_isfinite(tempChi)){ // last step was good double alpha = 1.-pow((2*rho-1),3); // crop lambda between minimum and maximum factors alpha = (std::min)(alpha, _goodStepUpperScale); double scaleFactor = (std::max)(_goodStepLowerScale, alpha); _currentLambda *= scaleFactor; _ni = 2; currentChi=tempChi; _optimizer->discardTop(); } else { _currentLambda*=_ni; _ni*=2; _optimizer->pop(); // restore the last state before trying to optimize } qmax++; } while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate()); if (qmax == _maxTrialsAfterFailure->value() || rho==0) return Terminate; //Stop criterium (Raul) if((iniChi-currentChi)*1e3=3) return Terminate; return OK; } double OptimizationAlgorithmLevenberg::computeLambdaInit() const { if (_userLambdaInit->value() > 0) return _userLambdaInit->value(); double maxDiagonal=0.; for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) { OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k]; assert(v); int dim = v->dimension(); for (int j = 0; j < dim; ++j){ maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal); } } return _tau*maxDiagonal; } double OptimizationAlgorithmLevenberg::computeScale() const { double scale = 0.; for (size_t j=0; j < _solver->vectorSize(); j++){ scale += _solver->x()[j] * (_currentLambda * _solver->x()[j] + _solver->b()[j]); } return scale; } void OptimizationAlgorithmLevenberg::setMaxTrialsAfterFailure(int max_trials) { _maxTrialsAfterFailure->setValue(max_trials); } void OptimizationAlgorithmLevenberg::setUserLambdaInit(double lambda) { _userLambdaInit->setValue(lambda); } void OptimizationAlgorithmLevenberg::printVerbose(std::ostream& os) const { os << "\t schur= " << _solver->schur() << "\t lambda= " << FIXED(_currentLambda) << "\t levenbergIter= " << _levenbergIterations; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.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_SOLVER_LEVENBERG_H #define G2O_SOLVER_LEVENBERG_H #include "optimization_algorithm_with_hessian.h" namespace g2o { /** * \brief Implementation of the Levenberg Algorithm */ class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian { public: /** * construct the Levenberg algorithm, which will use the given Solver for solving the * linearized system. */ explicit OptimizationAlgorithmLevenberg(Solver* solver); virtual ~OptimizationAlgorithmLevenberg(); virtual SolverResult solve(int iteration, bool online = false); virtual void printVerbose(std::ostream& os) const; //! return the currently used damping factor double currentLambda() const { return _currentLambda;} //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt void setMaxTrialsAfterFailure(int max_trials); //! get the number of inner iterations for Levenberg-Marquardt int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();} //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda double userLambdaInit() {return _userLambdaInit->value();} //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value void setUserLambdaInit(double lambda); //! return the number of levenberg iterations performed in the last round int levenbergIteration() { return _levenbergIterations;} protected: // Levenberg parameters Property* _maxTrialsAfterFailure; Property* _userLambdaInit; double _currentLambda; double _tau; double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step double _ni; int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step //RAUL int _nBad; /** * helper for Levenberg, this function computes the initial damping factor, if the user did not * specify an own value, see setUserLambdaInit() */ double computeLambdaInit() const; double computeScale() const; }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_property.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_OPTIMIZATION_ALGORITHM_PROPERTY_H #define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H #include namespace g2o { /** * \brief describe the properties of a solver */ struct OptimizationAlgorithmProperty { std::string name; ///< name of the solver, e.g., var std::string desc; ///< short description of the solver std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG" bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks int poseDim; ///< dimension of the pose vertices (-1 if variable) int landmarkDim; ///< dimension of the landmar vertices (-1 if variable) OptimizationAlgorithmProperty() : name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1) { } OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) : name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_) { } }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.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 "optimization_algorithm_with_hessian.h" #include "solver.h" #include "optimizable_graph.h" #include "sparse_optimizer.h" #include using namespace std; namespace g2o { OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) : OptimizationAlgorithm(), _solver(solver) { _writeDebug = _properties.makeProperty >("writeDebug", true); } OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian() { delete _solver; } bool OptimizationAlgorithmWithHessian::init(bool online) { assert(_optimizer && "_optimizer not set"); assert(_solver && "Solver not set"); _solver->setWriteDebug(_writeDebug->value()); bool useSchur=false; for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) { OptimizableGraph::Vertex* v= *it; if (v->marginalized()){ useSchur=true; break; } } if (useSchur){ if (_solver->supportsSchur()) _solver->setSchur(true); } else { if (_solver->supportsSchur()) _solver->setSchur(false); } bool initState = _solver->init(_optimizer, online); return initState; } bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) { return _solver ? _solver->computeMarginals(spinv, blockIndices) : false; } bool OptimizationAlgorithmWithHessian::buildLinearStructure() { return _solver ? _solver->buildStructure() : false; } void OptimizationAlgorithmWithHessian::updateLinearSystem() { if (_solver) _solver->buildSystem(); } bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) { return _solver ? _solver->updateStructure(vset, edges) : false; } void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug) { _writeDebug->setValue(writeDebug); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.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_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H #define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H #include "optimization_algorithm.h" namespace g2o { class Solver; /** * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg */ class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm { public: explicit OptimizationAlgorithmWithHessian(Solver* solver); virtual ~OptimizationAlgorithmWithHessian(); virtual bool init(bool online = false); virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); virtual bool buildLinearStructure(); virtual void updateLinearSystem(); virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges); //! return the underlying solver used to solve the linear system Solver* solver() { return _solver;} /** * write debug output of the Hessian if system is not positive definite */ virtual void setWriteDebug(bool writeDebug); virtual bool writeDebug() const { return _writeDebug->value();} protected: Solver* _solver; Property* _writeDebug; }; }// end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/parameter.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 "parameter.h" namespace g2o { Parameter::Parameter() : _id(-1) { } void Parameter::setId(int id_) { _id = id_; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/parameter.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_GRAPH_PARAMETER_HH_ #define G2O_GRAPH_PARAMETER_HH_ #include #include "hyper_graph.h" namespace g2o { class Parameter : public HyperGraph::HyperGraphElement { public: Parameter(); virtual ~Parameter() {}; //! read the data from a stream virtual bool read(std::istream& is) = 0; //! write the data to a stream virtual bool write(std::ostream& os) const = 0; int id() const {return _id;} void setId(int id_); virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;} protected: int _id; }; typedef std::vector ParameterVector; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/parameter_container.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 "parameter_container.h" #include #include "factory.h" #include "parameter.h" #include "../stuff/macros.h" #include "../stuff/color_macros.h" #include "../stuff/string_tools.h" namespace g2o { using namespace std; ParameterContainer::ParameterContainer(bool isMainStorage_) : _isMainStorage(isMainStorage_) { } void ParameterContainer::clear() { if (!_isMainStorage) return; for (iterator it = begin(); it!=end(); it++){ delete it->second; } BaseClass::clear(); } ParameterContainer::~ParameterContainer(){ clear(); } bool ParameterContainer::addParameter(Parameter* p){ if (p->id()<0) return false; iterator it=find(p->id()); if (it!=end()) return false; insert(make_pair(p->id(), p)); return true; } Parameter* ParameterContainer::getParameter(int id) { iterator it=find(id); if (it==end()) return 0; return it->second; } Parameter* ParameterContainer::detachParameter(int id){ iterator it=find(id); if (it==end()) return 0; Parameter* p=it->second; erase(it); return p; } bool ParameterContainer::write(std::ostream& os) const{ Factory* factory = Factory::instance(); for (const_iterator it=begin(); it!=end(); it++){ os << factory->tag(it->second) << " "; os << it->second->id() << " "; it->second->write(os); os << endl; } return true; } bool ParameterContainer::read(std::istream& is, const std::map* _renamedTypesLookup){ stringstream currentLine; string token; Factory* factory = Factory::instance(); HyperGraph::GraphElemBitset elemBitset; elemBitset[HyperGraph::HGET_PARAMETER] = 1; while (1) { int bytesRead = readLine(is, currentLine); if (bytesRead == -1) break; currentLine >> token; if (bytesRead == 0 || token.size() == 0 || token[0] == '#') continue; if (_renamedTypesLookup && _renamedTypesLookup->size()>0){ map::const_iterator foundIt = _renamedTypesLookup->find(token); if (foundIt != _renamedTypesLookup->end()) { token = foundIt->second; } } HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset); if (! element) // not a parameter or otherwise unknown tag continue; assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param"); Parameter* p = static_cast(element); int pid; currentLine >> pid; p->setId(pid); bool r = p->read(currentLine); if (! r) { cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl; delete p; } else { if (! addParameter(p) ){ cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl; } } } // while read line return true; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/parameter_container.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_GRAPH_PARAMETER_CONTAINER_HH_ #define G2O_GRAPH_PARAMETER_CONTAINER_HH_ #include #include #include namespace g2o { class Parameter; /** * \brief map id to parameters */ class ParameterContainer : protected std::map { public: typedef std::map BaseClass; /** * create a container for the parameters. * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor */ ParameterContainer(bool isMainStorage_=true); virtual ~ParameterContainer(); //! add parameter to the container bool addParameter(Parameter* p); //! return a parameter based on its ID Parameter* getParameter(int id); //! remove a parameter from the container, i.e., the user now owns the pointer Parameter* detachParameter(int id); //! read parameters from a stream virtual bool read(std::istream& is, const std::map* renamedMap =0); //! write the data to a stream virtual bool write(std::ostream& os) const; bool isMainStorage() const {return _isMainStorage;} void clear(); // stuff of the base class that should re-appear using BaseClass::size; protected: bool _isMainStorage; }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/robust_kernel.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 "robust_kernel.h" namespace g2o { RobustKernel::RobustKernel() : _delta(1.) { } RobustKernel::RobustKernel(double delta) : _delta(delta) { } void RobustKernel::setDelta(double delta) { _delta = delta; } } // end namespace g2o ================================================ FILE: Thirdparty/g2o/g2o/core/robust_kernel.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_ROBUST_KERNEL_H #define G2O_ROBUST_KERNEL_H #ifdef _MSC_VER #include #else #include #endif #include namespace g2o { /** * \brief base for all robust cost functions * * Note in all the implementations for the other cost functions the e in the * funtions corresponds to the sqaured errors, i.e., the robustification * functions gets passed the squared error. * * e.g. the robustified least squares function is * * chi^2 = sum_{e} rho( e^T Omega e ) */ class RobustKernel { public: RobustKernel(); explicit RobustKernel(double delta); virtual ~RobustKernel() {} /** * compute the scaling factor for a error: * The error is e^T Omega e * The output rho is * rho[0]: The actual scaled error value * rho[1]: First derivative of the scaling function * rho[2]: Second derivative of the scaling function */ virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0; /** * set the window size of the error. A squared error above delta^2 is considered * as outlier in the data. */ virtual void setDelta(double delta); double delta() const { return _delta;} protected: double _delta; }; typedef std::tr1::shared_ptr RobustKernelPtr; } // end namespace g2o #endif ================================================ FILE: Thirdparty/g2o/g2o/core/robust_kernel_factory.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 "robust_kernel_factory.h" #include "robust_kernel.h" #include using namespace std; namespace g2o { RobustKernelFactory* RobustKernelFactory::factoryInstance = 0; RobustKernelFactory::RobustKernelFactory() { } RobustKernelFactory::~RobustKernelFactory() { for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) { delete it->second; } _creator.clear(); } RobustKernelFactory* RobustKernelFactory::instance() { if (factoryInstance == 0) { factoryInstance = new RobustKernelFactory; } return factoryInstance; } void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c) { CreatorMap::const_iterator foundIt = _creator.find(tag); if (foundIt != _creator.end()) { cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl; assert(0); } _creator[tag] = c; } void RobustKernelFactory::unregisterType(const std::string& tag) { CreatorMap::iterator tagPosition = _creator.find(tag); if (tagPosition != _creator.end()) { AbstractRobustKernelCreator* c = tagPosition->second; delete c; _creator.erase(tagPosition); } } RobustKernel* RobustKernelFactory::construct(const std::string& tag) const { CreatorMap::const_iterator foundIt = _creator.find(tag); if (foundIt != _creator.end()) { return foundIt->second->construct(); } return 0; } AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const { CreatorMap::const_iterator foundIt = _creator.find(tag); if (foundIt != _creator.end()) { return foundIt->second; } return 0; } void RobustKernelFactory::fillKnownKernels(std::vector& types) const { types.clear(); for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) types.push_back(it->first); } void RobustKernelFactory::destroy() { delete factoryInstance; factoryInstance = 0; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/robust_kernel_factory.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_ROBUST_KERNEL_FACTORY_H #define G2O_ROBUST_KERNEL_FACTORY_H #include "../stuff/misc.h" #include #include #include #include namespace g2o { class RobustKernel; /** * \brief Abstract interface for allocating a robust kernel */ class AbstractRobustKernelCreator { public: /** * create a hyper graph element. Has to implemented in derived class. */ virtual RobustKernel* construct() = 0; virtual ~AbstractRobustKernelCreator() { } }; /** * \brief templatized creator class which creates graph elements */ template class RobustKernelCreator : public AbstractRobustKernelCreator { public: RobustKernel* construct() { return new T;} }; /** * \brief create robust kernels based on their human readable name */ class RobustKernelFactory { public: //! return the instance static RobustKernelFactory* instance(); //! free the instance static void destroy(); /** * register a tag for a specific creator */ void registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c); /** * unregister a tag for a specific creator */ void unregisterType(const std::string& tag); /** * construct a robust kernel based on its tag */ RobustKernel* construct(const std::string& tag) const; /** * return the creator for a specific tag */ AbstractRobustKernelCreator* creator(const std::string& tag) const; /** * get a list of all known robust kernels */ void fillKnownKernels(std::vector& types) const; protected: typedef std::map CreatorMap; RobustKernelFactory(); ~RobustKernelFactory(); CreatorMap _creator; ///< look-up map for the existing creators private: static RobustKernelFactory* factoryInstance; }; template class RegisterRobustKernelProxy { public: RegisterRobustKernelProxy(const std::string& name) : _name(name) { RobustKernelFactory::instance()->registerRobustKernel(_name, new RobustKernelCreator()); } ~RegisterRobustKernelProxy() { RobustKernelFactory::instance()->unregisterType(_name); } private: std::string _name; }; #if defined _MSC_VER && defined G2O_SHARED_LIBS # define G2O_ROBUST_KERNEL_FACTORY_EXPORT __declspec(dllexport) # define G2O_ROBUST_KERNEL_FACTORY_IMPORT __declspec(dllimport) #else # define G2O_ROBUST_KERNEL_FACTORY_EXPORT # define G2O_ROBUST_KERNEL_FACTORY_IMPORT #endif // These macros are used to automate registering of robust kernels and forcing linkage #define G2O_REGISTER_ROBUST_KERNEL(name, classname) \ extern "C" void G2O_ROBUST_KERNEL_FACTORY_EXPORT g2o_robust_kernel_##classname(void) {} \ static g2o::RegisterRobustKernelProxy g_robust_kernel_proxy_##classname(#name); #define G2O_USE_ROBUST_KERNEL(classname) \ extern "C" void G2O_ROBUST_KERNEL_FACTORY_IMPORT g2o_robust_kernel_##classname(void); \ static g2o::TypeFunctionProxy proxy_##classname(g2o_robust_kernel_##classname); } // end namespace g2o #endif ================================================ FILE: Thirdparty/g2o/g2o/core/robust_kernel_impl.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 "robust_kernel_impl.h" #include "robust_kernel_factory.h" #include namespace g2o { RobustKernelScaleDelta::RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta) : RobustKernel(delta), _kernel(kernel) { } RobustKernelScaleDelta::RobustKernelScaleDelta(double delta) : RobustKernel(delta) { } void RobustKernelScaleDelta::setKernel(const RobustKernelPtr& ptr) { _kernel = ptr; } void RobustKernelScaleDelta::robustify(double error, Eigen::Vector3d& rho) const { if (_kernel.get()) { double dsqr = _delta * _delta; double dsqrReci = 1. / dsqr; _kernel->robustify(dsqrReci * error, rho); rho[0] *= dsqr; rho[2] *= dsqrReci; } else { // no robustification rho[0] = error; rho[1] = 1.; rho[2] = 0.; } } void RobustKernelHuber::setDelta(double delta) { dsqr = delta*delta; _delta = delta; } void RobustKernelHuber::setDeltaSqr(const double &delta, const double &deltaSqr) { dsqr = deltaSqr; _delta = delta; } void RobustKernelHuber::robustify(double e, Eigen::Vector3d& rho) const { //dsqr = _delta * _delta; if (e <= dsqr) { // inlier rho[0] = e; rho[1] = 1.; rho[2] = 0.; } else { // outlier double sqrte = sqrt(e); // absolut value of the error rho[0] = 2*sqrte*_delta - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2 rho[1] = _delta / sqrte; // rho'(e) = delta / sqrt(e) rho[2] = - 0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e } } void RobustKernelTukey::setDeltaSqr(const double &deltaSqr, const double &inv) { _deltaSqr = deltaSqr; _invDeltaSqr = inv; } void RobustKernelTukey::robustify(double e, Eigen::Vector3d& rho) const { if (e <= _deltaSqr) { // inlier double factor = e*_invDeltaSqr; double d = 1-factor; double dd = d*d; rho[0] = _deltaSqr*(1-dd*d); rho[1] = 3*dd; rho[2] = -6*_invDeltaSqr*d; } else { // outlier rho[0] = _deltaSqr; // rho(e) = delta^2 rho[1] = 0.; rho[2] = 0.; } } void RobustKernelPseudoHuber::robustify(double e2, Eigen::Vector3d& rho) const { double dsqr = _delta * _delta; double dsqrReci = 1. / dsqr; double aux1 = dsqrReci * e2 + 1.0; double aux2 = sqrt(aux1); rho[0] = 2 * dsqr * (aux2 - 1); rho[1] = 1. / aux2; rho[2] = -0.5 * dsqrReci * rho[1] / aux1; } void RobustKernelCauchy::robustify(double e2, Eigen::Vector3d& rho) const { double dsqr = _delta * _delta; double dsqrReci = 1. / dsqr; double aux = dsqrReci * e2 + 1.0; rho[0] = dsqr * log(aux); rho[1] = 1. / aux; rho[2] = -dsqrReci * std::pow(rho[1], 2); } void RobustKernelSaturated::robustify(double e2, Eigen::Vector3d& rho) const { double dsqr = _delta * _delta; if (e2 <= dsqr) { // inlier rho[0] = e2; rho[1] = 1.; rho[2] = 0.; } else { // outlier rho[0] = dsqr; rho[1] = 0.; rho[2] = 0.; } } //delta is used as $phi$ void RobustKernelDCS::robustify(double e2, Eigen::Vector3d& rho) const { const double& phi = _delta; double scale = (2.0*phi)/(phi+e2); if(scale>=1.0) scale = 1.0; rho[0] = scale*e2*scale; rho[1] = (scale*scale); rho[2] = 0; } // register the kernel to their factory G2O_REGISTER_ROBUST_KERNEL(Huber, RobustKernelHuber) G2O_REGISTER_ROBUST_KERNEL(Tukey, RobustKernelTukey) G2O_REGISTER_ROBUST_KERNEL(PseudoHuber, RobustKernelPseudoHuber) G2O_REGISTER_ROBUST_KERNEL(Cauchy, RobustKernelCauchy) G2O_REGISTER_ROBUST_KERNEL(Saturated, RobustKernelSaturated) G2O_REGISTER_ROBUST_KERNEL(DCS, RobustKernelDCS) } // end namespace g2o ================================================ FILE: Thirdparty/g2o/g2o/core/robust_kernel_impl.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_ROBUST_KERNEL_IMPL_H #define G2O_ROBUST_KERNEL_IMPL_H #include "robust_kernel.h" namespace g2o { /** * \brief scale a robust kernel to another delta (window size) * * Scales a robust kernel to another window size. Useful, in case if * one implements a kernel which only is designed for a fixed window * size. */ class RobustKernelScaleDelta : public RobustKernel { public: /** * construct the scaled kernel ontop of another kernel which might be shared accross * several scaled kernels */ explicit RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta = 1.); explicit RobustKernelScaleDelta(double delta = 1.); //! return the underlying kernel const RobustKernelPtr kernel() const { return _kernel;} //! use another kernel for the underlying operation void setKernel(const RobustKernelPtr& ptr); void robustify(double error, Eigen::Vector3d& rho) const; protected: RobustKernelPtr _kernel; }; /** * \brief Huber Cost Function * * Loss function as described by Huber * See http://en.wikipedia.org/wiki/Huber_loss_function * * If e^(1/2) < d * rho(e) = e * * else * * 1/2 2 * rho(e) = 2 d e - d */ class RobustKernelHuber : public RobustKernel { public: virtual void setDelta(double delta); virtual void setDeltaSqr(const double &delta, const double &deltaSqr); virtual void robustify(double e2, Eigen::Vector3d& rho) const; private: float dsqr; }; /** * \brief Tukey Cost Function * * * If e^(1/2) < d * rho(e) = delta2(1-(1-e/delta2)^3) * * else * * rho(e) = delta2 */ class RobustKernelTukey : public RobustKernel { public: virtual void setDeltaSqr(const double &deltaSqr, const double &inv); virtual void robustify(double e2, Eigen::Vector3d& rho) const; private: float _deltaSqr; float _invDeltaSqr; }; /** * \brief Pseudo Huber Cost Function * * The smooth pseudo huber cost function: * See http://en.wikipedia.org/wiki/Huber_loss_function * * 2 e * 2 d (sqrt(-- + 1) - 1) * 2 * d */ class RobustKernelPseudoHuber : public RobustKernel { public: virtual void robustify(double e2, Eigen::Vector3d& rho) const; }; /** * \brief Cauchy cost function * * 2 e * d log(-- + 1) * 2 * d */ class RobustKernelCauchy : public RobustKernel { public: virtual void robustify(double e2, Eigen::Vector3d& rho) const; }; /** * \brief Saturated cost function. * * The error is at most delta^2 */ class RobustKernelSaturated : public RobustKernel { public: virtual void robustify(double e2, Eigen::Vector3d& rho) const; }; /** * \brief Dynamic covariance scaling - DCS * * See paper Robust Map Optimization from Agarwal et al. ICRA 2013 * * delta is used as $phi$ */ class RobustKernelDCS : public RobustKernel { public: virtual void robustify(double e2, Eigen::Vector3d& rho) const; }; } // end namespace g2o #endif ================================================ FILE: Thirdparty/g2o/g2o/core/solver.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 "solver.h" #include #include namespace g2o { Solver::Solver() : _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0), _isLevenberg(false), _additionalVectorSpace(0) { } Solver::~Solver() { delete[] _x; delete[] _b; } void Solver::resizeVector(size_t sx) { size_t oldSize = _xSize; _xSize = sx; sx += _additionalVectorSpace; // allocate some additional space if requested if (_maxXSize < sx) { _maxXSize = 2*sx; delete[] _x; _x = new double[_maxXSize]; #ifndef NDEBUG memset(_x, 0, _maxXSize * sizeof(double)); #endif if (_b) { // backup the former b, might still be needed for online processing memcpy(_x, _b, oldSize * sizeof(double)); delete[] _b; _b = new double[_maxXSize]; std::swap(_b, _x); } else { _b = new double[_maxXSize]; #ifndef NDEBUG memset(_b, 0, _maxXSize * sizeof(double)); #endif } } } void Solver::setOptimizer(SparseOptimizer* optimizer) { _optimizer = optimizer; } void Solver::setLevenberg(bool levenberg) { _isLevenberg = levenberg; } void Solver::setAdditionalVectorSpace(size_t s) { _additionalVectorSpace = s; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/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_SOLVER_H #define G2O_SOLVER_H #include "hyper_graph.h" #include "batch_stats.h" #include "sparse_block_matrix.h" #include namespace g2o { class SparseOptimizer; /** * \brief Generic interface for a sparse solver operating on a graph which solves one iteration of the linearized objective function */ class Solver { public: Solver(); virtual ~Solver(); public: /** * initialize the solver, called once before the first iteration */ virtual bool init(SparseOptimizer* optimizer, bool online = false) = 0; /** * build the structure of the system */ virtual bool buildStructure(bool zeroBlocks = false) = 0; /** * update the structures for online processing */ virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0; /** * build the current system */ virtual bool buildSystem() = 0; /** * solve Ax = b */ virtual bool solve() = 0; /** * computes the block diagonal elements of the pattern specified in the input * and stores them in given SparseBlockMatrix */ virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0; /** * update the system while performing Levenberg, i.e., modifying the diagonal * components of A by doing += lambda along the main diagonal of the Matrix. * Note that this function may be called with a positive and a negative lambda. * The latter is used to undo a former modification. * If backup is true, then the solver should store a backup of the diagonal, which * can be restored by restoreDiagonal() */ virtual bool setLambda(double lambda, bool backup = false) = 0; /** * restore a previosly made backup of the diagonal */ virtual void restoreDiagonal() = 0; //! return x, the solution vector double* x() { return _x;} const double* x() const { return _x;} //! return b, the right hand side of the system double* b() { return _b;} const double* b() const { return _b;} //! return the size of the solution vector (x) and b size_t vectorSize() const { return _xSize;} //! the optimizer (graph) on which the solver works SparseOptimizer* optimizer() const { return _optimizer;} void setOptimizer(SparseOptimizer* optimizer); //! the system is Levenberg-Marquardt bool levenberg() const { return _isLevenberg;} void setLevenberg(bool levenberg); /** * does this solver support the Schur complement for solving a system consisting of poses and * landmarks. Re-implemement in a derived solver, if your solver supports it. */ virtual bool supportsSchur() {return false;} //! should the solver perform the schur complement or not virtual bool schur()=0; virtual void setSchur(bool s)=0; size_t additionalVectorSpace() const { return _additionalVectorSpace;} void setAdditionalVectorSpace(size_t s); /** * write debug output of the Hessian if system is not positive definite */ virtual void setWriteDebug(bool) = 0; virtual bool writeDebug() const = 0; //! write the hessian to disk using the specified file name virtual bool saveHessian(const std::string& /*fileName*/) const = 0; protected: SparseOptimizer* _optimizer; double* _x; double* _b; size_t _xSize, _maxXSize; bool _isLevenberg; ///< the system we gonna solve is a Levenberg-Marquardt system size_t _additionalVectorSpace; void resizeVector(size_t sx); private: // Disable the copy constructor and assignment operator Solver(const Solver&) { } Solver& operator= (const Solver&) { return *this; } }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix.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_SPARSE_BLOCK_MATRIX_ #define G2O_SPARSE_BLOCK_MATRIX_ #include #include #include #include #include #include #include #include "sparse_block_matrix_ccs.h" #include "matrix_structure.h" #include "matrix_operations.h" #include "../../config.h" namespace g2o { using namespace Eigen; /** * \brief Sparse matrix which uses blocks * * Template class that specifies a sparse block matrix. A block * matrix is a sparse matrix made of dense blocks. These blocks * cannot have a random pattern, but follow a (variable) grid * structure. This structure is specified by a partition of the rows * and the columns of the matrix. The blocks are represented by the * Eigen::Matrix structure, thus they can be statically or dynamically * allocated. For efficiency reasons it is convenient to allocate them * statically, when possible. A static block matrix has all blocks of * the same size, and the size of the block is specified by the * template argument. If this is not the case, and you have different * block sizes than you have to use a dynamic-block matrix (default * template argument). */ template class SparseBlockMatrix { public: //! this is the type of the elementary block, it is an Eigen::Matrix. typedef MatrixType SparseMatrixBlock; //! columns of the matrix inline int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;} //! rows of the matrix inline int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;} typedef std::map IntBlockMap; /** * constructs a sparse block matrix having a specific layout * @param rbi: array of int containing the row layout of the blocks. * the component i of the array should contain the index of the first row of the block i+1. * @param rbi: array of int containing the column layout of the blocks. * the component i of the array should contain the index of the first col of the block i+1. * @param rb: number of row blocks * @param cb: number of col blocks * @param hasStorage: set it to true if the matrix "owns" the blocks, thus it deletes it on destruction. * if false the matrix is only a "view" over an existing structure. */ SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true); SparseBlockMatrix(); ~SparseBlockMatrix(); //! this zeroes all the blocks. If dealloc=true the blocks are removed from memory void clear(bool dealloc=false) ; //! returns the block at location r,c. if alloc=true he block is created if it does not exist SparseMatrixBlock* block(int r, int c, bool alloc=false); //! returns the block at location r,c const SparseMatrixBlock* block(int r, int c) const; //! how many rows does the block at block-row r has? inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; } //! how many cols does the block at block-col c has? inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; } //! where does the row at block-row r starts? inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; } //! where does the col at block-col r starts? inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; } //! number of non-zero elements size_t nonZeros() const; //! number of allocated blocks size_t nonZeroBlocks() const; //! deep copy of a sparse-block-matrix; SparseBlockMatrix* clone() const ; /** * returns a view or a copy of the block matrix * @param rmin: starting block row * @param rmax: ending block row * @param cmin: starting block col * @param cmax: ending block col * @param alloc: if true it makes a deep copy, if false it creates a view. */ SparseBlockMatrix* slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const; //! transposes a block matrix, The transposed type should match the argument false on failure template bool transpose(SparseBlockMatrix*& dest) const; //! adds the current matrix to the destination bool add(SparseBlockMatrix*& dest) const ; //! dest = (*this) * M template bool multiply(SparseBlockMatrix *& dest, const SparseBlockMatrix* M) const; //! dest = (*this) * src void multiply(double*& dest, const double* src) const; /** * compute dest = (*this) * src * However, assuming that this is a symmetric matrix where only the upper triangle is stored */ void multiplySymmetricUpperTriangle(double*& dest, const double* src) const; //! dest = M * (*this) void rightMultiply(double*& dest, const double* src) const; //! *this *= a void scale( double a); /** * writes in dest a block permutaton specified by pinv. * @param pinv: array such that new_block[i] = old_block[pinv[i]] */ bool symmPermutation(SparseBlockMatrix*& dest, const int* pinv, bool onlyUpper=false) const; /** * fill the CCS arrays of a matrix, arrays have to be allocated beforehand */ int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const; /** * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes * the values and assumes that column and row structures have already been written. */ int fillCCS(double* Cx, bool upperTriangle = false) const; //! exports the non zero blocks in the structure matrix ms void fillBlockStructure(MatrixStructure& ms) const; //! the block matrices per block-column const std::vector& blockCols() const { return _blockCols;} std::vector& blockCols() { return _blockCols;} //! indices of the row blocks const std::vector& rowBlockIndices() const { return _rowBlockIndices;} std::vector& rowBlockIndices() { return _rowBlockIndices;} //! indices of the column blocks const std::vector& colBlockIndices() const { return _colBlockIndices;} std::vector& colBlockIndices() { return _colBlockIndices;} /** * write the content of this matrix to a stream loadable by Octave * @param upperTriangle does this matrix store only the upper triangular blocks */ bool writeOctave(const char* filename, bool upperTriangle = true) const; /** * copy into CCS structure * @return number of processed blocks, -1 on error */ int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const; /** * copy as transposed into a CCS structure * @return number of processed blocks, -1 on error */ int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS& blockCCS) const; /** * take over the memory and matrix pattern from a hash matrix. * The structure of the hash matrix will be cleared. */ void takePatternFromHash(SparseBlockMatrixHashMap& hashMatrix); protected: std::vector _rowBlockIndices; ///< vector of the indices of the blocks along the rows. std::vector _colBlockIndices; ///< vector of the indices of the blocks along the cols //! array of maps of blocks. The index of the array represent a block column of the matrix //! and the block column is stored as a map row_block -> matrix_block_ptr. std::vector _blockCols; bool _hasStorage; }; template < class MatrixType > std::ostream& operator << (std::ostream&, const SparseBlockMatrix& m); typedef SparseBlockMatrix SparseBlockMatrixXd; } //end namespace #include "sparse_block_matrix.hpp" #endif ================================================ FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix.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. namespace g2o { using namespace Eigen; namespace { struct TripletEntry { int r, c; double x; TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {} }; struct TripletColSort { bool operator()(const TripletEntry& e1, const TripletEntry& e2) const { return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r); } }; /** Helper class to sort pair based on first elem */ template > struct CmpPairFirst { bool operator()(const std::pair& left, const std::pair& right) { return Pred()(left.first, right.first); } }; } template SparseBlockMatrix::SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage): _rowBlockIndices(rbi,rbi+rb), _colBlockIndices(cbi,cbi+cb), _blockCols(cb), _hasStorage(hasStorage) { } template SparseBlockMatrix::SparseBlockMatrix( ): _blockCols(0), _hasStorage(true) { } template void SparseBlockMatrix::clear(bool dealloc) { # ifdef G2O_OPENMP # pragma omp parallel for default (shared) if (_blockCols.size() > 100) # endif for (int i=0; i < static_cast(_blockCols.size()); ++i) { for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; if (_hasStorage && dealloc) delete b; else b->setZero(); } if (_hasStorage && dealloc) _blockCols[i].clear(); } } template SparseBlockMatrix::~SparseBlockMatrix(){ if (_hasStorage) clear(true); } template typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::block(int r, int c, bool alloc) { typename SparseBlockMatrix::IntBlockMap::iterator it =_blockCols[c].find(r); typename SparseBlockMatrix::SparseMatrixBlock* _block=0; if (it==_blockCols[c].end()){ if (!_hasStorage && ! alloc ) return 0; else { int rb=rowsOfBlock(r); int cb=colsOfBlock(c); _block=new typename SparseBlockMatrix::SparseMatrixBlock(rb,cb); _block->setZero(); std::pair < typename SparseBlockMatrix::IntBlockMap::iterator, bool> result =_blockCols[c].insert(std::make_pair(r,_block)); (void) result; assert (result.second); } } else { _block=it->second; } return _block; } template const typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::block(int r, int c) const { typename SparseBlockMatrix::IntBlockMap::const_iterator it =_blockCols[c].find(r); if (it==_blockCols[c].end()) return 0; return it->second; } template SparseBlockMatrix* SparseBlockMatrix::clone() const { SparseBlockMatrix* ret= new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size()); for (size_t i=0; i<_blockCols.size(); ++i){ for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ typename SparseBlockMatrix::SparseMatrixBlock* b=new typename SparseBlockMatrix::SparseMatrixBlock(*it->second); ret->_blockCols[i].insert(std::make_pair(it->first, b)); } } ret->_hasStorage=true; return ret; } template template bool SparseBlockMatrix::transpose(SparseBlockMatrix*& dest) const { if (! dest){ dest=new SparseBlockMatrix(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size()); } else { if (! dest->_hasStorage) return false; if(_rowBlockIndices.size()!=dest->_colBlockIndices.size()) return false; if (_colBlockIndices.size()!=dest->_rowBlockIndices.size()) return false; for (size_t i=0; i<_rowBlockIndices.size(); ++i){ if(_rowBlockIndices[i]!=dest->_colBlockIndices[i]) return false; } for (size_t i=0; i<_colBlockIndices.size(); ++i){ if(_colBlockIndices[i]!=dest->_rowBlockIndices[i]) return false; } } for (size_t i=0; i<_blockCols.size(); ++i){ for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ typename SparseBlockMatrix::SparseMatrixBlock* s=it->second; typename SparseBlockMatrix::SparseMatrixBlock* d=dest->block(i,it->first,true); *d = s->transpose(); } } return true; } template bool SparseBlockMatrix::add(SparseBlockMatrix*& dest) const { if (! dest){ dest=new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size()); } else { if (! dest->_hasStorage) return false; if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size()) return false; if (_colBlockIndices.size()!=dest->_colBlockIndices.size()) return false; for (size_t i=0; i<_rowBlockIndices.size(); ++i){ if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i]) return false; } for (size_t i=0; i<_colBlockIndices.size(); ++i){ if(_colBlockIndices[i]!=dest->_colBlockIndices[i]) return false; } } for (size_t i=0; i<_blockCols.size(); ++i){ for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ typename SparseBlockMatrix::SparseMatrixBlock* s=it->second; typename SparseBlockMatrix::SparseMatrixBlock* d=dest->block(it->first,i,true); (*d)+=*s; } } return true; } template template < class MatrixResultType, class MatrixFactorType > bool SparseBlockMatrix::multiply(SparseBlockMatrix*& dest, const SparseBlockMatrix * M) const { // sanity check if (_colBlockIndices.size()!=M->_rowBlockIndices.size()) return false; for (size_t i=0; i<_colBlockIndices.size(); ++i){ if (_colBlockIndices[i]!=M->_rowBlockIndices[i]) return false; } if (! dest) { dest=new SparseBlockMatrix(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() ); } if (! dest->_hasStorage) return false; for (size_t i=0; i_blockCols.size(); ++i){ for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){ // look for a non-zero block in a row of column it int colM=i; const typename SparseBlockMatrix::SparseMatrixBlock *b=it->second; typename SparseBlockMatrix::IntBlockMap::const_iterator rbt=_blockCols[it->first].begin(); while(rbt!=_blockCols[it->first].end()){ //int colA=it->first; int rowA=rbt->first; typename SparseBlockMatrix::SparseMatrixBlock *a=rbt->second; typename SparseBlockMatrix::SparseMatrixBlock *c=dest->block(rowA,colM,true); assert (c->rows()==a->rows()); assert (c->cols()==b->cols()); ++rbt; (*c)+=(*a)*(*b); } } } return false; } template void SparseBlockMatrix::multiply(double*& dest, const double* src) const { if (! dest){ dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ]; memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, rows()); const Eigen::Map srcVec(src, cols()); for (size_t i=0; i<_blockCols.size(); ++i){ int srcOffset = i ? _colBlockIndices[i-1] : 0; for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0; // destVec += *a * srcVec (according to the sub-vector parts) internal::axpy(*a, srcVec, srcOffset, destVec, destOffset); } } } template void SparseBlockMatrix::multiplySymmetricUpperTriangle(double*& dest, const double* src) const { if (! dest){ dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ]; memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, rows()); const Eigen::Map srcVec(src, cols()); for (size_t i=0; i<_blockCols.size(); ++i){ int srcOffset = colBaseOfBlock(i); for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; int destOffset = rowBaseOfBlock(it->first); if (destOffset > srcOffset) // only upper triangle break; // destVec += *a * srcVec (according to the sub-vector parts) internal::axpy(*a, srcVec, srcOffset, destVec, destOffset); if (destOffset < srcOffset) internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset); } } } template void SparseBlockMatrix::rightMultiply(double*& dest, const double* src) const { int destSize=cols(); if (! dest){ dest=new double [ destSize ]; memset(dest,0, destSize*sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, destSize); Eigen::Map srcVec(src, rows()); # ifdef G2O_OPENMP # pragma omp parallel for default (shared) schedule(dynamic, 10) # endif for (int i=0; i < static_cast(_blockCols.size()); ++i){ int destOffset = colBaseOfBlock(i); for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; int srcOffset = rowBaseOfBlock(it->first); // destVec += *a.transpose() * srcVec (according to the sub-vector parts) internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset); } } } template void SparseBlockMatrix::scale(double a_) { for (size_t i=0; i<_blockCols.size(); ++i){ for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; *a *= a_; } } } template SparseBlockMatrix* SparseBlockMatrix::slice(int rmin, int rmax, int cmin, int cmax, bool alloc) const { int m=rmax-rmin; int n=cmax-cmin; int rowIdx [m]; rowIdx[0] = rowsOfBlock(rmin); for (int i=1; i::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true); for (int i=0; i::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){ if (it->first >= rmin && it->first < rmax){ typename SparseBlockMatrix::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix::SparseMatrixBlock (* (it->second) ) : it->second; s->_blockCols[i].insert(std::make_pair(it->first-rmin, b)); } } } s->_hasStorage=alloc; return s; } template size_t SparseBlockMatrix::nonZeroBlocks() const { size_t count=0; for (size_t i=0; i<_blockCols.size(); ++i) count+=_blockCols[i].size(); return count; } template size_t SparseBlockMatrix::nonZeros() const{ if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) { size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime; return nnz; } else { size_t count=0; for (size_t i=0; i<_blockCols.size(); ++i){ for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second; count += a->cols()*a->rows(); } } return count; } } template std::ostream& operator << (std::ostream& os, const SparseBlockMatrix& m){ os << "RBI: " << m.rowBlockIndices().size(); for (size_t i=0; i::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){ const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; os << "BLOCK: " << it->first << " " << i << std::endl; os << *b << std::endl; } } return os; } template bool SparseBlockMatrix::symmPermutation(SparseBlockMatrix*& dest, const int* pinv, bool upperTriangle) const{ // compute the permuted version of the new row/column layout size_t n=_rowBlockIndices.size(); // computed the block sizes int blockSizes[_rowBlockIndices.size()]; blockSizes[0]=_rowBlockIndices[0]; for (size_t i=1; i_rowBlockIndices.size()!=n) return false; if (dest->_colBlockIndices.size()!=n) return false; for (size_t i=0; i_rowBlockIndices[i]!=pBlockIndices[i]) return false; if (dest->_colBlockIndices[i]!=pBlockIndices[i]) return false; } dest->clear(); } // now ready to permute the columns for (size_t i=0; i::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ int pj=pinv[it->first]; const typename SparseBlockMatrix::SparseMatrixBlock* s=it->second; typename SparseBlockMatrix::SparseMatrixBlock* b=0; if (! upperTriangle || pj<=pi) { b=dest->block(pj,pi,true); assert(b->cols()==s->cols()); assert(b->rows()==s->rows()); *b=*s; } else { b=dest->block(pi,pj,true); assert(b); assert(b->rows()==s->cols()); assert(b->cols()==s->rows()); *b=s->transpose(); } } //cerr << endl; // within each row, } return true; } template int SparseBlockMatrix::fillCCS(double* Cx, bool upperTriangle) const { assert(Cx && "Target destination is NULL"); double* CxStart = Cx; for (size_t i=0; i<_blockCols.size(); ++i){ int cstart=i ? _colBlockIndices[i-1] : 0; int csize=colsOfBlock(i); for (int c=0; c::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; int rstart=it->first ? _rowBlockIndices[it->first-1] : 0; int elemsToCopy = b->rows(); if (upperTriangle && rstart == cstart) elemsToCopy = c + 1; memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double)); Cx += elemsToCopy; } } } return Cx - CxStart; } template int SparseBlockMatrix::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const { assert(Cp && Ci && Cx && "Target destination is NULL"); int nz=0; for (size_t i=0; i<_blockCols.size(); ++i){ int cstart=i ? _colBlockIndices[i-1] : 0; int csize=colsOfBlock(i); for (int c=0; c::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){ const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second; int rstart=it->first ? _rowBlockIndices[it->first-1] : 0; int elemsToCopy = b->rows(); if (upperTriangle && rstart == cstart) elemsToCopy = c + 1; for (int r=0; r void SparseBlockMatrix::fillBlockStructure(MatrixStructure& ms) const { int n = _colBlockIndices.size(); int nzMax = (int)nonZeroBlocks(); ms.alloc(n, nzMax); ms.m = _rowBlockIndices.size(); int nz = 0; int* Cp = ms.Ap; int* Ci = ms.Aii; for (int i = 0; i < static_cast(_blockCols.size()); ++i){ *Cp = nz; const int& c = i; for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) { const int& r = it->first; if (r <= c) { *Ci++ = r; ++nz; } } Cp++; } *Cp=nz; assert(nz <= nzMax); } template bool SparseBlockMatrix::writeOctave(const char* filename, bool upperTriangle) const { std::string name = filename; std::string::size_type lastDot = name.find_last_of('.'); if (lastDot != std::string::npos) name = name.substr(0, lastDot); std::vector entries; for (size_t i = 0; i<_blockCols.size(); ++i){ const int& c = i; for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) { const int& r = it->first; const MatrixType& m = *(it->second); for (int cc = 0; cc < m.cols(); ++cc) for (int rr = 0; rr < m.rows(); ++rr) { int aux_r = rowBaseOfBlock(r) + rr; int aux_c = colBaseOfBlock(c) + cc; entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc))); if (upperTriangle && r != c) { entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc))); } } } } int nz = entries.size(); std::sort(entries.begin(), entries.end(), TripletColSort()); std::ofstream fout(filename); fout << "# name: " << name << std::endl; fout << "# type: sparse matrix" << std::endl; fout << "# nnz: " << nz << std::endl; fout << "# rows: " << rows() << std::endl; fout << "# columns: " << cols() << std::endl; fout << std::setprecision(9) << std::fixed << std::endl; for (std::vector::const_iterator it = entries.begin(); it != entries.end(); ++it) { const TripletEntry& entry = *it; fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl; } return fout.good(); } template int SparseBlockMatrix::fillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const { blockCCS.blockCols().resize(blockCols().size()); int numblocks = 0; for (size_t i = 0; i < blockCols().size(); ++i) { const IntBlockMap& row = blockCols()[i]; typename SparseBlockMatrixCCS::SparseColumn& dest = blockCCS.blockCols()[i]; dest.clear(); dest.reserve(row.size()); for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) { dest.push_back(typename SparseBlockMatrixCCS::RowBlock(it->first, it->second)); ++numblocks; } } return numblocks; } template int SparseBlockMatrix::fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS& blockCCS) const { blockCCS.blockCols().clear(); blockCCS.blockCols().resize(_rowBlockIndices.size()); int numblocks = 0; for (size_t i = 0; i < blockCols().size(); ++i) { const IntBlockMap& row = blockCols()[i]; for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) { typename SparseBlockMatrixCCS::SparseColumn& dest = blockCCS.blockCols()[it->first]; dest.push_back(typename SparseBlockMatrixCCS::RowBlock(i, it->second)); ++numblocks; } } return numblocks; } template void SparseBlockMatrix::takePatternFromHash(SparseBlockMatrixHashMap& hashMatrix) { // sort the sparse columns and add them to the map structures by // exploiting that we are inserting a sorted structure typedef std::pair SparseColumnPair; typedef typename SparseBlockMatrixHashMap::SparseColumn HashSparseColumn; for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) { // prepare a temporary vector for sorting HashSparseColumn& column = hashMatrix.blockCols()[i]; if (column.size() == 0) continue; std::vector sparseRowSorted; // temporary structure sparseRowSorted.reserve(column.size()); for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it) sparseRowSorted.push_back(*it); std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst()); // try to free some memory early HashSparseColumn aux; swap(aux, column); // now insert sorted vector to the std::map structure IntBlockMap& destColumnMap = blockCols()[i]; destColumnMap.insert(sparseRowSorted[0]); for (size_t j = 1; j < sparseRowSorted.size(); ++j) { typename SparseBlockMatrix::IntBlockMap::iterator hint = destColumnMap.end(); --hint; // cppreference says the element goes after the hint (until C++11) destColumnMap.insert(hint, sparseRowSorted[j]); } } } }// end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.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_SPARSE_BLOCK_MATRIX_CCS_H #define G2O_SPARSE_BLOCK_MATRIX_CCS_H #include #include #include #include "../../config.h" #include "matrix_operations.h" #ifdef _MSC_VER #include #else #include #endif namespace g2o { /** * \brief Sparse matrix which uses blocks * * This class is used as a const view on a SparseBlockMatrix * which allows a faster iteration over the elements of the * matrix. */ template class SparseBlockMatrixCCS { public: //! this is the type of the elementary block, it is an Eigen::Matrix. typedef MatrixType SparseMatrixBlock; //! columns of the matrix int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;} //! rows of the matrix int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;} /** * \brief A block within a column */ struct RowBlock { int row; ///< row of the block MatrixType* block; ///< matrix pointer for the block RowBlock() : row(-1), block(0) {} RowBlock(int r, MatrixType* b) : row(r), block(b) {} bool operator<(const RowBlock& other) const { return row < other.row;} }; typedef std::vector SparseColumn; SparseBlockMatrixCCS(const std::vector& rowIndices, const std::vector& colIndices) : _rowBlockIndices(rowIndices), _colBlockIndices(colIndices) {} //! how many rows does the block at block-row r has? int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; } //! how many cols does the block at block-col c has? int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; } //! where does the row at block-row r start? int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; } //! where does the col at block-col r start? int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; } //! the block matrices per block-column const std::vector& blockCols() const { return _blockCols;} std::vector& blockCols() { return _blockCols;} //! indices of the row blocks const std::vector& rowBlockIndices() const { return _rowBlockIndices;} //! indices of the column blocks const std::vector& colBlockIndices() const { return _colBlockIndices;} void rightMultiply(double*& dest, const double* src) const { int destSize=cols(); if (! dest){ dest=new double [ destSize ]; memset(dest,0, destSize*sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, destSize); Eigen::Map srcVec(src, rows()); # ifdef G2O_OPENMP # pragma omp parallel for default (shared) schedule(dynamic, 10) # endif for (int i=0; i < static_cast(_blockCols.size()); ++i){ int destOffset = colBaseOfBlock(i); for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) { const SparseMatrixBlock* a = it->block; int srcOffset = rowBaseOfBlock(it->row); // destVec += *a.transpose() * srcVec (according to the sub-vector parts) internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset); } } } /** * sort the blocks in each column */ void sortColumns() { for (int i=0; i < static_cast(_blockCols.size()); ++i){ std::sort(_blockCols[i].begin(), _blockCols[i].end()); } } /** * fill the CCS arrays of a matrix, arrays have to be allocated beforehand */ int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const { assert(Cp && Ci && Cx && "Target destination is NULL"); int nz=0; for (size_t i=0; i<_blockCols.size(); ++i){ int cstart=i ? _colBlockIndices[i-1] : 0; int csize=colsOfBlock(i); for (int c=0; cblock; int rstart=it->row ? _rowBlockIndices[it->row-1] : 0; int elemsToCopy = b->rows(); if (upperTriangle && rstart == cstart) elemsToCopy = c + 1; for (int r=0; rblock; int rstart = it->row ? _rowBlockIndices[it->row-1] : 0; int elemsToCopy = b->rows(); if (upperTriangle && rstart == cstart) elemsToCopy = c + 1; memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double)); Cx += elemsToCopy; } } cstart = _colBlockIndices[i]; } return Cx - CxStart; } protected: const std::vector& _rowBlockIndices; ///< vector of the indices of the blocks along the rows. const std::vector& _colBlockIndices; ///< vector of the indices of the blocks along the cols std::vector _blockCols; ///< the matrices stored in CCS order }; /** * \brief Sparse matrix which uses blocks based on hash structures * * This class is used to construct the pattern of a sparse block matrix */ template class SparseBlockMatrixHashMap { public: //! this is the type of the elementary block, it is an Eigen::Matrix. typedef MatrixType SparseMatrixBlock; //! columns of the matrix int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;} //! rows of the matrix int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;} typedef std::tr1::unordered_map SparseColumn; SparseBlockMatrixHashMap(const std::vector& rowIndices, const std::vector& colIndices) : _rowBlockIndices(rowIndices), _colBlockIndices(colIndices) {} //! how many rows does the block at block-row r has? int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; } //! how many cols does the block at block-col c has? int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; } //! where does the row at block-row r start? int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; } //! where does the col at block-col r start? int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; } //! the block matrices per block-column const std::vector& blockCols() const { return _blockCols;} std::vector& blockCols() { return _blockCols;} //! indices of the row blocks const std::vector& rowBlockIndices() const { return _rowBlockIndices;} //! indices of the column blocks const std::vector& colBlockIndices() const { return _colBlockIndices;} /** * add a block to the pattern, return a pointer to the added block */ MatrixType* addBlock(int r, int c, bool zeroBlock = false) { assert(c <(int)_blockCols.size() && "accessing column which is not available"); SparseColumn& sparseColumn = _blockCols[c]; typename SparseColumn::iterator foundIt = sparseColumn.find(r); if (foundIt == sparseColumn.end()) { int rb = rowsOfBlock(r); int cb = colsOfBlock(c); MatrixType* m = new MatrixType(rb, cb); if (zeroBlock) m->setZero(); sparseColumn[r] = m; return m; } return foundIt->second; } protected: const std::vector& _rowBlockIndices; ///< vector of the indices of the blocks along the rows. const std::vector& _colBlockIndices; ///< vector of the indices of the blocks along the cols std::vector _blockCols; ///< the matrices stored in CCS order }; } //end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.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_SPARSE_BLOCK_MATRIX_DIAGONAL_H #define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H #include #include #include #include "../../config.h" #include "matrix_operations.h" namespace g2o { /** * \brief Sparse matrix which uses blocks on the diagonal * * This class is used as a const view on a SparseBlockMatrix * which allows a faster iteration over the elements of the * matrix. */ template class SparseBlockMatrixDiagonal { public: //! this is the type of the elementary block, it is an Eigen::Matrix. typedef MatrixType SparseMatrixBlock; //! columns of the matrix int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;} //! rows of the matrix int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;} typedef std::vector > DiagonalVector; SparseBlockMatrixDiagonal(const std::vector& blockIndices) : _blockIndices(blockIndices) {} //! how many rows/cols does the block at block-row / block-column r has? inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; } //! where does the row /col at block-row / block-column r starts? inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; } //! the block matrices per block-column const DiagonalVector& diagonal() const { return _diagonal;} DiagonalVector& diagonal() { return _diagonal;} //! indices of the row blocks const std::vector& blockIndices() const { return _blockIndices;} void multiply(double*& dest, const double* src) const { int destSize=cols(); if (! dest) { dest=new double[destSize]; memset(dest,0, destSize*sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, destSize); Eigen::Map srcVec(src, rows()); # ifdef G2O_OPENMP # pragma omp parallel for default (shared) schedule(dynamic, 10) # endif for (int i=0; i < static_cast(_diagonal.size()); ++i){ int destOffset = baseOfBlock(i); int srcOffset = destOffset; const SparseMatrixBlock& A = _diagonal[i]; // destVec += *A.transpose() * srcVec (according to the sub-vector parts) internal::axpy(A, srcVec, srcOffset, destVec, destOffset); } } protected: const std::vector& _blockIndices; ///< vector of the indices of the blocks along the diagonal DiagonalVector _diagonal; }; } //end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/core/sparse_block_matrix_test.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 "sparse_block_matrix.h" #include using namespace std; using namespace g2o; using namespace Eigen; typedef SparseBlockMatrix< MatrixXd > SparseBlockMatrixX; std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) { for (int i=0; iblock(0,0, true); cerr << b->rows() << " " << b->cols() << endl; for (int i=0; irows(); ++i) for (int j=0; jcols(); ++j){ (*b)(i,j)=i*b->cols()+j; } cerr << "block access 2" << endl; b=M->block(0,2, true); cerr << b->rows() << " " << b->cols() << endl; for (int i=0; irows(); ++i) for (int j=0; jcols(); ++j){ (*b)(i,j)=i*b->cols()+j; } b=M->block(3,2, true); cerr << b->rows() << " " << b->cols() << endl; for (int i=0; irows(); ++i) for (int j=0; jcols(); ++j){ (*b)(i,j)=i*b->cols()+j; } cerr << *M << endl; cerr << "SUM" << endl; SparseBlockMatrixX* Ms=0; M->add(Ms); M->add(Ms); cerr << *Ms; SparseBlockMatrixX* Mt=0; M->transpose(Mt); cerr << *Mt << endl; SparseBlockMatrixX* Mp=0; M->multiply(Mp, Mt); cerr << *Mp << endl; int iperm[]={3,2,1,0}; SparseBlockMatrixX* PMp=0; Mp->symmPermutation(PMp,iperm, false); cerr << *PMp << endl; PMp->clear(true); Mp->block(3,0)->fill(0.); Mp->symmPermutation(PMp,iperm, true); cerr << *PMp << endl; } ================================================ FILE: Thirdparty/g2o/g2o/core/sparse_optimizer.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 "sparse_optimizer.h" #include #include #include #include #include #include #include "estimate_propagator.h" #include "optimization_algorithm.h" #include "batch_stats.h" #include "hyper_graph_action.h" #include "robust_kernel.h" #include "../stuff/timeutil.h" #include "../stuff/macros.h" #include "../stuff/misc.h" #include "../../config.h" namespace g2o{ using namespace std; SparseOptimizer::SparseOptimizer() : _forceStopFlag(0), _verbose(false), _algorithm(0), _computeBatchStatistics(false) { _graphActions.resize(AT_NUM_ELEMENTS); } SparseOptimizer::~SparseOptimizer(){ delete _algorithm; G2OBatchStatistics::setGlobalStats(0); } void SparseOptimizer::computeActiveErrors() { // call the callbacks in case there is something registered HyperGraphActionSet& actions = _graphActions[AT_COMPUTEACTIVERROR]; if (actions.size() > 0) { for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) (*(*it))(this); } # ifdef G2O_OPENMP # pragma omp parallel for default (shared) if (_activeEdges.size() > 50) # endif for (int k = 0; k < static_cast(_activeEdges.size()); ++k) { OptimizableGraph::Edge* e = _activeEdges[k]; e->computeError(); } # ifndef NDEBUG for (int k = 0; k < static_cast(_activeEdges.size()); ++k) { OptimizableGraph::Edge* e = _activeEdges[k]; bool hasNan = arrayHasNaN(e->errorData(), e->dimension()); if (hasNan) { cerr << "computeActiveErrors(): found NaN in error for edge " << e << endl; } } # endif } double SparseOptimizer::activeChi2( ) const { double chi = 0.0; for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) { const OptimizableGraph::Edge* e = *it; chi += e->chi2(); } return chi; } double SparseOptimizer::activeRobustChi2() const { Eigen::Vector3d rho; double chi = 0.0; for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) { const OptimizableGraph::Edge* e = *it; if (e->robustKernel()) { e->robustKernel()->robustify(e->chi2(), rho); chi += rho[0]; } else chi += e->chi2(); } return chi; } OptimizableGraph::Vertex* SparseOptimizer::findGauge(){ if (vertices().empty()) return 0; int maxDim=0; for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ OptimizableGraph::Vertex* v=static_cast(it->second); maxDim=std::max(maxDim,v->dimension()); } OptimizableGraph::Vertex* rut=0; for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ OptimizableGraph::Vertex* v=static_cast(it->second); if (v->dimension()==maxDim){ rut=v; break; } } return rut; } bool SparseOptimizer::gaugeFreedom() { if (vertices().empty()) return false; int maxDim=0; for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ OptimizableGraph::Vertex* v=static_cast(it->second); maxDim = std::max(maxDim,v->dimension()); } for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){ OptimizableGraph::Vertex* v=static_cast(it->second); if (v->dimension() == maxDim) { // test for fixed vertex if (v->fixed()) { return false; } // test for full dimension prior for (HyperGraph::EdgeSet::const_iterator eit = v->edges().begin(); eit != v->edges().end(); ++eit) { OptimizableGraph::Edge* e = static_cast(*eit); if (e->vertices().size() == 1 && e->dimension() == maxDim) return false; } } } return true; } bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){ if (! vlist.size()){ _ivMap.clear(); return false; } _ivMap.resize(vlist.size()); size_t i = 0; for (int k=0; k<2; k++) for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){ OptimizableGraph::Vertex* v = *it; if (! v->fixed()){ if (static_cast(v->marginalized()) == k){ v->setHessianIndex(i); _ivMap[i]=v; i++; } } else { v->setHessianIndex(-1); } } _ivMap.resize(i); return true; } void SparseOptimizer::clearIndexMapping(){ for (size_t i=0; i<_ivMap.size(); ++i){ _ivMap[i]->setHessianIndex(-1); _ivMap[i]=0; } } bool SparseOptimizer::initializeOptimization(int level){ HyperGraph::VertexSet vset; for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) vset.insert(it->second); return initializeOptimization(vset,level); } bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){ if (edges().size() == 0) { cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl; return false; } bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated; assert(workspaceAllocated && "Error while allocating memory for the Jacobians"); clearIndexMapping(); _activeVertices.clear(); _activeVertices.reserve(vset.size()); _activeEdges.clear(); set auxEdgeSet; // temporary structure to avoid duplicates for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){ OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it; const OptimizableGraph::EdgeSet& vEdges=v->edges(); // count if there are edges in that level. If not remove from the pool int levelEdges=0; for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){ OptimizableGraph::Edge* e=reinterpret_cast(*it); if (level < 0 || e->level() == level) { bool allVerticesOK = true; for (vector::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { if (vset.find(*vit) == vset.end()) { allVerticesOK = false; break; } } if (allVerticesOK && !e->allVerticesFixed()) { auxEdgeSet.insert(e); levelEdges++; } } } if (levelEdges){ _activeVertices.push_back(v); // test for NANs in the current estimate if we are debugging # ifndef NDEBUG int estimateDim = v->estimateDimension(); if (estimateDim > 0) { Eigen::VectorXd estimateData(estimateDim); if (v->getEstimateData(estimateData.data()) == true) { int k; bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k); if (hasNan) cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl; } } # endif } } _activeEdges.reserve(auxEdgeSet.size()); for (set::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it) _activeEdges.push_back(*it); sortVectorContainers(); return buildIndexMapping(_activeVertices); } bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset){ bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated; assert(workspaceAllocated && "Error while allocating memory for the Jacobians"); clearIndexMapping(); _activeVertices.clear(); _activeEdges.clear(); _activeEdges.reserve(eset.size()); set auxVertexSet; // temporary structure to avoid duplicates for (HyperGraph::EdgeSet::iterator it=eset.begin(); it!=eset.end(); ++it){ OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it); for (vector::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { auxVertexSet.insert(static_cast(*vit)); } _activeEdges.push_back(reinterpret_cast(*it)); } _activeVertices.reserve(auxVertexSet.size()); for (set::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it) _activeVertices.push_back(*it); sortVectorContainers(); return buildIndexMapping(_activeVertices); } void SparseOptimizer::setToOrigin(){ for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) { OptimizableGraph::Vertex* v = static_cast(it->second); v->setToOrigin(); } } void SparseOptimizer::computeInitialGuess() { EstimatePropagator::PropagateCost costFunction(this); computeInitialGuess(costFunction); } void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction) { OptimizableGraph::VertexSet emptySet; std::set backupVertices; HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) { OptimizableGraph::Edge* e = *it; for (size_t i = 0; i < e->vertices().size(); ++i) { OptimizableGraph::Vertex* v = static_cast(e->vertex(i)); if (v->fixed()) fixedVertices.insert(v); else { // check for having a prior which is able to fully initialize a vertex for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) { OptimizableGraph::Edge* vedge = static_cast(*vedgeIt); if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) { //cerr << "Initialize with prior for " << v->id() << endl; vedge->initialEstimate(emptySet, v); fixedVertices.insert(v); } } } if (v->hessianIndex() == -1) { std::set::const_iterator foundIt = backupVertices.find(v); if (foundIt == backupVertices.end()) { v->push(); backupVertices.insert(v); } } } } EstimatePropagator estimatePropagator(this); estimatePropagator.propagate(fixedVertices, costFunction); // restoring the vertices that should not be initialized for (std::set::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) { Vertex* v = *it; v->pop(); } if (verbose()) { computeActiveErrors(); cerr << "iteration= -1\t chi2= " << activeChi2() << "\t time= 0.0" << "\t cumTime= 0.0" << "\t (using initial guess from " << costFunction.name() << ")" << endl; } } int SparseOptimizer::optimize(int iterations, bool online) { if (_ivMap.size() == 0) { cerr << __PRETTY_FUNCTION__ << ": 0 vertices to optimize, maybe forgot to call initializeOptimization()" << endl; return -1; } int cjIterations=0; double cumTime=0; bool ok=true; ok = _algorithm->init(online); if (! ok) { cerr << __PRETTY_FUNCTION__ << " Error while initializing" << endl; return -1; } _batchStatistics.clear(); if (_computeBatchStatistics) _batchStatistics.resize(iterations); OptimizationAlgorithm::SolverResult result = OptimizationAlgorithm::OK; for (int i=0; isolve(i, online); ok = ( result == OptimizationAlgorithm::OK ); bool errorComputed = false; if (_computeBatchStatistics) { computeActiveErrors(); errorComputed = true; _batchStatistics[i].chi2 = activeRobustChi2(); _batchStatistics[i].timeIteration = get_monotonic_time()-ts; } if (verbose()){ double dts = get_monotonic_time()-ts; cumTime += dts; if (! errorComputed) computeActiveErrors(); cerr << "iteration= " << i << "\t chi2= " << FIXED(activeRobustChi2()) << "\t time= " << dts << "\t cumTime= " << cumTime << "\t edges= " << _activeEdges.size(); _algorithm->printVerbose(cerr); cerr << endl; } ++cjIterations; postIteration(i); } if (result == OptimizationAlgorithm::Fail) { return 0; } return cjIterations; } void SparseOptimizer::update(const double* update) { // update the graph by calling oplus on the vertices for (size_t i=0; i < _ivMap.size(); ++i) { OptimizableGraph::Vertex* v= _ivMap[i]; #ifndef NDEBUG bool hasNan = arrayHasNaN(update, v->dimension()); if (hasNan) cerr << __PRETTY_FUNCTION__ << ": Update contains a nan for vertex " << v->id() << endl; #endif v->oplus(update); update += v->dimension(); } } void SparseOptimizer::setComputeBatchStatistics(bool computeBatchStatistics) { if ((_computeBatchStatistics == true) && (computeBatchStatistics == false)) { G2OBatchStatistics::setGlobalStats(0); _batchStatistics.clear(); } _computeBatchStatistics = computeBatchStatistics; } bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset) { std::vector newVertices; newVertices.reserve(vset.size()); _activeVertices.reserve(_activeVertices.size() + vset.size()); _activeEdges.reserve(_activeEdges.size() + eset.size()); for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) { OptimizableGraph::Edge* e = static_cast(*it); if (!e->allVerticesFixed()) _activeEdges.push_back(e); } // update the index mapping size_t next = _ivMap.size(); for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) { OptimizableGraph::Vertex* v=static_cast(*it); if (! v->fixed()){ if (! v->marginalized()){ v->setHessianIndex(next); _ivMap.push_back(v); newVertices.push_back(v); _activeVertices.push_back(v); next++; } else // not supported right now abort(); } else { v->setHessianIndex(-1); } } //if (newVertices.size() != vset.size()) //cerr << __PRETTY_FUNCTION__ << ": something went wrong " << PVAR(vset.size()) << " " << PVAR(newVertices.size()) << endl; return _algorithm->updateStructure(newVertices, eset); } void SparseOptimizer::sortVectorContainers() { // sort vector structures to get deterministic ordering based on IDs sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare()); sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare()); } void SparseOptimizer::clear() { _ivMap.clear(); _activeVertices.clear(); _activeEdges.clear(); OptimizableGraph::clear(); } SparseOptimizer::VertexContainer::const_iterator SparseOptimizer::findActiveVertex(const OptimizableGraph::Vertex* v) const { VertexContainer::const_iterator lower = lower_bound(_activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare()); if (lower == _activeVertices.end()) return _activeVertices.end(); if ((*lower) == v) return lower; return _activeVertices.end(); } SparseOptimizer::EdgeContainer::const_iterator SparseOptimizer::findActiveEdge(const OptimizableGraph::Edge* e) const { EdgeContainer::const_iterator lower = lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare()); if (lower == _activeEdges.end()) return _activeEdges.end(); if ((*lower) == e) return lower; return _activeEdges.end(); } void SparseOptimizer::push(SparseOptimizer::VertexContainer& vlist) { for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it) (*it)->push(); } void SparseOptimizer::pop(SparseOptimizer::VertexContainer& vlist) { for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it) (*it)->pop(); } void SparseOptimizer::push(HyperGraph::VertexSet& vlist) { for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) { OptimizableGraph::Vertex* v = dynamic_cast(*it); if (v) v->push(); else cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl; } } void SparseOptimizer::pop(HyperGraph::VertexSet& vlist) { for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it){ OptimizableGraph::Vertex* v = dynamic_cast (*it); if (v) v->pop(); else cerr << __FUNCTION__ << ": FATAL POP SET" << endl; } } void SparseOptimizer::discardTop(SparseOptimizer::VertexContainer& vlist) { for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it) (*it)->discardTop(); } void SparseOptimizer::setVerbose(bool verbose) { _verbose = verbose; } void SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm) { if (_algorithm) // reset the optimizer for the formerly used solver _algorithm->setOptimizer(0); _algorithm = algorithm; if (_algorithm) _algorithm->setOptimizer(this); } bool SparseOptimizer::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices){ return _algorithm->computeMarginals(spinv, blockIndices); } void SparseOptimizer::setForceStopFlag(bool* flag) { _forceStopFlag=flag; } bool SparseOptimizer::removeVertex(HyperGraph::Vertex* v) { OptimizableGraph::Vertex* vv = static_cast(v); if (vv->hessianIndex() >= 0) { clearIndexMapping(); _ivMap.clear(); } return HyperGraph::removeVertex(v); } bool SparseOptimizer::addComputeErrorAction(HyperGraphAction* action) { std::pair insertResult = _graphActions[AT_COMPUTEACTIVERROR].insert(action); return insertResult.second; } bool SparseOptimizer::removeComputeErrorAction(HyperGraphAction* action) { return _graphActions[AT_COMPUTEACTIVERROR].erase(action) > 0; } void SparseOptimizer::push() { push(_activeVertices); } void SparseOptimizer::pop() { pop(_activeVertices); } void SparseOptimizer::discardTop() { discardTop(_activeVertices); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/core/sparse_optimizer.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_GRAPH_OPTIMIZER_CHOL_H_ #define G2O_GRAPH_OPTIMIZER_CHOL_H_ #include "../stuff/macros.h" #include "optimizable_graph.h" #include "sparse_block_matrix.h" #include "batch_stats.h" #include namespace g2o { // forward declaration class ActivePathCostFunction; class OptimizationAlgorithm; class EstimatePropagatorCost; class SparseOptimizer : public OptimizableGraph { public: enum { AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS, AT_NUM_ELEMENTS, // keep as last element }; friend class ActivePathCostFunction; // Attention: _solver & _statistics is own by SparseOptimizer and will be // deleted in its destructor. SparseOptimizer(); virtual ~SparseOptimizer(); // new interface for the optimizer // the old functions will be dropped /** * Initializes the structures for optimizing a portion of the graph specified by a subset of edges. * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the * schur complement or to set as fixed during the optimization. * @param eset: the subgraph to be optimized. * @returns false if somethings goes wrong */ virtual bool initializeOptimization(HyperGraph::EdgeSet& eset); /** * Initializes the structures for optimizing a portion of the graph specified by a subset of vertices. * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the * schur complement or to set as fixed during the optimization. * @param vset: the subgraph to be optimized. * @param level: is the level (in multilevel optimization) * @returns false if somethings goes wrong */ virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0); /** * Initializes the structures for optimizing the whole graph. * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the * schur complement or to set as fixed during the optimization. * @param level: is the level (in multilevel optimization) * @returns false if somethings goes wrong */ virtual bool initializeOptimization(int level=0); /** * HACK updating the internal structures for online processing */ virtual bool updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset); /** * Propagates an initial guess from the vertex specified as origin. * It should be called after initializeOptimization(...), as it relies on the _activeVertices/_edges structures. * It constructs a set of trees starting from the nodes in the graph which are fixed and eligible for preforming optimization. * The trees are constructed by utlizing a cost-function specified. * @param costFunction: the cost function used * @patam maxDistance: the distance where to stop the search */ virtual void computeInitialGuess(); /** * Same as above but using a specific propagator */ virtual void computeInitialGuess(EstimatePropagatorCost& propagator); /** * sets all vertices to their origin. */ virtual void setToOrigin(); /** * starts one optimization run given the current configuration of the graph, * and the current settings stored in the class instance. * It can be called only after initializeOptimization */ int optimize(int iterations, bool online = false); /** * computes the blocks of the inverse of the specified pattern. * the pattern is given via pairs of the blocks in the hessian * @param blockIndices: the pattern * @param spinv: the sparse block matrix with the result * @returns false if the operation is not supported by the solver */ bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices); /** * computes the inverse of the specified vertex. * @param vertex: the vertex whose state is to be marginalised * @param spinv: the sparse block matrix with the result * @returns false if the operation is not supported by the solver */ bool computeMarginals(SparseBlockMatrix& spinv, const Vertex* vertex) { if (vertex->hessianIndex() < 0) { return false; } std::vector > index; index.push_back(std::pair(vertex->hessianIndex(), vertex->hessianIndex())); return computeMarginals(spinv, index); } /** * computes the inverse of the set specified vertices, assembled into a single covariance matrix. * @param vertex: the pattern * @param spinv: the sparse block matrix with the result * @returns false if the operation is not supported by the solver */ bool computeMarginals(SparseBlockMatrix& spinv, const VertexContainer& vertices) { std::vector > indices; for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) { indices.push_back(std::pair((*it)->hessianIndex(),(*it)->hessianIndex())); } return computeMarginals(spinv, indices); } //! finds a gauge in the graph to remove the undefined dof. // The gauge should be fixed() and then the optimization can work (if no additional dof are in // the system. The default implementation returns a node with maximum dimension. virtual Vertex* findGauge(); bool gaugeFreedom(); /**returns the cached chi2 of the active portion of the graph*/ double activeChi2() const; /** * returns the cached chi2 of the active portion of the graph. * In contrast to activeChi2() this functions considers the weighting * of the error according to the robustification of the error functions. */ double activeRobustChi2() const; //! verbose information during optimization bool verbose() const {return _verbose;} void setVerbose(bool verbose); /** * sets a variable checked at every iteration to force a user stop. The iteration exits when the variable is true; */ void setForceStopFlag(bool* flag); bool* forceStopFlag() const { return _forceStopFlag;}; //! if external stop flag is given, return its state. False otherwise bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; } //! the index mapping of the vertices const VertexContainer& indexMapping() const {return _ivMap;} //! the vertices active in the current optimization const VertexContainer& activeVertices() const { return _activeVertices;} //! the edges active in the current optimization const EdgeContainer& activeEdges() const { return _activeEdges;} /** * Remove a vertex. If the vertex is contained in the currently active set * of vertices, then the internal temporary structures are cleaned, e.g., the index * mapping is erased. In case you need the index mapping for manipulating the * graph, you have to store it in your own copy. */ virtual bool removeVertex(HyperGraph::Vertex* v); /** * search for an edge in _activeVertices and return the iterator pointing to it * getActiveVertices().end() if not found */ VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const; /** * search for an edge in _activeEdges and return the iterator pointing to it * getActiveEdges().end() if not found */ EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const; //! the solver used by the optimizer const OptimizationAlgorithm* algorithm() const { return _algorithm;} OptimizationAlgorithm* solver() { return _algorithm;} void setAlgorithm(OptimizationAlgorithm* algorithm); //! push the estimate of a subset of the variables onto a stack void push(SparseOptimizer::VertexContainer& vlist); //! push the estimate of a subset of the variables onto a stack void push(HyperGraph::VertexSet& vlist); //! push all the active vertices onto a stack void push(); //! pop (restore) the estimate a subset of the variables from the stack void pop(SparseOptimizer::VertexContainer& vlist); //! pop (restore) the estimate a subset of the variables from the stack void pop(HyperGraph::VertexSet& vlist); //! pop (restore) the estimate of the active vertices from the stack void pop(); //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate void discardTop(SparseOptimizer::VertexContainer& vlist); //! same as above, but for the active vertices void discardTop(); using OptimizableGraph::discardTop; /** * clears the graph, and polishes some intermediate structures * Note that this only removes nodes / edges. Parameters can be removed * with clearParameters(). */ virtual void clear(); /** * computes the error vectors of all edges in the activeSet, and caches them */ void computeActiveErrors(); /** * Linearizes the system by computing the Jacobians for the nodes * and edges in the graph */ G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem()) { // nothing needed, linearization is now done inside the solver } /** * update the estimate of the active vertices * @param update: the double vector containing the stacked * elements of the increments on the vertices. */ void update(const double* update); /** returns the set of batch statistics about the optimisation */ const BatchStatisticsContainer& batchStatistics() const { return _batchStatistics;} /** returns the set of batch statistics about the optimisation */ BatchStatisticsContainer& batchStatistics() { return _batchStatistics;} void setComputeBatchStatistics(bool computeBatchStatistics); bool computeBatchStatistics() const { return _computeBatchStatistics;} /**** callbacks ****/ //! add an action to be executed before the error vectors are computed bool addComputeErrorAction(HyperGraphAction* action); //! remove an action that should no longer be execured before computing the error vectors bool removeComputeErrorAction(HyperGraphAction* action); protected: bool* _forceStopFlag; bool _verbose; VertexContainer _ivMap; VertexContainer _activeVertices; ///< sorted according to VertexIDCompare EdgeContainer _activeEdges; ///< sorted according to EdgeIDCompare void sortVectorContainers(); OptimizationAlgorithm* _algorithm; /** * builds the mapping of the active vertices to the (block) row / column in the Hessian */ bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist); void clearIndexMapping(); BatchStatisticsContainer _batchStatistics; ///< global statistics of the optimizer, e.g., timing, num-non-zeros bool _computeBatchStatistics; }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/solvers/linear_solver_dense.h ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // Copyright (C) 2012 R. Kümmerle // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef G2O_LINEAR_SOLVER_DENSE_H #define G2O_LINEAR_SOLVER_DENSE_H #include "../core/linear_solver.h" #include "../core/batch_stats.h" #include #include #include #include namespace g2o { /** * \brief linear solver using dense cholesky decomposition */ template class LinearSolverDense : public LinearSolver { public: LinearSolverDense() : LinearSolver(), _reset(true) { } virtual ~LinearSolverDense() { } virtual bool init() { _reset = true; return true; } bool solve(const SparseBlockMatrix& A, double* x, double* b) { int n = A.cols(); int m = A.cols(); Eigen::MatrixXd& H = _H; if (H.cols() != n) { H.resize(n, m); _reset = true; } if (_reset) { _reset = false; H.setZero(); } // copy the sparse block matrix into a dense matrix int c_idx = 0; for (size_t i = 0; i < A.blockCols().size(); ++i) { int c_size = A.colsOfBlock(i); assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices"); const typename SparseBlockMatrix::IntBlockMap& col = A.blockCols()[i]; if (col.size() > 0) { typename SparseBlockMatrix::IntBlockMap::const_iterator it; for (it = col.begin(); it != col.end(); ++it) { int r_idx = A.rowBaseOfBlock(it->first); // only the upper triangular block is processed if (it->first <= (int)i) { int r_size = A.rowsOfBlock(it->first); H.block(r_idx, c_idx, r_size, c_size) = *(it->second); if (r_idx != c_idx) // write the lower triangular block H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose(); } } } c_idx += c_size; } // solving via Cholesky decomposition Eigen::VectorXd::MapType xvec(x, m); Eigen::VectorXd::ConstMapType bvec(b, n); _cholesky.compute(H); if (_cholesky.isPositive()) { xvec = _cholesky.solve(bvec); return true; } return false; } protected: bool _reset; Eigen::MatrixXd _H; Eigen::LDLT _cholesky; }; }// end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/solvers/linear_solver_eigen.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_LINEAR_SOLVER_EIGEN_H #define G2O_LINEAR_SOLVER_EIGEN_H #include #include #include "../core/linear_solver.h" #include "../core/batch_stats.h" #include "../stuff/timeutil.h" #include "../core/eigen_types.h" #include #include namespace g2o { /** * \brief linear solver which uses the sparse Cholesky solver from Eigen * * Has no dependencies except Eigen. Hence, should compile almost everywhere * without to much issues. Performance should be similar to CSparse, I guess. */ template class LinearSolverEigen: public LinearSolver { public: typedef Eigen::SparseMatrix SparseMatrix; typedef Eigen::Triplet Triplet; typedef Eigen::PermutationMatrix PermutationMatrix; /** * \brief Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering */ class CholeskyDecomposition : public Eigen::SimplicialLDLT { public: CholeskyDecomposition() : Eigen::SimplicialLDLT() {} using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered; void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation) { m_Pinv = permutation; m_P = permutation.inverse(); int size = a.cols(); SparseMatrix ap(size, size); ap.selfadjointView() = a.selfadjointView().twistedBy(m_P); analyzePattern_preordered(ap, true); } }; public: LinearSolverEigen() : LinearSolver(), _init(true), _blockOrdering(false), _writeDebug(false) { } virtual ~LinearSolverEigen() { } virtual bool init() { _init = true; return true; } bool solve(const SparseBlockMatrix& A, double* x, double* b) { if (_init) _sparseMatrix.resize(A.rows(), A.cols()); fillSparseMatrix(A, !_init); if (_init) // compute the symbolic composition once computeSymbolicDecomposition(A); _init = false; double t=get_monotonic_time(); _cholesky.factorize(_sparseMatrix); if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite if (_writeDebug) { std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl; A.writeOctave("debug.txt"); } return false; } // Solving the system VectorXD::MapType xx(x, _sparseMatrix.cols()); VectorXD::ConstMapType bb(b, _sparseMatrix.cols()); xx = _cholesky.solve(bb); G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); if (globalStats) { globalStats->timeNumericDecomposition = get_monotonic_time() - t; globalStats->choleskyNNZ = _cholesky.matrixL().nestedExpression().nonZeros() + _sparseMatrix.cols(); // the elements of D } return true; } //! do the AMD ordering on the blocks or on the scalar matrix bool blockOrdering() const { return _blockOrdering;} void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering;} //! write a debug dump of the system matrix if it is not SPD in solve virtual bool writeDebug() const { return _writeDebug;} virtual void setWriteDebug(bool b) { _writeDebug = b;} protected: bool _init; bool _blockOrdering; bool _writeDebug; SparseMatrix _sparseMatrix; CholeskyDecomposition _cholesky; /** * compute the symbolic decompostion of the matrix only once. * Since A has the same pattern in all the iterations, we only * compute the fill-in reducing ordering once and re-use for all * the following iterations. */ void computeSymbolicDecomposition(const SparseBlockMatrix& A) { double t=get_monotonic_time(); if (! _blockOrdering) { _cholesky.analyzePattern(_sparseMatrix); } else { // block ordering with the Eigen Interface // This is really ugly currently, as it calls internal functions from Eigen // and modifies the SparseMatrix class Eigen::PermutationMatrix blockP; { // prepare a block structure matrix for calling AMD std::vector triplets; for (size_t c = 0; c < A.blockCols().size(); ++c){ const typename SparseBlockMatrix::IntBlockMap& column = A.blockCols()[c]; for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) { const int& r = it->first; if (r > static_cast(c)) // only upper triangle break; triplets.push_back(Triplet(r, c, 0.)); } } // call the AMD ordering on the block matrix. // Relies on Eigen's internal stuff, probably bad idea SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size()); auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end()); typename CholeskyDecomposition::CholMatrixType C; C = auxBlockMatrix.selfadjointView(); Eigen::internal::minimum_degree_ordering(C, blockP); } int rows = A.rows(); assert(rows == A.cols() && "Matrix A is not square"); // Adapt the block permutation to the scalar matrix PermutationMatrix scalarP; scalarP.resize(rows); int scalarIdx = 0; for (int i = 0; i < blockP.size(); ++i) { const int& p = blockP.indices()(i); int base = A.colBaseOfBlock(p); int nCols = A.colsOfBlock(p); for (int j = 0; j < nCols; ++j) scalarP.indices()(scalarIdx++) = base++; } assert(scalarIdx == rows && "did not completely fill the permutation matrix"); // analyze with the scalar permutation _cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP); } G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats(); if (globalStats) globalStats->timeSymbolicDecomposition = get_monotonic_time() - t; } void fillSparseMatrix(const SparseBlockMatrix& A, bool onlyValues) { if (onlyValues) { A.fillCCS(_sparseMatrix.valuePtr(), true); } else { // create from triplet structure std::vector triplets; triplets.reserve(A.nonZeros()); for (size_t c = 0; c < A.blockCols().size(); ++c) { int colBaseOfBlock = A.colBaseOfBlock(c); const typename SparseBlockMatrix::IntBlockMap& column = A.blockCols()[c]; for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) { int rowBaseOfBlock = A.rowBaseOfBlock(it->first); const MatrixType& m = *(it->second); for (int cc = 0; cc < m.cols(); ++cc) { int aux_c = colBaseOfBlock + cc; for (int rr = 0; rr < m.rows(); ++rr) { int aux_r = rowBaseOfBlock + rr; if (aux_r > aux_c) break; triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc))); } } } } _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end()); } } }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/color_macros.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_COLOR_MACROS_H #define G2O_COLOR_MACROS_H // font attributes #define FT_BOLD "\033[1m" #define FT_UNDERLINE "\033[4m" //background color #define BG_BLACK "\033[40m" #define BG_RED "\033[41m" #define BG_GREEN "\033[42m" #define BG_YELLOW "\033[43m" #define BG_LIGHTBLUE "\033[44m" #define BG_MAGENTA "\033[45m" #define BG_BLUE "\033[46m" #define BG_WHITE "\033[47m" // font color #define CL_BLACK(s) "\033[30m" << s << "\033[0m" #define CL_RED(s) "\033[31m" << s << "\033[0m" #define CL_GREEN(s) "\033[32m" << s << "\033[0m" #define CL_YELLOW(s) "\033[33m" << s << "\033[0m" #define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m" #define CL_MAGENTA(s) "\033[35m" << s << "\033[0m" #define CL_BLUE(s) "\033[36m" << s << "\033[0m" #define CL_WHITE(s) "\033[37m" << s << "\033[0m" #define FG_BLACK "\033[30m" #define FG_RED "\033[31m" #define FG_GREEN "\033[32m" #define FG_YELLOW "\033[33m" #define FG_LIGHTBLUE "\033[34m" #define FG_MAGENTA "\033[35m" #define FG_BLUE "\033[36m" #define FG_WHITE "\033[37m" #define FG_NORM "\033[0m" #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/macros.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_MACROS_H #define G2O_MACROS_H #ifndef DEG2RAD #define DEG2RAD(x) ((x) * 0.01745329251994329575) #endif #ifndef RAD2DEG #define RAD2DEG(x) ((x) * 57.29577951308232087721) #endif // GCC the one and only #if defined(__GNUC__) # define G2O_ATTRIBUTE_CONSTRUCTOR(func) \ static void func(void)__attribute__ ((constructor)); \ static void func(void) # define G2O_ATTRIBUTE_UNUSED __attribute__((unused)) # define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2))) # define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3))) # define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning)) # define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated)) #ifdef ANDROID # define g2o_isnan(x) isnan(x) # define g2o_isinf(x) isinf(x) # define g2o_isfinite(x) isfinite(x) #else # define g2o_isnan(x) std::isnan(x) # define g2o_isinf(x) std::isinf(x) # define g2o_isfinite(x) std::isfinite(x) #endif // MSVC on Windows #elif defined _MSC_VER # define __PRETTY_FUNCTION__ __FUNCTION__ /** Modified by Mark Pupilli from: "Initializer/finalizer sample for MSVC and GCC. 2010 Joe Lowe. Released into the public domain." "For MSVC, places a ptr to the function in the user initializer section (.CRT$XCU), basically the same thing the compiler does for the constructor calls for static C++ objects. For GCC, uses a constructor attribute." (As posted on Stack OVerflow) */ # define G2O_ATTRIBUTE_CONSTRUCTOR(f) \ __pragma(section(".CRT$XCU",read)) \ static void __cdecl f(void); \ __declspec(allocate(".CRT$XCU")) void (__cdecl*f##_)(void) = f; \ static void __cdecl f(void) # define G2O_ATTRIBUTE_UNUSED # define G2O_ATTRIBUTE_FORMAT12 # define G2O_ATTRIBUTE_FORMAT23 # define G2O_ATTRIBUTE_WARNING(func) func # define G2O_ATTRIBUTE_DEPRECATED(func) func #include # define g2o_isnan(x) _isnan(x) # define g2o_isinf(x) (_finite(x) == 0) # define g2o_isfinite(x) (_finite(x) != 0) // unknown compiler #else # ifndef __PRETTY_FUNCTION__ # define __PRETTY_FUNCTION__ "" # endif # define G2O_ATTRIBUTE_CONSTRUCTOR(func) func # define G2O_ATTRIBUTE_UNUSED # define G2O_ATTRIBUTE_FORMAT12 # define G2O_ATTRIBUTE_FORMAT23 # define G2O_ATTRIBUTE_WARNING(func) func # define G2O_ATTRIBUTE_DEPRECATED(func) func #include #define g2o_isnan(x) isnan(x) #define g2o_isinf(x) isinf(x) #define g2o_isfinite(x) isfinite(x) #endif // some macros that are only useful for c++ #ifdef __cplusplus #define G2O_FSKIP_LINE(f) \ {char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);} #ifndef PVAR #define PVAR(s) \ #s << " = " << (s) << std::flush #endif #ifndef PVARA #define PVARA(s) \ #s << " = " << RAD2DEG(s) << "deg" << std::flush #endif #ifndef FIXED #define FIXED(s) \ std::fixed << s << std::resetiosflags(std::ios_base::fixed) #endif #endif // __cplusplus #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/misc.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_STUFF_MISC_H #define G2O_STUFF_MISC_H #include "macros.h" #include #ifndef M_PI #define M_PI 3.14159265358979323846 #endif /** @addtogroup utils **/ // @{ /** \file misc.h * \brief some general case utility functions * * This file specifies some general case utility functions **/ namespace g2o { /** * return the square value */ template inline T square(T x) { return x*x; } /** * return the hypot of x and y */ template inline T hypot(T x, T y) { return (T) (sqrt(x*x + y*y)); } /** * return the squared hypot of x and y */ template inline T hypot_sqr(T x, T y) { return x*x + y*y; } /** * convert from degree to radian */ inline double deg2rad(double degree) { return degree * 0.01745329251994329576; } /** * convert from radian to degree */ inline double rad2deg(double rad) { return rad * 57.29577951308232087721; } /** * normalize the angle */ inline double normalize_theta(double theta) { if (theta >= -M_PI && theta < M_PI) return theta; double multiplier = floor(theta / (2*M_PI)); theta = theta - multiplier*2*M_PI; if (theta >= M_PI) theta -= 2*M_PI; if (theta < -M_PI) theta += 2*M_PI; return theta; } /** * inverse of an angle, i.e., +180 degree */ inline double inverse_theta(double th) { return normalize_theta(th + M_PI); } /** * average two angles */ inline double average_angle(double theta1, double theta2) { double x, y; x = cos(theta1) + cos(theta2); y = sin(theta1) + sin(theta2); if(x == 0 && y == 0) return 0; else return std::atan2(y, x); } /** * sign function. * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0 */ template inline int sign(T x) { if (x > 0) return 1; else if (x < 0) return -1; else return 0; } /** * clamp x to the interval [l, u] */ template inline T clamp(T l, T x, T u) { if (x < l) return l; if (x > u) return u; return x; } /** * wrap x to be in the interval [l, u] */ template inline T wrap(T l, T x, T u) { T intervalWidth = u - l; while (x < l) x += intervalWidth; while (x > u) x -= intervalWidth; return x; } /** * tests whether there is a NaN in the array */ inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0) { for (int i = 0; i < size; ++i) if (g2o_isnan(array[i])) { if (nanIndex) *nanIndex = i; return true; } return false; } /** * The following two functions are used to force linkage with static libraries. */ extern "C" { typedef void (* ForceLinkFunction) (void); } struct ForceLinker { ForceLinker(ForceLinkFunction function) { (function)(); } }; } // end namespace // @} #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/os_specific.c ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "os_specific.h" #ifdef WINDOWS int vasprintf(char** strp, const char* fmt, va_list ap) { int n; int size = 100; char* p; char* np; if ((p = (char*)malloc(size * sizeof(char))) == NULL) return -1; while (1) { #ifdef _MSC_VER n = vsnprintf_s(p, size, size - 1, fmt, ap); #else n = vsnprintf(p, size, fmt, ap); #endif if (n > -1 && n < size) { *strp = p; return n; } if (n > -1) size = n+1; else size *= 2; if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { free(p); return -1; } else p = np; } } #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/os_specific.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_OS_SPECIFIC_HH_ #define G2O_OS_SPECIFIC_HH_ #ifdef WINDOWS #include #include #include #ifndef _WINDOWS #include #endif #define drand48() ((double) rand()/(double)RAND_MAX) #ifdef __cplusplus extern "C" { #endif int vasprintf(char** strp, const char* fmt, va_list ap); #ifdef __cplusplus } #endif #endif #ifdef UNIX #include // nothing to do on real operating systems #endif #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/property.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 "property.h" #include #include #include "macros.h" #include "string_tools.h" using namespace std; namespace g2o { BaseProperty::BaseProperty(const std::string name_) :_name(name_){ } BaseProperty::~BaseProperty(){} bool PropertyMap::addProperty(BaseProperty* p) { std::pair result = insert(make_pair(p->name(), p)); return result.second; } bool PropertyMap::eraseProperty(const std::string& name) { PropertyMapIterator it=find(name); if (it==end()) return false; delete it->second; erase(it); return true; } PropertyMap::~PropertyMap() { for (PropertyMapIterator it=begin(); it!=end(); it++){ if (it->second) delete it->second; } } bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value) { PropertyMapIterator it = find(name); if (it == end()) return false; it->second->fromString(value); return true; } void PropertyMap::writeToCSV(std::ostream& os) const { for (PropertyMapConstIterator it=begin(); it!=end(); it++){ BaseProperty* p =it->second; os << p->name() << ", "; } os << std::endl; for (PropertyMapConstIterator it=begin(); it!=end(); it++){ BaseProperty* p =it->second; os << p->toString() << ", "; } os << std::endl; } bool PropertyMap::updateMapFromString(const std::string& values) { bool status = true; vector valuesMap = strSplit(values, ","); for (size_t i = 0; i < valuesMap.size(); ++i) { vector m = strSplit(valuesMap[i], "="); if (m.size() != 2) { cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl; continue; } string name = trim(m[0]); string value = trim(m[1]); status = status && updatePropertyFromString(name, value); } return status; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/stuff/property.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_PROPERTY_H_ #define G2O_PROPERTY_H_ #include #include #include #include "string_tools.h" namespace g2o { class BaseProperty { public: BaseProperty(const std::string name_); virtual ~BaseProperty(); const std::string& name() {return _name;} virtual std::string toString() const = 0; virtual bool fromString(const std::string& s) = 0; protected: std::string _name; }; template class Property: public BaseProperty { public: typedef T ValueType; Property(const std::string& name_): BaseProperty(name_){} Property(const std::string& name_, const T& v): BaseProperty(name_), _value(v){} void setValue(const T& v) {_value = v; } const T& value() const {return _value;} virtual std::string toString() const { std::stringstream sstr; sstr << _value; return sstr.str(); } virtual bool fromString(const std::string& s) { bool status = convertString(s, _value); return status; } protected: T _value; }; /** * \brief a collection of properties mapping from name to the property itself */ class PropertyMap : protected std::map { public: typedef std::map BaseClass; typedef BaseClass::iterator PropertyMapIterator; typedef BaseClass::const_iterator PropertyMapConstIterator; ~PropertyMap(); /** * add a property to the map */ bool addProperty(BaseProperty* p); /** * remove a property from the map */ bool eraseProperty(const std::string& name_); /** * return a property by its name */ template P* getProperty(const std::string& name_) { PropertyMapIterator it=find(name_); if (it==end()) return 0; return dynamic_cast(it->second); } template const P* getProperty(const std::string& name_) const { PropertyMapConstIterator it=find(name_); if (it==end()) return 0; return dynamic_cast(it->second); } /** * create a property and insert it */ template P* makeProperty(const std::string& name_, const typename P::ValueType& v) { PropertyMapIterator it=find(name_); if (it==end()){ P* p=new P(name_, v); addProperty(p); return p; } else return dynamic_cast(it->second); } /** * update a specfic property with a new value * @return true if the params is stored and update was carried out */ bool updatePropertyFromString(const std::string& name, const std::string& value); /** * update the map based on a name=value string, e.g., name1=val1,name2=val2 * @return true, if it was possible to update all parameters */ bool updateMapFromString(const std::string& values); void writeToCSV(std::ostream& os) const; using BaseClass::size; using BaseClass::begin; using BaseClass::end; using BaseClass::iterator; using BaseClass::const_iterator; }; typedef Property IntProperty; typedef Property BoolProperty; typedef Property FloatProperty; typedef Property DoubleProperty; typedef Property StringProperty; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/string_tools.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 "string_tools.h" #include "os_specific.h" #include "macros.h" #include #include #include #include #include #include #include #include #if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID) #include #endif namespace g2o { using namespace std; std::string trim(const std::string& s) { if(s.length() == 0) return s; string::size_type b = s.find_first_not_of(" \t\n"); string::size_type e = s.find_last_not_of(" \t\n"); if(b == string::npos) return ""; return std::string(s, b, e - b + 1); } std::string trimLeft(const std::string& s) { if(s.length() == 0) return s; string::size_type b = s.find_first_not_of(" \t\n"); string::size_type e = s.length() - 1; if(b == string::npos) return ""; return std::string(s, b, e - b + 1); } std::string trimRight(const std::string& s) { if(s.length() == 0) return s; string::size_type b = 0; string::size_type e = s.find_last_not_of(" \t\n"); if(b == string::npos) return ""; return std::string(s, b, e - b + 1); } std::string strToLower(const std::string& s) { string ret; std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::tolower); return ret; } std::string strToUpper(const std::string& s) { string ret; std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::toupper); return ret; } std::string formatString(const char* fmt, ...) { char* auxPtr = NULL; va_list arg_list; va_start(arg_list, fmt); int numChar = vasprintf(&auxPtr, fmt, arg_list); va_end(arg_list); string retString; if (numChar != -1) retString = auxPtr; else { cerr << __PRETTY_FUNCTION__ << ": Error while allocating memory" << endl; } free(auxPtr); return retString; } int strPrintf(std::string& str, const char* fmt, ...) { char* auxPtr = NULL; va_list arg_list; va_start(arg_list, fmt); int numChars = vasprintf(&auxPtr, fmt, arg_list); va_end(arg_list); str = auxPtr; free(auxPtr); return numChars; } std::string strExpandFilename(const std::string& filename) { #if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID) string result = filename; wordexp_t p; wordexp(filename.c_str(), &p, 0); if(p.we_wordc > 0) { result = p.we_wordv[0]; } wordfree(&p); return result; #else (void) filename; std::cerr << "WARNING: " << __PRETTY_FUNCTION__ << " not implemented" << std::endl; return std::string(); #endif } std::vector strSplit(const std::string& str, const std::string& delimiters) { std::vector tokens; string::size_type lastPos = 0; string::size_type pos = 0; do { pos = str.find_first_of(delimiters, lastPos); tokens.push_back(str.substr(lastPos, pos - lastPos)); lastPos = pos + 1; } while (string::npos != pos); return tokens; } bool strStartsWith(const std::string& s, const std::string& start) { if (s.size() < start.size()) return false; return equal(start.begin(), start.end(), s.begin()); } bool strEndsWith(const std::string& s, const std::string& end) { if (s.size() < end.size()) return false; return equal(end.rbegin(), end.rend(), s.rbegin()); } int readLine(std::istream& is, std::stringstream& currentLine) { if (is.eof()) return -1; currentLine.str(""); currentLine.clear(); is.get(*currentLine.rdbuf()); if (is.fail()) // fail is set on empty lines is.clear(); G2O_FSKIP_LINE(is); // read \n not read by get() return static_cast(currentLine.str().size()); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/stuff/string_tools.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_STRING_TOOLS_H #define G2O_STRING_TOOLS_H #include #include #include #include #include "macros.h" namespace g2o { /** @addtogroup utils **/ // @{ /** \file stringTools.h * \brief utility functions for handling strings */ /** * remove whitespaces from the start/end of a string */ std::string trim(const std::string& s); /** * remove whitespaces from the left side of the string */ std::string trimLeft(const std::string& s); /** * remove whitespaced from the right side of the string */ std::string trimRight(const std::string& s); /** * convert the string to lower case */ std::string strToLower(const std::string& s); /** * convert a string to upper case */ std::string strToUpper(const std::string& s); /** * read integer values (seperated by spaces) from a string and store * them in the given OutputIterator. */ template OutputIterator readInts(const char* str, OutputIterator out) { char* cl = (char*)str; char* cle = cl; while (1) { int id = strtol(cl, &cle, 10); if (cl == cle) break; *out++ = id; cl = cle; } return out; } /** * read float values (seperated by spaces) from a string and store * them in the given OutputIterator. */ template OutputIterator readFloats(const char* str, OutputIterator out) { char* cl = (char*)str; char* cle = cl; while (1) { double val = strtod(cl, &cle); if (cl == cle) break; *out++ = val; cl = cle; } return out; } /** * format a string and return a std::string. * Format is just like printf, see man 3 printf */ std::string formatString(const char* fmt, ...) G2O_ATTRIBUTE_FORMAT12; /** * replacement function for sprintf which fills a std::string instead of a char* */ int strPrintf(std::string& str, const char* fmt, ...) G2O_ATTRIBUTE_FORMAT23; /** * convert a string into an other type. */ template bool convertString(const std::string& s, T& x, bool failIfLeftoverChars = true) { std::istringstream i(s); char c; if (!(i >> x) || (failIfLeftoverChars && i.get(c))) return false; return true; } /** * convert a string into an other type. * Return the converted value. Throw error if parsing is wrong. */ template T stringToType(const std::string& s, bool failIfLeftoverChars = true) { T x; convertString(s, x, failIfLeftoverChars); return x; } /** * return true, if str starts with substr */ bool strStartsWith(const std::string & str, const std::string& substr); /** * return true, if str ends with substr */ bool strEndsWith(const std::string & str, const std::string& substr); /** * expand the given filename like a posix shell, e.g., ~ $CARMEN_HOME and other will get expanded. * Also command substitution, e.g. `pwd` will give the current directory. */ std::string strExpandFilename(const std::string& filename); /** * split a string into token based on the characters given in delim */ std::vector strSplit(const std::string& s, const std::string& delim); /** * read a line from is into currentLine. * @return the number of characters read into currentLine (excluding newline), -1 on eof() */ int readLine(std::istream& is, std::stringstream& currentLine); // @} } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/stuff/timeutil.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 "timeutil.h" #include #ifdef _WINDOWS #include #include #endif #ifdef UNIX #include #endif namespace g2o { #ifdef _WINDOWS #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 #else #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL #endif struct timezone { int tz_minuteswest; /* minutes W of Greenwich */ int tz_dsttime; /* type of dst correction */ }; int gettimeofday(struct timeval *tv, struct timezone *tz) { // Define a structure to receive the current Windows filetime FILETIME ft; // Initialize the present time to 0 and the timezone to UTC unsigned __int64 tmpres = 0; static int tzflag = 0; if (NULL != tv) { GetSystemTimeAsFileTime(&ft); // The GetSystemTimeAsFileTime returns the number of 100 nanosecond // intervals since Jan 1, 1601 in a structure. Copy the high bits to // the 64 bit tmpres, shift it left by 32 then or in the low 32 bits. tmpres |= ft.dwHighDateTime; tmpres <<= 32; tmpres |= ft.dwLowDateTime; // Convert to microseconds by dividing by 10 tmpres /= 10; // The Unix epoch starts on Jan 1 1970. Need to subtract the difference // in seconds from Jan 1 1601. tmpres -= DELTA_EPOCH_IN_MICROSECS; // Finally change microseconds to seconds and place in the seconds value. // The modulus picks up the microseconds. tv->tv_sec = (long)(tmpres / 1000000UL); tv->tv_usec = (long)(tmpres % 1000000UL); } if (NULL != tz) { if (!tzflag) { _tzset(); tzflag++; } long sec; int hours; _get_timezone(&sec); _get_daylight(&hours); // Adjust for the timezone west of Greenwich tz->tz_minuteswest = sec / 60; tz->tz_dsttime = hours; } return 0; } #endif ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {} ScopeTime::~ScopeTime() { std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n"; } double get_monotonic_time() { #if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK)) struct timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); return ts.tv_sec + ts.tv_nsec*1e-9; #else return get_time(); #endif } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/stuff/timeutil.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_TIMEUTIL_H #define G2O_TIMEUTIL_H #ifdef _WINDOWS #include #else #include #endif #include /** @addtogroup utils **/ // @{ /** \file timeutil.h * \brief utility functions for handling time related stuff */ /// Executes code, only if secs are gone since last exec. /// extended version, in which the current time is given, e.g., timestamp of IPC message #ifndef DO_EVERY_TS #define DO_EVERY_TS(secs, currentTime, code) \ if (1) {\ static double s_lastDone_ = (currentTime); \ double s_now_ = (currentTime); \ if (s_lastDone_ > s_now_) \ s_lastDone_ = s_now_; \ if (s_now_ - s_lastDone_ > (secs)) { \ code; \ s_lastDone_ = s_now_; \ }\ } else \ (void)0 #endif /// Executes code, only if secs are gone since last exec. #ifndef DO_EVERY #define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code) #endif #ifndef MEASURE_TIME #define MEASURE_TIME(text, code) \ if(1) { \ double _start_time_ = g2o::get_time(); \ code; \ fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \ } else \ (void) 0 #endif namespace g2o { #ifdef _WINDOWS typedef struct timeval { long tv_sec; long tv_usec; } timeval; int gettimeofday(struct timeval *tv, struct timezone *tz); #endif /** * return the current time in seconds since 1. Jan 1970 */ inline double get_time() { struct timeval ts; gettimeofday(&ts,0); return ts.tv_sec + ts.tv_usec*1e-6; } /** * return a monotonic increasing time which basically does not need to * have a reference point. Consider this for measuring how long some * code fragments required to execute. * * On Linux we call clock_gettime() on other systems we currently * call get_time(). */ double get_monotonic_time(); /** * \brief Class to measure the time spent in a scope * * To use this class, e.g. to measure the time spent in a function, * just create and instance at the beginning of the function. */ class ScopeTime { public: ScopeTime(const char* title); ~ScopeTime(); private: std::string _title; double _startTime; }; } // end namespace #ifndef MEASURE_FUNCTION_TIME #define MEASURE_FUNCTION_TIME \ g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__) #endif // @} #endif ================================================ FILE: Thirdparty/g2o/g2o/types/CMakeLists.txt ================================================ ADD_LIBRARY(types ${G2O_LIB_TYPE} types_sba.h types_six_dof_expmap.h types_sba.cpp types_six_dof_expmap.cpp types_seven_dof_expmap.cpp types_seven_dof_expmap.h se3quat.h se3_ops.h se3_ops.hpp ) SET_TARGET_PROPERTIES(types PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types) TARGET_LINK_LIBRARIES(types core) ================================================ FILE: Thirdparty/g2o/g2o/types/se3_ops.h ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef G2O_MATH_STUFF #define G2O_MATH_STUFF #include #include namespace g2o { using namespace Eigen; inline Matrix3d skew(const Vector3d&v); inline Vector3d deltaR(const Matrix3d& R); inline Vector2d project(const Vector3d&); inline Vector3d project(const Vector4d&); inline Vector3d unproject(const Vector2d&); inline Vector4d unproject(const Vector3d&); inline Eigen::Quaterniond zyx_euler_to_quat(const double &roll, const double &pitch, const double &yaw); inline void quat_to_euler_zyx(const Eigen::Quaterniond q, double& roll, double& pitch, double& yaw); #include "se3_ops.hpp" } #endif //MATH_STUFF ================================================ FILE: Thirdparty/g2o/g2o/types/se3_ops.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once Matrix3d skew(const Vector3d&v) { Matrix3d m; m.fill(0.); m(0,1) = -v(2); m(0,2) = v(1); m(1,2) = -v(0); m(1,0) = v(2); m(2,0) = -v(1); m(2,1) = v(0); return m; } Vector3d deltaR(const Matrix3d& R) { Vector3d v; v(0)=R(2,1)-R(1,2); v(1)=R(0,2)-R(2,0); v(2)=R(1,0)-R(0,1); return v; } Vector2d project(const Vector3d& v) { Vector2d res; res(0) = v(0)/v(2); res(1) = v(1)/v(2); return res; } Vector3d project(const Vector4d& v) { Vector3d res; res(0) = v(0)/v(3); res(1) = v(1)/v(3); res(2) = v(2)/v(3); return res; } Vector3d unproject(const Vector2d& v) { Vector3d res; res(0) = v(0); res(1) = v(1); res(2) = 1; return res; } Vector4d unproject(const Vector3d& v) { Vector4d res; res(0) = v(0); res(1) = v(1); res(2) = v(2); res(3) = 1; return res; } Eigen::Quaterniond zyx_euler_to_quat(const double &roll, const double &pitch, const double &yaw) { double sy = sin(yaw*0.5); double cy = cos(yaw*0.5); double sp = sin(pitch*0.5); double cp = cos(pitch*0.5); double sr = sin(roll*0.5); double cr = cos(roll*0.5); double w = cr*cp*cy + sr*sp*sy; double x = sr*cp*cy - cr*sp*sy; double y = cr*sp*cy + sr*cp*sy; double z = cr*cp*sy - sr*sp*cy; return Eigen::Quaterniond(w,x,y,z); } void quat_to_euler_zyx(const Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) { const double qw = q.w(); const double qx = q.x(); const double qy = q.y(); const double qz = q.z(); roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy)); pitch = asin(2*(qw*qy-qz*qx)); yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz)); } ================================================ FILE: Thirdparty/g2o/g2o/types/se3quat.h ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef G2O_SE3QUAT_H_ #define G2O_SE3QUAT_H_ #include "se3_ops.h" #include #include namespace g2o { using namespace Eigen; typedef Matrix Vector6d; typedef Matrix Vector7d; class SE3Quat { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; protected: Quaterniond _r; Vector3d _t; public: SE3Quat(){ _r.setIdentity(); _t.setZero(); } SE3Quat(const Matrix3d& R, const Vector3d& t):_r(Quaterniond(R)),_t(t){ normalizeRotation(); } SE3Quat(const Quaterniond& q, const Vector3d& t):_r(q),_t(t){ normalizeRotation(); } // x y z qx qy qz qw SE3Quat(const Vector7d& v){ fromVector(v); normalizeRotation(); } /** * templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6d or Map */ template explicit SE3Quat(const MatrixBase& v) { assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match"); if (v.size() == 6) { for (int i=0; i<3; i++){ _t[i]=v[i]; _r.coeffs()(i)=v[i+3]; } _r.w() = 0.; // recover the positive w if (_r.norm()>1.){ _r.normalize(); } else { double w2=1.-_r.squaredNorm(); _r.w()= (w2<0.) ? 0. : sqrt(w2); } } else if (v.size() == 7) { int idx = 0; for (int i=0; i<3; ++i, ++idx) _t(i) = v(idx); for (int i=0; i<4; ++i, ++idx) _r.coeffs()(i) = v(idx); normalizeRotation(); } } inline const Vector3d& translation() const {return _t;} inline void setTranslation(const Vector3d& t_) {_t = t_;} inline const Quaterniond& rotation() const {return _r;} void setRotation(const Quaterniond& r_) {_r=r_;} inline SE3Quat operator* (const SE3Quat& tr2) const{ SE3Quat result(*this); result._t += _r*tr2._t; result._r*=tr2._r; result.normalizeRotation(); return result; } inline SE3Quat& operator*= (const SE3Quat& tr2){ _t+=_r*tr2._t; _r*=tr2._r; normalizeRotation(); return *this; } inline Vector3d operator* (const Vector3d& v) const { return _t+_r*v; } inline SE3Quat inverse() const{ SE3Quat ret; ret._r=_r.conjugate(); ret._t=ret._r*(_t*-1.); return ret; } inline double operator [](int i) const { assert(i<7); if (i<3) return _t[i]; return _r.coeffs()[i-3]; } // x y z qx qy qz qw inline Vector7d toVector() const{ Vector7d v; v[0]=_t(0); v[1]=_t(1); v[2]=_t(2); v[3]=_r.x(); v[4]=_r.y(); v[5]=_r.z(); v[6]=_r.w(); return v; } // x y z qx qy qz qw inline void fromVector(const Vector7d& v){ _r=Quaterniond(v[6], v[3], v[4], v[5]); _t=Vector3d(v[0], v[1], v[2]); } // x y z qx qy qz inline Vector6d toMinimalVector() const{ Vector6d v; v[0]=_t(0); v[1]=_t(1); v[2]=_t(2); v[3]=_r.x(); v[4]=_r.y(); v[5]=_r.z(); return v; } // x y z qx qy qz inline void fromMinimalVector(const Vector6d& v){ double w = 1.-v[3]*v[3]-v[4]*v[4]-v[5]*v[5]; if (w>0){ _r=Quaterniond(sqrt(w), v[3], v[4], v[5]); } else { _r=Quaterniond(0, -v[3], -v[4], -v[5]); } _t=Vector3d(v[0], v[1], v[2]); } // copied from my quat to euler void quat_to_euler_zyx_infuc(const Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) const { const double qw = q.w(); const double qx = q.x(); const double qy = q.y(); const double qz = q.z(); roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy)); pitch = asin(2*(qw*qy-qz*qx)); yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz)); } inline Vector6d toXYZPRYVector() const{ double yaw, pitch, roll; quat_to_euler_zyx_infuc(_r, roll, pitch, yaw); Vector6d v; v[0]=_t(0); v[1]=_t(1); v[2]=_t(2); v[3]=roll; v[4]=pitch; v[5]=yaw; return v; } Eigen::Quaterniond zyx_euler_to_quat(const double &roll, const double &pitch, const double &yaw) { double sy = sin(yaw*0.5); double cy = cos(yaw*0.5); double sp = sin(pitch*0.5); double cp = cos(pitch*0.5); double sr = sin(roll*0.5); double cr = cos(roll*0.5); double w = cr*cp*cy + sr*sp*sy; double x = sr*cp*cy - cr*sp*sy; double y = cr*sp*cy + sr*cp*sy; double z = cr*cp*sy - sr*sp*cy; return Eigen::Quaterniond(w,x,y,z); } inline void fromXYZPRYVector(const Vector6d& v){ _r = zyx_euler_to_quat(v[3], v[4], v[5]); _t=Vector3d(v[0], v[1], v[2]); } Vector6d log() const { Vector6d res; Matrix3d _R = _r.toRotationMatrix(); double d = 0.5*(_R(0,0)+_R(1,1)+_R(2,2)-1); Vector3d omega; Vector3d upsilon; Vector3d dR = deltaR(_R); Matrix3d V_inv; if (d>0.99999) { omega=0.5*dR; Matrix3d Omega = skew(omega); V_inv = Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega); } else { double theta = acos(d); omega = theta/(2*sqrt(1-d*d))*dR; Matrix3d Omega = skew(omega); V_inv = ( Matrix3d::Identity() - 0.5*Omega + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) ); } upsilon = V_inv*_t; for (int i=0; i<3;i++){ res[i]=omega[i]; } for (int i=0; i<3;i++){ res[i+3]=upsilon[i]; } return res; } Vector3d map(const Vector3d & xyz) const { return _r*xyz + _t; } // angle, translation static SE3Quat exp(const Vector6d & update) // doesn't multiply into current... { Vector3d omega; for (int i=0; i<3; i++) omega[i]=update[i]; Vector3d upsilon; for (int i=0; i<3; i++) upsilon[i]=update[i+3]; double theta = omega.norm(); Matrix3d Omega = skew(omega); Matrix3d R; Matrix3d V; Quaterniond q; if (theta<0.00001) { //TODO: CHECK WHETHER THIS IS CORRECT!!! R = (Matrix3d::Identity() + Omega + Omega*Omega);// must add Omega... though it is small // double theta_sq=theta*theta;double theta_po4=theta_sq*theta_sq; // from LSD sophus // q.w() = 1-0.5*theta_sq + (1.0/384.0)*theta_po4; // q.vec() = (0.5-1.0/48.0*theta_sq + 1.0/3840.0*theta_po4)*omega; // q.normalize(); V = R; } else { Matrix3d Omega2 = Omega*Omega; R = (Matrix3d::Identity() + sin(theta)/theta *Omega + (1-cos(theta))/(theta*theta)*Omega2); // q.vec() = omega/theta*sin(theta/2); // from LSD sophus // q.w() = cos(theta/2); // q.normalize(); V = (Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2); } // if (q.w()<1) // std::cout<<"Quaterniond(R) "< adj() const { Matrix3d R = _r.toRotationMatrix(); Matrix res; res.block(0,0,3,3) = R; res.block(3,3,3,3) = R; res.block(3,0,3,3) = skew(_t)*R; res.block(0,3,3,3) = Matrix3d::Zero(3,3); return res; } Matrix to_homogeneous_matrix() const { Matrix homogeneous_matrix; homogeneous_matrix.setIdentity(); homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix(); homogeneous_matrix.col(3).head(3) = translation(); return homogeneous_matrix; } void normalizeRotation(){ if (_r.w()<0){ _r.coeffs() *= -1; } _r.normalize(); } /** * cast SE3Quat into an Eigen::Isometry3d */ operator Eigen::Isometry3d() const { Eigen::Isometry3d result = (Eigen::Isometry3d) rotation(); result.translation() = translation(); return result; } }; inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3) { out_str << se3.to_homogeneous_matrix() << std::endl; return out_str; } } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/types/sim3.h ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef G2O_SIM_3 #define G2O_SIM_3 #include "se3_ops.h" #include namespace g2o { using namespace Eigen; typedef Matrix Vector7d; typedef Matrix Matrix7d; struct Sim3 { EIGEN_MAKE_ALIGNED_OPERATOR_NEW protected: Quaterniond r; Vector3d t; double s; public: Sim3() { r.setIdentity(); t.fill(0.); s=1.; } Sim3(const Quaterniond & r, const Vector3d & t, double s) : r(r),t(t),s(s) { } Sim3(const Matrix3d & R, const Vector3d & t, double s) : r(Quaterniond(R)),t(t),s(s) { } Sim3(const Vector7d & update) // exponential update { Vector3d omega; for (int i=0; i<3; i++) omega[i]=update[i]; Vector3d upsilon; for (int i=0; i<3; i++) upsilon[i]=update[i+3]; double sigma = update[6]; double theta = omega.norm(); Matrix3d Omega = skew(omega); s = std::exp(sigma); Matrix3d Omega2 = Omega*Omega; Matrix3d I; I.setIdentity(); Matrix3d R; double eps = 0.00001; double A,B,C; if (fabs(sigma)1-eps) { omega=0.5*deltaR(R); Omega = skew(omega); A = 1./2.; B = 1./6.; } else { double theta = acos(d); double theta2 = theta*theta; omega = theta/(2*sqrt(1-d*d))*deltaR(R); Omega = skew(omega); A = (1-cos(theta))/(theta2); B = (theta-sin(theta))/(theta2*theta); } } else { C=(s-1)/sigma; if (d>1-eps) { double sigma2 = sigma*sigma; omega=0.5*deltaR(R); Omega = skew(omega); A = ((sigma-1)*s+1)/(sigma2); B = ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma); } else { double theta = acos(d); omega = theta/(2*sqrt(1-d*d))*deltaR(R); Omega = skew(omega); double theta2 = theta*theta; double a=s*sin(theta); double b=s*cos(theta); double c=theta2 + sigma*sigma; A = (a*sigma+ (1-b)*theta)/(theta*c); B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2); } } Matrix3d W = A*Omega + B*Omega*Omega + C*I; upsilon = W.lu().solve(t); for (int i=0; i<3; i++) res[i] = omega[i]; for (int i=0; i<3; i++) res[i+3] = upsilon[i]; res[6] = sigma; return res; } Sim3 inverse() const { return Sim3(r.conjugate(), r.conjugate()*((-1./s)*t), 1./s); } double operator[](int i) const { assert(i<8); if (i<4){ return r.coeffs()[i]; } if (i<7){ return t[i-4]; } return s; } double& operator[](int i) { assert(i<8); if (i<4){ return r.coeffs()[i]; } if (i<7) { return t[i-4]; } return s; } Sim3 operator *(const Sim3& other) const { Sim3 ret; ret.r = r*other.r; ret.t=s*(r*other.t)+t; ret.s=s*other.s; return ret; } Sim3& operator *=(const Sim3& other){ Sim3 ret=(*this)*other; *this=ret; return *this; } inline const Vector3d& translation() const {return t;} inline Vector3d& translation() {return t;} inline const Quaterniond& rotation() const {return r;} inline Quaterniond& rotation() {return r;} inline const double& scale() const {return s;} inline double& scale() {return s;} }; inline std::ostream& operator <<(std::ostream& out_str, const Sim3& sim3) { out_str << sim3.rotation().coeffs() << std::endl; out_str << sim3.translation() << std::endl; out_str << sim3.scale() << std::endl; return out_str; } } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/types/types_sba.cpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 Kurt Konolige // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "types_sba.h" #include namespace g2o { using namespace std; VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>() { } bool VertexSBAPointXYZ::read(std::istream& is) { Vector3d lv; for (int i=0; i<3; i++) is >> _estimate[i]; return true; } bool VertexSBAPointXYZ::write(std::ostream& os) const { Vector3d lv=estimate(); for (int i=0; i<3; i++){ os << lv[i] << " "; } return os.good(); } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/types/types_sba.h ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 Kurt Konolige // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef G2O_SBA_TYPES #define G2O_SBA_TYPES #include "../core/base_vertex.h" #include #include namespace g2o { /** * \brief Point vertex, XYZ */ class VertexSBAPointXYZ : public BaseVertex<3, Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSBAPointXYZ(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void setToOriginImpl() { _estimate.fill(0.); } virtual void oplusImpl(const double* update) { Eigen::Map v(update); _estimate += v; } }; } // end namespace #endif // SBA_TYPES ================================================ FILE: Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "types_seven_dof_expmap.h" namespace g2o { VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, Sim3>() { _marginalized=false; _fix_scale = false; } EdgeSim3::EdgeSim3() : BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>() { } bool VertexSim3Expmap::read(std::istream& is) { Vector7d cam2world; for (int i=0; i<6; i++){ is >> cam2world[i]; } is >> cam2world[6]; // if (! is) { // // if the scale is not specified we set it to 1; // std::cerr << "!s"; // cam2world[6]=0.; // } for (int i=0; i<2; i++) { is >> _focal_length1[i]; } for (int i=0; i<2; i++) { is >> _principle_point1[i]; } setEstimate(Sim3(cam2world).inverse()); return true; } bool VertexSim3Expmap::write(std::ostream& os) const { Sim3 cam2world(estimate().inverse()); Vector7d lv=cam2world.log(); for (int i=0; i<7; i++){ os << lv[i] << " "; } for (int i=0; i<2; i++) { os << _focal_length1[i] << " "; } for (int i=0; i<2; i++) { os << _principle_point1[i] << " "; } return os.good(); } bool EdgeSim3::read(std::istream& is) { Vector7d v7; for (int i=0; i<7; i++){ is >> v7[i]; } Sim3 cam2world(v7); setMeasurement(cam2world.inverse()); for (int i=0; i<7; i++) for (int j=i; j<7; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeSim3::write(std::ostream& os) const { Sim3 cam2world(measurement().inverse()); Vector7d v7 = cam2world.log(); for (int i=0; i<7; i++) { os << v7[i] << " "; } for (int i=0; i<7; i++) for (int j=i; j<7; j++){ os << " " << information()(i,j); } return os.good(); } /**Sim3ProjectXYZ*/ EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>() { } bool EdgeSim3ProjectXYZ::read(std::istream& is) { for (int i=0; i<2; i++) { is >> _measurement[i]; } for (int i=0; i<2; i++) for (int j=i; j<2; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeSim3ProjectXYZ::write(std::ostream& os) const { for (int i=0; i<2; i++){ os << _measurement[i] << " "; } for (int i=0; i<2; i++) for (int j=i; j<2; j++){ os << " " << information()(i,j); } return os.good(); } /**InverseSim3ProjectXYZ*/ EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>() { } bool EdgeInverseSim3ProjectXYZ::read(std::istream& is) { for (int i=0; i<2; i++) { is >> _measurement[i]; } for (int i=0; i<2; i++) for (int j=i; j<2; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const { for (int i=0; i<2; i++){ os << _measurement[i] << " "; } for (int i=0; i<2; i++) for (int j=i; j<2; j++){ os << " " << information()(i,j); } return os.good(); } // void EdgeSim3ProjectXYZ::linearizeOplus() // { // VertexSim3Expmap * vj = static_cast(_vertices[1]); // Sim3 T = vj->estimate(); // VertexPointXYZ* vi = static_cast(_vertices[0]); // Vector3d xyz = vi->estimate(); // Vector3d xyz_trans = T.map(xyz); // double x = xyz_trans[0]; // double y = xyz_trans[1]; // double z = xyz_trans[2]; // double z_2 = z*z; // Matrix tmp; // tmp(0,0) = _focal_length(0); // tmp(0,1) = 0; // tmp(0,2) = -x/z*_focal_length(0); // tmp(1,0) = 0; // tmp(1,1) = _focal_length(1); // tmp(1,2) = -y/z*_focal_length(1); // _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix(); // _jacobianOplusXj(0,0) = x*y/z_2 * _focal_length(0); // _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *_focal_length(0); // _jacobianOplusXj(0,2) = y/z *_focal_length(0); // _jacobianOplusXj(0,3) = -1./z *_focal_length(0); // _jacobianOplusXj(0,4) = 0; // _jacobianOplusXj(0,5) = x/z_2 *_focal_length(0); // _jacobianOplusXj(0,6) = 0; // scale is ignored // _jacobianOplusXj(1,0) = (1+y*y/z_2) *_focal_length(1); // _jacobianOplusXj(1,1) = -x*y/z_2 *_focal_length(1); // _jacobianOplusXj(1,2) = -x/z *_focal_length(1); // _jacobianOplusXj(1,3) = 0; // _jacobianOplusXj(1,4) = -1./z *_focal_length(1); // _jacobianOplusXj(1,5) = y/z_2 *_focal_length(1); // _jacobianOplusXj(1,6) = 0; // scale is ignored // } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Modified by Raúl Mur Artal (2014) // - Added EdgeInverseSim3ProjectXYZ // - Modified VertexSim3Expmap to represent relative transformation between two cameras. Includes calibration of both cameras. #ifndef G2O_SEVEN_DOF_EXPMAP_TYPES #define G2O_SEVEN_DOF_EXPMAP_TYPES #include "../core/base_vertex.h" #include "../core/base_binary_edge.h" #include "types_six_dof_expmap.h" #include "sim3.h" namespace g2o { using namespace Eigen; /** * \brief Sim3 Vertex, (x,y,z,qw,qx,qy,qz) * the parameterization for the increments constructed is a 7d vector * (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. */ class VertexSim3Expmap : public BaseVertex<7, Sim3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSim3Expmap(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void setToOriginImpl() { _estimate = Sim3(); } virtual void oplusImpl(const double* update_) { Eigen::Map update(const_cast(update_)); if (_fix_scale) update[6] = 0; Sim3 s(update); // exp map setEstimate(s*estimate()); } Vector2d _principle_point1, _principle_point2; Vector2d _focal_length1, _focal_length2; Vector2d cam_map1(const Vector2d & v) const { Vector2d res; res[0] = v[0]*_focal_length1[0] + _principle_point1[0]; res[1] = v[1]*_focal_length1[1] + _principle_point1[1]; return res; } Vector2d cam_map2(const Vector2d & v) const { Vector2d res; res[0] = v[0]*_focal_length2[0] + _principle_point2[0]; res[1] = v[1]*_focal_length2[1] + _principle_point2[1]; return res; } bool _fix_scale; protected: }; /** * \brief 7D edge between two Vertex7 */ class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSim3(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError() { const VertexSim3Expmap* v1 = static_cast(_vertices[0]); const VertexSim3Expmap* v2 = static_cast(_vertices[1]); Sim3 C(_measurement); Sim3 error_=C*v1->estimate()*v2->estimate().inverse(); _error = error_.log(); } virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;} virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) { VertexSim3Expmap* v1 = static_cast(_vertices[0]); VertexSim3Expmap* v2 = static_cast(_vertices[1]); if (from.count(v1) > 0) v2->setEstimate(measurement()*v1->estimate()); else v1->setEstimate(measurement().inverse()*v2->estimate()); } }; /**/ class EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSim3ProjectXYZ(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError() { const VertexSim3Expmap* v1 = static_cast(_vertices[1]); const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); Vector2d obs(_measurement); _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate()))); } // virtual void linearizeOplus(); }; /**/ class EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeInverseSim3ProjectXYZ(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError() { const VertexSim3Expmap* v1 = static_cast(_vertices[1]); const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); Vector2d obs(_measurement); _error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate()))); } // virtual void linearizeOplus(); }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "types_six_dof_expmap.h" #include "../core/factory.h" #include "../stuff/macros.h" namespace g2o { using namespace std; Vector2d project2d(const Vector3d& v) { Vector2d res; res(0) = v(0)/v(2); res(1) = v(1)/v(2); return res; } Vector3d unproject2d(const Vector2d& v) { Vector3d res; res(0) = v(0); res(1) = v(1); res(2) = 1; return res; } VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() { } bool VertexSE3Expmap::read(std::istream& is) { Vector7d est; for (int i=0; i<7; i++) is >> est[i]; SE3Quat cam2world; cam2world.fromVector(est); setEstimate(cam2world.inverse()); return true; } bool VertexSE3Expmap::write(std::ostream& os) const { SE3Quat cam2world(estimate().inverse()); for (int i=0; i<7; i++) os << cam2world[i] << " "; return os.good(); } bool EdgeSE3Expmap::read(std::istream& is) { Vector7d meas; for (int i=0; i<7; i++) is >> meas[i]; SE3Quat cam2world; cam2world.fromVector(meas); setMeasurement(cam2world.inverse()); //TODO: Convert information matrix!! for (int i=0; i<6; i++) for (int j=i; j<6; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeSE3Expmap::write(std::ostream& os) const { SE3Quat cam2world(measurement().inverse()); for (int i=0; i<7; i++) os << cam2world[i] << " "; for (int i=0; i<6; i++) for (int j=i; j<6; j++){ os << " " << information()(i,j); } return os.good(); } // void EdgeSE3Expmap::linearizeOplus() { // VertexSE3Expmap * vi = static_cast(_vertices[0]); // SE3Quat Ti(vi->estimate()); // // VertexSE3Expmap * vj = static_cast(_vertices[1]); // SE3Quat Tj(vj->estimate()); // // const SE3Quat & Tij = _measurement; // SE3Quat invTij = Tij.inverse(); // // SE3Quat invTj_Tij = Tj.inverse()*Tij; // SE3Quat infTi_invTij = Ti.inverse()*invTij; // // _jacobianOplusXi = invTj_Tij.adj(); // _jacobianOplusXj = -infTi_invTij.adj(); // } EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() { } bool EdgeSE3ProjectXYZ::read(std::istream& is){ for (int i=0; i<2; i++){ is >> _measurement[i]; } for (int i=0; i<2; i++) for (int j=i; j<2; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeSE3ProjectXYZ::write(std::ostream& os) const { for (int i=0; i<2; i++){ os << measurement()[i] << " "; } for (int i=0; i<2; i++) for (int j=i; j<2; j++){ os << " " << information()(i,j); } return os.good(); } void EdgeSE3ProjectXYZ::linearizeOplus() { VertexSE3Expmap * vj = static_cast(_vertices[1]); SE3Quat T(vj->estimate()); VertexSBAPointXYZ* vi = static_cast(_vertices[0]); Vector3d xyz = vi->estimate(); Vector3d xyz_trans = T.map(xyz); double x = xyz_trans[0]; double y = xyz_trans[1]; double z = xyz_trans[2]; double z_2 = z*z; Matrix tmp; tmp(0,0) = fx; tmp(0,1) = 0; tmp(0,2) = -x/z*fx; tmp(1,0) = 0; tmp(1,1) = fy; tmp(1,2) = -y/z*fy; _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix(); _jacobianOplusXj(0,0) = x*y/z_2 *fx; _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx; _jacobianOplusXj(0,2) = y/z *fx; _jacobianOplusXj(0,3) = -1./z *fx; _jacobianOplusXj(0,4) = 0; _jacobianOplusXj(0,5) = x/z_2 *fx; _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy; _jacobianOplusXj(1,1) = -x*y/z_2 *fy; _jacobianOplusXj(1,2) = -x/z *fy; _jacobianOplusXj(1,3) = 0; _jacobianOplusXj(1,4) = -1./z *fy; _jacobianOplusXj(1,5) = y/z_2 *fy; } Vector2d EdgeSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz) const{ Vector2d proj = project2d(trans_xyz); Vector2d res; res[0] = proj[0]*fx + cx; res[1] = proj[1]*fy + cy; return res; } Vector3d EdgeStereoSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz, const float &bf) const{ const float invz = 1.0f/trans_xyz[2]; Vector3d res; res[0] = trans_xyz[0]*invz*fx + cx; res[1] = trans_xyz[1]*invz*fy + cy; res[2] = res[0] - bf*invz; return res; } EdgeStereoSE3ProjectXYZ::EdgeStereoSE3ProjectXYZ() : BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>() { } bool EdgeStereoSE3ProjectXYZ::read(std::istream& is){ for (int i=0; i<=3; i++){ is >> _measurement[i]; } for (int i=0; i<=2; i++) for (int j=i; j<=2; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeStereoSE3ProjectXYZ::write(std::ostream& os) const { for (int i=0; i<=3; i++){ os << measurement()[i] << " "; } for (int i=0; i<=2; i++) for (int j=i; j<=2; j++){ os << " " << information()(i,j); } return os.good(); } void EdgeStereoSE3ProjectXYZ::linearizeOplus() { VertexSE3Expmap * vj = static_cast(_vertices[1]); SE3Quat T(vj->estimate()); VertexSBAPointXYZ* vi = static_cast(_vertices[0]); Vector3d xyz = vi->estimate(); Vector3d xyz_trans = T.map(xyz); const Matrix3d R = T.rotation().toRotationMatrix(); double x = xyz_trans[0]; double y = xyz_trans[1]; double z = xyz_trans[2]; double z_2 = z*z; _jacobianOplusXi(0,0) = -fx*R(0,0)/z+fx*x*R(2,0)/z_2; _jacobianOplusXi(0,1) = -fx*R(0,1)/z+fx*x*R(2,1)/z_2; _jacobianOplusXi(0,2) = -fx*R(0,2)/z+fx*x*R(2,2)/z_2; _jacobianOplusXi(1,0) = -fy*R(1,0)/z+fy*y*R(2,0)/z_2; _jacobianOplusXi(1,1) = -fy*R(1,1)/z+fy*y*R(2,1)/z_2; _jacobianOplusXi(1,2) = -fy*R(1,2)/z+fy*y*R(2,2)/z_2; _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*R(2,0)/z_2; _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)-bf*R(2,1)/z_2; _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*R(2,2)/z_2; _jacobianOplusXj(0,0) = x*y/z_2 *fx; _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx; _jacobianOplusXj(0,2) = y/z *fx; _jacobianOplusXj(0,3) = -1./z *fx; _jacobianOplusXj(0,4) = 0; _jacobianOplusXj(0,5) = x/z_2 *fx; _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy; _jacobianOplusXj(1,1) = -x*y/z_2 *fy; _jacobianOplusXj(1,2) = -x/z *fy; _jacobianOplusXj(1,3) = 0; _jacobianOplusXj(1,4) = -1./z *fy; _jacobianOplusXj(1,5) = y/z_2 *fy; _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2; _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2; _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2); _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3); _jacobianOplusXj(2,4) = 0; _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2; } //Only Pose bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is){ for (int i=0; i<2; i++){ is >> _measurement[i]; } for (int i=0; i<2; i++) for (int j=i; j<2; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const { for (int i=0; i<2; i++){ os << measurement()[i] << " "; } for (int i=0; i<2; i++) for (int j=i; j<2; j++){ os << " " << information()(i,j); } return os.good(); } void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() { VertexSE3Expmap * vi = static_cast(_vertices[0]); Vector3d xyz_trans = vi->estimate().map(Xw); double x = xyz_trans[0]; double y = xyz_trans[1]; double invz = 1.0/xyz_trans[2]; double invz_2 = invz*invz; _jacobianOplusXi(0,0) = x*y*invz_2 *fx; _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx; _jacobianOplusXi(0,2) = y*invz *fx; _jacobianOplusXi(0,3) = -invz *fx; _jacobianOplusXi(0,4) = 0; _jacobianOplusXi(0,5) = x*invz_2 *fx; _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy; _jacobianOplusXi(1,1) = -x*y*invz_2 *fy; _jacobianOplusXi(1,2) = -x*invz *fy; _jacobianOplusXi(1,3) = 0; _jacobianOplusXi(1,4) = -invz *fy; _jacobianOplusXi(1,5) = y*invz_2 *fy; } Vector2d EdgeSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{ Vector2d proj = project2d(trans_xyz); Vector2d res; res[0] = proj[0]*fx + cx; res[1] = proj[1]*fy + cy; return res; } Vector3d EdgeStereoSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{ const float invz = 1.0f/trans_xyz[2]; Vector3d res; res[0] = trans_xyz[0]*invz*fx + cx; res[1] = trans_xyz[1]*invz*fy + cy; res[2] = res[0] - bf*invz; return res; } bool EdgeStereoSE3ProjectXYZOnlyPose::read(std::istream& is){ for (int i=0; i<=3; i++){ is >> _measurement[i]; } for (int i=0; i<=2; i++) for (int j=i; j<=2; j++) { is >> information()(i,j); if (i!=j) information()(j,i)=information()(i,j); } return true; } bool EdgeStereoSE3ProjectXYZOnlyPose::write(std::ostream& os) const { for (int i=0; i<=3; i++){ os << measurement()[i] << " "; } for (int i=0; i<=2; i++) for (int j=i; j<=2; j++){ os << " " << information()(i,j); } return os.good(); } void EdgeStereoSE3ProjectXYZOnlyPose::linearizeOplus() { VertexSE3Expmap * vi = static_cast(_vertices[0]); Vector3d xyz_trans = vi->estimate().map(Xw); double x = xyz_trans[0]; double y = xyz_trans[1]; double invz = 1.0/xyz_trans[2]; double invz_2 = invz*invz; _jacobianOplusXi(0,0) = x*y*invz_2 *fx; _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx; _jacobianOplusXi(0,2) = y*invz *fx; _jacobianOplusXi(0,3) = -invz *fx; _jacobianOplusXi(0,4) = 0; _jacobianOplusXi(0,5) = x*invz_2 *fx; _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy; _jacobianOplusXi(1,1) = -x*y*invz_2 *fy; _jacobianOplusXi(1,2) = -x*invz *fy; _jacobianOplusXi(1,3) = 0; _jacobianOplusXi(1,4) = -invz *fy; _jacobianOplusXi(1,5) = y*invz_2 *fy; _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*y*invz_2; _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)+bf*x*invz_2; _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2); _jacobianOplusXi(2,3) = _jacobianOplusXi(0,3); _jacobianOplusXi(2,4) = 0; _jacobianOplusXi(2,5) = _jacobianOplusXi(0,5)-bf*invz_2; } } // end namespace ================================================ FILE: Thirdparty/g2o/g2o/types/types_six_dof_expmap.h ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 H. Strasdat // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Modified by Raúl Mur Artal (2014) // Added EdgeSE3ProjectXYZ (project using focal_length in x,y directions) // Modified by Raúl Mur Artal (2016) // Added EdgeStereoSE3ProjectXYZ (project using focal_length in x,y directions) // Added EdgeSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose) // Added EdgeStereoSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose) #ifndef G2O_SIX_DOF_TYPES_EXPMAP #define G2O_SIX_DOF_TYPES_EXPMAP #include "../core/base_vertex.h" #include "../core/base_binary_edge.h" #include "../core/base_unary_edge.h" #include "se3_ops.h" #include "se3quat.h" #include "types_sba.h" #include namespace g2o { namespace types_six_dof_expmap { void init(); } using namespace Eigen; typedef Matrix Matrix6d; /** * \brief SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map stores world to cam */ class VertexSE3Expmap : public BaseVertex<6, SE3Quat>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSE3Expmap(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void setToOriginImpl() { _estimate = SE3Quat(); } virtual void oplusImpl(const double* update_) { Eigen::Map update(update_); setEstimate(SE3Quat::exp(update)*estimate()); } }; /** * \brief 6D edge between two Vertex6 */ class EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3Expmap(){}; bool read(std::istream& is); bool write(std::ostream& os) const; void computeError() { const VertexSE3Expmap* v1 = static_cast(_vertices[0]); const VertexSE3Expmap* v2 = static_cast(_vertices[1]); SE3Quat C(_measurement); SE3Quat error_= C*v1->estimate()*v2->estimate().inverse(); // from sim3 orbslam vertex 保存的是 world to cam. measurent 是 from 1 to 2 // SE3Quat error_=C*v2->estimate().inverse()*v1->estimate(); // similar as LSD _error = error_.log(); } g2o::Vector6d showError() { const VertexSE3Expmap* v1 = static_cast(_vertices[0]); const VertexSE3Expmap* v2 = static_cast(_vertices[1]); SE3Quat C(_measurement); SE3Quat error_= C*v1->estimate()*v2->estimate().inverse(); // from sim3 orbslam // SE3Quat error_=C*v2->estimate().inverse()*v1->estimate(); // similar as LSD std::cout<<"error_ "<(_vertices[0]); const VertexSE3Expmap* v2 = static_cast(_vertices[1]); SE3Quat C(_measurement); SE3Quat error_= C*v1->estimate()*v2->estimate().inverse(); // from sim3 orbslam // SE3Quat error_=C*v2->estimate().inverse()*v1->estimate(); // similar as LSD return error_.log().norm(); } virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;} virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) { VertexSE3Expmap* v1 = static_cast(_vertices[0]); VertexSE3Expmap* v2 = static_cast(_vertices[1]); if (from.count(v1) > 0) v2->setEstimate(measurement()*v1->estimate()); // v2->setEstimate(v1->estimate()*measurement()); else v1->setEstimate(measurement().inverse()*v2->estimate()); // v1->setEstimate(v2->estimate()*measurement().inverse()); } void setMeasurement(const SE3Quat& m){ _measurement = m; } // void linearizeOplus(); // compute exact jacobian LSD and raw g2o has this. }; class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3ProjectXYZ(); bool read(std::istream& is); bool write(std::ostream& os) const; void computeError() { const VertexSE3Expmap* v1 = static_cast(_vertices[1]); const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); Vector2d obs(_measurement); _error = obs-cam_project(v1->estimate().map(v2->estimate())); } bool isDepthPositive() { const VertexSE3Expmap* v1 = static_cast(_vertices[1]); const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); return (v1->estimate().map(v2->estimate()))(2)>0.0; } virtual void linearizeOplus(); Vector2d cam_project(const Vector3d & trans_xyz) const; double fx, fy, cx, cy; }; class EdgeStereoSE3ProjectXYZ: public BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeStereoSE3ProjectXYZ(); bool read(std::istream& is); bool write(std::ostream& os) const; void computeError() { const VertexSE3Expmap* v1 = static_cast(_vertices[1]); const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); Vector3d obs(_measurement); _error = obs - cam_project(v1->estimate().map(v2->estimate()),bf); } bool isDepthPositive() { const VertexSE3Expmap* v1 = static_cast(_vertices[1]); const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); return (v1->estimate().map(v2->estimate()))(2)>0.0; } virtual void linearizeOplus(); Vector3d cam_project(const Vector3d & trans_xyz, const float &bf) const; double fx, fy, cx, cy, bf; }; class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3ProjectXYZOnlyPose(){} bool read(std::istream& is); bool write(std::ostream& os) const; void computeError() { const VertexSE3Expmap* v1 = static_cast(_vertices[0]); Vector2d obs(_measurement); _error = obs-cam_project(v1->estimate().map(Xw)); } bool isDepthPositive() { const VertexSE3Expmap* v1 = static_cast(_vertices[0]); return (v1->estimate().map(Xw))(2)>0.0; } virtual void linearizeOplus(); Vector2d cam_project(const Vector3d & trans_xyz) const; Vector3d Xw; double fx, fy, cx, cy; }; class EdgeStereoSE3ProjectXYZOnlyPose: public BaseUnaryEdge<3, Vector3d, VertexSE3Expmap>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeStereoSE3ProjectXYZOnlyPose(){} bool read(std::istream& is); bool write(std::ostream& os) const; void computeError() { const VertexSE3Expmap* v1 = static_cast(_vertices[0]); Vector3d obs(_measurement); _error = obs - cam_project(v1->estimate().map(Xw)); } bool isDepthPositive() { const VertexSE3Expmap* v1 = static_cast(_vertices[0]); return (v1->estimate().map(Xw))(2)>0.0; } virtual void linearizeOplus(); Vector3d cam_project(const Vector3d & trans_xyz) const; Vector3d Xw; double fx, fy, cx, cy, bf; }; } // end namespace #endif ================================================ FILE: Thirdparty/g2o/license-bsd.txt ================================================ g2o - General Graph Optimization Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: 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, # Copyright (c) 2008, 2009 Gael Guennebaud, # Copyright (c) 2009 Benoit Jacob # 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: include/core/BasicEllipsoidEdges.h ================================================ #include "include/core/Ellipsoid.h" #include "Thirdparty/g2o/g2o/core/base_multi_edge.h" #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" #include #include #include namespace g2o { class VertexEllipsoid:public BaseVertex<9,ellipsoid> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexEllipsoid(){}; virtual void setToOriginImpl(); virtual void oplusImpl(const double* update_); virtual bool read(std::istream& is) ; virtual bool write(std::ostream& os) const ; }; // 6 degrees class VertexEllipsoidXYZABC:public BaseVertex<6,ellipsoid> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexEllipsoidXYZABC(){}; virtual void setToOriginImpl(); virtual void oplusImpl(const double* update_); virtual bool read(std::istream& is) ; virtual bool write(std::ostream& os) const ; }; // camera-object 3D error class EdgeSE3Ellipsoid9DOF:public BaseBinaryEdge<9,ellipsoid, VertexSE3Expmap, VertexEllipsoid> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeSE3Ellipsoid9DOF(){}; virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError(); }; // camera -object 2D projection error, rectangle difference of two vertices (left-top and right-bottom points) class EdgeSE3EllipsoidProj:public BaseBinaryEdge<4,Vector4d, VertexSE3Expmap, VertexEllipsoid> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeSE3EllipsoidProj(){}; virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError(); Vector4d getProject(); void setKalib(Matrix3d& K); Matrix3d Kalib; }; // object supporting prior: the Z axis of the object must align with the normal direction of the supporting plane class EdgeEllipsoidGravityPlanePrior:public BaseUnaryEdge<1,Vector4d, VertexEllipsoid> { public: virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError(); }; } ================================================ FILE: include/core/DataAssociation.h ================================================ #ifndef ELLIPSOIDSLAM_DATAASSOCIATION_H #define ELLIPSOIDSLAM_DATAASSOCIATION_H #include #include #include namespace EllipsoidSLAM { class DataAssociationSolver { public: DataAssociationSolver(Map* pMap); // the solver need the ellipsoids in the map std::vector Solve(EllipsoidSLAM::Frame* pFrame, bool mb3D); private: std::vector SolveCostMat(MatrixXd& mat); int CreateInstance(); // return the new instance ID Map* mpMap; bool mbWithAssociation; int miInstanceNum; }; } #endif // ELLIPSOIDSLAM_DATAASSOCIATION_H ================================================ FILE: include/core/Ellipsoid.h ================================================ #pragma once #include "Thirdparty/g2o/g2o/core/base_multi_edge.h" #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" #include #include #include #include "include/utils/matrix_utils.h" typedef Eigen::Matrix Vector9d; typedef Eigen::Matrix Matrix9d; typedef Eigen::Matrix Matrix5d; typedef Eigen::Matrix Matrix38d; typedef Eigen::Matrix Vector10d; typedef Eigen::Matrix Vector6d; typedef Eigen::Matrix Vector5d; typedef Eigen::Matrix Vector2d; typedef Eigen::Matrix Vector4d; typedef Eigen::Matrix Matrix6d; namespace g2o { class ellipsoid{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW SE3Quat pose; // rigid body transformation, object in world coordinate Vector3d scale; // a,b,c : half length of axis x,y,z Vector9d vec_minimal; // x,y,z,roll,pitch,yaw,a,b,c double prob; // probability from single-frame ellipsoid estimation int miLabel; // semantic label. int miInstanceID; // instance id. ellipsoid(); // Copy constructor. ellipsoid(const ellipsoid &e); const ellipsoid& operator=(const ellipsoid& in); // xyz roll pitch yaw half_scale void fromMinimalVector(const Vector9d& v); // xyz quaternion, half_scale void fromVector(const Vector10d& v); const Vector3d& translation() const; void setTranslation(const Vector3d& t_); void setRotation(const Quaterniond& r_); void setRotation(const Matrix3d& R); void setScale(const Vector3d &scale_); // apply update to current ellipsoid. exponential map ellipsoid exp_update(const Vector9d& update); ellipsoid exp_update_XYZABC(const Vector6d& update); // actual error between two ellipsoid. Vector9d ellipsoid_log_error_9dof(const ellipsoid& newone) const; // change front face by rotate along current body z axis. another way of representing cuboid. representing same cuboid (IOU always 1) ellipsoid rotate_ellipsoid(double yaw_angle) const; // function called by g2o. Vector9d min_log_error_9dof(const ellipsoid& newone, bool print_details=false) const; // transform a local cuboid to global cuboid Twc is camera pose. from camera to world ellipsoid transform_from(const SE3Quat& Twc) const; // transform a global cuboid to local cuboid Twc is camera pose. from camera to world ellipsoid transform_to(const SE3Quat& Twc) const; // xyz roll pitch yaw half_scale Vector9d toMinimalVector() const; // xyz quaternion, half_scale Vector10d toVector() const; Matrix4d similarityTransform() const; // Get the projected point of ellipsoid center on image plane Vector2d projectCenterIntoImagePoint(const SE3Quat& campose_cw, const Matrix3d& Kalib); // The ellipsoid is projected onto the image plane to get an ellipse Vector5d projectOntoImageEllipse(const SE3Quat& campose_cw, const Matrix3d& Kalib) const; // Get projection matrix P = K [ R | t ] Matrix3Xd generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const; // Get Q^* Matrix4d generateQuadric() const; // Get the bounding box from ellipse in image plane Vector4d getBoundingBoxFromEllipse(Vector5d &ellipse) const; // Get the projected bounding box in the image plane of the ellipsoid using a camera pose and a calibration matrix. Vector4d getBoundingBoxFromProjection(const SE3Quat& campose_cw, const Matrix3d& Kalib) const; // *** The following 4 functions treat the ellipsoid as external cube // 3x8 matrix storing 8 corners; each row is x y z Matrix3Xd compute3D_BoxCorner() const; Matrix2Xd projectOntoImageBoxCorner(const SE3Quat& campose_cw, const Matrix3d& Kalib) const; // get rectangles after projection [topleft, bottomright] Vector4d projectOntoImageRect(const SE3Quat& campose_cw, const Matrix3d& Kalib) const; // get rectangles after projection [center, width, height] Vector4d projectOntoImageBbox(const SE3Quat& campose_cw, const Matrix3d& Kalib) const; // calculate the IoU error with another ellipsoid double calculateMIoU(const g2o::ellipsoid& e) const; double calculateIntersectionOnZ(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const; double calculateArea(const g2o::ellipsoid& e) const; double calculateIntersectionOnXY(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const; double calculateIntersectionError(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const; void setColor(const Vector3d &color, double alpha = 1.0); Vector3d getColor(); Vector4d getColorWithAlpha(); bool isColorSet(); // whether the camera could see the ellipsoid bool CheckObservability(const SE3Quat& campose_cw); private: bool mbColor; Vector4d mvColor; void UpdateValueFrom(const g2o::ellipsoid& e); // update the basic parameters from the given ellipsoid }; } // g2o ================================================ FILE: include/core/Frame.h ================================================ #ifndef ELLIPSOIDSLAM_FRAME_H #define ELLIPSOIDSLAM_FRAME_H #include #include #include "Ellipsoid.h" namespace EllipsoidSLAM{ class SymmetryOutputData; class Frame{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Frame(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &im); Frame(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB); Frame(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB); int static total_frame; int frame_seq_id; // image topic sequence id, fixed cv::Mat frame_img; // depth img for processing cv::Mat rgb_img; // rgb img for visualization. cv::Mat ellipsoids_2d_img; double timestamp; g2o::VertexSE3Expmap* pose_vertex; g2o::SE3Quat cam_pose_Tcw; // optimized pose world to cam g2o::SE3Quat cam_pose_Twc; // optimized pose cam to world Eigen::MatrixXd mmObservations; // id x1 y1 x2 y2 label rate instanceID std::vector mvbOutlier; // For depth ellipsoid extraction. bool mbHaveLocalObject; std::vector mpLocalObjects; // local 3d ellipsoid }; } #endif //ELLIPSOIDSLAM_FRAME_H ================================================ FILE: include/core/FrameDrawer.h ================================================ #ifndef ELLIPSOIDSLAM_FRAMEDRAWER_H #define ELLIPSOIDSLAM_FRAMEDRAWER_H #include "Map.h" #include "Tracking.h" namespace EllipsoidSLAM{ class Map; class Tracking; class FrameDrawer { public: FrameDrawer(Map* pMap); void setTracker(Tracking* pTracker); cv::Mat drawFrame(); cv::Mat drawFrameOnImage(cv::Mat &im); cv::Mat drawDepthFrame(); cv::Mat getCurrentFrameImage(); cv::Mat getCurrentDepthFrameImage(); private: Map* mpMap; Tracking* mpTracking; cv::Mat drawProjectionOnImage(cv::Mat &im); cv::Mat drawObservationOnImage(cv::Mat &im); cv::Mat mmRGB; cv::Mat mmDepth; }; } #endif //ELLIPSOIDSLAM_FRAMEDRAWER_H ================================================ FILE: include/core/Geometry.h ================================================ #ifndef ELLIPSOIDSLAM_GEOMETRY_H #define ELLIPSOIDSLAM_GEOMETRY_H #include #include #include #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" using namespace std; using namespace Eigen; namespace EllipsoidSLAM { struct PointXYZRGB { double x; double y; double z; unsigned char r; // 0-255 unsigned char g; unsigned char b; int size = 1; }; typedef vector PointCloud; struct camera_intrinsic{ double fx; double fy; double cx; double cy; double scale; }; PointCloud getPointCloud(cv::Mat &depth, cv::Mat &rgb, VectorXd &detect, camera_intrinsic &camera); PointCloud* transformPointCloud(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc); // generate a new cloud void transformPointCloudSelf(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc); // change the points of the origin cloud PointCloud loadPointsToPointVector(Eigen::MatrixXd &pMat); void SetPointCloudProperty(PointCloud* pCloud, uchar r, uchar g, uchar b, int size); Matrix3d getCalibFromCamera(camera_intrinsic &camera); void SavePointCloudToTxt(const string& path, PointCloud* pCloud); Vector3d GetPointcloudCenter(PointCloud* pCloud); Vector3d TransformPoint(Vector3d &point, const Eigen::Matrix4d &T); } #endif //ELLIPSOIDSLAM_GEOMETRY_H ================================================ FILE: include/core/Initializer.h ================================================ #ifndef ELLIPSOIDSLAM_INITIALIZER_H #define ELLIPSOIDSLAM_INITIALIZER_H #include "Ellipsoid.h" #include "Frame.h" #include using namespace Eigen; using namespace g2o; namespace EllipsoidSLAM { class Observation { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW int label; Vector4d bbox; // left-top x1 y1, right-down x2 y2 double rate; // accuracy: 0 - 1.0 Frame* pFrame; // which frame is the observation from int instance; // useless , for debugging }; typedef std::vector Observations; class Observation3D { public: int label; g2o::ellipsoid* pObj; double rate; // prob: 0 - 1.0 Frame* pFrame; }; typedef std::vector Observation3Ds; class Initializer { public: Initializer(int rows, int cols); // image rows and cols /* * pose_mat: x y z qx qy qz qw * detection_mat: x1 y1 x2 y2 (accuracy) */ g2o::ellipsoid initializeQuadric(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib); g2o::ellipsoid initializeQuadric(Observations &obs, Matrix3d &calib); // calculate the error between a given ellipsoid and planes double quadricErrorWithPlanes(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib, g2o::ellipsoid &e); // solve pose and scale from a matrix Q^* and construct an Ellipsoid class g2o::ellipsoid getEllipsoidFromQStar(Matrix4d &QStar); // get the initialization result. bool getInitializeResult(); private: // get the homogeneous expressions of planes MatrixXd getPlanesHomo(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib); // get the vector form of plane constraints to prepare a linear equation system MatrixXd getVectorFromPlanesHomo(MatrixXd &planes); // solve a least square solution from SVD Matrix4d getQStarFromVectors(MatrixXd &planeVecs); Matrix3Xd generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const; MatrixXd fromDetectionsToLines(VectorXd &detection_mat); void getDetectionAndPoseMatFromObservations(EllipsoidSLAM::Observations &obs, MatrixXd &pose_mat, MatrixXd &detection_mat); // sort eigen values in ascending order. void sortEigenValues(VectorXcd &eigens, MatrixXcd &V); bool mbResult; int miImageRows, miImageCols; }; } #endif //ELLIPSOIDSLAM_INITIALIZER_H ================================================ FILE: include/core/Map.h ================================================ #ifndef ELLIPSOIDSLAM_MAP_H #define ELLIPSOIDSLAM_MAP_H #include "Ellipsoid.h" #include "Geometry.h" #include "Plane.h" #include #include #include using namespace g2o; namespace EllipsoidSLAM { class Map { public: Map(); void addEllipsoid(ellipsoid* pObj); std::vector GetAllEllipsoids(); void addPlane(plane* pPlane); std::vector GetAllPlanes(); void clearPlanes(); void setCameraState(g2o::SE3Quat* state); g2o::SE3Quat* getCameraState(); void addCameraStateToTrajectory(g2o::SE3Quat* state); std::vector getCameraStateTrajectory(); void addPoint(PointXYZRGB* pPoint); void addPointCloud(PointCloud* pPointCloud); void clearPointCloud(); std::vector GetAllPoints(); std::vector getEllipsoidsUsingLabel(int label); std::map GetAllEllipsoidsMap(); bool AddPointCloudList(const string& name, PointCloud* pCloud, int type = 0); // type 0: replace when exist, type 1: add when exist bool DeletePointCloudList(const string& name, int type = 0); // type 0: complete matching, 1: partial matching bool ClearPointCloudLists(); std::map GetPointCloudList(); protected: std::vector mspEllipsoids; std::set mspPlanes; std::mutex mMutexMap; g2o::SE3Quat* mCameraState; // Twc std::vector mvCameraStates; // Twc camera in world std::set mspPoints; std::map mmPointCloudLists; // name-> pClouds public: // those visual ellipsoids are for visualization only and DO NOT join the optimization void addEllipsoidVisual(ellipsoid* pObj); std::vector GetAllEllipsoidsVisual(); void ClearEllipsoidsVisual(); protected: std::vector mspEllipsoidsVisual; }; } #endif //ELLIPSOIDSLAM_MAP_H ================================================ FILE: include/core/MapDrawer.h ================================================ #ifndef ELLIPSOIDSLAM_MAPDRAWER_H #define ELLIPSOIDSLAM_MAPDRAWER_H #include "Map.h" #include #include #include #include using namespace std; namespace EllipsoidSLAM{ class Map; class MapDrawer { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW MapDrawer(const string &strSettingPath, Map* pMap); bool updateObjects(); bool updateCameraState(); bool drawObjects(); bool drawCameraState(); bool drawEllipsoids(); bool drawPlanes(); bool drawPoints(); void setCalib(Eigen::Matrix3d& calib); bool drawTrajectory(); void SE3ToOpenGLCameraMatrix(g2o::SE3Quat &matIn, pangolin::OpenGlMatrix &M); // inverse matIn void SE3ToOpenGLCameraMatrixOrigin(g2o::SE3Quat &matIn, pangolin::OpenGlMatrix &M); // don't inverse matIn void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); void drawPointCloudLists(); // draw all the point cloud lists void drawPointCloudWithOptions(const std::map &options); // draw the point cloud lists with options opened private: float mKeyFrameSize; float mKeyFrameLineWidth; float mGraphLineWidth; float mPointSize; float mCameraSize; float mCameraLineWidth; cv::Mat mCameraPose; std::mutex mMutexCamera; Map* mpMap; Eigen::Matrix3d mCalib; void drawPlaneWithEquation(plane* p); pangolin::OpenGlMatrix getGLMatrixFromCenterAndNormal(Vector3f& center, Vector3f& normal); }; } #endif //ELLIPSOIDSLAM_MAPDRAWER_H ================================================ FILE: include/core/Optimizer.h ================================================ #ifndef ELLIPSOIDSLAM_OPTIMIZER_H #define ELLIPSOIDSLAM_OPTIMIZER_H #include "Frame.h" #include "Map.h" #include "Initializer.h" #include "src/symmetry/Symmetry.h" namespace EllipsoidSLAM { class Frame; class Map; class Optimizer { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Optimizer(); void GlobalObjectGraphOptimization(std::vector &pFrames, Map *pMap, int rows, int cols, Matrix3d &mCalib, std::map& objectObservations, bool save_graph = false, bool withAssociation = false, bool check_visibility = false ); void SetGroundPlane(Vector4d& normal); private: std::map> mMapObjectConstrain; bool mbGroundPlaneSet; Vector4d mGroundPlaneNormal; }; } #endif //ELLIPSOIDSLAM_OPTIMIZER_H ================================================ FILE: include/core/Plane.h ================================================ #pragma once #include #include #include #include "include/utils/matrix_utils.h" #include "Ellipsoid.h" namespace g2o { class plane { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW plane(); plane(Vector4d param, Vector3d color=Vector3d(1.0,0.0,0.0)); // Copy constructor. plane(const plane &p); const plane& operator=(const plane& p); // initialize a plane from a point and a normal vector void fromPointAndNormal(const Vector3d &point, const Vector3d &normal); void fromDisAndAngle(double dis, double angle); void fromDisAngleTrans(double dis, double angle, double trans); // update plane parameters in 3 degrees plane exp_update(const Vector3d& update); // update plane parameters in 2 degrees plane exp_update2DOF(const Vector2d& update); // distance from a point to plane double distanceToPoint(const Vector3d& point, bool keep_flag = false); void transform(g2o::SE3Quat& Twc); // init visualization for a finite plane given a point and a size void InitFinitePlane(const Vector3d& center, double size); // update function for optimization inline void oplus(const Vector3d& v){ //construct a normal from azimuth and evelation; double _azimuth=v[0]; double _elevation=v[1]; double s=std::sin(_elevation), c=std::cos(_elevation); Vector3d n (c*std::cos(_azimuth), c*std::sin(_azimuth), s) ; // rotate the normal Matrix3d R=rotation(normal()); double d=distance()+v[2]; // why is plus? param.head<3>() = R*n; param(3) = -d; normalize(param); } // update function for optimization inline void oplus_dual(const Vector3d& v){ //construct a normal from azimuth and evelation; double _azimuth=v[0]; double _elevation=0; double s=std::sin(_elevation), c=std::cos(_elevation); Vector3d n (c*std::cos(_azimuth), c*std::sin(_azimuth), s) ; // rotate the normal Matrix3d R=rotation(normal()); double d=distance()+v[1]; // why is plus? param.head<3>() = R*n; param(3) = -d; normalize(param); mdDualDis += v[2]; } static inline void normalize(Vector4d& coeffs) { double n=coeffs.head<3>().norm(); coeffs = coeffs * (1./n); } Vector3d normal() const { return param.head<3>(); } static Matrix3d rotation(const Vector3d& v) { double _azimuth = azimuth(v); double _elevation = elevation(v); return (AngleAxisd(_azimuth, Vector3d::UnitZ())* AngleAxisd(- _elevation, Vector3d::UnitY())).toRotationMatrix(); } // self double azimuth() const { return atan2(param[1], param[0]); } static double azimuth(const Vector3d& v) { return std::atan2(v(1),v(0)); } static double elevation(const Vector3d& v) { return std::atan2(v(2), v.head<2>().norm()); } double distance() const { return -param(3); } Eigen::Vector4d GeneratePlaneVec(); Eigen::Vector4d GenerateAnotherPlaneVec(); // finite plane parameters; treat as a square double mdPlaneSize; // side length (meter) bool mbLimited; // dual plane parameter double mdDualDis; Vector4d param; // A B C : AX+BY+CZ+D=0 Vector3d color; // r g b , [0,1.0] Vector3d mvPlaneCenter; // the center of the square. roughly defined. private: Eigen::Vector3d GetLineFromCenterAngle(const Eigen::Vector2d center, double angle); Eigen::Vector4d LineToPlane(const Eigen::Vector3d line); }; } // namespace g2o ================================================ FILE: include/core/System.h ================================================ #ifndef ELLIPSOIDSLAM_SYSTEM_H #define ELLIPSOIDSLAM_SYSTEM_H #include "Frame.h" #include "Map.h" #include "MapDrawer.h" #include "Viewer.h" #include "Tracking.h" #include "FrameDrawer.h" #include #include #include namespace EllipsoidSLAM{ class Viewer; class Frame; class Map; class Tracking; class FrameDrawer; class System { public: System(const string &strSettingsFile, const bool bUseViewer = true); // Interface. // timestamp: sec // pose: camera pose in the world coordinate ; x y z qx qy qz qw // imDepth: depth image; CV16UC1 // imRGB: rgb image for visualization; CV8UC3, BGR // bboxMat: id x1 y1 x2 y2 label prob [Instance] // withAssociation: // true, use [instance] as association result; false, use single-frame ellipsoid estimation to solve data association bool TrackWithObjects(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd & bboxMat, const cv::Mat &imDepth, const cv::Mat &imRGB = cv::Mat(), bool withAssociation = false); Map* getMap(); MapDrawer* getMapDrawer(); FrameDrawer* getFrameDrawer(); Viewer* getViewer(); Tracking* getTracker(); // save objects to file void SaveObjectsToFile(string &path); void OpenDepthEllipsoid(); void OpenOptimization(); void CloseOptimization(); private: Map* mpMap; Viewer* mpViewer; MapDrawer* mpMapDrawer; Tracking* mpTracker; FrameDrawer* mpFrameDrawer; std::thread* mptViewer; }; } // namespace EllipsoidSLAM #endif //ELLIPSOIDSLAM_SYSTEM_H ================================================ FILE: include/core/Tracking.h ================================================ #ifndef ELLIPSOIDSLAM_TRACKING_H #define ELLIPSOIDSLAM_TRACKING_H #include "System.h" #include "FrameDrawer.h" #include "Viewer.h" #include "Map.h" #include "MapDrawer.h" #include "Initializer.h" #include "Optimizer.h" #include #include #include #include "DataAssociation.h" #include namespace EllipsoidSLAM{ class System; class FrameDrawer; class MapDrawer; class Map; class Viewer; class Frame; class Initializer; class Optimizer; class Symmetry; class Tracking { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Tracking(System* pSys, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, const string &strSettingPath); bool GrabPoseAndSingleObjectAnnotation(const Eigen::VectorXd &pose, const Eigen::VectorXd &detection); bool GrabPoseAndObjects(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB = cv::Mat(), bool withAssociation = false); bool GrabPoseAndObjects(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB = cv::Mat(), bool withAssociation = false); Frame* mCurrFrame; Eigen::Matrix3d mCalib; void outputBboxMatWithAssociation(); void SaveObjectHistory(const string& path); void OpenOptimization(); void CloseOptimization(); bool SavePointCloudMap(const string& path); std::vector checkKeyFrameForInstances(std::vector& associations); // Single-frame Ellipsoid Extraction void OpenDepthEllipsoid(); // Groundplane Estimation void OpenGroundPlaneEstimation(); void CloseGroundPlaneEstimation(); int GetGroundPlaneEstimationState(); private: void ProcessCurrentFrame(bool withAssociation); void UpdateObjectObservation(Frame* pFrame, bool withAssociation = false); void JudgeInitialization(); bool isKeyFrameForVisualization(); void ProcessVisualization(); void RefreshObjectHistory(); void ProcessGroundPlaneEstimation(); void Update3DObservationDataAssociation(EllipsoidSLAM::Frame* pFrame, std::vector& associations, std::vector& KeyFrameChecks); void UpdateDepthEllipsoidEstimation(EllipsoidSLAM::Frame* pFrame, bool withAssociation); std::vector GetMannualAssociation(Eigen::MatrixXd &obsMat); protected: g2o::ellipsoid* getObjectDataAssociation(const Eigen::VectorXd &pose, const Eigen::VectorXd &detection); // System System* mpSystem; //Drawers Viewer* mpViewer; FrameDrawer* mpFrameDrawer; MapDrawer* mpMapDrawer; Initializer* mpInitializer; //Map Map* mpMap; // Optimizer Optimizer* mpOptimizer; //Calibration matrix cv::Mat mK; cv::Mat mDistCoef; float mbf; camera_intrinsic mCamera; int mRows, mCols; std::vector mvpFrames; // Store observations in a map with instance id. // In the future, storing observations under Ellipsoid class separately would make it clearer. std::map mmObjectObservations; Builder* mpBuilder; // a dense pointcloud builder from visualization std::map mmObjectHistory; bool mbOpenOptimization; bool mbDepthEllipsoidOpened; EllipsoidExtractor* mpEllipsoidExtractor; std::map mmObjectObservations3D; // 3d observations indexed by instance ID DataAssociationSolver* pDASolver; int miGroundPlaneState; // 0: Closed 1: estimating 2: estimated g2o::plane mGroundPlane; PlaneExtractor* pPlaneExtractor; }; } #endif //ELLIPSOIDSLAM_TRACKING_H ================================================ FILE: include/core/Viewer.h ================================================ #ifndef ELLIPSOIDSLAM_VIEWER_H #define ELLIPSOIDSLAM_VIEWER_H #include #include #include "MapDrawer.h" #include "System.h" #include "FrameDrawer.h" #include using namespace std; struct MenuStruct { double min; double max; double def; string name; }; namespace EllipsoidSLAM { class MapDrawer; class System; class FrameDrawer; class Viewer { public: Viewer(const string &strSettingPath, MapDrawer* pMapDrawer); Viewer(System* pSystem, const string &strSettingPath, MapDrawer* pMapDrawer); void SetFrameDrawer(FrameDrawer* p); void run(); bool updateObject(); bool updateCamera(); void RequestFinish(); bool isFinished(); // dynamic menu. int addDoubleMenu(string name, double min, double max, double def); bool getValueDoubleMenu(int id, double &value); private: float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; bool CheckFinish(); void SetFinish(); bool mbFinishRequested; bool mbFinished; std::mutex mMutexFinish; MapDrawer* mpMapDrawer; System* mpSystem; FrameDrawer* mpFrameDrawer; // menu lists vector*> mvDoubleMenus; vector mvMenuStruct; // Manual control of the point cloud visualization map*> mmPointCloudOptionMenus; std::map mmPointCloudOptionMap; void RefreshPointCloudOptions(); void RefreshMenu(); int miRows; int miCols; }; } #endif //ELLIPSOIDSLAM_VIEWER_H ================================================ FILE: include/utils/dataprocess_utils.h ================================================ #ifndef ELLIPSOIDSLAM_DATAPROCESS_UTILS_H #define ELLIPSOIDSLAM_DATAPROCESS_UTILS_H // Eigen #include #include #include #include using namespace std; // read data from a txt file and store as an eigen matrix Eigen::MatrixXd readDataFromFile(const char* fileName, bool dropFirstline = false); std::vector> readStringFromFile(const char* fileName, int dropLines = 0); // save eigen matrix data to a text file bool saveMatToFile(Eigen::MatrixXd &matIn, const char* fileName); // get all the file names under a dir. void GetFileNamesUnderDir(string path,vector& filenames); string splitFileNameFromFullDir(string &s, bool bare = false); // sort file names in ascending order void sortFileNames(vector& filenames, vector& filenamesSorted); bool calibrateMeasurement(Eigen::Vector4d &measure , int rows, int cols, int config_boarder = 10, int config_size = 100); #endif //ELLIPSOIDSLAM_DATAPROCESS_UTILS_H ================================================ FILE: include/utils/matrix_utils.h ================================================ #ifndef ELLIPSOIDSLAM_MATRIX_UTILS_H #define ELLIPSOIDSLAM_MATRIX_UTILS_H #include // Eigen #include #include Eigen::Matrix4d getTransformFromVector(Eigen::VectorXd& pose); void addVecToMatirx(Eigen::MatrixXd &mat, Eigen::VectorXd &vec); template Eigen::Quaternion zyx_euler_to_quat(const T &roll, const T &pitch, const T &yaw); template void quat_to_euler_zyx(const Eigen::Quaternion& q, T& roll, T& pitch, T& yaw); template void rot_to_euler_zyx(const Eigen::Matrix& R, T& roll, T& pitch, T& yaw); template Eigen::Matrix euler_zyx_to_rot(const T& roll,const T& pitch,const T& yaw); // input is 3*n (or 2*n) output is 4*n (or 3*n) template Eigen::Matrix real_to_homo_coord(const Eigen::Matrix& pts_in); template void real_to_homo_coord(const Eigen::Matrix& pts_in, Eigen::Matrix& pts_homo_out); template // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function. Eigen::Matrix real_to_homo_coord_vec(const Eigen::Matrix& pts_in); // input is 3*n (or 4*n) output is 2*n(or 3*n) template Eigen::Matrix homo_to_real_coord(const Eigen::Matrix& pts_homo_in); template void homo_to_real_coord(const Eigen::Matrix& pts_homo_in, Eigen::Matrix& pts_out); template // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function. Eigen::Matrix homo_to_real_coord_vec(const Eigen::Matrix& pts_homo_in); //vertically stack a matrix to a matrix, increase rows, keep columns void vert_stack_m(const Eigen::MatrixXd &a_in, const Eigen::MatrixXd &b_in, Eigen::MatrixXd &combined_out); void vert_stack_m_self(Eigen::MatrixXf &a_in, const Eigen::MatrixXf &b_in); void fast_RemoveRow(Eigen::MatrixXd& matrix,int rowToRemove, int& total_line_number); // make sure column size is given. not check here. row will be adjusted automatically. if more cols given, will be zero. template bool read_all_number_txt(const std::string txt_file_name, Eigen::Matrix& read_number_mat); // each line: one string, several numbers. make sure column size is correct. bool read_obj_detection_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& strings); // each line several numbers then one string . make sure column size is correct. bool read_obj_detection2_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& strings); // (partial) sort a vector, only need top_K element, result is stored in idx by default increasing void sort_indexes(const Eigen::VectorXd &vec, std::vector& idx, int top_k ); void sort_indexes(const Eigen::VectorXd &vec, std::vector& idx); // change [-180 180] to [-90 90] by +-90 template T normalize_to_pi(T angle); template void print_vector(const std::vector& vec); //TODO could be replaced by eigen linespace template void linespace(T starting, T ending, T step, std::vector& res); #endif //ELLIPSOIDSLAM_MATRIX_UTILS_H ================================================ FILE: install_g2o.sh ================================================ #!/bin/bash cd Thirdparty/g2o mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release make -j2 ================================================ FILE: src/Polygon/CMakeLists.txt ================================================ add_library(Polygon SHARED Polygon.cpp) target_link_libraries(Polygon ${OpenCV_LIBS}) ================================================ FILE: src/Polygon/Polygon.cpp ================================================ #include "Polygon.hpp" namespace EllipsoidSLAM { float distPoint(cv::Point2d v,cv::Point2d w) { return sqrtf((v.x - w.x)*(v.x - w.x) + (v.y - w.y)*(v.y - w.y)) ; } bool segementIntersection(cv::Point2d p0,cv::Point2d p1,cv::Point2d p2,cv::Point2d p3,cv::Point2d * intersection) { cv::Point2d s1, s2; s1 = cv::Point2d(p1.x - p0.x, p1.y - p0.y); s2 = cv::Point2d(p3.x - p2.x, p3.y - p2.y); float s10_x = p1.x - p0.x; float s10_y = p1.y - p0.y; float s32_x = p3.x - p2.x; float s32_y = p3.y - p2.y; float denom = s10_x * s32_y - s32_x * s10_y; if(denom == 0) { return false; } bool denom_positive = denom > 0; float s02_x = p0.x - p2.x; float s02_y = p0.y - p2.y; float s_numer = s10_x * s02_y - s10_y * s02_x; if((s_numer < 0.f) == denom_positive) { return false; } float t_numer = s32_x * s02_y - s32_y * s02_x; if((t_numer < 0) == denom_positive) { return false; } if((s_numer > denom) == denom_positive || (t_numer > denom) == denom_positive) { return false; } float t = t_numer / denom; *intersection = cv::Point2d(p0.x + (t * s10_x), p0.y + (t * s10_y) ); return true; } bool pointInPolygon(cv::Point2d p,const cv::Point2d * points,int n) { int i, j ; bool c = false; for(i = 0, j = n - 1; i < n; j = i++) { if( ( (points[i].y >= p.y ) != (points[j].y >= p.y) ) && (p.x <= (points[j].x - points[i].x) * (p.y - points[i].y) / (points[j].y - points[i].y) + points[i].x) ) c = !c; } return c; } float computeArea(const cv::Point2d * pt,int n ) { float area0 = 0.f; for (int i = 0 ; i < n ; i++ ) { int j = (i+1)%n; area0 += pt[i].x * pt[j].y; area0 -= pt[i].y * pt[j].x; } return 0.5f * fabs(area0); } struct PointAngle{ cv::Point2d p; float angle; }; cv::Point2d Polygon::getCenter() const { cv::Point2d center; center.x = 0; center.y = 0; for (int i = 0 ; i < n ; i++ ) { center.x += pt[i].x; center.y += pt[i].y; } center.x /= n; center.y /= n; return center; } static int comp_point_with_angle(const void * a, const void * b) { if ( ((PointAngle*)a)->angle < ((PointAngle*)b)->angle ) return -1; else if ( ((PointAngle*)a)->angle > ((PointAngle*)b)->angle ) return 1; else //if ( ((PointAngle*)a)->angle == ((PointAngle*)b)->angle ) return 0; return 0; } float Polygon::area() const { return computeArea(pt,n); } void Polygon::pointsOrdered() { if( n <= 0) return; cv::Point2d center = getCenter(); PointAngle pc[MAX_POINT_POLYGON]; for (int i = 0 ; i < n ; i++ ) { pc[i].p.x = pt[i].x; pc[i].p.y = pt[i].y; pc[i].angle = atan2f((float)(pt[i].y - center.y),(float)(pt[i].x - center.x)); } qsort(pc,n,sizeof(PointAngle),comp_point_with_angle); for (int i = 0 ; i < n ; i++ ) { pt[i].x = pc[i].p.x; pt[i].y = pc[i].p.y; } } bool Polygon::pointIsInPolygon(cv::Point2d p) const { return pointInPolygon(p,pt,n); } void intersectPolygon( const cv::Point2d * poly0, int n0,const cv::Point2d * poly1,int n1, Polygon & inter ) { inter.clear(); for (int i = 0 ; i < n0 ;i++) { if( pointInPolygon(poly0[i],poly1,n1) ) { inter.add(poly0[i]); } } for (int i = 0 ; i < n1 ;i++) { if( pointInPolygon(poly1[i],poly0,n0) ) inter.add(poly1[i]); } for (int i = 0 ; i < n0 ;i++) { cv::Point2d p0,p1,p2,p3; p0 = poly0[i]; p1 = poly0[(i+1)%n0]; for (int j = 0 ; j < n1 ;j++) { p2 = poly1[j]; p3 = poly1[(j+1)%n1]; cv::Point2d pinter; if(segementIntersection(p0,p1,p2,p3,&pinter)) { inter.add(pinter); } } } inter.pointsOrdered(); } void intersectPolygon( const Polygon & poly0, const Polygon & poly1, Polygon & inter ) { intersectPolygon(&poly0[0],poly0.size(),&poly1[0],poly1.size(),inter); } //------------------------------------------------------------------------------------------------------------ // SHPC : Sutherland-Hodgeman-Polygon-Clipping Algorihtm //------------------------------------------------------------------------------------------------------------ static inline int cross(const cv::Point2d* a,const cv::Point2d* b) { return a->x * b->y - a->y * b->x; } static inline cv::Point2d* vsub(const cv::Point2d* a,const cv::Point2d* b, cv::Point2d* res) { res->x = a->x - b->x; res->y = a->y - b->y; return res; } static int line_sect(const cv::Point2d* x0,const cv::Point2d* x1,const cv::Point2d* y0,const cv::Point2d* y1, cv::Point2d* res) { cv::Point2d dx, dy, d; vsub(x1, x0, &dx); vsub(y1, y0, &dy); vsub(x0, y0, &d); float dyx = (float)cross(&dy, &dx); if (!dyx) return 0; dyx = cross(&d, &dx) / dyx; if (dyx <= 0 || dyx >= 1) return 0; res->x = int(y0->x + dyx * dy.x); res->y = int(y0->y + dyx * dy.y); return 1; } static int left_of(const cv::Point2d* a,const cv::Point2d* b,const cv::Point2d* c) { cv::Point2d tmp1, tmp2; int x; vsub(b, a, &tmp1); vsub(c, b, &tmp2); x = cross(&tmp1, &tmp2); return x < 0 ? -1 : x > 0; } static void poly_edge_clip(const Polygon* sub,const cv::Point2d* x0,const cv::Point2d* x1, int left, Polygon* res) { int i, side0, side1; cv::Point2d tmp; const cv::Point2d* v0 = sub->pt+ sub->n - 1; const cv::Point2d* v1; res->clear(); side0 = left_of(x0, x1, v0); if (side0 != -left) res->add(*v0); for (i = 0; i < sub->n; i++) { v1 = sub->pt + i; side1 = left_of(x0, x1, v1); if (side0 + side1 == 0 && side0) /* last point and current straddle the edge */ if (line_sect(x0, x1, v0, v1, &tmp)) res->add(tmp); if (i == sub->n - 1) break; if (side1 != -left) res->add(*v1); v0 = v1; side0 = side1; } } static int poly_winding(const Polygon* p) { return left_of(p->pt, p->pt + 1, p->pt + 2); } void intersectPolygonSHPC(const Polygon * sub,const Polygon* clip,Polygon* res) { int i; Polygon P1,P2; Polygon * p1 = &P1; Polygon * p2 = &P2; Polygon * tmp ; int dir = poly_winding(clip); poly_edge_clip(sub, clip->pt + clip->n - 1, clip->pt, dir, p2); for (i = 0; i < clip->n - 1; i++) { tmp = p2; p2 = p1; p1 = tmp; if(p1->n == 0) { p2->n = 0; break; } poly_edge_clip(p1, clip->pt + i, clip->pt + i + 1, dir, p2); } res->clear(); for (i = 0 ; i < p2->n ; i++) res->add(p2->pt[i]); } void intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Polygon& res) { intersectPolygonSHPC(&sub,&clip,&res); } } ================================================ FILE: src/Polygon/Polygon.hpp ================================================ /* * This file is dependent on github open-source. All rights are perserved by original authors. * Web: https://github.com/abreheret/polygon-intersection */ // Update: Add the API for EllipsoidSLAM. #ifndef __EllipsoidSLAM_POLYGON_HPP__ #define __EllipsoidSLAM_POLYGON_HPP__ // #include // not recoginized by OpenCV 4.x #include namespace EllipsoidSLAM { float distPoint( cv::Point2d p1, cv::Point2d p2 ) ; bool segementIntersection(cv::Point2d p0_seg0,cv::Point2d p1_seg0,cv::Point2d p0_seg1,cv::Point2d p1_seg1,cv::Point2d * intersection) ; bool pointInPolygon(cv::Point2d p,const cv::Point2d * points,int n) ; #define MAX_POINT_POLYGON 64 struct Polygon { cv::Point2d pt[MAX_POINT_POLYGON]; int n; Polygon(int n_ = 0 ) { assert(n_>= 0 && n_ < MAX_POINT_POLYGON); n = n_;} virtual ~Polygon() {} void clear() { n = 0; } void add(const cv::Point2d &p) {if(n < MAX_POINT_POLYGON) pt[n++] = p;} void push_back(const cv::Point2d &p) {add(p);} int size() const { return n;} cv::Point2d getCenter() const ; const cv::Point2d & operator[] (int index) const { assert(index >= 0 && index < n); return pt[index];} cv::Point2d& operator[] (int index) { assert(index >= 0 && index < n); return pt[index]; } void pointsOrdered() ; float area() const ; bool pointIsInPolygon(cv::Point2d p) const ; }; void intersectPolygon( const cv::Point2d * poly0, int n0,const cv::Point2d * poly1,int n1, Polygon & inter ) ; void intersectPolygon( const Polygon & poly0, const Polygon & poly1, Polygon & inter ) ; void intersectPolygonSHPC(const Polygon * sub,const Polygon* clip,Polygon* res) ; void intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Polygon& res) ; } #endif // ================================================ FILE: src/config/CMakeLists.txt ================================================ add_library(Config SHARED Config.cpp ) target_link_libraries(Config ${OpenCV_LIBS} ) ================================================ FILE: src/config/Config.cpp ================================================ #include "Config.h" namespace EllipsoidSLAM{ void Config::SetParameterFile( const std::string& filename ) { if ( mConfig == nullptr ) mConfig = shared_ptr(new Config); mConfig->mFile.open( filename.c_str(), cv::FileStorage::READ ); if ( !mConfig->mFile.isOpened()) { std::cerr<<"parameter file "<< filename <<" does not exist."<mFile.release(); return; } } Config::~Config() { if ( mFile.isOpened() ) mFile.release(); } void Config::Init(){ if ( mConfig == nullptr ){ mConfig = shared_ptr(new Config); // ************ default parameters here ************* mConfig->SetValue("Tracking_MINIMUM_INITIALIZATION_FRAME", 15); // The minimum frame number required for ellipsoid initialization using 2d observations. mConfig->SetValue("EllipsoidExtractor_DEPTH_RANGE", 6); // the valid depth range (m) } } shared_ptr Config::mConfig = nullptr; } ================================================ FILE: src/config/Config.h ================================================ // A parameter file supporting global dynamic changes #ifndef ELLIPSOIDSLAM_CONFIG_H #define ELLIPSOIDSLAM_CONFIG_H #include #include #include #include using namespace std; namespace EllipsoidSLAM { class Config{ public: static void SetParameterFile( const string& filename ); // Get function reads value from parameter file (.yaml) template static T Get(const string& key){ return T(Config::mConfig->mFile[key]); } template static void Set(const string& key, T value){ Config::mConfig->mFile << key << value; // tobe test } ~Config(); static void Init(); // set the parameter value, if do not exist, create a new one template static void SetValue(const string& key, T value) { auto pos = Config::mConfig->mKeyValueMap.find(key); if( pos == Config::mConfig->mKeyValueMap.end() ) // do not exist { Config::mConfig->mKeyValueMap.insert(make_pair(key, double(value))); } else { pos->second = double(value); } } // ReadValue function reads parameter value set by SetValue function, and return default value if it does not exist template static T ReadValue(const string& key, T default_value = 0) { auto pos = Config::mConfig->mKeyValueMap.find(key); if( pos == Config::mConfig->mKeyValueMap.end() ) // do not exist { return Get(key); // return value in the parameter file } else { return T(pos->second); } } private: Config(){}; static std::shared_ptr mConfig; cv::FileStorage mFile; std::map mKeyValueMap; }; } #endif //ELLIPSOIDSLAM_CONFIG_H ================================================ FILE: src/core/BasicEllipsoidEdges.cpp ================================================ #include "include/core/BasicEllipsoidEdges.h" namespace g2o { // ---- VertexEllipsoid void VertexEllipsoid::setToOriginImpl() { _estimate = ellipsoid(); } void VertexEllipsoid::oplusImpl(const double* update_) { Eigen::Map update(update_); setEstimate(_estimate.exp_update(update)); } bool VertexEllipsoid::read(std::istream& is) { Vector9d est; for (int i=0; i<9; i++) is >> est[i]; ellipsoid Onecube; Onecube.fromMinimalVector(est); setEstimate(Onecube); return true; } bool VertexEllipsoid::write(std::ostream& os) const { Vector9d lv=_estimate.toMinimalVector(); for (int i=0; i update(update_); setEstimate(_estimate.exp_update_XYZABC(update)); } bool VertexEllipsoidXYZABC::read(std::istream& is) { Vector9d est; for (int i=0; i<9; i++) is >> est[i]; ellipsoid Onecube; Onecube.fromMinimalVector(est); setEstimate(Onecube); return true; } bool VertexEllipsoidXYZABC::write(std::ostream& os) const { Vector9d lv=_estimate.toMinimalVector(); for (int i=0; i(_vertices[0]); // world to camera pose const VertexEllipsoid* cuboidVertex = static_cast(_vertices[1]); // object pose to world SE3Quat cam_pose_Twc = SE3Vertex->estimate().inverse(); ellipsoid global_cube = cuboidVertex->estimate(); ellipsoid esti_global_cube = _measurement.transform_from(cam_pose_Twc); _error = global_cube.min_log_error_9dof(esti_global_cube); } // EdgeSE3EllipsoidProj bool EdgeSE3EllipsoidProj::read(std::istream& is){ return true; }; bool EdgeSE3EllipsoidProj::write(std::ostream& os) const { return os.good(); }; Vector4d EdgeSE3EllipsoidProj::getProject(){ const VertexSE3Expmap* SE3Vertex = static_cast(_vertices[0]); // world to camera pose const VertexEllipsoid* cuboidVertex = static_cast(_vertices[1]); // object pose to world SE3Quat cam_pose_Tcw = SE3Vertex->estimate(); ellipsoid global_cube = cuboidVertex->estimate(); Vector4d proj = global_cube.getBoundingBoxFromProjection(cam_pose_Tcw, Kalib); // topleft, rightdown return proj; } void EdgeSE3EllipsoidProj::computeError() { Vector4d rect_project = getProject(); _error = Vector4d(0,0,0,0); for(int i=0;i<4;i++) { if( _measurement[i] >= 5 ) // the invalid measurement has been set to -1. _error[i] = rect_project[i] - _measurement[i]; } } void EdgeSE3EllipsoidProj::setKalib(Matrix3d& K) { Kalib = K; } // Prior: objects should be vertical to their supporting planes bool EdgeEllipsoidGravityPlanePrior::read(std::istream& is){ return true; }; bool EdgeEllipsoidGravityPlanePrior::write(std::ostream& os) const { return os.good(); }; void EdgeEllipsoidGravityPlanePrior::computeError() { const VertexEllipsoid* cuboidVertex = static_cast(_vertices[0]); ellipsoid global_cube = cuboidVertex->estimate(); Eigen::Matrix3d rotMat = global_cube.pose.to_homogeneous_matrix().topLeftCorner(3,3); Vector3d zAxis = rotMat.col(2); Vector3d goundPlaneNormalAxis = _measurement.head(3); double cos_angle = zAxis.dot(goundPlaneNormalAxis)/zAxis.norm()/goundPlaneNormalAxis.norm(); // prevent NaN if( cos_angle > 1 ) cos_angle = cos_angle - 0.0001; else if( cos_angle < -1 ) cos_angle = cos_angle + 0.0001; double angle = acos(cos_angle); double diff_angle = angle - 0; _error = Eigen::Matrix(diff_angle); } }// g2o ================================================ FILE: src/core/DataAssociation.cpp ================================================ #include "core/DataAssociation.h" #include #include namespace EllipsoidSLAM { DataAssociationSolver::DataAssociationSolver(Map* pMap) { mpMap = pMap; miInstanceNum = 0; } std::vector DataAssociationSolver::Solve(EllipsoidSLAM::Frame* pFrame, bool mb3D) { Eigen::MatrixXd &obsMat = pFrame->mmObservations; g2o::SE3Quat campose_wc = pFrame->cam_pose_Twc; auto instanceObjMap = mpMap->GetAllEllipsoidsMap(); int num_object = instanceObjMap.size(); int num_obs = obsMat.rows(); std::map valid3DObsMap; std::vector associations; associations.resize(num_obs); if( mb3D ) // only support 3d observations { // filter invalid 3d observaions for( int i=0;impLocalObjects[i]; if(pObjObserved != NULL ) { valid3DObsMap.insert(make_pair(i, pObjObserved)); } else associations[i] = -1; } // construct a new list for those valid observations int validObNum = valid3DObsMap.size(); if( validObNum < 1 ) return associations; // calculate the costMat MatrixXd costMat; costMat.resize(validObNum, num_object); // row : observations , col : objects in the map int objObservedId = 0; for( auto& validIdObPair : valid3DObsMap ) { g2o::ellipsoid* pObjObserved = validIdObPair.second; g2o::ellipsoid pObjObservedWorld = pObjObserved->transform_from(campose_wc); VectorXd costVec; costVec.resize(num_object); int objInMapId = 0; for( auto& instanceObj : instanceObjMap ) { g2o::ellipsoid* pObj = instanceObj.second; // calculate the distance of between their centers double distance = (pObj->pose.translation() - pObjObservedWorld.pose.translation()).norm(); double cost = distance; costVec[objInMapId] = cost; objInMapId ++; } costMat.row(objObservedId) = costVec.transpose(); objObservedId++; } // solve results from costMat std::vector matAssociations = SolveCostMat(costMat); // save the association results of the valid observations to output objObservedId = 0; for( auto& validIdObPair : valid3DObsMap ) { associations[validIdObPair.first] = matAssociations[objObservedId]; objObservedId++; } return associations; } } // Solve associations from every rows to every cols separately. // If an observation doesn't belong to any object in the map, it will get a new instance ID. std::vector DataAssociationSolver::SolveCostMat(MatrixXd& mat){ // Method : Iterate through each row in turn, taking the column with minimum cost value if the value is also less than the thresh, // and delete the column from the cost matrix. Repeat the process for every row until there are no columns left. // ----- Settings --- double SETTING_DIS_THRESH = 1.0; // the minimum cost value must be less than the thresh to be considered valid. MatrixXd costMat = mat; int rows = costMat.rows(); std::vector matAssocitions; matAssocitions.resize(rows); for( int i=0; i #include #include namespace g2o { ellipsoid::ellipsoid() { } // xyz roll pitch yaw half_scale void ellipsoid::fromMinimalVector(const Vector9d& v){ Eigen::Quaterniond posequat = zyx_euler_to_quat(v(3),v(4),v(5)); pose = SE3Quat(posequat, v.head<3>()); scale = v.tail<3>(); vec_minimal = v; } // xyz quaternion, half_scale void ellipsoid::fromVector(const Vector10d& v){ pose.fromVector(v.head<7>()); scale = v.tail<3>(); vec_minimal = toMinimalVector(); } const Vector3d& ellipsoid::translation() const {return pose.translation();} void ellipsoid::setTranslation(const Vector3d& t_) {pose.setTranslation(t_);} void ellipsoid::setRotation(const Quaterniond& r_) {pose.setRotation(r_);} void ellipsoid::setRotation(const Matrix3d& R) {pose.setRotation(Quaterniond(R));} void ellipsoid::setScale(const Vector3d &scale_) {scale=scale_;} // apply update to current ellipsoid. exponential map ellipsoid ellipsoid::exp_update(const Vector9d& update) { ellipsoid res; res.pose = this->pose*SE3Quat::exp(update.head<6>()); res.scale = this->scale + update.tail<3>(); res.UpdateValueFrom(*this); res.vec_minimal = res.toMinimalVector(); return res; } // TOBE DELETED. ellipsoid ellipsoid::exp_update_XYZABC(const Vector6d& update) { ellipsoid res; Vector6d pose_vec; pose_vec << 0, 0, 0, update[0], update[1], update[2]; res.pose = this->pose*SE3Quat::exp(pose_vec); res.scale = this->scale + update.tail<3>(); res.UpdateValueFrom(*this); res.vec_minimal = res.toMinimalVector(); return res; } Vector9d ellipsoid::ellipsoid_log_error_9dof(const ellipsoid& newone) const { Vector9d res; SE3Quat pose_diff = newone.pose.inverse()*this->pose; res.head<6>() = pose_diff.log(); res.tail<3>() = this->scale - newone.scale; return res; } // change front face by rotate along current body z axis. // another way of representing cuboid. representing same cuboid (IOU always 1) ellipsoid ellipsoid::rotate_ellipsoid(double yaw_angle) const // to deal with different front surface of cuboids { ellipsoid res; SE3Quat rot(Eigen::Quaterniond(cos(yaw_angle*0.5),0,0,sin(yaw_angle*0.5)),Vector3d(0,0,0)); // change yaw to rotation. res.pose = this->pose*rot; res.scale = this->scale; res.UpdateValueFrom(*this); res.vec_minimal = res.toMinimalVector(); const double eps = 1e-6; if ( (std::abs(yaw_angle-M_PI/2.0) < eps) || (std::abs(yaw_angle+M_PI/2.0) < eps) || (std::abs(yaw_angle-3*M_PI/2.0) < eps)) std::swap(res.scale(0),res.scale(1)); return res; } Vector9d ellipsoid::min_log_error_9dof(const ellipsoid& newone, bool print_details) const { bool whether_rotate_ellipsoid=true; // whether rotate cube to find smallest error if (!whether_rotate_ellipsoid) return ellipsoid_log_error_9dof(newone); // NOTE rotating ellipsoid... since we cannot determine the front face consistenly, different front faces indicate different yaw, scale representation. // need to rotate all 360 degrees (global cube might be quite different from local cube) // this requires the sequential object insertion. In this case, object yaw practically should not change much. If we observe a jump, we can use code // here to adjust the yaw. Vector4d rotate_errors_norm; Vector4d rotate_angles(-1,0,1,2); // rotate -90 0 90 180 Eigen::Matrix rotate_errors; for (int i=0;iellipsoid_log_error_9dof(rotated_cuboid); rotate_errors_norm(i) = cuboid_error.norm(); rotate_errors.col(i) = cuboid_error; } int min_label; rotate_errors_norm.minCoeff(&min_label); if (print_details) if (min_label!=1) std::cout<<"Rotate ellipsoid "<pose; res.scale = this->scale; res.UpdateValueFrom(*this); res.vec_minimal = res.toMinimalVector(); return res; } // transform a global cuboid to local cuboid Twc is camera pose. from camera to world ellipsoid ellipsoid::transform_to(const SE3Quat& Twc) const{ ellipsoid res; res.pose = Twc.inverse()*this->pose; res.scale = this->scale; res.UpdateValueFrom(*this); res.vec_minimal = res.toMinimalVector(); return res; } // xyz roll pitch yaw half_scale Vector9d ellipsoid::toMinimalVector() const{ Vector9d v; v.head<6>() = pose.toXYZPRYVector(); v.tail<3>() = scale; return v; } // xyz quaternion, half_scale Vector10d ellipsoid::toVector() const{ Vector10d v; v.head<7>() = pose.toVector(); v.tail<3>() = scale; return v; } Matrix4d ellipsoid::similarityTransform() const { Matrix4d res = pose.to_homogeneous_matrix(); // 4x4 transform matrix Matrix3d scale_mat = scale.asDiagonal(); res.topLeftCorner<3,3>() = res.topLeftCorner<3,3>()*scale_mat; return res; } void ellipsoid::UpdateValueFrom(const g2o::ellipsoid& e){ this->miLabel = e.miLabel; this->mbColor = e.mbColor; this->mvColor = e.mvColor; this->miInstanceID = e.miInstanceID; this->prob = e.prob; } ellipsoid::ellipsoid(const g2o::ellipsoid &e) { pose = e.pose; scale = e.scale; vec_minimal = e.vec_minimal; UpdateValueFrom(e); } const ellipsoid& ellipsoid::operator=(const g2o::ellipsoid &e) { pose = e.pose; scale = e.scale; vec_minimal = e.vec_minimal; UpdateValueFrom(e); return e; } // ************* Functions As Ellipsoids *************** Vector2d ellipsoid::projectCenterIntoImagePoint(const SE3Quat& campose_cw, const Matrix3d& Kalib) { Matrix3Xd P = generateProjectionMatrix(campose_cw, Kalib); Vector3d center_pos = pose.translation(); Vector4d center_homo = real_to_homo_coord(center_pos); Vector3d u_homo = P * center_homo; Vector2d u = homo_to_real_coord_vec(u_homo); return u; } // project the ellipsoid into the image plane, and get an ellipse represented by a Vector5d. // Ellipse: x_c, y_c, theta, axis1, axis2 Vector5d ellipsoid::projectOntoImageEllipse(const SE3Quat& campose_cw, const Matrix3d& Kalib) const { Matrix4d Q_star = generateQuadric(); Matrix3Xd P = generateProjectionMatrix(campose_cw, Kalib); Matrix3d C_star = P * Q_star * P.transpose(); Matrix3d C = C_star.inverse(); C = C / C(2,2); // normalize SelfAdjointEigenSolver es(C); // ascending sort by default VectorXd eigens = es.eigenvalues(); // If it is an ellipse, the sign of eigen values must be : 1 1 -1 // Ref book : Multiple View Geometry in Computer Vision int num_pos = int(eigens(0)>0) +int(eigens(1)>0) +int(eigens(2)>0); int num_neg = int(eigens(0)<0) +int(eigens(1)<0) +int(eigens(2)<0); // matrix to equation coefficients: ax^2+bxy+cy^2+dx+ey+f=0 double a = C(0,0); double b = C(0,1)*2; double c = C(1,1); double d = C(0,2)*2; double e = C(2,1)*2; double f = C(2,2); // get x_c, y_c, theta, axis1, axis2 from coefficients double delta = c*c - 4.0*a*b; double k = (a*f-e*e/4.0) - pow((2*a*e-c*d),2)/(4*(4*a*b-c*c)); double theta = 1/2.0*atan2(b,(a-c)); double x_c = (b*e-2*c*d)/(4*a*c-b*b); double y_c = (b*d-2*a*e)/(4*a*c-b*b); double a_2 = 2*(a* x_c*x_c+ c * y_c*y_c+ b *x_c*y_c -1) /(a + c + sqrt((a-c)*(a-c)+b*b)); double b_2 = 2*(a*x_c*x_c+c*y_c*y_c+b*x_c*y_c -1) /( a + c - sqrt((a-c)*(a-c)+b*b)); double axis1= sqrt(a_2); double axis2= sqrt(b_2); Vector5d output; output << x_c, y_c, theta, axis1, axis2; return output; } // Get the bounding box from ellipse in image plane Vector4d ellipsoid::getBoundingBoxFromEllipse(Vector5d &ellipse) const { double a = ellipse[3]; double b = ellipse[4]; double theta = ellipse[2]; double x = ellipse[0]; double y = ellipse[1]; double cos_theta_2 = cos(theta)*cos(theta); double sin_theta_2 = 1- cos_theta_2; double x_limit = sqrt(a*a*cos_theta_2+b*b*sin_theta_2); double y_limit = sqrt(a*a*sin_theta_2+b*b*cos_theta_2); Vector4d output; output[0] = x-x_limit; // left up output[1] = y-y_limit; output[2] = x+x_limit; // right down output[3] = y+y_limit; return output; } // Get projection matrix P = K [ R | t ] Matrix3Xd ellipsoid::generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const { Matrix3Xd identity_lefttop; identity_lefttop.resize(3, 4); identity_lefttop.col(3)=Vector3d(0,0,0); identity_lefttop.topLeftCorner<3,3>() = Matrix3d::Identity(3,3); Matrix3Xd proj_mat = Kalib * identity_lefttop; proj_mat = proj_mat * campose_cw.to_homogeneous_matrix(); return proj_mat; } // Get Q^* Matrix4d ellipsoid::generateQuadric() const { Vector4d axisVec; axisVec << 1/(scale[0]*scale[0]), 1/(scale[1]*scale[1]), 1/(scale[2]*scale[2]), -1; Matrix4d Q_c = axisVec.asDiagonal(); Matrix4d Q_c_star = Q_c.inverse(); Matrix4d Q_pose_matrix = pose.to_homogeneous_matrix(); // Twm model in world, world to model Matrix4d Q_c_star_trans = Q_pose_matrix * Q_c_star * Q_pose_matrix.transpose(); return Q_c_star_trans; } // Get the projected bounding box in the image plane of the ellipsoid using a camera pose and a calibration matrix. Vector4d ellipsoid::getBoundingBoxFromProjection(const SE3Quat& campose_cw, const Matrix3d& Kalib) const { Vector5d ellipse = projectOntoImageEllipse(campose_cw, Kalib); return getBoundingBoxFromEllipse(ellipse); } Vector3d ellipsoid::getColor(){ return mvColor.head(3); } Vector4d ellipsoid::getColorWithAlpha(){ return mvColor; } void ellipsoid::setColor(const Vector3d &color_, double alpha){ mbColor = true; mvColor.head<3>() = color_; mvColor[3] = alpha; } bool ellipsoid::isColorSet(){ return mbColor; } bool ellipsoid::CheckObservability(const SE3Quat& campose_cw) { Vector3d ellipsoid_center = toMinimalVector().head(3); // Pwo Vector4d center_homo = real_to_homo_coord_vec(ellipsoid_center); Eigen::Matrix4d projMat = campose_cw.to_homogeneous_matrix(); // Tcw Vector4d center_inCameraAxis_homo = projMat * center_homo; // Pco = Tcw * Pwo Vector3d center_inCameraAxis = homo_to_real_coord_vec(center_inCameraAxis_homo); if( center_inCameraAxis_homo(2) < 0) // if the center is behind the camera ; z<0 { return false; } else return true; } // calculate the IoU Error between two axis-aligned ellipsoid double ellipsoid::calculateMIoU(const g2o::ellipsoid& e) const { return calculateIntersectionError(*this, e); } double ellipsoid::calculateIntersectionOnZ(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const { g2o::SE3Quat pose_diff = e1.pose.inverse() * e2.pose; double z1 = 0; double z2 = pose_diff.translation()[2]; bool flag_oneBigger = false; if( z1 > z2 ) flag_oneBigger = true; double length; if( flag_oneBigger ) { length = (z2 + e2.scale[2]) - (z1 - e1.scale[2]); } else length = (z1 + e1.scale[2]) - (z2 - e2.scale[2]); if( length < 0 ) length = 0; // if they are not intersected return length; } double ellipsoid::calculateArea(const g2o::ellipsoid& e) const { return e.scale[0]*e.scale[1]*e.scale[2]*8; } void OutputPolygon(EllipsoidSLAM::Polygon& polygon, double resolution) { int num = polygon.n; for( int i=0;i(similarityTransform()*real_to_homo_coord(corners_body)); return corners_world; } Matrix2Xd ellipsoid::projectOntoImageBoxCorner(const SE3Quat& campose_cw, const Matrix3d& Kalib) const { Matrix3Xd corners_3d_world = compute3D_BoxCorner(); Matrix2Xd corner_2d = homo_to_real_coord(Kalib*homo_to_real_coord(campose_cw.to_homogeneous_matrix()*real_to_homo_coord(corners_3d_world))); return corner_2d; } // get rectangles after projection [topleft, bottomright] Vector4d ellipsoid::projectOntoImageRect(const SE3Quat& campose_cw, const Matrix3d& Kalib) const { Matrix2Xd corner_2d = projectOntoImageBoxCorner(campose_cw, Kalib); Vector2d bottomright = corner_2d.rowwise().maxCoeff(); // x y Vector2d topleft = corner_2d.rowwise().minCoeff(); return Vector4d(topleft(0),topleft(1),bottomright(0),bottomright(1)); } // get rectangles after projection [center, width, height] Vector4d ellipsoid::projectOntoImageBbox(const SE3Quat& campose_cw, const Matrix3d& Kalib) const { Vector4d rect_project = projectOntoImageRect(campose_cw, Kalib); // top_left, bottom_right x1 y1 x2 y2 Vector2d rect_center = (rect_project.tail<2>()+rect_project.head<2>())/2; Vector2d widthheight = rect_project.tail<2>()-rect_project.head<2>(); return Vector4d(rect_center(0),rect_center(1),widthheight(0),widthheight(1)); } } // g2o ================================================ FILE: src/core/Frame.cpp ================================================ #include namespace EllipsoidSLAM { int Frame::total_frame=0; Frame::Frame(double timestamp_, const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB) { timestamp = timestamp_; rgb_img = imRGB.clone(); frame_img = imDepth.clone(); cam_pose_Twc.fromVector(pose.tail(7)); cam_pose_Tcw = cam_pose_Twc.inverse(); frame_seq_id = total_frame++; mvbOutlier.resize(bboxMap.rows()); fill(mvbOutlier.begin(), mvbOutlier.end(), false); mmObservations = bboxMap; std::cout << "--------> New Frame : " << frame_seq_id << " Timestamp: " << std::to_string(timestamp) << std::endl; std::cout << "[Frame.cpp] bboxMap : " << std::endl << bboxMap << std::endl << std::endl; mbHaveLocalObject = false; } } ================================================ FILE: src/core/FrameDrawer.cpp ================================================ #include "include/core/FrameDrawer.h" #include "src/config/Config.h" #include "utils/dataprocess_utils.h" using namespace cv; namespace EllipsoidSLAM { FrameDrawer::FrameDrawer(EllipsoidSLAM::Map *pMap) { mpMap = pMap; mmRGB = cv::Mat(); mmDepth = cv::Mat(); } void FrameDrawer::setTracker(EllipsoidSLAM::Tracking *pTracker) { mpTracking = pTracker; } cv::Mat FrameDrawer::drawFrame() { Frame * frame = mpTracking->mCurrFrame; if( frame == NULL ) return cv::Mat(); cv::Mat im; if(!frame->rgb_img.empty()) // use rgb image if it exists, or use depth image instead. im = frame->rgb_img; else im = frame->frame_img; cv::Mat out = drawFrameOnImage(im); mmRGB = out.clone(); return mmRGB; } cv::Mat FrameDrawer::drawDepthFrame() { Frame * frame = mpTracking->mCurrFrame; cv::Mat I = frame->frame_img; // U16C1 , ushort cv::Mat im,R,G,B; // Vector3d color1(51,25,0); // Vector3d color2(255,229,204); // r = color1 + value/255*(color2-color1) // (255-51)/255 51 // 220-25 I.convertTo(R, CV_8UC1, 0.028, 51); I.convertTo(G, CV_8UC1, 0.028, 25); I.convertTo(B, CV_8UC1, 0.028, 0); std::vector array_to_merge; array_to_merge.push_back(B); array_to_merge.push_back(G); array_to_merge.push_back(R); cv::merge(array_to_merge, im); cv::Mat out = drawObservationOnImage(im); mmDepth = out.clone(); return mmDepth; } cv::Mat FrameDrawer::drawProjectionOnImage(cv::Mat &im) { std::map pEllipsoidsMapWithLabel = mpMap->GetAllEllipsoidsMap(); // draw projected bounding boxes of the ellipsoids in the map cv::Mat imageProj = im.clone(); for(auto iter=pEllipsoidsMapWithLabel.begin(); iter!=pEllipsoidsMapWithLabel.end();iter++) { ellipsoid* e = iter->second; // check whether it could be seen if( e->CheckObservability(mpTracking->mCurrFrame->cam_pose_Tcw) ) { Vector4d rect = e->getBoundingBoxFromProjection(mpTracking->mCurrFrame->cam_pose_Tcw, mpTracking->mCalib); // center, width, height cv::rectangle(imageProj, cv::Rect(cv::Point(rect[0],rect[1]),cv::Point(rect[2],rect[3])), cv::Scalar(0,0,255), 4); } } return imageProj.clone(); } cv::Mat FrameDrawer::drawFrameOnImage(cv::Mat &in) { cv::Mat out = in.clone(); out = drawObservationOnImage(out); out = drawProjectionOnImage(out); return out.clone(); } cv::Mat FrameDrawer::drawObservationOnImage(cv::Mat &in) { cv::Mat im = in.clone(); Frame * frame = mpTracking->mCurrFrame; // draw observation Eigen::MatrixXd mat_det = frame->mmObservations; int obs = mat_det.rows(); for(int r=0;r("Measurement.Border.Pixels"), Config::Get("Measurement.LengthLimit.Pixels")); int labelId = int(vDet(5)); Rect rec(Point(vDet(1), vDet(2)), Point(vDet(3), vDet(4))); if( !is_border ) { rectangle(im, rec, Scalar(255,0,0), 3); putText(im, to_string(labelId), Point(vDet(1), vDet(2)), cv::FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0,255,0), 2); } } return im.clone(); } cv::Mat FrameDrawer::getCurrentFrameImage(){ return mmRGB; } cv::Mat FrameDrawer::getCurrentDepthFrameImage(){ return mmDepth; } } ================================================ FILE: src/core/Geometry.cpp ================================================ #include "core/Geometry.h" #include "utils/matrix_utils.h" #include "utils/dataprocess_utils.h" namespace EllipsoidSLAM { // Generates all points in the depth image as point cloud. PointCloud getPointCloud(cv::Mat &depth, cv::Mat &rgb, Eigen::VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera) { PointCloud cloud; int x1 = 0; int y1 = 0; int x2 = depth.cols; int y2 = depth.rows; for (int y = y1; y < y2; y++){ for (int x = x1; x < x2; x++) { ushort *ptd = depth.ptr(y); ushort d = ptd[x]; PointXYZRGB p; p.z = d / camera.scale; if (p.z <= 0.1 || p.z > 100) // limit the depth range in [0.1,100]m continue; p.x = (x - camera.cx) * p.z / camera.fx; p.y = (y - camera.cy) * p.z / camera.fy; // get rgb color p.b = rgb.ptr(y)[x * 3]; p.g = rgb.ptr(y)[x * 3 + 1]; p.r = rgb.ptr(y)[x * 3 + 2]; p.size = 1; cloud.push_back(p); } } return cloud; } // apply transformation on every points in the pointcloud PointCloud* transformPointCloud(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc) { Eigen::Matrix4d Twc = pCampose_wc->to_homogeneous_matrix(); auto * pPoints_global = new PointCloud(); for (auto &p : *pPoints_local) { Eigen::Vector3d xyz(p.x, p.y, p.z); Eigen::Vector3d xyz_w = TransformPoint(xyz, Twc); PointXYZRGB p_w; p_w.x = xyz_w[0]; p_w.y = xyz_w[1]; p_w.z = xyz_w[2]; p_w.r = p.r; p_w.g = p.g; p_w.b = p.b; p_w.size = p.size; pPoints_global->push_back(p_w); } return pPoints_global; } void transformPointCloudSelf(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc) { Eigen::Matrix4d Twc = pCampose_wc->to_homogeneous_matrix(); for (auto &p : *pPoints_local) { Eigen::Vector3d xyz(p.x, p.y, p.z); Eigen::Vector3d xyz_w = TransformPoint(xyz, Twc); p.x = xyz_w[0]; p.y = xyz_w[1]; p.z = xyz_w[2]; } return; } PointCloud loadPointsToPointVector(Eigen::MatrixXd &pMat){ int total_num = pMat.rows(); PointCloud points; for( int i=0; isize(); MatrixXd outMat; outMat.resize(num, 3); for( int i=0;isize(); Vector3d total(0,0,0); for( int i=0; i(point); Eigen::Vector4d Xw = T * Xc; Eigen::Vector3d xyz_w = homo_to_real_coord(Xw); return xyz_w; } } ================================================ FILE: src/core/Initializer.cpp ================================================ #include "core/Initializer.h" #include #include #include using namespace Eigen; using namespace std; namespace EllipsoidSLAM { Initializer::Initializer(int rows, int cols) { miImageRows = rows; miImageCols = cols; } // ascending sort bool cmp(paira, pairb) { return a.second > b.second; } // Initialize an ellipsoid from several bounding boxes and camera poses. g2o::ellipsoid Initializer::initializeQuadric(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib) { mbResult = false; int input_size = pose_mat.rows(); // 1) Generate tangent planes from bounding boxes and camera cneter. // Those invalid observations are ignored. MatrixXd planesHomo = getPlanesHomo(pose_mat, detection_mat, calib); int plane_size = planesHomo.cols(); int invalid_plane = input_size*4 - plane_size; if( invalid_plane > 0) std::cout << " * invalid_plane: " << invalid_plane << std::endl; if(plane_size < 9) // at least 9 planes are needed { g2o::ellipsoid e_bad; mbResult = false; return e_bad; } // Using SVD to generate a quadric MatrixXd planesVector = getVectorFromPlanesHomo(planesHomo); Matrix4d QStar = getQStarFromVectors(planesVector); // generate ellipsoid from the quadric g2o::ellipsoid e = getEllipsoidFromQStar(QStar); // Set color : blue. Eigen::Vector3d blueScalar(0,0,255); e.setColor(blueScalar); return e; } MatrixXd Initializer::getPlanesHomo(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib) { assert( pose_mat.rows() == detection_mat.rows() && " Two matrices should match. " ); assert( pose_mat.rows() > 2 && " At least 3 measurements are required. " ); MatrixXd planes_all(4,0); int rows = pose_mat.rows(); for(int i=0;i() = Matrix3d::Identity(3,3); Matrix3Xd proj_mat = Kalib * identity_lefttop; proj_mat = proj_mat * campose_cw.to_homogeneous_matrix(); return proj_mat; } MatrixXd Initializer::fromDetectionsToLines(VectorXd &detections) { bool flag_openFilter = true; // filter those lines lying on the image boundary double x1 = detections(0); double y1 = detections(1); double x2 = detections(2); double y2 = detections(3); Vector3d line1 (1, 0, -x1); Vector3d line2 (0, 1, -y1); Vector3d line3 (1, 0, -x2); Vector3d line4 (0, 1, -y2); // those lying on the image boundary have been marked -1 MatrixXd line_selected(3, 0); MatrixXd line_selected_none(3, 0); if( !flag_openFilter || ( x1>0 && x10 && y10 && x20 && y2 svd(A, ComputeThinU | ComputeThinV ); MatrixXd V = svd.matrixV(); VectorXd qj_hat = V.col(V.cols()-1); // Get QStar Matrix4d QStar; QStar << qj_hat(0),qj_hat(1),qj_hat(2),qj_hat(3), qj_hat(1),qj_hat(4),qj_hat(5),qj_hat(6), qj_hat(2),qj_hat(5),qj_hat(7),qj_hat(8), qj_hat(3),qj_hat(6),qj_hat(8),qj_hat(9); return QStar; } g2o::ellipsoid Initializer::getEllipsoidFromQStar(Matrix4d &QStar) { g2o::ellipsoid e; Matrix4d Q = QStar.inverse() * cbrt(QStar.determinant()); SelfAdjointEigenSolver es(Q); // ascending order by default MatrixXd D = es.eigenvalues().asDiagonal(); MatrixXd V = es.eigenvectors(); VectorXd eigens = es.eigenvalues(); // For an ellipsoid, the signs of the eigenvalues must be ---+ or +++- int num_pos = int(eigens(0)>0) +int(eigens(1)>0) +int(eigens(2)>0) +int(eigens(3)>0); int num_neg = int(eigens(0)<0) +int(eigens(1)<0) +int(eigens(2)<0) +int(eigens(3)<0); if( !(num_pos ==3 && num_neg == 1) && !(num_pos ==1 && num_neg == 3) ){ cout << " Not Ellipsoid : pos/neg " << num_pos << " / " << num_neg << endl; cout << "eigens :" << eigens.transpose() << endl; mbResult = false; return e; } else mbResult = true; if( eigens(3) > 0 ) // normalize to - - - + { Q=-Q; SelfAdjointEigenSolver es_2(Q); D = es_2.eigenvalues().asDiagonal(); V = es_2.eigenvectors(); eigens = es_2.eigenvalues(); } // Solve ellipsoid parameters from matrix Q Vector3d lambda_mat = eigens.head(3).array().inverse(); Matrix3d Q33 = Q.block(0,0,3,3); double k = Q.determinant()/Q33.determinant(); Vector3d value = -k*(lambda_mat); Vector3d s = value.array().abs().sqrt(); Vector4d t = QStar.col(3); t = t/t(3); Vector3d translation = t.head(3); SelfAdjointEigenSolver es2(Q33); MatrixXd D_Q33 = es2.eigenvalues().asDiagonal(); MatrixXd rot = es2.eigenvectors(); double r,p,y; rot_to_euler_zyx(rot, r, p, y); Vector3d rpy(r,p,y); // generate ellipsoid Vector9d objectVec; objectVec << t(0),t(1),t(2),rpy(0),rpy(1),rpy(2),s(0),s(1),s(2); e.fromMinimalVector(objectVec); return e; } void Initializer::sortEigenValues(VectorXcd &eigens, MatrixXcd &V) { vector> pairs; for(int i=0;i<4;i++) pairs.push_back( make_pair( i, eigens(i).real() ) ); sort(pairs.begin(), pairs.end(), cmp); // Construct a new matrix in order MatrixXcd V_new(4,4); Vector4cd eigens_new; for(int i=0;i<4;i++) { int oldID = pairs[i].first; V_new.col(i) = V.col(oldID); eigens_new(i) = complex(pairs[i].second, 0); } eigens = eigens_new; V = V_new; } double Initializer::quadricErrorWithPlanes(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib, g2o::ellipsoid &e) { Matrix4d QStar = e.generateQuadric(); // get Q^* Vector10d qj_hat; qj_hat << QStar(0,0),QStar(0,1),QStar(0,2),QStar(0,3),QStar(1,1),QStar(1,2),QStar(1,3),QStar(2,2),QStar(2,3),QStar(3,3); // Get planes vector MatrixXd planesHomo = getPlanesHomo(pose_mat, detection_mat, calib); MatrixXd planesVector = getVectorFromPlanesHomo(planesHomo); // Get error VectorXd result = planesVector.transpose() * qj_hat; return result.transpose() * result; } bool Initializer::getInitializeResult() { return mbResult; } g2o::ellipsoid Initializer::initializeQuadric(EllipsoidSLAM::Observations &obs, Matrix3d &calib) { MatrixXd pose_mat; MatrixXd detection_mat; // get pose matrix and detection matrix from observations getDetectionAndPoseMatFromObservations(obs, pose_mat, detection_mat); g2o::ellipsoid e = initializeQuadric(pose_mat, detection_mat, calib); if( getInitializeResult() ) { e.miLabel = obs[0]->label; } return e; } void Initializer::getDetectionAndPoseMatFromObservations(EllipsoidSLAM::Observations &obs, MatrixXd &pose_mat, MatrixXd &detection_mat) { int frameSize = obs.size(); pose_mat.resize(frameSize, 7); // x y z qx qy qz qw detection_mat.resize(frameSize, 5); // x1 y1 x2 y2 accuracy int id = 0; for (auto iter = obs.begin(); iter!=obs.end(); iter++) { Vector5d det_vec; det_vec << (*iter)->bbox, (*iter)->rate; Vector7d pos_vec = (*iter)->pFrame->cam_pose_Twc.toVector(); pose_mat.row(id) = pos_vec; detection_mat.row(id) = det_vec; id ++; } } } ================================================ FILE: src/core/Map.cpp ================================================ #include "include/core/Map.h" using namespace std; namespace EllipsoidSLAM { Map::Map() { mCameraState = new g2o::SE3Quat(); } void Map::addPoint(EllipsoidSLAM::PointXYZRGB *pPoint) { unique_lock lock(mMutexMap); mspPoints.insert(pPoint); } void Map::addPointCloud(EllipsoidSLAM::PointCloud *pPointCloud) { unique_lock lock(mMutexMap); for(auto iter=pPointCloud->begin();iter!=pPointCloud->end();++iter){ mspPoints.insert(&(*iter)); } } void Map::clearPointCloud() { unique_lock lock(mMutexMap); mspPoints.clear(); } void Map::addEllipsoid(ellipsoid *pObj) { unique_lock lock(mMutexMap); mspEllipsoids.push_back(pObj); } vector Map::GetAllEllipsoids() { unique_lock lock(mMutexMap); return mspEllipsoids; } std::vector Map::GetAllPoints() { unique_lock lock(mMutexMap); return vector(mspPoints.begin(),mspPoints.end()); } void Map::addPlane(plane *pPlane) { unique_lock lock(mMutexMap); mspPlanes.insert(pPlane); } vector Map::GetAllPlanes() { unique_lock lock(mMutexMap); return vector(mspPlanes.begin(),mspPlanes.end()); } void Map::setCameraState(g2o::SE3Quat* state) { unique_lock lock(mMutexMap); mCameraState = state; } void Map::addCameraStateToTrajectory(g2o::SE3Quat* state) { unique_lock lock(mMutexMap); mvCameraStates.push_back(state); } g2o::SE3Quat* Map::getCameraState() { unique_lock lock(mMutexMap); return mCameraState; } std::vector Map::getCameraStateTrajectory() { unique_lock lock(mMutexMap); return mvCameraStates; } std::vector Map::getEllipsoidsUsingLabel(int label) { unique_lock lock(mMutexMap); std::vector mvpObjects; auto iter = mspEllipsoids.begin(); for(; iter!=mspEllipsoids.end(); iter++) { if( (*iter)->miLabel == label ) mvpObjects.push_back(*iter); } return mvpObjects; } std::map Map::GetAllEllipsoidsMap() { std::map maps; for(auto iter= mspEllipsoids.begin(); iter!=mspEllipsoids.end();iter++) { maps.insert(make_pair((*iter)->miInstanceID, *iter)); } return maps; } void Map::clearPlanes(){ unique_lock lock(mMutexMap); mspPlanes.clear(); } void Map::addEllipsoidVisual(ellipsoid *pObj) { unique_lock lock(mMutexMap); mspEllipsoidsVisual.push_back(pObj); } vector Map::GetAllEllipsoidsVisual() { unique_lock lock(mMutexMap); return mspEllipsoidsVisual; } void Map::ClearEllipsoidsVisual() { unique_lock lock(mMutexMap); mspEllipsoidsVisual.clear(); } bool Map::AddPointCloudList(const string& name, PointCloud* pCloud, int type){ unique_lock lock(mMutexMap); // Check repetition if(mmPointCloudLists.find(name) != mmPointCloudLists.end() ) { // Exist if( type == 0){ // replace it. mmPointCloudLists[name]->clear(); // release it mmPointCloudLists[name] = pCloud; } else if( type == 1 ) { // add together for( auto &p : *pCloud ) mmPointCloudLists[name]->push_back(p); } return false; } else{ mmPointCloudLists.insert(make_pair(name, pCloud)); return true; } } bool Map::DeletePointCloudList(const string& name, int type){ unique_lock lock(mMutexMap); if( type == 0 ) // complete matching: the name must be the same { auto iter = mmPointCloudLists.find(name); if (iter != mmPointCloudLists.end() ) { mmPointCloudLists.erase(iter); return true; } else{ std::cerr << "PointCloud name " << name << " doesn't exsit. Can't delete it." << std::endl; return false; } } else if ( type == 1 ) // partial matching { bool deleteSome = false; for( auto iter = mmPointCloudLists.begin();iter!=mmPointCloudLists.end();) { auto strPoints = *iter; if( strPoints.first.find(name) != strPoints.first.npos ) { iter = mmPointCloudLists.erase(iter); deleteSome = true; } else iter++; } return deleteSome; } } bool Map::ClearPointCloudLists(){ unique_lock lock(mMutexMap); mmPointCloudLists.clear(); } std::map Map::GetPointCloudList(){ unique_lock lock(mMutexMap); return mmPointCloudLists; } } // namespace ================================================ FILE: src/core/MapDrawer.cpp ================================================ #include "include/core/MapDrawer.h" #include #include #include #include namespace EllipsoidSLAM { // draw axis for ellipsoids void drawAxisNormal() { float length = 2.0; // x glColor3f(1.0,0.0,0.0); // red x glBegin(GL_LINES); glVertex3f(0.0, 0.0f, 0.0f); glVertex3f(length, 0.0f, 0.0f); glEnd(); // y glColor3f(0.0,1.0,0.0); // green y glBegin(GL_LINES); glVertex3f(0.0, 0.0f, 0.0f); glVertex3f(0.0, length, 0.0f); glEnd(); // z glColor3f(0.0,0.0,1.0); // blue z glBegin(GL_LINES); glVertex3f(0.0, 0.0f ,0.0f ); glVertex3f(0.0, 0.0f ,length ); glEnd(); } MapDrawer::MapDrawer(const string &strSettingPath, Map* pMap):mpMap(pMap) { cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); mKeyFrameSize = fSettings["Viewer.KeyFrameSize"]; mKeyFrameLineWidth = fSettings["Viewer.KeyFrameLineWidth"]; mGraphLineWidth = fSettings["Viewer.GraphLineWidth"]; mPointSize = fSettings["Viewer.PointSize"]; mCameraSize = fSettings["Viewer.CameraSize"]; mCameraLineWidth = fSettings["Viewer.CameraLineWidth"]; float fx = fSettings["Camera.fx"]; float fy = fSettings["Camera.fy"]; float cx = fSettings["Camera.cx"]; float cy = fSettings["Camera.cy"]; mCalib << fx, 0, cx, 0, fy, cy, 0, 0, 1; } // draw external cubes. bool MapDrawer::drawObjects() { std::vector ellipsoids = mpMap->GetAllEllipsoids(); std::vector ellipsoidsVisual = mpMap->GetAllEllipsoidsVisual(); ellipsoids.insert(ellipsoids.end(), ellipsoidsVisual.begin(), ellipsoidsVisual.end()); for( size_t i=0; icompute3D_BoxCorner(); glPushMatrix(); glLineWidth(mCameraLineWidth); if(ellipsoids[i]->isColorSet()){ Vector3d color = ellipsoids[i]->getColor(); glColor3f(1.0f,0.0f,0.0f); // red color. } else glColor3f(0.0f,0.0f,1.0f); glBegin(GL_LINES); // draw cube lines. for(int m=0;m ellipsoids = mpMap->GetAllEllipsoids(); std::vector ellipsoidsVisual = mpMap->GetAllEllipsoidsVisual(); ellipsoids.insert(ellipsoids.end(), ellipsoidsVisual.begin(), ellipsoidsVisual.end()); for( size_t i=0; ipose.inverse(); Vector3d scale = ellipsoids[i]->scale; glPushMatrix(); glLineWidth(mCameraLineWidth*3/4.0); if(ellipsoids[i]->isColorSet()){ Vector4d color = ellipsoids[i]->getColorWithAlpha(); glColor4f(color(0),color(1),color(2),color(3)); } else glColor3f(0.0f,0.0f,1.0f); GLUquadricObj *pObj; pObj = gluNewQuadric(); gluQuadricDrawStyle(pObj, GLU_LINE); pangolin::OpenGlMatrix Twm; // model to world SE3ToOpenGLCameraMatrix(TmwSE3, Twm); glMultMatrixd(Twm.m); glScaled(scale[0],scale[1],scale[2]); gluSphere(pObj, 1.0, 26, 13); // draw a sphere with radius 1.0, center (0,0,0), slices 26, and stacks 13. drawAxisNormal(); glPopMatrix(); } return true; } // draw all the planes bool MapDrawer::drawPlanes() { std::vector planes = mpMap->GetAllPlanes(); for( size_t i=0; i x,y,z; if( p->mbLimited ) // draw a finite plane. { pieces = 100; double area_range = p->mdPlaneSize; double step = area_range/pieces; // ----- x x.clear(); x.reserve(pieces+2); starting = p->mvPlaneCenter[0] - area_range/2; ending = p->mvPlaneCenter[0] + area_range/2; while(starting <= ending) { x.push_back(starting); starting += step; } // ----- y y.clear(); y.reserve(pieces+2); starting = p->mvPlaneCenter[1] - area_range/2; ending = p->mvPlaneCenter[1] + area_range/2; while(starting <= ending) { y.push_back(starting); starting += step; } // ----- z z.clear(); z.reserve(pieces+2); starting = p->mvPlaneCenter[2] - area_range/2; ending = p->mvPlaneCenter[2] + area_range/2; while(starting <= ending) { z.push_back(starting); starting += step; } } else // draw an infinite plane, make it big enough { starting = -5; ending = 5; x.clear(); double step = (ending-starting)/pieces; x.reserve(pieces+2); while(starting <= ending) { x.push_back(starting); starting += step; } y=x; z=x; } Vector4d param = p->param; Vector3d color = p->color; glPushMatrix(); glBegin(GL_POINTS); glColor3f(color[0], color[1], color[2]); double param_abs_x = std::abs(param[0]); double param_abs_y = std::abs(param[1]); double param_abs_z = std::abs(param[2]); if( param_abs_z > param_abs_x && param_abs_z > param_abs_y ){ // if the plane is extending toward x axis, use x,y to calculate z. for(int i=0; i Z = (-D-BY-AX)/C double z_ = (-param[3]-param[1]*y[j]-param[0]*x[i])/param[2]; glVertex3f(float(x[i]), float(y[j]), float(z_)); } } } else if( param_abs_x > param_abs_z && param_abs_x > param_abs_y ) { // if the plane is extending toward z axis, use y,z to calculate x. for(int i=0; i X = (-D-BY-CZ)/A double x_ = (-param[3]-param[1]*y[j]-param[2]*z[i])/param[0]; glVertex3f(float(x_), float(y[j]), float(z[i])); } } } else { // if the plane is extending toward y axis, use x,z to calculate y. for(int i=0; i Y = (-D-AX-CZ)/B double y_ = (-param[3]-param[0]*x[j]-param[2]*z[i])/param[1]; glVertex3f(float(x[j]), float(y_), float(z[i])); } } } glEnd(); glPopMatrix(); } bool MapDrawer::drawCameraState() { g2o::SE3Quat* cameraState = mpMap->getCameraState(); // Twc pangolin::OpenGlMatrix Twc; if( cameraState!=NULL ) SE3ToOpenGLCameraMatrixOrigin(*cameraState, Twc); else { std::cerr << "Can't load camera state." << std::endl; Twc.SetIdentity(); } const float &w = mCameraSize*1.5; const float h = w*0.75; const float z = w*0.6; glPushMatrix(); #ifdef HAVE_GLES glMultMatrixf(Twc.m); #else glMultMatrixd(Twc.m); #endif glLineWidth(mCameraLineWidth); glColor3f(1.0f,0.0f,0.0f); glBegin(GL_LINES); glVertex3f(0,0,0); glVertex3f(w,h,z); glVertex3f(0,0,0); glVertex3f(w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,-h,z); glVertex3f(0,0,0); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(w,-h,z); glVertex3f(-w,h,z); glVertex3f(-w,-h,z); glVertex3f(-w,h,z); glVertex3f(w,h,z); glVertex3f(-w,-h,z); glVertex3f(w,-h,z); glEnd(); glPopMatrix(); return true; } bool MapDrawer::drawTrajectory() { std::vector traj = mpMap->getCameraStateTrajectory(); for(int i=0; i pPoints = mpMap->GetAllPoints(); glPushMatrix(); for(int i=0; i lock(mMutexCamera); Rwc = matIn.rowRange(0,3).colRange(0,3).t(); twc = -Rwc*matIn.rowRange(0,3).col(3); } M.m[0] = Rwc.at(0,0); M.m[1] = Rwc.at(1,0); M.m[2] = Rwc.at(2,0); M.m[3] = 0.0; M.m[4] = Rwc.at(0,1); M.m[5] = Rwc.at(1,1); M.m[6] = Rwc.at(2,1); M.m[7] = 0.0; M.m[8] = Rwc.at(0,2); M.m[9] = Rwc.at(1,2); M.m[10] = Rwc.at(2,2); M.m[11] = 0.0; M.m[12] = twc.at(0); M.m[13] = twc.at(1); M.m[14] = twc.at(2); M.m[15] = 1.0; } else M.SetIdentity(); } // not inverse, keep origin void MapDrawer::SE3ToOpenGLCameraMatrixOrigin(g2o::SE3Quat &matInSe3, pangolin::OpenGlMatrix &M) { // eigen to cv Eigen::Matrix4d matEigen = matInSe3.to_homogeneous_matrix(); cv::Mat matIn; eigen2cv(matEigen, matIn); if(!matIn.empty()) { cv::Mat Rwc(3,3,CV_64F); cv::Mat twc(3,1,CV_64F); { unique_lock lock(mMutexCamera); Rwc = matIn.rowRange(0,3).colRange(0,3); twc = matIn.rowRange(0,3).col(3); } M.m[0] = Rwc.at(0,0); M.m[1] = Rwc.at(1,0); M.m[2] = Rwc.at(2,0); M.m[3] = 0.0; M.m[4] = Rwc.at(0,1); M.m[5] = Rwc.at(1,1); M.m[6] = Rwc.at(2,1); M.m[7] = 0.0; M.m[8] = Rwc.at(0,2); M.m[9] = Rwc.at(1,2); M.m[10] = Rwc.at(2,2); M.m[11] = 0.0; M.m[12] = twc.at(0); M.m[13] = twc.at(1); M.m[14] = twc.at(2); M.m[15] = 1.0; } else M.SetIdentity(); } void MapDrawer::setCalib(Eigen::Matrix3d& calib) { mCalib = calib; } void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M) { g2o::SE3Quat *cameraState = mpMap->getCameraState(); // Twc if (cameraState != NULL) SE3ToOpenGLCameraMatrixOrigin(*cameraState, M); else { M.SetIdentity(); } } void MapDrawer::drawPointCloudLists() { auto pointLists = mpMap->GetPointCloudList(); glPushMatrix(); for(auto pair:pointLists){ auto pPoints = pair.second; if( pPoints == NULL ) continue; for(int i=0; isize(); i=i+1) { PointXYZRGB &p = (*pPoints)[i]; glPointSize( p.size ); glBegin(GL_POINTS); glColor3d(p.r/255.0, p.g/255.0, p.b/255.0); glVertex3d(p.x, p.y, p.z); glEnd(); } } glPointSize( 1 ); glPopMatrix(); } void MapDrawer::drawPointCloudWithOptions(const std::map &options) { auto pointLists = mpMap->GetPointCloudList(); glPushMatrix(); for(auto pair:pointLists){ auto pPoints = pair.second; if( pPoints == NULL ) continue; auto iter = options.find(pair.first); if(iter == options.end()) { continue; // not exist } if(iter->second == false) continue; // menu is closed for(int i=0; isize(); i=i+1) { PointXYZRGB &p = (*pPoints)[i]; glPointSize( p.size ); glBegin(GL_POINTS); glColor3d(p.r/255.0, p.g/255.0, p.b/255.0); glVertex3d(p.x, p.y, p.z); glEnd(); } } glPointSize( 1 ); glPopMatrix(); } } ================================================ FILE: src/core/Optimizer.cpp ================================================ #include "include/core/Optimizer.h" #include "include/core/Ellipsoid.h" #include "include/core/BasicEllipsoidEdges.h" #include "Thirdparty/g2o/g2o/core/block_solver.h" #include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h" #include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h" #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" #include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h" #include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h" #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" #include using namespace cv; namespace EllipsoidSLAM { bool isInImage(Eigen::Vector2d& uv, int rows, int cols){ if( uv(0) >0 && uv(0) < cols ) if( uv(1) >0 && uv(1) < rows ) return true; return false; } // [ Unused ] // The function checks several conditions to decide whether the edge will be activated: // 1) if the object is behind the camera // 2) if the camera is inside the elipsoid, which generates an ill condition // 3) if the projected ellipse lies in the image width and height // if one of the boundingbox vertces lies in the image, active it too. bool checkVisibility(g2o::EdgeSE3EllipsoidProj *edge, g2o::VertexSE3Expmap *vSE3, g2o::VertexEllipsoid *vEllipsoid, Eigen::Matrix3d &mCalib, int rows, int cols) { g2o::ellipsoid e = vEllipsoid->estimate(); Vector3d ellipsoid_center = e.toMinimalVector().head(3); // Pwo Vector4d center_homo = real_to_homo_coord_vec(ellipsoid_center); g2o::SE3Quat campose_Tcw = vSE3->estimate(); Eigen::Matrix4d projMat = campose_Tcw.to_homogeneous_matrix(); // Tcw // project to image plane. Vector4d center_inCameraAxis_homo = projMat * center_homo; // Pco = Tcw * Pwo Vector3d center_inCameraAxis = homo_to_real_coord_vec(center_inCameraAxis_homo); if( center_inCameraAxis_homo(2) < 0) // if the object is behind the camera. z< 0 { return false; } // check if the camera is inside the elipsoid, which generates an ill condition g2o::SE3Quat campose_Twc = campose_Tcw.inverse(); Eigen::Vector3d X_cam = campose_Twc.translation(); Eigen::Vector4d X_homo = real_to_homo_coord_vec(X_cam); Eigen::Matrix4d Q_star = e.generateQuadric(); Eigen::Matrix4d Q = Q_star.inverse(); double point_in_Q = X_homo.transpose() * Q * X_homo; if(point_in_Q < 0) // the center of the camera is inside the ellipsoid return false; // check if the projected ellipse lies in the image width and height Eigen::Matrix3Xd P = e.generateProjectionMatrix(campose_Tcw, mCalib); Eigen::Vector3d uv = P * center_homo; uv = uv/uv(2); Eigen::Vector2d uv_2d(uv(0), uv(1)); if( isInImage(uv_2d, rows, cols) ) return true; // if one of the boundingbox vertces lies in the image, active it too. Vector4d vec_proj = edge->getProject(); Vector2d point_lu(vec_proj(0), vec_proj(1)); Vector2d point_rd(vec_proj(2), vec_proj(3)); if( isInImage(point_lu,rows,cols) || isInImage(point_rd, rows, cols) ) return true; return false; } void Optimizer::GlobalObjectGraphOptimization(std::vector &pFrames, Map *pMap, int rows, int cols, Matrix3d &mCalib, std::map& objectObservations, bool save_graph, bool withAssociation, bool check_visibility) { // ************************ LOAD CONFIGURATION ************************ double config_ellipsoid_3d_scale = Config::Get("Optimizer.Edges.3DEllipsoid.Scale"); bool mbSetGravityPrior = Config::Get("Optimizer.Edges.GravityPrior.Open") == 1; double dGravityPriorScale = Config::Get("Optimizer.Edges.GravityPrior.Scale"); bool mbOpen3DProb = true; // OUTPUT std::cout << " -- Optimization parameters : " << std::endl; if(mbGroundPlaneSet) std::cout << " [ Using Ground Plane: " << mGroundPlaneNormal.transpose() << " ] " << std::endl; if(!mbGroundPlaneSet || !mbSetGravityPrior ) std::cout << " * Gravity Prior : closed." << std::endl; else std::cout << " * Gravity Prior : Open." << std::endl; cout<<" * Scale_3dedge: " << config_ellipsoid_3d_scale << endl; cout<<" * Scale_GravityPrior: " << dGravityPriorScale << endl; // ************************************************************************ // Initialize variables. int total_frame_number = int(pFrames.size()); std::map pEllipsoidsMapWithInstance = pMap->GetAllEllipsoidsMap(); int objects_num = int(pEllipsoidsMapWithInstance.size()); // initialize graph optimization. g2o::SparseOptimizer graph; g2o::BlockSolverX::LinearSolverType* linearSolver; linearSolver = new g2o::LinearSolverDense(); g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); graph.setAlgorithm(solver); graph.setVerbose(false); // Set output. std::map vEllipsoidVertexMaps; std::vector edges, edgesValid, edgesInValid; std::vector validVec; validVec.resize(total_frame_number); std::vector edgesEllipsoidGravityPlanePrior; // Gravity prior std::vector vSE3Vertex; // Add SE3 vertices for camera poses bool bSLAM_mode = false; // Mapping Mode : Fix camera poses and mapping ellipsoids only for( int frame_index=0; frame_index< total_frame_number ; frame_index++) { g2o::SE3Quat curr_cam_pose_Twc = pFrames[frame_index]->cam_pose_Twc; Eigen::MatrixXd det_mat = pFrames[frame_index]->mmObservations; g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); vSE3->setId(graph.vertices().size()); graph.addVertex(vSE3); vSE3->setEstimate(pFrames[frame_index]->cam_pose_Tcw); // Tcw if(!bSLAM_mode) vSE3->setFixed(true); // Fix all the poses in mapping mode. else vSE3->setFixed(frame_index == 0); vSE3Vertex.push_back(vSE3); // Add odom edges if in SLAM Mode if(bSLAM_mode && frame_index > 0){ g2o::SE3Quat prev_cam_pose_Tcw = pFrames[frame_index-1]->cam_pose_Twc.inverse(); g2o::SE3Quat curr_cam_pose_Tcw = curr_cam_pose_Twc.inverse(); g2o::SE3Quat odom_val = curr_cam_pose_Tcw*prev_cam_pose_Tcw.inverse();; g2o::EdgeSE3Expmap* e = new g2o::EdgeSE3Expmap(); e->setVertex(0, dynamic_cast( vSE3Vertex[frame_index-1] )); e->setVertex(1, dynamic_cast( vSE3Vertex[frame_index] )); e->setMeasurement(odom_val); e->setId(graph.edges().size()); Vector6d inv_sigma;inv_sigma<<1,1,1,1,1,1; inv_sigma = inv_sigma*1.0; Matrix6d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal(); e->setInformation(info); graph.addEdge(e); } } // Initialize objects vertices and add edges of camera-objects 2d observations int objects_ob_num = objectObservations.size(); int objectid_in_edge = 0; int current_ob_id = 0; int symplaneid_in_edge = 0; for(auto instanceObs : objectObservations ) { int instance = instanceObs.first; if (pEllipsoidsMapWithInstance.find(instance) == pEllipsoidsMapWithInstance.end()) // if the instance has not been initialized yet continue; Observations obs = instanceObs.second; // Add objects vertices g2o::VertexEllipsoid *vEllipsoid = new g2o::VertexEllipsoid(); vEllipsoid->setEstimate(*pEllipsoidsMapWithInstance[instance]); vEllipsoid->setId(graph.vertices().size()); vEllipsoid->setFixed(false); graph.addVertex(vEllipsoid); vEllipsoidVertexMaps.insert(make_pair(instance, vEllipsoid)); // Add gravity prior if(mbGroundPlaneSet && mbSetGravityPrior ){ g2o::EdgeEllipsoidGravityPlanePrior *vGravityPriorEdge = new g2o::EdgeEllipsoidGravityPlanePrior; vGravityPriorEdge->setVertex(0, dynamic_cast( vEllipsoid )); vGravityPriorEdge->setMeasurement(mGroundPlaneNormal); Matrix inv_sigma; inv_sigma << 1 * dGravityPriorScale; MatrixXd info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal(); vGravityPriorEdge->setInformation(info); graph.addEdge(vGravityPriorEdge); edgesEllipsoidGravityPlanePrior.push_back(vGravityPriorEdge); } // Add camera-objects 2d constraints // At least 3 observations are needed to activiate 2d edges. Since one ellipsoid has 9 degrees, // and every observation offers 4 constrains, only at least 3 observations could fully constrain an ellipsoid. bool bVvalid_2d_constraints = (obs.size() > 2); if( bVvalid_2d_constraints ){ for( int i=0; ilabel; Vector4d measure_bbox = vOb->bbox; double measure_prob = vOb->rate; // find the corresponding frame vertex. int frame_index = vOb->pFrame->frame_seq_id; g2o::VertexSE3Expmap *vSE3 = vSE3Vertex[frame_index]; // create 2d edge g2o::EdgeSE3EllipsoidProj *e = new g2o::EdgeSE3EllipsoidProj(); e->setVertex(0, dynamic_cast( vSE3 )); e->setVertex(1, dynamic_cast( vEllipsoidVertexMaps[instance] )); e->setMeasurement(measure_bbox); e->setId(graph.edges().size()); Vector4d inv_sigma; inv_sigma << 1, 1, 1, 1; Matrix4d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal(); info = info * measure_prob; e->setInformation(info); // e->setRobustKernel(new g2o::RobustKernelHuber()); // Huber Kernel e->setKalib(mCalib); e->setLevel(0); edges.push_back(e); // Two conditions for valid 2d edges: // 1) [closed] visibility check: see the comments of function checkVisibility for detail. // 2) NaN check bool c1 = (!check_visibility) || checkVisibility(e, vSE3, vEllipsoidVertexMaps[instance], mCalib, rows, cols); e->computeError(); double e_error = e->chi2(); bool c2 = !isnan(e_error); // NaN check if( c1 && c2 ){ graph.addEdge(e); edgesValid.push_back(e); // valid edges and invalid edges are for debug output } else edgesInValid.push_back(e); } } } // Add 3d constraints std::vector vEllipsoid3DEdges; for( int frame_index=0; frame_index< total_frame_number ; frame_index++) { auto &ObjectsInFrame = pFrames[frame_index]->mpLocalObjects; for( auto obj : ObjectsInFrame ) { if( obj == NULL ) continue; int instance = obj->miInstanceID; if(vEllipsoidVertexMaps.find(instance) == vEllipsoidVertexMaps.end()) continue; // if the instance has not been initialized auto vEllipsoid = vEllipsoidVertexMaps[instance]; auto vSE3 = vSE3Vertex[frame_index]; // create 3d edges g2o::EdgeSE3Ellipsoid9DOF* vEllipsoid3D = new g2o::EdgeSE3Ellipsoid9DOF; vEllipsoid3D->setId(graph.edges().size()); vEllipsoid3D->setVertex(0, dynamic_cast( vSE3 )); vEllipsoid3D->setVertex(1, dynamic_cast( vEllipsoidVertexMaps[instance] )); vEllipsoid3D->setMeasurement(*obj); Vector9d inv_sigma; inv_sigma << 1,1,1,1,1,1,1,1,1; if(mbOpen3DProb) inv_sigma = inv_sigma * sqrt(obj->prob); Matrix9d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal() * config_ellipsoid_3d_scale; vEllipsoid3D->setInformation(info); graph.addEdge(vEllipsoid3D); vEllipsoid3DEdges.push_back(vEllipsoid3D); } } // output std::cout << " -- GRAPH INFORMATION : " << std::endl; cout << " * Object Num : " << objects_num << endl; cout<<" * Vertices: "<estimate(); } else { cerr << "Find Error in ellipsoid storage. " << endl; cout << "Map: instance" << (*iter).first << endl; } } // Output optimization information. // object list ofstream out_obj("./object_list.txt"); auto iter = vEllipsoidVertexMaps.begin(); for(;iter!=vEllipsoidVertexMaps.end();iter++) { out_obj << iter->first << "\t" << iter->second->estimate().toMinimalVector().transpose() << "\t" << iter->second->estimate().miLabel << std::endl; } out_obj.close(); } Optimizer::Optimizer() { mbGroundPlaneSet = false; } void Optimizer::SetGroundPlane(Vector4d& normal){ mbGroundPlaneSet = true; mGroundPlaneNormal = normal; } } ================================================ FILE: src/core/Plane.cpp ================================================ #include "include/core/Plane.h" namespace g2o { plane::plane() { param = Vector4d(1,0,0,0); color = Vector3d(1,0,0); mdDualDis = 0; mbLimited = false; } plane::plane(Vector4d param_, Eigen::Vector3d color_) { param = param_; color = color_; mdDualDis = 0; mbLimited = false; } plane plane::exp_update(const Vector3d& update) { g2o::plane plane_update; plane_update.param[0] = param[0] + update[0]; // A plane_update.param[1] = param[1] + update[1]; // B plane_update.param[2] = 0; // C plane_update.param[3] = param[3] + update[2]; // D plane_update.color = color; return plane_update; } plane plane::exp_update2DOF(const Vector2d& update) { double k_update = update[0]; double b_update = update[1]; double k_current = -param[0]/param[1]; double b_current = -param[3]/param[1]; double k = k_current + k_update; double b = b_current + b_update; double B = -1; double A = k; double C = 0; double D = b; g2o::plane plane_update; plane_update.param[0] = A; // A plane_update.param[1] = B; // B plane_update.param[2] = C; // C ; z remains unchanged plane_update.param[3] = D; // D plane_update.color = color; return plane_update; } plane::plane(const plane &p){ param = p.param; color = p.color; mbLimited = p.mbLimited; mvPlaneCenter = p.mvPlaneCenter; mdPlaneSize = p.mdPlaneSize; mdDualDis = p.mdDualDis; } const plane& plane::operator=(const plane& p){ param = p.param; color = p.color; mbLimited = p.mbLimited; mvPlaneCenter = p.mvPlaneCenter; mdPlaneSize = p.mdPlaneSize; mdDualDis = p.mdDualDis; return p; } void plane::fromPointAndNormal(const Vector3d &point, const Vector3d &normal) { param.head(3) = normal; // normal : [ a, b, c]^T param[3] = -point.transpose() * normal; // X^T * pi = 0 ; ax0+by0+cz0+d=0 color = Vector3d(1,0,0); mdDualDis = 0; } void plane::fromDisAndAngle(double dis, double angle) { fromDisAngleTrans(dis, angle, 0); } void plane::fromDisAngleTrans(double dis, double angle, double trans) { param[0] = sin(angle); param[1] = -cos(angle); param[2] = 0; param[3] = -dis; mdDualDis = trans; } double plane::distanceToPoint(const Vector3d& point, bool keep_flag){ double fenzi = param(0)*point(0) + param(1)*point(1) +param(2)*point(2) + param(3); double fenmu = std::sqrt ( param(0)*param(0)+param(1)*param(1)+param(2)*param(2) ); double value = fenzi/fenmu; if(keep_flag) return value; else return std::abs(value); } void plane::transform(g2o::SE3Quat& Twc){ Matrix4d matTwc = Twc.to_homogeneous_matrix(); Matrix4d matTwc_trans = matTwc.transpose(); Matrix4d matTwc_trans_inv = matTwc_trans.inverse(); param = matTwc_trans_inv * param; } // for the visualization of symmetry planes void plane::InitFinitePlane(const Vector3d& center, double size) { mdPlaneSize = size; mvPlaneCenter = center; mbLimited = true; } Eigen::Vector4d plane::GeneratePlaneVec() { return param; } Eigen::Vector4d plane::GenerateAnotherPlaneVec() { // azimuth : angle of the normal g2o::plane p2; p2.fromDisAndAngle(mdDualDis, azimuth()); return p2.param; } Vector3d plane::GetLineFromCenterAngle(const Vector2d center, double angle) { // x = center[0] + t * cos(theta) // y = center[1] + t * sin(theta) // goal : // AX + BY + C = 0 ; get A,B,C // get rid of t: // sin(theta) * x - cos(theta) * y = // sin(theta) * center[0] - cos(theta) * center[1] // so: // sint * x + (- cost) * y + (cost*c1 - sint*c0) = 0 Vector3d param; param[0] = sin(angle); param[1] = -cos(angle); param[2] = cos(angle) * center[1] - sin(angle) * center[0]; return param; } Vector4d plane::LineToPlane(const Vector3d line) { Vector4d plane; plane << line[0], line[1], 0, line[2]; return plane; } } // g2o namespace ================================================ FILE: src/core/System.cpp ================================================ #include "include/core/System.h" #include "include/utils/dataprocess_utils.h" #include "src/config/Config.h" namespace EllipsoidSLAM { System::System(const string &strSettingsFile, const bool bUseViewer) { cout << endl << "EllipsoidSLAM Project 2019, Beihang University." << endl; cout << " Input Sensor: RGB-D " << endl; cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); if(!fsSettings.isOpened()) { cerr << "Failed to open settings file at: " << strSettingsFile << endl; exit(-1); } // Initialize global settings. Config::Init(); Config::SetParameterFile(strSettingsFile); //Create the Map mpMap = new Map(); mpFrameDrawer = new FrameDrawer(mpMap); //Create Drawers. These are used by the Viewer mpMapDrawer = new MapDrawer(strSettingsFile, mpMap); mpTracker = new Tracking(this, mpFrameDrawer, mpMapDrawer, mpMap, strSettingsFile); mpFrameDrawer->setTracker(mpTracker); //Initialize the Viewer thread and launch if(bUseViewer) { mpViewer = new Viewer(this, strSettingsFile, mpMapDrawer); mptViewer = new thread(&Viewer::run, mpViewer); mpViewer->SetFrameDrawer(mpFrameDrawer); } OpenDepthEllipsoid(); // Open Single-Frame Ellipsoid Extraction mpTracker->OpenGroundPlaneEstimation(); // Open Groundplane Estimation. } bool System::TrackWithObjects(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd & bboxMat, const cv::Mat &imDepth, const cv::Mat &imRGB, bool withAssociation) { return mpTracker->GrabPoseAndObjects(timestamp, pose, bboxMat, imDepth, imRGB, withAssociation); } Map* System::getMap() { return mpMap; } MapDrawer* System::getMapDrawer() { return mpMapDrawer; } FrameDrawer* System::getFrameDrawer() { return mpFrameDrawer; } Viewer* System::getViewer() { return mpViewer; } Tracking* System::getTracker() { return mpTracker; } void System::SaveObjectsToFile(string &path){ auto ellipsoids = mpMap->GetAllEllipsoids(); MatrixXd objMat;objMat.resize(ellipsoids.size(), 11); int i=0; for(auto e : ellipsoids) { Vector10d vec = e->toVector(); Eigen::Matrix vec_instance; vec_instance << e->miInstanceID, vec; objMat.row(i++) = vec_instance; } saveMatToFile(objMat, path.c_str()); std::cout << "Save " << ellipsoids.size() << " objects to " << path << std::endl; } void System::OpenDepthEllipsoid() { mpTracker->OpenDepthEllipsoid(); } void System::OpenOptimization() { mpTracker->OpenOptimization(); } void System::CloseOptimization() { mpTracker->CloseOptimization(); } } ================================================ FILE: src/core/Tracking.cpp ================================================ #include "include/core/Tracking.h" #include "src/config/Config.h" #include "utils/dataprocess_utils.h" Eigen::MatrixXd matSymPlanes; namespace EllipsoidSLAM { void outputObjectObservations(std::map &mmObjectObservations) { ofstream out("./log_mmObjectObservations.txt"); out << " --------- ObjectObservations : " << std::endl; for ( auto obPair: mmObjectObservations) { out << " ---- Instance " << obPair.first << " (" << obPair.second.size() << ") :" << std::endl; for( auto ob : obPair.second ) { out << " -- ob : " << ob->pFrame->frame_seq_id << " | " << ob->bbox.transpose() << " | " << ob->label << " | " << ob->rate << std::endl; } out << std::endl; } out.close(); std::cout << "Save to log_mmObjectObservations.txt..." << std::endl; } void Tracking::outputBboxMatWithAssociation() { std::map mapTimestampToObservations; for ( auto obPair: mmObjectObservations) { int instance = obPair.first; for( auto ob : obPair.second ) { ob->instance = instance; // save with timestamp if(mapTimestampToObservations.find(ob->pFrame->timestamp)!=mapTimestampToObservations.end()) mapTimestampToObservations[ob->pFrame->timestamp].push_back(ob); else { mapTimestampToObservations.insert(make_pair(ob->pFrame->timestamp, Observations())); mapTimestampToObservations[ob->pFrame->timestamp].push_back(ob); } } } for( auto frameObsPair : mapTimestampToObservations ){ string str_timestamp = to_string(frameObsPair.first); string filename = string("./bbox/") + str_timestamp + ".txt"; ofstream out(filename.c_str()); int num = 0; for ( auto ob: frameObsPair.second) { out << num++ << " " << ob->bbox.transpose() << " " << ob->label << " " << ob->rate << " " << ob->instance << std::endl; } out.close(); std::cout << "Save to " << filename << std::endl; } std::cout << "Finish... " << std::endl; } Tracking::Tracking(EllipsoidSLAM::System *pSys, EllipsoidSLAM::FrameDrawer *pFrameDrawer, EllipsoidSLAM::MapDrawer *pMapDrawer, EllipsoidSLAM::Map *pMap, const string &strSettingPath) :mpMap(pMap), mpSystem(pSys), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer) { cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); float fx = fSettings["Camera.fx"]; float fy = fSettings["Camera.fy"]; float cx = fSettings["Camera.cx"]; float cy = fSettings["Camera.cy"]; cv::Mat K = cv::Mat::eye(3,3,CV_32F); K.at(0,0) = fx; K.at(1,1) = fy; K.at(0,2) = cx; K.at(1,2) = cy; K.copyTo(mK); cv::Mat DistCoef(4,1,CV_32F); DistCoef.at(0) = fSettings["Camera.k1"]; DistCoef.at(1) = fSettings["Camera.k2"]; DistCoef.at(2) = fSettings["Camera.p1"]; DistCoef.at(3) = fSettings["Camera.p2"]; const float k3 = fSettings["Camera.k3"]; if(k3!=0) { DistCoef.resize(5); DistCoef.at(4) = k3; } DistCoef.copyTo(mDistCoef); mbf = fSettings["Camera.bf"]; float fps = fSettings["Camera.fps"]; if(fps==0) fps=30; int rows = fSettings["Camera.height"]; int cols = fSettings["Camera.width"]; mCalib << fx, 0, cx, 0, fy, cy, 0, 0, 1; mCamera.cx = cx; mCamera.cy = cy; mCamera.fx = fx; mCamera.fy = fy; mCamera.scale = fSettings["Camera.scale"]; mpInitializer = new Initializer(rows, cols); mpOptimizer = new Optimizer; mRows = rows; mCols = cols; mbDepthEllipsoidOpened = false; mbOpenOptimization = true; pDASolver = new DataAssociationSolver(mpMap); mpBuilder = new Builder(); mpBuilder->setCameraIntrinsic(mCalib, mCamera.scale); mCurrFrame = NULL; // output cout << endl << "Camera Parameters: " << endl; cout << "- fx: " << fx << endl; cout << "- fy: " << fy << endl; cout << "- cx: " << cx << endl; cout << "- cy: " << cy << endl; cout << "- k1: " << DistCoef.at(0) << endl; cout << "- k2: " << DistCoef.at(1) << endl; if(DistCoef.rows==5) cout << "- k3: " << DistCoef.at(4) << endl; cout << "- p1: " << DistCoef.at(2) << endl; cout << "- p2: " << DistCoef.at(3) << endl; cout << "- fps: " << fps << endl; cout << "- rows: " << rows << endl; cout << "- cols: " << cols << endl; cout << "- Scale: " << mCamera.scale << endl; // ********** DEBUG *********** matSymPlanes.resize(0, 5); } g2o::ellipsoid* Tracking::getObjectDataAssociation(const Eigen::VectorXd &pose, const Eigen::VectorXd &detection) { auto objects = mpMap->GetAllEllipsoids(); if( objects.size() > 0 ) return objects[0]; else return NULL; } bool Tracking::GrabPoseAndObjects(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB, bool withAssociation) { return GrabPoseAndObjects(0, pose, bboxMap, imDepth, imRGB, withAssociation); } bool Tracking::GrabPoseAndObjects(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB, bool withAssociation) { clock_t time_start = clock(); Frame *pF = new Frame(timestamp, pose, bboxMap, imDepth, imRGB); mvpFrames.push_back(pF); mCurrFrame = pF; clock_t time_init_frame_process = clock(); UpdateObjectObservation(mCurrFrame, withAssociation); // Store object observation in a specific data structure. clock_t time_UpdateObjectObservation = clock(); JudgeInitialization(); if(mbOpenOptimization){ ProcessCurrentFrame(withAssociation); } clock_t time_ProcessCurrentFrame = clock(); // Visualization ProcessVisualization(); clock_t time_Visualization = clock(); // // Output running time // cout << " - System Time: " << endl; // cout << " -- time_init_frame_process: " <<(double)(time_init_frame_process - time_start) / CLOCKS_PER_SEC << "s" << endl; // cout << " -- time_UpdateObjectObservation: " <<(double)(time_UpdateObjectObservation - time_init_frame_process) / CLOCKS_PER_SEC << "s" << endl; // cout << " -- time_ProcessCurrentFrame: " <<(double)(time_ProcessCurrentFrame - time_UpdateObjectObservation) / CLOCKS_PER_SEC << "s" << endl; // cout << " -- time_Visualization: " <<(double)(time_Visualization - time_ProcessCurrentFrame) / CLOCKS_PER_SEC << "s" << endl; // cout << " - [ total_frame: " <<(double)(time_Visualization - time_start) / CLOCKS_PER_SEC << "s ]" << endl; return true; } void Tracking::ProcessVisualization() { // Visualize frames with intervals if( isKeyFrameForVisualization() ) mpMap->addCameraStateToTrajectory(&mCurrFrame->cam_pose_Twc); mpMap->setCameraState(&mCurrFrame->cam_pose_Twc); // Render rgb images and depth images for visualization. cv::Mat imForShow = mpFrameDrawer->drawFrame(); cv::Mat imForShowDepth = mpFrameDrawer->drawDepthFrame(); } void Tracking::ProcessCurrentFrame(bool withAssociation){ clock_t time_start = clock(); double depth_range = Config::ReadValue("EllipsoidExtractor_DEPTH_RANGE"); // Only consider pointcloud within depth_range // Begin Global Optimization when there are objects in map. if(mpMap->GetAllEllipsoids().size()>0){ mpOptimizer->GlobalObjectGraphOptimization(mvpFrames, mpMap, mRows, mCols, mCalib, mmObjectObservations, true, withAssociation); RefreshObjectHistory(); } clock_t time_optimization = clock(); // [A visualization tool] When Builder is opened, it generates local pointcloud from depth and rgb images of current frame, // and global pointcloud by simply adding local pointcloud in world coordinate and then downsampling them for visualization. bool mbOpenBuilder = Config::Get("Visualization.Builder.Open") == 1; if(mbOpenBuilder) { if(!mCurrFrame->rgb_img.empty()){ // RGB images are needed. Eigen::VectorXd pose = mCurrFrame->cam_pose_Twc.toVector(); mpBuilder->processFrame(mCurrFrame->rgb_img, mCurrFrame->frame_img, pose, depth_range); mpBuilder->voxelFilter(0.01); // Down sample threshold; smaller the finer; depend on the hardware. PointCloudPCL::Ptr pCloudPCL = mpBuilder->getMap(); PointCloudPCL::Ptr pCurrentCloudPCL = mpBuilder->getCurrentMap(); auto pCloud = pclToQuadricPointCloudPtr(pCloudPCL); auto pCloudLocal = pclToQuadricPointCloudPtr(pCurrentCloudPCL); mpMap->AddPointCloudList("Builder.Global Points", pCloud); mpMap->AddPointCloudList("Builder.Local Points", pCloudLocal); } } clock_t time_builder = clock(); // // Output running time // cout << " -- ProcessCurrentFrame Time: " << endl; // cout << " --- time_optimization: " <<(double)(time_optimization - time_start) / CLOCKS_PER_SEC << "s" << endl; // cout << " --- time_builder: " <<(double)(time_builder - time_optimization) / CLOCKS_PER_SEC << "s" << endl; } void AddSegCloudsToQuadricStorage(std::vector::Ptr>& segClouds, EllipsoidSLAM::PointCloud* pSegCloud){ int cloud_num = segClouds.size(); srand(time(0)); for(int i=0;ipoints.size(); int r = rand()%155; int g = rand()%155; int b = rand()%155; for( int n=0;npoints[n].x; point.y = segClouds[i]->points[n].y; point.z = segClouds[i]->points[n].z; point.r = r; point.g = g; point.b = b; point.size = 2; pSegCloud->push_back(point); } } return; } // Process Ellipsoid Estimation for every boundingboxes in current frame. // Finally, store 3d Ellipsoids into the membter variable mpLocalObjects of pFrame. void Tracking::UpdateDepthEllipsoidEstimation(EllipsoidSLAM::Frame* pFrame, bool withAssociation) { Eigen::MatrixXd &obs_mat = pFrame->mmObservations; int rows = obs_mat.rows(); Eigen::VectorXd pose = pFrame->cam_pose_Twc.toVector(); EllipsoidSLAM::PointCloud* pCenterCloud = new EllipsoidSLAM::PointCloud; EllipsoidSLAM::PointCloud* pSegCloud = new EllipsoidSLAM::PointCloud; mpEllipsoidExtractor->ClearPointCloudList(); // clear point cloud visualization bool bPlaneNotClear = true; for(int i=0;i("Measurement.Border.Pixels"), Config::Get("Measurement.LengthLimit.Pixels")); g2o::ellipsoid* pE_extracted = NULL; // 2 conditions must meet to start ellipsoid extraction: // C1 : the bounding box is not on border bool c1 = !is_border; // C2 : the groundplane has been estimated successfully bool c2 = miGroundPlaneState == 2; // in condition 3, it will not start // C3 : under with association mode, and the association is invalid, no need to extract ellipsoids again. bool c3 = false; if( withAssociation ) { int instance = round(det_vec(7)); if ( instance < 0 ) c3 = true; // invalid instance } if( c1 && c2 && !c3 ){ g2o::ellipsoid e_extractByFitting_newSym = mpEllipsoidExtractor->EstimateLocalEllipsoid(pFrame->frame_img, measurement, label, pose, mCamera); if(bPlaneNotClear){ mpMap->clearPlanes(); if(miGroundPlaneState == 2) // if the groundplane has been estimated mpMap->addPlane(&mGroundPlane); bPlaneNotClear = false; } if( mpEllipsoidExtractor->GetResult() ) { g2o::ellipsoid *pE_extractByFitting = new g2o::ellipsoid(e_extractByFitting_newSym); // std::cout << "[Tracking.cpp] Get an Ellipsoid [fixSym]: " << e_extractByFitting_newSym.toMinimalVector().transpose() << std::endl; pE_extracted = pE_extractByFitting; // Store result to pE_extracted. // Visualize estimated ellipsoid g2o::ellipsoid* pObjByFitting = new g2o::ellipsoid(e_extractByFitting_newSym.transform_from(pFrame->cam_pose_Twc)); pObjByFitting->setColor(Vector3d(0.8,0.0,0.0), 1); // Set green color mpMap->addEllipsoidVisual(pObjByFitting); // Visualize symmetry plane SymmetryOutputData symOutputData = mpEllipsoidExtractor->GetSymmetryOutputData(); if(symOutputData.result){ Vector3d planeCenter = pObjByFitting->pose.translation(); if(symOutputData.symmetryType == 1){ g2o::plane* pSymPlane = new g2o::plane(symOutputData.planeVec, Vector3d(1.0,0,0.0)); pSymPlane->InitFinitePlane(planeCenter, 1); mpMap->addPlane(pSymPlane); } else if(symOutputData.symmetryType == 2) // dual reflection { g2o::plane* pSymPlane = new g2o::plane(symOutputData.planeVec, Vector3d(0.0,0.8,0.0)); pSymPlane->InitFinitePlane(planeCenter, 1); mpMap->addPlane(pSymPlane); g2o::plane* pSymPlane2 = new g2o::plane(symOutputData.planeVec2, Vector3d(0.0,0.8,0)); pSymPlane2->InitFinitePlane(planeCenter, 1); mpMap->addPlane(pSymPlane2); } // output ; Save every estimated symmetry parameter for every frame. VectorXd symPlaneVec; symPlaneVec.resize(5); // Param(4) ; error symPlaneVec << symOutputData.planeVec, symOutputData.prob; addVecToMatirx(matSymPlanes, symPlaneVec); } } // successful estimation. } pFrame->mpLocalObjects.push_back(pE_extracted); } } void Tracking::Update3DObservationDataAssociation(EllipsoidSLAM::Frame* pFrame, std::vector& associations, std::vector& KeyFrameChecks) { int num = associations.size(); if( mbDepthEllipsoidOpened ) { std::vector pLocalObjects = pFrame->mpLocalObjects; for( int i=0; impLocalObjects[i] = NULL; continue; } // Save 3D observations Observation3D* pOb3d = new Observation3D; pOb3d->pFrame = pFrame; pOb3d->pObj = pLocalObjects[i]; mmObjectObservations3D[instance].push_back(pOb3d); // Set instance to the ellipsoid according to the associations pLocalObjects[i]->miInstanceID = instance; } } return; } // Consider key observations for every object instances. // key observations: two valid observations for the same instance should have enough intervals( distance or angles between the two poses ). std::vector Tracking::checkKeyFrameForInstances(std::vector& associations) { double CONFIG_KEYFRAME_DIS; double CONFIG_KEYFRAME_ANGLE; if( Config::Get("Tracking.KeyFrameCheck.Close") == 1) { CONFIG_KEYFRAME_DIS = 0; CONFIG_KEYFRAME_ANGLE = 0; } else { CONFIG_KEYFRAME_DIS = 0.4; CONFIG_KEYFRAME_ANGLE = CV_PI/180.0*15; } int num =associations.size(); std::vector checks; checks.resize(num); fill(checks.begin(), checks.end(), false); for( int i=0;ipFrame->cam_pose_Twc; g2o::SE3Quat &pose_curr_wc = mCurrFrame->cam_pose_Twc; g2o::SE3Quat pose_diff = pose_curr_wc.inverse() * pose_last_wc; double dis = pose_diff.translation().norm(); Eigen::Quaterniond quat = pose_diff.rotation(); Eigen::AngleAxisd axis(quat); double angle = axis.angle(); if( dis > CONFIG_KEYFRAME_DIS || angle > CONFIG_KEYFRAME_ANGLE) checks[i] = true; else checks[i] = false; } } } return checks; } // for the mannual data association, // this function will directly return the results of [instance] in the object detection matrix // PS. one row of detection matrix is : id x1 y1 x2 y2 label rate instanceID std::vector Tracking::GetMannualAssociation(Eigen::MatrixXd &obsMat) { int num = obsMat.rows(); std::vector associations; associations.resize(num); for( int i=0; iClearEllipsoidsVisual(); // Clear the Visual Ellipsoids in the map // [1] Process 3d observations // 1.1 process groundplane estimation if(miGroundPlaneState == 1) // State 1: Groundplane estimation opened, and not done yet. ProcessGroundPlaneEstimation(); // 1.2 process single-frame ellipsoid estimation if( mbDepthEllipsoidOpened ) UpdateDepthEllipsoidEstimation(pFrame, withAssociation); // 1.3 process data association // if data association is given, directly pass on; // if not, the DASolver will solve it automatically by comparing the estimated ellipsoid with those ellipsoids in the map. // This function is not steady and still under Test. std::vector associations; if(withAssociation) { associations = GetMannualAssociation(pFrame->mmObservations); } else { assert( mbDepthEllipsoidOpened && "ATTENTION: ONLY 3D MODE IS SUPPORTED FOR AUTOMATIC DATA ASSOCIATION NOW." ); associations = pDASolver->Solve(pFrame, mbDepthEllipsoidOpened); } std::vector KeyFrameChecks = checkKeyFrameForInstances(associations); // Check whether they are key observations // Save data associations to the member variable of pFrame Update3DObservationDataAssociation(pFrame, associations, KeyFrameChecks); // [2] Process 2D observations Eigen::MatrixXd &obs_mat = pFrame->mmObservations; int rows = obs_mat.rows(); // load parameters int config_border_pixels = Config::Get("Measurement.Border.Pixels"); int config_lengthlimit_pixels = Config::Get("Measurement.LengthLimit.Pixels"); for(int i=0;ilabel = label; pOb->bbox = measurement; pOb->rate = det_vec(6); pOb->pFrame = pFrame; // Save 2d observations in mmObjectObservations, which will be loaded into the Optimization. if( mmObjectObservations.find(instance) != mmObjectObservations.end()) mmObjectObservations[instance].push_back(pOb); else{ Observations obs; obs.push_back(pOb); mmObjectObservations.insert(make_pair(instance, obs)); } } } // Initilize those instances that have enough observations. void Tracking::JudgeInitialization() { // 1. Consider 2d initialization int CONFIG_MINIMUM_INITIALIZATION_FRAME = Config::ReadValue("Tracking_MINIMUM_INITIALIZATION_FRAME"); std::vector objects = mpMap->GetAllEllipsoids(); std::set existInstances; // Save the existed instances in the map for(auto iter=objects.begin(); iter!=objects.end(); iter++) { existInstances.insert((*iter)->miInstanceID); } // outputObjectObservations(mmObjectObservations); for(auto iter=mmObjectObservations.begin(); iter!=mmObjectObservations.end(); iter++) { if( existInstances.find(iter->first) == existInstances.end() ) { // if the instance has not been initialized Observations obs = iter->second; int config_frame_num = obs.size(); if(config_frame_num < CONFIG_MINIMUM_INITIALIZATION_FRAME) continue; // if there are enough observations ellipsoid e = mpInitializer->initializeQuadric(obs, mCalib); // initialize quadrics by SVD e.miInstanceID = iter->first; if( mpInitializer->getInitializeResult() ) { ellipsoid* pBox = new ellipsoid(e); mpMap->addEllipsoid(pBox); // output cout << std::endl; cout << "-------- INITIALIZE NEW OBJECT BY SVD ---------" << endl; cout << "Label id: " << pBox->miLabel << endl; cout << "Instance id: " << pBox->miInstanceID << endl; cout << "Initialization box: " << pBox->vec_minimal.transpose() << endl; cout << std::endl; continue; } } } // 2. Consider 3d initilization // Refresh exsited instance id. objects = mpMap->GetAllEllipsoids(); existInstances.clear(); for(auto iter=objects.begin(); iter!=objects.end(); iter++) { existInstances.insert((*iter)->miInstanceID); } // Try initialization using single-frame ellipsoid estimation. if( mbDepthEllipsoidOpened ){ srand(time(0)); for(auto objPair : mmObjectObservations3D) { int instance = objPair.first; if( existInstances.find(instance) == existInstances.end() ) // if the instance has not been initialized yet { Observation3D* pOb3D = objPair.second.back(); g2o::ellipsoid* pE = pOb3D->pObj; Frame* pFrame = pOb3D->pFrame; g2o::ellipsoid* pBox = new ellipsoid(pE->transform_from(pFrame->cam_pose_Twc)); pBox->setColor(Vector3d(0, 0, 1)); mpMap->addEllipsoid(pBox); } } } } void Tracking::OpenDepthEllipsoid(){ mbDepthEllipsoidOpened = true; mpEllipsoidExtractor = new EllipsoidExtractor; // Open visualization during the estimation process mpEllipsoidExtractor->OpenVisualization(mpMap); // Open symmetry if(Config::Get("EllipsoidExtraction.Symmetry.Open") == 1) mpEllipsoidExtractor->OpenSymmetry(); std::cout << std::endl; cout << " * Open Single-Frame Ellipsoid Estimation. " << std::endl; std::cout << std::endl; } bool Tracking::isKeyFrameForVisualization() { static Frame* lastVisualizedFrame; if( mvpFrames.size() < 2 ) { lastVisualizedFrame = mCurrFrame; return true; } auto lastPose = lastVisualizedFrame->cam_pose_Twc; auto currPose = mCurrFrame->cam_pose_Twc; auto diffPose = lastPose.inverse() * currPose; Vector6d vec = diffPose.toXYZPRYVector(); if( (vec.head(3).norm() > 0.4) || (vec.tail(3).norm() > M_PI/180.0*15) ) // Visualization param for camera poses { lastVisualizedFrame = mCurrFrame; return true; } else return false; } void Tracking::OpenOptimization(){ mbOpenOptimization = true; std::cout << std::endl << "Optimization Opens." << std::endl << std::endl ; } void Tracking::CloseOptimization(){ mbOpenOptimization = false; std::cout << std::endl << "Optimization Closes." << std::endl << std::endl ; } void Tracking::OpenGroundPlaneEstimation(){ miGroundPlaneState = 1; pPlaneExtractor = new PlaneExtractor; PlaneExtractorParam param; param.fx = mK.at(0,0); param.fy = mK.at(1,1); param.cx = mK.at(0,2); param.cy = mK.at(1,2); param.scale = Config::Get("Camera.scale"); pPlaneExtractor->SetParam(param); std::cout << " * Open Groundplane Estimation" << std::endl; std::cout << std::endl; } void Tracking::CloseGroundPlaneEstimation(){ miGroundPlaneState = 0; std::cout << std::endl; std::cout << " * Close Groundplane Estimation* " << std::endl; std::cout << std::endl; } int Tracking::GetGroundPlaneEstimationState(){ return miGroundPlaneState; } void Tracking::ProcessGroundPlaneEstimation() { cv::Mat depth = mCurrFrame->frame_img; g2o::plane groundPlane; bool result = pPlaneExtractor->extractGroundPlane(depth, groundPlane); if( result ) { g2o::SE3Quat& Twc = mCurrFrame->cam_pose_Twc; groundPlane.transform(Twc); // transform to the world coordinate. mGroundPlane = groundPlane; miGroundPlaneState = 2; std::cout << " * Estimate Ground Plane Succeeds: " << mGroundPlane.param.transpose() << std::endl; mGroundPlane.color = Vector3d(0.0,0.8,0.0); mpMap->addPlane(&mGroundPlane); // Visualize the pointcloud during ground plane extraction PointCloudPCL::Ptr pCloud = pPlaneExtractor->GetCloudDense(); EllipsoidSLAM::PointCloud cloudQuadr = pclToQuadricPointCloud(pCloud); EllipsoidSLAM::PointCloud* pCloudQuadr = new EllipsoidSLAM::PointCloud(cloudQuadr); EllipsoidSLAM::PointCloud* pCloudQuadrGlobal = transformPointCloud(pCloudQuadr, &mCurrFrame->cam_pose_Twc); SetPointCloudProperty(pCloudQuadrGlobal, 0,255,100,2); mpMap->clearPointCloud(); auto vPotentialGroundplanePoints = pPlaneExtractor->GetPotentialGroundPlanePoints(); srand(time(0)); for( auto& cloud : vPotentialGroundplanePoints ) { PointCloudPCL::Ptr pCloudPCL(new PointCloudPCL(cloud)); EllipsoidSLAM::PointCloud cloudQuadri = pclToQuadricPointCloud(pCloudPCL); EllipsoidSLAM::PointCloud* pCloudGlobal = transformPointCloud(&cloudQuadri, &Twc); int r = rand()%155; int g = 155; int b = rand()%155; SetPointCloudProperty(pCloudGlobal, r, g, b, 4); mpMap->AddPointCloudList(string("pPlaneExtractor.PotentialGround"), pCloudGlobal, 1); } // Active the mannual check of groundplane estimation. int active_mannual_groundplane_check = Config::Get("Plane.MannualCheck.Open"); int key = -1; bool open_mannual_check = active_mannual_groundplane_check==1; bool result_mannual_check = false; if(open_mannual_check) { std::cout << "Estimate Groundplane Done." << std::endl; std::cout << "As Groundplane estimation is a simple implementation, please mannually check its correctness." << std::endl; std::cout << "Enter Key \'Y\' to confirm, and any other key to cancel this estimation: " << std::endl; key = getchar(); } result_mannual_check = (key == 'Y' || key == 'y'); if( !open_mannual_check || (open_mannual_check && result_mannual_check) ) { // Set groundplane to EllipsoidExtractor if( mbDepthEllipsoidOpened ){ std::cout << " * Add supporting plane to Ellipsoid Extractor." << std::endl; mpEllipsoidExtractor->SetSupportingPlane(&mGroundPlane); } // Set groundplane to Optimizer std::cout << " * Set groundplane param to optimizer. " << std::endl; mpOptimizer->SetGroundPlane(mGroundPlane.param); } else { std::cout << " * Cancel this Estimation. " << std::endl; miGroundPlaneState = 1; } } else { std::cout << " * Estimate Ground Plane Fails " << std::endl; } } bool Tracking::SavePointCloudMap(const string& path) { std::cout << "Save pointcloud Map to : " << path << std::endl; mpBuilder->saveMap(path); return true; } // This function saves the object history, which stores all the optimized object vector after every new observations. void Tracking::RefreshObjectHistory() { // Object Vector[11]: optimized_time[1] | Valid/inValid(1/0)[1] | minimal_vec[9] std::map pEllipsoidsMapWithInstance = mpMap->GetAllEllipsoidsMap(); for( auto pairInsPEllipsoid : pEllipsoidsMapWithInstance ) { int instance = pairInsPEllipsoid.first; if( mmObjectHistory.find(instance) == mmObjectHistory.end() ) // when the instance has no record in the history { MatrixXd obHistory; obHistory.resize(0, 11); mmObjectHistory.insert(make_pair(instance, obHistory)); } // Add new history VectorXd hisVec; hisVec.resize(11); assert(mmObjectObservations.find(instance)!=mmObjectObservations.end() && "How does the ellipsoid get into the map without observations?"); int currentObs = mmObjectObservations[instance].size(); hisVec[0] = currentObs; // observation num. hisVec[1] = 1; Vector9d vec = pairInsPEllipsoid.second->toMinimalVector(); hisVec.tail<9>() = vec; // Get the observation num of the last history, add new row if the current observation num is newer. MatrixXd &obHisMat = mmObjectHistory[instance]; if( obHisMat.rows() == 0) { // Save to the matrix addVecToMatirx(obHisMat, hisVec); } else { int lastObNum = round(obHisMat.row(obHisMat.rows()-1)[0]); if( lastObNum == currentObs ) // Compare with last observation { // Cover it and remain the same obHisMat.row(obHisMat.rows()-1) = hisVec; } else addVecToMatirx(obHisMat, hisVec); // Add a new row. } } } // Save the object history into a text file. void Tracking::SaveObjectHistory(const string& path) { /* * TotalInstanceNum * instanceID1 historyNum * 0 Valid(1/0) minimalVec * 1 Valid(1/0) minimalVec * 2 Valid(1/0) minimalVec * ... * instanceID2 historyNum * 0 Valid(1/0) minimalVec * 1 Valid(1/0) minimalVec * ... * */ ofstream out(path.c_str()); int total_num = mmObjectHistory.size(); out << total_num << std::endl; for( auto obPair : mmObjectHistory ) { int instance = obPair.first; MatrixXd &hisMat = obPair.second; int hisNum = hisMat.rows(); out << instance << " " << hisNum << std::endl; for( int n=0;n #include namespace EllipsoidSLAM { Viewer::Viewer(System *pSystem, const string &strSettingPath, EllipsoidSLAM::MapDrawer *pMapDrawer){ cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); mViewpointX = fSettings["Viewer.ViewpointX"]; mViewpointY = fSettings["Viewer.ViewpointY"]; mViewpointZ = fSettings["Viewer.ViewpointZ"]; mViewpointF = fSettings["Viewer.ViewpointF"]; mbFinishRequested=false; mpSystem = pSystem; mpMapDrawer = pMapDrawer; miRows = fSettings["Camera.height"]; miCols = fSettings["Camera.width"]; mvMenuStruct.clear(); } Viewer::Viewer(const string &strSettingPath, MapDrawer* pMapDrawer):mpMapDrawer(pMapDrawer) { cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); mViewpointX = fSettings["Viewer.ViewpointX"]; mViewpointY = fSettings["Viewer.ViewpointY"]; mViewpointZ = fSettings["Viewer.ViewpointZ"]; mViewpointF = fSettings["Viewer.ViewpointF"]; mbFinishRequested=false; miRows = fSettings["Camera.height"]; miCols = fSettings["Camera.width"]; mvMenuStruct.clear(); } void Viewer::SetFrameDrawer(FrameDrawer* pFrameDrawer) { mpFrameDrawer = pFrameDrawer; } void Viewer::run() { mbFinished = false; pangolin::CreateWindowAndBind("EllipsoidSLAM: Map Viewer", 1024, 768); // 3D Mouse handler requires depth testing to be enabled glEnable(GL_DEPTH_TEST); // Issue specific OpenGl we might need glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175)); pangolin::Var menuFollowCamera("menu.Follow Camera", true, true); pangolin::Var menuShowKeyFrames("menu.Show KeyFrames", true, true); pangolin::Var menuShowEllipsoids("menu.Show Ellipsoids", true, true); pangolin::Var menuShowPlanes("menu.Show Planes", true, true); pangolin::Var menuShowCuboids("menu.Show Cuboids", false, true); // Define Camera Render Object (for view / scene browsing) pangolin::OpenGlRenderState s_cam( pangolin::ProjectionMatrix(1024, 768, mViewpointF, mViewpointF, 512, 389, 0.1, 1000), pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0) ); // Add named OpenGL viewport to window and provide 3D Handler pangolin::View &d_cam = pangolin::Display("cam") .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -float(miCols) / float(miRows)) .SetHandler(new pangolin::Handler3D(s_cam)); // Add view for images pangolin::View& rgb_image = pangolin::Display("rgb") .SetBounds(0,0.3,0.2,0.5,float(miCols) / float(miRows)) .SetLock(pangolin::LockLeft, pangolin::LockBottom); pangolin::View& depth_image = pangolin::Display("depth") .SetBounds(0,0.3,0.5,0.8,float(miCols) / float(miRows)) .SetLock(pangolin::LockLeft, pangolin::LockBottom); pangolin::OpenGlMatrix Twc; Twc.SetIdentity(); bool bFollow = true; pangolin::GlTexture imageTexture(miCols,miRows,GL_RGB,false,0,GL_BGR,GL_UNSIGNED_BYTE); while (1) { RefreshMenu(); // Deal with dynamic menu bars glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc); // get current camera pose if(menuFollowCamera && bFollow) // Follow camera { s_cam.Follow(Twc); } else if(menuFollowCamera && !bFollow) { s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); s_cam.Follow(Twc); bFollow = true; } else if(!menuFollowCamera && bFollow) { bFollow = false; } d_cam.Activate(s_cam); glClearColor(1.0f,1.0f,1.0f,1.0f); pangolin::glDrawAxis(3); // draw world coordintates if(menuShowKeyFrames) { mpMapDrawer->drawCameraState(); mpMapDrawer->drawTrajectory(); } // draw external cubes of ellipsoids if(menuShowCuboids) mpMapDrawer->drawObjects(); // draw ellipsoids if(menuShowEllipsoids) mpMapDrawer->drawEllipsoids(); // draw planes, including grounplanes and symmetry planes if(menuShowPlanes) mpMapDrawer->drawPlanes(); mpMapDrawer->drawPoints(); // draw point clouds // draw pointclouds with names RefreshPointCloudOptions(); mpMapDrawer->drawPointCloudWithOptions(mmPointCloudOptionMap); // mpMapDrawer->drawPointCloudLists(); // draw images : rgb cv::Mat rgb = mpFrameDrawer->getCurrentFrameImage(); if(!rgb.empty()) { imageTexture.Upload(rgb.data,GL_BGR,GL_UNSIGNED_BYTE); //display the image rgb_image.Activate(); glColor3f(1.0,1.0,1.0); imageTexture.RenderToViewportFlipY(); } // draw images : depth cv::Mat depth = mpFrameDrawer->getCurrentDepthFrameImage(); if(!depth.empty()) { imageTexture.Upload(depth.data,GL_BGR,GL_UNSIGNED_BYTE); //display the image depth_image.Activate(); glColor3f(1.0,1.0,1.0); imageTexture.RenderToViewportFlipY(); } pangolin::FinishFrame(); if (CheckFinish()) break; } SetFinish(); } bool Viewer::CheckFinish() { unique_lock lock(mMutexFinish); return mbFinishRequested; } void Viewer::SetFinish() { unique_lock lock(mMutexFinish); mbFinished = true; } void Viewer::RequestFinish() { unique_lock lock(mMutexFinish); mbFinishRequested = true; } bool Viewer::isFinished() { unique_lock lock(mMutexFinish); return mbFinished; } int Viewer::addDoubleMenu(string name, double min, double max, double def){ unique_lock lock(mMutexFinish); MenuStruct menu; menu.min = min; menu.max = max; menu.def = def; menu.name = name; mvMenuStruct.push_back(menu); return mvMenuStruct.size()-1; } bool Viewer::getValueDoubleMenu(int id, double &value){ unique_lock lock(mMutexFinish); if( 0 <= id && 0< mvDoubleMenus.size()) { value = mvDoubleMenus[id]->Get(); return true; } else { return false; } } void Viewer::RefreshPointCloudOptions() { // generate options from mmPointCloudOptionMenus, pointclouds with names will only be drawn when their options are activated. std::map options; for( auto pair : mmPointCloudOptionMenus) options.insert(make_pair(pair.first, pair.second->Get())); mmPointCloudOptionMap.clear(); mmPointCloudOptionMap = options; } void Viewer::RefreshMenu(){ unique_lock lock(mMutexFinish); // Generate menu bar for every pointcloud in pointcloud list. auto pointLists = mpSystem->getMap()->GetPointCloudList(); // Iterate over the menu and delete the menu if the corresponding clouds are no longer available for( auto menuPair = mmPointCloudOptionMenus.begin(); menuPair!=mmPointCloudOptionMenus.end(); ) { if(pointLists.find(menuPair->first) == pointLists.end()) { delete menuPair->second; // destroy the dynamic menu menuPair = mmPointCloudOptionMenus.erase(menuPair); } else menuPair++; } // Iterate over the cloud lists to add new menu. for( auto cloudPair: pointLists ) { if(mmPointCloudOptionMenus.find(cloudPair.first) == mmPointCloudOptionMenus.end()) { pangolin::Var* pMenu = new pangolin::Var(string("menu.") + cloudPair.first, true, true); mmPointCloudOptionMenus.insert(make_pair(cloudPair.first, pMenu)); } } // refresh double bars int doubleBarNum = mvDoubleMenus.size(); int structNum = mvMenuStruct.size(); if( structNum > 0 && structNum > doubleBarNum ) { for(int i = doubleBarNum; i < structNum; i++) { pangolin::Var* pMenu = new pangolin::Var(string("menu.")+mvMenuStruct[i].name, mvMenuStruct[i].def, mvMenuStruct[i].min, mvMenuStruct[i].max); mvDoubleMenus.push_back(pMenu); } } } } ================================================ FILE: src/dense_builder/CMakeLists.txt ================================================ add_library(dense_builder SHARED builder.cpp ) target_link_libraries(dense_builder ${OpenCV_LIBS} ${PCL_LIBRARIES} utils ) ================================================ FILE: src/dense_builder/builder.cpp ================================================ #include "builder.h" #include "include/utils/matrix_utils.h" #include #include #include // for make_shared #include // //PCL #include #include using namespace Eigen; using namespace std; void Builder::processFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose, double depth_thresh){ if(!mbInitialized) { std::cerr << "Please initialize the builder first." << std::endl; return; } PointCloudPCL::Ptr pLocalPointCloud = image2PointCloud(rgb, depth, depth_thresh); PointCloudPCL::Ptr pGlobalPointCloud = transformToWolrd(pLocalPointCloud, pose); // pose: Twc mpCurrentMap = pGlobalPointCloud; // add to map addPointCloudToMap(pGlobalPointCloud); return; } void Builder::setCameraIntrinsic(Matrix3d &calibMat, double scale){ mmCalib = calibMat; mdScale = scale; mbInitialized = true; } Builder::Builder():mbInitialized(false), mpMap(new PointCloudPCL){ } PointCloudPCL::Ptr Builder::image2PointCloud( cv::Mat& rgb, cv::Mat& depth, double depth_thresh) { PointCloudPCL::Ptr temp_cloud ( new PointCloudPCL ); Camera camera; getCameraParam(mmCalib, camera); for (int m = 0; m < depth.rows; m++) for (int n=0; n < depth.cols; n++) { ushort * ptd = depth.ptr(m); ushort d = ptd[n]; if (d == 0) continue; PointT p; p.z = double(d) / mdScale; if( p.z < 0.5 || p.z > depth_thresh ) continue; p.x = (n - camera.cx) * p.z / camera.fx; p.y = (m - camera.cy) * p.z / camera.fy; // B G R p.b = rgb.ptr(m)[n*3]; p.g = rgb.ptr(m)[n*3+1]; p.r = rgb.ptr(m)[n*3+2]; temp_cloud->points.push_back( p ); } temp_cloud->height = 1; temp_cloud->width = temp_cloud->points.size(); temp_cloud->is_dense = true; return temp_cloud; } void Builder::getCameraParam(Matrix3d &calib, Camera &cam){ // kalib matrix: // fx s cx // 0 fy cy // 0 0 1 cam.fx = calib(0,0); cam.fy = calib(1,1); cam.cx = calib(0,2); cam.cy = calib(1,2); } PointCloudPCL::Ptr Builder::getMap() { return mpMap; } PointCloudPCL::Ptr Builder::getCurrentMap() { return mpCurrentMap; } PointCloudPCL::Ptr Builder::transformToWolrd( PointCloudPCL::Ptr& pPointCloud, VectorXd &pose){ // pose: Tcw // tx ty tz (3 floats) give the position of // the optical center of the color camera // with respect to the world origin as defined by the motion capture system. PointCloudPCL::Ptr transformed_cloud(new PointCloudPCL); Matrix4d transform = getTransformFromVector(pose); std::cout << "transform " << transform << std::endl; pcl::transformPointCloud (*pPointCloud, *transformed_cloud, transform); return transformed_cloud; } void Builder::addPointCloudToMap(PointCloudPCL::Ptr pPointCloud){ *mpMap = *mpMap + *pPointCloud; } void Builder::saveMap(const string& path){ if(mpMap->points.size() < 1) return; pcl::io::savePCDFileASCII(path, *mpMap); std::cerr << "Saved " << mpMap->points.size () << " data points to " << path << std::endl; } void Builder::voxelFilter(double grid_size){ // Voxel grid downsample static pcl::VoxelGrid voxel; double gridsize = grid_size; voxel.setLeafSize( gridsize, gridsize, gridsize ); voxel.setInputCloud( mpMap ); PointCloudPCL::Ptr tmp( new PointCloudPCL() ); voxel.filter( *tmp ); pcl::copyPointCloud(*tmp, *mpMap); } ================================================ FILE: src/dense_builder/builder.h ================================================ // Builder generates dense point cloud from RGB-D data and groundtruth camera poses. #include #include //PCL #include #include #include typedef pcl::PointXYZRGB PointT; typedef pcl::PointCloud PointCloudPCL; #include using namespace Eigen; using namespace std; struct Camera { double fx; double fy; double cx; double cy; }; class Builder { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; Builder(); void setCameraIntrinsic(Matrix3d &calibMat, double scale); void processFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose, double depth_thresh = 1000); PointCloudPCL::Ptr getMap(); PointCloudPCL::Ptr getCurrentMap(); void saveMap(const string& path); void voxelFilter(double grid_size); private: void getCameraParam(Matrix3d &calib, Camera &cam); PointCloudPCL::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, double depth_thresh); PointCloudPCL::Ptr transformToWolrd( PointCloudPCL::Ptr& pPointCloud, VectorXd &pose); void addPointCloudToMap(PointCloudPCL::Ptr pPointCloud); private: Matrix3d mmCalib; // calibration mat double mdScale; // scale for depth bool mbInitialized; PointCloudPCL::Ptr mpMap; PointCloudPCL::Ptr mpCurrentMap; }; ================================================ FILE: src/pca/CMakeLists.txt ================================================ # set(CMAKE_BUILD_TYPE Debug) add_library(EllipsoidExtractor SHARED EllipsoidExtractor.cpp ) target_link_libraries(EllipsoidExtractor ${OpenCV_LIBS} symmetry utils ) ================================================ FILE: src/pca/EllipsoidExtractor.cpp ================================================ #include "EllipsoidExtractor.h" #include // For Euclidean Cluster Extraction #include #include #include #include #include #include #include #include #include #include #include // g2o #include "Thirdparty/g2o/g2o/core/block_solver.h" #include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h" #include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h" #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" #include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h" #include "Thirdparty/g2o/g2o/core/robust_kernel.h" #include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h" #include #include namespace EllipsoidSLAM { bool compare_cloud_with_z(EllipsoidSLAM::PointXYZRGB &p1, EllipsoidSLAM::PointXYZRGB &p2) { return p1.z < p2.z; } EllipsoidExtractor::EllipsoidExtractor() { mResult = false; mbSetPlane = false; mDebugCenterCloud = NULL; mbOpenVisualization = false; miExtractCount = 0; mbOpenSymmetry = false; } void EllipsoidExtractor::LoadSymmetryPrior() { // the semantic label comes from coco names, which is suitable for YOLO. // ************************************* // Object-type LabelID SymmetryType // potted plant | 58 | None // bed | 59 | Reflection // tvmonitor | 62 | Reflection // sofa | 57 | Reflection // keyboard | 66 | Dual Reflection // laptop | 63 | Reflection // mouse | 64 | Reflection // cup | 41 | Reflection // suitcase| 28 | Dual Reflection std::map labelSymmetry; labelSymmetry.insert(make_pair(58,0)); labelSymmetry.insert(make_pair(59,1)); labelSymmetry.insert(make_pair(62,1)); labelSymmetry.insert(make_pair(57,1)); labelSymmetry.insert(make_pair(66,1)); labelSymmetry.insert(make_pair(63,1)); labelSymmetry.insert(make_pair(64,1)); labelSymmetry.insert(make_pair(41,1)); labelSymmetry.insert(make_pair(28,2)); mmLabelSymmetry = labelSymmetry; // load to the config } bool EllipsoidExtractor::GetResult() { return mResult; } pcl::PointCloud::Ptr EllipsoidExtractor::ExtractPointCloud(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera) { clock_t time_1_start = clock(); assert( mbSetPlane && "Please set the supporting plane first."); double depth_range = Config::ReadValue("EllipsoidExtractor_DEPTH_RANGE", 6); PointCloud* pPoints_local = new PointCloud(getPointCloudInRect(depth, bbox, camera, depth_range)); // downsample points with small grid PointCloud* pPoints_local_downsample = new PointCloud; DownSamplePointCloudOnly(*pPoints_local, *pPoints_local_downsample, 0.01); // transform to the world coordinate. g2o::SE3Quat campose_wc; campose_wc.fromVector(pose); PointCloud* pPoints_global = transformPointCloud(pPoints_local_downsample, &campose_wc); mpPointsDebug = pPoints_global; clock_t time_2_getPointsDownsampleTransToWorld = clock(); PointCloud* pPoints_planeFiltered = ApplySupportingPlaneFilter(pPoints_global); clock_t time_3_SupportingPlaneFilter = clock(); // VisualizePointCloud("OriginObjectPoints", pPoints_planeFiltered, Vector3d(1.0,0,0), 2);; clock_t time_4_VisualizePointCloud = clock(); if( pPoints_planeFiltered->size() < 1 ) { std::cout << "No point left." << std::endl; std::cout << "pPoints_global points: " << pPoints_global->size() << std::endl; std::cout << "No enough point cloud after Filtering. Num: " << pPoints_planeFiltered->size() << std::endl; miSystemState = 4; return NULL; } PointCloud* pPoints_sampled = pPoints_planeFiltered; if( pPoints_sampled->size() < 1 ) { std::cout << "No enough point cloud after sampling. Num: " << pPoints_sampled->size() << std::endl; miSystemState = 3; return NULL; } Vector3d center; bool bCenter = GetCenter(depth, bbox, pose, camera, center); if(!bCenter) { miSystemState = 1; std::cout << "Can't Find Center. Bbox: " << bbox.transpose() << std::endl; return NULL; } clock_t time_5_GetCenter = clock(); mDebugCenter = center; PointCloud* pPointsEuFiltered = ApplyEuclideanFilter(pPoints_sampled, center); if( miEuclideanFilterState > 0 ) { miSystemState = 2; // fail to filter return NULL; } clock_t time_6_ApplyEuclideanFilter = clock(); // we have gotten the object points in the world coordinate pcl::PointCloud::Ptr clear_cloud_ptr = QuadricPointCloudToPclXYZ(*pPointsEuFiltered); mpPoints = pPointsEuFiltered; VisualizePointCloud("Object Points", mpPoints, Vector3d(0.4,0,1.0), 2);; clock_t time_7_VisualizePointCloud = clock(); // output: time efficiency // cout << "****** System Time [ExtractPoints.cpp] ******" << endl ; // cout << "time_2_getPointsDownsampleTransToWorld: " <<(double)(time_2_getPointsDownsampleTransToWorld - time_1_start) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_3_SupportingPlaneFilter: " <<(double)(time_3_SupportingPlaneFilter - time_2_getPointsDownsampleTransToWorld) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_4_VisualizePointCloud: " <<(double)(time_4_VisualizePointCloud - time_3_SupportingPlaneFilter) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_5_GetCenter: " <<(double)(time_5_GetCenter - time_4_VisualizePointCloud) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_6_ApplyEuclideanFilter: " <<(double)(time_6_ApplyEuclideanFilter - time_5_GetCenter) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_7_VisualizePointCloud: " <<(double)(time_7_VisualizePointCloud - time_6_ApplyEuclideanFilter) / CLOCKS_PER_SEC << "s" << endl; return clear_cloud_ptr; } PCAResult EllipsoidExtractor::ProcessPCA(pcl::PointCloud::Ptr &pCloudPCL) { pcl::PointCloud::Ptr cloud = pCloudPCL; pcl::PointCloud::Ptr cloud_normal(new pcl::PointCloud()); Eigen::Vector4d pcaCentroid; pcl::compute3DCentroid(*cloud, pcaCentroid); Eigen::Matrix3d covariance; pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance); Eigen::SelfAdjointEigenSolver eigen_solver(covariance, Eigen::ComputeEigenvectors); Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors(); Eigen::Vector3d eigenValuesPCA = eigen_solver.eigenvalues(); // center point PointType c; c.x = pcaCentroid(0); c.y = pcaCentroid(1); c.z = pcaCentroid(2); PCAResult output; output.result = true; output.center = Eigen::Vector3d(c.x,c.y,c.z); output.rotMat = eigenVectorsPCA; // Rotation matrix(in world coordinate) output.covariance = Eigen::Vector3d(eigenValuesPCA(0),eigenValuesPCA(1),eigenValuesPCA(2)); // covariance used for scale estimation of ellipsoid return output; } void EllipsoidExtractor::ApplyGravityPrior(PCAResult &data) { assert( mbSetPlane && "Please set the supporting plane first."); Eigen::Matrix3d matRot = calibRotMatAccordingToGroundPlane( data.rotMat, mpPlane->param.head(3)); data.rotMat = matRot; return; } void EllipsoidExtractor::AlignZAxisToGravity(PCAResult &data){ // the goal for aligning Z axis : // make sure the z axis of the ellipsoid is along the direction of the groundplane normal, // so that when calling computeError() in the edge, we could rotate 90 deg along Z axis three times and use the minimum angle difference as the error // First, get the z axis double max_cos_theta_abs = 0; bool max_flag_pos; int max_id = -1; Vector3d z_axis; if( mbSetPlane ) z_axis = mpPlane->param.head(3).normalized(); else z_axis = Vector3d(0,0,1); // find which axis in rotMat has minimum angle difference with z_axis for( int i=0;i<3;i++ ) { Vector3d axis = data.rotMat.col(i); double cos_theta = axis.dot(z_axis); // a*b = |a||b|cos(theta) bool flag_pos = cos_theta > 0; double cos_theta_abs = std::abs(cos_theta); if( cos_theta_abs > max_cos_theta_abs ) { max_cos_theta_abs = cos_theta_abs; max_flag_pos = flag_pos; max_id = i; } } assert( max_id >= 0 && "Must find a biggest one." ); // swap the rotMat to get the correct z axis Matrix3d rotMatSwap; Vector3d covarianceSwap; Vector3d z_axis_vec; // invert the direction if(max_flag_pos) z_axis_vec = data.rotMat.col(max_id); else z_axis_vec = -data.rotMat.col(max_id); rotMatSwap.col(2) = z_axis_vec; covarianceSwap(2) = data.covariance[max_id]; // get other two axes. int x_axis_id = (max_id+1)%3; // next axis. rotMatSwap.col(0) = data.rotMat.col(x_axis_id); covarianceSwap(0) = data.covariance[x_axis_id]; int y_axis_id = (max_id+2)%3; rotMatSwap.col(1) = rotMatSwap.col(2).cross(rotMatSwap.col(0)); covarianceSwap(1) = data.covariance[y_axis_id]; data.rotMat = rotMatSwap; data.covariance = covarianceSwap; return ; } // This function is a simple implementation for constructing an ellipsoid from rgb-d data, which differs from the paper. g2o::ellipsoid EllipsoidExtractor::ConstructEllipsoid(PCAResult &data) { g2o::ellipsoid e; // 1) use the calibrated rotation matrix from PCA as the orientation of the ellipsoid. // the z axis has been adjusted to be along the normal of the groundplane. Eigen::Quaterniond quat(data.rotMat); // 2) the scale will be decided using an approximation: the max x,y,z value of the object points. Eigen::Vector3d scale = data.scale; // x y z qx qy qz qw a b c Vector10d ellip_vec; ellip_vec << data.center[0], data.center[1], data.center[2], (quat.x()), (quat.y()), (quat.z()), (quat.w()), scale[0], scale[1], scale[2]; e.fromVector(ellip_vec); return e; } g2o::ellipsoid EllipsoidExtractor::EstimateLocalEllipsoid(cv::Mat& depth, Eigen::Vector4d& bbox, int label, Eigen::VectorXd &pose, camera_intrinsic& camera){ miExtractCount ++; // the total extraction times for naming point clouds. g2o::ellipsoid e; miSystemState = 0; // reset the state mSymmetryOutputData.result = false; // reset mResult = false; clock_t time_start = clock(); // 1. Get the object points after supporting plane filter and euclidean filter in the world coordinate pcl::PointCloud::Ptr pCloudPCL = ExtractPointCloud(depth,bbox,pose,camera); if(miSystemState > 0 ) return e; clock_t time_1_ExtractPointCloud = clock(); // process the principle components analysis to get the rotation matrix, scale, and center point of the point cloud PCAResult data = ProcessPCA(pCloudPCL); // adjust the rotation matrix to be right-handed AdjustChirality(data); // adjust the x,y,z order AlignZAxisToGravity(data); // align z axis with the normal of the supporting plane ApplyGravityPrior(data); Vector3d center = data.center; // center point of the object points from PCA. EllipsoidSLAM::PointCloud* pObjectClearCloud = pclXYZToQuadricPointCloudPtr(pCloudPCL); // EllipsoidSLAM::PointCloud* pObjectCloud = pObjectClearCloud; // world coordinate // downsample to estimate symmetry. double grid_size_for_symmetry = Config::ReadValue("EllipsoidExtraction.Symmetry.GridSize"); EllipsoidSLAM::PointCloud* pObjectCloud = new EllipsoidSLAM::PointCloud; DownSamplePointCloudOnly(*pObjectClearCloud, *pObjectCloud, grid_size_for_symmetry); VisualizePointCloud("Points For Sym", pObjectCloud, Vector3d(0.5,0.5,0.0), 6); // construct a normalized rotation matrix using the normal of the supporting plane and the normal of the symmetry plane. Vector3d rot_vec_z; if( mbSetPlane ) rot_vec_z = mpPlane->param.head(3).normalized(); else rot_vec_z = Vector3d(0,0,1); Vector3d rot_vec_x = data.rotMat.col(0).normalized(); // or use the origin PCA result. Vector3d rot_vec_y = rot_vec_z.cross(rot_vec_x); Matrix3d rotMat_wo; // object in world rotMat_wo.col(0) = rot_vec_x; rotMat_wo.col(1) = rot_vec_y; rotMat_wo.col(2) = rot_vec_z; // transform to the normalized coordinate g2o::SE3Quat* pSE3Two = new g2o::SE3Quat; Eigen::Quaterniond quat_wo(rotMat_wo); pSE3Two->setRotation(quat_wo); pSE3Two->setTranslation(center); // it is the center of the old object points; it's better to use the center of the new complete points g2o::SE3Quat SE3Tow(pSE3Two->inverse()); EllipsoidSLAM::PointCloud* pObjectCloudNormalized = transformPointCloud(pObjectCloud, &SE3Tow); // normalized coordinate // VisualizePointCloud("normalizedPoints", pObjectCloudNormalized, Vector3d(0,0.4,0), 2); clock_t time_2_partPCA = clock(); // begin symmetry plane estimation. SymmetryOutputData dataSymOutput; dataSymOutput.result = false; bool runSymmetry = false; if( mbOpenSymmetry ) { // 1. Check symmetry type bool hasSymmetry = (mmLabelSymmetry.find(label) != mmLabelSymmetry.end()); int symmetryType = -1; if(hasSymmetry) { symmetryType = mmLabelSymmetry[label]; if(symmetryType > 0) runSymmetry = true; // have valid symmetry type } if(runSymmetry) { // 3. initialize the symmetry solver Symmetry ext; // Get a depth map whose values store the straight distances between the 3d points and camera center cv::Mat projDepth = ext.getProjDepthMat(depth, camera); g2o::SE3Quat campose_wc; campose_wc.fromVector(pose.tail(7)); g2o::SE3Quat campose_oc = SE3Tow * campose_wc; Eigen::VectorXd poseNormalized = campose_oc.toVector(); SymmetrySolverData result = ext.estimateSymmetry(bbox, pObjectCloudNormalized, poseNormalized, projDepth, camera, symmetryType); // store the output dataSymOutput.pCloud = pObjectCloudNormalized; dataSymOutput.prob = result.prob; dataSymOutput.result = result.result; dataSymOutput.center = center; dataSymOutput.symmetryType = symmetryType; // store the sym plane g2o::plane symPlaneWorld(*result.pPlane); symPlaneWorld.transform(*pSE3Two); dataSymOutput.planeVec = symPlaneWorld.param; // store the symmetry plane in the world coordinate if(symmetryType == 2) // another symmetry plane for dual reflection { g2o::plane symPlaneWorld2(*result.pPlane2); symPlaneWorld2.transform(*pSE3Two); dataSymOutput.planeVec2 = symPlaneWorld2.param; } // complete the pointcloud using the symmetry plane if the estimation is successful if(dataSymOutput.result) { PointCloud* pSymCloudNormalized = SymmetrySolver::GetSymmetryPointCloud(pObjectCloudNormalized, *result.pPlane); // the plane in result is in normalized coordinate. if( symmetryType == 2) { PointCloud* pSymCloudNormalized2_1 = SymmetrySolver::GetSymmetryPointCloud(pObjectCloudNormalized, *result.pPlane2); PointCloud* pSymCloudNormalized2_2 = SymmetrySolver::GetSymmetryPointCloud(pSymCloudNormalized, *result.pPlane2); CombinePointCloud(pSymCloudNormalized, pSymCloudNormalized2_1); CombinePointCloud(pSymCloudNormalized, pSymCloudNormalized2_2); } // add the mirrored points to the object points int sym_num = pSymCloudNormalized->size(); for( int i=0;ipush_back((*pSymCloudNormalized)[i]); // here, mirrwoed points have changed the center and rotation of the object points, // so we need a new transformation to move back the object points to the normalized coordinate // get the new center of the objects with mirrored points Vector3d centerCombined = GetPointcloudCenter(pObjectCloudNormalized); // to world coordinate Vector3d centerCombinedWorld = TransformPoint(centerCombined, pSE3Two->to_homogeneous_matrix()); dataSymOutput.center = centerCombinedWorld; g2o::SE3Quat Tom; // mirror objects in normalized coordinate Tom.setTranslation(centerCombined); Matrix3d rotMat_om; // object in world rotMat_om.col(0) = result.pPlane->param.head(3); // x: the normal of symmetry plane 1 rotMat_om.col(0) = rotMat_om.col(0)/(rotMat_om.col(0).norm()); // normalize rotMat_om.col(2) = Vector3d(0,0,1); // z: the normal of the groundplane, which is Z(0,0,1) rotMat_om.col(1) = rotMat_om.col(2).cross(rotMat_om.col(0)); // y: z cross x Eigen::Quaterniond quat_om(rotMat_om); Tom.setRotation(quat_om); // transform points g2o::SE3Quat Tmo = Tom.inverse(); transformPointCloudSelf(pObjectCloudNormalized, &Tmo); // po->pm // change T g2o::SE3Quat* pSE3Twm = new g2o::SE3Quat(); (*pSE3Twm) = (*pSE3Two) * Tom; pSE3Two = pSE3Twm; } mSymmetryOutputData = dataSymOutput; // VisualizePointCloud("WithSymNormalized", pObjectCloudNormalized, Vector3d(0,0,0.4), 2); // transform to the world EllipsoidSLAM::PointCloud* pObjectCloudWithSymWorld = EllipsoidSLAM::transformPointCloud(pObjectCloudNormalized, pSE3Two); VisualizePointCloud("Mirrored Points", pObjectCloudWithSymWorld, Vector3d(0,1.0,0.2), 8); } // end of symmetry type } // end of the symmetry. clock_t time_3_symmetryEstimation = clock(); // Estimate an ellipsoid from the complete object points // calculate the covariance along three main axes PCAResult dataCenterPCA = ProcessPCANormalized(pObjectCloudNormalized); g2o::ellipsoid e_zero_normalized = ConstructEllipsoid(dataCenterPCA); // transform back to the world coordinate g2o::ellipsoid e_global_normalized = e_zero_normalized.transform_from(*pSE3Two); // transform to the local coordinate. g2o::SE3Quat campose_wc; campose_wc.fromVector(pose); g2o::ellipsoid e_local_normalized = e_global_normalized.transform_from(campose_wc.inverse()); clock_t time_5_zeroPCA = clock(); // output the main running time // cout << "****** System Time [EllipsoidExtractor.cpp] ******" << endl ; // cout << "time_ExtractPointCloud: " <<(double)(time_1_ExtractPointCloud - time_start) / CLOCKS_PER_SEC << "s" << endl; // // cout << "time_partPCA: " <<(double)(time_2_partPCA - time_1_ExtractPointCloud) / CLOCKS_PER_SEC << "s" << endl; // too small // cout << "time_symmetryEstimation: " <<(double)(time_3_symmetryEstimation - time_2_partPCA) / CLOCKS_PER_SEC << "s" << endl; // // cout << "time_zeroPCA: " <<(double)(time_5_zeroPCA - time_3_symmetryEstimation) / CLOCKS_PER_SEC << "s" << endl << endl; // too small // cout << "total_ellipsoidExtraction: " <<(double)(time_5_zeroPCA - time_start) / CLOCKS_PER_SEC << "s" << endl; g2o::ellipsoid e_local = e_local_normalized; // calculate the probability of the single-frame ellipsoid estimation double prob_symmetry; if(runSymmetry) prob_symmetry = dataSymOutput.prob; // when the symmetry is opened else prob_symmetry = 1.0; // when the symmetry is closed, set it to a constant e_local.prob = prob_symmetry; mResult = true; return e_local; } PCAResult EllipsoidExtractor::ProcessPCANormalized(EllipsoidSLAM::PointCloud* pObject) { PCAResult data; double x,y,z; x=0;y=0;z=0; int num = pObject->size(); double max_x = 0; double max_y = 0; double max_z = 0; for( int i=0;i max_x ) max_x = std::abs(px); if( std::abs(py) > max_y ) max_y = std::abs(py); if( std::abs(pz) > max_z ) max_z = std::abs(pz); } x /= num; y /= num; z /= num; data.covariance = Vector3d(x,y,z); data.center = Vector3d(0,0,0); data.rotMat = Matrix3d::Identity(); data.scale << max_x, max_y, max_z; return data; } SymmetryOutputData EllipsoidExtractor::GetSymmetryOutputData() { return mSymmetryOutputData; } EllipsoidSLAM::PointCloud* EllipsoidExtractor::GetPointCloudInProcess() { return mpPoints; } EllipsoidSLAM::PointCloud* EllipsoidExtractor::GetPointCloudDebug() { return mpPointsDebug; } // pointcloud process EllipsoidSLAM::PointCloud* EllipsoidExtractor::ApplyPlaneFilter(EllipsoidSLAM::PointCloud* pCloud, double z) { EllipsoidSLAM::PointCloud *pCloudFiltered = new EllipsoidSLAM::PointCloud; int num = pCloud->size(); for(auto p: (*pCloud)) { if(p.z > z) pCloudFiltered->push_back(p); } return pCloudFiltered; } EllipsoidSLAM::PointCloud* EllipsoidExtractor::ApplySupportingPlaneFilter(EllipsoidSLAM::PointCloud* pCloud) { EllipsoidSLAM::PointCloud *pCloudFiltered = new EllipsoidSLAM::PointCloud; int num = pCloud->size(); int i=0; for(auto p: (*pCloud)) { double dis = mpPlane->distanceToPoint(Vector3d(p.x,p.y,p.z),true); // ture means keeping the flag. The PlaneExtractor has made sure the positive value means the point is above the plane. bool ok = dis>0.05; if(ok) pCloudFiltered->push_back(p); } return pCloudFiltered; } // The function will generate the 3D center point of the object from the bounding box and the depth image, which will help select the object cluster among all the clusters from Euclidean filter. // Considering the robustness, several points will be sampled around the center point of the bounding box, // and their average 3D positions will be taken as the output. bool EllipsoidExtractor::GetCenter(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera, Vector3d& center){ double depth_range = Config::ReadValue("EllipsoidExtractor_DEPTH_RANGE"); // get the center of the bounding box int x = int((bbox(0)+bbox(2))/2.0); int y = int((bbox(1)+bbox(3))/2.0); int point_num = 10; // sample 10 * 10 points int x_delta = std::abs(bbox(0) - bbox(2))/4.0 / point_num; int y_delta = std::abs(bbox(1) - bbox(3))/4.0 / point_num; PointCloudPCL::Ptr pCloud (new PointCloudPCL); PointCloudPCL& cloud = *pCloud; for( int x_id = -point_num/2; x_id(y_); ushort d = ptd[x_]; PointT p; p.z = d / camera.scale; // if the depth value is invalid, ignore this point if (p.z <= 0.1 || p.z > depth_range) continue; p.x = (x_ - camera.cx) * p.z / camera.fx; p.y = (y_ - camera.cy) * p.z / camera.fy; cloud.points.push_back(p); } } mDebugCenterCloud = pCloud; // store for visualization if( cloud.size() < 2 ) return false; // we need at least 2 valid points Eigen::Vector4d centroid; pcl::compute3DCentroid(cloud, centroid); // get their centroid EllipsoidSLAM::PointXYZRGB p; p.x = centroid[0]; p.y = centroid[1]; p.z = centroid[2]; // transform to the world coordintate g2o::SE3Quat campose_wc;campose_wc.fromVector(pose); Eigen::Matrix4d Twc = campose_wc.to_homogeneous_matrix(); Eigen::Vector3d xyz(p.x, p.y, p.z); Eigen::Vector4d Xc = real_to_homo_coord(xyz); Eigen::Vector4d Xw = Twc * Xc; Eigen::Vector3d xyz_w = homo_to_real_coord(Xw); center = xyz_w; return true; } EllipsoidSLAM::PointCloud* EllipsoidExtractor::ApplyEuclideanFilter(EllipsoidSLAM::PointCloud* pCloud, Vector3d ¢er) { clock_t time_1_start = clock(); // load config parameters int CONFIG_MinClusterSize = Config::Get( "EllipsoidExtraction.Euclidean.MinClusterSize"); double CONFIG_ClusterTolerance = Config::Get( "EllipsoidExtraction.Euclidean.ClusterTolerance"); double CONFIG_CENTER_DIS = Config::Get( "EllipsoidExtraction.Euclidean.CenterDis"); assert( CONFIG_MinClusterSize>0&&CONFIG_ClusterTolerance>0&&CONFIG_CENTER_DIS>0 && "Forge to set param. " ); pcl::PointCloud::Ptr pCloudPCL = QuadricPointCloudToPclXYZ(*pCloud); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); // use kdTree to speed up tree->setInputCloud (pCloudPCL); int point_num = pCloudPCL->size(); std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance (CONFIG_ClusterTolerance); // an important parameter. it must be larger than the grid size in the down sampling process ec.setMinClusterSize (CONFIG_MinClusterSize); ec.setMaxClusterSize (point_num); ec.setSearchMethod (tree); ec.setInputCloud (pCloudPCL); ec.extract (cluster_indices); bool bFindCluster = false; pcl::PointCloud::Ptr pFinalPoints; clock_t time_2_extractClusters = clock(); int cluster_size = cluster_indices.size(); // store the point clouds std::vector::Ptr> cloud_cluster_vector; for(auto it = cluster_indices.begin(); it!=cluster_indices.end();it++) { pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); for (std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) cloud_cluster->points.push_back (pCloudPCL->points[*pit]); cloud_cluster_vector.push_back(cloud_cluster); } mDebugEuclideanFilterClouds = cloud_cluster_vector; // store for debugging clock_t time_3_getClusterPoints = clock(); // select the cluster that has a distance below a threshold with the 3D center point projected near the center of the 2D bounding box for(int i=0; i::Ptr cloud_cluster = cloud_cluster_vector[i]; if( cluster_size == 1) { pFinalPoints = cloud_cluster; bFindCluster = true; } double dis = getDistanceFromPointToCloud(center, cloud_cluster); bool c2 = false; if( dis < CONFIG_CENTER_DIS ) c2 = true; // the distance should be small enough if(c2) { pFinalPoints = cloud_cluster; bFindCluster = true; break; } } EllipsoidSLAM::PointCloud* pCloudFiltered; if(!bFindCluster) { miEuclideanFilterState = 3; return pCloudFiltered; } clock_t time_4_selectTheBestCluster = clock(); pCloudFiltered = new EllipsoidSLAM::PointCloud(pclXYZToQuadricPointCloud(pFinalPoints)); miEuclideanFilterState = 0; clock_t time_5_copyNewCloud = clock(); // cout << "****** System Time [Euclidean Filter] ******" << endl ; // cout << "time_2_extractClusters: " <<(double)(time_2_extractClusters - time_1_start) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_3_getClusterPoints: " <<(double)(time_3_getClusterPoints - time_2_extractClusters) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_4_selectTheBestCluster: " <<(double)(time_4_selectTheBestCluster - time_3_getClusterPoints) / CLOCKS_PER_SEC << "s" << endl; // cout << "time_5_copyNewCloud: " <<(double)(time_5_copyNewCloud - time_4_selectTheBestCluster) / CLOCKS_PER_SEC << "s" << endl; return pCloudFiltered; } double EllipsoidExtractor::getDistanceFromPointToCloud(Vector3d& point, pcl::PointCloud::Ptr pCloud){ double mini_dis = 999999; if(pCloud->size() < 1) return -1; for(auto p : *pCloud) { Vector3d p_(p.x, p.y, p.z); double dis = (point - p_).norm(); if( dis < mini_dis ) mini_dis = dis; } return mini_dis; } void EllipsoidExtractor::SetSupportingPlane(g2o::plane* pPlane){ mpPlane = pPlane; mbSetPlane = true; } void EllipsoidExtractor::AdjustChirality(PCAResult &data){ // using cross operation to generate a new axis to make the coordinate right-handed data.rotMat.col(2) = data.rotMat.col(0).cross(data.rotMat.col(1)); return; } Eigen::Matrix3d EllipsoidExtractor::calibRotMatAccordingToGroundPlane(Matrix3d& rotMat, const Vector3d& normal){ // in order to apply a small rotation to align the z axis of the object and the normal vector of the groundplane, // we need calculate the rotation axis and its angle. // first get the rotation axis Vector3d ellipsoid_zAxis = rotMat.col(2); Vector3d rot_axis = ellipsoid_zAxis.cross(normal); rot_axis.normalize(); // then get the angle between the normal of the groundplane and the z axis of the object double norm1 = normal.norm(); double norm2 = ellipsoid_zAxis.norm(); double vec_dot = normal.transpose() * ellipsoid_zAxis; double cos_theta = vec_dot/norm1/norm2; double theta = acos(cos_theta); // generate the rotation vector AngleAxisd rot_angleAxis(theta,rot_axis); Matrix3d rotMat_calibrated = rot_angleAxis * rotMat; return rotMat_calibrated; } void EllipsoidExtractor::OpenVisualization(Map* pMap) { mbOpenVisualization = true; mpMap = pMap; } void EllipsoidExtractor::ClearPointCloudList() { if( mbOpenVisualization ) { mpMap->DeletePointCloudList("EllipsoidExtractor", 1); // partial martching } } void EllipsoidExtractor::VisualizePointCloud(const string& name, EllipsoidSLAM::PointCloud* pCloud, const Vector3d &color, int point_size){ if( mbOpenVisualization ) { // if the color is not set, use random color uchar r,g,b; if( color[0] < -0.01 || color[1] < -0.01 || color[2] < -0.01) { srand(time(0)); r = rand()%155; g = rand()%155; b = rand()%155; } else { r = 255 * color[0]; g = 255 * color[1]; b = 255 * color[2]; } SetPointCloudProperty(pCloud, r,g,b,point_size); string full_name = string("EllipsoidExtractor.") + name; mpMap->AddPointCloudList(full_name, pCloud, 1); } } void EllipsoidExtractor::VisualizeEllipsoid(const string& name, g2o::ellipsoid* pObj) { if( mbOpenVisualization ) { mpMap->addEllipsoidVisual(pObj); } } void EllipsoidExtractor::OpenSymmetry() { std::cout << std::endl; std::cout << " * Open Symmetry Estimation. " << std::endl; LoadSymmetryPrior(); mbOpenSymmetry = true; } } // namespace: EllipsoidSLAM ================================================ FILE: src/pca/EllipsoidExtractor.h ================================================ // Single-frame ellipsoid extraction from RGB-D data #ifndef ELLIPSOIDSLAM_ELLIPSOIDEXTRACTOR_H #define ELLIPSOIDSLAM_ELLIPSOIDEXTRACTOR_H #include #include #include #include #include #include #include #include #include #include #include #include #include #include typedef pcl::PointXYZ PointType; typedef pcl::Normal NormalType; namespace EllipsoidSLAM { struct PCAResult { EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool result; Eigen::Vector3d center; Eigen::Matrix3d rotMat; Eigen::Vector3d covariance; Eigen::Vector3d scale; }; class EllipsoidExtractor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EllipsoidExtractor(); // open symmetry plane estimation to finish point cloud completion void OpenSymmetry(); // The supporting plane is used to segment object points and estimate the object orientation void SetSupportingPlane(g2o::plane* pPlane); // API: estimate a 3d ellipsoid from RGB-D data and a bounding box, the ellipsoid is in global coordinate g2o::ellipsoid EstimateLocalEllipsoid(cv::Mat& depth, Eigen::Vector4d& bbox, int label, Eigen::VectorXd &pose, camera_intrinsic& camera); void OpenVisualization(Map* pMap); // if opened, the pointcloud during the process will be visualized void ClearPointCloudList(); // clear the visualized point cloud bool GetResult(); // get extraction result. SymmetryOutputData GetSymmetryOutputData(); // get the detail of symmetry estimation EllipsoidSLAM::PointCloud* GetPointCloudInProcess(); // get the object point cloud EllipsoidSLAM::PointCloud* GetPointCloudDebug(); // get the debug point cloud before Eucliden filter private: void LoadSymmetryPrior(); // define object symmetry prior pcl::PointCloud::Ptr ExtractPointCloud(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera); // extract point cloud from depth image. PCAResult ProcessPCA(pcl::PointCloud::Ptr &pCloudPCL); // apply principal component analysis g2o::ellipsoid ConstructEllipsoid(PCAResult &data); // generate a sparse ellipsoid estimation from PCA result. void ApplyGravityPrior(PCAResult &data); // add the supporting groundplane prior to calibrate the rotation matrix EllipsoidSLAM::PointCloud* ApplyEuclideanFilter(EllipsoidSLAM::PointCloud* pCloud, Vector3d ¢er); // apply euclidean filter to get the object points EllipsoidSLAM::PointCloud* ApplyPlaneFilter(EllipsoidSLAM::PointCloud* pCloud, double z); // filter points lying under the supporting plane EllipsoidSLAM::PointCloud* ApplySupportingPlaneFilter(EllipsoidSLAM::PointCloud* pCloud); bool GetCenter(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera, Vector3d& center); // get a coarse 3d center of the object double getDistanceFromPointToCloud(Vector3d& point, pcl::PointCloud::Ptr pCloud); // get the minimum distance between a point and a pointcloud // make sure the rotation matrix is right-handed void AdjustChirality(PCAResult &data); // adjust the axis order to make z axis near the normal of the ground plane void AlignZAxisToGravity(PCAResult &data); // this function will be called if groundplane is set when aligning Z axis Eigen::Matrix3d calibRotMatAccordingToGroundPlane( Matrix3d& rotMat, const Vector3d& normal); void VisualizePointCloud(const string& name, EllipsoidSLAM::PointCloud* pCloud, const Vector3d &color = Vector3d(-1,-1,-1), int point_size = 2); void VisualizeEllipsoid(const string& name, g2o::ellipsoid* pObj); PCAResult ProcessPCANormalized(EllipsoidSLAM::PointCloud* pObject); private: bool mResult; // estimation result. int miEuclideanFilterState; // Euclidean filter result: 1 no clusters 2: not the biggest cluster 3: fail to find valid cluster 0: success int miSystemState; // 0: normal 1: no depth value for center point 2: fail to filter. 3: no point left after downsample EllipsoidSLAM::PointCloud* mpPoints; // store object points EllipsoidSLAM::PointCloud* mpPointsDebug; // store points for debugging ( points before Euclidean filter) // supporting plane bool mbSetPlane; g2o::plane* mpPlane; // symmetry prior std::map mmLabelSymmetry; public: // data generated in the process Vector3d mDebugCenter; std::vector::Ptr> mDebugEuclideanFilterClouds; PointCloudPCL::Ptr mDebugCenterCloud; private: SymmetrySolver* mpSymSolver; SymmetryOutputData mSymmetryOutputData; bool mbOpenVisualization; Map* mpMap; int miExtractCount; bool mbOpenSymmetry; }; } #endif // ELLIPSOIDSLAM_ELLIPSOIDEXTRACTOR_H ================================================ FILE: src/plane/CMakeLists.txt ================================================ # SET(CMAKE_BUILD_TYPE Debug) add_library(PlaneExtractor SHARED PlaneExtractor.cpp ) target_link_libraries(PlaneExtractor ${OpenCV_LIBS} ${PCL_LIBRARIES} Config ) ================================================ FILE: src/plane/PlaneExtractor.cpp ================================================ #include "PlaneExtractor.h" #include "src/symmetry/PointCloudFilter.h" #include namespace EllipsoidSLAM { bool compare_func_plane_dis(g2o::plane* &p1, g2o::plane* &p2) { Vector3d cam_pose(0,0,0); double dis1 = p1->distanceToPoint(cam_pose); double dis2 = p2->distanceToPoint(cam_pose); return dis1 < dis2; } bool compare_func_plane_size(std::pair &p1, std::pair &p2) { return p1.second > p2.second; } void PlaneExtractor::extractPlanes(const cv::Mat &imDepth) { mvPlaneCoefficients.clear(); mvPlanePoints.clear(); int row_start; if( mParam.RangeOpen ) // if this flag is opened, only the bottom half part of the depth image is considered row_start = imDepth.rows - mParam.RangeHeight; // it normally starts from the half height of the image else row_start = 0; PointCloudPCL::Ptr inputCloud( new PointCloudPCL() ); for ( int m=row_start; m(m)[n]; double d = depthValue/ mParam.scale; PointT p; p.z = d; p.x = ( n - mParam.cx) * p.z / mParam.fx; p.y = ( m - mParam.cy) * p.z / mParam.fy; p.r = 0; p.g = 0; p.b = 250; inputCloud->points.push_back(p); } } inputCloud->height = ceil(imDepth.rows-row_start); inputCloud->width = ceil(imDepth.cols); mpCloudDense = inputCloud; // estimate the normal pcl::IntegralImageNormalEstimation ne; pcl::PointCloud::Ptr cloud_normals(new pcl::PointCloud); ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT); ne.setMaxDepthChangeFactor(0.05f); ne.setNormalSmoothingSize(10.0f); ne.setInputCloud(inputCloud); ne.compute(*cloud_normals); // load config parameters int min_plane = Config::Get("Plane.MinSize"); float AngTh = Config::Get("Plane.AngleThreshold"); float DisTh = Config::Get("Plane.DistanceThreshold"); vector coefficients; vector inliers; pcl::PointCloud::Ptr labels ( new pcl::PointCloud ); vector label_indices; vector boundary; pcl::OrganizedMultiPlaneSegmentation< PointT, pcl::Normal, pcl::Label > mps; mps.setMinInliers (100); mps.setAngularThreshold (0.017453 * AngTh); // deg to rad mps.setDistanceThreshold (DisTh); mps.setInputNormals (cloud_normals); mps.setInputCloud (inputCloud); std::vector, Eigen::aligned_allocator>> regions; mps.segmentAndRefine (regions, coefficients, inliers, labels, label_indices, boundary); pcl::ExtractIndices extract; extract.setInputCloud(inputCloud); extract.setNegative(false); // save the points belonging to each planes, and the coefficients for (int i = 0; i < inliers.size(); ++i) { int planeSize = inliers[i].indices.size(); if( planeSize < min_plane ) continue; PointCloudPCL::Ptr planeCloud(new PointCloudPCL()); cv::Mat coef = (cv::Mat_(4,1) << coefficients[i].values[0], coefficients[i].values[1], coefficients[i].values[2], coefficients[i].values[3]); if(coef.at(3) < 0) coef = -coef; extract.setIndices(boost::make_shared(inliers[i])); extract.filter(*planeCloud); mvPlanePoints.push_back(*planeCloud); mvPlaneCoefficients.push_back(coef); } } void PlaneExtractor::SetParam(PlaneExtractorParam& param){ mParam = param; } std::vector PlaneExtractor::GetPoints(){ return mvPlanePoints; } std::vector PlaneExtractor::GetPotentialGroundPlanePoints(){ return mvPotentialGroundPlanePoints; } std::vector PlaneExtractor::GetCoefficients(){ return mvPlaneCoefficients; } bool PlaneExtractor::extractGroundPlane(const cv::Mat &depth, g2o::plane& plane){ mParam.RangeOpen = false; mParam.RangeHeight = depth.rows/2; extractPlanes(depth); // first extract all the potential planes if( mvPlaneCoefficients.size() < 1) // there should be more than 1 potential planes return false; // the groundplane should meet two criteria: // 1) its normal vector should have a small angle difference with the gravity direction. ( filter the walls ) // 2) its distance with the center of the camera is the smallest among all the extracted planes. Vector3d cam_pose(0,0,0); std::vector vpPlanes; std::vector> mapPlaneSize; for( int i=0; i(0)), double(coeff.at(1)), double(coeff.at(2)), double(coeff.at(3)); // suppose the Y axis of the camera coordinate is the gravity direction and set a tolerence of angle difference of [pi/4, 3*pi/4]. // it always meets the criteria when the camera is located on a mobile robot. // it is a loose criteria only for filtering the walls. Eigen::Vector3d axisY(0,1,0); Eigen::Vector3d axisNorm = vec.head(3); double cos_theta = axisNorm.transpose() * axisY; cos_theta = cos_theta / axisNorm.norm() / axisY.norm(); double theta = acos( cos_theta ); // acos : [0,pi] if( theta > M_PI/4 && theta < 3*M_PI/4 ) continue; g2o::plane* pPlane = new g2o::plane(); pPlane->param= vec; vpPlanes.push_back(pPlane); mvPotentialGroundPlanePoints.push_back(mvPlanePoints[i]); mapPlaneSize.push_back(make_pair(pPlane, mvPlanePoints[i].size())); } if( vpPlanes.size() < 1) { // there should be more than 1 valid planes std::cout << "Please let the camera be parallel to the ground for initialization." << std::endl; return false; } // sort according to the size of planes sort(mapPlaneSize.begin(), mapPlaneSize.end(), compare_func_plane_size); g2o::plane* pPlaneGround = mapPlaneSize[0].first; // adjust the flag of the plane coefficients to make the distance between the camera center and the plane positive double value = pPlaneGround->distanceToPoint(Vector3d(0,0,0), true); if( value < 0 ) pPlaneGround->param = -pPlaneGround->param; plane = *pPlaneGround; return true; } PlaneExtractor::PlaneExtractor(const string& settings){ std::cout << "Init plane extractor using : " << settings << std::endl; Config::Init(); Config::SetParameterFile(settings); mParam.fx = Config::Get("Camera.fx"); mParam.fy = Config::Get("Camera.fy"); ; mParam.cx = Config::Get("Camera.cx"); ; mParam.cy = Config::Get("Camera.cy"); ; mParam.scale = Config::Get("DepthMapFactor"); ; } PointCloudPCL::Ptr PlaneExtractor::GetCloudDense(){ return mpCloudDense; } } ================================================ FILE: src/plane/PlaneExtractor.h ================================================ // this class offers a simple demo for extracting the groundplane from a single RGB-D frame #ifndef PLANEEXTRACTOR_H #define PLANEEXTRACTOR_H #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include typedef pcl::PointCloud PointCloudPCL; using namespace std; namespace EllipsoidSLAM { struct PlaneExtractorParam { double fx,fy,cx,cy; double scale; bool RangeOpen = false; // if this flag is opened, only the bottom half part of the depth image is considered int RangeHeight; }; class PlaneExtractor { public: PlaneExtractor(){}; PlaneExtractor(const string& settings); bool extractGroundPlane(const cv::Mat &depth, g2o::plane& plane); void extractPlanes(const cv::Mat &depth); void SetParam(PlaneExtractorParam& param); std::vector GetPoints(); std::vector GetPotentialGroundPlanePoints(); std::vector GetCoefficients(); PointCloudPCL::Ptr GetCloudDense(); private: // params int mParamRangeHeight; // whether to consider the bottom half part only PlaneExtractorParam mParam; std::vector mvPlanePoints; std::vector mvPotentialGroundPlanePoints; std::vector mvPlaneCoefficients; PointCloudPCL::Ptr mpCloudDense; }; } // EllipsoidSLAM #endif // PLANEEXTRACTOR_H ================================================ FILE: src/symmetry/BorderExtractor.cpp ================================================ #include "BorderExtractor.h" namespace EllipsoidSLAM { pcl::PointCloud::Ptr BorderExtractor::ToPointTCloud(pcl::PointCloud::Ptr pCloud) { pcl::PointCloud::Ptr pCloudXYZ(new pcl::PointCloud); for( auto p : pCloud->points ) { PointType pXYZ; pXYZ.x = p.x; pXYZ.y = p.y; pXYZ.z = p.z; pCloudXYZ->points.push_back(pXYZ); } return pCloudXYZ; } pcl::PointCloud::Ptr BorderExtractor::CombineCloud(pcl::PointCloud::Ptr& pCloud1, pcl::PointCloud::Ptr& pCloud2, pcl::PointIndices& indices1) { pcl::PointCloud::Ptr pComposite(new pcl::PointCloud); int i=0; for( auto p: pCloud1->points ){ pComposite->points.push_back(p); indices1.indices.push_back(i++); } for( auto p: pCloud2->points ) pComposite->points.push_back(p); return pComposite; } pcl::PointCloud::Ptr BorderExtractor::FilterBordersBasedOnPointCloud(pcl::PointCloud::Ptr& pBordersNoisy, pcl::PointCloud::Ptr &point_cloud_ptr){ pcl::PointCloud::Ptr pBorders_PointTCloud = ToPointTCloud(pBordersNoisy); pcl::PointIndices indices; pcl::PointCloud::Ptr pComposite = CombineCloud(pBorders_PointTCloud, point_cloud_ptr, indices); // filter the outliers of the borders. pcl::PointCloud::Ptr borderCloudFiltered(new pcl::PointCloud()); pcl::RadiusOutlierRemoval outrem; outrem.setInputCloud(pComposite); outrem.setIndices(boost::make_shared(indices)); outrem.setRadiusSearch(0.05); outrem.setMinNeighborsInRadius(6); outrem.filter(*borderCloudFiltered); return borderCloudFiltered; } pcl::PointCloud::Ptr BorderExtractor::extractBordersFromPointCloud(pcl::PointCloud::Ptr& point_cloud_ptr, double resolution) { clock_t startTime = clock(); float angular_resolution = pcl::deg2rad( float(resolution) ); pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; bool setUnseenToMaxRange = true; Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ()); pcl::PointCloud& point_cloud = *point_cloud_ptr; pcl::PointCloud far_ranges; scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0], point_cloud.sensor_origin_[1], point_cloud.sensor_origin_[2])) * Eigen::Affine3f (point_cloud.sensor_orientation_); // ----------------------------------------------- // -----Create RangeImage from the PointCloud----- // ----------------------------------------------- float noise_level = 0.0; float min_range = 0.0f; int border_size = 1; pcl::RangeImage::Ptr range_image_ptr (new pcl::RangeImage); pcl::RangeImage& range_image = *range_image_ptr; range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f), scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size); range_image.integrateFarRanges (far_ranges); if (setUnseenToMaxRange) range_image.setUnseenToMaxRange (); // ------------------------- // -----Extract borders----- // ------------------------- pcl::RangeImageBorderExtractor border_extractor (&range_image); pcl::PointCloud border_descriptions; // set parameter pcl::RangeImageBorderExtractor::Parameters& param = border_extractor.getParameters(); // param.pixel_radius_borders = 1; // default : 3 // param.minimum_border_probability = 0.99; // defualt: 0.8 border_extractor.compute (border_descriptions); // ---------------------------------- // -----Show points in 3D viewer----- // ---------------------------------- pcl::PointCloud::Ptr border_points_ptr(new pcl::PointCloud), veil_points_ptr(new pcl::PointCloud), shadow_points_ptr(new pcl::PointCloud); pcl::PointCloud& border_points = *border_points_ptr, & veil_points = * veil_points_ptr, & shadow_points = *shadow_points_ptr; for (int y=0; y< (int)range_image.height; ++y) { for (int x=0; x< (int)range_image.width; ++x) { if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER]) border_points.points.push_back (range_image.points[y*range_image.width + x]); if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT]) veil_points.points.push_back (range_image.points[y*range_image.width + x]); if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER]) shadow_points.points.push_back (range_image.points[y*range_image.width + x]); } } std::cout << "border_points: " << border_points.size() << std::endl; std::cout << "veil_points: " << veil_points.size() << std::endl; std::cout << "shadow_points: " << shadow_points.size() << std::endl; cout << "Border Extraction - No Floor: " <<(double)(clock() - startTime) / CLOCKS_PER_SEC << "s" << endl; return border_points_ptr; } } ================================================ FILE: src/symmetry/BorderExtractor.h ================================================ // extract borders from point objects to speed up the symmetry plane estimation #ifndef ELLIPSOIDSLAM_BORDEREXTRACTOR_H #define ELLIPSOIDSLAM_BORDEREXTRACTOR_H #include #include #include #include #include #include #include #include "src/symmetry/PointCloudFilter.h" #include #include typedef pcl::PointXYZ PointType; namespace EllipsoidSLAM { class BorderExtractor { public: pcl::PointCloud::Ptr ToPointTCloud(pcl::PointCloud::Ptr pCloud); pcl::PointCloud::Ptr CombineCloud(pcl::PointCloud::Ptr& pCloud1, pcl::PointCloud::Ptr& pCloud2, pcl::PointIndices& indices1); pcl::PointCloud::Ptr FilterBordersBasedOnPointCloud(pcl::PointCloud::Ptr& pBordersNoisy, pcl::PointCloud::Ptr &point_cloud_ptr); pcl::PointCloud::Ptr extractBordersFromPointCloud(pcl::PointCloud::Ptr& point_cloud_ptr, double resolution); }; // class }// namespace #endif //ELLIPSOIDSLAM_BORDEREXTRACTOR_H ================================================ FILE: src/symmetry/CMakeLists.txt ================================================ # set(CMAKE_BUILD_TYPE Debug) add_library(symmetry SHARED Symmetry.cpp SymmetrySolver.cpp BorderExtractor.cpp PointCloudFilter.cpp ) message ("source dir of symmetry: " ${PROJECT_SOURCE_DIR}) target_link_libraries(symmetry ${OpenCV_LIBS} ${PCL_LIBRARIES} utils ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so ) ================================================ FILE: src/symmetry/PointCloudFilter.cpp ================================================ #include "PointCloudFilter.h" Vector2d getXYCenterOfPointCloud(EllipsoidSLAM::PointCloud* pPoints) { double x=0; double y=0; int num=0; for( auto p:(*pPoints)) { x+=p.x; y+=p.y; num++; } x = x / double(num); y = y / double(num); return Vector2d(x,y); } // coordinates : x -> right, y-> down, z->front EllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, cv::Mat &rgb, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range) { // detect : x1 y1 x2 y2 EllipsoidSLAM::PointCloud cloud; // scan the points in the bounding box int x1 = int(detect(0)); int y1 = int(detect(1)); int x2 = int(detect(2)); int y2 = int(detect(3)); for (int y = y1; y < y2; y = y+3){ for (int x = x1; x < x2; x = x+3) { ushort *ptd = depth.ptr(y); ushort d = ptd[x]; EllipsoidSLAM::PointXYZRGB p; p.z = d / camera.scale; if (p.z <= 0.1 || p.z > range) // if the depth is valid continue; p.x = (x - camera.cx) * p.z / camera.fx; p.y = (y - camera.cy) * p.z / camera.fy; p.b = rgb.ptr(y)[x * 3]; p.g = rgb.ptr(y)[x * 3 + 1]; p.r = rgb.ptr(y)[x * 3 + 2]; p.size = 1; cloud.push_back(p); } } return cloud; } EllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range) { cv::Mat rgb = cv::Mat(depth.rows, depth.cols, CV_8UC3, cv::Scalar(0,0,0)); return getPointCloudInRect(depth, rgb, detect, camera, range); } void filterGround(EllipsoidSLAM::PointCloud** ppCloud) { EllipsoidSLAM::PointCloud *pCloudFiltered = new EllipsoidSLAM::PointCloud; EllipsoidSLAM::PointCloud *pCloud = *ppCloud; int num = pCloud->size(); for(auto p: (*pCloud)) { if(p.z > 0.05) pCloudFiltered->push_back(p); } delete pCloud; (*ppCloud) = pCloudFiltered; } void outputCloud(EllipsoidSLAM::PointCloud *pCloud, int num ) { int total_num = pCloud->size(); cout << "===== Point Cloud ====" << endl; int count = 0; for(auto p: *pCloud) { cout << count << ": " << p.x <<","<< p.y <<","<< p.z << endl ; if(count++ > num) break; } } EllipsoidSLAM::PointCloud pclToQuadricPointCloud(PointCloudPCL::Ptr &pCloud) { EllipsoidSLAM::PointCloud cloud; int num = pCloud->points.size(); for(int i=0;ipoints[i]; p.r = pT.r; p.g = pT.g; p.b = pT.b; p.x = pT.x; p.y = pT.y; p.z = pT.z; cloud.push_back(p); } return cloud; } EllipsoidSLAM::PointCloud* pclToQuadricPointCloudPtr(PointCloudPCL::Ptr &pCloud) { EllipsoidSLAM::PointCloud* cloudPtr = new EllipsoidSLAM::PointCloud; EllipsoidSLAM::PointCloud& cloud = *cloudPtr; int num = pCloud->points.size(); for(int i=0;ipoints[i]; p.r = pT.r; p.g = pT.g; p.b = pT.b; p.x = pT.x; p.y = pT.y; p.z = pT.z; cloud.push_back(p); } return cloudPtr; } PointCloudPCL::Ptr QuadricPointCloudToPcl(EllipsoidSLAM::PointCloud &cloud) { PointCloudPCL::Ptr pCloud(new PointCloudPCL); int num = cloud.size(); for(int i=0;ipoints.push_back(p); } return pCloud; } pcl::PointCloud::Ptr QuadricPointCloudToPclXYZ(EllipsoidSLAM::PointCloud &cloud) { pcl::PointCloud::Ptr pCloud(new pcl::PointCloud); int num = cloud.size(); for(int i=0;ipoints.push_back(p); } pCloud->width = (int) pCloud->points.size (); pCloud->height = 1; return pCloud; } EllipsoidSLAM::PointCloud pclXYZToQuadricPointCloud(pcl::PointCloud::Ptr &pCloud) { EllipsoidSLAM::PointCloud cloud; int num = pCloud->points.size(); for(int i=0;ipoints[i]; p.x = pT.x; p.y = pT.y; p.z = pT.z; cloud.push_back(p); } return cloud; } EllipsoidSLAM::PointCloud* pclXYZToQuadricPointCloudPtr(pcl::PointCloud::Ptr &pCloud) { EllipsoidSLAM::PointCloud* cloudPtr = new EllipsoidSLAM::PointCloud; EllipsoidSLAM::PointCloud &cloud = *cloudPtr; int num = pCloud->points.size(); for(int i=0;ipoints[i]; p.x = pT.x; p.y = pT.y; p.z = pT.z; cloud.push_back(p); } return cloudPtr; } void DownSamplePointCloud(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int param_num) { clock_t startTime,endTime; startTime = clock(); PointCloudPCL::Ptr pPclCloud = QuadricPointCloudToPcl(cloudIn); endTime = clock(); clock_t startTime_downsample = clock(); pcl::VoxelGrid voxel; double gridsize = 0.02; voxel.setLeafSize( gridsize, gridsize, gridsize ); voxel.setInputCloud( pPclCloud ); PointCloudPCL::Ptr tmp( new PointCloudPCL() ); voxel.filter( *tmp ); clock_t endTime_downsample = clock(); clock_t startTime_outlier = clock(); PointCloudPCL::Ptr pPclCloudFiltered(new PointCloudPCL); pcl::StatisticalOutlierRemoval sor; sor.setInputCloud (tmp); sor.setMeanK (param_num); sor.setStddevMulThresh (1.0); sor.filter (*pPclCloudFiltered); clock_t endTime_outlier = clock(); clock_t startTime2,endTime2; startTime2 = clock(); cloudOut = pclToQuadricPointCloud(pPclCloudFiltered); endTime2 = clock(); // cout << "Time diff QuadricPointCloudToPcl: " <<(double)(endTime - startTime) / CLOCKS_PER_SEC << "s" << endl; // cout << "Time diff downsample: " <<(double)(endTime_downsample - startTime_downsample) / CLOCKS_PER_SEC << "s" << endl; // cout << "Time diff outlier: " <<(double)(endTime_outlier - startTime_outlier) / CLOCKS_PER_SEC << "s" << endl; } void DownSamplePointCloudOnly(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, double grid) { PointCloudPCL::Ptr pPclCloud = QuadricPointCloudToPcl(cloudIn); pcl::VoxelGrid voxel; double gridsize = grid; voxel.setLeafSize( gridsize, gridsize, gridsize ); voxel.setInputCloud( pPclCloud ); PointCloudPCL::Ptr tmp( new PointCloudPCL() ); voxel.filter( *tmp ); cloudOut = pclToQuadricPointCloud(tmp); } void FiltOutliers(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int num_neighbor) { PointCloudPCL::Ptr pPclCloud = QuadricPointCloudToPcl(cloudIn); PointCloudPCL::Ptr pPclCloudFiltered(new PointCloudPCL); pcl::StatisticalOutlierRemoval sor; sor.setInputCloud (pPclCloud); sor.setMeanK (num_neighbor); sor.setStddevMulThresh (1.0); sor.filter (*pPclCloudFiltered); cloudOut = pclToQuadricPointCloud(pPclCloudFiltered); } void FiltPointsInBox(EllipsoidSLAM::PointCloud* pPoints_global, EllipsoidSLAM::PointCloud* pPoints_global_inBox, g2o::ellipsoid &e){ double radius = MAX(MAX(e.scale[0], e.scale[1]), e.scale[2]); Vector3d center = e.toVector().head(3); pPoints_global_inBox->clear(); Eigen::Matrix4d Q_star = e.generateQuadric(); Eigen::Matrix4d Q = Q_star.inverse(); for(auto p : (*pPoints_global) ) { Vector3d xyz(p.x, p.y, p.z); Eigen::Vector4d X = real_to_homo_coord_vec(xyz); double abstract_dis = X.transpose() * Q * X; bool isInside = (abstract_dis <0); if( isInside ) pPoints_global_inBox->push_back(p); } return; } // add the points in point cloud p2 to p1 void CombinePointCloud(EllipsoidSLAM::PointCloud *p1, EllipsoidSLAM::PointCloud *p2){ if( p1 ==NULL || p2==NULL){ cerr<< " point cloud is NULL. " << endl; return; } for(auto point : *p2) p1->push_back(point); return ; } ================================================ FILE: src/symmetry/PointCloudFilter.h ================================================ // process point cloud: segmentation, downsample, filter... #ifndef ELLIPSOIDSLAM_POINTCLOUDFILTER_H #define ELLIPSOIDSLAM_POINTCLOUDFILTER_H // pcl #include #include #include #include #include typedef pcl::PointXYZRGB PointT; typedef pcl::PointCloud PointCloudPCL; #include #include #include #include #include #include Vector2d getXYCenterOfPointCloud(EllipsoidSLAM::PointCloud* pPoints); EllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, cv::Mat &rgb, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range=100); EllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range=100); void filterGround(EllipsoidSLAM::PointCloud** ppCloud); void outputCloud(EllipsoidSLAM::PointCloud *pCloud, int num = 10); EllipsoidSLAM::PointCloud pclToQuadricPointCloud(PointCloudPCL::Ptr &pCloud); PointCloudPCL::Ptr QuadricPointCloudToPcl(EllipsoidSLAM::PointCloud &cloud); pcl::PointCloud::Ptr QuadricPointCloudToPclXYZ(EllipsoidSLAM::PointCloud &cloud); EllipsoidSLAM::PointCloud* pclToQuadricPointCloudPtr(PointCloudPCL::Ptr &pCloud); EllipsoidSLAM::PointCloud pclXYZToQuadricPointCloud(pcl::PointCloud::Ptr &pCloud); EllipsoidSLAM::PointCloud* pclXYZToQuadricPointCloudPtr(pcl::PointCloud::Ptr &pCloud); // downsample and outlier filter void DownSamplePointCloud(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int param_num = 100); // only downsample void DownSamplePointCloudOnly(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, double grid=0.02); // filter outliers void FiltOutliers(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int num_neighbor = 100); // filter points and keep those in the ellipsoid void FiltPointsInBox(EllipsoidSLAM::PointCloud* pPoints_global, EllipsoidSLAM::PointCloud* pPoints_global_inBox, g2o::ellipsoid &e); void CombinePointCloud(EllipsoidSLAM::PointCloud *p1, EllipsoidSLAM::PointCloud *p2); #endif // POINTCLOUDFILTER ================================================ FILE: src/symmetry/Symmetry.cpp ================================================ #include "PointCloudFilter.h" #include "SymmetrySolver.h" #include "Symmetry.h" #include namespace EllipsoidSLAM { // ---- compare functions bool comp_func_mapPlane(const pair& t1, const pair& t2) { return t1.first > t2.first; } bool comp_func_mapProbData(const pair& t1, const pair& t2) { return t1.first > t2.first; } void SaveSymmetryResultToText(std::vector> &mapProbData, const string& path) { ofstream out(path.c_str()); for( auto pair : mapProbData ) { double prob = pair.first; auto data = pair.second; out << data.pInitPlane->azimuth() << "\t" << data.pInitPlane->distance() << "\t" << data.init_error << "\t" << data.pPlane->azimuth() << "\t" << data.pPlane->distance() << "\t" << data.final_error << std::endl; } std::cout << "[ Save symmetry result to : " << path << " ]" << std::endl; } std::vector GenerateInitPlanes(int symType = 1) { /* * sample new planes along two degrees: angle and distance. * on each of them sample (2*step_num+1) planes. * totally, sample (step_num*2+1)*(step_num*2+1) planes around the init planes. */ bool open_multiple_samples = true; if(open_multiple_samples) { // CONFIGURATION int step_num = 1; double diff_dis = 0.2; // 0.2m double diff_angle = M_PI/180.0*5; // 5deg double start_dis = -diff_dis * step_num; double start_angle = -diff_angle * step_num; int loop_num = 2 * step_num + 1; // ------------ std::vector initPlanes; for(int i = 0; ifromDisAngleTrans(dis, angle, 0); initPlanes.push_back(pPlane); } } return initPlanes; } else { std::vector initPlanes; g2o::plane *pPlane = new g2o::plane; pPlane->fromDisAngleTrans(0, 0, 0); initPlanes.push_back(pPlane); return initPlanes; } } SymmetrySolverData Symmetry::estimateSymmetry(Vector4d &bbox, PointCloud* pCloud, VectorXd& pose, cv::Mat& projDepth, camera_intrinsic& camera, int symType) { SymmetrySolverData output; output.result = false; if( pCloud->size() < 1 ) { std::cerr << " Point Cloud Size has some problems: " << pCloud->size() << std::endl; return output; } Matrix3d calib = getCalibFromCamera(camera); SymmetrySolver solver; solver.SetCameraParam(camera); // set the border if( mbOpenSparseEstimation ) solver.SetBorders(mpBorders); std::vector> mapProbData; std::vector initPlanes = GenerateInitPlanes(symType); int opt_time = initPlanes.size(); for(int i=0; i< opt_time ; i ++){ g2o::plane* pPlane= initPlanes[i]; SymmetrySolverData data; if(symType == 1) data = solver.OptimizeSymmetryPlane(bbox, *pPlane, pCloud, projDepth, pose, calib, camera.scale, symType); else if(symType == 2) data = solver.OptimizeSymmetryDualPlane(bbox, *pPlane, pCloud, projDepth, pose, calib, camera.scale, symType); double prob = data.prob; mapProbData.push_back(make_pair(prob, data)); } sort(mapProbData.begin(), mapProbData.end(), comp_func_mapProbData); assert(mapProbData.size() > 0 && "Wrong size in mapProbData."); auto pair = mapProbData[0]; output = pair.second; return output; } void Symmetry::releaseData(SymmetryOutputData& data) { if(data.pCloud != NULL) delete data.pCloud; } // change the distance between a point to the image plane to the distance between a point to the camera center double calculateProjZ(double f, double d, double xi, double yi){ return d*sqrt(xi*xi+f*f+yi*yi)/f; } cv::Mat Symmetry::getProjDepthMat(cv::Mat& depth, camera_intrinsic& camera){ int rows = depth.rows; int cols = depth.cols; cv::Mat depthProj = cv::Mat(rows, cols, CV_16UC1, cv::Scalar(0)); double fx = camera.fx; double cx = camera.cx; double cy = camera.cy; for( int x=0;x(y,x); double realz = calculateProjZ(fx, double(d), (x - cx), (y - cy)); depthProj.at(y, x) = ushort(realz); } return depthProj; } void Symmetry::SetBorders(EllipsoidSLAM::PointCloud* pBorders) { mbOpenSparseEstimation = true; mpBorders = pBorders; } Symmetry::Symmetry() { mbOpenSparseEstimation = false; mpExtractor = new BorderExtractor; miParamFilterPointNum = 100; mbGroundPlaneSet = false; mbObjInitialGuessSet = false; mbInitPlanesSet = false; } } ================================================ FILE: src/symmetry/Symmetry.h ================================================ #ifndef ELLIPSOIDSLAM_SYMMETRY_H #define ELLIPSOIDSLAM_SYMMETRY_H #include #include #include using namespace Eigen; #include #include "SymmetrySolver.h" #include "BorderExtractor.h" namespace EllipsoidSLAM { class SymmetryOutputData { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool result; PointCloud* pCloud; // object point cloud Vector4d planeVec; Vector4d planeVec2; // the second plane of dual reflection double prob; PointCloud* pBorders; // object borders, used in the sparse mode; invalid now. Vector3d center; // for visualizing the symmetry plane as an finite plane int symmetryType; }; class Symmetry { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Symmetry(); // API: estimate a symmetry plane from a point cloud using all the points // symType: 1 reflection symmetry; 2 dual reflection symmetry SymmetrySolverData estimateSymmetry(Vector4d& bbox, PointCloud* pCloud, VectorXd& pose, cv::Mat& projDepth, camera_intrinsic& camera, int symType=1); void static releaseData(SymmetryOutputData& data); cv::Mat static getProjDepthMat(cv::Mat& depth, camera_intrinsic& camera); public: void SetGroundPlane(Vector4d& normal); void SetBorders(EllipsoidSLAM::PointCloud* pBorders); void SetConfigResolution(double res); double GetConfigResolution(); void SetConfigFilterPointNum(int num); private: bool mbOpenSparseEstimation; PointCloud* mpBorders; BorderExtractor* mpExtractor; int miParamFilterPointNum; Vector4d mGroundPlaneNormal; bool mbGroundPlaneSet; g2o::ellipsoid mObjInitialGuess; bool mbObjInitialGuessSet; std::vector mvpInitPlanes; bool mbInitPlanesSet; }; } #endif //ELLIPSOIDSLAM_SYMMETRY_H ================================================ FILE: src/symmetry/SymmetrySolver.cpp ================================================ #include "core/Ellipsoid.h" #include "SymmetrySolver.h" #include "PointCloudFilter.h" #include "utils/matrix_utils.h" // g2o #include "Thirdparty/g2o/g2o/core/block_solver.h" #include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h" #include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h" #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" #include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h" #include "Thirdparty/g2o/g2o/core/robust_kernel.h" #include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h" #include #include using namespace EllipsoidSLAM; static int calcu_time = 0; bool isInRange(int x, int y, int range_x, int range_x2, int range_y, int range_y2) { if( range_x < x && x < range_x2 ) if( range_y& kdTree) { g2o::ellipsoid e; g2o::SE3Quat campose_wc, campose_cw; campose_wc.fromVector(pose.tail(7)); campose_cw = campose_wc.inverse(); Matrix3Xd projMat = e.generateProjectionMatrix(campose_cw, calib); int num = pCloudSym->size(); double ln_total_P = 0; int num_invalid = 0; for( int i=0; i(uvHomo); int x = int(uv[0]); int y = int(uv[1]); double dis_diff = 0; if( isInRange(x,y,bbox[0],bbox[2],bbox[1],bbox[3]) ) { ushort *ptd = depth.ptr(int(uv[1])); ushort d = ptd[int(uv[0])]; if( d == 0 ) // occluded area { dis_diff = 0; } else { double depth = d / scale; Vector3d cam_c = campose_wc.toVector().head(3); Vector3d point(p.x,p.y,p.z); double dis_cam_point = (cam_c-point).norm(); if( dis_cam_point > depth ) // occluded area { dis_diff = 0; } else // observable area { if(!checkValidPoint(p)) { dis_diff = 0; num_invalid++; } else dis_diff = findMinimalDistanceWithKdTree(p, kdTree); } } } else // observable area { if(!checkValidPoint(p)) { dis_diff = 0; num_invalid++; } else dis_diff = findMinimalDistanceWithKdTree(p, kdTree); } // Sigma is used for probability calculation. it's useless in the optimization process. double Sigma = Config::ReadValue("SymmetrySolver.Sigma"); double Sigma_inv = 1.0 / Sigma; double ln_P = -0.5*Sigma_inv*Sigma_inv*dis_diff*dis_diff; ln_total_P += ln_P; } int num_valid = num - num_invalid; double aver_ln_P; if(num_valid > 0) aver_ln_P = ln_total_P / double(num_valid); else aver_ln_P = -INFINITY; mData.pSymetryCloud = pCloudSym; if(num_invalid>0) std::cout << " - Invalid/Valid SymPoints Num : " << num_invalid << " / " << num_valid << std::endl; return aver_ln_P; } double SymmetrySolver::findMinimalDistanceWithKdTree(PointXYZRGB &p, pcl::KdTreeFLANN& kdTree) { pcl::PointXYZ searchPoint; searchPoint.x = p.x; searchPoint.y = p.y; searchPoint.z = p.z; // K nearest neighbor search int K = 1; std::vector pointIdxNKNSearch(K); std::vector pointNKNSquaredDistance(K); kdTree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance); assert( pointIdxNKNSearch.size() == 1 && "Error in KdTree Search." ); int nearest_id = pointIdxNKNSearch[0]; pcl::PointXYZ nearest_point = kdTree.getInputCloud()->points[nearest_id]; Vector3d pos1(p.x,p.y,p.z); Vector3d pos2(nearest_point.x,nearest_point.y,nearest_point.z); double dis = (pos1-pos2).norm(); return dis; } PointCloud* SymmetrySolver::GetSymmetryPointCloud(PointCloud* pCloud, g2o::plane &plane, int symType) { PointCloud* pCloudSym = new PointCloud; int num = pCloud->size(); for(int i=0;i(point); Vector4d pointSymHomo = GetSymmetryPointOfPlane(point_homo, plane); Vector3d pointSym = homo_to_real_coord_vec(pointSymHomo); auto pSym = p; pSym.g = 255; pSym.x = pointSym[0]; pSym.y = pointSym[1]; pSym.z = pointSym[2]; pCloudSym->push_back(pSym); } return pCloudSym; } Vector4d SymmetrySolver::GetSymmetryPointOfPlane(Vector4d& point, g2o::plane &plane) { // get the plane normal vector Vector3d normal = plane.param.head(3); normal.normalize(); double up_equation = std::abs(plane.param.transpose()*point); double down_equation = std::sqrt(plane.param.head(3).transpose()*plane.param.head(3)); double dis = up_equation/down_equation; Vector3d point_3 = homo_to_real_coord_vec(point); double symbol; double symbol_value = point.transpose()*plane.param; if(symbol_value > 0) symbol =-1 ; else symbol = 1; Vector3d symPoint = point_3 + 2*symbol*dis*normal; Vector4d symPointHomo = real_to_homo_coord_vec(symPoint); return symPointHomo; } // Given an initial symmetry plane, output an optimized symmetry plane SymmetrySolverData SymmetrySolver::OptimizeSymmetryPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, int symType) { g2o::SparseOptimizer graph; g2o::BlockSolverX::LinearSolverType* linearSolver; linearSolver = new g2o::LinearSolverDense(); g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); graph.setAlgorithm(solver); graph.setVerbose(false); // output // add vertices g2o::plane plane = initPlane; g2o::VertexPlane* vpVertexPlane = new g2o::VertexPlane; vpVertexPlane->setEstimate(plane); vpVertexPlane->setId(0); vpVertexPlane->setFixed(false); graph.addVertex(vpVertexPlane); // add edges g2o::EdgeSymmetryPlane* e = new g2o::EdgeSymmetryPlane(); e->setVertex(0, dynamic_cast( vpVertexPlane )); e->setMeasurement(1); e->setId(0); double inv_sigma;inv_sigma = 1; inv_sigma = inv_sigma*1; Matrix info; info(0,0) = inv_sigma; e->setInformation(info); // initialize edge parameter e->initializeParam(bbox, pCloud, depth_origin, pose, calib, scale); e->initializeKDTree(); if( mbOpenSparseEstimation ) e->setBordersPointCloud( mpBorders ); graph.addEdge(e); // save mData.pInitPlane = new g2o::plane(vpVertexPlane->estimate()); e->computeError(); Matrix errorMat_init = e->error(); mData.init_error = errorMat_init(0,0); graph.initializeOptimization(); graph.optimize(5); // the number could be adjusted according to your time-efficiency trade Matrix errorMat = e->error(); // save result. mData.prob = exp(-errorMat(0,0)); // probability calculation could be adjusted here. mData.pPlane = new g2o::plane(vpVertexPlane->estimate()); mData.pPlane->color = Vector3d(1,0,0); mData.symType = symType; mData.final_error = errorMat(0,0); mData.result = true; return mData; } // this one is the dual plane version. SymmetrySolverData SymmetrySolver::OptimizeSymmetryDualPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, int symType) { g2o::SparseOptimizer graph; g2o::BlockSolverX::LinearSolverType* linearSolver; linearSolver = new g2o::LinearSolverDense(); g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); graph.setAlgorithm(solver); graph.setVerbose(false); g2o::VertexDualPlane* vpVertexDualPlane = new g2o::VertexDualPlane; vpVertexDualPlane->setEstimate(initPlane); vpVertexDualPlane->setId(0); vpVertexDualPlane->setFixed(false); graph.addVertex(vpVertexDualPlane); g2o::EdgeSymmetryDualPlane* e = new g2o::EdgeSymmetryDualPlane(); e->setVertex(0, dynamic_cast( vpVertexDualPlane )); e->setMeasurement(1); e->setId(0); double inv_sigma;inv_sigma = 1; Matrix info; info(0,0) = inv_sigma; e->setInformation(info); e->initializeParam(bbox, pCloud, depth_origin, pose, calib, scale); e->initializeKDTree(); if( mbOpenSparseEstimation ) e->setBordersPointCloud( mpBorders ); graph.addEdge(e); mData.pInitPlane = new g2o::plane(initPlane.GeneratePlaneVec()); mData.pInitPlane2 = new g2o::plane(initPlane.GenerateAnotherPlaneVec()); e->computeError(); Matrix errorMat_init = e->error(); mData.init_error = errorMat_init(0,0); graph.initializeOptimization(); graph.optimize(5); Matrix errorMat = e->error(); mData.prob = exp(-errorMat(0,0)); g2o::plane dualplaneOptimized = vpVertexDualPlane->estimate(); mData.pPlane = new g2o::plane(dualplaneOptimized.GeneratePlaneVec()); mData.pPlane2 = new g2o::plane(dualplaneOptimized.GenerateAnotherPlaneVec()); mData.pPlane->color = Vector3d(1.0,0.3,0); mData.pPlane2->color = Vector3d(1.0,0.3,0); mData.symType = symType; mData.result = true; mData.final_error = errorMat(0,0); return mData; } void SymmetrySolver::SetCameraParam(camera_intrinsic& camera){ mCamera = camera; } void SymmetrySolver::SetInitPlane(g2o::plane* plane){ mpInitPlane = plane; } SymmetrySolverData SymmetrySolver::getResult() { return mData; } void SymmetrySolver::SetBorders(EllipsoidSLAM::PointCloud* pBorders){ mpBorders = pBorders; mbOpenSparseEstimation = true; } SymmetrySolverData SymmetrySolver::mData; namespace g2o { EdgeSymmetryPlane::EdgeSymmetryPlane() { mbInitilized = false; mbKdTreeInitialized = false; mpBorders = NULL; miSymmetryType = 1; // default: reflection symmetry } bool EdgeSymmetryPlane::read(std::istream& is){ return true; }; bool EdgeSymmetryPlane::write(std::ostream& os) const { return os.good(); }; void EdgeSymmetryPlane::computeError() { if(!mbInitilized) cerr << "Symmetry Edge has not been initialized yet." << endl; const VertexPlane* planeVertex = static_cast(_vertices[0]); assert( mpCloud != NULL && "Point cloud shouldn't be NULL."); g2o::plane plane = planeVertex->estimate(); // generate mirrored pointcloud PointCloud* pObjectCloud; if(mpBorders == NULL ) pObjectCloud = mpCloud; else pObjectCloud = mpBorders; PointCloud* pCloudSym = SymmetrySolver::GetSymmetryPointCloud(pObjectCloud, plane); double cost; assert( mbKdTreeInitialized == true && "Please initialize kdTree."); cost = SymmetrySolver::GetPointCloudProb(mBbox, pCloudSym, mpCloud, mDepth, mPose, mCalib, mScale, mKdtree); _error = Matrix( - cost); } void EdgeSymmetryPlane::initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale) { mpCloud = pCloud; mDepth = depth; mPose = pose; mCalib = calib; mScale = scale; mBbox = bbox; mbInitilized = true; } void EdgeSymmetryPlane::initializeKDTree() { if(mpCloud == NULL ) { cerr << "Point cloud is NULL." << endl; return; } pcl::PointCloud::Ptr cloudPCL (new pcl::PointCloud); int num = mpCloud->size(); for(int i=0;ipoints.push_back(p); } mKdtree.setInputCloud (cloudPCL); mbKdTreeInitialized = true; } void EdgeSymmetryPlane::setBordersPointCloud(PointCloud* pBorders) { mpBorders = pBorders; } EdgeSymmetryDualPlane::EdgeSymmetryDualPlane() { mbInitilized = false; mbKdTreeInitialized = false; mpBorders = NULL; miSymmetryType = 1; // default: reflection symmetry } bool EdgeSymmetryDualPlane::read(std::istream& is){ return true; }; bool EdgeSymmetryDualPlane::write(std::ostream& os) const { return os.good(); }; void EdgeSymmetryDualPlane::computeError() { if(!mbInitilized) cerr << "Symmetry Edge has not been initialized yet." << endl; const VertexDualPlane* dualplaneVertex = static_cast(_vertices[0]); assert( mpCloud != NULL && "Point cloud shouldn't be NULL."); g2o::plane dualplane = dualplaneVertex->estimate(); g2o::plane plane1(dualplane.GeneratePlaneVec()); g2o::plane plane2(dualplane.GenerateAnotherPlaneVec()); PointCloud* pObjectCloud; if(mpBorders == NULL ) pObjectCloud = mpCloud; else pObjectCloud = mpBorders; PointCloud* pCloudSym1 = SymmetrySolver::GetSymmetryPointCloud(pObjectCloud, plane1); PointCloud* pCloudSym2 = SymmetrySolver::GetSymmetryPointCloud(pObjectCloud, plane2); CombinePointCloud(pCloudSym1, pCloudSym2); assert( mbKdTreeInitialized && " Please initialize the kdtree " ); double cost = SymmetrySolver::GetPointCloudProb(mBbox, pCloudSym1, mpCloud, mDepth, mPose, mCalib, mScale, mKdtree); _error = Matrix( - cost); } void EdgeSymmetryDualPlane::initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale) { mpCloud = pCloud; mDepth = depth; mPose = pose; mCalib = calib; mScale = scale; mBbox = bbox; mbInitilized = true; } void EdgeSymmetryDualPlane::initializeKDTree() { if(mpCloud == NULL ) { cerr << "Point cloud is NULL." << endl; return; } pcl::PointCloud::Ptr cloudPCL (new pcl::PointCloud); int num = mpCloud->size(); for(int i=0;ipoints.push_back(p); } mKdtree.setInputCloud (cloudPCL); mbKdTreeInitialized = true; } void EdgeSymmetryDualPlane::setBordersPointCloud(PointCloud* pBorders) { mpBorders = pBorders; } void VertexPlane::setToOriginImpl() { _estimate = g2o::plane(); } void VertexPlane::oplusImpl(const double* update_) { Eigen::Map update(update_); Vector3d update2DOF(update[0], 0, update[1]); // yaw, 0, dis _estimate.oplus(update2DOF); } bool VertexPlane::read(std::istream& is) { return true; } bool VertexPlane::write(std::ostream& os) const { return os.good(); } void VertexDualPlane::setToOriginImpl() { _estimate = g2o::plane(); } void VertexDualPlane::oplusImpl(const double* update_) { Eigen::Map update(update_); _estimate.oplus_dual(update); } bool VertexDualPlane::read(std::istream& is) { return true; } bool VertexDualPlane::write(std::ostream& os) const { return os.good(); } } // namespace g2o ================================================ FILE: src/symmetry/SymmetrySolver.h ================================================ #ifndef ELLIPSOIDSLAM_SYMMETRYSOLVER_H #define ELLIPSOIDSLAM_SYMMETRYSOLVER_H #include #include #include #include #include #include using namespace Eigen; using namespace EllipsoidSLAM; class SymmetrySolverData { public: PointCloud* pSymetryCloud; g2o::plane* pPlane; g2o::plane* pPlane2; double prob; int symType; bool result; // save the initial data g2o::plane* pInitPlane; g2o::plane* pInitPlane2; double init_error; double final_error; }; class SymmetrySolver { public: SymmetrySolver(); SymmetrySolverData OptimizeSymmetryPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, int symType); SymmetrySolverData OptimizeSymmetryDualPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, int symType); static double GetPlaneProbWithKdTree(g2o::plane &plane, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale, pcl::KdTreeFLANN& kdTree, PointCloud* pBorders = NULL); static double GetPointCloudProb(Vector4d &bbox, PointCloud* pCloudSym, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale, pcl::KdTreeFLANN& kdTree); static double GetPlaneProbWithKdTreeUsingAdvanceProbEquation(g2o::plane &plane, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale, pcl::KdTreeFLANN& kdTree, PointCloud* pBorders = NULL); static double calculatePlaneCost(g2o::plane &plane, PointCloud* pCloud); static PointCloud* GetSymmetryPointCloud(PointCloud* pCloud, g2o::plane &p, int symType = 1); public: void SetBorders(EllipsoidSLAM::PointCloud* pBorders); private: static double calculateOcclProbOfPoints(PointCloud *pCloudSym, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale); static double calculateMatchProbOfPoints(PointCloud* pCloud, PointCloud* pCloudSym); static double findMinimalDistance(PointXYZRGB &p, PointCloud* pCloud); static Vector4d GetSymmetryPointOfPlane(Vector4d& point, g2o::plane &plane); static double calculateMatchProbOfPointsWithKdTree(PointCloud* pCloud, PointCloud* pCloudSym, pcl::KdTreeFLANN& kdTree); static double findMinimalDistanceWithKdTree(PointXYZRGB &p, pcl::KdTreeFLANN& kdTree); public: static SymmetrySolverData mData; private: bool mbOpenSparseEstimation; PointCloud* mpBorders; std::map mmLabelSymmetry; public: void SetCameraParam(camera_intrinsic& camera); void SetInitPlane(g2o::plane* plane); SymmetrySolverData getResult(); private: camera_intrinsic mCamera; g2o::plane* mpInitPlane; }; /* * This part contains Vertex, Edges in symmetry optimization using g2o. */ namespace g2o { class VertexPlane:public BaseVertex<2,g2o::plane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexPlane(){}; virtual void setToOriginImpl(); virtual void oplusImpl(const double* update_); virtual bool read(std::istream& is) ; virtual bool write(std::ostream& os) const ; }; // Dual Plane class VertexDualPlane:public BaseVertex<3,g2o::plane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexDualPlane(){}; virtual void setToOriginImpl(); virtual void oplusImpl(const double* update_); virtual bool read(std::istream& is) ; virtual bool write(std::ostream& os) const ; }; class EdgeSymmetryPlane:public BaseUnaryEdge<1,double, VertexPlane> { public: EdgeSymmetryPlane(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError(); void initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale); void initializeKDTree(); void setBordersPointCloud(PointCloud* pBorders); private: EllipsoidSLAM::PointCloud* mpCloud; cv::Mat mDepth; VectorXd mPose; Matrix3d mCalib; double mScale; Vector4d mBbox; //bounding box. bool mbInitilized; pcl::KdTreeFLANN mKdtree; bool mbKdTreeInitialized; EllipsoidSLAM::PointCloud* mpBorders; int miSymmetryType; }; class EdgeSymmetryDualPlane:public BaseUnaryEdge<1,double, VertexDualPlane> { public: EdgeSymmetryDualPlane(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; void computeError(); void initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale); void initializeKDTree(); void setBordersPointCloud(PointCloud* pBorders); void setSymmetryType(int type); private: EllipsoidSLAM::PointCloud* mpCloud; cv::Mat mDepth; VectorXd mPose; Matrix3d mCalib; double mScale; Vector4d mBbox; //bounding box. bool mbInitilized; pcl::KdTreeFLANN mKdtree; bool mbKdTreeInitialized; EllipsoidSLAM::PointCloud* mpBorders; int miSymmetryType; }; } #endif ================================================ FILE: src/tum_rgbd/CMakeLists.txt ================================================ # set(CMAKE_BUILD_TYPE Debug) add_library(tum_rgbd SHARED io.cpp ) target_link_libraries(tum_rgbd ${OpenCV_LIBS} utils ) ================================================ FILE: src/tum_rgbd/io.cpp ================================================ // Basic input and output of the TUM-RGBD dataset. #include "io.h" #include "utils/dataprocess_utils.h" #include #include using namespace std; namespace TUMRGBD { void Dataset::loadDataset(string &path){ cout << "Load dataset from: " << path << endl; msDatasetDir = path; msRGBDir = msDatasetDir + "rgb/"; msDepthDir = msDatasetDir + "depth/"; msGroundtruthPath = msDatasetDir + "groundtruth.txt"; msAssociatePath = msDatasetDir + "associate.txt"; msAssociateGroundtruthPath = msDatasetDir + "associateGroundtruth.txt"; // get all the file names under the directory GetFileNamesUnderDir(msRGBDir, mvRGBFileNames); sortFileNames(mvRGBFileNames, mvRGBFileNames); miTotalNum = mvRGBFileNames.size(); // generate the map between the timestamps to the depth images loadGroundTruthToMap(msGroundtruthPath); // generate the index from ID to the timestamps of rgb images generateIndexIdToRGBTimeStamp(); // generate the associations from rgb Timestamp to depth Timestamp LoadAssociationRGBToDepth(msAssociatePath); // generate the associations from rgb timestamp to the groundtruth LoadAssociationRGBToGroundtruth(msAssociateGroundtruthPath); // get debug file num: std::cout << "mmTimeStampToPose: " << mmTimeStampToPose.size() << std::endl; std::cout << "mvIdToDepthImagePath: " << mvIdToDepthImagePath.size() << std::endl; std::cout << "mvIdToGroundtruthTimeStamp: " << mvIdToGroundtruthTimeStamp.size() << std::endl; miCurrentID = 0; mbDetectionLoaded = false; mbOdomSet = false; } bool Dataset::readFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose){ if(miCurrentID < miTotalNum) { miCurrentID++; bool result = findFrameUsingID(miCurrentID-1, rgb, depth, pose); return result; } else { std::cout << "[Dataset] no data left." << std::endl; return false; } } bool Dataset::findFrameUsingID(int id, cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose){ if( id <0 || id>=miTotalNum) return false; int currentID = id; string depthTimeStampAssociated = mvIdToDepthTimeStamp[currentID]; if(depthTimeStampAssociated == ""){ std::cout << "[Dataset] fail to load the depth timestamp." << std::endl; return false; } string depthPath = msDatasetDir + "/" + mvIdToDepthImagePath[currentID]; string gtTimeStamp = mvIdToGroundtruthTimeStamp[currentID]; bool bFindPose = false; if( !mbOdomSet ) // if the odometry is set, return the pose of the odometry instead of the groundtruth bFindPose = getPoseFromTimeStamp(gtTimeStamp, pose); else bFindPose = getPoseFromRGBTimeStamp(mvIdToRGBTimeStamp[currentID], pose); if(bFindPose){ rgb = cv::imread(mvRGBFileNames[currentID], IMREAD_UNCHANGED); depth = cv::imread(depthPath, IMREAD_UNCHANGED); return true; } else { std::cout << "[Dataset] fail to find the pose ." << std::endl; return false; } } map::const_iterator AssociateWithNumber(const map& map, const string ×tamp){ auto iter = map.begin(); for( ; iter != map.end(); iter++ ) { if( iter->first == "" || timestamp == "" ) continue; if( std::abs(atof(iter->first.c_str()) - atof(timestamp.c_str())) < 0.001 ) { // get it return iter; } } return iter; } map::const_iterator AssociateWithNumber(const map& map, const string ×tamp){ auto iter = map.begin(); for( ; iter != map.end(); iter++ ) { if( iter->first == "" || timestamp == "" ) continue; if( std::abs(atof(iter->first.c_str()) - atof(timestamp.c_str())) < 0.001 ) { // get it return iter; } } return iter; } // get pose from the map using the timestamp as index. // attention: this version uses string type of timestamp to search, which means their timestamp MUST be totally the same. // it will be updated to double type in the future. bool Dataset::getPoseFromTimeStamp(string ×tamp, VectorXd &pose){ for( auto iter : mmTimeStampToPose ) { if( std::abs(std::atof(iter.first.c_str()) - std::atof(timestamp.c_str())) < 0.001 ) { pose = iter.second; return true; } } return false; } bool Dataset::getPoseFromRGBTimeStamp(string ×tamp, VectorXd &pose){ auto iter = AssociateWithNumber(mmOdomRGBStampToPose, timestamp); // auto iter = mmOdomRGBStampToPose.find(timestamp); if( iter != mmOdomRGBStampToPose.end() ) { pose = iter->second; return true; } else { return false; } } void Dataset::loadGroundTruthToMap(string &path){ std::vector> strMat = readStringFromFile(path.c_str(), 0); int totalPose = strMat.size(); for(int i=0;i strVec = strMat[i]; string timestamp = strVec[0]; VectorXd pose; pose.resize(7); for(int p=1;p<8;p++) pose(p-1) = stod(strVec[p]); mmTimeStampToPose.insert(make_pair(timestamp, pose)); } } void Dataset::LoadAssociationRGBToDepth(string &path) { std::vector> associateMat = readStringFromFile(msAssociatePath.c_str()); map mapRGBToDepth; map mapRGBToDepthImagePath; int associationNum = associateMat.size(); for(int i=0;i lineVec = associateMat[i]; string rgbTS = lineVec[0]; string depthTS = lineVec[2]; mapRGBToDepth.insert(make_pair(rgbTS, depthTS)); mapRGBToDepthImagePath.insert(make_pair(rgbTS, lineVec[3])); } // for every rgb timestamp, find an associated depth timestamp mvIdToDepthTimeStamp.resize(miTotalNum); mvIdToDepthImagePath.resize(miTotalNum); for( int p=0;psecond; } else { mvIdToDepthTimeStamp[p] = ""; // empty stands for null } mvIdToDepthImagePath[p] = mapRGBToDepthImagePath[iter->first]; } } void Dataset::LoadAssociationRGBToGroundtruth(string &path) { std::vector> associateMat = readStringFromFile(msAssociateGroundtruthPath.c_str()); map mapRGBToGt; int associationNum = associateMat.size(); for(int i=0;i lineVec = associateMat[i]; string rgbTS = lineVec[0]; string gtTS = lineVec[2]; // Considering the precision of the timestamps of the result from the associate.py in TUM-RGB-D dataset, // we need to eliminate two zeros in the tails to make the groundtruth and the association have the same precision gtTS = gtTS.substr(0, gtTS.length()-2); mapRGBToGt.insert(make_pair(rgbTS, gtTS)); } // for every rgb timestamp, find an associated groundtruth timestamp mvIdToGroundtruthTimeStamp.resize(miTotalNum); for( int p=0;psecond; } else { mvIdToGroundtruthTimeStamp[p] = ""; // empty stands for null } } } void Dataset::generateIndexIdToRGBTimeStamp(){ // extract the bare name from a full path mvIdToRGBTimeStamp.clear(); for(auto s:mvRGBFileNames) { string bareName = splitFileNameFromFullDir(s, true); mvIdToRGBTimeStamp.push_back(bareName); } } bool Dataset::empty(){ return miCurrentID >= miTotalNum; } int Dataset::getCurrentID(){ return miCurrentID; } int Dataset::getTotalNum() { return miTotalNum; } bool Dataset::loadDetectionDir(string &path) { msDetectionDir = path; mbDetectionLoaded = true; return true; } Eigen::MatrixXd Dataset::getDetectionMat(){ Eigen::MatrixXd detMat; if(!mbDetectionLoaded){ std::cerr << "Detection dir has not loaded yet." << std::endl; return detMat; } // get the RGB timestamp as the name of the object detection file string bairName = mvIdToRGBTimeStamp[miCurrentID-1]; string fullPath = msDetectionDir + bairName + ".txt"; detMat = readDataFromFile(fullPath.c_str()); return detMat; } vector Dataset::generateValidVector(){ vector validVec; for(int i=0; i=miTotalNum) return false; int currentID = id; string depthTimeStampAssociated = mvIdToDepthTimeStamp[currentID]; if(depthTimeStampAssociated == ""){ std::cout << "No depthTimeStampAssociated. " << std::endl; return false; } string gtTimeStamp = mvIdToGroundtruthTimeStamp[currentID]; VectorXd pose; if(getPoseFromTimeStamp(gtTimeStamp, pose)) return true; else { return false; } } bool Dataset::SetOdometry(const string& dir_odom, bool calibrate){ std::vector> strMat = readStringFromFile(dir_odom.c_str(), 0); if(strMat.size() == 0) { std::cerr << " Odometry dir error! Keep gt. " << std::endl; return false; } mmOdomRGBStampToPose.clear(); int totalPose = strMat.size(); for(int i=0;i strVec = strMat[i]; string timestamp = strVec[0]; VectorXd pose; pose.resize(7); for(int p=1;p<8;p++) pose(p-1) = stod(strVec[p]); mmOdomRGBStampToPose.insert(make_pair(timestamp, pose)); } std::cout << "Setting odometry succeeds."; if( calibrate ) { std::cout << "Get calibrate transform... " << std::endl; // find the corresponding groundtruth of the first timestamp of the odometry bool findCalibTrans = false; int transId = 0; for(auto timestampOdomPair: mmOdomRGBStampToPose) { string timestamp_gt = mvIdToGroundtruthTimeStamp[transId]; // assume that all the rgb images have corresponding odometry values assert( mvIdToRGBTimeStamp[transId] == timestampOdomPair.first && "Odom should start from the first rgb frame." ); VectorXd gtPose; if( getPoseFromTimeStamp(timestamp_gt, gtPose) ) { g2o::SE3Quat pose_wc; pose_wc.fromVector(gtPose); g2o::SE3Quat pose_oc; pose_oc.fromVector(timestampOdomPair.second); mTransGtCalibrate = new g2o::SE3Quat; *mTransGtCalibrate = pose_wc * pose_oc.inverse(); findCalibTrans = true; break; } transId ++ ; } if( !findCalibTrans) { std::cerr << "Can't find calibrate transformation... Close calibraton!"<< std::endl; calibrate = false; } else { std::cout << "Find calibration trans ID: " << transId << std::endl; for(auto timestampOdomPair: mmOdomRGBStampToPose) { // calibrate all VectorXd pose = timestampOdomPair.second; VectorXd pose_processed; if(calibrate) pose_processed = calibratePose(pose); else pose_processed = pose; mmOdomRGBStampToPose[timestampOdomPair.first] = pose_processed; } } } mbOdomSet = true; } VectorXd Dataset::calibratePose(VectorXd& pose) { g2o::SE3Quat pose_c; pose_c.fromVector(pose.tail(7)); g2o::SE3Quat pose_w = (*mTransGtCalibrate) * pose_c; return pose_w.toVector(); } void Dataset::SetCurrentID(int id) { if ( id >= miTotalNum ){ std::cout << "Fail. id is larger than totalNum : " << miTotalNum << std::endl; return; } miCurrentID = id; return; } double Dataset::GetCurrentTimestamp() { return GetTimestamp(miCurrentID-1); } double Dataset::GetTimestamp(int id) { return stod(mvIdToRGBTimeStamp[id]); } } ================================================ FILE: src/tum_rgbd/io.h ================================================ // Basic input and output of the TUM-RGBD dataset. #include #include #include #include #include #include #include using namespace std; using namespace cv; using namespace Eigen; namespace TUMRGBD { /* * All the timestamp is based on RGB images. */ class Dataset { public: // Load the next RGB-D frame containing a RGB image, a depth image and a camera pose. // pose: x y z qx qy qz qw bool readFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose); void loadDataset(string &path); std::vector generateValidVector(); // load object detections bool loadDetectionDir(string &path); Eigen::MatrixXd getDetectionMat(); bool empty(); int getCurrentID(); int getTotalNum(); // load a specified frame using its ID bool findFrameUsingID(int id, cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose); // load an odometry data generated from wheels or visual odometry algorithm like ORB-SLAM. // the system will automatically calibrate its coordinate by aligning the pose of the first frame to the corresponding ground truth. bool SetOdometry(const string& dir_odom, bool calibrate = true); // jump to a specified frame using its ID void SetCurrentID(int id); // Get the timestamp of the current frame double GetCurrentTimestamp(); double GetTimestamp(int id); bool getPoseFromTimeStamp(string ×tamp, VectorXd &pose); private: void associateRGBWithGroundtruth(); void loadGroundTruthToMap(string &path); // load the associations from the timestamps of rgb images to the timestamps of depth images void LoadAssociationRGBToDepth(string &path); void LoadAssociationRGBToGroundtruth(string &path); void generateIndexIdToRGBTimeStamp(); bool judgeValid(int id); // the frame with the id is valid when it contains valid depth and rgb images ... bool getPoseFromRGBTimeStamp(string ×tamp, VectorXd &pose); VectorXd calibratePose(VectorXd& pose); // calibrate the odom data by aligning to the first frame of the groundtruth private: string msDatasetDir; // the root directory of the dataset string msRGBDir; string msDepthDir; string msGroundtruthPath; string msAssociatePath; string msAssociateGroundtruthPath; string msDetectionDir; vector mvRGBFileNames; // store the full paths of all the rgb images vector mvPoses; int miCurrentID; int miTotalNum; vector mvIdToGroundtruthTimeStamp; vector mvIdToDepthTimeStamp; vector mvIdToDepthImagePath; vector mvIdToRGBTimeStamp; map mmTimeStampToPose; map mmOdomRGBStampToPose; bool mbDetectionLoaded; bool mbOdomSet; string msOdomDir; g2o::SE3Quat* mTransGtCalibrate; }; } ================================================ FILE: src/utils/dataprocess_utils.cpp ================================================ #include "utils/dataprocess_utils.h" #include #include #include #include #include #include #include #include #include #include // for setprecision #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" using namespace std; using namespace Eigen; typedef Matrix Vector7d; typedef Matrix Vector6d; bool compare_func_stringasdouble(string &s1, string &s2) { string bareS1 = splitFileNameFromFullDir(s1, true); string bareS2 = splitFileNameFromFullDir(s2, true); return stod(bareS1) < stod(bareS2); } void GetFileNamesUnderDir(string path,vector& filenames) { filenames.clear(); DIR *pDir; struct dirent* ptr; if(!(pDir = opendir(path.c_str()))){ cout<<"Folder doesn't Exist!"<d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0){ filenames.push_back(path + "/" + ptr->d_name); } } closedir(pDir); } string splitFileNameFromFullDir(string &s, bool bare) { int pos = s.find_last_of('/'); string name(s.substr(pos+1)); if( bare ) { int posBare = name.find_last_of('.'); string bare_name(name.substr(0, posBare)); return bare_name; } return name; } void sortFileNames(vector& filenames, vector& filenamesSorted) { filenamesSorted = filenames; sort(filenamesSorted.begin(), filenamesSorted.end(), compare_func_stringasdouble); } Eigen::MatrixXd readDataFromFile(const char* fileName, bool dropFirstline){ ifstream fin(fileName); string line; if(dropFirstline) getline(fin, line); // drop this line MatrixXd mat; int line_num = 0; while( getline(fin, line) ) { vector s; boost::split( s, line, boost::is_any_of( " \t," ), boost::token_compress_on ); VectorXd lineVector(s.size()); for (int i=0;i> readStringFromFile(const char* fileName, int dropLines){ ifstream fin(fileName); string line; for(int i=0; i> strMat; while( getline(fin, line) ) { vector s; boost::split( s, line, boost::is_any_of( " \t," ), boost::token_compress_on ); strMat.push_back(s); } fin.close(); return strMat; } // return True if the measure lies on the image border or does not meet the size requirement bool calibrateMeasurement(Vector4d &measure , int rows, int cols, int config_boarder, int config_size){ int x_length = measure[2] - measure[0]; int y_length = measure[3] - measure[1]; if( x_length < config_size || y_length < config_size ){ std::cout << " [small detection " << config_size << "] invalid. " << std::endl; return true; } Vector4d measure_calibrated(-1,-1,-1,-1); Vector4d measure_uncalibrated(-1,-1,-1,-1); int correct_num = 0; if( measure[0]>config_boarder && measure[0]config_boarder && measure[2]config_boarder && measure[1]config_boarder && measure[3] #include #include #include #include #include #include #include #include using namespace Eigen; Matrix4d getTransformFromVector(VectorXd& pose) { if( pose.rows() == 7) { // x y z qx qy qz qw Matrix4d homogeneous_matrix; Quaterniond _r(pose(6),pose(3),pose(4),pose(5)); // w x y z homogeneous_matrix.setIdentity(); homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix(); homogeneous_matrix.col(3).head(3) = pose.head(3); return homogeneous_matrix; } } void addVecToMatirx(Eigen::MatrixXd &mat, Eigen::VectorXd &vec) { assert( mat.cols() == vec.rows() && "the size of vec and mat must match." ); mat.conservativeResize(mat.rows()+1, mat.cols()); mat.row(mat.rows()-1) = vec; } template Eigen::Quaternion zyx_euler_to_quat(const T &roll, const T &pitch, const T &yaw) { T sy = sin(yaw*0.5); T cy = cos(yaw*0.5); T sp = sin(pitch*0.5); T cp = cos(pitch*0.5); T sr = sin(roll*0.5); T cr = cos(roll*0.5); T w = cr*cp*cy + sr*sp*sy; T x = sr*cp*cy - cr*sp*sy; T y = cr*sp*cy + sr*cp*sy; T z = cr*cp*sy - sr*sp*cy; return Eigen::Quaternion(w,x,y,z); } template Eigen::Quaterniond zyx_euler_to_quat(const double&, const double&, const double&); template Eigen::Quaternionf zyx_euler_to_quat(const float&, const float&, const float&); template void quat_to_euler_zyx(const Eigen::Quaternion& q, T& roll, T& pitch, T& yaw) { T qw = q.w(); T qx = q.x(); T qy = q.y(); T qz = q.z(); roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy)); pitch = asin(2*(qw*qy-qz*qx)); yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz)); } template void quat_to_euler_zyx(const Eigen::Quaterniond&, double&, double&, double&); template void quat_to_euler_zyx(const Eigen::Quaternionf&, float&, float&, float&); template void rot_to_euler_zyx(const Eigen::Matrix& R, T& roll, T& pitch, T& yaw) { pitch = asin(-R(2,0)); if (fabs(pitch - M_PI/2.0) < 1.0e-3) { roll = 0.0; yaw = atan2(R(1,2) - R(0,1), R(0,2) + R(1,1)) + roll; } else if (fabs(pitch + M_PI/2.0) < 1.0e-3) { roll = 0.0; yaw = atan2(R(1,2) - R(0,1), R(0,2) + R(1,1)) - roll; } else { roll = atan2(R(2,1), R(2,2)); yaw = atan2(R(1,0), R(0,0)); } } template void rot_to_euler_zyx(const Matrix3d&, double&, double&, double&); template void rot_to_euler_zyx(const Matrix3f&, float&, float&, float&); template Eigen::Matrix euler_zyx_to_rot(const T& roll,const T& pitch,const T& yaw) { T cp = cos(pitch); T sp = sin(pitch); T sr = sin(roll); T cr = cos(roll); T sy = sin(yaw); T cy = cos(yaw); Eigen::Matrix R; R<< cp*cy, (sr*sp*cy)-(cr*sy), (cr*sp*cy)+(sr*sy), cp*sy, (sr*sp*sy)+(cr*cy), (cr*sp*sy)-(sr*cy), -sp, sr*cp, cr * cp; return R; } template Matrix3d euler_zyx_to_rot(const double&, const double&, const double&); template Matrix3f euler_zyx_to_rot(const float&, const float&, const float&); template Eigen::Matrix real_to_homo_coord(const Eigen::Matrix& pts_in) { Eigen::Matrix pts_homo_out; int raw_rows=pts_in.rows(); int raw_cols=pts_in.cols(); pts_homo_out.resize(raw_rows+1,raw_cols); pts_homo_out<::Ones(raw_cols); return pts_homo_out; } template MatrixXd real_to_homo_coord(const MatrixXd&);template MatrixXf real_to_homo_coord(const MatrixXf&); template void real_to_homo_coord(const Eigen::Matrix& pts_in, Eigen::Matrix& pts_homo_out) { int raw_rows=pts_in.rows(); int raw_cols=pts_in.cols(); pts_homo_out.resize(raw_rows+1,raw_cols); pts_homo_out<::Ones(raw_cols); } template void real_to_homo_coord(const MatrixXd&,MatrixXd&);template void real_to_homo_coord(const MatrixXf&,MatrixXf&); template // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function. Eigen::Matrix real_to_homo_coord_vec(const Eigen::Matrix& pts_in) { Eigen::Matrix pts_homo_out; int raw_rows=pts_in.rows();; pts_homo_out.resize(raw_rows+1); pts_homo_out<(const VectorXd&); template VectorXf real_to_homo_coord_vec(const VectorXf&); template Eigen::Matrix homo_to_real_coord(const Eigen::Matrix& pts_homo_in) { Eigen::Matrix pts_out(pts_homo_in.rows()-1,pts_homo_in.cols()); for (int i=0;i(const MatrixXd&);template MatrixXf homo_to_real_coord(const MatrixXf&); template void homo_to_real_coord(const Eigen::Matrix& pts_homo_in, Eigen::Matrix& pts_out) { pts_out.resize(pts_homo_in.rows()-1,pts_homo_in.cols()); for (int i=0;i(const MatrixXd&,MatrixXd&);template void homo_to_real_coord(const MatrixXf&,MatrixXf&); template // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function. Eigen::Matrix homo_to_real_coord_vec(const Eigen::Matrix& pts_homo_in) { Eigen::Matrix pt_out; if (pts_homo_in.rows()==4) pt_out=pts_homo_in.head(3)/pts_homo_in(3); else if (pts_homo_in.rows()==3) pt_out=pts_homo_in.head(2)/pts_homo_in(2); return pt_out; } template VectorXd homo_to_real_coord_vec(const VectorXd&); template VectorXf homo_to_real_coord_vec(const VectorXf&); void fast_RemoveRow(MatrixXd& matrix,int rowToRemove, int& total_line_number) { matrix.row(rowToRemove) = matrix.row(total_line_number-1); total_line_number--; } void vert_stack_m(const MatrixXd &a_in, const MatrixXd &b_in, MatrixXd &combined_out) { assert (a_in.cols() == b_in.cols()); combined_out.resize(a_in.rows()+b_in.rows(),a_in.cols()); combined_out< bool read_all_number_txt(const std::string txt_file_name, Eigen::Matrix& read_number_mat) { if (!std::ifstream(txt_file_name)) { std::cout<<"ERROR!!! Cannot read txt file "<> t) { read_number_mat(row_counter,colu)=t; colu++; } row_counter++; if (row_counter>=read_number_mat.rows()) // if matrix row is not enough, make more space. read_number_mat.conservativeResize(read_number_mat.rows()*2,read_number_mat.cols()); } } filetxt.close(); read_number_mat.conservativeResize(row_counter,read_number_mat.cols()); // cut into actual rows return true; } template bool read_all_number_txt(const std::string, MatrixXd&); template bool read_all_number_txt(const std::string, MatrixXi&); bool read_obj_detection_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& all_strings) { if (!std::ifstream(txt_file_name)) { std::cout<<"ERROR!!! Cannot read txt file "<>classname; all_strings.push_back(classname); int colu=0; while (ss >> t) { read_number_mat(row_counter,colu)=t; colu++; } row_counter++; if (row_counter>=read_number_mat.rows()) // if matrix row is not enough, make more space. read_number_mat.conservativeResize(read_number_mat.rows()*2,read_number_mat.cols()); } } filetxt.close(); read_number_mat.conservativeResize(row_counter,read_number_mat.cols()); // cut into actual rows } bool read_obj_detection2_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& all_strings) { if (!std::ifstream(txt_file_name)) { std::cout<<"ERROR!!! Cannot read txt file "<> t) { read_number_mat(row_counter,colu)=t; colu++; if (colu>read_number_mat.cols()-1) break; } std::string classname; ss>>classname; all_strings.push_back(classname); row_counter++; if (row_counter>=read_number_mat.rows()) // if matrix row is not enough, make more space. read_number_mat.conservativeResize(read_number_mat.rows()*2,read_number_mat.cols()); } } filetxt.close(); read_number_mat.conservativeResize(row_counter,read_number_mat.cols()); // cut into actual rows } void sort_indexes(const Eigen::VectorXd &vec, std::vector& idx, int top_k ) { std::partial_sort(idx.begin(),idx.begin()+top_k, idx.end(), [&vec](int i1, int i2){return vec(i1) < vec(i2);} ); } void sort_indexes(const Eigen::VectorXd &vec, std::vector& idx) { sort(idx.begin(), idx.end(), [&vec](int i1, int i2){return vec(i1) < vec(i2);} ); } template T normalize_to_pi(T angle) { if (angle > M_PI/2.0) return angle-M_PI; // # change to -90 ~90 else if (angle<-M_PI/2.0) return angle+M_PI; else return angle; } template double normalize_to_pi(double); template void print_vector(const std::vector& vec) { for (size_t i=0;i&);template void print_vector(const std::vector&);template void print_vector(const std::vector&); template void linespace(T starting, T ending, T step, std::vector& res) { res.reserve((ending-starting)/step+2); while(starting <= ending) { res.push_back(starting); starting += step; // TODO could recode to better handle rounding errors if (res.size()>1000) { std::cout<<"Linespace too large size!!!!"<&); template void linespace(double, double, double, std::vector&);