[ { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8)\nproject(EllipsoidSLAM)\n\nSET(CMAKE_BUILD_TYPE Release)\n# SET(CMAKE_BUILD_TYPE Debug)\nMESSAGE(\"Build type: \" ${CMAKE_BUILD_TYPE})\n\n# Compile with C14\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++14\")\n\n# set no warnings\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -Wno-deprecated\")\n\nLIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)\n\n# For Opencv, check environment first\nif(NOT DEFINED ENV{OpenCV_DIR})\n message(\"not defined environment variable: OpenCV_DIR\")\n find_package(OpenCV REQUIRED)\n if(NOT OpenCV_FOUND)\n message(FATAL_ERROR \"OpenCV > 2.4.3 not found.\")\n endif()\nelse()\n message(\"Environment variable OpenCV_DIR: \" $ENV{OpenCV_DIR})\n set(OpenCV_DIR $ENV{OpenCV_DIR})\n find_package(OpenCV)\n if(NOT OpenCV_FOUND)\n message(FATAL_ERROR \"OpenCV not found.\")\n endif()\nendif()\n\nfind_package(Eigen3 3.1.0 REQUIRED)\nfind_package(Pangolin REQUIRED)\n\ninclude_directories(\n ${PROJECT_SOURCE_DIR}\n ${PROJECT_SOURCE_DIR}/include\n ${EIGEN3_INCLUDE_DIR}\n ${Pangolin_INCLUDE_DIRS}\n ${OpenCV_INCLUDE_DIRS}\n)\n\nset(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)\n\nadd_library(utils SHARED\n src/utils/matrix_utils.cpp\n src/utils/dataprocess_utils.cpp\n )\ntarget_link_libraries(utils\n${OpenCV_LIBS}\n)\n\nadd_library(${PROJECT_NAME} SHARED\n src/core/Ellipsoid.cpp\n src/core/Map.cpp\n src/core/MapDrawer.cpp\n src/core/Viewer.cpp\n src/core/Initializer.cpp\n src/core/Geometry.cpp\n src/core/System.cpp\n src/core/Tracking.cpp\n src/core/FrameDrawer.cpp\n src/core/Optimizer.cpp\n src/core/Frame.cpp\n src/core/Plane.cpp\n src/core/DataAssociation.cpp\n src/core/BasicEllipsoidEdges.cpp\n )\n\ntarget_link_libraries(${PROJECT_NAME}\n ${OpenCV_LIBS}\n ${EIGEN3_LIBS}\n ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so\n ${Pangolin_LIBRARIES}\n utils\n Config\n symmetry\n EllipsoidExtractor\n PlaneExtractor\n dense_builder\n Polygon\n )\n\nFIND_PACKAGE( PCL REQUIRED )\nlist(REMOVE_ITEM PCL_LIBRARIES \"vtkproj4\") # use this in Ubuntu 16.04\n\nADD_DEFINITIONS( ${PCL_DEFINITIONS} )\nINCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS} )\nLINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )\n\n# add modules\nadd_subdirectory(src/tum_rgbd)\nadd_subdirectory(src/dense_builder)\nadd_subdirectory(src/symmetry)\nadd_subdirectory(src/config)\nadd_subdirectory(src/pca)\nadd_subdirectory(src/plane)\nadd_subdirectory(src/Polygon)\n\n# interface\nadd_executable(rgbd\n ./Example/interface/rgbd.cpp\n)\ntarget_link_libraries(rgbd\n tum_rgbd\n boost_system\n EllipsoidSLAM\n ${PCL_LIBRARIES}\n) \n" }, { "path": "Example/dataset/cabinet/associate.py", "content": "#!/usr/bin/python\n# Software License Agreement (BSD License)\n#\n# Copyright (c) 2013, Juergen Sturm, TUM\n# All rights reserved.\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions\n# are met:\n#\n# * Redistributions of source code must retain the above copyright\n# notice, this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above\n# copyright notice, this list of conditions and the following\n# disclaimer in the documentation and/or other materials provided\n# with the distribution.\n# * Neither the name of TUM nor the names of its\n# contributors may be used to endorse or promote products derived\n# from this software without specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n# \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Requirements: \n# sudo apt-get install python-argparse\n\n\"\"\"\nThe 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.\n\nFor 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.\n\"\"\"\n\nimport argparse\nimport sys\nimport os\nimport numpy\n\n\ndef read_file_list(filename):\n \"\"\"\n Reads a trajectory from a text file. \n \n File format:\n The file format is \"stamp d1 d2 d3 ...\", where stamp denotes the time stamp (to be matched)\n and \"d1 d2 d3..\" is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. \n \n Input:\n filename -- File name\n \n Output:\n dict -- dictionary of (stamp,data) tuples\n \n \"\"\"\n file = open(filename)\n data = file.read()\n lines = data.replace(\",\",\" \").replace(\"\\t\",\" \").split(\"\\n\") \n list = [[v.strip() for v in line.split(\" \") if v.strip()!=\"\"] for line in lines if len(line)>0 and line[0]!=\"#\"]\n list = [(float(l[0]),l[1:]) for l in list if len(l)>1]\n return dict(list)\n\ndef associate(first_list, second_list,offset,max_difference):\n \"\"\"\n Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim \n to find the closest match for every input tuple.\n \n Input:\n first_list -- first dictionary of (stamp,data) tuples\n second_list -- second dictionary of (stamp,data) tuples\n offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)\n max_difference -- search radius for candidate generation\n\n Output:\n matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))\n \n \"\"\"\n first_keys = first_list.keys()\n second_keys = second_list.keys()\n potential_matches = [(abs(a - (b + offset)), a, b) \n for a in first_keys \n for b in second_keys \n if abs(a - (b + offset)) < max_difference]\n potential_matches.sort()\n matches = []\n for diff, a, b in potential_matches:\n if a in first_keys and b in second_keys:\n first_keys.remove(a)\n second_keys.remove(b)\n matches.append((a, b))\n \n matches.sort()\n return matches\n\nif __name__ == '__main__':\n \n # parse command line\n parser = argparse.ArgumentParser(description='''\n This script takes two data files with timestamps and associates them \n ''')\n parser.add_argument('first_file', help='first text file (format: timestamp data)')\n parser.add_argument('second_file', help='second text file (format: timestamp data)')\n parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')\n parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)\n parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)\n args = parser.parse_args()\n\n first_list = read_file_list(args.first_file)\n second_list = read_file_list(args.second_file)\n\n matches = associate(first_list, second_list,float(args.offset),float(args.max_difference)) \n\n if args.first_only:\n for a,b in matches:\n print(\"%f %s\"%(a,\" \".join(first_list[a])))\n else:\n for a,b in matches:\n print(\"%f %s %f %s\"%(a,\" \".join(first_list[a]),b-float(args.offset),\" \".join(second_list[b])))\n \n \n" }, { "path": "Example/dataset/cabinet/associate.txt", "content": "1341841278.842700 rgb/1341841278.8427.jpg 1341841278.842700 depth/1341841278.8427.png\n1341841279.510700 rgb/1341841279.5107.jpg 1341841279.510700 depth/1341841279.5107.png\n1341841280.182500 rgb/1341841280.1825.jpg 1341841280.182500 depth/1341841280.1825.png\n1341841280.850700 rgb/1341841280.8507.jpg 1341841280.850700 depth/1341841280.8507.png\n1341841281.554600 rgb/1341841281.5546.jpg 1341841281.554600 depth/1341841281.5546.png\n1341841282.226500 rgb/1341841282.2265.jpg 1341841282.226500 depth/1341841282.2265.png\n1341841282.898700 rgb/1341841282.8987.jpg 1341841282.898700 depth/1341841282.8987.png\n1341841283.566500 rgb/1341841283.5665.jpg 1341841283.566500 depth/1341841283.5665.png\n1341841284.238800 rgb/1341841284.2388.jpg 1341841284.238800 depth/1341841284.2388.png\n1341841284.906700 rgb/1341841284.9067.jpg 1341841284.906700 depth/1341841284.9067.png\n1341841285.612400 rgb/1341841285.6124.jpg 1341841285.612400 depth/1341841285.6124.png\n1341841286.282700 rgb/1341841286.2827.jpg 1341841286.282700 depth/1341841286.2827.png\n1341841286.950600 rgb/1341841286.9506.jpg 1341841286.950600 depth/1341841286.9506.png\n1341841287.622700 rgb/1341841287.6227.jpg 1341841287.622700 depth/1341841287.6227.png\n1341841288.294600 rgb/1341841288.2946.jpg 1341841288.294600 depth/1341841288.2946.png\n1341841288.962700 rgb/1341841288.9627.jpg 1341841288.962700 depth/1341841288.9627.png\n1341841289.634600 rgb/1341841289.6346.jpg 1341841289.634600 depth/1341841289.6346.png\n1341841290.303000 rgb/1341841290.303.jpg 1341841290.303000 depth/1341841290.303.png\n1341841290.974500 rgb/1341841290.9745.jpg 1341841290.974500 depth/1341841290.9745.png\n1341841291.642600 rgb/1341841291.6426.jpg 1341841291.642600 depth/1341841291.6426.png\n1341841292.314700 rgb/1341841292.3147.jpg 1341841292.314700 depth/1341841292.3147.png\n1341841292.986600 rgb/1341841292.9866.jpg 1341841292.986600 depth/1341841292.9866.png\n1341841293.654800 rgb/1341841293.6548.jpg 1341841293.654800 depth/1341841293.6548.png\n1341841294.326600 rgb/1341841294.3266.jpg 1341841294.326600 depth/1341841294.3266.png\n1341841294.995200 rgb/1341841294.9952.jpg 1341841294.995200 depth/1341841294.9952.png\n1341841295.666600 rgb/1341841295.6666.jpg 1341841295.666600 depth/1341841295.6666.png\n1341841296.334800 rgb/1341841296.3348.jpg 1341841296.334800 depth/1341841296.3348.png\n1341841297.006500 rgb/1341841297.0065.jpg 1341841297.006500 depth/1341841297.0065.png\n1341841297.678600 rgb/1341841297.6786.jpg 1341841297.678600 depth/1341841297.6786.png\n1341841298.346600 rgb/1341841298.3466.jpg 1341841298.346600 depth/1341841298.3466.png\n1341841299.018500 rgb/1341841299.0185.jpg 1341841299.018500 depth/1341841299.0185.png\n1341841299.686600 rgb/1341841299.6866.jpg 1341841299.686600 depth/1341841299.6866.png\n1341841300.365600 rgb/1341841300.3656.jpg 1341841300.365600 depth/1341841300.3656.png\n1341841301.033600 rgb/1341841301.0336.jpg 1341841301.033600 depth/1341841301.0336.png\n1341841301.705700 rgb/1341841301.7057.jpg 1341841301.705700 depth/1341841301.7057.png\n1341841302.377600 rgb/1341841302.3776.jpg 1341841302.377600 depth/1341841302.3776.png\n1341841303.045600 rgb/1341841303.0456.jpg 1341841303.045600 depth/1341841303.0456.png\n1341841303.710600 rgb/1341841303.7106.jpg 1341841303.710600 depth/1341841303.7106.png\n1341841304.378700 rgb/1341841304.3787.jpg 1341841304.378700 depth/1341841304.3787.png\n1341841305.050600 rgb/1341841305.0506.jpg 1341841305.050600 depth/1341841305.0506.png\n1341841305.722700 rgb/1341841305.7227.jpg 1341841305.722700 depth/1341841305.7227.png\n1341841306.390700 rgb/1341841306.3907.jpg 1341841306.390700 depth/1341841306.3907.png\n1341841307.062700 rgb/1341841307.0627.jpg 1341841307.062700 depth/1341841307.0627.png\n1341841307.730600 rgb/1341841307.7306.jpg 1341841307.730600 depth/1341841307.7306.png\n1341841308.409900 rgb/1341841308.4099.jpg 1341841308.409900 depth/1341841308.4099.png\n1341841309.109500 rgb/1341841309.1095.jpg 1341841309.109500 depth/1341841309.1095.png\n1341841309.781600 rgb/1341841309.7816.jpg 1341841309.781600 depth/1341841309.7816.png\n1341841310.446800 rgb/1341841310.4468.jpg 1341841310.446800 depth/1341841310.4468.png\n1341841311.150700 rgb/1341841311.1507.jpg 1341841311.150700 depth/1341841311.1507.png\n1341841311.818600 rgb/1341841311.8186.jpg 1341841311.818600 depth/1341841311.8186.png\n1341841312.490500 rgb/1341841312.4905.jpg 1341841312.490500 depth/1341841312.4905.png\n1341841313.169600 rgb/1341841313.1696.jpg 1341841313.169600 depth/1341841313.1696.png\n1341841313.869500 rgb/1341841313.8695.jpg 1341841313.869500 depth/1341841313.8695.png\n1341841314.570600 rgb/1341841314.5706.jpg 1341841314.570600 depth/1341841314.5706.png\n1341841315.245600 rgb/1341841315.2456.jpg 1341841315.245600 depth/1341841315.2456.png\n1341841315.910800 rgb/1341841315.9108.jpg 1341841315.910800 depth/1341841315.9108.png\n1341841316.578600 rgb/1341841316.5786.jpg 1341841316.578600 depth/1341841316.5786.png\n1341841317.250600 rgb/1341841317.2506.jpg 1341841317.250600 depth/1341841317.2506.png\n" }, { "path": "Example/dataset/cabinet/associateGroundtruth.txt", "content": "1341841278.842700 rgb/1341841278.8427.jpg 1341841278.842700 -2.5508 0.9872 1.1019 -0.4871 0.7673 -0.3519 0.2239\n1341841279.510700 rgb/1341841279.5107.jpg 1341841279.510700 -2.4691 1.0757 1.0996 -0.4454 0.7908 -0.3627 0.2114\n1341841280.182500 rgb/1341841280.1825.jpg 1341841280.182500 -2.3319 1.1799 1.1107 -0.4077 0.8121 -0.3749 0.1837\n1341841280.850700 rgb/1341841280.8507.jpg 1341841280.850700 -2.1921 1.3039 1.112 -0.3546 0.8399 -0.3802 0.1557\n1341841281.554600 rgb/1341841281.5546.jpg 1341841281.554600 -2.0579 1.405 1.0916 -0.2848 0.8704 -0.3827 0.1221\n1341841282.226500 rgb/1341841282.2265.jpg 1341841282.226500 -1.8585 1.4244 1.0901 -0.2096 0.8898 -0.3963 0.0858\n1341841282.898700 rgb/1341841282.8987.jpg 1341841282.898700 -1.6681 1.4397 1.0889 -0.1669 0.8981 -0.4036 0.0515\n1341841283.566500 rgb/1341841283.5665.jpg 1341841283.566500 -1.4768 1.494 1.0823 -0.0381 0.9119 -0.408 -0.024\n1341841284.238800 rgb/1341841284.2388.jpg 1341841284.238800 -1.3308 1.5641 1.0771 -0.0058 -0.9108 0.4085 0.0598\n1341841284.906700 rgb/1341841284.9067.jpg 1341841284.906700 -1.1279 1.5557 1.0799 -0.0682 -0.913 0.3959 0.0705\n1341841285.612400 rgb/1341841285.6124.jpg 1341841285.612400 -0.9343 1.4993 1.0839 -0.1445 -0.897 0.4038 0.107\n1341841286.282700 rgb/1341841286.2827.jpg 1341841286.282700 -0.7291 1.4183 1.0731 -0.1904 -0.8912 0.3889 0.1349\n1341841286.950600 rgb/1341841286.9506.jpg 1341841286.950600 -0.6052 1.3015 1.0644 -0.2859 -0.8628 0.375 0.1821\n1341841287.622700 rgb/1341841287.6227.jpg 1341841287.622700 -0.5046 1.1646 1.0717 -0.3173 -0.8491 0.3709 0.2018\n1341841288.294600 rgb/1341841288.2946.jpg 1341841288.294600 -0.371 1.0006 1.0469 -0.3852 -0.8263 0.3359 0.2366\n1341841288.962700 rgb/1341841288.9627.jpg 1341841288.962700 -0.2798 0.8379 1.041 -0.4218 -0.801 0.3441 0.2491\n1341841289.634600 rgb/1341841289.6346.jpg 1341841289.634600 -0.244 0.6645 1.037 -0.4767 -0.7642 0.339 0.2717\n1341841290.303000 rgb/1341841290.303.jpg 1341841290.303000 -0.2237 0.5276 1.0291 -0.5267 -0.7302 0.3317 0.2818\n1341841290.974500 rgb/1341841290.9745.jpg 1341841290.974500 -0.1652 0.3855 0.992 -0.5928 -0.6755 0.2919 0.3272\n1341841291.642600 rgb/1341841291.6426.jpg 1341841291.642600 -0.1583 0.2306 0.9804 -0.5968 -0.6637 0.304 0.333\n1341841292.314700 rgb/1341841292.3147.jpg 1341841292.314700 -0.2461 0.138 0.9661 -0.6592 -0.5475 0.3345 0.3922\n1341841292.986600 rgb/1341841292.9866.jpg 1341841292.986600 -0.2727 0.0571 0.9256 -0.6528 -0.5484 0.304 0.425\n1341841293.654800 rgb/1341841293.6548.jpg 1341841293.654800 -0.2643 -0.0883 0.8924 -0.6995 -0.4827 0.2912 0.4392\n1341841294.326600 rgb/1341841294.3266.jpg 1341841294.326600 -0.2652 -0.1506 0.9245 -0.7059 -0.5037 0.2472 0.4323\n1341841294.995200 rgb/1341841294.9952.jpg 1341841294.995200 -0.3045 -0.2513 1.0231 -0.7576 -0.4739 0.1949 0.4043\n1341841295.666600 rgb/1341841295.6666.jpg 1341841295.666600 -0.3214 -0.3483 1.0935 -0.7881 -0.4131 0.205 0.4077\n1341841296.334800 rgb/1341841296.3348.jpg 1341841296.334800 -0.355 -0.5434 1.1132 -0.8137 -0.3519 0.2025 0.4159\n1341841297.006500 rgb/1341841297.0065.jpg 1341841297.006500 -0.4216 -0.632 1.0842 -0.8338 -0.3215 0.1805 0.4109\n1341841297.678600 rgb/1341841297.6786.jpg 1341841297.678600 -0.5709 -0.7039 1.0905 -0.853 -0.2736 0.1487 0.4189\n1341841298.346600 rgb/1341841298.3466.jpg 1341841298.346600 -0.6784 -0.7764 1.0892 -0.8701 -0.2298 0.1137 0.421\n1341841299.018500 rgb/1341841299.0185.jpg 1341841299.018500 -0.8697 -0.8985 1.0975 -0.8758 -0.1786 0.1105 0.4346\n1341841299.686600 rgb/1341841299.6866.jpg 1341841299.686600 -1.0402 -0.9193 1.0879 -0.8837 -0.1347 0.0856 0.44\n1341841300.365600 rgb/1341841300.3656.jpg 1341841300.365600 -1.2054 -0.9138 1.0942 -0.8959 -0.0775 0.0593 0.4334\n1341841301.033600 rgb/1341841301.0336.jpg 1341841301.033600 -1.3841 -0.9139 1.1004 -0.9012 -0.034 0.0297 0.431\n1341841301.705700 rgb/1341841301.7057.jpg 1341841301.705700 -1.5678 -0.8897 1.09 -0.9056 0.0124 0.0063 0.4239\n1341841302.377600 rgb/1341841302.3776.jpg 1341841302.377600 -1.7028 -0.7873 1.097 -0.8994 0.0622 -0.0177 0.4323\n1341841303.045600 rgb/1341841303.0456.jpg 1341841303.045600 -1.8151 -0.7055 1.0994 -0.8989 0.1032 -0.0413 0.4238\n1341841303.710600 rgb/1341841303.7106.jpg 1341841303.710600 -1.98 -0.6541 1.1132 -0.8913 0.16 -0.067 0.4189\n1341841304.378700 rgb/1341841304.3787.jpg 1341841304.378700 -2.1474 -0.6164 1.1051 -0.8818 0.2145 -0.0843 0.4114\n1341841305.050600 rgb/1341841305.0506.jpg 1341841305.050600 -2.2441 -0.5275 1.1066 -0.8624 0.2864 -0.0937 0.4067\n1341841305.722700 rgb/1341841305.7227.jpg 1341841305.722700 -2.3441 -0.4434 1.1156 -0.8676 0.2986 -0.1047 0.3837\n1341841306.390700 rgb/1341841306.3907.jpg 1341841306.390700 -2.3844 -0.3738 1.1478 -0.8363 0.3529 -0.154 0.3903\n1341841307.062700 rgb/1341841307.0627.jpg 1341841307.062700 -2.3806 -0.3684 1.1422 -0.8237 0.4174 -0.1444 0.3556\n1341841307.730600 rgb/1341841307.7306.jpg 1341841307.730600 -2.4697 -0.2827 1.1886 -0.8146 0.3928 -0.1827 0.3856\n1341841308.409900 rgb/1341841308.4099.jpg 1341841308.409900 -2.5874 -0.1709 1.1961 -0.8269 0.3908 -0.1601 0.3712\n1341841309.109500 rgb/1341841309.1095.jpg 1341841309.109500 -2.6213 -0.0986 1.2251 -0.8072 0.4179 -0.1686 0.3812\n1341841309.781600 rgb/1341841309.7816.jpg 1341841309.781600 -2.6543 -0.0312 1.2287 -0.7813 0.4828 -0.1931 0.3452\n1341841310.446800 rgb/1341841310.4468.jpg 1341841310.446800 -2.7015 0.0672 1.2179 -0.7367 0.5665 -0.2032 0.3084\n1341841311.150700 rgb/1341841311.1507.jpg 1341841311.150700 -2.7779 0.1718 1.2415 -0.7324 0.5586 -0.2273 0.316\n1341841311.818600 rgb/1341841311.8186.jpg 1341841311.818600 -2.7911 0.2175 1.2397 -0.7031 0.5976 -0.2545 0.2893\n1341841312.490500 rgb/1341841312.4905.jpg 1341841312.490500 -2.8112 0.3029 1.2555 -0.6862 0.6212 -0.2554 0.2794\n1341841313.169600 rgb/1341841313.1696.jpg 1341841313.169600 -2.8302 0.4497 1.2677 -0.6376 0.6605 -0.2868 0.2738\n1341841313.869500 rgb/1341841313.8695.jpg 1341841313.869500 -2.8575 0.5893 1.2733 -0.6207 0.6707 -0.3104 0.2617\n1341841314.570600 rgb/1341841314.5706.jpg 1341841314.570600 -2.8391 0.6635 1.2827 -0.5653 0.7276 -0.3093 0.2352\n1341841315.245600 rgb/1341841315.2456.jpg 1341841315.245600 -2.8206 0.7592 1.2867 -0.5539 0.7424 -0.3032 0.2237\n1341841315.910800 rgb/1341841315.9108.jpg 1341841315.910800 -2.7594 0.9179 1.3087 -0.5035 0.7696 -0.3156 0.2336\n1341841316.578600 rgb/1341841316.5786.jpg 1341841316.578600 -2.7223 1.0345 1.3228 -0.4835 0.7802 -0.3255 0.2271\n1341841317.250600 rgb/1341841317.2506.jpg 1341841317.250600 -2.7302 1.051 1.3149 -0.4803 0.7867 -0.332 0.2003\n" }, { "path": "Example/dataset/cabinet/bbox/1341841278.8427.txt", "content": "0 175 24 560 397 28 0.42 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841279.5107.txt", "content": "0 201 40 542 426 28 0.32 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841280.1825.txt", "content": "0 208 54 559 425 28 0.54 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841280.8507.txt", "content": "0 182 56 587 420 28 0.54 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841281.5546.txt", "content": "0 180 41 585 388 28 0.57 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841282.2265.txt", "content": "0 173 57 569 418 28 0.6 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841282.8987.txt", "content": "0 170 55 603 425 28 0.44 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841283.5665.txt", "content": "0 127 52 585 379 28 0.16 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841284.2388.txt", "content": "0 156 49 551 348 28 0.23 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841284.9067.txt", "content": "0 218 56 557 326 28 0.18 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841285.6124.txt", "content": "0 204 62 576 341 28 0.29 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841286.2827.txt", "content": "0 231 51 594 341 28 0.52 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841286.9506.txt", "content": "0 172 40 537 348 28 0.28 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841287.6227.txt", "content": "0 206 49 553 378 28 0.48 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841288.2946.txt", "content": "0 188 28 574 359 28 0.45 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841288.9627.txt", "content": "0 236 40 605 376 28 0.51 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841289.6346.txt", "content": "0 273 57 551 362 28 0.52 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841290.303.txt", "content": "0 291 62 535 352 28 0.28 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841290.9745.txt", "content": "0 260 52 493 344 28 0.58 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841291.6426.txt", "content": "0 312 59 558 351 28 0.33 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841292.3147.txt", "content": "" }, { "path": "Example/dataset/cabinet/bbox/1341841292.9866.txt", "content": "" }, { "path": "Example/dataset/cabinet/bbox/1341841293.6548.txt", "content": "" }, { "path": "Example/dataset/cabinet/bbox/1341841294.3266.txt", "content": "" }, { "path": "Example/dataset/cabinet/bbox/1341841294.9952.txt", "content": "0 168 30 490 359 28 0.28 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841295.6666.txt", "content": "0 162 49 469 371 28 0.31 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841296.3348.txt", "content": "0 173 51 488 368 28 0.42 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841297.0065.txt", "content": "0 164 33 484 326 28 0.25 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841297.6786.txt", "content": "0 142 28 465 336 28 0.48 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841298.3466.txt", "content": "0 109 40 453 321 28 0.36 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841299.0185.txt", "content": "0 151 50 466 313 28 0.28 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841299.6866.txt", "content": "0 153 74 466 317 28 0.28 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841300.3656.txt", "content": "0 131 84 457 309 28 0.23 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841301.0336.txt", "content": "" }, { "path": "Example/dataset/cabinet/bbox/1341841301.7057.txt", "content": "0 151 55 524 295 28 0.2 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841302.3776.txt", "content": "" }, { "path": "Example/dataset/cabinet/bbox/1341841303.0456.txt", "content": "0 155 67 522 347 28 0.24 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841303.7106.txt", "content": "0 163 61 547 364 28 0.39 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841304.3787.txt", "content": "0 175 45 551 368 28 0.61 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841305.0506.txt", "content": "0 131 46 541 365 28 0.7 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841305.7227.txt", "content": "0 185 11 579 347 28 0.72 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841306.3907.txt", "content": "0 132 26 516 402 28 0.69 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841307.0627.txt", "content": "0 85 1 465 359 28 0.63 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841307.7306.txt", "content": "0 144 47 477 429 28 0.53 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841308.4099.txt", "content": "0 200 34 571 387 28 0.4 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841309.1095.txt", "content": "0 221 45 585 421 28 0.29 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841309.7816.txt", "content": "0 159 21 512 399 28 0.33 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841310.4468.txt", "content": "0 122 12 440 335 28 0.38 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841311.1507.txt", "content": "0 202 36 458 357 28 0.25 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841311.8186.txt", "content": "0 148 25 402 329 28 0.21 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841312.4905.txt", "content": "0 157 33 410 321 28 0.25 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841313.1696.txt", "content": "0 193 40 408 358 28 0.22 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841313.8695.txt", "content": "0 215 47 425 356 28 0.18 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841314.5706.txt", "content": "" }, { "path": "Example/dataset/cabinet/bbox/1341841315.2456.txt", "content": "0 204 27 420 322 28 0.18 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841315.9108.txt", "content": "0 192 26 482 383 28 0.5 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841316.5786.txt", "content": "0 212 30 497 391 28 0.33 0\n" }, { "path": "Example/dataset/cabinet/bbox/1341841317.2506.txt", "content": "0 185 7 473 382 28 0.34 0\n" }, { "path": "Example/dataset/cabinet/depth.txt", "content": "1341841278.8427 depth/1341841278.8427.png\n1341841279.5107 depth/1341841279.5107.png\n1341841280.1825 depth/1341841280.1825.png\n1341841280.8507 depth/1341841280.8507.png\n1341841281.5546 depth/1341841281.5546.png\n1341841282.2265 depth/1341841282.2265.png\n1341841282.8987 depth/1341841282.8987.png\n1341841283.5665 depth/1341841283.5665.png\n1341841284.2388 depth/1341841284.2388.png\n1341841284.9067 depth/1341841284.9067.png\n1341841285.6124 depth/1341841285.6124.png\n1341841286.2827 depth/1341841286.2827.png\n1341841286.9506 depth/1341841286.9506.png\n1341841287.6227 depth/1341841287.6227.png\n1341841288.2946 depth/1341841288.2946.png\n1341841288.9627 depth/1341841288.9627.png\n1341841289.6346 depth/1341841289.6346.png\n1341841290.303 depth/1341841290.303.png\n1341841290.9745 depth/1341841290.9745.png\n1341841291.6426 depth/1341841291.6426.png\n1341841292.3147 depth/1341841292.3147.png\n1341841292.9866 depth/1341841292.9866.png\n1341841293.6548 depth/1341841293.6548.png\n1341841294.3266 depth/1341841294.3266.png\n1341841294.9952 depth/1341841294.9952.png\n1341841295.6666 depth/1341841295.6666.png\n1341841296.3348 depth/1341841296.3348.png\n1341841297.0065 depth/1341841297.0065.png\n1341841297.6786 depth/1341841297.6786.png\n1341841298.3466 depth/1341841298.3466.png\n1341841299.0185 depth/1341841299.0185.png\n1341841299.6866 depth/1341841299.6866.png\n1341841300.3656 depth/1341841300.3656.png\n1341841301.0336 depth/1341841301.0336.png\n1341841301.7057 depth/1341841301.7057.png\n1341841302.3776 depth/1341841302.3776.png\n1341841303.0456 depth/1341841303.0456.png\n1341841303.7106 depth/1341841303.7106.png\n1341841304.3787 depth/1341841304.3787.png\n1341841305.0506 depth/1341841305.0506.png\n1341841305.7227 depth/1341841305.7227.png\n1341841306.3907 depth/1341841306.3907.png\n1341841307.0627 depth/1341841307.0627.png\n1341841307.7306 depth/1341841307.7306.png\n1341841308.4099 depth/1341841308.4099.png\n1341841309.1095 depth/1341841309.1095.png\n1341841309.7816 depth/1341841309.7816.png\n1341841310.4468 depth/1341841310.4468.png\n1341841311.1507 depth/1341841311.1507.png\n1341841311.8186 depth/1341841311.8186.png\n1341841312.4905 depth/1341841312.4905.png\n1341841313.1696 depth/1341841313.1696.png\n1341841313.8695 depth/1341841313.8695.png\n1341841314.5706 depth/1341841314.5706.png\n1341841315.2456 depth/1341841315.2456.png\n1341841315.9108 depth/1341841315.9108.png\n1341841316.5786 depth/1341841316.5786.png\n1341841317.2506 depth/1341841317.2506.png\n" }, { "path": "Example/dataset/cabinet/groundtruth.txt", "content": "1341841278.8427 -2.5508 0.9872 1.1019 -0.4871 0.7673 -0.3519 0.2239\n1341841279.5107 -2.4691 1.0757 1.0996 -0.4454 0.7908 -0.3627 0.2114\n1341841280.1825 -2.3319 1.1799 1.1107 -0.4077 0.8121 -0.3749 0.1837\n1341841280.8507 -2.1921 1.3039 1.112 -0.3546 0.8399 -0.3802 0.1557\n1341841281.5546 -2.0579 1.405 1.0916 -0.2848 0.8704 -0.3827 0.1221\n1341841282.2265 -1.8585 1.4244 1.0901 -0.2096 0.8898 -0.3963 0.0858\n1341841282.8987 -1.6681 1.4397 1.0889 -0.1669 0.8981 -0.4036 0.0515\n1341841283.5665 -1.4768 1.494 1.0823 -0.0381 0.9119 -0.408 -0.024\n1341841284.2388 -1.3308 1.5641 1.0771 -0.0058 -0.9108 0.4085 0.0598\n1341841284.9067 -1.1279 1.5557 1.0799 -0.0682 -0.913 0.3959 0.0705\n1341841285.6124 -0.9343 1.4993 1.0839 -0.1445 -0.897 0.4038 0.107\n1341841286.2827 -0.7291 1.4183 1.0731 -0.1904 -0.8912 0.3889 0.1349\n1341841286.9506 -0.6052 1.3015 1.0644 -0.2859 -0.8628 0.375 0.1821\n1341841287.6227 -0.5046 1.1646 1.0717 -0.3173 -0.8491 0.3709 0.2018\n1341841288.2946 -0.371 1.0006 1.0469 -0.3852 -0.8263 0.3359 0.2366\n1341841288.9627 -0.2798 0.8379 1.041 -0.4218 -0.801 0.3441 0.2491\n1341841289.6346 -0.244 0.6645 1.037 -0.4767 -0.7642 0.339 0.2717\n1341841290.303 -0.2237 0.5276 1.0291 -0.5267 -0.7302 0.3317 0.2818\n1341841290.9745 -0.1652 0.3855 0.992 -0.5928 -0.6755 0.2919 0.3272\n1341841291.6426 -0.1583 0.2306 0.9804 -0.5968 -0.6637 0.304 0.333\n1341841292.3147 -0.2461 0.138 0.9661 -0.6592 -0.5475 0.3345 0.3922\n1341841292.9866 -0.2727 0.0571 0.9256 -0.6528 -0.5484 0.304 0.425\n1341841293.6548 -0.2643 -0.0883 0.8924 -0.6995 -0.4827 0.2912 0.4392\n1341841294.3266 -0.2652 -0.1506 0.9245 -0.7059 -0.5037 0.2472 0.4323\n1341841294.9952 -0.3045 -0.2513 1.0231 -0.7576 -0.4739 0.1949 0.4043\n1341841295.6666 -0.3214 -0.3483 1.0935 -0.7881 -0.4131 0.205 0.4077\n1341841296.3348 -0.355 -0.5434 1.1132 -0.8137 -0.3519 0.2025 0.4159\n1341841297.0065 -0.4216 -0.632 1.0842 -0.8338 -0.3215 0.1805 0.4109\n1341841297.6786 -0.5709 -0.7039 1.0905 -0.853 -0.2736 0.1487 0.4189\n1341841298.3466 -0.6784 -0.7764 1.0892 -0.8701 -0.2298 0.1137 0.421\n1341841299.0185 -0.8697 -0.8985 1.0975 -0.8758 -0.1786 0.1105 0.4346\n1341841299.6866 -1.0402 -0.9193 1.0879 -0.8837 -0.1347 0.0856 0.44\n1341841300.3656 -1.2054 -0.9138 1.0942 -0.8959 -0.0775 0.0593 0.4334\n1341841301.0336 -1.3841 -0.9139 1.1004 -0.9012 -0.034 0.0297 0.431\n1341841301.7057 -1.5678 -0.8897 1.09 -0.9056 0.0124 0.0063 0.4239\n1341841302.3776 -1.7028 -0.7873 1.097 -0.8994 0.0622 -0.0177 0.4323\n1341841303.0456 -1.8151 -0.7055 1.0994 -0.8989 0.1032 -0.0413 0.4238\n1341841303.7106 -1.98 -0.6541 1.1132 -0.8913 0.16 -0.067 0.4189\n1341841304.3787 -2.1474 -0.6164 1.1051 -0.8818 0.2145 -0.0843 0.4114\n1341841305.0506 -2.2441 -0.5275 1.1066 -0.8624 0.2864 -0.0937 0.4067\n1341841305.7227 -2.3441 -0.4434 1.1156 -0.8676 0.2986 -0.1047 0.3837\n1341841306.3907 -2.3844 -0.3738 1.1478 -0.8363 0.3529 -0.154 0.3903\n1341841307.0627 -2.3806 -0.3684 1.1422 -0.8237 0.4174 -0.1444 0.3556\n1341841307.7306 -2.4697 -0.2827 1.1886 -0.8146 0.3928 -0.1827 0.3856\n1341841308.4099 -2.5874 -0.1709 1.1961 -0.8269 0.3908 -0.1601 0.3712\n1341841309.1095 -2.6213 -0.0986 1.2251 -0.8072 0.4179 -0.1686 0.3812\n1341841309.7816 -2.6543 -0.0312 1.2287 -0.7813 0.4828 -0.1931 0.3452\n1341841310.4468 -2.7015 0.0672 1.2179 -0.7367 0.5665 -0.2032 0.3084\n1341841311.1507 -2.7779 0.1718 1.2415 -0.7324 0.5586 -0.2273 0.316\n1341841311.8186 -2.7911 0.2175 1.2397 -0.7031 0.5976 -0.2545 0.2893\n1341841312.4905 -2.8112 0.3029 1.2555 -0.6862 0.6212 -0.2554 0.2794\n1341841313.1696 -2.8302 0.4497 1.2677 -0.6376 0.6605 -0.2868 0.2738\n1341841313.8695 -2.8575 0.5893 1.2733 -0.6207 0.6707 -0.3104 0.2617\n1341841314.5706 -2.8391 0.6635 1.2827 -0.5653 0.7276 -0.3093 0.2352\n1341841315.2456 -2.8206 0.7592 1.2867 -0.5539 0.7424 -0.3032 0.2237\n1341841315.9108 -2.7594 0.9179 1.3087 -0.5035 0.7696 -0.3156 0.2336\n1341841316.5786 -2.7223 1.0345 1.3228 -0.4835 0.7802 -0.3255 0.2271\n1341841317.2506 -2.7302 1.051 1.3149 -0.4803 0.7867 -0.332 0.2003\n" }, { "path": "Example/dataset/cabinet/rgb.txt", "content": "1341841278.8427 rgb/1341841278.8427.jpg\n1341841279.5107 rgb/1341841279.5107.jpg\n1341841280.1825 rgb/1341841280.1825.jpg\n1341841280.8507 rgb/1341841280.8507.jpg\n1341841281.5546 rgb/1341841281.5546.jpg\n1341841282.2265 rgb/1341841282.2265.jpg\n1341841282.8987 rgb/1341841282.8987.jpg\n1341841283.5665 rgb/1341841283.5665.jpg\n1341841284.2388 rgb/1341841284.2388.jpg\n1341841284.9067 rgb/1341841284.9067.jpg\n1341841285.6124 rgb/1341841285.6124.jpg\n1341841286.2827 rgb/1341841286.2827.jpg\n1341841286.9506 rgb/1341841286.9506.jpg\n1341841287.6227 rgb/1341841287.6227.jpg\n1341841288.2946 rgb/1341841288.2946.jpg\n1341841288.9627 rgb/1341841288.9627.jpg\n1341841289.6346 rgb/1341841289.6346.jpg\n1341841290.303 rgb/1341841290.303.jpg\n1341841290.9745 rgb/1341841290.9745.jpg\n1341841291.6426 rgb/1341841291.6426.jpg\n1341841292.3147 rgb/1341841292.3147.jpg\n1341841292.9866 rgb/1341841292.9866.jpg\n1341841293.6548 rgb/1341841293.6548.jpg\n1341841294.3266 rgb/1341841294.3266.jpg\n1341841294.9952 rgb/1341841294.9952.jpg\n1341841295.6666 rgb/1341841295.6666.jpg\n1341841296.3348 rgb/1341841296.3348.jpg\n1341841297.0065 rgb/1341841297.0065.jpg\n1341841297.6786 rgb/1341841297.6786.jpg\n1341841298.3466 rgb/1341841298.3466.jpg\n1341841299.0185 rgb/1341841299.0185.jpg\n1341841299.6866 rgb/1341841299.6866.jpg\n1341841300.3656 rgb/1341841300.3656.jpg\n1341841301.0336 rgb/1341841301.0336.jpg\n1341841301.7057 rgb/1341841301.7057.jpg\n1341841302.3776 rgb/1341841302.3776.jpg\n1341841303.0456 rgb/1341841303.0456.jpg\n1341841303.7106 rgb/1341841303.7106.jpg\n1341841304.3787 rgb/1341841304.3787.jpg\n1341841305.0506 rgb/1341841305.0506.jpg\n1341841305.7227 rgb/1341841305.7227.jpg\n1341841306.3907 rgb/1341841306.3907.jpg\n1341841307.0627 rgb/1341841307.0627.jpg\n1341841307.7306 rgb/1341841307.7306.jpg\n1341841308.4099 rgb/1341841308.4099.jpg\n1341841309.1095 rgb/1341841309.1095.jpg\n1341841309.7816 rgb/1341841309.7816.jpg\n1341841310.4468 rgb/1341841310.4468.jpg\n1341841311.1507 rgb/1341841311.1507.jpg\n1341841311.8186 rgb/1341841311.8186.jpg\n1341841312.4905 rgb/1341841312.4905.jpg\n1341841313.1696 rgb/1341841313.1696.jpg\n1341841313.8695 rgb/1341841313.8695.jpg\n1341841314.5706 rgb/1341841314.5706.jpg\n1341841315.2456 rgb/1341841315.2456.jpg\n1341841315.9108 rgb/1341841315.9108.jpg\n1341841316.5786 rgb/1341841316.5786.jpg\n1341841317.2506 rgb/1341841317.2506.jpg\n" }, { "path": "Example/interface/rgbd.cpp", "content": "#include \"core/Initializer.h\"\n#include \"core/Geometry.h\"\n#include \"utils/dataprocess_utils.h\"\n#include \"utils/matrix_utils.h\"\n\n#include \n\n#include \"include/core/Viewer.h\"\n#include \"include/core/MapDrawer.h\"\n#include \"include/core/Map.h\"\n\n#include \n#include \n\n#include \"include/core/Ellipsoid.h\"\n#include \"src/tum_rgbd/io.h\"\n\n#include \n#include \ntypedef pcl::PointXYZRGB PointT;\ntypedef pcl::PointCloud PointCloudPCL;\n\n#include \"src/config/Config.h\"\n\nusing namespace std;\nusing namespace Eigen;\n\nint main(int argc,char* argv[]) {\n\n if( argc != 3)\n {\n std::cout << \"usage: \" << argv[0] << \" path_to_settings path_to_dataset\" << std::endl;\n return 1;\n }\n string strSettingPath = string(argv[1]);\n string dataset_path(argv[2]);\n string strDetectionDir = dataset_path + \"bbox/\";\n\n std::cout << \"- settings file: \" << strSettingPath << std::endl;\n std::cout << \"- dataset_path: \" << dataset_path << std::endl;\n std::cout << \"- strDetectionDir: \" << strDetectionDir << std::endl;\n\n TUMRGBD::Dataset loader;\n loader.loadDataset(dataset_path);\n loader.loadDetectionDir(strDetectionDir);\n\n EllipsoidSLAM::System SLAM(strSettingPath, true);\n\n cv::Mat rgb,depth;\n VectorXd pose;\n while(!loader.empty())\n {\n bool valid = loader.readFrame(rgb,depth,pose);\n int current_id = loader.getCurrentID();\n\n Eigen::MatrixXd detMat = loader.getDetectionMat();\n double timestamp = loader.GetCurrentTimestamp();\n\n if(valid)\n {\n std::cout << \"*****************************\" << std::endl;\n std::cout << \"Press [ENTER] to continue ... \" << std::endl;\n std::cout << \"*****************************\" << std::endl;\n getchar();\n\n SLAM.TrackWithObjects(timestamp, pose, detMat, depth, rgb, true); // Process frame. \n std::cout << std::endl;\n }\n\n std::cout << \" -> \" << loader.getCurrentID() << \"/\" << loader.getTotalNum() << std::endl;\n }\n\n std::cout << \"Finished all data.\" << std::endl;\n\n // save objects \n string output_path(\"./objects.txt\");\n SLAM.SaveObjectsToFile(output_path);\n SLAM.getTracker()->SaveObjectHistory(\"./object_history.txt\");\n\n cout << \"Use Ctrl+C to quit.\" << endl;\n while(1);\n\n cout << \"End.\" << endl;\n return 0;\n}" }, { "path": "Example/param/TUM3.yaml", "content": "%YAML:1.0\nDataset.Type: \"TUM\"\n\n# ------------------------------------\n# Single-frame ellipsoid estimation\n# ------------------------------------\n# Pointcloud segmentation parameters\nEllipsoidExtraction.Euclidean.ClusterTolerance: 0.02\nEllipsoidExtraction.Euclidean.MinClusterSize: 100\nEllipsoidExtraction.Euclidean.CenterDis: 0.5\n\n# weights of 3d, 2d edges in optimization\nOptimizer.Edges.3DEllipsoid.Scale: 10000\nOptimizer.Edges.2D.Scale: 1\n\n# ------------------------------\n# Symmetry Estimation\n# ------------------------------\n# Whether open symmetry estimation\nEllipsoidExtraction.Symmetry.Open: 1\n\n# Downsample grid size. Extremely influence the time and efficiency.\nEllipsoidExtraction.Symmetry.GridSize: 0.1\n\n# Parameters for symmetry probability\nSymmetrySolver.Sigma: 0.1\n\n# ----------------------------------\n# Symmetry types of semantic labels\n# ----------------------------------\n \n# Please see the definitions in the function:\n# EllipsoidExtractor::LoadSymmetryPrior()\n\n# -------------------------------\n# Ground Plane Extraction\n# -------------------------------\nPlane.MinSize: 200\nPlane.AngleThreshold: 5\nPlane.DistanceThreshold: 0.1\n\n# Whether use the normal of the groundplane to constrain the ellipsoid\nOptimizer.Edges.GravityPrior.Open: 1\nOptimizer.Edges.GravityPrior.Scale: 100\n\n# ------------------------------\n# Other Parameters\n# ------------------------------\n# Bounding box filter\nMeasurement.Border.Pixels: 10\nMeasurement.LengthLimit.Pixels: 0\n\n# A observation will only be valid when the robot has a change of view.\n# Close it to consider every observations.\nTracking.KeyFrameCheck.Close: 1\n\n#--------------------------------------------------------------------------------------------\n# Camera Parameters. \n#--------------------------------------------------------------------------------------------\n\n# Camera calibration and distortion parameters (OpenCV) \nCamera.fx: 535.4\nCamera.fy: 539.2\nCamera.cx: 320.1\nCamera.cy: 247.6\n\nCamera.width: 640\nCamera.height: 480\n\nCamera.scale: 5000.0\n#--------------------------------------------------------------------------------------------\n# Viewer Parameters\n#--------------------------------------------------------------------------------------------\nViewer.KeyFrameSize: 0.05\nViewer.KeyFrameLineWidth: 1\nViewer.GraphLineWidth: 0.9\nViewer.PointSize: 2\nViewer.CameraSize: 0.1\nViewer.CameraLineWidth: 3\nViewer.ViewpointX: 0\nViewer.ViewpointY: -0.7\nViewer.ViewpointZ: -1.8\nViewer.ViewpointF: 500\n\n" }, { "path": "LICENSE", "content": "BSD 3-Clause License\n\nCopyright (c) 2020, Liao Ziwei\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\n contributors may be used to endorse or promote products derived from\n this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n" }, { "path": "README.md", "content": "# EllipsoidSLAM\n\n## Update\n### Aug 22, 2021\n* Now support Ubuntu 20.04 and OpenCV 4.2\n* Fix bugs for crushes\n\n## Introduction\n\n![](.pics/demo.png)\n\nWe 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.\n\nWe have released a C++ implementation and a demo trajectory. We need to point out that this code is only a basic demo:\n * 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. \n * By default, only mapping is supported. If you want, it's possible to make simple changes to the Optimizer to enable the SLAM mode.\n\n## Author\n\nZiwei Liao et al., Robotics Institute, School of Mechanical Engineering & Automation, Beihang University, Beijing, China (Email: liaoziwei{at}buaa.edu.cn)\n\n## Related Paper\nPlease 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.\n\n[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\n) [[Video]](https://www.youtube.com/watch?v=u9zRBp4TPIs\n) \n\n[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)\n\n## Codes\n### Dependencies\nThe code has been tested on Ubuntu 16.04/18.04. The main dependencies are:\n\n* OpenCV (4.X recommended)\n* PCL 1.7+\n* Pangolin\n* g2o\n\nFor g2o, a modified version with SE3 transformation has been attached in the code. Use one simple command to compile it:\n \n $ sh install_g2o.sh\n\n### Build\nThese commands will automatically generate the Core module and an interface for RGB-D dataset:\n\n $ mkdir build\n $ cd build\n $ cmake .. \n $ make -j\n\nif there occurs a linking problem, add the lib directory to the environment variable:\n\n export LD_LIBRARY_PATH={YOUR_SOUCE_CODE_PATH}/lib:$LD_LIBRARY_PATH\n\n## Demos\n\n### TUM-Cabinet\nThe 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:\n\n#### Run\n ./build/rgbd ./Example/param/TUM3.yaml ./Example/dataset/cabinet/\n\nPlease 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.\n\n### Your Own Dataset\nFirst, please run object detector like YOLO to generate bounding boxes and store the result as text files, with each line containing:\n\n* id x1 y1 x2 y2 label probability instance\n\nwhere, (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]. \n\nSecond, 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.\n\n## Notes\n* All the important parameters could be adjusted in the .yaml file. See the comments in the file for details.\n\n* 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. \n\n* 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). \n\n* If you have any further questions, feel free to contact the author: liaoziwei{at}buaa.edu.cn\n" }, { "path": "Thirdparty/g2o/CMakeLists.txt", "content": "CMAKE_MINIMUM_REQUIRED(VERSION 2.6)\nset(CMAKE_CXX_FLAGS \"-std=c++11 ${CMAKE_CXX_FLAGS}\") # add by me. synced with flags in my cmakelist.txt\nSET(CMAKE_LEGACY_CYGWIN_WIN32 0)\n\nPROJECT(g2o)\n\nSET(g2o_C_FLAGS)\nSET(g2o_CXX_FLAGS)\n\n# default built type\nIF(NOT CMAKE_BUILD_TYPE)\n SET(CMAKE_BUILD_TYPE Release)\nENDIF()\n\nMESSAGE(STATUS \"BUILD TYPE:\" ${CMAKE_BUILD_TYPE})\n\nSET (G2O_LIB_TYPE SHARED)\n\n# There seems to be an issue with MSVC8\n# see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=83\nif(MSVC90)\n add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY=1)\n message(STATUS \"Disabling memory alignment for MSVC8\")\nendif(MSVC90)\n\n# Set the output directory for the build executables and libraries\nIF(WIN32)\n SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH \"Target for the libraries\")\nELSE(WIN32)\n SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH \"Target for the libraries\")\nENDIF(WIN32)\nSET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})\nSET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})\nSET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${g2o_RUNTIME_OUTPUT_DIRECTORY})\n\n# Set search directory for looking for our custom CMake scripts to\n# look for SuiteSparse, QGLViewer, and Eigen3.\nLIST(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules)\n\n# Detect OS and define macros appropriately\nIF(UNIX)\n ADD_DEFINITIONS(-DUNIX)\n MESSAGE(STATUS \"Compiling on Unix\")\nENDIF(UNIX)\n\n# For building the CHOLMOD / CSPARSE solvers\nFIND_PACKAGE(BLAS REQUIRED)\nFIND_PACKAGE(LAPACK REQUIRED)\n\n# Eigen library parallelise itself, though, presumably due to performance issues\n# OPENMP is experimental. We experienced some slowdown with it\nFIND_PACKAGE(OpenMP)\nSET(G2O_USE_OPENMP OFF CACHE BOOL \"Build g2o with OpenMP support (EXPERIMENTAL)\")\nIF(OPENMP_FOUND AND G2O_USE_OPENMP)\n SET (G2O_OPENMP 1)\n SET(g2o_C_FLAGS \"${g2o_C_FLAGS} ${OpenMP_C_FLAGS}\")\n SET(g2o_CXX_FLAGS \"${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}\")\n MESSAGE(STATUS \"Compiling with OpenMP support\")\nENDIF(OPENMP_FOUND AND G2O_USE_OPENMP)\n\n# Compiler specific options for gcc #comment by me\n# SET(CMAKE_CXX_FLAGS_RELEASE \"${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native\") \n# SET(CMAKE_C_FLAGS_RELEASE \"${CMAKE_C_FLAGS_RELEASE} -O3 -march=native\") \n\n# activate warnings !!!\n#SET(g2o_C_FLAGS \"${g2o_C_FLAGS} -Wall -W\")\n#SET(g2o_CXX_FLAGS \"${g2o_CXX_FLAGS} -Wall -W\")\n\n# specifying compiler flags\n#SET(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}\")\n#SET(CMAKE_C_FLAGS \"${CMAKE_C_FLAGS} ${g2o_C_FLAGS}\")\n\n# Find Eigen3\nSET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE})\nFIND_PACKAGE(Eigen3 3.1.0 REQUIRED)\nIF(EIGEN3_FOUND)\n SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH \"Directory of Eigen3\")\nELSE(EIGEN3_FOUND)\n SET(G2O_EIGEN3_INCLUDE \"\" CACHE PATH \"Directory of Eigen3\")\nENDIF(EIGEN3_FOUND)\n\n# Generate config.h\nSET(G2O_CXX_COMPILER \"${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}\")\nconfigure_file(config.h.in ${g2o_SOURCE_DIR}/config.h)\n\n# Set up the top-level include directories\nINCLUDE_DIRECTORIES(\n${g2o_SOURCE_DIR}/core\n${g2o_SOURCE_DIR}/types\n${g2o_SOURCE_DIR}/stuff \n${G2O_EIGEN3_INCLUDE})\n\n# Include the subdirectories\nADD_LIBRARY(g2o ${G2O_LIB_TYPE}\n#types\ng2o/types/types_sba.h\ng2o/types/types_sba.cpp\ng2o/types/types_six_dof_expmap.h\ng2o/types/types_six_dof_expmap.cpp\ng2o/types/types_seven_dof_expmap.h\ng2o/types/types_seven_dof_expmap.cpp\ng2o/types/se3quat.h\ng2o/types/se3_ops.h\ng2o/types/se3_ops.hpp\n#core\ng2o/core/base_edge.h\ng2o/core/base_binary_edge.h\ng2o/core/hyper_graph_action.cpp\ng2o/core/base_binary_edge.hpp\ng2o/core/hyper_graph_action.h\ng2o/core/base_multi_edge.h \ng2o/core/eigen_types.h \ng2o/core/hyper_graph.cpp\ng2o/core/base_multi_edge.hpp \ng2o/core/hyper_graph.h\ng2o/core/base_unary_edge.h \ng2o/core/linear_solver.h\ng2o/core/base_unary_edge.hpp \ng2o/core/marginal_covariance_cholesky.cpp\ng2o/core/base_vertex.h \ng2o/core/marginal_covariance_cholesky.h\ng2o/core/base_vertex.hpp \ng2o/core/matrix_structure.cpp\ng2o/core/batch_stats.cpp \ng2o/core/matrix_structure.h\ng2o/core/batch_stats.h \ng2o/core/openmp_mutex.h\ng2o/core/block_solver.h \ng2o/core/block_solver.hpp \ng2o/core/parameter.cpp \ng2o/core/parameter.h \ng2o/core/cache.cpp \ng2o/core/cache.h\ng2o/core/optimizable_graph.cpp \ng2o/core/optimizable_graph.h \ng2o/core/solver.cpp \ng2o/core/solver.h\ng2o/core/creators.h \ng2o/core/optimization_algorithm_factory.cpp\ng2o/core/estimate_propagator.cpp \ng2o/core/optimization_algorithm_factory.h\ng2o/core/estimate_propagator.h \ng2o/core/factory.cpp \ng2o/core/optimization_algorithm_property.h\ng2o/core/factory.h \ng2o/core/sparse_block_matrix.h\ng2o/core/sparse_optimizer.cpp \ng2o/core/sparse_block_matrix.hpp\ng2o/core/sparse_optimizer.h\ng2o/core/hyper_dijkstra.cpp \ng2o/core/hyper_dijkstra.h\ng2o/core/parameter_container.cpp \ng2o/core/parameter_container.h\ng2o/core/optimization_algorithm.cpp \ng2o/core/optimization_algorithm.h\ng2o/core/optimization_algorithm_with_hessian.cpp \ng2o/core/optimization_algorithm_with_hessian.h\ng2o/core/optimization_algorithm_levenberg.cpp \ng2o/core/optimization_algorithm_levenberg.h\ng2o/core/jacobian_workspace.cpp \ng2o/core/jacobian_workspace.h\ng2o/core/robust_kernel.cpp \ng2o/core/robust_kernel.h\ng2o/core/robust_kernel_factory.cpp\ng2o/core/robust_kernel_factory.h\ng2o/core/robust_kernel_impl.cpp \ng2o/core/robust_kernel_impl.h\n#stuff\ng2o/stuff/string_tools.h\ng2o/stuff/color_macros.h \ng2o/stuff/macros.h\ng2o/stuff/timeutil.cpp\ng2o/stuff/misc.h\ng2o/stuff/timeutil.h\ng2o/stuff/os_specific.c \ng2o/stuff/os_specific.h\ng2o/stuff/string_tools.cpp\ng2o/stuff/property.cpp \ng2o/stuff/property.h \n)\n" }, { "path": "Thirdparty/g2o/README.txt", "content": "Modified g2o version\nSee the original g2o library at: https://github.com/RainerKuemmerle/g2o\nAll files included in this g2o version are BSD, see license-bsd.txt\n" }, { "path": "Thirdparty/g2o/cmake_modules/FindBLAS.cmake", "content": "# Find BLAS library\n#\n# This module finds an installed library that implements the BLAS\n# linear-algebra interface (see http://www.netlib.org/blas/).\n# The list of libraries searched for is mainly taken\n# from the autoconf macro file, acx_blas.m4 (distributed at\n# http://ac-archive.sourceforge.net/ac-archive/acx_blas.html).\n#\n# This module sets the following variables:\n# BLAS_FOUND - set to true if a library implementing the BLAS interface\n# is found\n# BLAS_INCLUDE_DIR - Directories containing the BLAS header files\n# BLAS_DEFINITIONS - Compilation options to use BLAS\n# BLAS_LINKER_FLAGS - Linker flags to use BLAS (excluding -l\n# and -L).\n# BLAS_LIBRARIES_DIR - Directories containing the BLAS libraries.\n# May be null if BLAS_LIBRARIES contains libraries name using full path.\n# BLAS_LIBRARIES - List of libraries to link against BLAS interface.\n# May be null if the compiler supports auto-link (e.g. VC++).\n# BLAS_USE_FILE - The name of the cmake module to include to compile\n# applications or libraries using BLAS.\n#\n# This module was modified by CGAL team:\n# - find libraries for a C++ compiler, instead of Fortran\n# - added BLAS_INCLUDE_DIR, BLAS_DEFINITIONS and BLAS_LIBRARIES_DIR\n# - removed BLAS95_LIBRARIES\n\ninclude(CheckFunctionExists)\n\n\n# This macro checks for the existence of the combination of fortran libraries\n# given by _list. If the combination is found, this macro checks (using the\n# check_function_exists macro) whether can link against that library\n# combination using the name of a routine given by _name using the linker\n# flags given by _flags. If the combination of libraries is found and passes\n# the link test, LIBRARIES is set to the list of complete library paths that\n# have been found and DEFINITIONS to the required definitions.\n# Otherwise, LIBRARIES is set to FALSE.\n# N.B. _prefix is the prefix applied to the names of all cached variables that\n# are generated internally and marked advanced by this macro.\nmacro(check_fortran_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _path)\n #message(\"DEBUG: check_fortran_libraries(${_list} in ${_path})\")\n\n # Check for the existence of the libraries given by _list\n set(_libraries_found TRUE)\n set(_libraries_work FALSE)\n set(${DEFINITIONS} \"\")\n set(${LIBRARIES} \"\")\n set(_combined_name)\n foreach(_library ${_list})\n set(_combined_name ${_combined_name}_${_library})\n\n if(_libraries_found)\n # search first in ${_path}\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS ${_path} NO_DEFAULT_PATH\n )\n # if not found, search in environment variables and system\n if ( WIN32 )\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS ENV LIB\n )\n elseif ( APPLE )\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH\n )\n else ()\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH\n )\n endif()\n mark_as_advanced(${_prefix}_${_library}_LIBRARY)\n set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})\n set(_libraries_found ${${_prefix}_${_library}_LIBRARY})\n endif(_libraries_found)\n endforeach(_library ${_list})\n if(_libraries_found)\n set(_libraries_found ${${LIBRARIES}})\n endif()\n\n # Test this combination of libraries with the Fortran/f2c interface.\n # We test the Fortran interface first as it is well standardized.\n if(_libraries_found AND NOT _libraries_work)\n set(${DEFINITIONS} \"-D${_prefix}_USE_F2C\")\n set(${LIBRARIES} ${_libraries_found})\n # Some C++ linkers require the f2c library to link with Fortran libraries.\n # I do not know which ones, thus I just add the f2c library if it is available.\n find_package( F2C QUIET )\n if ( F2C_FOUND )\n set(${DEFINITIONS} ${${DEFINITIONS}} ${F2C_DEFINITIONS})\n set(${LIBRARIES} ${${LIBRARIES}} ${F2C_LIBRARIES})\n endif()\n set(CMAKE_REQUIRED_DEFINITIONS ${${DEFINITIONS}})\n set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}})\n #message(\"DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}\")\n #message(\"DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}\")\n # Check if function exists with f2c calling convention (ie a trailing underscore)\n check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)\n set(CMAKE_REQUIRED_DEFINITIONS} \"\")\n set(CMAKE_REQUIRED_LIBRARIES \"\")\n mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)\n set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})\n endif(_libraries_found AND NOT _libraries_work)\n\n # If not found, test this combination of libraries with a C interface.\n # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.\n if(_libraries_found AND NOT _libraries_work)\n set(${DEFINITIONS} \"\")\n set(${LIBRARIES} ${_libraries_found})\n set(CMAKE_REQUIRED_DEFINITIONS \"\")\n set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}})\n #message(\"DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}\")\n check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)\n set(CMAKE_REQUIRED_LIBRARIES \"\")\n mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)\n set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})\n endif(_libraries_found AND NOT _libraries_work)\n\n # on failure\n if(NOT _libraries_work)\n set(${DEFINITIONS} \"\")\n set(${LIBRARIES} FALSE)\n endif()\n #message(\"DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}\")\n #message(\"DEBUG: ${LIBRARIES} = ${${LIBRARIES}}\")\nendmacro(check_fortran_libraries)\n\n\n#\n# main\n#\n\n# Is it already configured?\nif (BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)\n\n set(BLAS_FOUND TRUE)\n\nelse()\n\n # reset variables\n set( BLAS_INCLUDE_DIR \"\" )\n set( BLAS_DEFINITIONS \"\" )\n set( BLAS_LINKER_FLAGS \"\" )\n set( BLAS_LIBRARIES \"\" )\n set( BLAS_LIBRARIES_DIR \"\" )\n\n #\n # If Unix, search for BLAS function in possible libraries\n #\n\n # BLAS in ATLAS library? (http://math-atlas.sourceforge.net/)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"cblas;f77blas;atlas\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n # BLAS in PhiPACK libraries? (requires generic BLAS lib, too)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"sgemm;dgemm;blas\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n # BLAS in Alpha CXML library?\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"cxml\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n # BLAS in Alpha DXML library? (now called CXML, see above)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"dxml\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n # BLAS in Sun Performance library?\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"-xlic_lib=sunperf\"\n \"sunperf;sunmath\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n if(BLAS_LIBRARIES)\n # Extra linker flag\n set(BLAS_LINKER_FLAGS \"-xlic_lib=sunperf\")\n endif()\n endif()\n\n # BLAS in SCSL library? (SGI/Cray Scientific Library)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"scsl\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n # BLAS in SGIMATH library?\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"complib.sgimath\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n # BLAS in IBM ESSL library? (requires generic BLAS lib, too)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"essl;blas\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n #BLAS in intel mkl 10 library? (em64t 64bit)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"mkl_intel_lp64;mkl_intel_thread;mkl_core;guide;pthread\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n ### windows version of intel mkl 10?\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n SGEMM\n \"\"\n \"mkl_c_dll;mkl_intel_thread_dll;mkl_core_dll;libguide40\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n #older versions of intel mkl libs\n\n # BLAS in intel mkl library? (shared)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"mkl;guide;pthread\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n #BLAS in intel mkl library? (static, 32bit)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"mkl_ia32;guide;pthread\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n #BLAS in intel mkl library? (static, em64t 64bit)\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"mkl_em64t;guide;pthread\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n #BLAS in acml library?\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"acml\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n # Apple BLAS library?\n if(NOT BLAS_LIBRARIES)\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"Accelerate\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n if ( NOT BLAS_LIBRARIES )\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"vecLib\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif ( NOT BLAS_LIBRARIES )\n\n # Generic BLAS library?\n # This configuration *must* be the last try as this library is notably slow.\n if ( NOT BLAS_LIBRARIES )\n check_fortran_libraries(\n BLAS_DEFINITIONS\n BLAS_LIBRARIES\n BLAS\n sgemm\n \"\"\n \"blas\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR\"\n )\n endif()\n\n if(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)\n set(BLAS_FOUND TRUE)\n else()\n set(BLAS_FOUND FALSE)\n endif()\n\n if(NOT BLAS_FIND_QUIETLY)\n if(BLAS_FOUND)\n message(STATUS \"A library with BLAS API found.\")\n else(BLAS_FOUND)\n if(BLAS_FIND_REQUIRED)\n message(FATAL_ERROR \"A required library with BLAS API not found. Please specify library location.\")\n else()\n message(STATUS \"A library with BLAS API not found. Please specify library location.\")\n endif()\n endif(BLAS_FOUND)\n endif(NOT BLAS_FIND_QUIETLY)\n\n # Add variables to cache\n set( BLAS_INCLUDE_DIR \"${BLAS_INCLUDE_DIR}\"\n CACHE PATH \"Directories containing the BLAS header files\" FORCE )\n set( BLAS_DEFINITIONS \"${BLAS_DEFINITIONS}\"\n CACHE STRING \"Compilation options to use BLAS\" FORCE )\n set( BLAS_LINKER_FLAGS \"${BLAS_LINKER_FLAGS}\"\n CACHE STRING \"Linker flags to use BLAS\" FORCE )\n set( BLAS_LIBRARIES \"${BLAS_LIBRARIES}\"\n CACHE FILEPATH \"BLAS libraries name\" FORCE )\n set( BLAS_LIBRARIES_DIR \"${BLAS_LIBRARIES_DIR}\"\n CACHE PATH \"Directories containing the BLAS libraries\" FORCE )\n\n #message(\"DEBUG: BLAS_INCLUDE_DIR = ${BLAS_INCLUDE_DIR}\")\n #message(\"DEBUG: BLAS_DEFINITIONS = ${BLAS_DEFINITIONS}\")\n #message(\"DEBUG: BLAS_LINKER_FLAGS = ${BLAS_LINKER_FLAGS}\")\n #message(\"DEBUG: BLAS_LIBRARIES = ${BLAS_LIBRARIES}\")\n #message(\"DEBUG: BLAS_LIBRARIES_DIR = ${BLAS_LIBRARIES_DIR}\")\n #message(\"DEBUG: BLAS_FOUND = ${BLAS_FOUND}\")\n\nendif(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)\n" }, { "path": "Thirdparty/g2o/cmake_modules/FindEigen3.cmake", "content": "# - Try to find Eigen3 lib\n#\n# This module supports requiring a minimum version, e.g. you can do\n# find_package(Eigen3 3.1.2)\n# to require version 3.1.2 or newer of Eigen3.\n#\n# Once done this will define\n#\n# EIGEN3_FOUND - system has eigen lib with correct version\n# EIGEN3_INCLUDE_DIR - the eigen include directory\n# EIGEN3_VERSION - eigen version\n\n# Copyright (c) 2006, 2007 Montel Laurent, \n# Copyright (c) 2008, 2009 Gael Guennebaud, \n# Copyright (c) 2009 Benoit Jacob \n# Redistribution and use is allowed according to the terms of the 2-clause BSD license.\n\nif(NOT Eigen3_FIND_VERSION)\n if(NOT Eigen3_FIND_VERSION_MAJOR)\n set(Eigen3_FIND_VERSION_MAJOR 2)\n endif(NOT Eigen3_FIND_VERSION_MAJOR)\n if(NOT Eigen3_FIND_VERSION_MINOR)\n set(Eigen3_FIND_VERSION_MINOR 91)\n endif(NOT Eigen3_FIND_VERSION_MINOR)\n if(NOT Eigen3_FIND_VERSION_PATCH)\n set(Eigen3_FIND_VERSION_PATCH 0)\n endif(NOT Eigen3_FIND_VERSION_PATCH)\n\n set(Eigen3_FIND_VERSION \"${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}\")\nendif(NOT Eigen3_FIND_VERSION)\n\nmacro(_eigen3_check_version)\n file(READ \"${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h\" _eigen3_version_header)\n\n string(REGEX MATCH \"define[ \\t]+EIGEN_WORLD_VERSION[ \\t]+([0-9]+)\" _eigen3_world_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_WORLD_VERSION \"${CMAKE_MATCH_1}\")\n string(REGEX MATCH \"define[ \\t]+EIGEN_MAJOR_VERSION[ \\t]+([0-9]+)\" _eigen3_major_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_MAJOR_VERSION \"${CMAKE_MATCH_1}\")\n string(REGEX MATCH \"define[ \\t]+EIGEN_MINOR_VERSION[ \\t]+([0-9]+)\" _eigen3_minor_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_MINOR_VERSION \"${CMAKE_MATCH_1}\")\n\n set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})\n if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})\n set(EIGEN3_VERSION_OK FALSE)\n else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})\n set(EIGEN3_VERSION_OK TRUE)\n endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})\n\n if(NOT EIGEN3_VERSION_OK)\n\n message(STATUS \"Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, \"\n \"but at least version ${Eigen3_FIND_VERSION} is required\")\n endif(NOT EIGEN3_VERSION_OK)\nendmacro(_eigen3_check_version)\n\nif (EIGEN3_INCLUDE_DIR)\n\n # in cache already\n _eigen3_check_version()\n set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})\n\nelse (EIGEN3_INCLUDE_DIR)\n\n # specific additional paths for some OS\n if (WIN32)\n set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} \"C:/Program Files/Eigen/include\" \"C:/Program Files (x86)/Eigen/include\")\n endif(WIN32)\n\n find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library\n PATHS\n ${CMAKE_INSTALL_PREFIX}/include\n ${EIGEN_ADDITIONAL_SEARCH_PATHS}\n ${KDE4_INCLUDE_DIR}\n PATH_SUFFIXES eigen3 eigen\n )\n\n if(EIGEN3_INCLUDE_DIR)\n _eigen3_check_version()\n endif(EIGEN3_INCLUDE_DIR)\n\n include(FindPackageHandleStandardArgs)\n find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)\n\n mark_as_advanced(EIGEN3_INCLUDE_DIR)\n\nendif(EIGEN3_INCLUDE_DIR)\n\n" }, { "path": "Thirdparty/g2o/cmake_modules/FindLAPACK.cmake", "content": "# Find LAPACK library\n#\n# This module finds an installed library that implements the LAPACK\n# linear-algebra interface (see http://www.netlib.org/lapack/).\n# The approach follows mostly that taken for the autoconf macro file, acx_lapack.m4\n# (distributed at http://ac-archive.sourceforge.net/ac-archive/acx_lapack.html).\n#\n# This module sets the following variables:\n# LAPACK_FOUND - set to true if a library implementing the LAPACK interface\n# is found\n# LAPACK_INCLUDE_DIR - Directories containing the LAPACK header files\n# LAPACK_DEFINITIONS - Compilation options to use LAPACK\n# LAPACK_LINKER_FLAGS - Linker flags to use LAPACK (excluding -l\n# and -L).\n# LAPACK_LIBRARIES_DIR - Directories containing the LAPACK libraries.\n# May be null if LAPACK_LIBRARIES contains libraries name using full path.\n# LAPACK_LIBRARIES - List of libraries to link against LAPACK interface.\n# May be null if the compiler supports auto-link (e.g. VC++).\n# LAPACK_USE_FILE - The name of the cmake module to include to compile\n# applications or libraries using LAPACK.\n#\n# This module was modified by CGAL team:\n# - find libraries for a C++ compiler, instead of Fortran\n# - added LAPACK_INCLUDE_DIR, LAPACK_DEFINITIONS and LAPACK_LIBRARIES_DIR\n# - removed LAPACK95_LIBRARIES\n\n\ninclude(CheckFunctionExists)\n\n# This macro checks for the existence of the combination of fortran libraries\n# given by _list. If the combination is found, this macro checks (using the\n# check_function_exists macro) whether can link against that library\n# combination using the name of a routine given by _name using the linker\n# flags given by _flags. If the combination of libraries is found and passes\n# the link test, LIBRARIES is set to the list of complete library paths that\n# have been found and DEFINITIONS to the required definitions.\n# Otherwise, LIBRARIES is set to FALSE.\n# N.B. _prefix is the prefix applied to the names of all cached variables that\n# are generated internally and marked advanced by this macro.\nmacro(check_lapack_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _blas _path)\n #message(\"DEBUG: check_lapack_libraries(${_list} in ${_path} with ${_blas})\")\n\n # Check for the existence of the libraries given by _list\n set(_libraries_found TRUE)\n set(_libraries_work FALSE)\n set(${DEFINITIONS} \"\")\n set(${LIBRARIES} \"\")\n set(_combined_name)\n foreach(_library ${_list})\n set(_combined_name ${_combined_name}_${_library})\n\n if(_libraries_found)\n # search first in ${_path}\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS ${_path} NO_DEFAULT_PATH\n )\n # if not found, search in environment variables and system\n if ( WIN32 )\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS ENV LIB\n )\n elseif ( APPLE )\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH\n )\n else ()\n find_library(${_prefix}_${_library}_LIBRARY\n NAMES ${_library}\n PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH\n )\n endif()\n mark_as_advanced(${_prefix}_${_library}_LIBRARY)\n set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})\n set(_libraries_found ${${_prefix}_${_library}_LIBRARY})\n endif(_libraries_found)\n endforeach(_library ${_list})\n if(_libraries_found)\n set(_libraries_found ${${LIBRARIES}})\n endif()\n\n # Test this combination of libraries with the Fortran/f2c interface.\n # We test the Fortran interface first as it is well standardized.\n if(_libraries_found AND NOT _libraries_work)\n set(${DEFINITIONS} \"-D${_prefix}_USE_F2C\")\n set(${LIBRARIES} ${_libraries_found})\n # Some C++ linkers require the f2c library to link with Fortran libraries.\n # I do not know which ones, thus I just add the f2c library if it is available.\n find_package( F2C QUIET )\n if ( F2C_FOUND )\n set(${DEFINITIONS} ${${DEFINITIONS}} ${F2C_DEFINITIONS})\n set(${LIBRARIES} ${${LIBRARIES}} ${F2C_LIBRARIES})\n endif()\n set(CMAKE_REQUIRED_DEFINITIONS ${${DEFINITIONS}})\n set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}} ${_blas})\n #message(\"DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}\")\n #message(\"DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}\")\n # Check if function exists with f2c calling convention (ie a trailing underscore)\n check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)\n set(CMAKE_REQUIRED_DEFINITIONS} \"\")\n set(CMAKE_REQUIRED_LIBRARIES \"\")\n mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)\n set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})\n endif(_libraries_found AND NOT _libraries_work)\n\n # If not found, test this combination of libraries with a C interface.\n # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.\n if(_libraries_found AND NOT _libraries_work)\n set(${DEFINITIONS} \"\")\n set(${LIBRARIES} ${_libraries_found})\n set(CMAKE_REQUIRED_DEFINITIONS \"\")\n set(CMAKE_REQUIRED_LIBRARIES ${_flags} ${${LIBRARIES}} ${_blas})\n #message(\"DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}\")\n check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)\n set(CMAKE_REQUIRED_LIBRARIES \"\")\n mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)\n set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})\n endif(_libraries_found AND NOT _libraries_work)\n\n # on failure\n if(NOT _libraries_work)\n set(${DEFINITIONS} \"\")\n set(${LIBRARIES} FALSE)\n endif()\n #message(\"DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}\")\n #message(\"DEBUG: ${LIBRARIES} = ${${LIBRARIES}}\")\nendmacro(check_lapack_libraries)\n\n\n#\n# main\n#\n\n# LAPACK requires BLAS\nif(LAPACK_FIND_QUIETLY OR NOT LAPACK_FIND_REQUIRED)\n find_package(BLAS)\nelse()\n find_package(BLAS REQUIRED)\nendif()\n\nif (NOT BLAS_FOUND)\n\n message(STATUS \"LAPACK requires BLAS.\")\n set(LAPACK_FOUND FALSE)\n\n# Is it already configured?\nelseif (LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)\n\n set(LAPACK_FOUND TRUE)\n\nelse()\n\n # reset variables\n set( LAPACK_INCLUDE_DIR \"\" )\n set( LAPACK_DEFINITIONS \"\" )\n set( LAPACK_LINKER_FLAGS \"\" ) # unused (yet)\n set( LAPACK_LIBRARIES \"\" )\n set( LAPACK_LIBRARIES_DIR \"\" )\n\n #\n # If Unix, search for LAPACK function in possible libraries\n #\n\n #intel mkl lapack?\n if(NOT LAPACK_LIBRARIES)\n check_lapack_libraries(\n LAPACK_DEFINITIONS\n LAPACK_LIBRARIES\n LAPACK\n cheev\n \"\"\n \"mkl_lapack\"\n \"${BLAS_LIBRARIES}\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR\"\n )\n endif()\n\n #acml lapack?\n if(NOT LAPACK_LIBRARIES)\n check_lapack_libraries(\n LAPACK_DEFINITIONS\n LAPACK_LIBRARIES\n LAPACK\n cheev\n \"\"\n \"acml\"\n \"${BLAS_LIBRARIES}\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR\"\n )\n endif()\n\n # Apple LAPACK library?\n if(NOT LAPACK_LIBRARIES)\n check_lapack_libraries(\n LAPACK_DEFINITIONS\n LAPACK_LIBRARIES\n LAPACK\n cheev\n \"\"\n \"Accelerate\"\n \"${BLAS_LIBRARIES}\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR\"\n )\n endif()\n\n if ( NOT LAPACK_LIBRARIES )\n check_lapack_libraries(\n LAPACK_DEFINITIONS\n LAPACK_LIBRARIES\n LAPACK\n cheev\n \"\"\n \"vecLib\"\n \"${BLAS_LIBRARIES}\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR\"\n )\n endif ( NOT LAPACK_LIBRARIES )\n\n # Generic LAPACK library?\n # This configuration *must* be the last try as this library is notably slow.\n if ( NOT LAPACK_LIBRARIES )\n check_lapack_libraries(\n LAPACK_DEFINITIONS\n LAPACK_LIBRARIES\n LAPACK\n cheev\n \"\"\n \"lapack\"\n \"${BLAS_LIBRARIES}\"\n \"${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR\"\n )\n endif()\n\n if(LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)\n set(LAPACK_FOUND TRUE)\n else()\n set(LAPACK_FOUND FALSE)\n endif()\n\n if(NOT LAPACK_FIND_QUIETLY)\n if(LAPACK_FOUND)\n message(STATUS \"A library with LAPACK API found.\")\n else(LAPACK_FOUND)\n if(LAPACK_FIND_REQUIRED)\n message(FATAL_ERROR \"A required library with LAPACK API not found. Please specify library location.\")\n else()\n message(STATUS \"A library with LAPACK API not found. Please specify library location.\")\n endif()\n endif(LAPACK_FOUND)\n endif(NOT LAPACK_FIND_QUIETLY)\n\n # Add variables to cache\n set( LAPACK_INCLUDE_DIR \"${LAPACK_INCLUDE_DIR}\"\n CACHE PATH \"Directories containing the LAPACK header files\" FORCE )\n set( LAPACK_DEFINITIONS \"${LAPACK_DEFINITIONS}\"\n CACHE STRING \"Compilation options to use LAPACK\" FORCE )\n set( LAPACK_LINKER_FLAGS \"${LAPACK_LINKER_FLAGS}\"\n CACHE STRING \"Linker flags to use LAPACK\" FORCE )\n set( LAPACK_LIBRARIES \"${LAPACK_LIBRARIES}\"\n CACHE FILEPATH \"LAPACK libraries name\" FORCE )\n set( LAPACK_LIBRARIES_DIR \"${LAPACK_LIBRARIES_DIR}\"\n CACHE PATH \"Directories containing the LAPACK libraries\" FORCE )\n\n #message(\"DEBUG: LAPACK_INCLUDE_DIR = ${LAPACK_INCLUDE_DIR}\")\n #message(\"DEBUG: LAPACK_DEFINITIONS = ${LAPACK_DEFINITIONS}\")\n #message(\"DEBUG: LAPACK_LINKER_FLAGS = ${LAPACK_LINKER_FLAGS}\")\n #message(\"DEBUG: LAPACK_LIBRARIES = ${LAPACK_LIBRARIES}\")\n #message(\"DEBUG: LAPACK_LIBRARIES_DIR = ${LAPACK_LIBRARIES_DIR}\")\n #message(\"DEBUG: LAPACK_FOUND = ${LAPACK_FOUND}\")\n\nendif(NOT BLAS_FOUND)\n" }, { "path": "Thirdparty/g2o/config.h", "content": "#ifndef G2O_CONFIG_H\n#define G2O_CONFIG_H\n\n/* #undef G2O_OPENMP */\n/* #undef G2O_SHARED_LIBS */\n\n// give a warning if Eigen defaults to row-major matrices.\n// We internally assume column-major matrices throughout the code.\n#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR\n# error \"g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)\"\n#endif\n\n#endif\n" }, { "path": "Thirdparty/g2o/config.h.in", "content": "#ifndef G2O_CONFIG_H\n#define G2O_CONFIG_H\n\n#cmakedefine G2O_OPENMP 1\n#cmakedefine G2O_SHARED_LIBS 1\n\n// give a warning if Eigen defaults to row-major matrices.\n// We internally assume column-major matrices throughout the code.\n#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR\n# error \"g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)\"\n#endif\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/base_binary_edge.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_BASE_BINARY_EDGE_H\n#define G2O_BASE_BINARY_EDGE_H\n\n#include \n#include \n\n#include \"base_edge.h\"\n#include \"robust_kernel.h\"\n#include \"../../config.h\"\n\nnamespace g2o {\n\n using namespace Eigen;\n\n template \n class BaseBinaryEdge : public BaseEdge\n {\n public:\n\n typedef VertexXi VertexXiType;\n typedef VertexXj VertexXjType;\n\n static const int Di = VertexXiType::Dimension;\n static const int Dj = VertexXjType::Dimension;\n\n static const int Dimension = BaseEdge::Dimension;\n typedef typename BaseEdge::Measurement Measurement;\n typedef typename Matrix::AlignedMapType JacobianXiOplusType;\n typedef typename Matrix::AlignedMapType JacobianXjOplusType;\n typedef typename BaseEdge::ErrorVector ErrorVector;\n typedef typename BaseEdge::InformationType InformationType;\n\n typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;\n typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType;\n\n BaseBinaryEdge() : BaseEdge(),\n _hessianRowMajor(false),\n _hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps\n _hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension),\n _jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj)\n {\n _vertices.resize(2);\n }\n\n virtual OptimizableGraph::Vertex* createFrom();\n virtual OptimizableGraph::Vertex* createTo();\n\n virtual void resize(size_t size);\n\n virtual bool allVerticesFixed() const;\n\n virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);\n\n /**\n * Linearizes the oplus operator in the vertex, and stores\n * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj\n */\n virtual void linearizeOplus();\n\n //! returns the result of the linearization in the manifold space for the node xi\n const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}\n //! returns the result of the linearization in the manifold space for the node xj\n const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;}\n\n virtual void constructQuadraticForm() ;\n\n virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);\n\n using BaseEdge::resize;\n using BaseEdge::computeError;\n\n protected:\n using BaseEdge::_measurement;\n using BaseEdge::_information;\n using BaseEdge::_error;\n using BaseEdge::_vertices;\n using BaseEdge::_dimension;\n\n bool _hessianRowMajor;\n HessianBlockType _hessian;\n HessianBlockTransposedType _hessianTransposed;\n JacobianXiOplusType _jacobianOplusXi;\n JacobianXjOplusType _jacobianOplusXj;\n\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n };\n\n#include \"base_binary_edge.hpp\"\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/base_binary_edge.hpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\ntemplate \nOptimizableGraph::Vertex* BaseBinaryEdge::createFrom(){\n return new VertexXiType();\n}\n\ntemplate \nOptimizableGraph::Vertex* BaseBinaryEdge::createTo(){\n return new VertexXjType();\n}\n\n\ntemplate \nvoid BaseBinaryEdge::resize(size_t size)\n{\n if (size != 2) {\n std::cerr << \"WARNING, attempting to resize binary edge \" << BaseEdge::id() << \" to \" << size << std::endl;\n }\n BaseEdge::resize(size);\n}\n\ntemplate \nbool BaseBinaryEdge::allVerticesFixed() const\n{\n return (static_cast (_vertices[0])->fixed() &&\n static_cast (_vertices[1])->fixed());\n}\n\ntemplate \nvoid BaseBinaryEdge::constructQuadraticForm()\n{\n VertexXiType* from = static_cast(_vertices[0]);\n VertexXjType* to = static_cast(_vertices[1]);\n\n // get the Jacobian of the nodes in the manifold domain\n const JacobianXiOplusType& A = jacobianOplusXi();\n const JacobianXjOplusType& B = jacobianOplusXj();\n\n\n bool fromNotFixed = !(from->fixed());\n bool toNotFixed = !(to->fixed());\n\n if (fromNotFixed || toNotFixed) {\n#ifdef G2O_OPENMP\n from->lockQuadraticForm();\n to->lockQuadraticForm();\n#endif\n const InformationType& omega = _information;\n Matrix omega_r = - omega * _error;\n if (this->robustKernel() == 0) {\n if (fromNotFixed) {\n Matrix AtO = A.transpose() * omega;\n from->b().noalias() += A.transpose() * omega_r;\n from->A().noalias() += AtO*A;\n if (toNotFixed ) {\n if (_hessianRowMajor) // we have to write to the block as transposed\n _hessianTransposed.noalias() += B.transpose() * AtO.transpose();\n else\n _hessian.noalias() += AtO * B;\n }\n } \n if (toNotFixed) {\n to->b().noalias() += B.transpose() * omega_r;\n to->A().noalias() += B.transpose() * omega * B;\n }\n } else { // robust (weighted) error according to some kernel\n double error = this->chi2();\n Eigen::Vector3d rho;\n this->robustKernel()->robustify(error, rho);\n InformationType weightedOmega = this->robustInformation(rho);\n //std::cout << PVAR(rho.transpose()) << std::endl;\n //std::cout << PVAR(weightedOmega) << std::endl;\n\n omega_r *= rho[1];\n if (fromNotFixed) {\n from->b().noalias() += A.transpose() * omega_r;\n from->A().noalias() += A.transpose() * weightedOmega * A;\n if (toNotFixed ) {\n if (_hessianRowMajor) // we have to write to the block as transposed\n _hessianTransposed.noalias() += B.transpose() * weightedOmega * A;\n else\n _hessian.noalias() += A.transpose() * weightedOmega * B;\n }\n } \n if (toNotFixed) {\n to->b().noalias() += B.transpose() * omega_r;\n to->A().noalias() += B.transpose() * weightedOmega * B;\n }\n }\n#ifdef G2O_OPENMP\n to->unlockQuadraticForm();\n from->unlockQuadraticForm();\n#endif\n }\n}\n\ntemplate \nvoid BaseBinaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace)\n{\n new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di);\n new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj);\n linearizeOplus();\n}\n\ntemplate \nvoid BaseBinaryEdge::linearizeOplus()\n{\n VertexXiType* vi = static_cast(_vertices[0]);\n VertexXjType* vj = static_cast(_vertices[1]);\n\n bool iNotFixed = !(vi->fixed());\n bool jNotFixed = !(vj->fixed());\n\n if (!iNotFixed && !jNotFixed)\n return;\n\n#ifdef G2O_OPENMP\n vi->lockQuadraticForm();\n vj->lockQuadraticForm();\n#endif\n\n const double delta = 1e-9;\n const double scalar = 1.0 / (2*delta);\n ErrorVector errorBak;\n ErrorVector errorBeforeNumeric = _error;\n\n if (iNotFixed) {\n //Xi - estimate the jacobian numerically\n double add_vi[VertexXiType::Dimension];\n std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);\n // add small step along the unit vector in each dimension\n for (int d = 0; d < VertexXiType::Dimension; ++d) {\n vi->push();\n add_vi[d] = delta;\n vi->oplus(add_vi);\n computeError();\n errorBak = _error;\n vi->pop();\n vi->push();\n add_vi[d] = -delta;\n vi->oplus(add_vi);\n computeError();\n errorBak -= _error;\n vi->pop();\n add_vi[d] = 0.0;\n\n _jacobianOplusXi.col(d) = scalar * errorBak;\n } // end dimension\n }\n\n if (jNotFixed) {\n //Xj - estimate the jacobian numerically\n double add_vj[VertexXjType::Dimension];\n std::fill(add_vj, add_vj + VertexXjType::Dimension, 0.0);\n // add small step along the unit vector in each dimension\n for (int d = 0; d < VertexXjType::Dimension; ++d) {\n vj->push();\n add_vj[d] = delta;\n vj->oplus(add_vj);\n computeError();\n errorBak = _error;\n vj->pop();\n vj->push();\n add_vj[d] = -delta;\n vj->oplus(add_vj);\n computeError();\n errorBak -= _error;\n vj->pop();\n add_vj[d] = 0.0;\n\n _jacobianOplusXj.col(d) = scalar * errorBak;\n }\n } // end dimension\n\n _error = errorBeforeNumeric;\n#ifdef G2O_OPENMP\n vj->unlockQuadraticForm();\n vi->unlockQuadraticForm();\n#endif\n}\n\ntemplate \nvoid BaseBinaryEdge::mapHessianMemory(double* d, int i, int j, bool rowMajor)\n{\n (void) i; (void) j;\n //assert(i == 0 && j == 1);\n if (rowMajor) {\n new (&_hessianTransposed) HessianBlockTransposedType(d, VertexXjType::Dimension, VertexXiType::Dimension);\n } else {\n new (&_hessian) HessianBlockType(d, VertexXiType::Dimension, VertexXjType::Dimension);\n }\n _hessianRowMajor = rowMajor;\n}\n" }, { "path": "Thirdparty/g2o/g2o/core/base_edge.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_BASE_EDGE_H\n#define G2O_BASE_EDGE_H\n\n#include \n#include \n\n#include \n\n#include \"optimizable_graph.h\"\n\nnamespace g2o {\n\n using namespace Eigen;\n\n template \n class BaseEdge : public OptimizableGraph::Edge\n {\n public:\n\n static const int Dimension = D;\n typedef E Measurement;\n typedef Matrix ErrorVector;\n typedef Matrix InformationType;\n\n BaseEdge() : OptimizableGraph::Edge()\n {\n _dimension = D;\n }\n\n virtual ~BaseEdge() {}\n\n virtual double chi2() const \n {\n return _error.dot(information()*_error);\n }\n\n virtual const double* errorData() const { return _error.data();}\n virtual double* errorData() { return _error.data();}\n const ErrorVector& error() const { return _error;}\n ErrorVector& error() { return _error;}\n\n //! information matrix of the constraint\n const InformationType& information() const { return _information;}\n InformationType& information() { return _information;}\n void setInformation(const InformationType& information) { _information = information;}\n\n virtual const double* informationData() const { return _information.data();}\n virtual double* informationData() { return _information.data();}\n\n //! accessor functions for the measurement represented by the edge\n const Measurement& measurement() const { return _measurement;}\n virtual void setMeasurement(const Measurement& m) { _measurement = m;}\n\n virtual int rank() const {return _dimension;}\n\n virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)\n {\n std::cerr << \"inititialEstimate() is not implemented, please give implementation in your derived class\" << std::endl;\n }\n\n protected:\n\n Measurement _measurement;\n InformationType _information;\n ErrorVector _error;\n\n /**\n * calculate the robust information matrix by updating the information matrix of the error\n */\n InformationType robustInformation(const Eigen::Vector3d& rho)\n {\n InformationType result = rho[1] * _information;\n //ErrorVector weightedErrror = _information * _error;\n //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());\n return result;\n }\n\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n };\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/base_multi_edge.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_BASE_MULTI_EDGE_H\n#define G2O_BASE_MULTI_EDGE_H\n\n#include \n#include \n#include \n\n#include \n\n#include \"base_edge.h\"\n#include \"robust_kernel.h\"\n#include \"../../config.h\"\n\nnamespace g2o {\n\n using namespace Eigen;\n\n /**\n * \\brief base class to represent an edge connecting an arbitrary number of nodes\n *\n * D - Dimension of the measurement\n * E - type to represent the measurement\n */\n template \n class BaseMultiEdge : public BaseEdge\n {\n public:\n /**\n * \\brief helper for mapping the Hessian memory of the upper triangular block\n */\n struct HessianHelper {\n Eigen::Map matrix; ///< the mapped memory\n bool transposed; ///< the block has to be transposed\n HessianHelper() : matrix(0, 0, 0), transposed(false) {}\n };\n\n public:\n static const int Dimension = BaseEdge::Dimension;\n typedef typename BaseEdge::Measurement Measurement;\n typedef MatrixXd::MapType JacobianType;\n typedef typename BaseEdge::ErrorVector ErrorVector;\n typedef typename BaseEdge::InformationType InformationType;\n typedef Eigen::Map HessianBlockType;\n\n BaseMultiEdge() : BaseEdge()\n {\n }\n \n virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);\n\n /**\n * Linearizes the oplus operator in the vertex, and stores\n * the result in temporary variable vector _jacobianOplus\n */\n virtual void linearizeOplus();\n \n virtual void resize(size_t size);\n\n virtual bool allVerticesFixed() const;\n\n virtual void constructQuadraticForm() ;\n\n virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);\n\n using BaseEdge::computeError;\n\n protected:\n using BaseEdge::_measurement;\n using BaseEdge::_information;\n using BaseEdge::_error;\n using BaseEdge::_vertices;\n using BaseEdge::_dimension;\n\n std::vector _hessian;\n std::vector > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus)\n\n void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError);\n\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n };\n\n#include \"base_multi_edge.hpp\"\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/base_multi_edge.hpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\nnamespace internal {\n inline int computeUpperTriangleIndex(int i, int j)\n {\n int elemsUpToCol = ((j-1) * j) / 2;\n return elemsUpToCol + i;\n }\n}\n\ntemplate \nvoid BaseMultiEdge::constructQuadraticForm()\n{\n if (this->robustKernel()) {\n double error = this->chi2();\n Eigen::Vector3d rho;\n this->robustKernel()->robustify(error, rho);\n Matrix omega_r = - _information * _error;\n omega_r *= rho[1];\n computeQuadraticForm(this->robustInformation(rho), omega_r);\n } else {\n computeQuadraticForm(_information, - _information * _error);\n }\n}\n\n\ntemplate \nvoid BaseMultiEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace)\n{\n for (size_t i = 0; i < _vertices.size(); ++i) {\n OptimizableGraph::Vertex* v = static_cast(_vertices[i]);\n assert(v->dimension() >= 0);\n new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension());\n }\n linearizeOplus();\n}\n\ntemplate \nvoid BaseMultiEdge::linearizeOplus()\n{\n#ifdef G2O_OPENMP\n for (size_t i = 0; i < _vertices.size(); ++i) {\n OptimizableGraph::Vertex* v = static_cast(_vertices[i]);\n v->lockQuadraticForm();\n }\n#endif\n\n const double delta = 1e-9;\n const double scalar = 1.0 / (2*delta);\n ErrorVector errorBak;\n ErrorVector errorBeforeNumeric = _error;\n\n for (size_t i = 0; i < _vertices.size(); ++i) {\n //Xi - estimate the jacobian numerically\n OptimizableGraph::Vertex* vi = static_cast(_vertices[i]);\n\n if (vi->fixed())\n continue;\n\n const int vi_dim = vi->dimension();\n assert(vi_dim >= 0);\n#ifdef _MSC_VER\n double* add_vi = new double[vi_dim];\n#else\n double add_vi[vi_dim];\n#endif\n std::fill(add_vi, add_vi + vi_dim, 0.0);\n assert(_dimension >= 0);\n assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && \"jacobian cache dimension does not match\");\n _jacobianOplus[i].resize(_dimension, vi_dim);\n // add small step along the unit vector in each dimension\n for (int d = 0; d < vi_dim; ++d) {\n vi->push();\n add_vi[d] = delta;\n vi->oplus(add_vi);\n computeError();\n errorBak = _error;\n vi->pop();\n vi->push();\n add_vi[d] = -delta;\n vi->oplus(add_vi);\n computeError();\n errorBak -= _error;\n vi->pop();\n add_vi[d] = 0.0;\n\n _jacobianOplus[i].col(d) = scalar * errorBak;\n } // end dimension\n#ifdef _MSC_VER\n delete[] add_vi;\n#endif\n }\n _error = errorBeforeNumeric;\n\n#ifdef G2O_OPENMP\n for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {\n OptimizableGraph::Vertex* v = static_cast(_vertices[i]);\n v->unlockQuadraticForm();\n }\n#endif\n\n}\n\ntemplate \nvoid BaseMultiEdge::mapHessianMemory(double* d, int i, int j, bool rowMajor)\n{\n int idx = internal::computeUpperTriangleIndex(i, j);\n assert(idx < (int)_hessian.size());\n OptimizableGraph::Vertex* vi = static_cast(HyperGraph::Edge::vertex(i));\n OptimizableGraph::Vertex* vj = static_cast(HyperGraph::Edge::vertex(j));\n assert(vi->dimension() >= 0);\n assert(vj->dimension() >= 0);\n HessianHelper& h = _hessian[idx];\n if (rowMajor) {\n if (h.matrix.data() != d || h.transposed != rowMajor)\n new (&h.matrix) HessianBlockType(d, vj->dimension(), vi->dimension());\n } else {\n if (h.matrix.data() != d || h.transposed != rowMajor)\n new (&h.matrix) HessianBlockType(d, vi->dimension(), vj->dimension());\n }\n h.transposed = rowMajor;\n}\n\ntemplate \nvoid BaseMultiEdge::resize(size_t size)\n{\n BaseEdge::resize(size);\n int n = (int)_vertices.size();\n int maxIdx = (n * (n-1))/2;\n assert(maxIdx >= 0);\n _hessian.resize(maxIdx);\n _jacobianOplus.resize(size, JacobianType(0,0,0));\n}\n\ntemplate \nbool BaseMultiEdge::allVerticesFixed() const\n{\n for (size_t i = 0; i < _vertices.size(); ++i) {\n if (!static_cast (_vertices[i])->fixed()) {\n return false;\n }\n }\n return true;\n}\n\ntemplate \nvoid BaseMultiEdge::computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError)\n{\n for (size_t i = 0; i < _vertices.size(); ++i) {\n OptimizableGraph::Vertex* from = static_cast(_vertices[i]);\n bool istatus = !(from->fixed());\n\n if (istatus) {\n const MatrixXd& A = _jacobianOplus[i];\n\n MatrixXd AtO = A.transpose() * omega;\n int fromDim = from->dimension();\n assert(fromDim >= 0);\n Eigen::Map fromMap(from->hessianData(), fromDim, fromDim);\n Eigen::Map fromB(from->bData(), fromDim);\n\n // ii block in the hessian\n#ifdef G2O_OPENMP\n from->lockQuadraticForm();\n#endif\n fromMap.noalias() += AtO * A;\n fromB.noalias() += A.transpose() * weightedError;\n\n // compute the off-diagonal blocks ij for all j\n for (size_t j = i+1; j < _vertices.size(); ++j) {\n OptimizableGraph::Vertex* to = static_cast(_vertices[j]);\n#ifdef G2O_OPENMP\n to->lockQuadraticForm();\n#endif\n bool jstatus = !(to->fixed());\n if (jstatus) {\n const MatrixXd& B = _jacobianOplus[j];\n int idx = internal::computeUpperTriangleIndex(i, j);\n assert(idx < (int)_hessian.size());\n HessianHelper& hhelper = _hessian[idx];\n if (hhelper.transposed) { // we have to write to the block as transposed\n hhelper.matrix.noalias() += B.transpose() * AtO.transpose();\n } else {\n hhelper.matrix.noalias() += AtO * B;\n }\n }\n#ifdef G2O_OPENMP\n to->unlockQuadraticForm();\n#endif\n }\n\n#ifdef G2O_OPENMP\n from->unlockQuadraticForm();\n#endif\n }\n\n }\n}\n" }, { "path": "Thirdparty/g2o/g2o/core/base_unary_edge.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_BASE_UNARY_EDGE_H\n#define G2O_BASE_UNARY_EDGE_H\n\n#include \n#include \n#include \n\n#include \"base_edge.h\"\n#include \"robust_kernel.h\"\n#include \"../../config.h\"\n\nnamespace g2o {\n\n using namespace Eigen;\n\n template \n class BaseUnaryEdge : public BaseEdge\n {\n public:\n static const int Dimension = BaseEdge::Dimension;\n typedef typename BaseEdge::Measurement Measurement;\n typedef VertexXi VertexXiType;\n typedef typename Matrix::AlignedMapType JacobianXiOplusType;\n typedef typename BaseEdge::ErrorVector ErrorVector;\n typedef typename BaseEdge::InformationType InformationType;\n\n BaseUnaryEdge() : BaseEdge(),\n _jacobianOplusXi(0, D, VertexXiType::Dimension)\n {\n _vertices.resize(1);\n }\n\n virtual void resize(size_t size);\n\n virtual bool allVerticesFixed() const;\n\n virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);\n\n /**\n * Linearizes the oplus operator in the vertex, and stores\n * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj\n */\n virtual void linearizeOplus();\n\n //! returns the result of the linearization in the manifold space for the node xi\n const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}\n\n virtual void constructQuadraticForm();\n\n virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to);\n\n virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && \"BaseUnaryEdge does not map memory of the Hessian\");}\n\n using BaseEdge::resize;\n using BaseEdge::computeError;\n\n protected:\n using BaseEdge::_measurement;\n using BaseEdge::_information;\n using BaseEdge::_error;\n using BaseEdge::_vertices;\n using BaseEdge::_dimension;\n\n JacobianXiOplusType _jacobianOplusXi;\n\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n };\n\n#include \"base_unary_edge.hpp\"\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/base_unary_edge.hpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\ntemplate \nvoid BaseUnaryEdge::resize(size_t size)\n{\n if (size != 1) {\n std::cerr << \"WARNING, attempting to resize unary edge \" << BaseEdge::id() << \" to \" << size << std::endl;\n }\n BaseEdge::resize(size);\n}\n\ntemplate \nbool BaseUnaryEdge::allVerticesFixed() const\n{\n return static_cast (_vertices[0])->fixed();\n}\n\ntemplate \nvoid BaseUnaryEdge::constructQuadraticForm()\n{\n VertexXiType* from=static_cast(_vertices[0]);\n\n // chain rule to get the Jacobian of the nodes in the manifold domain\n const JacobianXiOplusType& A = jacobianOplusXi();\n const InformationType& omega = _information;\n\n bool istatus = !from->fixed();\n if (istatus) {\n#ifdef G2O_OPENMP\n from->lockQuadraticForm();\n#endif\n if (this->robustKernel()) {\n double error = this->chi2();\n Eigen::Vector3d rho;\n this->robustKernel()->robustify(error, rho);\n InformationType weightedOmega = this->robustInformation(rho);\n\n from->b().noalias() -= rho[1] * A.transpose() * omega * _error;\n from->A().noalias() += A.transpose() * weightedOmega * A;\n } else {\n from->b().noalias() -= A.transpose() * omega * _error;\n from->A().noalias() += A.transpose() * omega * A;\n }\n#ifdef G2O_OPENMP\n from->unlockQuadraticForm();\n#endif\n }\n}\n\ntemplate \nvoid BaseUnaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace)\n{\n new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension);\n linearizeOplus();\n}\n\ntemplate \nvoid BaseUnaryEdge::linearizeOplus()\n{\n //Xi - estimate the jacobian numerically\n VertexXiType* vi = static_cast(_vertices[0]);\n\n if (vi->fixed())\n return;\n\n#ifdef G2O_OPENMP\n vi->lockQuadraticForm();\n#endif\n\n const double delta = 1e-9;\n const double scalar = 1.0 / (2*delta);\n ErrorVector error1;\n ErrorVector errorBeforeNumeric = _error;\n\n double add_vi[VertexXiType::Dimension];\n std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);\n // add small step along the unit vector in each dimension\n for (int d = 0; d < VertexXiType::Dimension; ++d) {\n vi->push();\n add_vi[d] = delta;\n vi->oplus(add_vi);\n computeError();\n error1 = _error;\n vi->pop();\n vi->push();\n add_vi[d] = -delta;\n vi->oplus(add_vi);\n computeError();\n vi->pop();\n add_vi[d] = 0.0;\n\n _jacobianOplusXi.col(d) = scalar * (error1 - _error);\n } // end dimension\n\n _error = errorBeforeNumeric;\n#ifdef G2O_OPENMP\n vi->unlockQuadraticForm();\n#endif\n}\n\ntemplate \nvoid BaseUnaryEdge::initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)\n{\n std::cerr << __PRETTY_FUNCTION__ << \" is not implemented, please give implementation in your derived class\" << std::endl;\n}\n" }, { "path": "Thirdparty/g2o/g2o/core/base_vertex.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_BASE_VERTEX_H\n#define G2O_BASE_VERTEX_H\n\n#include \"optimizable_graph.h\"\n#include \"creators.h\"\n#include \"../stuff/macros.h\"\n\n#include \n#include \n#include \n#include \n#include \n\nnamespace g2o {\n\n using namespace Eigen;\n\n\n/**\n * \\brief Templatized BaseVertex\n *\n * Templatized BaseVertex\n * D : minimal dimension of the vertex, e.g., 3 for rotation in 3D\n * T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D\n */\n template \n class BaseVertex : public OptimizableGraph::Vertex {\n public:\n typedef T EstimateType;\n typedef std::stack > >\n BackupStackType;\n\n static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space\n\n typedef Eigen::Map, Matrix::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;\n\n public:\n BaseVertex();\n\n virtual const double& hessian(int i, int j) const { assert(i(_hessian.data());}\n\n virtual void mapHessianMemory(double* d);\n\n virtual int copyB(double* b_) const {\n memcpy(b_, _b.data(), Dimension * sizeof(double));\n return Dimension; \n }\n\n virtual const double& b(int i) const { assert(i < D); return _b(i);}\n virtual double& b(int i) { assert(i < D); return _b(i);}\n virtual double* bData() { return _b.data();}\n\n virtual void clearQuadraticForm();\n\n //! updates the current vertex with the direct solution x += H_ii\\b_ii\n //! @returns the determinant of the inverted hessian\n virtual double solveDirect(double lambda=0);\n\n //! return right hand side b of the constructed linear system\n Matrix& b() { return _b;}\n const Matrix& b() const { return _b;}\n //! return the hessian block associated with the vertex\n HessianBlockType& A() { return _hessian;}\n const HessianBlockType& A() const { return _hessian;}\n\n virtual void push() { _backup.push(_estimate);}\n virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}\n virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}\n virtual int stackSize() const {return _backup.size();}\n\n //! return the current estimate of the vertex\n const EstimateType& estimate() const { return _estimate;}\n //! set the estimate for the vertex also calls updateCache()\n void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}\n\n protected:\n HessianBlockType _hessian;\n Matrix _b;\n EstimateType _estimate;\n BackupStackType _backup;\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n};\n\n#include \"base_vertex.hpp\"\n\n} // end namespace g2o\n\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/base_vertex.hpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\ntemplate \nBaseVertex::BaseVertex() :\n OptimizableGraph::Vertex(),\n _hessian(0, D, D)\n{\n _dimension = D;\n}\n\ntemplate \ndouble BaseVertex::solveDirect(double lambda) {\n Matrix tempA=_hessian + Matrix ::Identity()*lambda;\n double det=tempA.determinant();\n if (g2o_isnan(det) || det < std::numeric_limits::epsilon())\n return det;\n Matrix dx=tempA.llt().solve(_b);\n oplus(&dx[0]);\n return det;\n}\n\ntemplate \nvoid BaseVertex::clearQuadraticForm() {\n _b.setZero();\n}\n\ntemplate \nvoid BaseVertex::mapHessianMemory(double* d)\n{\n new (&_hessian) HessianBlockType(d, D, D);\n}\n" }, { "path": "Thirdparty/g2o/g2o/core/batch_stats.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"batch_stats.h\"\n#include \n\nnamespace g2o {\n using namespace std;\n\n G2OBatchStatistics* G2OBatchStatistics::_globalStats=0;\n\n #ifndef PTHING\n #define PTHING(s) \\\n #s << \"= \" << (st.s) << \"\\t \"\n #endif\n\n G2OBatchStatistics::G2OBatchStatistics(){\n // zero all.\n memset (this, 0, sizeof(G2OBatchStatistics));\n\n // set the iteration to -1 to show that it isn't valid\n iteration = -1;\n }\n\n std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st)\n {\n os << PTHING(iteration);\n\n os << PTHING( numVertices ); // how many vertices are involved\n os << PTHING( numEdges ); // hoe many edges\n os << PTHING( chi2 ); // total chi2\n \n /** timings **/\n // nonlinear part\n os << PTHING( timeResiduals ); \n os << PTHING( timeLinearize ); // jacobians\n os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph\n \n // block_solver (constructs Ax=b, plus maybe schur);\n os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done);\n \n // linear solver (computes Ax=b); );\n os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done);\n os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done);\n os << PTHING( timeLinearSolution ); // total time for solving Ax=b\n os << PTHING( iterationsLinearSolver ); // iterations of PCG\n os << PTHING( timeUpdate ); // oplus\n os << PTHING( timeIteration ); // total time );\n\n os << PTHING( levenbergIterations );\n os << PTHING( timeLinearSolver);\n\n os << PTHING(hessianDimension);\n os << PTHING(hessianPoseDimension);\n os << PTHING(hessianLandmarkDimension);\n os << PTHING(choleskyNNZ);\n os << PTHING(timeMarginals);\n\n return os;\n };\n\n void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b)\n {\n _globalStats = b;\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/batch_stats.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_BATCH_STATS_H_\n#define G2O_BATCH_STATS_H_\n\n#include \n#include \n\n\nnamespace g2o {\n\n /**\n * \\brief statistics about the optimization\n */\n struct G2OBatchStatistics {\n G2OBatchStatistics();\n int iteration; ///< which iteration\n int numVertices; ///< how many vertices are involved\n int numEdges; ///< how many edges\n double chi2; ///< total chi2\n\n /** timings **/\n // nonlinear part\n double timeResiduals; ///< residuals\n double timeLinearize; ///< jacobians\n double timeQuadraticForm; ///< construct the quadratic form in the graph\n int levenbergIterations; ///< number of iterations performed by LM\n // block_solver (constructs Ax=b, plus maybe schur)\n double timeSchurComplement; ///< compute schur complement (0 if not done)\n\n // linear solver (computes Ax=b);\n double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done)\n double timeNumericDecomposition; ///< numeric decomposition (0 if not done)\n double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur)\n double timeLinearSolver; ///< time for solving, excluding Schur setup\n int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky)\n double timeUpdate; ///< time to apply the update\n double timeIteration; ///< total time;\n\n double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances\n\n // information about the Hessian matrix\n size_t hessianDimension; ///< rows / cols of the Hessian\n size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur\n size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur\n size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor\n\n static G2OBatchStatistics* globalStats() {return _globalStats;}\n static void setGlobalStats(G2OBatchStatistics* b);\n protected:\n static G2OBatchStatistics* _globalStats;\n };\n\n std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&);\n\n typedef std::vector BatchStatisticsContainer;\n}\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/block_solver.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_BLOCK_SOLVER_H\n#define G2O_BLOCK_SOLVER_H\n#include \n#include \"solver.h\"\n#include \"linear_solver.h\"\n#include \"sparse_block_matrix.h\"\n#include \"sparse_block_matrix_diagonal.h\"\n#include \"openmp_mutex.h\"\n#include \"../../config.h\"\n\nnamespace g2o {\n using namespace Eigen;\n\n /**\n * \\brief traits to summarize the properties of the fixed size optimization problem\n */\n template \n struct BlockSolverTraits\n {\n static const int PoseDim = _PoseDim;\n static const int LandmarkDim = _LandmarkDim;\n typedef Matrix PoseMatrixType;\n typedef Matrix LandmarkMatrixType;\n typedef Matrix PoseLandmarkMatrixType;\n typedef Matrix PoseVectorType;\n typedef Matrix LandmarkVectorType;\n\n typedef SparseBlockMatrix PoseHessianType;\n typedef SparseBlockMatrix LandmarkHessianType;\n typedef SparseBlockMatrix PoseLandmarkHessianType;\n typedef LinearSolver LinearSolverType;\n };\n\n /**\n * \\brief traits to summarize the properties of the dynamic size optimization problem\n */\n template <>\n struct BlockSolverTraits\n {\n static const int PoseDim = Eigen::Dynamic;\n static const int LandmarkDim = Eigen::Dynamic;\n typedef MatrixXd PoseMatrixType;\n typedef MatrixXd LandmarkMatrixType;\n typedef MatrixXd PoseLandmarkMatrixType;\n typedef VectorXd PoseVectorType;\n typedef VectorXd LandmarkVectorType;\n\n typedef SparseBlockMatrix PoseHessianType;\n typedef SparseBlockMatrix LandmarkHessianType;\n typedef SparseBlockMatrix PoseLandmarkHessianType;\n typedef LinearSolver LinearSolverType;\n };\n\n /**\n * \\brief base for the block solvers with some basic function interfaces\n */\n class BlockSolverBase : public Solver\n {\n public:\n virtual ~BlockSolverBase() {}\n /**\n * compute dest = H * src\n */\n virtual void multiplyHessian(double* dest, const double* src) const = 0;\n };\n\n /**\n * \\brief Implementation of a solver operating on the blocks of the Hessian\n */\n template \n class BlockSolver: public BlockSolverBase {\n public:\n\n static const int PoseDim = Traits::PoseDim;\n static const int LandmarkDim = Traits::LandmarkDim;\n typedef typename Traits::PoseMatrixType PoseMatrixType;\n typedef typename Traits::LandmarkMatrixType LandmarkMatrixType; \n typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;\n typedef typename Traits::PoseVectorType PoseVectorType;\n typedef typename Traits::LandmarkVectorType LandmarkVectorType;\n\n typedef typename Traits::PoseHessianType PoseHessianType;\n typedef typename Traits::LandmarkHessianType LandmarkHessianType;\n typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;\n typedef typename Traits::LinearSolverType LinearSolverType;\n\n public:\n\n /**\n * allocate a block solver ontop of the underlying linear solver.\n * NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer\n * in its destructor.\n */\n BlockSolver(LinearSolverType* linearSolver);\n ~BlockSolver();\n\n virtual bool init(SparseOptimizer* optmizer, bool online = false);\n virtual bool buildStructure(bool zeroBlocks = false);\n virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges);\n virtual bool buildSystem();\n virtual bool solve();\n virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices);\n virtual bool setLambda(double lambda, bool backup = false);\n virtual void restoreDiagonal();\n virtual bool supportsSchur() {return true;}\n virtual bool schur() { return _doSchur;}\n virtual void setSchur(bool s) { _doSchur = s;}\n\n LinearSolver* linearSolver() const { return _linearSolver;}\n\n virtual void setWriteDebug(bool writeDebug);\n virtual bool writeDebug() const {return _linearSolver->writeDebug();}\n\n virtual bool saveHessian(const std::string& fileName) const;\n\n virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}\n\n protected:\n void resize(int* blockPoseIndices, int numPoseBlocks, \n int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);\n\n void deallocate();\n\n SparseBlockMatrix* _Hpp;\n SparseBlockMatrix* _Hll;\n SparseBlockMatrix* _Hpl;\n\n SparseBlockMatrix* _Hschur;\n SparseBlockMatrixDiagonal* _DInvSchur;\n\n SparseBlockMatrixCCS* _HplCCS;\n SparseBlockMatrixCCS* _HschurTransposedCCS;\n\n LinearSolver* _linearSolver;\n\n std::vector > _diagonalBackupPose;\n std::vector > _diagonalBackupLandmark;\n\n# ifdef G2O_OPENMP\n std::vector _coefficientsMutex;\n# endif\n\n bool _doSchur;\n\n double* _coefficients;\n double* _bschur;\n\n int _numPoses, _numLandmarks;\n int _sizePoses, _sizeLandmarks;\n };\n\n\n //variable size solver\n typedef BlockSolver< BlockSolverTraits > BlockSolverX;\n // solver for BA/3D SLAM\n typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3; \n // solver fo BA with scale\n typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3; \n typedef BlockSolver< BlockSolverTraits<6, 9> > BlockSolver_6_9; \n // 2Dof landmarks 3Dof poses\n typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;\n\n} // end namespace\n\n#include \"block_solver.hpp\"\n\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/block_solver.hpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"sparse_optimizer.h\"\n#include \n#include \n#include \n\n#include \"../stuff/timeutil.h\"\n#include \"../stuff/macros.h\"\n#include \"../stuff/misc.h\"\n\nnamespace g2o {\n\nusing namespace std;\nusing namespace Eigen;\n\ntemplate \nBlockSolver::BlockSolver(LinearSolverType* linearSolver) :\n BlockSolverBase(),\n _linearSolver(linearSolver)\n{\n // workspace\n _Hpp=0;\n _Hll=0;\n _Hpl=0;\n _HplCCS = 0;\n _HschurTransposedCCS = 0;\n _Hschur=0;\n _DInvSchur=0;\n _coefficients=0;\n _bschur = 0;\n _xSize=0;\n _numPoses=0;\n _numLandmarks=0;\n _sizePoses=0;\n _sizeLandmarks=0;\n _doSchur=true;\n}\n\ntemplate \nvoid BlockSolver::resize(int* blockPoseIndices, int numPoseBlocks, \n int* blockLandmarkIndices, int numLandmarkBlocks,\n int s)\n{\n deallocate();\n\n resizeVector(s);\n\n if (_doSchur) {\n // the following two are only used in schur\n assert(_sizePoses > 0 && \"allocating with wrong size\");\n _coefficients = new double [s];\n _bschur = new double[_sizePoses];\n }\n\n _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);\n if (_doSchur) {\n _Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);\n _Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);\n _DInvSchur = new SparseBlockMatrixDiagonal(_Hll->colBlockIndices());\n _Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);\n _HplCCS = new SparseBlockMatrixCCS(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());\n _HschurTransposedCCS = new SparseBlockMatrixCCS(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());\n#ifdef G2O_OPENMP\n _coefficientsMutex.resize(numPoseBlocks);\n#endif\n }\n}\n\ntemplate \nvoid BlockSolver::deallocate()\n{\n if (_Hpp){\n delete _Hpp;\n _Hpp=0;\n }\n if (_Hll){\n delete _Hll;\n _Hll=0;\n }\n if (_Hpl){\n delete _Hpl;\n _Hpl = 0;\n }\n if (_Hschur){\n delete _Hschur;\n _Hschur=0;\n }\n if (_DInvSchur){\n delete _DInvSchur;\n _DInvSchur=0;\n }\n if (_coefficients) {\n delete[] _coefficients;\n _coefficients = 0;\n }\n if (_bschur) {\n delete[] _bschur;\n _bschur = 0;\n }\n if (_HplCCS) {\n delete _HplCCS;\n _HplCCS = 0;\n }\n if (_HschurTransposedCCS) {\n delete _HschurTransposedCCS;\n _HschurTransposedCCS = 0;\n }\n}\n\ntemplate \nBlockSolver::~BlockSolver()\n{\n delete _linearSolver;\n deallocate();\n}\n\ntemplate \nbool BlockSolver::buildStructure(bool zeroBlocks)\n{\n assert(_optimizer);\n\n size_t sparseDim = 0;\n _numPoses=0;\n _numLandmarks=0;\n _sizePoses=0;\n _sizeLandmarks=0;\n int* blockPoseIndices = new int[_optimizer->indexMapping().size()];\n int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];\n\n for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {\n OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];\n int dim = v->dimension();\n if (! v->marginalized()){\n v->setColInHessian(_sizePoses);\n _sizePoses+=dim;\n blockPoseIndices[_numPoses]=_sizePoses;\n ++_numPoses;\n } else {\n v->setColInHessian(_sizeLandmarks);\n _sizeLandmarks+=dim;\n blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;\n ++_numLandmarks;\n }\n sparseDim += dim;\n }\n resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);\n delete[] blockLandmarkIndices;\n delete[] blockPoseIndices;\n\n // allocate the diagonal on Hpp and Hll\n int poseIdx = 0;\n int landmarkIdx = 0;\n for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {\n OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];\n if (! v->marginalized()){\n //assert(poseIdx == v->hessianIndex());\n PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);\n if (zeroBlocks)\n m->setZero();\n v->mapHessianMemory(m->data());\n ++poseIdx;\n } else {\n LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);\n if (zeroBlocks)\n m->setZero();\n v->mapHessianMemory(m->data());\n ++landmarkIdx;\n }\n }\n assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);\n\n // temporary structures for building the pattern of the Schur complement\n SparseBlockMatrixHashMap* schurMatrixLookup = 0;\n if (_doSchur) {\n schurMatrixLookup = new SparseBlockMatrixHashMap(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());\n schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());\n }\n\n // here we assume that the landmark indices start after the pose ones\n // create the structure in Hpp, Hll and in Hpl\n for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){\n OptimizableGraph::Edge* e = *it;\n\n for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {\n OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);\n int ind1 = v1->hessianIndex();\n if (ind1 == -1)\n continue;\n int indexV1Bak = ind1;\n for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {\n OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);\n int ind2 = v2->hessianIndex();\n if (ind2 == -1)\n continue;\n ind1 = indexV1Bak;\n bool transposedBlock = ind1 > ind2;\n if (transposedBlock){ // make sure, we allocate the upper triangle block\n swap(ind1, ind2);\n }\n if (! v1->marginalized() && !v2->marginalized()){\n PoseMatrixType* m = _Hpp->block(ind1, ind2, true);\n if (zeroBlocks)\n m->setZero();\n e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);\n if (_Hschur) {// assume this is only needed in case we solve with the schur complement\n schurMatrixLookup->addBlock(ind1, ind2);\n }\n } else if (v1->marginalized() && v2->marginalized()){\n // RAINER hmm.... should we ever reach this here????\n LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);\n if (zeroBlocks)\n m->setZero();\n e->mapHessianMemory(m->data(), viIdx, vjIdx, false);\n } else { \n if (v1->marginalized()){ \n PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);\n if (zeroBlocks)\n m->setZero();\n e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it\n } else {\n PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);\n if (zeroBlocks)\n m->setZero();\n e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block\n }\n }\n }\n }\n }\n\n if (! _doSchur)\n return true;\n\n _DInvSchur->diagonal().resize(landmarkIdx);\n _Hpl->fillSparseBlockMatrixCCS(*_HplCCS);\n\n for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {\n OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];\n if (v->marginalized()){\n const HyperGraph::EdgeSet& vedges=v->edges();\n for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){\n for (size_t i=0; i<(*it1)->vertices().size(); ++i)\n {\n OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);\n if (v1->hessianIndex()==-1 || v1==v)\n continue;\n for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){\n for (size_t j=0; j<(*it2)->vertices().size(); ++j)\n {\n OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);\n if (v2->hessianIndex()==-1 || v2==v)\n continue;\n int i1=v1->hessianIndex();\n int i2=v2->hessianIndex();\n if (i1<=i2) {\n schurMatrixLookup->addBlock(i1, i2);\n }\n }\n }\n }\n }\n }\n }\n\n _Hschur->takePatternFromHash(*schurMatrixLookup);\n delete schurMatrixLookup;\n _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);\n\n return true;\n}\n\ntemplate \nbool BlockSolver::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges)\n{\n for (std::vector::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {\n OptimizableGraph::Vertex* v = static_cast(*vit);\n int dim = v->dimension();\n if (! v->marginalized()){\n v->setColInHessian(_sizePoses);\n _sizePoses+=dim;\n _Hpp->rowBlockIndices().push_back(_sizePoses);\n _Hpp->colBlockIndices().push_back(_sizePoses);\n _Hpp->blockCols().push_back(typename SparseBlockMatrix::IntBlockMap());\n ++_numPoses;\n int ind = v->hessianIndex();\n PoseMatrixType* m = _Hpp->block(ind, ind, true);\n v->mapHessianMemory(m->data());\n } else {\n std::cerr << \"updateStructure(): Schur not supported\" << std::endl;\n abort();\n }\n }\n resizeVector(_sizePoses + _sizeLandmarks);\n\n for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {\n OptimizableGraph::Edge* e = static_cast(*it);\n\n for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {\n OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);\n int ind1 = v1->hessianIndex();\n int indexV1Bak = ind1;\n if (ind1 == -1)\n continue;\n for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {\n OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);\n int ind2 = v2->hessianIndex();\n if (ind2 == -1)\n continue;\n ind1 = indexV1Bak;\n bool transposedBlock = ind1 > ind2;\n if (transposedBlock) // make sure, we allocate the upper triangular block\n swap(ind1, ind2);\n\n if (! v1->marginalized() && !v2->marginalized()) {\n PoseMatrixType* m = _Hpp->block(ind1, ind2, true);\n e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);\n } else { \n std::cerr << __PRETTY_FUNCTION__ << \": not supported\" << std::endl;\n }\n }\n }\n\n }\n\n return true;\n}\n\ntemplate \nbool BlockSolver::solve(){\n //cerr << __PRETTY_FUNCTION__ << endl;\n if (! _doSchur){\n double t=get_monotonic_time();\n bool ok = _linearSolver->solve(*_Hpp, _x, _b);\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats) {\n globalStats->timeLinearSolver = get_monotonic_time() - t;\n globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();\n }\n return ok;\n }\n\n // schur thing\n\n // backup the coefficient matrix\n double t=get_monotonic_time();\n\n // _Hschur = _Hpp, but keeping the pattern of _Hschur\n _Hschur->clear();\n _Hpp->add(_Hschur);\n\n //_DInvSchur->clear();\n memset (_coefficients, 0, _sizePoses*sizeof(double));\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) schedule(dynamic, 10)\n# endif\n for (int landmarkIndex = 0; landmarkIndex < static_cast(_Hll->blockCols().size()); ++landmarkIndex) {\n const typename SparseBlockMatrix::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];\n assert(marginalizeColumn.size() == 1 && \"more than one block in _Hll column\");\n\n // calculate inverse block for the landmark\n const LandmarkMatrixType * D = marginalizeColumn.begin()->second;\n assert (D && D->rows()==D->cols() && \"Error in landmark matrix\");\n LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];\n Dinv = D->inverse();\n\n LandmarkVectorType db(D->rows());\n for (int j=0; jrows(); ++j) {\n db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];\n }\n db=Dinv*db;\n\n assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && \"Index out of bounds\");\n const typename SparseBlockMatrixCCS::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];\n\n for (typename SparseBlockMatrixCCS::SparseColumn::const_iterator it_outer = landmarkColumn.begin();\n it_outer != landmarkColumn.end(); ++it_outer) {\n int i1 = it_outer->row;\n\n const PoseLandmarkMatrixType* Bi = it_outer->block;\n assert(Bi);\n\n PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);\n assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && \"Index out of bounds\");\n typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());\n# ifdef G2O_OPENMP\n ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);\n# endif\n Bb.noalias() += (*Bi)*db;\n\n assert(i1 >= 0 && i1 < static_cast(_HschurTransposedCCS->blockCols().size()) && \"Index out of bounds\");\n typename SparseBlockMatrixCCS::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();\n\n typename SparseBlockMatrixCCS::RowBlock aux(i1, 0);\n typename SparseBlockMatrixCCS::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);\n for (; it_inner != landmarkColumn.end(); ++it_inner) {\n int i2 = it_inner->row;\n const PoseLandmarkMatrixType* Bj = it_inner->block;\n assert(Bj); \n while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)\n ++targetColumnIt;\n assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && \"invalid iterator, something wrong with the matrix structure\");\n PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);\n assert(Hi1i2);\n (*Hi1i2).noalias() -= BDinv*Bj->transpose();\n }\n }\n }\n //cerr << \"Solve [marginalize] = \" << get_monotonic_time()-t << endl;\n\n // _bschur = _b for calling solver, and not touching _b\n memcpy(_bschur, _b, _sizePoses * sizeof(double));\n for (int i=0; i<_sizePoses; ++i){\n _bschur[i]-=_coefficients[i];\n }\n\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats){\n globalStats->timeSchurComplement = get_monotonic_time() - t;\n }\n\n t=get_monotonic_time();\n bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);\n if (globalStats) {\n globalStats->timeLinearSolver = get_monotonic_time() - t;\n globalStats->hessianPoseDimension = _Hpp->cols();\n globalStats->hessianLandmarkDimension = _Hll->cols();\n globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;\n }\n //cerr << \"Solve [decompose and solve] = \" << get_monotonic_time()-t << endl;\n\n if (! solvedPoses)\n return false;\n\n // _x contains the solution for the poses, now applying it to the landmarks to get the new part of the\n // solution;\n double* xp = _x;\n double* cp = _coefficients;\n\n double* xl=_x+_sizePoses;\n double* cl=_coefficients + _sizePoses;\n double* bl=_b+_sizePoses;\n\n // cp = -xp\n for (int i=0; i<_sizePoses; ++i)\n cp[i]=-xp[i];\n\n // cl = bl\n memcpy(cl,bl,_sizeLandmarks*sizeof(double));\n\n // cl = bl - Bt * xp\n //Bt->multiply(cl, cp);\n _HplCCS->rightMultiply(cl, cp);\n\n // xl = Dinv * cl\n memset(xl,0, _sizeLandmarks*sizeof(double));\n _DInvSchur->multiply(xl,cl);\n //_DInvSchur->rightMultiply(xl,cl);\n //cerr << \"Solve [landmark delta] = \" << get_monotonic_time()-t << endl;\n\n return true;\n}\n\n\ntemplate \nbool BlockSolver::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices)\n{\n double t = get_monotonic_time();\n bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats) {\n globalStats->timeMarginals = get_monotonic_time() - t;\n }\n return ok;\n}\n\ntemplate \nbool BlockSolver::buildSystem()\n{\n // clear b vector\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)\n# endif\n for (int i = 0; i < static_cast(_optimizer->indexMapping().size()); ++i) {\n OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];\n assert(v);\n v->clearQuadraticForm();\n }\n _Hpp->clear();\n if (_doSchur) {\n _Hll->clear();\n _Hpl->clear();\n }\n\n // resetting the terms for the pairwise constraints\n // built up the current system by storing the Hessian blocks in the edges and vertices\n# ifndef G2O_OPENMP\n // no threading, we do not need to copy the workspace\n JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();\n# else\n // if running with threads need to produce copies of the workspace for each thread\n JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();\n# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)\n# endif\n for (int k = 0; k < static_cast(_optimizer->activeEdges().size()); ++k) {\n OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];\n e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)\n e->constructQuadraticForm();\n# ifndef NDEBUG\n for (size_t i = 0; i < e->vertices().size(); ++i) {\n const OptimizableGraph::Vertex* v = static_cast(e->vertex(i));\n if (! v->fixed()) {\n bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());\n if (hasANan) {\n cerr << \"buildSystem(): NaN within Jacobian for edge \" << e << \" for vertex \" << i << endl;\n break;\n }\n }\n }\n# endif\n }\n\n // flush the current system in a sparse block matrix\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)\n# endif\n for (int i = 0; i < static_cast(_optimizer->indexMapping().size()); ++i) {\n OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];\n int iBase = v->colInHessian();\n if (v->marginalized())\n iBase+=_sizePoses;\n v->copyB(_b+iBase);\n }\n\n return 0;\n}\n\n\ntemplate \nbool BlockSolver::setLambda(double lambda, bool backup)\n{\n if (backup) {\n _diagonalBackupPose.resize(_numPoses);\n _diagonalBackupLandmark.resize(_numLandmarks);\n }\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) if (_numPoses > 100)\n# endif\n for (int i = 0; i < _numPoses; ++i) {\n PoseMatrixType *b=_Hpp->block(i,i);\n if (backup)\n _diagonalBackupPose[i] = b->diagonal();\n b->diagonal().array() += lambda;\n }\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) if (_numLandmarks > 100)\n# endif\n for (int i = 0; i < _numLandmarks; ++i) {\n LandmarkMatrixType *b=_Hll->block(i,i);\n if (backup)\n _diagonalBackupLandmark[i] = b->diagonal();\n b->diagonal().array() += lambda;\n }\n return true;\n}\n\ntemplate \nvoid BlockSolver::restoreDiagonal()\n{\n assert((int) _diagonalBackupPose.size() == _numPoses && \"Mismatch in dimensions\");\n assert((int) _diagonalBackupLandmark.size() == _numLandmarks && \"Mismatch in dimensions\");\n for (int i = 0; i < _numPoses; ++i) {\n PoseMatrixType *b=_Hpp->block(i,i);\n b->diagonal() = _diagonalBackupPose[i];\n }\n for (int i = 0; i < _numLandmarks; ++i) {\n LandmarkMatrixType *b=_Hll->block(i,i);\n b->diagonal() = _diagonalBackupLandmark[i];\n }\n}\n\ntemplate \nbool BlockSolver::init(SparseOptimizer* optimizer, bool online)\n{\n _optimizer = optimizer;\n if (! online) {\n if (_Hpp)\n _Hpp->clear();\n if (_Hpl)\n _Hpl->clear();\n if (_Hll)\n _Hll->clear();\n }\n _linearSolver->init();\n return true;\n}\n\ntemplate \nvoid BlockSolver::setWriteDebug(bool writeDebug)\n{\n _linearSolver->setWriteDebug(writeDebug);\n}\n\ntemplate \nbool BlockSolver::saveHessian(const std::string& fileName) const\n{\n return _Hpp->writeOctave(fileName.c_str(), true);\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/cache.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"cache.h\"\n#include \"optimizable_graph.h\"\n#include \"factory.h\"\n\n#include \n\nnamespace g2o {\n using namespace std;\n\n Cache::CacheKey::CacheKey() :\n _type(), _parameters()\n {\n }\n\n Cache::CacheKey::CacheKey(const std::string& type_, const ParameterVector& parameters_) :\n _type(type_), _parameters(parameters_)\n {\n }\n\n Cache::Cache(CacheContainer* container_, const ParameterVector& parameters_) :\n _updateNeeded(true), _parameters(parameters_), _container(container_)\n {\n }\n\n bool Cache::CacheKey::operator<(const Cache::CacheKey& c) const{\n if (_type < c._type)\n return true;\n return std::lexicographical_compare (_parameters.begin( ), _parameters.end( ),\n c._parameters.begin( ), c._parameters.end( ) );\n }\n\n\n OptimizableGraph::Vertex* Cache::vertex() { \n if (container() ) \n return container()->vertex(); \n return 0; \n }\n\n OptimizableGraph* Cache::graph() {\n if (container())\n return container()->graph();\n return 0;\n }\n\n CacheContainer* Cache::container() {\n return _container;\n }\n\n ParameterVector& Cache::parameters() {\n return _parameters;\n }\n \n Cache::CacheKey Cache::key() const {\n Factory* factory=Factory::instance();\n return CacheKey(factory->tag(this), _parameters);\n };\n\n \n void Cache::update(){\n if (! _updateNeeded)\n return;\n for(std::vector::iterator it=_parentCaches.begin(); it!=_parentCaches.end(); it++){\n (*it)->update();\n }\n updateImpl();\n _updateNeeded=false;\n }\n\n Cache* Cache::installDependency(const std::string& type_, const std::vector& parameterIndices){\n ParameterVector pv(parameterIndices.size());\n for (size_t i=0; i=(int)_parameters.size())\n return 0;\n pv[i]=_parameters[ parameterIndices[i] ];\n }\n CacheKey k(type_, pv);\n if (!container())\n return 0;\n Cache* c=container()->findCache(k);\n if (!c) {\n c = container()->createCache(k);\n }\n if (c)\n _parentCaches.push_back(c);\n return c;\n }\n \n bool Cache::resolveDependancies(){\n return true;\n }\n\n CacheContainer::CacheContainer(OptimizableGraph::Vertex* vertex_) {\n _vertex = vertex_;\n }\n\n Cache* CacheContainer::findCache(const Cache::CacheKey& key) {\n iterator it=find(key);\n if (it==end())\n return 0;\n return it->second;\n }\n \n Cache* CacheContainer::createCache(const Cache::CacheKey& key){\n Factory* f = Factory::instance();\n HyperGraph::HyperGraphElement* e = f->construct(key.type());\n if (!e) {\n cerr << __PRETTY_FUNCTION__ << endl;\n cerr << \"fatal error in creating cache of type \" << key.type() << endl;\n return 0;\n }\n Cache* c = dynamic_cast(e);\n if (! c){\n cerr << __PRETTY_FUNCTION__ << endl;\n cerr << \"fatal error in creating cache of type \" << key.type() << endl;\n return 0;\n }\n c->_container = this;\n c->_parameters = key._parameters;\n if (c->resolveDependancies()){\n insert(make_pair(key,c));\n c->update();\n return c;\n } \n return 0;\n }\n \n OptimizableGraph::Vertex* CacheContainer::vertex() {\n return _vertex;\n }\n\n OptimizableGraph* CacheContainer::graph(){\n if (_vertex)\n return _vertex->graph();\n return 0;\n }\n\n void CacheContainer::update() {\n for (iterator it=begin(); it!=end(); it++){\n (it->second)->update();\n }\n _updateNeeded=false;\n }\n\n void CacheContainer::setUpdateNeeded(bool needUpdate) {\n _updateNeeded=needUpdate;\n for (iterator it=begin(); it!=end(); ++it){\n (it->second)->_updateNeeded = needUpdate;\n }\n }\n\n CacheContainer::~CacheContainer(){\n for (iterator it=begin(); it!=end(); ++it){\n delete (it->second);\n }\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/cache.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_CACHE_HH_\n#define G2O_CACHE_HH_\n\n#include \n\n#include \"optimizable_graph.h\"\n\nnamespace g2o {\n\n class CacheContainer;\n \n class Cache: public HyperGraph::HyperGraphElement\n {\n public:\n friend class CacheContainer;\n class CacheKey\n {\n public:\n friend class CacheContainer;\n CacheKey();\n CacheKey(const std::string& type_, const ParameterVector& parameters_);\n\n bool operator<(const CacheKey& c) const;\n\n const std::string& type() const { return _type;}\n const ParameterVector& parameters() const { return _parameters;}\n\n protected:\n std::string _type;\n ParameterVector _parameters;\n };\n\n Cache(CacheContainer* container_ = 0, const ParameterVector& parameters_ = ParameterVector());\n\n CacheKey key() const;\n\n OptimizableGraph::Vertex* vertex();\n OptimizableGraph* graph();\n CacheContainer* container();\n ParameterVector& parameters();\n\n void update();\n\n virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_CACHE;}\n\n protected:\n //! redefine this to do the update\n virtual void updateImpl() = 0;\n\n /**\n * this function installs and satisfies a cache\n * @param type_: the typename of the dependency\n * @param parameterIndices: a vector containing the indices if the parameters\n * in _parameters that will be used to assemble the Key of the cache being created\n * For example if I have a cache of type C2, having parameters \"A, B, and C\",\n * and it depends on a cache of type C1 that depends on the parameters A and C, \n * the parameterIndices should contain \"0, 2\", since they are the positions in the\n * parameter vector of C2 of the parameters needed to construct C1.\n * @returns the newly created cache\n */\n Cache* installDependency(const std::string& type_, const std::vector& parameterIndices);\n\n /**\n * Function to be called from a cache that has dependencies. It just invokes a\n * sequence of installDependency().\n * Although the caches returned are stored in the _parentCache vector,\n * it is better that you redefine your own cache member variables, for better readability\n */\n virtual bool resolveDependancies();\n\n bool _updateNeeded;\n ParameterVector _parameters;\n std::vector _parentCaches;\n CacheContainer* _container;\n };\n\n class CacheContainer: public std::map\n {\n public:\n CacheContainer(OptimizableGraph::Vertex* vertex_);\n virtual ~CacheContainer();\n OptimizableGraph::Vertex* vertex();\n OptimizableGraph* graph();\n Cache* findCache(const Cache::CacheKey& key);\n Cache* createCache(const Cache::CacheKey& key);\n void setUpdateNeeded(bool needUpdate=true);\n void update();\n protected:\n OptimizableGraph::Vertex* _vertex;\n bool _updateNeeded;\n };\n\n\n template \n void OptimizableGraph::Edge::resolveCache(CacheType*& cache, \n OptimizableGraph::Vertex* v, \n const std::string& type_, \n const ParameterVector& parameters_)\n {\n cache = 0;\n CacheContainer* container= v->cacheContainer();\n Cache::CacheKey key(type_, parameters_);\n Cache* c = container->findCache(key);\n if (!c) {\n c = container->createCache(key);\n }\n if (c) {\n cache = dynamic_cast(c); \n }\n }\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/creators.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_CREATORS_H\n#define G2O_CREATORS_H\n\n#include \"hyper_graph.h\"\n\n#include \n#include \n\nnamespace g2o\n{\n\n /**\n * \\brief Abstract interface for allocating HyperGraphElement\n */\n class AbstractHyperGraphElementCreator\n {\n public:\n /**\n * create a hyper graph element. Has to implemented in derived class.\n */\n virtual HyperGraph::HyperGraphElement* construct() = 0;\n /**\n * name of the class to be created. Has to implemented in derived class.\n */\n virtual const std::string& name() const = 0;\n\n virtual ~AbstractHyperGraphElementCreator() { }\n };\n\n /**\n * \\brief templatized creator class which creates graph elements\n */\n template \n class HyperGraphElementCreator : public AbstractHyperGraphElementCreator\n {\n public:\n HyperGraphElementCreator() : _name(typeid(T).name()) {}\n#if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC\n __attribute__((force_align_arg_pointer))\n#endif\n HyperGraph::HyperGraphElement* construct() { return new T;}\n virtual const std::string& name() const { return _name;}\n protected:\n std::string _name;\n };\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/eigen_types.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_EIGEN_TYPES_H\n#define G2O_EIGEN_TYPES_H\n\n#include \n#include \n\nnamespace g2o {\n\n typedef Eigen::Matrix Vector2I;\n typedef Eigen::Matrix Vector3I;\n typedef Eigen::Matrix Vector4I;\n typedef Eigen::Matrix VectorXI; \n\n typedef Eigen::Matrix Vector2F; \n typedef Eigen::Matrix Vector3F; \n typedef Eigen::Matrix Vector4F; \n typedef Eigen::Matrix VectorXF; \n\n typedef Eigen::Matrix Vector2D;\n typedef Eigen::Matrix Vector3D;\n typedef Eigen::Matrix Vector4D;\n typedef Eigen::Matrix VectorXD;\n\n typedef Eigen::Matrix Matrix2I;\n typedef Eigen::Matrix Matrix3I;\n typedef Eigen::Matrix Matrix4I;\n typedef Eigen::Matrix MatrixXI;\n\n typedef Eigen::Matrix Matrix2F;\n typedef Eigen::Matrix Matrix3F;\n typedef Eigen::Matrix Matrix4F;\n typedef Eigen::Matrix MatrixXF;\n\n typedef Eigen::Matrix Matrix2D;\n typedef Eigen::Matrix Matrix3D;\n typedef Eigen::Matrix Matrix4D;\n typedef Eigen::Matrix MatrixXD;\n\n typedef Eigen::Transform Isometry2D;\n typedef Eigen::Transform Isometry3D;\n\n typedef Eigen::Transform Affine2D;\n typedef Eigen::Transform Affine3D;\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/estimate_propagator.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"estimate_propagator.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n//#define DEBUG_ESTIMATE_PROPAGATOR\n\nusing namespace std;\n\nnamespace g2o {\n\n# ifdef DEBUG_ESTIMATE_PROPAGATOR\n struct FrontierLevelCmp {\n bool operator()(EstimatePropagator::AdjacencyMapEntry* e1, EstimatePropagator::AdjacencyMapEntry* e2) const\n {\n return e1->frontierLevel() < e2->frontierLevel();\n }\n };\n# endif\n\n EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry()\n {\n reset();\n }\n\n void EstimatePropagator::AdjacencyMapEntry::reset()\n {\n _child = 0;\n _parent.clear();\n _edge = 0;\n _distance = numeric_limits::max();\n _frontierLevel = -1;\n inQueue = false;\n }\n\n EstimatePropagator::EstimatePropagator(OptimizableGraph* g): _graph(g)\n {\n for (OptimizableGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it){\n AdjacencyMapEntry entry;\n entry._child = static_cast(it->second);\n _adjacencyMap.insert(make_pair(entry.child(), entry));\n }\n }\n\n void EstimatePropagator::reset()\n {\n for (OptimizableGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); ++it){\n OptimizableGraph::Vertex* v = static_cast(*it);\n AdjacencyMap::iterator at = _adjacencyMap.find(v);\n assert(at != _adjacencyMap.end());\n at->second.reset();\n }\n _visited.clear();\n }\n\n void EstimatePropagator::propagate(OptimizableGraph::Vertex* v, \n const EstimatePropagator::PropagateCost& cost, \n const EstimatePropagator::PropagateAction& action,\n double maxDistance, \n double maxEdgeCost)\n {\n OptimizableGraph::VertexSet vset;\n vset.insert(v);\n propagate(vset, cost, action, maxDistance, maxEdgeCost);\n }\n\n void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset, \n const EstimatePropagator::PropagateCost& cost, \n const EstimatePropagator::PropagateAction& action,\n double maxDistance, \n double maxEdgeCost)\n {\n reset();\n\n PriorityQueue frontier;\n for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){\n OptimizableGraph::Vertex* v = static_cast(*vit);\n AdjacencyMap::iterator it = _adjacencyMap.find(v);\n assert(it != _adjacencyMap.end());\n it->second._distance = 0.;\n it->second._parent.clear();\n it->second._frontierLevel = 0;\n frontier.push(&it->second);\n }\n\n while(! frontier.empty()){\n AdjacencyMapEntry* entry = frontier.pop();\n OptimizableGraph::Vertex* u = entry->child();\n double uDistance = entry->distance();\n //cerr << \"uDistance \" << uDistance << endl;\n\n // initialize the vertex\n if (entry->_frontierLevel > 0) {\n action(entry->edge(), entry->parent(), u);\n }\n\n /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);\n OptimizableGraph::EdgeSet::iterator et = u->edges().begin();\n while (et != u->edges().end()){\n OptimizableGraph::Edge* edge = static_cast(*et);\n ++et;\n\n int maxFrontier = -1;\n OptimizableGraph::VertexSet initializedVertices;\n for (size_t i = 0; i < edge->vertices().size(); ++i) {\n OptimizableGraph::Vertex* z = static_cast(edge->vertex(i));\n AdjacencyMap::iterator ot = _adjacencyMap.find(z);\n if (ot->second._distance != numeric_limits::max()) {\n initializedVertices.insert(z);\n maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);\n }\n }\n assert(maxFrontier >= 0);\n\n for (size_t i = 0; i < edge->vertices().size(); ++i) {\n OptimizableGraph::Vertex* z = static_cast(edge->vertex(i));\n if (z == u)\n continue;\n\n size_t wasInitialized = initializedVertices.erase(z);\n\n double edgeDistance = cost(edge, initializedVertices, z);\n if (edgeDistance > 0. && edgeDistance != std::numeric_limits::max() && edgeDistance < maxEdgeCost) {\n double zDistance = uDistance + edgeDistance;\n //cerr << z->id() << \" \" << zDistance << endl;\n\n AdjacencyMap::iterator ot = _adjacencyMap.find(z);\n assert(ot!=_adjacencyMap.end());\n\n if (zDistance < ot->second.distance() && zDistance < maxDistance){\n //if (ot->second.inQueue)\n //cerr << \"Updating\" << endl;\n ot->second._distance = zDistance;\n ot->second._parent = initializedVertices;\n ot->second._edge = edge;\n ot->second._frontierLevel = maxFrontier + 1;\n frontier.push(&ot->second);\n }\n }\n\n if (wasInitialized > 0)\n initializedVertices.insert(z);\n\n }\n }\n }\n\n // writing debug information like cost for reaching each vertex and the parent used to initialize\n#ifdef DEBUG_ESTIMATE_PROPAGATOR\n cerr << \"Writing cost.dat\" << endl;\n ofstream costStream(\"cost.dat\");\n for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {\n HyperGraph::Vertex* u = it->second.child();\n costStream << \"vertex \" << u->id() << \" cost \" << it->second._distance << endl;\n }\n cerr << \"Writing init.dat\" << endl;\n ofstream initStream(\"init.dat\");\n vector frontierLevels;\n for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {\n if (it->second._frontierLevel > 0)\n frontierLevels.push_back(&it->second);\n }\n sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());\n for (vector::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {\n AdjacencyMapEntry* entry = *it;\n OptimizableGraph::Vertex* to = entry->child();\n\n initStream << \"calling init level = \" << entry->_frontierLevel << \"\\t (\";\n for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {\n initStream << \" \" << (*pit)->id();\n }\n initStream << \" ) -> \" << to->id() << endl;\n }\n#endif\n\n }\n\n void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry)\n {\n assert(entry != NULL);\n if (entry->inQueue) {\n assert(entry->queueIt->second == entry);\n erase(entry->queueIt);\n }\n\n entry->queueIt = insert(std::make_pair(entry->distance(), entry));\n assert(entry->queueIt != end());\n entry->inQueue = true;\n }\n\n EstimatePropagator::AdjacencyMapEntry* EstimatePropagator::PriorityQueue::pop()\n {\n assert(!empty());\n iterator it = begin();\n AdjacencyMapEntry* entry = it->second;\n erase(it);\n\n assert(entry != NULL);\n entry->queueIt = end();\n entry->inQueue = false;\n return entry;\n }\n\n EstimatePropagatorCost::EstimatePropagatorCost (SparseOptimizer* graph) :\n _graph(graph)\n {\n }\n\n double EstimatePropagatorCost::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const\n {\n OptimizableGraph::Edge* e = dynamic_cast(edge);\n OptimizableGraph::Vertex* to = dynamic_cast(to_);\n SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);\n if (it == _graph->activeEdges().end()) // it has to be an active edge\n return std::numeric_limits::max();\n return e->initialEstimatePossible(from, to);\n }\n\n EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(SparseOptimizer* graph) :\n EstimatePropagatorCost(graph)\n {\n }\n\n double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const\n {\n OptimizableGraph::Edge* e = dynamic_cast(edge);\n OptimizableGraph::Vertex* from = dynamic_cast(*from_.begin());\n OptimizableGraph::Vertex* to = dynamic_cast(to_);\n if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph\n return std::numeric_limits::max();\n SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);\n if (it == _graph->activeEdges().end()) // it has to be an active edge\n return std::numeric_limits::max();\n return e->initialEstimatePossible(from_, to);\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/estimate_propagator.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_ESTIMATE_PROPAGATOR_H\n#define G2O_ESTIMATE_PROPAGATOR_H\n\n#include \"optimizable_graph.h\"\n#include \"sparse_optimizer.h\"\n\n#include \n#include \n#include \n\n#ifdef _MSC_VER\n#include \n#else\n#include \n#endif\n\nnamespace g2o {\n\n /**\n * \\brief cost for traversing along active edges in the optimizer\n *\n * You may derive an own one, if necessary. The default is to return initialEstimatePossible(from, to) for the edge.\n */\n class EstimatePropagatorCost {\n public:\n EstimatePropagatorCost (SparseOptimizer* graph);\n virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const;\n virtual const char* name() const { return \"spanning tree\";}\n protected:\n SparseOptimizer* _graph;\n };\n\n /**\n * \\brief cost for traversing only odometry edges.\n *\n * Initialize your graph along odometry edges. An odometry edge is assumed to connect vertices\n * whose IDs only differs by one.\n */\n class EstimatePropagatorCostOdometry : public EstimatePropagatorCost {\n public:\n EstimatePropagatorCostOdometry(SparseOptimizer* graph);\n virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const;\n virtual const char* name() const { return \"odometry\";}\n };\n\n /**\n * \\brief propagation of an initial guess\n */\n class EstimatePropagator {\n public:\n\n /**\n * \\brief Applying the action for propagating.\n *\n * You may derive an own one, if necessary. The default is to call initialEstimate(from, to) for the edge.\n */\n struct PropagateAction {\n virtual void operator()(OptimizableGraph::Edge* e, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) const\n {\n if (! to->fixed())\n e->initialEstimate(from, to);\n }\n };\n\n typedef EstimatePropagatorCost PropagateCost;\n\n class AdjacencyMapEntry;\n\n /**\n * \\brief priority queue for AdjacencyMapEntry\n */\n class PriorityQueue : public std::multimap {\n public:\n void push(AdjacencyMapEntry* entry);\n AdjacencyMapEntry* pop();\n };\n\n /**\n * \\brief data structure for loopuk during Dijkstra\n */\n class AdjacencyMapEntry {\n public:\n friend class EstimatePropagator;\n friend class PriorityQueue;\n AdjacencyMapEntry();\n void reset();\n OptimizableGraph::Vertex* child() const {return _child;}\n const OptimizableGraph::VertexSet& parent() const {return _parent;}\n OptimizableGraph::Edge* edge() const {return _edge;}\n double distance() const {return _distance;}\n int frontierLevel() const { return _frontierLevel;}\n\n protected:\n OptimizableGraph::Vertex* _child;\n OptimizableGraph::VertexSet _parent;\n OptimizableGraph::Edge* _edge;\n double _distance;\n int _frontierLevel;\n private: // for PriorityQueue\n bool inQueue;\n PriorityQueue::iterator queueIt;\n };\n\n /**\n * \\brief hash function for a vertex\n */\n class VertexIDHashFunction {\n public:\n size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}\n };\n\n typedef std::tr1::unordered_map AdjacencyMap;\n\n public:\n EstimatePropagator(OptimizableGraph* g);\n OptimizableGraph::VertexSet& visited() {return _visited; }\n AdjacencyMap& adjacencyMap() {return _adjacencyMap; }\n OptimizableGraph* graph() {return _graph;} \n\n /**\n * propagate an initial guess starting from v. The function computes a spanning tree\n * whereas the cost for each edge is determined by calling cost() and the action applied to\n * each vertex is action().\n */\n void propagate(OptimizableGraph::Vertex* v, \n const EstimatePropagator::PropagateCost& cost, \n const EstimatePropagator::PropagateAction& action = PropagateAction(),\n double maxDistance=std::numeric_limits::max(), \n double maxEdgeCost=std::numeric_limits::max());\n\n /**\n * same as above but starting to propagate from a set of vertices instead of just a single one.\n */\n void propagate(OptimizableGraph::VertexSet& vset, \n const EstimatePropagator::PropagateCost& cost, \n const EstimatePropagator::PropagateAction& action = PropagateAction(),\n double maxDistance=std::numeric_limits::max(), \n double maxEdgeCost=std::numeric_limits::max());\n\n protected:\n void reset();\n\n AdjacencyMap _adjacencyMap;\n OptimizableGraph::VertexSet _visited;\n OptimizableGraph* _graph;\n };\n\n}\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/factory.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"factory.h\"\n\n#include \"creators.h\"\n#include \"parameter.h\"\n#include \"cache.h\"\n#include \"optimizable_graph.h\"\n#include \"../stuff/color_macros.h\"\n\n#include \n#include \n#include \n\nusing namespace std;\n\nnamespace g2o {\n\nFactory* Factory::factoryInstance = 0;\n\nFactory::Factory()\n{\n}\n\nFactory::~Factory()\n{\n# ifdef G2O_DEBUG_FACTORY\n cerr << \"# Factory destroying \" << (void*)this << endl;\n# endif\n for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {\n delete it->second->creator;\n }\n _creator.clear();\n _tagLookup.clear();\n}\n\nFactory* Factory::instance()\n{\n if (factoryInstance == 0) {\n factoryInstance = new Factory;\n# ifdef G2O_DEBUG_FACTORY\n cerr << \"# Factory allocated \" << (void*)factoryInstance << endl;\n# endif\n }\n\n return factoryInstance;\n}\n\nvoid Factory::registerType(const std::string& tag, AbstractHyperGraphElementCreator* c)\n{\n CreatorMap::const_iterator foundIt = _creator.find(tag);\n if (foundIt != _creator.end()) {\n cerr << \"FACTORY WARNING: Overwriting Vertex tag \" << tag << endl;\n assert(0);\n }\n TagLookup::const_iterator tagIt = _tagLookup.find(c->name());\n if (tagIt != _tagLookup.end()) {\n cerr << \"FACTORY WARNING: Registering same class for two tags \" << c->name() << endl;\n assert(0);\n }\n\n CreatorInformation* ci = new CreatorInformation();\n ci->creator = c;\n\n#ifdef G2O_DEBUG_FACTORY\n cerr << \"# Factory \" << (void*)this << \" constructing type \" << tag << \" \";\n#endif\n // construct an element once to figure out its type\n HyperGraph::HyperGraphElement* element = c->construct();\n ci->elementTypeBit = element->elementType();\n\n#ifdef G2O_DEBUG_FACTORY\n cerr << \"done.\" << endl;\n cerr << \"# Factory \" << (void*)this << \" registering \" << tag;\n cerr << \" \" << (void*) c << \" \";\n switch (element->elementType()) {\n case HyperGraph::HGET_VERTEX:\n cerr << \" -> Vertex\";\n break;\n case HyperGraph::HGET_EDGE:\n cerr << \" -> Edge\";\n break;\n case HyperGraph::HGET_PARAMETER:\n cerr << \" -> Parameter\";\n break;\n case HyperGraph::HGET_CACHE:\n cerr << \" -> Cache\";\n break;\n case HyperGraph::HGET_DATA:\n cerr << \" -> Data\";\n break;\n default:\n assert(0 && \"Unknown element type occured, fix elementTypes\");\n break;\n }\n cerr << endl;\n#endif\n\n _creator[tag] = ci;\n _tagLookup[c->name()] = tag;\n delete element;\n}\n\n void Factory::unregisterType(const std::string& tag)\n {\n // Look for the tag\n CreatorMap::iterator tagPosition = _creator.find(tag);\n\n if (tagPosition != _creator.end()) {\n\n AbstractHyperGraphElementCreator* c = tagPosition->second->creator;\n\n // If we found it, remove the creator from the tag lookup map\n TagLookup::iterator classPosition = _tagLookup.find(c->name());\n if (classPosition != _tagLookup.end())\n {\n _tagLookup.erase(classPosition);\n }\n _creator.erase(tagPosition);\n }\n }\n\nHyperGraph::HyperGraphElement* Factory::construct(const std::string& tag) const\n{\n CreatorMap::const_iterator foundIt = _creator.find(tag);\n if (foundIt != _creator.end()) {\n //cerr << \"tag \" << tag << \" -> \" << (void*) foundIt->second->creator << \" \" << foundIt->second->creator->name() << endl;\n return foundIt->second->creator->construct();\n }\n return 0;\n}\n\nconst std::string& Factory::tag(const HyperGraph::HyperGraphElement* e) const\n{\n static std::string emptyStr(\"\");\n TagLookup::const_iterator foundIt = _tagLookup.find(typeid(*e).name());\n if (foundIt != _tagLookup.end())\n return foundIt->second;\n return emptyStr;\n}\n\nvoid Factory::fillKnownTypes(std::vector& types) const\n{\n types.clear();\n for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)\n types.push_back(it->first);\n}\n\nbool Factory::knowsTag(const std::string& tag, int* elementType) const\n{\n CreatorMap::const_iterator foundIt = _creator.find(tag);\n if (foundIt == _creator.end()) {\n if (elementType)\n *elementType = -1;\n return false;\n }\n if (elementType)\n *elementType = foundIt->second->elementTypeBit;\n return true;\n}\n\nvoid Factory::destroy()\n{\n delete factoryInstance;\n factoryInstance = 0;\n}\n\nvoid Factory::printRegisteredTypes(std::ostream& os, bool comment) const\n{\n if (comment)\n os << \"# \";\n os << \"types:\" << endl;\n for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {\n if (comment)\n os << \"#\";\n cerr << \"\\t\" << it->first << endl;\n }\n}\n\nHyperGraph::HyperGraphElement* Factory::construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const\n{\n if (elemsToConstruct.none()) {\n return construct(tag);\n }\n CreatorMap::const_iterator foundIt = _creator.find(tag);\n if (foundIt != _creator.end() && foundIt->second->elementTypeBit >= 0 && elemsToConstruct.test(foundIt->second->elementTypeBit)) {\n return foundIt->second->creator->construct();\n }\n return 0;\n}\n\n} // end namespace\n\n" }, { "path": "Thirdparty/g2o/g2o/core/factory.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_FACTORY_H\n#define G2O_FACTORY_H\n\n#include \"../../config.h\"\n#include \"../stuff/misc.h\"\n#include \"hyper_graph.h\"\n#include \"creators.h\"\n\n#include \n#include \n#include \n\n// define to get some verbose output\n//#define G2O_DEBUG_FACTORY\n\nnamespace g2o {\n\n class AbstractHyperGraphElementCreator;\n \n /**\n * \\brief create vertices and edges based on TAGs in, for example, a file\n */\n class Factory\n {\n public:\n\n //! return the instance\n static Factory* instance();\n\n //! free the instance\n static void destroy();\n\n /**\n * register a tag for a specific creator\n */\n void registerType(const std::string& tag, AbstractHyperGraphElementCreator* c);\n\n /**\n * unregister a tag for a specific creator\n */\n void unregisterType(const std::string& tag);\n\n /**\n * construct a graph element based on its tag\n */\n HyperGraph::HyperGraphElement* construct(const std::string& tag) const;\n\n /**\n * construct a graph element based on its tag, but only if it's type (a bitmask) matches. A bitmask without any\n * bit set will construct any item. Otherwise a bit has to be set to allow construction of a graph element.\n */\n HyperGraph::HyperGraphElement* construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const;\n\n /**\n * return whether the factory knows this tag or not\n */\n bool knowsTag(const std::string& tag, int* elementType = 0) const;\n\n //! return the TAG given a vertex\n const std::string& tag(const HyperGraph::HyperGraphElement* v) const;\n\n /**\n * get a list of all known types\n */\n void fillKnownTypes(std::vector& types) const;\n\n /**\n * print a list of the known registered types to the given stream\n */\n void printRegisteredTypes(std::ostream& os, bool comment = false) const;\n\n protected:\n class CreatorInformation\n {\n public:\n AbstractHyperGraphElementCreator* creator;\n int elementTypeBit;\n CreatorInformation()\n {\n creator = 0;\n elementTypeBit = -1;\n }\n \n ~CreatorInformation()\n {\n std::cout << \"Deleting \" << (void*) creator << std::endl;\n \n delete creator;\n }\n };\n\n typedef std::map CreatorMap;\n typedef std::map TagLookup;\n Factory();\n ~Factory();\n\n CreatorMap _creator; ///< look-up map for the existing creators\n TagLookup _tagLookup; ///< reverse look-up, class name to tag\n\n private:\n static Factory* factoryInstance;\n };\n\n template\n class RegisterTypeProxy\n {\n public:\n RegisterTypeProxy(const std::string& name) : _name(name)\n {\n#ifdef G2O_DEBUG_FACTORY\n std::cout << __FUNCTION__ << \": Registering \" << _name << \" of type \" << typeid(T).name() << std::endl;\n#endif\n Factory::instance()->registerType(_name, new HyperGraphElementCreator());\n }\n\n ~RegisterTypeProxy()\n {\n#ifdef G2O_DEBUG_FACTORY\n std::cout << __FUNCTION__ << \": Unregistering \" << _name << \" of type \" << typeid(T).name() << std::endl;\n#endif\n Factory::instance()->unregisterType(_name);\n }\n\n private:\n std::string _name;\n };\n\n#if defined _MSC_VER && defined G2O_SHARED_LIBS\n# define G2O_FACTORY_EXPORT __declspec(dllexport)\n# define G2O_FACTORY_IMPORT __declspec(dllimport)\n#else\n# define G2O_FACTORY_EXPORT\n# define G2O_FACTORY_IMPORT\n#endif\n\n // These macros are used to automate registering types and forcing linkage\n#define G2O_REGISTER_TYPE(name, classname) \\\n extern \"C\" void G2O_FACTORY_EXPORT g2o_type_##classname(void) {} \\\n static g2o::RegisterTypeProxy g_type_proxy_##classname(#name);\n\n#define G2O_USE_TYPE_BY_CLASS_NAME(classname) \\\n extern \"C\" void G2O_FACTORY_IMPORT g2o_type_##classname(void); \\\n static g2o::ForceLinker proxy_##classname(g2o_type_##classname);\n\n#define G2O_REGISTER_TYPE_GROUP(typeGroupName) \\\n extern \"C\" void G2O_FACTORY_EXPORT g2o_type_group_##typeGroupName(void) {}\n\n#define G2O_USE_TYPE_GROUP(typeGroupName) \\\n extern \"C\" void G2O_FACTORY_IMPORT g2o_type_group_##typeGroupName(void); \\\n static g2o::ForceLinker g2o_force_type_link_##typeGroupName(g2o_type_group_##typeGroupName);\n}\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \n#include \n#include \n#include \n#include \n#include \"hyper_dijkstra.h\"\n#include \"../stuff/macros.h\"\n\nnamespace g2o{\n\n using namespace std;\n\n double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e){\n (void) v;\n (void) vParent;\n (void) e;\n return std::numeric_limits::max();\n }\n\n double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance){\n if (distance==-1)\n return perform (v,vParent,e);\n return std::numeric_limits::max();\n }\n\n HyperDijkstra::AdjacencyMapEntry::AdjacencyMapEntry(HyperGraph::Vertex* child_, HyperGraph::Vertex* parent_, \n HyperGraph::Edge* edge_, double distance_)\n {\n _child=child_;\n _parent=parent_;\n _edge=edge_;\n _distance=distance_;\n }\n\n HyperDijkstra::HyperDijkstra(HyperGraph* g): _graph(g)\n {\n for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){\n AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());\n _adjacencyMap.insert(make_pair(entry.child(), entry));\n }\n }\n\n void HyperDijkstra::reset()\n {\n for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){\n AdjacencyMap::iterator at=_adjacencyMap.find(*it);\n assert(at!=_adjacencyMap.end());\n at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());\n }\n _visited.clear();\n }\n\n\n bool operator<(const HyperDijkstra::AdjacencyMapEntry& a, const HyperDijkstra::AdjacencyMapEntry& b)\n {\n return a.distance()>b.distance();\n }\n\n\n void HyperDijkstra::shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost, \n double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost)\n {\n reset();\n std::priority_queue< AdjacencyMapEntry > frontier;\n for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){\n HyperGraph::Vertex* v=*vit;\n assert(v!=0);\n AdjacencyMap::iterator it=_adjacencyMap.find(v);\n if (it == _adjacencyMap.end()) {\n cerr << __PRETTY_FUNCTION__ << \"Vertex \" << v->id() << \" is not in the adjacency map\" << endl;\n }\n assert(it!=_adjacencyMap.end());\n it->second._distance=0.;\n it->second._parent=0;\n frontier.push(it->second);\n }\n\n while(! frontier.empty()){\n AdjacencyMapEntry entry=frontier.top();\n frontier.pop();\n HyperGraph::Vertex* u=entry.child();\n AdjacencyMap::iterator ut=_adjacencyMap.find(u);\n if (ut == _adjacencyMap.end()) {\n cerr << __PRETTY_FUNCTION__ << \"Vertex \" << u->id() << \" is not in the adjacency map\" << endl;\n }\n assert(ut!=_adjacencyMap.end());\n double uDistance=ut->second.distance();\n\n std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult;\n HyperGraph::EdgeSet::iterator et=u->edges().begin();\n while (et != u->edges().end()){\n HyperGraph::Edge* edge=*et;\n ++et;\n\n if (directed && edge->vertex(0) != u)\n continue;\n\n for (size_t i = 0; i < edge->vertices().size(); ++i) {\n HyperGraph::Vertex* z = edge->vertex(i);\n if (z == u)\n continue;\n\n double edgeDistance=(*cost)(edge, u, z);\n if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost)\n continue;\n double zDistance=uDistance+edgeDistance;\n //cerr << z->id() << \" \" << zDistance << endl;\n\n AdjacencyMap::iterator ot=_adjacencyMap.find(z);\n assert(ot!=_adjacencyMap.end());\n\n if (zDistance+comparisonConditionersecond.distance() && zDistancesecond._distance=zDistance;\n ot->second._parent=u;\n ot->second._edge=edge;\n frontier.push(ot->second);\n }\n }\n }\n }\n }\n\n void HyperDijkstra::shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance, \n double comparisonConditioner, bool directed, double maxEdgeCost)\n {\n HyperGraph::VertexSet vset;\n vset.insert(v);\n shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost);\n }\n\n void HyperDijkstra::computeTree(AdjacencyMap& amap)\n {\n for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){\n AdjacencyMapEntry& entry(it->second);\n entry._children.clear();\n }\n for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){\n AdjacencyMapEntry& entry(it->second);\n HyperGraph::Vertex* parent=entry.parent();\n if (!parent){\n continue;\n }\n HyperGraph::Vertex* v=entry.child();\n assert (v==it->first);\n\n AdjacencyMap::iterator pt=amap.find(parent);\n assert(pt!=amap.end());\n pt->second._children.insert(v);\n }\n }\n\n\n void HyperDijkstra::visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance)\n {\n \n typedef std::deque Deque;\n Deque q;\n // scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them.\n for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){\n AdjacencyMapEntry& entry(it->second);\n if (! entry.parent()) {\n action->perform(it->first,0,0);\n q.push_back(it->first);\n }\n }\n\n //std::cerr << \"q.size()\" << q.size() << endl;\n int count=0;\n while (! q.empty()){\n HyperGraph::Vertex* parent=q.front();\n q.pop_front();\n ++count;\n AdjacencyMap::iterator parentIt=amap.find(parent);\n if (parentIt==amap.end()) {\n continue;\n }\n //cerr << \"parent= \" << parent << \" parent id= \" << parent->id() << \"\\t children id =\";\n HyperGraph::VertexSet& childs(parentIt->second.children());\n for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){\n HyperGraph::Vertex* child=*childsIt;\n //cerr << child->id();\n AdjacencyMap::iterator adjacencyIt=amap.find(child);\n assert (adjacencyIt!=amap.end());\n HyperGraph::Edge* edge=adjacencyIt->second.edge(); \n\n assert(adjacencyIt->first==child);\n assert(adjacencyIt->second.child()==child);\n assert(adjacencyIt->second.parent()==parent);\n if (! useDistance) {\n action->perform(child, parent, edge);\n } else {\n action->perform(child, parent, edge, adjacencyIt->second.distance());\n }\n q.push_back(child);\n }\n //cerr << endl;\n }\n\n }\n\n void HyperDijkstra::connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, \n HyperGraph::VertexSet& startingSet, \n HyperGraph* g, HyperGraph::Vertex* v,\n HyperDijkstra::CostFunction* cost, double distance, \n double comparisonConditioner, double maxEdgeCost)\n {\n typedef std::queue VertexDeque;\n visited.clear();\n connected.clear();\n VertexDeque frontier;\n HyperDijkstra dv(g);\n connected.insert(v);\n frontier.push(v);\n while (! frontier.empty()){\n HyperGraph::Vertex* v0=frontier.front();\n frontier.pop();\n dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost);\n for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){\n visited.insert(*it);\n if (startingSet.find(*it)==startingSet.end())\n continue;\n std::pair insertOutcome=connected.insert(*it);\n if (insertOutcome.second){ // the node was not in the connectedSet;\n frontier.push(dynamic_cast(*it));\n }\n }\n }\n }\n\n double UniformCostFunction::operator () (HyperGraph::Edge* /*edge*/, HyperGraph::Vertex* /*from*/, HyperGraph::Vertex* /*to*/)\n {\n return 1.;\n }\n\n};\n" }, { "path": "Thirdparty/g2o/g2o/core/hyper_dijkstra.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_AIS_GENERAL_DIJKSTRA_HH\n#define G2O_AIS_GENERAL_DIJKSTRA_HH\n\n#include \n#include \n#include \n\n#include \"hyper_graph.h\"\n\nnamespace g2o{\n\n struct HyperDijkstra{\n struct CostFunction {\n virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0;\n virtual ~CostFunction() { }\n };\n\n struct TreeAction {\n virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e);\n virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance);\n };\n\n \n struct AdjacencyMapEntry{\n friend struct HyperDijkstra;\n AdjacencyMapEntry(HyperGraph::Vertex* _child=0, \n HyperGraph::Vertex* _parent=0, \n HyperGraph::Edge* _edge=0, \n double _distance=std::numeric_limits::max());\n HyperGraph::Vertex* child() const {return _child;}\n HyperGraph::Vertex* parent() const {return _parent;}\n HyperGraph::Edge* edge() const {return _edge;}\n double distance() const {return _distance;}\n HyperGraph::VertexSet& children() {return _children;}\n const HyperGraph::VertexSet& children() const {return _children;}\n protected:\n HyperGraph::Vertex* _child;\n HyperGraph::Vertex* _parent;\n HyperGraph::Edge* _edge;\n double _distance;\n HyperGraph::VertexSet _children;\n };\n\n typedef std::map AdjacencyMap;\n HyperDijkstra(HyperGraph* g);\n HyperGraph::VertexSet& visited() {return _visited; }\n AdjacencyMap& adjacencyMap() {return _adjacencyMap; }\n HyperGraph* graph() {return _graph;} \n\n void shortestPaths(HyperGraph::Vertex* v, \n HyperDijkstra::CostFunction* cost, \n double maxDistance=std::numeric_limits< double >::max(), \n double comparisonConditioner=1e-3, \n bool directed=false,\n double maxEdgeCost=std::numeric_limits< double >::max());\n\n void shortestPaths(HyperGraph::VertexSet& vset, \n HyperDijkstra::CostFunction* cost, \n double maxDistance=std::numeric_limits< double >::max(), \n double comparisonConditioner=1e-3, \n bool directed=false,\n double maxEdgeCost=std::numeric_limits< double >::max());\n\n\n static void computeTree(AdjacencyMap& amap);\n static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false);\n static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited, \n HyperGraph::VertexSet& startingSet, \n HyperGraph* g, HyperGraph::Vertex* v,\n HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner,\n double maxEdgeCost=std::numeric_limits< double >::max() );\n\n protected:\n void reset();\n\n AdjacencyMap _adjacencyMap;\n HyperGraph::VertexSet _visited;\n HyperGraph* _graph;\n };\n\n struct UniformCostFunction: public HyperDijkstra::CostFunction {\n virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to);\n };\n\n}\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/hyper_graph.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"hyper_graph.h\"\n\n#include \n#include \n\nnamespace g2o {\n\n HyperGraph::Vertex::Vertex(int id) : _id(id)\n {\n }\n\n HyperGraph::Vertex::~Vertex()\n {\n }\n\n HyperGraph::Edge::Edge(int id) : _id(id)\n {\n }\n\n HyperGraph::Edge::~Edge()\n {\n }\n\n void HyperGraph::Edge::resize(size_t size)\n {\n _vertices.resize(size, 0);\n }\n\n void HyperGraph::Edge::setId(int id)\n {\n _id = id;\n }\n\n HyperGraph::Vertex* HyperGraph::vertex(int id)\n {\n VertexIDMap::iterator it=_vertices.find(id);\n if (it==_vertices.end())\n return 0;\n return it->second;\n }\n\n const HyperGraph::Vertex* HyperGraph::vertex(int id) const\n {\n VertexIDMap::const_iterator it=_vertices.find(id);\n if (it==_vertices.end())\n return 0;\n return it->second;\n }\n\n bool HyperGraph::addVertex(Vertex* v)\n {\n Vertex* vn=vertex(v->id());\n if (vn)\n return false;\n _vertices.insert( std::make_pair(v->id(),v) );\n return true;\n }\n\n /**\n * changes the id of a vertex already in the graph, and updates the bookkeeping\n @ returns false if the vertex is not in the graph;\n */\n bool HyperGraph::changeId(Vertex* v, int newId){\n Vertex* v2 = vertex(v->id());\n if (v != v2)\n return false;\n _vertices.erase(v->id());\n v->setId(newId);\n _vertices.insert(std::make_pair(v->id(), v));\n return true;\n }\n\n bool HyperGraph::addEdge(Edge* e)\n {\n std::pair result = _edges.insert(e);\n if (! result.second)\n return false;\n for (std::vector::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {\n Vertex* v = *it;\n v->edges().insert(e);\n }\n return true;\n }\n\n bool HyperGraph::removeVertex(Vertex* v)\n {\n VertexIDMap::iterator it=_vertices.find(v->id());\n if (it==_vertices.end())\n return false;\n assert(it->second==v);\n //remove all edges which are entering or leaving v;\n EdgeSet tmp(v->edges());\n for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){\n if (!removeEdge(*it)){\n assert(0);\n }\n }\n _vertices.erase(it);\n delete v;\n return true;\n }\n\n bool HyperGraph::removeEdge(Edge* e)\n {\n EdgeSet::iterator it = _edges.find(e);\n if (it == _edges.end())\n return false;\n _edges.erase(it);\n\n for (std::vector::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {\n Vertex* v = *vit;\n it = v->edges().find(e);\n assert(it!=v->edges().end());\n v->edges().erase(it);\n }\n\n delete e;\n return true;\n }\n\n HyperGraph::HyperGraph()\n {\n }\n\n void HyperGraph::clear()\n {\n for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it)\n delete (it->second);\n for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it)\n delete (*it);\n _vertices.clear();\n _edges.clear();\n }\n\n HyperGraph::~HyperGraph()\n {\n clear();\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/hyper_graph.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_AIS_HYPER_GRAPH_HH\n#define G2O_AIS_HYPER_GRAPH_HH\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#ifdef _MSC_VER\n#include \n#else\n#include \n#endif\n\n\n/** @addtogroup graph */\n//@{\nnamespace g2o {\n\n /**\n Class that models a directed Hyper-Graph. An hyper graph is a graph where an edge\n can connect one or more nodes. Both Vertices and Edges of an hyoper graph\n derive from the same class HyperGraphElement, thus one can implement generic algorithms\n that operate transparently on edges or vertices (see HyperGraphAction).\n\n The vertices are uniquely identified by an int id, while the edges are\n identfied by their pointers. \n */\n class HyperGraph\n {\n public:\n\n /**\n * \\brief enum of all the types we have in our graphs\n */\n enum HyperGraphElementType {\n HGET_VERTEX,\n HGET_EDGE,\n HGET_PARAMETER,\n HGET_CACHE,\n HGET_DATA,\n HGET_NUM_ELEMS // keep as last elem\n };\n\n typedef std::bitset GraphElemBitset;\n\n class Vertex;\n class Edge;\n \n /**\n * base hyper graph element, specialized in vertex and edge\n */\n struct HyperGraphElement {\n virtual ~HyperGraphElement() {}\n /**\n * returns the type of the graph element, see HyperGraphElementType\n */\n virtual HyperGraphElementType elementType() const = 0;\n };\n\n typedef std::set EdgeSet;\n typedef std::set VertexSet;\n\n typedef std::tr1::unordered_map VertexIDMap;\n typedef std::vector VertexContainer;\n\n //! abstract Vertex, your types must derive from that one\n class Vertex : public HyperGraphElement {\n public:\n //! creates a vertex having an ID specified by the argument\n explicit Vertex(int id=-1);\n virtual ~Vertex();\n //! returns the id\n int id() const {return _id;}\n\t virtual void setId( int newId) { _id=newId; }\n //! returns the set of hyper-edges that are leaving/entering in this vertex\n const EdgeSet& edges() const {return _edges;}\n //! returns the set of hyper-edges that are leaving/entering in this vertex\n EdgeSet& edges() {return _edges;}\n virtual HyperGraphElementType elementType() const { return HGET_VERTEX;}\n protected:\n int _id;\n EdgeSet _edges;\n };\n\n /** \n * Abstract Edge class. Your nice edge classes should inherit from that one.\n * An hyper-edge has pointers to the vertices it connects and stores them in a vector.\n */\n class Edge : public HyperGraphElement {\n public:\n //! creates and empty edge with no vertices\n explicit Edge(int id = -1);\n virtual ~Edge();\n\n /**\n * resizes the number of vertices connected by this edge\n */\n virtual void resize(size_t size);\n /**\n returns the vector of pointers to the vertices connected by the hyper-edge.\n */\n const VertexContainer& vertices() const { return _vertices;}\n /**\n returns the vector of pointers to the vertices connected by the hyper-edge.\n */\n VertexContainer& vertices() { return _vertices;}\n /**\n returns the pointer to the ith vertex connected to the hyper-edge.\n */\n const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && \"index out of bounds\"); return _vertices[i];}\n /**\n returns the pointer to the ith vertex connected to the hyper-edge.\n */\n Vertex* vertex(size_t i) { assert(i < _vertices.size() && \"index out of bounds\"); return _vertices[i];}\n /**\n set the ith vertex on the hyper-edge to the pointer supplied\n */\n void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && \"index out of bounds\"); _vertices[i]=v;}\n\n int id() const {return _id;}\n void setId(int id);\n virtual HyperGraphElementType elementType() const { return HGET_EDGE;}\n protected:\n VertexContainer _vertices;\n int _id; ///< unique id\n };\n\n public:\n //! constructs an empty hyper graph\n HyperGraph();\n //! destroys the hyper-graph and all the vertices of the graph\n virtual ~HyperGraph();\n\n //! returns a vertex id in the hyper-graph, or 0 if the vertex id is not present\n Vertex* vertex(int id);\n //! returns a vertex id in the hyper-graph, or 0 if the vertex id is not present\n const Vertex* vertex(int id) const;\n\n //! removes a vertex from the graph. Returns true on success (vertex was present)\n virtual bool removeVertex(Vertex* v);\n //! removes a vertex from the graph. Returns true on success (edge was present)\n virtual bool removeEdge(Edge* e);\n //! clears the graph and empties all structures.\n virtual void clear();\n\n //! @returns the map id -> vertex where the vertices are stored\n const VertexIDMap& vertices() const {return _vertices;}\n //! @returns the map id -> vertex where the vertices are stored\n VertexIDMap& vertices() {return _vertices;}\n\n //! @returns the set of edges of the hyper graph\n const EdgeSet& edges() const {return _edges;}\n //! @returns the set of edges of the hyper graph\n EdgeSet& edges() {return _edges;}\n\n /**\n * adds a vertex to the graph. The id of the vertex should be set before\n * invoking this function. the function fails if another vertex\n * with the same id is already in the graph.\n * returns true, on success, or false on failure.\n */\n virtual bool addVertex(Vertex* v);\n\n /**\n * Adds an edge to the graph. If the edge is already in the graph, it\n * does nothing and returns false. Otherwise it returns true.\n */\n virtual bool addEdge(Edge* e);\n\n /**\n * changes the id of a vertex already in the graph, and updates the bookkeeping\n @ returns false if the vertex is not in the graph;\n */\n virtual bool changeId(Vertex* v, int newId);\n\n protected:\n VertexIDMap _vertices;\n EdgeSet _edges;\n\n private:\n // Disable the copy constructor and assignment operator\n HyperGraph(const HyperGraph&) { }\n HyperGraph& operator= (const HyperGraph&) { return *this; }\n };\n\n} // end namespace\n\n//@}\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/hyper_graph_action.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"hyper_graph_action.h\"\n#include \"optimizable_graph.h\"\n#include \"../stuff/macros.h\"\n\n\n#include \n\nnamespace g2o {\n using namespace std;\n\n HyperGraphActionLibrary* HyperGraphActionLibrary::actionLibInstance = 0;\n\n HyperGraphAction::Parameters::~Parameters()\n {\n }\n\n HyperGraphAction::ParametersIteration::ParametersIteration(int iter) :\n HyperGraphAction::Parameters(),\n iteration(iter)\n {\n }\n\n HyperGraphAction::~HyperGraphAction()\n {\n }\n\n HyperGraphAction* HyperGraphAction::operator()(const HyperGraph*, Parameters*)\n {\n return 0;\n }\n\n HyperGraphElementAction::Parameters::~Parameters()\n {\n }\n\n HyperGraphElementAction::HyperGraphElementAction(const std::string& typeName_)\n {\n _typeName = typeName_;\n }\n\n void HyperGraphElementAction::setTypeName(const std::string& typeName_)\n {\n _typeName = typeName_;\n }\n\n\n HyperGraphElementAction* HyperGraphElementAction::operator()(HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )\n {\n return 0;\n }\n \n HyperGraphElementAction* HyperGraphElementAction::operator()(const HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )\n {\n return 0;\n }\n \n HyperGraphElementAction::~HyperGraphElementAction()\n {\n }\n\n HyperGraphElementActionCollection::HyperGraphElementActionCollection(const std::string& name_)\n {\n _name = name_;\n }\n\n HyperGraphElementActionCollection::~HyperGraphElementActionCollection()\n {\n for (ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {\n delete it->second;\n }\n }\n\n HyperGraphElementAction* HyperGraphElementActionCollection::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)\n {\n ActionMap::iterator it=_actionMap.find(typeid(*element).name());\n //cerr << typeid(*element).name() << endl;\n if (it==_actionMap.end())\n return 0;\n HyperGraphElementAction* action=it->second;\n return (*action)(element, params);\n }\n\n HyperGraphElementAction* HyperGraphElementActionCollection::operator()(const HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)\n {\n ActionMap::iterator it=_actionMap.find(typeid(*element).name());\n if (it==_actionMap.end())\n return 0;\n HyperGraphElementAction* action=it->second;\n return (*action)(element, params);\n }\n\n bool HyperGraphElementActionCollection::registerAction(HyperGraphElementAction* action)\n {\n# ifdef G2O_DEBUG_ACTIONLIB\n cerr << __PRETTY_FUNCTION__ << \" \" << action->name() << \" \" << action->typeName() << endl;\n# endif\n if (action->name()!=name()){\n cerr << __PRETTY_FUNCTION__ << \": invalid attempt to register an action in a collection with a different name \" << name() << \" \" << action->name() << endl;\n }\n _actionMap.insert(make_pair ( action->typeName(), action) );\n return true;\n }\n\n bool HyperGraphElementActionCollection::unregisterAction(HyperGraphElementAction* action)\n {\n for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {\n if (it->second == action){\n _actionMap.erase(it);\n return true;\n }\n }\n return false;\n }\n \n HyperGraphActionLibrary::HyperGraphActionLibrary()\n {\n }\n\n HyperGraphActionLibrary* HyperGraphActionLibrary::instance()\n {\n if (actionLibInstance == 0) {\n actionLibInstance = new HyperGraphActionLibrary;\n }\n return actionLibInstance;\n }\n\n void HyperGraphActionLibrary::destroy()\n {\n delete actionLibInstance;\n actionLibInstance = 0;\n }\n\n HyperGraphActionLibrary::~HyperGraphActionLibrary()\n {\n for (HyperGraphElementAction::ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {\n delete it->second;\n }\n }\n \n HyperGraphElementAction* HyperGraphActionLibrary::actionByName(const std::string& name)\n {\n HyperGraphElementAction::ActionMap::iterator it=_actionMap.find(name);\n if (it!=_actionMap.end())\n return it->second;\n return 0;\n }\n\n bool HyperGraphActionLibrary::registerAction(HyperGraphElementAction* action)\n {\n HyperGraphElementAction* oldAction = actionByName(action->name());\n HyperGraphElementActionCollection* collection = 0;\n if (oldAction) {\n collection = dynamic_cast(oldAction);\n if (! collection) {\n cerr << __PRETTY_FUNCTION__ << \": fatal error, a collection is not at the first level in the library\" << endl;\n return 0;\n }\n }\n if (! collection) {\n#ifdef G2O_DEBUG_ACTIONLIB\n cerr << __PRETTY_FUNCTION__ << \": creating collection for \\\"\" << action->name() << \"\\\"\" << endl;\n#endif\n collection = new HyperGraphElementActionCollection(action->name());\n _actionMap.insert(make_pair(action->name(), collection));\n }\n return collection->registerAction(action);\n }\n \n bool HyperGraphActionLibrary::unregisterAction(HyperGraphElementAction* action)\n {\n list collectionDeleteList;\n\n // 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\n for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {\n HyperGraphElementActionCollection* collection = dynamic_cast (it->second);\n if (collection != 0) {\n collection->unregisterAction(action);\n if (collection->actionMap().size() == 0) {\n collectionDeleteList.push_back(collection);\n }\n }\n }\n\n // Delete any empty action collections\n for (list::iterator itc = collectionDeleteList.begin(); itc != collectionDeleteList.end(); ++itc) {\n //cout << \"Deleting collection \" << (*itc)->name() << endl;\n _actionMap.erase((*itc)->name());\n }\n\n return true;\n }\n\n\n WriteGnuplotAction::WriteGnuplotAction(const std::string& typeName_)\n : HyperGraphElementAction(typeName_)\n {\n _name=\"writeGnuplot\";\n }\n\n DrawAction::Parameters::Parameters(){\n }\n\n DrawAction::DrawAction(const std::string& typeName_) \n : HyperGraphElementAction(typeName_)\n {\n _name=\"draw\";\n _previousParams = (Parameters*)0x42;\n refreshPropertyPtrs(0);\n }\n\n bool DrawAction::refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_){\n if (_previousParams == params_)\n return false;\n DrawAction::Parameters* p=dynamic_cast(params_);\n if (! p){\n _previousParams = 0;\n _show = 0;\n _showId = 0;\n } else {\n _previousParams = p;\n _show = p->makeProperty(_typeName+\"::SHOW\", true);\n _showId = p->makeProperty(_typeName+\"::SHOW_ID\", false);\n }\n return true;\n }\n\n void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* params, const std::string& typeName)\n {\n for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin(); \n it!=graph->vertices().end(); ++it){\n if ( typeName.empty() || typeid(*it->second).name()==typeName){\n (*action)(it->second, params);\n }\n }\n for (HyperGraph::EdgeSet::iterator it=graph->edges().begin(); \n it!=graph->edges().end(); ++it){\n if ( typeName.empty() || typeid(**it).name()==typeName)\n (*action)(*it, params);\n }\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/hyper_graph_action.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_HYPER_GRAPH_ACTION_H\n#define G2O_HYPER_GRAPH_ACTION_H\n\n#include \"hyper_graph.h\"\n#include \"../stuff/property.h\"\n\n#include \n#include \n#include \n#include \n#include \n\n\n// define to get verbose output\n//#define G2O_DEBUG_ACTIONLIB\n\nnamespace g2o {\n\n /**\n * \\brief Abstract action that operates on an entire graph\n */\n class HyperGraphAction {\n public:\n class Parameters {\n public:\n virtual ~Parameters();\n };\n\n class ParametersIteration : public Parameters {\n public:\n explicit ParametersIteration(int iter);\n int iteration;\n };\n\n virtual ~HyperGraphAction();\n\n /**\n * re-implement to carry out an action given the graph\n */\n virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0);\n };\n\n /**\n * \\brief Abstract action that operates on a graph entity\n */\n class HyperGraphElementAction{\n public:\n struct Parameters{\n virtual ~Parameters();\n };\n typedef std::map ActionMap;\n //! an action should be instantiated with the typeid.name of the graph element \n //! on which it operates\n HyperGraphElementAction(const std::string& typeName_=\"\");\n\n //! redefine this to do the action stuff. If successful, the action returns a pointer to itself\n virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);\n\n //! redefine this to do the action stuff. If successful, the action returns a pointer to itself\n virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);\n\n //! destroyed actions release the memory\n virtual ~HyperGraphElementAction();\n\n //! returns the typeid name of the action\n const std::string& typeName() const { return _typeName;}\n\n //! returns the name of an action, e.g \"draw\"\n const std::string& name() const{ return _name;}\n\n //! sets the type on which an action has to operate\n void setTypeName(const std::string& typeName_);\n\n protected:\n std::string _typeName;\n std::string _name;\n };\n\n /**\n * \\brief collection of actions\n *\n * collection of actions calls contains homogeneous actions operating on different types\n * all collected actions have the same name and should have the same functionality\n */\n class HyperGraphElementActionCollection: public HyperGraphElementAction{\n public:\n //! constructor. name_ is the name of the action e.g.draw).\n HyperGraphElementActionCollection(const std::string& name_);\n //! destructor: it deletes all actions in the pool.\n virtual ~HyperGraphElementActionCollection();\n //! calling functions, they return a pointer to the instance of action in actionMap\n //! that was active on element\n virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);\n virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);\n ActionMap& actionMap() {return _actionMap;}\n //! inserts an action in the pool. The action should have the same name of the container.\n //! returns false on failure (the container has a different name than the action);\n bool registerAction(HyperGraphElementAction* action);\n bool unregisterAction(HyperGraphElementAction* action);\n protected:\n ActionMap _actionMap;\n };\n\n /**\n * \\brief library of actions, indexed by the action name;\n *\n * library of actions, indexed by the action name;\n * one can use ti to register a collection of actions\n */\n class HyperGraphActionLibrary{\n public:\n //! return the single instance of the HyperGraphActionLibrary\n static HyperGraphActionLibrary* instance();\n //! free the instance\n static void destroy();\n\n // returns a pointer to a collection indexed by name\n HyperGraphElementAction* actionByName(const std::string& name);\n // registers a basic action in the pool. If necessary a container is created\n bool registerAction(HyperGraphElementAction* action);\n bool unregisterAction(HyperGraphElementAction* action);\n \n inline HyperGraphElementAction::ActionMap& actionMap() {return _actionMap;}\n protected:\n HyperGraphActionLibrary();\n ~HyperGraphActionLibrary();\n HyperGraphElementAction::ActionMap _actionMap;\n private:\n static HyperGraphActionLibrary* actionLibInstance;\n };\n\n /**\n * apply an action to all the elements of the graph.\n */\n void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* parameters=0, const std::string& typeName=\"\");\n\n /**\n * brief write into gnuplot\n */\n class WriteGnuplotAction: public HyperGraphElementAction{\n public:\n struct Parameters: public HyperGraphElementAction::Parameters{\n std::ostream* os;\n };\n WriteGnuplotAction(const std::string& typeName_);\n };\n\n /**\n * \\brief draw actions\n */\n\n class DrawAction : public HyperGraphElementAction{\n public:\n class Parameters: public HyperGraphElementAction::Parameters, public PropertyMap{\n public:\n Parameters();\n };\n DrawAction(const std::string& typeName_);\n protected:\n virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_);\n Parameters* _previousParams;\n BoolProperty* _show;\n BoolProperty* _showId;\n };\n\n template class RegisterActionProxy\n {\n public:\n RegisterActionProxy()\n {\n#ifdef G2O_DEBUG_ACTIONLIB\n std::cout << __FUNCTION__ << \": Registering action of type \" << typeid(T).name() << std::endl;\n#endif\n _action = new T();\n HyperGraphActionLibrary::instance()->registerAction(_action);\n }\n \n ~RegisterActionProxy()\n {\n#ifdef G2O_DEBUG_ACTIONLIB\n std::cout << __FUNCTION__ << \": Unregistering action of type \" << typeid(T).name() << std::endl;\n#endif\n HyperGraphActionLibrary::instance()->unregisterAction(_action);\n delete _action;\n }\n\n private:\n HyperGraphElementAction* _action;\n };\n\n#define G2O_REGISTER_ACTION(classname) \\\n extern \"C\" void g2o_action_##classname(void) {} \\\n static g2o::RegisterActionProxy g_action_proxy_##classname;\n};\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/jacobian_workspace.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"jacobian_workspace.h\"\n\n#include \n\n#include \"optimizable_graph.h\"\n\nusing namespace std;\n\nnamespace g2o {\n\nJacobianWorkspace::JacobianWorkspace() :\n _maxNumVertices(-1), _maxDimension(-1)\n{\n}\n\nJacobianWorkspace::~JacobianWorkspace()\n{\n}\n\nbool JacobianWorkspace::allocate()\n{\n //cerr << __PRETTY_FUNCTION__ << \" \" << PVAR(this) << \" \" << PVAR(_maxNumVertices) << \" \" << PVAR(_maxDimension) << endl;\n if (_maxNumVertices <=0 || _maxDimension <= 0)\n return false;\n _workspace.resize(_maxNumVertices);\n for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) {\n it->resize(_maxDimension);\n it->setZero();\n }\n return true;\n}\n\nvoid JacobianWorkspace::updateSize(const HyperGraph::Edge* e_)\n{\n const OptimizableGraph::Edge* e = static_cast(e_);\n int errorDimension = e->dimension();\n int numVertices = e->vertices().size();\n int maxDimensionForEdge = -1;\n for (int i = 0; i < numVertices; ++i) {\n const OptimizableGraph::Vertex* v = static_cast(e->vertex(i));\n assert(v && \"Edge has no vertex assigned\");\n maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge);\n }\n _maxNumVertices = max(numVertices, _maxNumVertices);\n _maxDimension = max(maxDimensionForEdge, _maxDimension);\n //cerr << __PRETTY_FUNCTION__ << \" \" << PVAR(this) << \" \" << PVAR(_maxNumVertices) << \" \" << PVAR(_maxDimension) << endl;\n}\n\nvoid JacobianWorkspace::updateSize(const OptimizableGraph& graph)\n{\n for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) {\n const OptimizableGraph::Edge* e = static_cast(*it);\n updateSize(e);\n }\n}\n\nvoid JacobianWorkspace::updateSize(int numVertices, int dimension)\n{\n _maxNumVertices = max(numVertices, _maxNumVertices);\n _maxDimension = max(dimension, _maxDimension);\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/jacobian_workspace.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef JACOBIAN_WORKSPACE_H\n#define JACOBIAN_WORKSPACE_H\n\n#include \n#include \n\n#include \n#include \n\n#include \"hyper_graph.h\"\n\nnamespace g2o {\n\n struct OptimizableGraph;\n\n /**\n * \\brief provide memory workspace for computing the Jacobians\n *\n * The workspace is used by an OptimizableGraph to provide temporary memory\n * for computing the Jacobian of the error functions.\n * Before calling linearizeOplus on an edge, the workspace needs to be allocated\n * by calling allocate().\n */\n class JacobianWorkspace\n {\n public:\n typedef std::vector > WorkspaceVector;\n\n public:\n JacobianWorkspace();\n ~JacobianWorkspace();\n\n /**\n * allocate the workspace\n */\n bool allocate();\n\n /**\n * update the maximum required workspace needed by taking into account this edge\n */\n void updateSize(const HyperGraph::Edge* e);\n\n /**\n * update the required workspace by looking at a full graph\n */\n void updateSize(const OptimizableGraph& graph);\n\n /**\n * manually update with the given parameters\n */\n void updateSize(int numVertices, int dimension);\n\n /**\n * return the workspace for a vertex in an edge\n */\n double* workspaceForVertex(int vertexIndex)\n {\n assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && \"Index out of bounds\");\n return _workspace[vertexIndex].data();\n }\n\n protected:\n WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians\n int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge\n int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian\n };\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/linear_solver.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_LINEAR_SOLVER_H\n#define G2O_LINEAR_SOLVER_H\n#include \"sparse_block_matrix.h\"\n#include \"sparse_block_matrix_ccs.h\"\n\nnamespace g2o {\n\n/**\n * \\brief basic solver for Ax = b\n *\n * basic solver for Ax = b which has to reimplemented for different linear algebra libraries.\n * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit.\n */\ntemplate \nclass LinearSolver\n{\n public:\n LinearSolver() {};\n virtual ~LinearSolver() {}\n\n /**\n * init for operating on matrices with a different non-zero pattern like before\n */\n virtual bool init() = 0;\n\n /**\n * Assumes that A is the same matrix for several calls.\n * Among other assumptions, the non-zero pattern does not change!\n * If the matrix changes call init() before.\n * solve system Ax = b, x and b have to allocated beforehand!!\n */\n virtual bool solve(const SparseBlockMatrix& A, double* x, double* b) = 0;\n\n /**\n * Inverts the diagonal blocks of A\n * @returns false if not defined.\n */\n virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix& A) { (void)blocks; (void) A; return false; }\n\n\n /**\n * Inverts the a block pattern of A in spinv\n * @returns false if not defined.\n */\n virtual bool solvePattern(SparseBlockMatrix& spinv, const std::vector >& blockIndices, const SparseBlockMatrix& A){\n (void) spinv;\n (void) blockIndices;\n (void) A;\n return false;\n }\n\n //! write a debug dump of the system matrix if it is not PSD in solve\n virtual bool writeDebug() const { return false;}\n virtual void setWriteDebug(bool) {}\n};\n\n/**\n * \\brief Solver with faster iterating structure for the linear matrix\n */\ntemplate \nclass LinearSolverCCS : public LinearSolver\n{\n public:\n LinearSolverCCS() : LinearSolver(), _ccsMatrix(0) {}\n ~LinearSolverCCS()\n {\n delete _ccsMatrix;\n }\n\n protected:\n SparseBlockMatrixCCS* _ccsMatrix;\n\n void initMatrixStructure(const SparseBlockMatrix& A)\n {\n delete _ccsMatrix;\n _ccsMatrix = new SparseBlockMatrixCCS(A.rowBlockIndices(), A.colBlockIndices());\n A.fillSparseBlockMatrixCCS(*_ccsMatrix);\n }\n};\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"marginal_covariance_cholesky.h\"\n\n#include \n#include \nusing namespace std;\n\nnamespace g2o {\n\nstruct MatrixElem\n{\n int r, c;\n MatrixElem(int r_, int c_) : r(r_), c(c_) {}\n bool operator<(const MatrixElem& other) const\n {\n return c > other.c || (c == other.c && r > other.r);\n }\n};\n\nMarginalCovarianceCholesky::MarginalCovarianceCholesky() :\n _n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0)\n{\n}\n\nMarginalCovarianceCholesky::~MarginalCovarianceCholesky()\n{\n}\n\nvoid MarginalCovarianceCholesky::setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv)\n{\n _n = n;\n _Ap = Lp;\n _Ai = Li;\n _Ax = Lx;\n _perm = permInv;\n\n // pre-compute reciprocal values of the diagonal of L\n _diag.resize(n);\n for (int r = 0; r < n; ++r) {\n const int& sc = _Ap[r]; // L is lower triangular, thus the first elem in the column is the diagonal entry\n assert(r == _Ai[sc] && \"Error in CCS storage of L\");\n _diag[r] = 1.0 / _Ax[sc];\n }\n}\n\ndouble MarginalCovarianceCholesky::computeEntry(int r, int c)\n{\n assert(r <= c);\n int idx = computeIndex(r, c);\n\n LookupMap::const_iterator foundIt = _map.find(idx);\n if (foundIt != _map.end()) {\n return foundIt->second;\n }\n\n // compute the summation over column r\n double s = 0.;\n const int& sc = _Ap[r];\n const int& ec = _Ap[r+1];\n for (int j = sc+1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal\n const int& rr = _Ai[j];\n double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr);\n s += val * _Ax[j];\n }\n\n double result;\n if (r == c) {\n const double& diagElem = _diag[r];\n result = diagElem * (diagElem - s);\n } else {\n result = -s * _diag[r];\n }\n _map[idx] = result;\n return result;\n}\n\nvoid MarginalCovarianceCholesky::computeCovariance(double** covBlocks, const std::vector& blockIndices)\n{\n _map.clear();\n int base = 0;\n vector elemsToCompute;\n for (size_t i = 0; i < blockIndices.size(); ++i) {\n int nbase = blockIndices[i];\n int vdim = nbase - base;\n for (int rr = 0; rr < vdim; ++rr)\n for (int cc = rr; cc < vdim; ++cc) {\n int r = _perm ? _perm[rr + base] : rr + base; // apply permutation\n int c = _perm ? _perm[cc + base] : cc + base;\n if (r > c) // make sure it's still upper triangular after applying the permutation\n swap(r, c);\n elemsToCompute.push_back(MatrixElem(r, c));\n }\n base = nbase;\n }\n\n // sort the elems to reduce the recursive calls\n sort(elemsToCompute.begin(), elemsToCompute.end());\n\n // compute the inverse elements we need\n for (size_t i = 0; i < elemsToCompute.size(); ++i) {\n const MatrixElem& me = elemsToCompute[i];\n computeEntry(me.r, me.c);\n }\n\n // set the marginal covariance for the vertices, by writing to the blocks memory\n base = 0;\n for (size_t i = 0; i < blockIndices.size(); ++i) {\n int nbase = blockIndices[i];\n int vdim = nbase - base;\n double* cov = covBlocks[i];\n for (int rr = 0; rr < vdim; ++rr)\n for (int cc = rr; cc < vdim; ++cc) {\n int r = _perm ? _perm[rr + base] : rr + base; // apply permutation\n int c = _perm ? _perm[cc + base] : cc + base;\n if (r > c) // upper triangle\n swap(r, c);\n int idx = computeIndex(r, c);\n LookupMap::const_iterator foundIt = _map.find(idx);\n assert(foundIt != _map.end());\n cov[rr*vdim + cc] = foundIt->second;\n if (rr != cc)\n cov[cc*vdim + rr] = foundIt->second;\n }\n base = nbase;\n }\n}\n\n\nvoid MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices)\n{\n // allocate the sparse\n spinv = SparseBlockMatrix(&rowBlockIndices[0], \n &rowBlockIndices[0], \n rowBlockIndices.size(),\n rowBlockIndices.size(), true);\n _map.clear();\n vector elemsToCompute;\n for (size_t i = 0; i < blockIndices.size(); ++i) {\n int blockRow=blockIndices[i].first; \n int blockCol=blockIndices[i].second;\n assert(blockRow>=0);\n assert(blockRow < (int)rowBlockIndices.size());\n assert(blockCol>=0);\n assert(blockCol < (int)rowBlockIndices.size());\n\n int rowBase=spinv.rowBaseOfBlock(blockRow);\n int colBase=spinv.colBaseOfBlock(blockCol);\n \n MatrixXd *block=spinv.block(blockRow, blockCol, true);\n assert(block);\n for (int iRow=0; iRowrows(); ++iRow)\n for (int iCol=0; iColcols(); ++iCol){\n int rr=rowBase+iRow;\n int cc=colBase+iCol;\n int r = _perm ? _perm[rr] : rr; // apply permutation\n int c = _perm ? _perm[cc] : cc;\n if (r > c)\n swap(r, c);\n elemsToCompute.push_back(MatrixElem(r, c));\n }\n }\n\n // sort the elems to reduce the number of recursive calls\n sort(elemsToCompute.begin(), elemsToCompute.end());\n\n // compute the inverse elements we need\n for (size_t i = 0; i < elemsToCompute.size(); ++i) {\n const MatrixElem& me = elemsToCompute[i];\n computeEntry(me.r, me.c);\n }\n\n // set the marginal covariance \n for (size_t i = 0; i < blockIndices.size(); ++i) {\n int blockRow=blockIndices[i].first; \n int blockCol=blockIndices[i].second;\n int rowBase=spinv.rowBaseOfBlock(blockRow);\n int colBase=spinv.colBaseOfBlock(blockCol);\n \n MatrixXd *block=spinv.block(blockRow, blockCol);\n assert(block);\n for (int iRow=0; iRowrows(); ++iRow)\n for (int iCol=0; iColcols(); ++iCol){\n int rr=rowBase+iRow;\n int cc=colBase+iCol;\n int r = _perm ? _perm[rr] : rr; // apply permutation\n int c = _perm ? _perm[cc] : cc;\n if (r > c)\n swap(r, c);\n int idx = computeIndex(r, c);\n LookupMap::const_iterator foundIt = _map.find(idx);\n assert(foundIt != _map.end());\n (*block)(iRow, iCol) = foundIt->second;\n }\n }\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_MARGINAL_COVARIANCE_CHOLESKY_H\n#define G2O_MARGINAL_COVARIANCE_CHOLESKY_H\n\n#include \"optimizable_graph.h\"\n#include \"sparse_block_matrix.h\"\n\n#include \n#include \n\n#ifdef _MSC_VER\n#include \n#else\n#include \n#endif\n\n\nnamespace g2o {\n\n /**\n * \\brief computing the marginal covariance given a cholesky factor (lower triangle of the factor)\n */\n class MarginalCovarianceCholesky {\n protected:\n /**\n * hash struct for storing the matrix elements needed to compute the covariance\n */\n typedef std::tr1::unordered_map LookupMap;\n \n public:\n MarginalCovarianceCholesky();\n ~MarginalCovarianceCholesky();\n\n /**\n * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to\n * be provided by the caller).\n */\n void computeCovariance(double** covBlocks, const std::vector& blockIndices);\n\n\n /**\n * compute the marginal cov for the given block indices, write the result in spinv).\n */\n void computeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector< std::pair >& blockIndices);\n\n\n /**\n * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in.\n * permInv might be 0, will then not permute the entries.\n *\n * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers\n * are owned by the caller, MarginalCovarianceCholesky does not free the pointers.\n */\n void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv);\n\n protected:\n // information about the cholesky factor (lower triangle)\n int _n; ///< L is an n X n matrix\n int* _Ap; ///< column pointer of the CCS storage\n int* _Ai; ///< row indices of the CCS storage\n double* _Ax; ///< values of the cholesky factor\n int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in\n\n LookupMap _map; ///< hash look up table for the already computed entries\n std::vector _diag; ///< cache 1 / H_ii to avoid recalculations\n\n //! compute the index used for hashing\n int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;}\n /**\n * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular.\n * May issue recursive calls to itself to compute the missing values.\n */\n double computeEntry(int r, int c);\n };\n\n}\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/matrix_operations.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_CORE_MATRIX_OPERATIONS_H\n#define G2O_CORE_MATRIX_OPERATIONS_H\n\n#include \n\nnamespace g2o {\n namespace internal {\n\n template\n inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff)\n {\n y.segment(yoff) += A * x.segment(xoff);\n }\n\n template\n inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff)\n {\n y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff);\n }\n\n template<>\n inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff)\n {\n y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols());\n }\n\n template\n inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff)\n {\n y.segment(yoff) += A.transpose() * x.segment(xoff);\n }\n\n template\n inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff)\n {\n y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows());\n }\n\n template<>\n inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff)\n {\n y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows());\n }\n\n } // end namespace internal\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/matrix_structure.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"matrix_structure.h\"\n\n#include \n#include \n#include \n#include \nusing namespace std;\n\nnamespace g2o {\n\nstruct ColSort\n{\n bool operator()(const pair& e1, const pair& e2) const\n {\n return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first);\n }\n};\n\nMatrixStructure::MatrixStructure() :\n n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0)\n{\n}\n\nMatrixStructure::~MatrixStructure()\n{\n free();\n}\n\nvoid MatrixStructure::alloc(int n_, int nz)\n{\n if (n == 0) {\n maxN = n = n_;\n maxNz = nz;\n Ap = new int[maxN + 1];\n Aii = new int[maxNz];\n }\n else {\n n = n_;\n if (maxNz < nz) {\n maxNz = 2 * nz;\n delete[] Aii;\n Aii = new int[maxNz];\n }\n if (maxN < n) {\n maxN = 2 * n;\n delete[] Ap;\n Ap = new int[maxN + 1];\n }\n }\n}\n\nvoid MatrixStructure::free()\n{\n n = 0;\n m = 0;\n maxN = 0;\n maxNz = 0;\n delete[] Aii; Aii = 0;\n delete[] Ap; Ap = 0;\n}\n\nbool MatrixStructure::write(const char* filename) const\n{\n const int& cols = n;\n const int& rows = m;\n\n string name = filename;\n std::string::size_type lastDot = name.find_last_of('.');\n if (lastDot != std::string::npos) \n name = name.substr(0, lastDot);\n\n vector > entries;\n for (int i=0; i < cols; ++i) {\n const int& rbeg = Ap[i];\n const int& rend = Ap[i+1];\n for (int j = rbeg; j < rend; ++j) {\n entries.push_back(make_pair(Aii[j], i));\n if (Aii[j] != i)\n entries.push_back(make_pair(i, Aii[j]));\n }\n }\n\n sort(entries.begin(), entries.end(), ColSort());\n\n std::ofstream fout(filename);\n fout << \"# name: \" << name << std::endl;\n fout << \"# type: sparse matrix\" << std::endl;\n fout << \"# nnz: \" << entries.size() << std::endl;\n fout << \"# rows: \" << rows << std::endl;\n fout << \"# columns: \" << cols << std::endl;\n for (vector >::const_iterator it = entries.begin(); it != entries.end(); ++it) {\n const pair& entry = *it;\n fout << entry.first << \" \" << entry.second << \" 0\" << std::endl; // write a constant value of 0\n }\n\n return fout.good();\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/matrix_structure.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_MATRIX_STRUCTURE_H\n#define G2O_MATRIX_STRUCTURE_H\n\n\nnamespace g2o {\n\n/**\n * \\brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix)\n */\nclass MatrixStructure\n{\n public:\n MatrixStructure();\n ~MatrixStructure();\n /**\n * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will\n * then reallocate the memory + additional space (double the required space).\n */\n void alloc(int n_, int nz);\n\n void free();\n \n /**\n * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix)\n */\n bool write(const char* filename) const;\n\n int n; ///< A is m-by-n. n must be >= 0.\n int m; ///< A is m-by-n. m must be >= 0.\n int* Ap; ///< column pointers for A, of size n+1\n int* Aii; ///< row indices of A, of size nz = Ap [n]\n\n //! max number of non-zeros blocks\n int nzMax() const { return maxNz;}\n\n protected:\n int maxN; ///< size of the allocated memory\n int maxNz; ///< size of the allocated memory\n};\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/openmp_mutex.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OPENMP_MUTEX\n#define G2O_OPENMP_MUTEX\n\n#include \"../../config.h\"\n\n#ifdef G2O_OPENMP\n#include \n#else\n#include \n#endif\n\nnamespace g2o {\n\n#ifdef G2O_OPENMP\n\n /**\n * \\brief Mutex realized via OpenMP\n */\n class OpenMPMutex\n {\n public:\n OpenMPMutex() { omp_init_lock(&_lock); }\n ~OpenMPMutex() { omp_destroy_lock(&_lock); }\n void lock() { omp_set_lock(&_lock); }\n void unlock() { omp_unset_lock(&_lock); }\n protected:\n omp_lock_t _lock;\n };\n\n#else\n\n /*\n * dummy which does nothing in case we don't have OpenMP support.\n * In debug mode, the mutex allows to verify the correct lock and unlock behavior\n */\n class OpenMPMutex\n {\n public:\n#ifdef NDEBUG\n OpenMPMutex() {}\n#else\n OpenMPMutex() : _cnt(0) {}\n#endif\n ~OpenMPMutex() { assert(_cnt == 0 && \"Freeing locked mutex\");}\n void lock() { assert(++_cnt == 1 && \"Locking already locked mutex\");}\n void unlock() { assert(--_cnt == 0 && \"Trying to unlock a mutex which is not locked\");}\n protected:\n#ifndef NDEBUG\n char _cnt;\n#endif\n };\n\n#endif\n\n /**\n * \\brief lock a mutex within a scope\n */\n class ScopedOpenMPMutex\n {\n public:\n explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); }\n ~ScopedOpenMPMutex() { _mutex->unlock(); }\n private:\n OpenMPMutex* const _mutex;\n ScopedOpenMPMutex(const ScopedOpenMPMutex&);\n void operator=(const ScopedOpenMPMutex&);\n };\n\n}\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimizable_graph.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"optimizable_graph.h\"\n\n#include \n#include \n#include \n#include \n#include \n\n#include \n\n#include \"estimate_propagator.h\"\n#include \"factory.h\"\n#include \"optimization_algorithm_property.h\"\n#include \"hyper_graph_action.h\"\n#include \"cache.h\"\n#include \"robust_kernel.h\"\n\n#include \"../stuff/macros.h\"\n#include \"../stuff/color_macros.h\"\n#include \"../stuff/string_tools.h\"\n#include \"../stuff/misc.h\"\n\nnamespace g2o {\n\n using namespace std;\n\n OptimizableGraph::Data::Data(){\n _next = 0;\n }\n \n OptimizableGraph::Data::~Data(){\n if (_next)\n delete _next;\n }\n \n\n OptimizableGraph::Vertex::Vertex() :\n HyperGraph::Vertex(),\n _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false),\n _colInHessian(-1), _cacheContainer(0)\n {\n }\n\n CacheContainer* OptimizableGraph::Vertex::cacheContainer(){\n if (! _cacheContainer)\n _cacheContainer = new CacheContainer(this);\n return _cacheContainer;\n }\n\n\n void OptimizableGraph::Vertex::updateCache(){\n if (_cacheContainer){\n _cacheContainer->setUpdateNeeded();\n _cacheContainer->update();\n }\n }\n\n OptimizableGraph::Vertex::~Vertex()\n {\n if (_cacheContainer)\n delete (_cacheContainer);\n if (_userData)\n delete _userData;\n }\n \n OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const\n {\n return 0;\n }\n\n bool OptimizableGraph::Vertex::setEstimateData(const double* v)\n {\n bool ret = setEstimateDataImpl(v);\n updateCache();\n return ret;\n }\n\n bool OptimizableGraph::Vertex::getEstimateData(double *) const\n {\n return false;\n }\n\n int OptimizableGraph::Vertex::estimateDimension() const\n {\n return -1;\n }\n\n bool OptimizableGraph::Vertex::setMinimalEstimateData(const double* v)\n {\n bool ret = setMinimalEstimateDataImpl(v);\n updateCache();\n return ret;\n }\n\n bool OptimizableGraph::Vertex::getMinimalEstimateData(double *) const\n {\n return false;\n }\n\n int OptimizableGraph::Vertex::minimalEstimateDimension() const\n {\n return -1;\n }\n\n\n OptimizableGraph::Edge::Edge() :\n HyperGraph::Edge(),\n _dimension(-1), _level(0), _robustKernel(0)\n {\n }\n\n OptimizableGraph::Edge::~Edge()\n {\n delete _robustKernel;\n }\n\n OptimizableGraph* OptimizableGraph::Edge::graph(){\n if (! _vertices.size())\n return 0;\n OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)_vertices[0];\n if (!v)\n return 0;\n return v->graph();\n }\n \n const OptimizableGraph* OptimizableGraph::Edge::graph() const{\n if (! _vertices.size())\n return 0;\n const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) _vertices[0];\n if (!v)\n return 0;\n return v->graph();\n }\n\n bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){\n if ((int)_parameters.size()<=argNum)\n return false;\n if (argNum<0)\n return false;\n *_parameters[argNum] = 0;\n _parameterIds[argNum] = paramId;\n return true;\n }\n\n bool OptimizableGraph::Edge::resolveParameters() {\n if (!graph()) {\n cerr << __PRETTY_FUNCTION__ << \": edge not registered with a graph\" << endl;\n return false;\n }\n \n assert (_parameters.size() == _parameterIds.size());\n //cerr << __PRETTY_FUNCTION__ << \": encountered \" << _parameters.size() << \" parameters\" << endl;\n for (size_t i=0; i<_parameters.size(); i++){\n int index = _parameterIds[i];\n *_parameters[i] = graph()->parameter(index);\n if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){\n cerr << __PRETTY_FUNCTION__ << \": FATAL, parameter type mismatch - encountered \" << typeid(**_parameters[i]).name() << \"; should be \" << _parameterTypes[i] << endl;\n }\n if (!*_parameters[i]) {\n cerr << __PRETTY_FUNCTION__ << \": FATAL, *_parameters[i] == 0\" << endl;\n return false;\n }\n }\n return true;\n }\n\n void OptimizableGraph::Edge::setRobustKernel(RobustKernel* ptr)\n {\n if (_robustKernel)\n delete _robustKernel;\n _robustKernel = ptr;\n }\n\n bool OptimizableGraph::Edge::resolveCaches() {\n return true;\n }\n\n bool OptimizableGraph::Edge::setMeasurementData(const double *)\n {\n return false;\n }\n\n bool OptimizableGraph::Edge::getMeasurementData(double *) const\n {\n return false;\n }\n\n int OptimizableGraph::Edge::measurementDimension() const\n {\n return -1;\n }\n\n bool OptimizableGraph::Edge::setMeasurementFromState(){\n return false;\n }\n\n\n OptimizableGraph::Edge* OptimizableGraph::Edge::clone() const\n {\n // TODO\n return 0;\n }\n\n\n OptimizableGraph::OptimizableGraph()\n {\n _nextEdgeId = 0; _edge_has_id = false;\n _graphActions.resize(AT_NUM_ELEMENTS);\n }\n\n OptimizableGraph::~OptimizableGraph()\n {\n clear();\n clearParameters();\n }\n\n bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData)\n {\n Vertex* inserted = vertex(v->id());\n if (inserted) {\n cerr << __FUNCTION__ << \": FATAL, a vertex with ID \" << v->id() << \" has already been registered with this graph\" << endl;\n assert(0 && \"Vertex with this ID already contained in the graph\");\n return false;\n }\n OptimizableGraph::Vertex* ov=dynamic_cast(v);\n assert(ov && \"Vertex does not inherit from OptimizableGraph::Vertex\");\n if (ov->_graph != 0 && ov->_graph != this) {\n cerr << __FUNCTION__ << \": FATAL, vertex with ID \" << v->id() << \" has already registered with another graph \" << ov->_graph << endl;\n assert(0 && \"Vertex already registered with another graph\");\n return false;\n }\n if (userData)\n ov->setUserData(userData);\n ov->_graph=this;\n return HyperGraph::addVertex(v);\n }\n\n bool OptimizableGraph::addEdge(HyperGraph::Edge* e_)\n {\n OptimizableGraph::Edge* e = dynamic_cast(e_);\n assert(e && \"Edge does not inherit from OptimizableGraph::Edge\");\n if (! e)\n return false;\n bool eresult = HyperGraph::addEdge(e);\n if (! eresult)\n return false;\n e->_internalId = _nextEdgeId++;\n if (! e->resolveParameters()){\n cerr << __FUNCTION__ << \": FATAL, cannot resolve parameters for edge \" << e << endl;\n return false;\n }\n if (! e->resolveCaches()){\n cerr << __FUNCTION__ << \": FATAL, cannot resolve caches for edge \" << e << endl;\n return false;\n } \n _jacobianWorkspace.updateSize(e);\n\n return true;\n }\n\n int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;}\n\ndouble OptimizableGraph::chi2() const\n{\n double chi = 0.0;\n for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); ++it) {\n const OptimizableGraph::Edge* e = static_cast(*it);\n chi += e->chi2();\n }\n return chi;\n}\n\nvoid OptimizableGraph::push()\n{\n for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(it->second);\n v->push();\n }\n}\n\nvoid OptimizableGraph::pop()\n{\n for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {\n OptimizableGraph::Vertex* v= static_cast(it->second);\n v->pop();\n }\n}\n\nvoid OptimizableGraph::discardTop()\n{\n for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {\n OptimizableGraph::Vertex* v= static_cast(it->second);\n v->discardTop();\n }\n}\n\nvoid OptimizableGraph::push(HyperGraph::VertexSet& vset)\n{\n for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(*it);\n v->push();\n }\n}\n\nvoid OptimizableGraph::pop(HyperGraph::VertexSet& vset)\n{\n for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(*it);\n v->pop();\n }\n}\n\nvoid OptimizableGraph::discardTop(HyperGraph::VertexSet& vset)\n{\n for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(*it);\n v->discardTop();\n }\n}\n\n void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed)\n{\n for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(*it);\n v->setFixed(fixed);\n }\n}\n\n\nbool OptimizableGraph::load(istream& is, bool createEdges)\n{\n // scna for the paramers in the whole file\n if (!_parameters.read(is,&_renamedTypesLookup))\n return false;\n#ifndef NDEBUG\n cerr << \"Loaded \" << _parameters.size() << \" parameters\" << endl;\n#endif\n is.clear();\n is.seekg(ios_base::beg);\n set warnedUnknownTypes;\n stringstream currentLine;\n string token;\n\n Factory* factory = Factory::instance();\n HyperGraph::GraphElemBitset elemBitset;\n elemBitset[HyperGraph::HGET_PARAMETER] = 1;\n elemBitset.flip();\n\n Vertex* previousVertex = 0;\n Data* previousData = 0;\n\n while (1) {\n int bytesRead = readLine(is, currentLine);\n if (bytesRead == -1)\n break;\n currentLine >> token;\n //cerr << \"Token=\" << token << endl;\n if (bytesRead == 0 || token.size() == 0 || token[0] == '#')\n continue;\n\n // handle commands encoded in the file\n bool handledCommand = false;\n \n if (token == \"FIX\") {\n handledCommand = true;\n int id;\n while (currentLine >> id) {\n OptimizableGraph::Vertex* v = static_cast(vertex(id));\n if (v) {\n# ifndef NDEBUG\n cerr << \"Fixing vertex \" << v->id() << endl;\n# endif\n v->setFixed(true);\n } else {\n cerr << \"Warning: Unable to fix vertex with id \" << id << \". Not found in the graph.\" << endl;\n }\n }\n }\n\n if (handledCommand)\n continue;\n \n // do the mapping to an internal type if it matches\n if (_renamedTypesLookup.size() > 0) {\n map::const_iterator foundIt = _renamedTypesLookup.find(token);\n if (foundIt != _renamedTypesLookup.end()) {\n token = foundIt->second;\n }\n }\n\n if (! factory->knowsTag(token)) {\n if (warnedUnknownTypes.count(token) != 1) {\n warnedUnknownTypes.insert(token);\n cerr << CL_RED(__PRETTY_FUNCTION__ << \" unknown type: \" << token) << endl;\n }\n continue;\n }\n\n HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);\n if (dynamic_cast(element)) { // it's a vertex type\n //cerr << \"it is a vertex\" << endl;\n previousData = 0;\n Vertex* v = static_cast(element);\n int id;\n currentLine >> id;\n bool r = v->read(currentLine);\n if (! r)\n cerr << __PRETTY_FUNCTION__ << \": Error reading vertex \" << token << \" \" << id << endl;\n v->setId(id);\n if (!addVertex(v)) {\n cerr << __PRETTY_FUNCTION__ << \": Failure adding Vertex, \" << token << \" \" << id << endl;\n delete v;\n } else {\n previousVertex = v;\n }\n }\n else if (dynamic_cast(element)) {\n //cerr << \"it is an edge\" << endl;\n previousData = 0;\n Edge* e = static_cast(element);\n int numV = e->vertices().size();\n if (_edge_has_id){\n int id;\n currentLine >> id;\n e->setId(id);\n }\n //cerr << PVAR(token) << \" \" << PVAR(numV) << endl;\n if (numV == 2) { // it's a pairwise / binary edge type which we handle in a special way\n int id1, id2;\n currentLine >> id1 >> id2;\n Vertex* from = vertex(id1);\n Vertex* to = vertex(id2);\n int doInit=0;\n if ((!from || !to) ) {\n if (! createEdges) {\n cerr << __PRETTY_FUNCTION__ << \": Unable to find vertices for edge \" << token << \" \" << id1 << \" <-> \" << id2 << endl;\n delete e;\n } else {\n if (! from) {\n from=e->createFrom();\n from->setId(id1);\n addVertex(from);\n doInit=2;\n }\n if (! to) {\n to=e->createTo();\n to->setId(id2);\n addVertex(to);\n doInit=1;\n }\n }\n }\n if (from && to) {\n e->setVertex(0, from);\n e->setVertex(1, to);\n e->read(currentLine);\n if (!addEdge(e)) {\n cerr << __PRETTY_FUNCTION__ << \": Unable to add edge \" << token << \" \" << id1 << \" <-> \" << id2 << endl;\n delete e;\n } else {\n switch (doInit){\n case 1: \n {\n HyperGraph::VertexSet fromSet;\n fromSet.insert(from);\n e->initialEstimate(fromSet, to);\n break;\n }\n case 2:\n {\n HyperGraph::VertexSet toSet;\n toSet.insert(to);\n e->initialEstimate(toSet, from);\n break;\n }\n default:;\n }\n }\n }\n }\n else {\n vector ids;\n ids.resize(numV);\n for (int l = 0; l < numV; ++l)\n currentLine >> ids[l];\n bool vertsOkay = true;\n for (int l = 0; l < numV; ++l) {\n e->setVertex(l, vertex(ids[l]));\n if (e->vertex(l) == 0) {\n vertsOkay = false;\n break;\n }\n }\n if (! vertsOkay) {\n cerr << __PRETTY_FUNCTION__ << \": Unable to find vertices for edge \" << token;\n for (int l = 0; l < numV; ++l) {\n if (l > 0)\n cerr << \" <->\";\n cerr << \" \" << ids[l];\n }\n delete e;\n } else {\n bool r = e->read(currentLine);\n if (!r || !addEdge(e)) {\n cerr << __PRETTY_FUNCTION__ << \": Unable to add edge \" << token; \n for (int l = 0; l < numV; ++l) {\n if (l > 0)\n cerr << \" <->\";\n cerr << \" \" << ids[l];\n }\n delete e;\n }\n }\n }\n } else if (dynamic_cast(element)) { // reading in the data packet for the vertex\n //cerr << \"read data packet \" << token << \" vertex \" << previousVertex->id() << endl;\n Data* d = static_cast(element);\n bool r = d->read(currentLine);\n if (! r) {\n\tcerr << __PRETTY_FUNCTION__ << \": Error reading data \" << token << \" for vertex \" << previousVertex->id() << endl;\n\tdelete d;\n\tpreviousData = 0;\n } else if (previousData){\n\t//cerr << \"chaining\" << endl;\n\tpreviousData->setNext(d);\n previousData = d;\n\t//cerr << \"done\" << endl;\n } else if (previousVertex){\n\t//cerr << \"embedding in vertex\" << endl;\n\tpreviousVertex->setUserData(d);\n\tpreviousData = d;\n\tpreviousVertex = 0;\n\t//cerr << \"done\" << endl;\n } else {\n cerr << __PRETTY_FUNCTION__ << \": got data element, but no vertex available\" << endl;\n delete d;\n\tpreviousData = 0;\n }\n }\n } // while read line\n \n return true;\n}\n\nbool OptimizableGraph::load(const char* filename, bool createEdges)\n{\n ifstream ifs(filename);\n if (!ifs) {\n cerr << __PRETTY_FUNCTION__ << \" unable to open file \" << filename << endl;\n return false;\n }\n return load(ifs, createEdges);\n}\n\nbool OptimizableGraph::save(const char* filename, int level) const\n{\n ofstream ofs(filename);\n if (!ofs)\n return false;\n return save(ofs, level);\n}\n\nbool OptimizableGraph::save(ostream& os, int level) const\n{\n if (! _parameters.write(os))\n return false;\n set verticesToSave;\n for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {\n OptimizableGraph::Edge* e = static_cast(*it);\n if (e->level() == level) {\n for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {\n verticesToSave.insert(static_cast(*it));\n }\n }\n }\n\n for (set::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){\n OptimizableGraph::Vertex* v = *it;\n saveVertex(os, v);\n }\n\n EdgeContainer edgesToSave;\n for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {\n const OptimizableGraph::Edge* e = dynamic_cast(*it);\n if (e->level() == level)\n edgesToSave.push_back(const_cast(e));\n }\n sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare());\n\n for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) {\n OptimizableGraph::Edge* e = *it;\n saveEdge(os, e);\n }\n\n return os.good();\n}\n\n\nbool OptimizableGraph::saveSubset(ostream& os, HyperGraph::VertexSet& vset, int level)\n{\n if (! _parameters.write(os))\n return false;\n\n for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){\n OptimizableGraph::Vertex* v = dynamic_cast(*it);\n saveVertex(os, v);\n }\n for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {\n OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);\n if (e->level() != level)\n continue;\n\n bool verticesInEdge = true;\n for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {\n if (vset.find(*it) == vset.end()) {\n verticesInEdge = false;\n break;\n }\n }\n if (! verticesInEdge)\n continue;\n\n saveEdge(os, e);\n }\n\n return os.good();\n}\n\nbool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset)\n{\n if (!_parameters.write(os))\n return false;\n std::set vset;\n for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {\n HyperGraph::Edge* e = *it;\n for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(*it);\n vset.insert(v);\n }\n }\n\n for (std::set::const_iterator it=vset.begin(); it!=vset.end(); ++it){\n OptimizableGraph::Vertex* v = dynamic_cast(*it);\n saveVertex(os, v);\n }\n\n for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {\n OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);\n saveEdge(os, e);\n }\n\n return os.good();\n}\n \nvoid OptimizableGraph::addGraph(OptimizableGraph* g){\n for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){\n OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second);\n if (vertex(v->id()))\n continue;\n OptimizableGraph::Vertex* v2=v->clone();\n v2->edges().clear();\n v2->setHessianIndex(-1);\n addVertex(v2);\n }\n for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){\n OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it);\n OptimizableGraph::Edge* en = e->clone();\n en->resize(e->vertices().size());\n int cnt = 0;\n for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {\n OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) vertex((*it)->id());\n assert(v);\n en->setVertex(cnt++, v);\n }\n addEdge(en);\n }\n}\n\nint OptimizableGraph::maxDimension() const{\n int maxDim=0;\n for (HyperGraph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); ++it){\n const OptimizableGraph::Vertex* v= static_cast< const OptimizableGraph::Vertex*>(it->second);\n maxDim = (std::max)(maxDim, v->dimension());\n }\n return maxDim;\n}\n\nvoid OptimizableGraph::setRenamedTypesFromString(const std::string& types)\n{\n Factory* factory = Factory::instance();\n vector typesMap = strSplit(types, \",\");\n for (size_t i = 0; i < typesMap.size(); ++i) {\n vector m = strSplit(typesMap[i], \"=\");\n if (m.size() != 2) {\n cerr << __PRETTY_FUNCTION__ << \": unable to extract type map from \" << typesMap[i] << endl;\n continue;\n }\n string typeInFile = trim(m[0]);\n string loadedType = trim(m[1]);\n if (! factory->knowsTag(loadedType)) {\n cerr << __PRETTY_FUNCTION__ << \": unknown type \" << loadedType << endl;\n continue;\n }\n\n _renamedTypesLookup[typeInFile] = loadedType;\n }\n\n cerr << \"# load look up table\" << endl;\n for (std::map::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) {\n cerr << \"#\\t\" << it->first << \" -> \" << it->second << endl;\n }\n}\n\nbool OptimizableGraph::isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set& vertDims_) const\n{\n std::set auxDims;\n if (vertDims_.size() == 0) {\n auxDims = dimensions();\n }\n const set& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;\n bool suitableSolver = true;\n if (vertDims.size() == 2) {\n if (solverProperty.requiresMarginalize) {\n suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1;\n }\n else {\n suitableSolver = solverProperty.poseDim == -1;\n }\n } else if (vertDims.size() == 1) {\n suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1;\n } else {\n suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize;\n }\n return suitableSolver;\n}\n\nstd::set OptimizableGraph::dimensions() const\n{\n std::set auxDims;\n for (VertexIDMap::const_iterator it = vertices().begin(); it != vertices().end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(it->second);\n auxDims.insert(v->dimension());\n }\n return auxDims;\n}\n\nvoid OptimizableGraph::preIteration(int iter)\n{\n HyperGraphActionSet& actions = _graphActions[AT_PREITERATION];\n if (actions.size() > 0) {\n HyperGraphAction::ParametersIteration params(iter);\n for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {\n (*(*it))(this, ¶ms);\n }\n }\n}\n\nvoid OptimizableGraph::postIteration(int iter)\n{\n HyperGraphActionSet& actions = _graphActions[AT_POSTITERATION];\n if (actions.size() > 0) {\n HyperGraphAction::ParametersIteration params(iter);\n for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {\n (*(*it))(this, ¶ms);\n }\n }\n}\n\nbool OptimizableGraph::addPostIterationAction(HyperGraphAction* action)\n{\n std::pair insertResult = _graphActions[AT_POSTITERATION].insert(action);\n return insertResult.second;\n}\n\nbool OptimizableGraph::addPreIterationAction(HyperGraphAction* action)\n{\n std::pair insertResult = _graphActions[AT_PREITERATION].insert(action);\n return insertResult.second;\n}\n\nbool OptimizableGraph::removePreIterationAction(HyperGraphAction* action)\n{\n return _graphActions[AT_PREITERATION].erase(action) > 0;\n}\n\nbool OptimizableGraph::removePostIterationAction(HyperGraphAction* action)\n{\n return _graphActions[AT_POSTITERATION].erase(action) > 0;\n}\n\nbool OptimizableGraph::saveVertex(std::ostream& os, OptimizableGraph::Vertex* v) const\n{\n Factory* factory = Factory::instance();\n string tag = factory->tag(v);\n if (tag.size() > 0) {\n os << tag << \" \" << v->id() << \" \";\n v->write(os);\n os << endl;\n Data* d=v->userData();\n while (d) { // write the data packet for the vertex\n tag = factory->tag(d);\n if (tag.size() > 0) {\n os << tag << \" \";\n d->write(os);\n os << endl;\n }\n d=d->next();\n }\n if (v->fixed()) {\n os << \"FIX \" << v->id() << endl;\n }\n return os.good();\n }\n return false;\n}\n\nbool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) const\n{\n Factory* factory = Factory::instance();\n string tag = factory->tag(e);\n if (tag.size() > 0) {\n os << tag << \" \";\n if (_edge_has_id)\n os << e->id() << \" \";\n for (vector::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(*it);\n os << v->id() << \" \";\n }\n e->write(os);\n os << endl;\n return os.good();\n }\n return false;\n}\n\nvoid OptimizableGraph::clearParameters()\n{\n _parameters.clear();\n}\n\nbool OptimizableGraph::verifyInformationMatrices(bool verbose) const\n{\n bool allEdgeOk = true;\n SelfAdjointEigenSolver eigenSolver;\n for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {\n OptimizableGraph::Edge* e = static_cast(*it);\n Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension());\n // test on symmetry\n bool isSymmetric = information.transpose() == information;\n bool okay = isSymmetric;\n if (isSymmetric) {\n // compute the eigenvalues\n eigenSolver.compute(information, Eigen::EigenvaluesOnly);\n bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;\n okay = okay && isSPD;\n }\n allEdgeOk = allEdgeOk && okay;\n if (! okay) {\n if (verbose) {\n if (! isSymmetric)\n cerr << \"Information Matrix for an edge is not symmetric:\";\n else\n cerr << \"Information Matrix for an edge is not SPD:\";\n for (size_t i = 0; i < e->vertices().size(); ++i)\n cerr << \" \" << e->vertex(i)->id();\n if (isSymmetric)\n cerr << \"\\teigenvalues: \" << eigenSolver.eigenvalues().transpose();\n cerr << endl;\n }\n }\n }\n return allEdgeOk;\n}\n\nbool OptimizableGraph::initMultiThreading()\n{\n# if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0)\n Eigen::initParallel();\n# endif\n return true;\n}\n\n} // end namespace\n\n" }, { "path": "Thirdparty/g2o/g2o/core/optimizable_graph.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_AIS_OPTIMIZABLE_GRAPH_HH_\n#define G2O_AIS_OPTIMIZABLE_GRAPH_HH_\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"openmp_mutex.h\"\n#include \"hyper_graph.h\"\n#include \"parameter.h\"\n#include \"parameter_container.h\"\n#include \"jacobian_workspace.h\"\n\n#include \"../stuff/macros.h\"\n\nnamespace g2o {\n\n class HyperGraphAction;\n struct OptimizationAlgorithmProperty;\n class Cache;\n class CacheContainer;\n class RobustKernel;\n\n /**\n @addtogroup g2o\n */\n /**\n This is an abstract class that represents one optimization\n problem. It specializes the general graph to contain special\n vertices and edges. The vertices represent parameters that can\n be optimized, while the edges represent constraints. This class\n also provides basic functionalities to handle the backup/restore\n of portions of the vertices.\n */\n struct OptimizableGraph : public HyperGraph {\n\n enum ActionType {\n AT_PREITERATION, AT_POSTITERATION,\n AT_NUM_ELEMENTS, // keep as last element\n };\n\n typedef std::set HyperGraphActionSet;\n\n // forward declarations\n class Vertex;\n class Edge;\n\n /**\n * \\brief data packet for a vertex. Extend this class to store in the vertices\n * the potential additional information you need (e.g. images, laser scans, ...).\n */\n class Data : public HyperGraph::HyperGraphElement\n {\n friend struct OptimizableGraph;\n public:\n virtual ~Data();\n Data();\n //! read the data from a stream\n virtual bool read(std::istream& is) = 0;\n //! write the data to a stream\n virtual bool write(std::ostream& os) const = 0;\n virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;}\n const Data* next() const {return _next;}\n Data* next() {return _next;}\n void setNext(Data* next_) { _next = next_; }\n protected:\n Data* _next; // linked list of multiple data;\n };\n\n /**\n * \\brief order vertices based on their ID\n */\n struct VertexIDCompare {\n bool operator() (const Vertex* v1, const Vertex* v2) const\n {\n return v1->id() < v2->id();\n }\n };\n\n /**\n * \\brief order edges based on the internal ID, which is assigned to the edge in addEdge()\n */\n struct EdgeIDCompare {\n bool operator() (const Edge* e1, const Edge* e2) const\n {\n return e1->internalId() < e2->internalId();\n }\n };\n\n //! vector container for vertices\n typedef std::vector VertexContainer;\n //! vector container for edges\n typedef std::vector EdgeContainer;\n\n /**\n * \\brief A general case Vertex for optimization\n */\n class Vertex : public HyperGraph::Vertex {\n private:\n friend struct OptimizableGraph;\n public:\n Vertex();\n\n //! returns a deep copy of the current vertex\n virtual Vertex* clone() const ;\n\n //! the user data associated with this vertex\n const Data* userData() const { return _userData; }\n Data* userData() { return _userData; }\n\n void setUserData(Data* obs) { _userData = obs;}\n\tvoid addUserData(Data* obs) { \n\t if (obs) {\n\t obs->setNext(_userData);\n\t _userData=obs;\n\t }\n\t}\n\t\n virtual ~Vertex();\n\n //! sets the node to the origin (used in the multilevel stuff)\n void setToOrigin() { setToOriginImpl(); updateCache();}\n\n //! get the element from the hessian matrix\n virtual const double& hessian(int i, int j) const = 0;\n virtual double& hessian(int i, int j) = 0;\n virtual double hessianDeterminant() const = 0;\n virtual double* hessianData() = 0;\n\n /** maps the internal matrix to some external memory location */\n virtual void mapHessianMemory(double* d) = 0;\n\n /**\n * copies the b vector in the array b_\n * @return the number of elements copied\n */\n virtual int copyB(double* b_) const = 0;\n\n //! get the b vector element\n virtual const double& b(int i) const = 0;\n virtual double& b(int i) = 0;\n //! return a pointer to the b vector associated with this vertex\n virtual double* bData() = 0;\n\n /**\n * set the b vector part of this vertex to zero\n */\n virtual void clearQuadraticForm() = 0;\n\n /**\n * updates the current vertex with the direct solution x += H_ii\\b_ii\n * @return the determinant of the inverted hessian\n */\n virtual double solveDirect(double lambda=0) = 0;\n\n /**\n * sets the initial estimate from an array of double\n * Implement setEstimateDataImpl()\n * @return true on success\n */\n bool setEstimateData(const double* estimate);\n\n /**\n * sets the initial estimate from an array of double\n * Implement setEstimateDataImpl()\n * @return true on success\n */\n bool setEstimateData(const std::vector& estimate) { \n#ifndef NDEBUG\n int dim = estimateDimension();\n assert((dim == -1) || (estimate.size() == std::size_t(dim)));\n#endif\n return setEstimateData(&estimate[0]);\n };\n\n /**\n * writes the estimater to an array of double\n * @returns true on success\n */\n virtual bool getEstimateData(double* estimate) const;\n\n /**\n * writes the estimater to an array of double\n * @returns true on success\n */\n virtual bool getEstimateData(std::vector& estimate) const {\n int dim = estimateDimension();\n if (dim < 0)\n return false;\n estimate.resize(dim);\n return getEstimateData(&estimate[0]);\n };\n\n /**\n * returns the dimension of the extended representation used by get/setEstimate(double*)\n * -1 if it is not supported\n */\n virtual int estimateDimension() const;\n\n /**\n * sets the initial estimate from an array of double.\n * Implement setMinimalEstimateDataImpl()\n * @return true on success\n */\n bool setMinimalEstimateData(const double* estimate);\n\n /**\n * sets the initial estimate from an array of double.\n * Implement setMinimalEstimateDataImpl()\n * @return true on success\n */\n bool setMinimalEstimateData(const std::vector& estimate) {\n#ifndef NDEBUG\n int dim = minimalEstimateDimension();\n assert((dim == -1) || (estimate.size() == std::size_t(dim)));\n#endif\n return setMinimalEstimateData(&estimate[0]);\n };\n\n /**\n * writes the estimate to an array of double\n * @returns true on success\n */\n virtual bool getMinimalEstimateData(double* estimate) const ;\n\n /**\n * writes the estimate to an array of double\n * @returns true on success\n */\n virtual bool getMinimalEstimateData(std::vector& estimate) const {\n int dim = minimalEstimateDimension();\n if (dim < 0)\n return false;\n estimate.resize(dim);\n return getMinimalEstimateData(&estimate[0]);\n };\n\n /**\n * returns the dimension of the extended representation used by get/setEstimate(double*)\n * -1 if it is not supported\n */\n virtual int minimalEstimateDimension() const;\n\n //! backup the position of the vertex to a stack\n virtual void push() = 0;\n\n //! restore the position of the vertex by retrieving the position from the stack\n virtual void pop() = 0;\n\n //! pop the last element from the stack, without restoring the current estimate\n virtual void discardTop() = 0;\n\n //! return the stack size\n virtual int stackSize() const = 0;\n\n /**\n * Update the position of the node from the parameters in v.\n * Depends on the implementation of oplusImpl in derived classes to actually carry\n * out the update.\n * Will also call updateCache() to update the caches of depending on the vertex.\n */\n void oplus(const double* v)\n {\n oplusImpl(v);\n updateCache();\n }\n\n //! temporary index of this node in the parameter vector obtained from linearization\n int hessianIndex() const { return _hessianIndex;}\n int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex();}\n //! set the temporary index of the vertex in the parameter blocks\n void setHessianIndex(int ti) { _hessianIndex = ti;}\n void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti);}\n\n //! true => this node is fixed during the optimization\n bool fixed() const {return _fixed;}\n //! true => this node should be considered fixed during the optimization\n void setFixed(bool fixed) { _fixed = fixed;}\n\n //! true => this node is marginalized out during the optimization\n bool marginalized() const {return _marginalized;}\n //! true => this node should be marginalized out during the optimization\n void setMarginalized(bool marginalized) { _marginalized = marginalized;}\n\n //! dimension of the estimated state belonging to this node\n int dimension() const { return _dimension;}\n\n //! sets the id of the node in the graph be sure that the graph keeps consistent after changing the id\n virtual void setId(int id) {_id = id;}\n\n //! set the row of this vertex in the Hessian\n void setColInHessian(int c) { _colInHessian = c;}\n //! get the row of this vertex in the Hessian\n int colInHessian() const {return _colInHessian;}\n\n const OptimizableGraph* graph() const {return _graph;}\n\n OptimizableGraph* graph() {return _graph;}\n\n /**\n * lock for the block of the hessian and the b vector associated with this vertex, to avoid\n * race-conditions if multi-threaded.\n */\n void lockQuadraticForm() { _quadraticFormMutex.lock();}\n /**\n * unlock the block of the hessian and the b vector associated with this vertex\n */\n void unlockQuadraticForm() { _quadraticFormMutex.unlock();}\n\n //! read the vertex from a stream, i.e., the internal state of the vertex\n virtual bool read(std::istream& is) = 0;\n //! write the vertex to a stream\n virtual bool write(std::ostream& os) const = 0;\n\n virtual void updateCache();\n\n CacheContainer* cacheContainer();\n protected:\n OptimizableGraph* _graph;\n Data* _userData;\n int _hessianIndex;\n bool _fixed;\n bool _marginalized;\n int _dimension;\n int _colInHessian;\n OpenMPMutex _quadraticFormMutex;\n\n CacheContainer* _cacheContainer;\n\n /**\n * update the position of the node from the parameters in v.\n * Implement in your class!\n */\n virtual void oplusImpl(const double* v) = 0;\n\n //! sets the node to the origin (used in the multilevel stuff)\n virtual void setToOriginImpl() = 0;\n\n /**\n * writes the estimater to an array of double\n * @returns true on success\n */\n virtual bool setEstimateDataImpl(const double* ) { return false;}\n\n /**\n * sets the initial estimate from an array of double\n * @return true on success\n */\n virtual bool setMinimalEstimateDataImpl(const double* ) { return false;}\n\n };\n \n class Edge: public HyperGraph::Edge {\n private:\n friend struct OptimizableGraph;\n public:\n Edge();\n virtual ~Edge();\n virtual Edge* clone() const;\n\n // indicates if all vertices are fixed\n virtual bool allVerticesFixed() const = 0;\n \n // computes the error of the edge and stores it in an internal structure\n virtual void computeError() = 0;\n\n //! sets the measurement from an array of double\n //! @returns true on success\n virtual bool setMeasurementData(const double* m);\n\n //! writes the measurement to an array of double\n //! @returns true on success\n virtual bool getMeasurementData(double* m) const;\n\n //! returns the dimension of the measurement in the extended representation which is used\n //! by get/setMeasurement;\n virtual int measurementDimension() const;\n\n /**\n * sets the estimate to have a zero error, based on the current value of the state variables\n * returns false if not supported.\n */\n virtual bool setMeasurementFromState();\n\n //! if NOT NULL, error of this edge will be robustifed with the kernel\n RobustKernel* robustKernel() const { return _robustKernel;}\n /**\n * specify the robust kernel to be used in this edge\n */\n void setRobustKernel(RobustKernel* ptr);\n\n //! returns the error vector cached after calling the computeError;\n virtual const double* errorData() const = 0;\n virtual double* errorData() = 0;\n\n //! returns the memory of the information matrix, usable for example with a Eigen::Map\n virtual const double* informationData() const = 0;\n virtual double* informationData() = 0;\n\n //! computes the chi2 based on the cached error value, only valid after computeError has been called.\n virtual double chi2() const = 0;\n\n /**\n * Linearizes the constraint in the edge.\n * Makes side effect on the vertices of the graph by changing\n * the parameter vector b and the hessian blocks ii and jj.\n * The off diagoinal block is accesed via _hessian.\n */\n virtual void constructQuadraticForm() = 0;\n\n /**\n * maps the internal matrix to some external memory location,\n * you need to provide the memory before calling constructQuadraticForm\n * @param d the memory location to which we map\n * @param i index of the vertex i\n * @param j index of the vertex j (j > i, upper triangular fashion)\n * @param rowMajor if true, will write in rowMajor order to the block. Since EIGEN is columnMajor by default, this results in writing the transposed\n */\n virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0;\n\n /**\n * Linearizes the constraint in the edge in the manifold space, and store\n * the result in the given workspace\n */\n virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0;\n\n /** set the estimate of the to vertex, based on the estimate of the from vertices in the edge. */\n virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) = 0;\n\n /**\n * override in your class if it's possible to initialize the vertices in certain combinations.\n * The return value may correspond to the cost for initiliaizng the vertex but should be positive if\n * the initialization is possible and negative if not possible.\n */\n virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) from; (void) to; return -1.;}\n\n //! returns the level of the edge\n int level() const { return _level;}\n //! sets the level of the edge\n void setLevel(int l) { _level=l;}\n\n //! returns the dimensions of the error function\n int dimension() const { return _dimension;}\n\n virtual Vertex* createFrom() {return 0;}\n virtual Vertex* createTo() {return 0;}\n\n //! read the vertex from a stream, i.e., the internal state of the vertex\n virtual bool read(std::istream& is) = 0;\n //! write the vertex to a stream\n virtual bool write(std::ostream& os) const = 0;\n\n //! the internal ID of the edge\n long long internalId() const { return _internalId;}\n\n OptimizableGraph* graph();\n const OptimizableGraph* graph() const;\n\n bool setParameterId(int argNum, int paramId);\n inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);}\n inline size_t numParameters() const {return _parameters.size();}\n inline void resizeParameters(size_t newSize) {\n _parameters.resize(newSize, 0); \n _parameterIds.resize(newSize, -1);\n _parameterTypes.resize(newSize, typeid(void*).name());\n }\n protected:\n int _dimension;\n int _level;\n RobustKernel* _robustKernel;\n long long _internalId;\n\n std::vector _cacheIds;\n\n template \n bool installParameter(ParameterType*& p, size_t argNo, int paramId=-1){\n if (argNo>=_parameters.size())\n return false;\n _parameterIds[argNo] = paramId;\n _parameters[argNo] = (Parameter**)&p;\n _parameterTypes[argNo] = typeid(ParameterType).name();\n return true;\n }\n\n template \n void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*, \n const std::string& _type, \n const ParameterVector& parameters);\n\n bool resolveParameters();\n virtual bool resolveCaches();\n\n std::vector _parameterTypes;\n std::vector _parameters;\n std::vector _parameterIds;\n };\n\n //! returns the vertex number id appropriately casted\n inline Vertex* vertex(int id) { return reinterpret_cast(HyperGraph::vertex(id));}\n\n //! returns the vertex number id appropriately casted\n inline const Vertex* vertex (int id) const{ return reinterpret_cast(HyperGraph::vertex(id));}\n\n //! empty constructor\n OptimizableGraph();\n virtual ~OptimizableGraph();\n\n //! adds all edges and vertices of the graph g to this graph.\n void addGraph(OptimizableGraph* g);\n \n /**\n * adds a new vertex. The new vertex is then \"taken\".\n * @return false if a vertex with the same id as v is already in the graph, true otherwise.\n */\n virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);\n virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);}\n\n /**\n * adds a new edge.\n * The edge should point to the vertices that it is connecting (setFrom/setTo).\n * @return false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise.\n */\n virtual bool addEdge(HyperGraph::Edge* e);\n\n //! returns the chi2 of the current configuration\n double chi2() const;\n\n //! return the maximum dimension of all vertices in the graph\n int maxDimension() const;\n\n /**\n * iterates over all vertices and returns a set of all the vertex dimensions in the graph\n */\n std::set dimensions() const;\n\n /**\n * carry out n iterations\n * @return the number of performed iterations\n */\n virtual int optimize(int iterations, bool online=false);\n\n //! called at the beginning of an iteration (argument is the number of the iteration)\n virtual void preIteration(int);\n //! called at the end of an iteration (argument is the number of the iteration)\n virtual void postIteration(int);\n\n //! add an action to be executed before each iteration\n bool addPreIterationAction(HyperGraphAction* action);\n //! add an action to be executed after each iteration\n bool addPostIterationAction(HyperGraphAction* action);\n\n //! remove an action that should no longer be execured before each iteration\n bool removePreIterationAction(HyperGraphAction* action);\n //! remove an action that should no longer be execured after each iteration\n bool removePostIterationAction(HyperGraphAction* action);\n\n //! push the estimate of all variables onto a stack\n virtual void push();\n //! pop (restore) the estimate of all variables from the stack\n virtual void pop();\n //! discard the last backup of the estimate for all variables by removing it from the stack\n virtual void discardTop();\n\n //! load the graph from a stream. Uses the Factory singleton for creating the vertices and edges.\n virtual bool load(std::istream& is, bool createEdges=true);\n bool load(const char* filename, bool createEdges=true);\n //! save the graph to a stream. Again uses the Factory system.\n virtual bool save(std::ostream& os, int level = 0) const;\n //! function provided for convenience, see save() above\n bool save(const char* filename, int level = 0) const;\n\n \n //! save a subgraph to a stream. Again uses the Factory system.\n bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0);\n\n //! save a subgraph to a stream. Again uses the Factory system.\n bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset);\n\n //! push the estimate of a subset of the variables onto a stack\n virtual void push(HyperGraph::VertexSet& vset);\n //! pop (restore) the estimate a subset of the variables from the stack\n virtual void pop(HyperGraph::VertexSet& vset);\n //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate\n virtual void discardTop(HyperGraph::VertexSet& vset);\n\n //! fixes/releases a set of vertices\n virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed);\n\n /**\n * set the renamed types lookup from a string, format is for example:\n * VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP\n * This will change the occurance of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP\n */\n void setRenamedTypesFromString(const std::string& types);\n\n /**\n * test whether a solver is suitable for optimizing this graph.\n * @param solverProperty the solver property to evaluate.\n * @param vertDims should equal to the set returned by dimensions() to avoid re-evaluating.\n */\n bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set& vertDims = std::set()) const;\n\n /**\n * remove the parameters of the graph\n */\n virtual void clearParameters();\n\n bool addParameter(Parameter* p) {\n return _parameters.addParameter(p);\n }\n\n Parameter* parameter(int id) {\n return _parameters.getParameter(id);\n }\n\n /**\n * verify that all the information of the edges are semi positive definite, i.e.,\n * all Eigenvalues are >= 0.\n * @param verbose output edges with not PSD information matrix on cerr\n * @return true if all edges have PSD information matrix\n */\n bool verifyInformationMatrices(bool verbose = false) const;\n\n // helper functions to save an individual vertex\n bool saveVertex(std::ostream& os, Vertex* v) const;\n\n // helper functions to save an individual edge\n bool saveEdge(std::ostream& os, Edge* e) const;\n //! the workspace for storing the Jacobians of the graph\n JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace;}\n const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace;}\n\n /**\n * Eigen starting from version 3.1 requires that we call an initialize\n * function, if we perform calls to Eigen from several threads.\n * Currently, this function calls Eigen::initParallel if g2o is compiled\n * with OpenMP support and Eigen's version is at least 3.1\n */\n static bool initMultiThreading();\n\n protected:\n std::map _renamedTypesLookup;\n long long _nextEdgeId;\n std::vector _graphActions;\n\n // do not watch this. To be removed soon, or integrated in a nice way\n bool _edge_has_id;\n\n ParameterContainer _parameters;\n JacobianWorkspace _jacobianWorkspace;\n };\n \n /**\n @}\n */\n \n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"optimization_algorithm.h\"\n\nusing namespace std;\n\nnamespace g2o {\n\nOptimizationAlgorithm::OptimizationAlgorithm() :\n _optimizer(0)\n{\n}\n\nOptimizationAlgorithm::~OptimizationAlgorithm()\n{\n}\n\nvoid OptimizationAlgorithm::printProperties(std::ostream& os) const\n{\n os << \"------------- Algorithm Properties -------------\" << endl;\n for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) {\n BaseProperty* p = it->second;\n os << it->first << \"\\t\" << p->toString() << endl;\n }\n os << \"------------------------------------------------\" << endl;\n}\n\nbool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString)\n{\n return _properties.updateMapFromString(propString);\n}\n\nvoid OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer)\n{\n _optimizer = optimizer;\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OPTIMIZATION_ALGORITHM_H\n#define G2O_OPTIMIZATION_ALGORITHM_H\n\n#include \n#include \n#include \n\n#include \"../stuff/property.h\"\n\n#include \"hyper_graph.h\"\n#include \"sparse_block_matrix.h\"\n\nnamespace g2o {\n\n class SparseOptimizer;\n\n /**\n * \\brief Generic interface for a non-linear solver operating on a graph\n */\n class OptimizationAlgorithm\n {\n public:\n enum SolverResult {Terminate=2, OK=1, Fail=-1};\n OptimizationAlgorithm();\n virtual ~OptimizationAlgorithm();\n\n /**\n * initialize the solver, called once before the first call to solve()\n */\n virtual bool init(bool online = false) = 0;\n\n /**\n * Solve one iteration. The SparseOptimizer running on-top will call this\n * for the given number of iterations.\n * @param iteration indicates the current iteration\n */\n virtual SolverResult solve(int iteration, bool online = false) = 0;\n\n /**\n * computes the block diagonal elements of the pattern specified in the input\n * and stores them in given SparseBlockMatrix.\n * If your solver does not support computing the marginals, return false.\n */\n virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0;\n\n /**\n * update the structures for online processing\n */\n virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0;\n\n /**\n * called by the optimizer if verbose. re-implement, if you want to print something\n */\n virtual void printVerbose(std::ostream& os) const {(void) os;};\n\n public:\n //! return the optimizer operating on\n const SparseOptimizer* optimizer() const { return _optimizer;}\n SparseOptimizer* optimizer() { return _optimizer;}\n\n /**\n * specify on which optimizer the solver should work on\n */\n void setOptimizer(SparseOptimizer* optimizer);\n\n //! return the properties of the solver\n const PropertyMap& properties() const { return _properties;}\n\n /**\n * update the properties from a string, see PropertyMap::updateMapFromString()\n */\n bool updatePropertiesFromString(const std::string& propString);\n \n /**\n * print the properties to a stream in a human readable fashion\n */\n void printProperties(std::ostream& os) const;\n\n protected:\n SparseOptimizer* _optimizer; ///< the optimizer the solver is working on\n PropertyMap _properties; ///< the properties of your solver, use this to store the parameters of your solver\n\n private:\n // Disable the copy constructor and assignment operator\n OptimizationAlgorithm(const OptimizationAlgorithm&) { }\n OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; }\n };\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"optimization_algorithm_dogleg.h\"\n\n#include \n\n#include \"../stuff/timeutil.h\"\n\n#include \"block_solver.h\"\n#include \"sparse_optimizer.h\"\n#include \"solver.h\"\n#include \"batch_stats.h\"\nusing namespace std;\n\nnamespace g2o {\n\n OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(BlockSolverBase* solver) :\n OptimizationAlgorithmWithHessian(solver)\n {\n _userDeltaInit = _properties.makeProperty >(\"initialDelta\", 1e4);\n _maxTrialsAfterFailure = _properties.makeProperty >(\"maxTrialsAfterFailure\", 100);\n _initialLambda = _properties.makeProperty >(\"initialLambda\", 1e-7);\n _lamdbaFactor = _properties.makeProperty >(\"lambdaFactor\", 10.);\n _delta = _userDeltaInit->value();\n _lastStep = STEP_UNDEFINED;\n _wasPDInAllIterations = true;\n }\n\n OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg()\n {\n }\n\n OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(int iteration, bool online)\n {\n assert(_optimizer && \"_optimizer not set\");\n assert(_solver->optimizer() == _optimizer && \"underlying linear solver operates on different graph\");\n assert(dynamic_cast(_solver) && \"underlying linear solver is not a block solver\");\n\n BlockSolverBase* blockSolver = static_cast(_solver);\n\n if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure\n bool ok = _solver->buildStructure();\n if (! ok) {\n cerr << __PRETTY_FUNCTION__ << \": Failure while building CCS structure\" << endl;\n return OptimizationAlgorithm::Fail;\n }\n\n // init some members to the current size of the problem\n _hsd.resize(_solver->vectorSize());\n _hdl.resize(_solver->vectorSize());\n _auxVector.resize(_solver->vectorSize());\n _delta = _userDeltaInit->value();\n _currentLambda = _initialLambda->value();\n _wasPDInAllIterations = true;\n }\n\n double t=get_monotonic_time();\n _optimizer->computeActiveErrors();\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats) {\n globalStats->timeResiduals = get_monotonic_time()-t;\n t=get_monotonic_time();\n }\n\n double currentChi = _optimizer->activeRobustChi2();\n\n _solver->buildSystem();\n if (globalStats) {\n globalStats->timeQuadraticForm = get_monotonic_time()-t;\n }\n\n Eigen::VectorXd::ConstMapType b(_solver->b(), _solver->vectorSize());\n\n // compute alpha\n _auxVector.setZero();\n blockSolver->multiplyHessian(_auxVector.data(), _solver->b());\n double bNormSquared = b.squaredNorm();\n double alpha = bNormSquared / _auxVector.dot(b);\n\n _hsd = alpha * b;\n double hsdNorm = _hsd.norm();\n double hgnNorm = -1.;\n\n bool solvedGaussNewton = false;\n bool goodStep = false;\n int& numTries = _lastNumTries;\n numTries = 0;\n do {\n ++numTries;\n\n if (! solvedGaussNewton) {\n const double minLambda = 1e-12;\n const double maxLambda = 1e3;\n solvedGaussNewton = true;\n // apply a damping factor to enforce positive definite Hessian, if the matrix appeared\n // to be not positive definite in at least one iteration before.\n // We apply a damping factor to obtain a PD matrix.\n bool solverOk = false;\n while(!solverOk) {\n if (! _wasPDInAllIterations)\n _solver->setLambda(_currentLambda, true); // add _currentLambda to the diagonal\n solverOk = _solver->solve();\n if (! _wasPDInAllIterations)\n _solver->restoreDiagonal();\n _wasPDInAllIterations = _wasPDInAllIterations && solverOk;\n if (! _wasPDInAllIterations) {\n // simple strategy to control the damping factor\n if (solverOk) {\n _currentLambda = std::max(minLambda, _currentLambda / (0.5 * _lamdbaFactor->value()));\n } else {\n _currentLambda *= _lamdbaFactor->value();\n if (_currentLambda > maxLambda) {\n _currentLambda = maxLambda;\n return Fail;\n }\n }\n }\n }\n if (!solverOk) {\n return Fail;\n }\n hgnNorm = Eigen::VectorXd::ConstMapType(_solver->x(), _solver->vectorSize()).norm();\n }\n\n Eigen::VectorXd::ConstMapType hgn(_solver->x(), _solver->vectorSize());\n assert(hgnNorm >= 0. && \"Norm of the GN step is not computed\");\n\n if (hgnNorm < _delta) {\n _hdl = hgn;\n _lastStep = STEP_GN;\n }\n else if (hsdNorm > _delta) {\n _hdl = _delta / hsdNorm * _hsd;\n _lastStep = STEP_SD;\n } else {\n _auxVector = hgn - _hsd; // b - a\n double c = _hsd.dot(_auxVector);\n double bmaSquaredNorm = _auxVector.squaredNorm();\n double beta;\n if (c <= 0.)\n beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm;\n else {\n double hsdSqrNorm = _hsd.squaredNorm();\n beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm)));\n }\n assert(beta > 0. && beta < 1 && \"Error while computing beta\");\n _hdl = _hsd + beta * (hgn - _hsd);\n _lastStep = STEP_DL;\n assert(_hdl.norm() < _delta + 1e-5 && \"Computed step does not correspond to the trust region\");\n }\n\n // compute the linear gain\n _auxVector.setZero();\n blockSolver->multiplyHessian(_auxVector.data(), _hdl.data());\n double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl));\n\n // apply the update and see what happens\n _optimizer->push();\n _optimizer->update(_hdl.data());\n _optimizer->computeActiveErrors();\n double newChi = _optimizer-> activeRobustChi2();\n double nonLinearGain = currentChi - newChi;\n if (fabs(linearGain) < 1e-12)\n linearGain = 1e-12;\n double rho = nonLinearGain / linearGain;\n //cerr << PVAR(nonLinearGain) << \" \" << PVAR(linearGain) << \" \" << PVAR(rho) << endl;\n if (rho > 0) { // step is good and will be accepted\n _optimizer->discardTop();\n goodStep = true;\n } else { // recover previous state\n _optimizer->pop();\n }\n\n // update trust region based on the step quality\n if (rho > 0.75)\n _delta = std::max(_delta, 3. * _hdl.norm());\n else if (rho < 0.25)\n _delta *= 0.5;\n } while (!goodStep && numTries < _maxTrialsAfterFailure->value());\n if (numTries == _maxTrialsAfterFailure->value() || !goodStep)\n return Terminate;\n return OK;\n }\n\n void OptimizationAlgorithmDogleg::printVerbose(std::ostream& os) const\n {\n os\n << \"\\t Delta= \" << _delta\n << \"\\t step= \" << stepType2Str(_lastStep)\n << \"\\t tries= \" << _lastNumTries;\n if (! _wasPDInAllIterations)\n os << \"\\t lambda= \" << _currentLambda;\n }\n\n const char* OptimizationAlgorithmDogleg::stepType2Str(int stepType)\n {\n switch (stepType) {\n case STEP_SD: return \"Descent\";\n case STEP_GN: return \"GN\";\n case STEP_DL: return \"Dogleg\";\n default: return \"Undefined\";\n }\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H\n#define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H\n\n#include \"optimization_algorithm_with_hessian.h\"\n\nnamespace g2o {\n\n class BlockSolverBase;\n\n /**\n * \\brief Implementation of Powell's Dogleg Algorithm\n */\n class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian\n {\n public:\n /** \\brief type of the step to take */\n enum {\n STEP_UNDEFINED,\n STEP_SD, STEP_GN, STEP_DL\n };\n\n public:\n /**\n * construct the Dogleg algorithm, which will use the given Solver for solving the\n * linearized system.\n */\n explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver);\n virtual ~OptimizationAlgorithmDogleg();\n\n virtual SolverResult solve(int iteration, bool online = false);\n\n virtual void printVerbose(std::ostream& os) const;\n\n //! return the type of the last step taken by the algorithm\n int lastStep() const { return _lastStep;}\n //! return the diameter of the trust region\n double trustRegion() const { return _delta;}\n\n //! convert the type into an integer\n static const char* stepType2Str(int stepType);\n\n protected:\n // parameters\n Property* _maxTrialsAfterFailure;\n Property* _userDeltaInit;\n // damping to enforce positive definite matrix\n Property* _initialLambda;\n Property* _lamdbaFactor;\n\n Eigen::VectorXd _hsd; ///< steepest decent step\n Eigen::VectorXd _hdl; ///< final dogleg step\n Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff\n\n double _currentLambda; ///< the damping factor to force positive definite matrix\n double _delta; ///< trust region\n int _lastStep; ///< type of the step taken by the algorithm\n bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping\n int _lastNumTries;\n };\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"optimization_algorithm_factory.h\"\n\n#include \n#include \n#include \n\nusing namespace std;\n\nnamespace g2o {\n\n AbstractOptimizationAlgorithmCreator::AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p) :\n _property(p)\n {\n }\n\n OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::factoryInstance = 0;\n\n OptimizationAlgorithmFactory::OptimizationAlgorithmFactory()\n {\n }\n\n OptimizationAlgorithmFactory::~OptimizationAlgorithmFactory()\n {\n for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it)\n delete *it;\n }\n\n OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::instance()\n {\n if (factoryInstance == 0) {\n factoryInstance = new OptimizationAlgorithmFactory;\n }\n return factoryInstance;\n }\n\n void OptimizationAlgorithmFactory::registerSolver(AbstractOptimizationAlgorithmCreator* c)\n {\n const string& name = c->property().name;\n CreatorList::iterator foundIt = findSolver(name);\n if (foundIt != _creator.end()) {\n _creator.erase(foundIt);\n cerr << \"SOLVER FACTORY WARNING: Overwriting Solver creator \" << name << endl;\n assert(0);\n }\n _creator.push_back(c);\n }\n\n void OptimizationAlgorithmFactory::unregisterSolver(AbstractOptimizationAlgorithmCreator* c)\n {\n const string& name = c->property().name;\n CreatorList::iterator foundIt = findSolver(name);\n if (foundIt != _creator.end()) {\n delete *foundIt;\n _creator.erase(foundIt);\n }\n }\n\n OptimizationAlgorithm* OptimizationAlgorithmFactory::construct(const std::string& name, OptimizationAlgorithmProperty& solverProperty) const\n {\n CreatorList::const_iterator foundIt = findSolver(name);\n if (foundIt != _creator.end()) {\n solverProperty = (*foundIt)->property();\n return (*foundIt)->construct();\n }\n cerr << \"SOLVER FACTORY WARNING: Unable to create solver \" << name << endl;\n return 0;\n }\n\n void OptimizationAlgorithmFactory::destroy()\n {\n delete factoryInstance;\n factoryInstance = 0;\n }\n\n void OptimizationAlgorithmFactory::listSolvers(std::ostream& os) const\n {\n size_t solverNameColumnLength = 0;\n for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it)\n solverNameColumnLength = std::max(solverNameColumnLength, (*it)->property().name.size());\n solverNameColumnLength += 4;\n\n for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {\n const OptimizationAlgorithmProperty& sp = (*it)->property();\n os << sp.name;\n for (size_t i = sp.name.size(); i < solverNameColumnLength; ++i)\n os << ' ';\n os << sp.desc << endl;\n }\n }\n\n OptimizationAlgorithmFactory::CreatorList::const_iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) const\n {\n for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {\n const OptimizationAlgorithmProperty& sp = (*it)->property();\n if (sp.name == name)\n return it;\n }\n return _creator.end();\n }\n\n OptimizationAlgorithmFactory::CreatorList::iterator OptimizationAlgorithmFactory::findSolver(const std::string& name)\n {\n for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) {\n const OptimizationAlgorithmProperty& sp = (*it)->property();\n if (sp.name == name)\n return it;\n }\n return _creator.end();\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OPTMIZATION_ALGORITHM_PROPERTY_H\n#define G2O_OPTMIZATION_ALGORITHM_PROPERTY_H\n\n#include \"../../config.h\"\n#include \"../stuff/misc.h\"\n#include \"optimization_algorithm_property.h\"\n\n#include \n#include \n#include \n\n\n// define to get some verbose output\n//#define G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY\n\nnamespace g2o {\n\n // forward decl\n class OptimizationAlgorithm;\n class SparseOptimizer;\n\n /**\n * \\brief base for allocating an optimization algorithm\n *\n * Allocating a solver for a given optimizer. The method construct() has to be\n * implemented in your derived class to allocate the desired solver.\n */\n class AbstractOptimizationAlgorithmCreator\n {\n public:\n AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p);\n virtual ~AbstractOptimizationAlgorithmCreator() { }\n //! allocate a solver operating on optimizer, re-implement for your creator\n virtual OptimizationAlgorithm* construct() = 0;\n //! return the properties of the solver\n const OptimizationAlgorithmProperty& property() const { return _property;}\n protected:\n OptimizationAlgorithmProperty _property;\n };\n \n /**\n * \\brief create solvers based on their short name\n *\n * Factory to allocate solvers based on their short name.\n * The Factory is implemented as a sigleton and the single\n * instance can be accessed via the instance() function.\n */\n class OptimizationAlgorithmFactory\n {\n public:\n typedef std::list CreatorList;\n\n //! return the instance\n static OptimizationAlgorithmFactory* instance();\n\n //! free the instance\n static void destroy();\n\n /**\n * register a specific creator for allocating a solver\n */\n void registerSolver(AbstractOptimizationAlgorithmCreator* c);\n\n /**\n * unregister a specific creator for allocating a solver\n */\n void unregisterSolver(AbstractOptimizationAlgorithmCreator* c);\n \n /**\n * construct a solver based on its name, e.g., var, fix3_2_cholmod\n */\n OptimizationAlgorithm* construct(const std::string& tag, OptimizationAlgorithmProperty& solverProperty) const;\n\n //! list the known solvers into a stream\n void listSolvers(std::ostream& os) const;\n\n //! return the underlying list of creators\n const CreatorList& creatorList() const { return _creator;}\n\n protected:\n OptimizationAlgorithmFactory();\n ~OptimizationAlgorithmFactory();\n\n CreatorList _creator;\n\n CreatorList::const_iterator findSolver(const std::string& name) const;\n CreatorList::iterator findSolver(const std::string& name);\n\n private:\n static OptimizationAlgorithmFactory* factoryInstance;\n };\n\n class RegisterOptimizationAlgorithmProxy\n {\n public:\n RegisterOptimizationAlgorithmProxy(AbstractOptimizationAlgorithmCreator* c)\n {\n _creator = c;\n#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY\n std::cout << __FUNCTION__ << \": Registering \" << _creator->property().name << \" of type \" << typeid(*_creator).name() << std::endl;\n#endif\n OptimizationAlgorithmFactory::instance()->registerSolver(c);\n }\n\n ~RegisterOptimizationAlgorithmProxy()\n {\n#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY\n std::cout << __FUNCTION__ << \": Unregistering \" << _creator->property().name << std::endl;\n#endif\n OptimizationAlgorithmFactory::instance()->unregisterSolver(_creator);\n }\n private:\n AbstractOptimizationAlgorithmCreator* _creator;\n };\n\n}\n\n#if defined _MSC_VER && defined G2O_SHARED_LIBS\n# define G2O_OAF_EXPORT __declspec(dllexport)\n# define G2O_OAF_IMPORT __declspec(dllimport)\n#else\n# define G2O_OAF_EXPORT\n# define G2O_OAF_IMPORT\n#endif\n\n#define G2O_REGISTER_OPTIMIZATION_LIBRARY(libraryname) \\\n extern \"C\" void G2O_OAF_EXPORT g2o_optimization_library_##libraryname(void) {}\n\n#define G2O_USE_OPTIMIZATION_LIBRARY(libraryname) \\\n extern \"C\" void G2O_OAF_IMPORT g2o_optimization_library_##libraryname(void); \\\n static g2o::ForceLinker g2o_force_optimization_algorithm_library_##libraryname(g2o_optimization_library_##libraryname);\n\n#define G2O_REGISTER_OPTIMIZATION_ALGORITHM(optimizername, instance) \\\n extern \"C\" void G2O_OAF_EXPORT g2o_optimization_algorithm_##optimizername(void) {} \\\n static g2o::RegisterOptimizationAlgorithmProxy g_optimization_algorithm_proxy_##optimizername(instance);\n\n#define G2O_USE_OPTIMIZATION_ALGORITHM(optimizername) \\\n extern \"C\" void G2O_OAF_IMPORT g2o_optimization_algorithm_##optimizername(void); \\\n static g2o::ForceLinker g2o_force_optimization_algorithm_link_##optimizername(g2o_optimization_algorithm_##optimizername);\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"optimization_algorithm_gauss_newton.h\"\n\n#include \n\n#include \"../stuff/timeutil.h\"\n#include \"../stuff/macros.h\"\n\n#include \"solver.h\"\n#include \"batch_stats.h\"\n#include \"sparse_optimizer.h\"\nusing namespace std;\n\nnamespace g2o {\n\n OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) :\n OptimizationAlgorithmWithHessian(solver)\n {\n }\n\n OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton()\n {\n }\n\n OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online)\n {\n assert(_optimizer && \"_optimizer not set\");\n assert(_solver->optimizer() == _optimizer && \"underlying linear solver operates on different graph\");\n bool ok = true;\n \n //here so that correct component for max-mixtures can be computed before the build structure\n double t=get_monotonic_time();\n _optimizer->computeActiveErrors();\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats) {\n globalStats->timeResiduals = get_monotonic_time()-t;\n }\n \n if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure\n ok = _solver->buildStructure();\n if (! ok) {\n cerr << __PRETTY_FUNCTION__ << \": Failure while building CCS structure\" << endl;\n return OptimizationAlgorithm::Fail;\n }\n }\n\n t=get_monotonic_time();\n _solver->buildSystem();\n if (globalStats) {\n globalStats->timeQuadraticForm = get_monotonic_time()-t;\n t=get_monotonic_time();\n }\n\n ok = _solver->solve();\n if (globalStats) {\n globalStats->timeLinearSolution = get_monotonic_time()-t;\n t=get_monotonic_time();\n }\n\n _optimizer->update(_solver->x());\n if (globalStats) {\n globalStats->timeUpdate = get_monotonic_time()-t;\n }\n if (ok)\n return OK;\n else\n return Fail;\n }\n\n void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const\n {\n os\n << \"\\t schur= \" << _solver->schur();\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H\n#define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H\n\n#include \"optimization_algorithm_with_hessian.h\"\n\nnamespace g2o {\n\n /**\n * \\brief Implementation of the Gauss Newton Algorithm\n */\n class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian\n {\n public:\n /**\n * construct the Gauss Newton algorithm, which use the given Solver for solving the\n * linearized system.\n */\n explicit OptimizationAlgorithmGaussNewton(Solver* solver);\n virtual ~OptimizationAlgorithmGaussNewton();\n\n virtual SolverResult solve(int iteration, bool online = false);\n\n virtual void printVerbose(std::ostream& os) const;\n };\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n// Modified Raul Mur Artal (2014)\n// - Stop criterium (solve function)\n\n#include \"optimization_algorithm_levenberg.h\"\n\n#include \n\n#include \"../stuff/timeutil.h\"\n\n#include \"sparse_optimizer.h\"\n#include \"solver.h\"\n#include \"batch_stats.h\"\nusing namespace std;\n\nnamespace g2o {\n\n OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Solver* solver) :\n OptimizationAlgorithmWithHessian(solver)\n {\n _currentLambda = -1.;\n _tau = 1e-5;\n _goodStepUpperScale = 2./3.;\n _goodStepLowerScale = 1./3.;\n _userLambdaInit = _properties.makeProperty >(\"initialLambda\", 0.);\n _maxTrialsAfterFailure = _properties.makeProperty >(\"maxTrialsAfterFailure\", 10);\n _ni=2.;\n _levenbergIterations = 0;\n _nBad = 0;\n }\n\n OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg()\n {\n }\n\n OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online)\n {\n assert(_optimizer && \"_optimizer not set\");\n assert(_solver->optimizer() == _optimizer && \"underlying linear solver operates on different graph\");\n\n if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure\n bool ok = _solver->buildStructure();\n if (! ok) {\n cerr << __PRETTY_FUNCTION__ << \": Failure while building CCS structure\" << endl;\n return OptimizationAlgorithm::Fail;\n }\n }\n\n double t=get_monotonic_time();\n _optimizer->computeActiveErrors();\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats) {\n globalStats->timeResiduals = get_monotonic_time()-t;\n t=get_monotonic_time();\n }\n\n double currentChi = _optimizer->activeRobustChi2();\n double tempChi=currentChi;\n\n double iniChi = currentChi;\n\n _solver->buildSystem();\n if (globalStats) {\n globalStats->timeQuadraticForm = get_monotonic_time()-t;\n }\n\n // core part of the Levenbarg algorithm\n if (iteration == 0) { \n _currentLambda = computeLambdaInit();\n _ni = 2;\n _nBad = 0;\n }\n\n double rho=0;\n int& qmax = _levenbergIterations;\n qmax = 0;\n do {\n _optimizer->push();\n if (globalStats) {\n globalStats->levenbergIterations++;\n t=get_monotonic_time();\n }\n // update the diagonal of the system matrix\n _solver->setLambda(_currentLambda, true);\n bool ok2 = _solver->solve();\n if (globalStats) {\n globalStats->timeLinearSolution+=get_monotonic_time()-t;\n t=get_monotonic_time();\n }\n _optimizer->update(_solver->x());\n if (globalStats) {\n globalStats->timeUpdate = get_monotonic_time()-t;\n }\n\n // restore the diagonal\n _solver->restoreDiagonal();\n\n _optimizer->computeActiveErrors();\n tempChi = _optimizer->activeRobustChi2();\n\n if (! ok2)\n tempChi=std::numeric_limits::max();\n\n rho = (currentChi-tempChi);\n double scale = computeScale();\n scale += 1e-3; // make sure it's non-zero :)\n rho /= scale;\n\n if (rho>0 && g2o_isfinite(tempChi)){ // last step was good\n double alpha = 1.-pow((2*rho-1),3);\n // crop lambda between minimum and maximum factors\n alpha = (std::min)(alpha, _goodStepUpperScale);\n double scaleFactor = (std::max)(_goodStepLowerScale, alpha);\n _currentLambda *= scaleFactor;\n _ni = 2;\n currentChi=tempChi;\n _optimizer->discardTop();\n } else {\n _currentLambda*=_ni;\n _ni*=2;\n _optimizer->pop(); // restore the last state before trying to optimize\n }\n qmax++;\n } while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate());\n\n if (qmax == _maxTrialsAfterFailure->value() || rho==0)\n return Terminate;\n\n //Stop criterium (Raul)\n if((iniChi-currentChi)*1e3=3)\n return Terminate;\n\n return OK;\n }\n\n double OptimizationAlgorithmLevenberg::computeLambdaInit() const\n {\n if (_userLambdaInit->value() > 0)\n return _userLambdaInit->value();\n double maxDiagonal=0.;\n for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) {\n OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k];\n assert(v);\n int dim = v->dimension();\n for (int j = 0; j < dim; ++j){\n maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal);\n }\n }\n return _tau*maxDiagonal;\n }\n\n double OptimizationAlgorithmLevenberg::computeScale() const\n {\n double scale = 0.;\n for (size_t j=0; j < _solver->vectorSize(); j++){\n scale += _solver->x()[j] * (_currentLambda * _solver->x()[j] + _solver->b()[j]);\n }\n return scale;\n }\n\n void OptimizationAlgorithmLevenberg::setMaxTrialsAfterFailure(int max_trials)\n {\n _maxTrialsAfterFailure->setValue(max_trials);\n }\n\n void OptimizationAlgorithmLevenberg::setUserLambdaInit(double lambda)\n {\n _userLambdaInit->setValue(lambda);\n }\n\n void OptimizationAlgorithmLevenberg::printVerbose(std::ostream& os) const\n {\n os\n << \"\\t schur= \" << _solver->schur()\n << \"\\t lambda= \" << FIXED(_currentLambda)\n << \"\\t levenbergIter= \" << _levenbergIterations;\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SOLVER_LEVENBERG_H\n#define G2O_SOLVER_LEVENBERG_H\n\n#include \"optimization_algorithm_with_hessian.h\"\n\nnamespace g2o {\n\n /**\n * \\brief Implementation of the Levenberg Algorithm\n */\n class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian\n {\n public:\n /**\n * construct the Levenberg algorithm, which will use the given Solver for solving the\n * linearized system.\n */\n explicit OptimizationAlgorithmLevenberg(Solver* solver);\n virtual ~OptimizationAlgorithmLevenberg();\n\n virtual SolverResult solve(int iteration, bool online = false);\n\n virtual void printVerbose(std::ostream& os) const;\n\n //! return the currently used damping factor\n double currentLambda() const { return _currentLambda;}\n\n //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt\n void setMaxTrialsAfterFailure(int max_trials);\n\n //! get the number of inner iterations for Levenberg-Marquardt\n int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();}\n\n //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda\n double userLambdaInit() {return _userLambdaInit->value();}\n //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value\n void setUserLambdaInit(double lambda);\n\n //! return the number of levenberg iterations performed in the last round\n int levenbergIteration() { return _levenbergIterations;}\n\n protected:\n // Levenberg parameters\n Property* _maxTrialsAfterFailure;\n Property* _userLambdaInit;\n double _currentLambda;\n double _tau;\n double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step\n double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step\n double _ni;\n int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step\n //RAUL\n int _nBad;\n\n /**\n * helper for Levenberg, this function computes the initial damping factor, if the user did not\n * specify an own value, see setUserLambdaInit()\n */\n double computeLambdaInit() const;\n double computeScale() const;\n\n };\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_property.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H\n#define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H\n\n#include \n\nnamespace g2o {\n\n/**\n * \\brief describe the properties of a solver\n */\nstruct OptimizationAlgorithmProperty\n{\n std::string name; ///< name of the solver, e.g., var\n std::string desc; ///< short description of the solver\n std::string type; ///< type of solver, e.g., \"CSparse Cholesky\", \"PCG\"\n bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks\n int poseDim; ///< dimension of the pose vertices (-1 if variable)\n int landmarkDim; ///< dimension of the landmar vertices (-1 if variable)\n OptimizationAlgorithmProperty() :\n name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1)\n {\n }\n OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) :\n name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_)\n {\n }\n};\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"optimization_algorithm_with_hessian.h\"\n\n#include \"solver.h\"\n#include \"optimizable_graph.h\"\n#include \"sparse_optimizer.h\"\n\n#include \nusing namespace std;\n\nnamespace g2o {\n\n OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) :\n OptimizationAlgorithm(),\n _solver(solver)\n {\n _writeDebug = _properties.makeProperty >(\"writeDebug\", true);\n }\n\n OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian()\n {\n delete _solver;\n }\n\n bool OptimizationAlgorithmWithHessian::init(bool online)\n {\n assert(_optimizer && \"_optimizer not set\");\n assert(_solver && \"Solver not set\");\n _solver->setWriteDebug(_writeDebug->value());\n bool useSchur=false;\n for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) {\n OptimizableGraph::Vertex* v= *it;\n if (v->marginalized()){\n useSchur=true;\n break;\n }\n }\n if (useSchur){\n if (_solver->supportsSchur())\n _solver->setSchur(true);\n } else {\n if (_solver->supportsSchur())\n _solver->setSchur(false);\n }\n\n bool initState = _solver->init(_optimizer, online);\n return initState;\n }\n\n bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices)\n {\n return _solver ? _solver->computeMarginals(spinv, blockIndices) : false;\n }\n\n bool OptimizationAlgorithmWithHessian::buildLinearStructure()\n {\n return _solver ? _solver->buildStructure() : false;\n }\n\n void OptimizationAlgorithmWithHessian::updateLinearSystem()\n {\n if (_solver)\n _solver->buildSystem();\n }\n\n bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges)\n {\n return _solver ? _solver->updateStructure(vset, edges) : false;\n }\n\n void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug)\n {\n _writeDebug->setValue(writeDebug);\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H\n#define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H\n\n#include \"optimization_algorithm.h\"\n\nnamespace g2o {\n\n class Solver;\n\n /**\n * \\brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg\n */\n class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm\n {\n public:\n explicit OptimizationAlgorithmWithHessian(Solver* solver);\n virtual ~OptimizationAlgorithmWithHessian();\n\n virtual bool init(bool online = false);\n\n virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices);\n\n virtual bool buildLinearStructure();\n\n virtual void updateLinearSystem();\n\n virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges);\n\n //! return the underlying solver used to solve the linear system\n Solver* solver() { return _solver;}\n\n /**\n * write debug output of the Hessian if system is not positive definite\n */\n virtual void setWriteDebug(bool writeDebug);\n virtual bool writeDebug() const { return _writeDebug->value();}\n\n protected:\n Solver* _solver;\n Property* _writeDebug;\n\n };\n\n}// end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/parameter.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"parameter.h\"\n\nnamespace g2o {\n\n Parameter::Parameter() : _id(-1)\n {\n }\n\n void Parameter::setId(int id_)\n {\n _id = id_;\n }\n \n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/parameter.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_GRAPH_PARAMETER_HH_\n#define G2O_GRAPH_PARAMETER_HH_\n\n#include \n\n#include \"hyper_graph.h\"\n\nnamespace g2o {\n\n class Parameter : public HyperGraph::HyperGraphElement\n {\n public:\n Parameter();\n virtual ~Parameter() {};\n //! read the data from a stream\n virtual bool read(std::istream& is) = 0;\n //! write the data to a stream\n virtual bool write(std::ostream& os) const = 0;\n int id() const {return _id;}\n void setId(int id_);\n virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;}\n protected:\n int _id;\n };\n\n typedef std::vector ParameterVector;\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/parameter_container.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"parameter_container.h\"\n\n#include \n\n#include \"factory.h\"\n#include \"parameter.h\"\n\n#include \"../stuff/macros.h\"\n#include \"../stuff/color_macros.h\"\n#include \"../stuff/string_tools.h\"\n\nnamespace g2o {\n\n using namespace std;\n\n ParameterContainer::ParameterContainer(bool isMainStorage_) :\n _isMainStorage(isMainStorage_)\n {\n }\n\n void ParameterContainer::clear() {\n if (!_isMainStorage)\n return;\n for (iterator it = begin(); it!=end(); it++){\n delete it->second;\n }\n BaseClass::clear();\n }\n\n ParameterContainer::~ParameterContainer(){\n clear();\n }\n\n bool ParameterContainer::addParameter(Parameter* p){\n if (p->id()<0)\n return false;\n iterator it=find(p->id());\n if (it!=end())\n return false;\n insert(make_pair(p->id(), p));\n return true;\n }\n\n Parameter* ParameterContainer::getParameter(int id) {\n iterator it=find(id);\n if (it==end())\n return 0;\n return it->second;\n }\n\n Parameter* ParameterContainer::detachParameter(int id){\n iterator it=find(id);\n if (it==end())\n return 0;\n Parameter* p=it->second;\n erase(it);\n return p;\n }\n \n bool ParameterContainer::write(std::ostream& os) const{\n Factory* factory = Factory::instance();\n for (const_iterator it=begin(); it!=end(); it++){\n os << factory->tag(it->second) << \" \";\n os << it->second->id() << \" \";\n it->second->write(os);\n os << endl;\n }\n return true;\n }\n\n bool ParameterContainer::read(std::istream& is, const std::map* _renamedTypesLookup){\n stringstream currentLine;\n string token;\n\n Factory* factory = Factory::instance();\n HyperGraph::GraphElemBitset elemBitset;\n elemBitset[HyperGraph::HGET_PARAMETER] = 1;\n \n while (1) {\n int bytesRead = readLine(is, currentLine);\n if (bytesRead == -1)\n break;\n currentLine >> token;\n if (bytesRead == 0 || token.size() == 0 || token[0] == '#')\n continue;\n if (_renamedTypesLookup && _renamedTypesLookup->size()>0){\n\tmap::const_iterator foundIt = _renamedTypesLookup->find(token);\n\tif (foundIt != _renamedTypesLookup->end()) {\n\t token = foundIt->second;\n\t}\n }\n\n HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);\n if (! element) // not a parameter or otherwise unknown tag\n continue;\n assert(element->elementType() == HyperGraph::HGET_PARAMETER && \"Should be a param\");\n\n Parameter* p = static_cast(element);\n int pid;\n currentLine >> pid;\n p->setId(pid);\n bool r = p->read(currentLine);\n if (! r) {\n cerr << __PRETTY_FUNCTION__ << \": Error reading data \" << token << \" for parameter \" << pid << endl;\n delete p;\n } else {\n if (! addParameter(p) ){\n cerr << __PRETTY_FUNCTION__ << \": Parameter of type:\" << token << \" id:\" << pid << \" already defined\" << endl;\n }\n }\n } // while read line\n \n return true;\n }\n \n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/parameter_container.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_\n#define G2O_GRAPH_PARAMETER_CONTAINER_HH_\n\n#include \n#include \n#include \n\nnamespace g2o {\n \n class Parameter;\n\n /**\n * \\brief map id to parameters\n */\n class ParameterContainer : protected std::map \n {\n public:\n typedef std::map BaseClass;\n\n /**\n * create a container for the parameters.\n * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor\n */\n ParameterContainer(bool isMainStorage_=true);\n virtual ~ParameterContainer();\n //! add parameter to the container\n bool addParameter(Parameter* p);\n //! return a parameter based on its ID\n Parameter* getParameter(int id);\n //! remove a parameter from the container, i.e., the user now owns the pointer\n Parameter* detachParameter(int id);\n //! read parameters from a stream\n virtual bool read(std::istream& is, const std::map* renamedMap =0);\n //! write the data to a stream\n virtual bool write(std::ostream& os) const;\n bool isMainStorage() const {return _isMainStorage;}\n void clear();\n\n // stuff of the base class that should re-appear\n using BaseClass::size;\n\n protected:\n bool _isMainStorage;\n };\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/robust_kernel.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"robust_kernel.h\"\n\nnamespace g2o {\n\nRobustKernel::RobustKernel() :\n _delta(1.)\n{\n}\n\nRobustKernel::RobustKernel(double delta) :\n _delta(delta)\n{\n}\n\nvoid RobustKernel::setDelta(double delta)\n{\n _delta = delta;\n}\n\n} // end namespace g2o\n" }, { "path": "Thirdparty/g2o/g2o/core/robust_kernel.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_ROBUST_KERNEL_H\n#define G2O_ROBUST_KERNEL_H\n\n#ifdef _MSC_VER\n#include \n#else\n#include \n#endif\n#include \n\n\nnamespace g2o {\n\n /**\n * \\brief base for all robust cost functions\n *\n * Note in all the implementations for the other cost functions the e in the\n * funtions corresponds to the sqaured errors, i.e., the robustification\n * functions gets passed the squared error.\n *\n * e.g. the robustified least squares function is\n *\n * chi^2 = sum_{e} rho( e^T Omega e )\n */\n class RobustKernel\n {\n public:\n RobustKernel();\n explicit RobustKernel(double delta);\n virtual ~RobustKernel() {}\n /**\n * compute the scaling factor for a error:\n * The error is e^T Omega e\n * The output rho is\n * rho[0]: The actual scaled error value\n * rho[1]: First derivative of the scaling function\n * rho[2]: Second derivative of the scaling function\n */\n virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0;\n\n /**\n * set the window size of the error. A squared error above delta^2 is considered\n * as outlier in the data.\n */\n virtual void setDelta(double delta);\n double delta() const { return _delta;}\n\n protected:\n double _delta;\n };\n typedef std::tr1::shared_ptr RobustKernelPtr;\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"robust_kernel_factory.h\"\n#include \"robust_kernel.h\"\n\n#include \n\nusing namespace std;\n\nnamespace g2o {\n\nRobustKernelFactory* RobustKernelFactory::factoryInstance = 0;\n\nRobustKernelFactory::RobustKernelFactory()\n{\n}\n\nRobustKernelFactory::~RobustKernelFactory()\n{\n for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {\n delete it->second;\n }\n _creator.clear();\n}\n\nRobustKernelFactory* RobustKernelFactory::instance()\n{\n if (factoryInstance == 0) {\n factoryInstance = new RobustKernelFactory;\n }\n\n return factoryInstance;\n}\n\nvoid RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c)\n{\n CreatorMap::const_iterator foundIt = _creator.find(tag);\n if (foundIt != _creator.end()) {\n cerr << \"RobustKernelFactory WARNING: Overwriting robust kernel tag \" << tag << endl;\n assert(0);\n }\n\n _creator[tag] = c;\n}\n\nvoid RobustKernelFactory::unregisterType(const std::string& tag)\n{\n CreatorMap::iterator tagPosition = _creator.find(tag);\n if (tagPosition != _creator.end()) {\n AbstractRobustKernelCreator* c = tagPosition->second;\n delete c;\n _creator.erase(tagPosition);\n }\n}\n\nRobustKernel* RobustKernelFactory::construct(const std::string& tag) const\n{\n CreatorMap::const_iterator foundIt = _creator.find(tag);\n if (foundIt != _creator.end()) {\n return foundIt->second->construct();\n }\n return 0;\n}\n\nAbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const\n{\n CreatorMap::const_iterator foundIt = _creator.find(tag);\n if (foundIt != _creator.end()) {\n return foundIt->second;\n }\n return 0;\n}\n\nvoid RobustKernelFactory::fillKnownKernels(std::vector& types) const\n{\n types.clear();\n for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)\n types.push_back(it->first);\n}\n\nvoid RobustKernelFactory::destroy()\n{\n delete factoryInstance;\n factoryInstance = 0;\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/robust_kernel_factory.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_ROBUST_KERNEL_FACTORY_H\n#define G2O_ROBUST_KERNEL_FACTORY_H\n\n#include \"../stuff/misc.h\"\n\n#include \n#include \n#include \n#include \n\nnamespace g2o {\n\n class RobustKernel;\n\n /**\n * \\brief Abstract interface for allocating a robust kernel\n */\n class AbstractRobustKernelCreator\n {\n public:\n /**\n * create a hyper graph element. Has to implemented in derived class.\n */\n virtual RobustKernel* construct() = 0;\n virtual ~AbstractRobustKernelCreator() { }\n };\n\n /**\n * \\brief templatized creator class which creates graph elements\n */\n template \n class RobustKernelCreator : public AbstractRobustKernelCreator\n {\n public:\n RobustKernel* construct() { return new T;}\n };\n\n /**\n * \\brief create robust kernels based on their human readable name\n */\n class RobustKernelFactory\n {\n public:\n\n //! return the instance\n static RobustKernelFactory* instance();\n\n //! free the instance\n static void destroy();\n\n /**\n * register a tag for a specific creator\n */\n void registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c);\n\n /**\n * unregister a tag for a specific creator\n */\n void unregisterType(const std::string& tag);\n\n /**\n * construct a robust kernel based on its tag\n */\n RobustKernel* construct(const std::string& tag) const;\n\n /**\n * return the creator for a specific tag\n */\n AbstractRobustKernelCreator* creator(const std::string& tag) const;\n\n /**\n * get a list of all known robust kernels\n */\n void fillKnownKernels(std::vector& types) const;\n\n protected:\n\n typedef std::map CreatorMap;\n RobustKernelFactory();\n ~RobustKernelFactory();\n\n CreatorMap _creator; ///< look-up map for the existing creators\n\n private:\n static RobustKernelFactory* factoryInstance;\n };\n\n template\n class RegisterRobustKernelProxy\n {\n public:\n RegisterRobustKernelProxy(const std::string& name) : _name(name)\n {\n RobustKernelFactory::instance()->registerRobustKernel(_name, new RobustKernelCreator());\n }\n\n ~RegisterRobustKernelProxy()\n {\n RobustKernelFactory::instance()->unregisterType(_name);\n }\n\n private:\n std::string _name;\n };\n\n#if defined _MSC_VER && defined G2O_SHARED_LIBS\n# define G2O_ROBUST_KERNEL_FACTORY_EXPORT __declspec(dllexport)\n# define G2O_ROBUST_KERNEL_FACTORY_IMPORT __declspec(dllimport)\n#else\n# define G2O_ROBUST_KERNEL_FACTORY_EXPORT\n# define G2O_ROBUST_KERNEL_FACTORY_IMPORT\n#endif\n\n // These macros are used to automate registering of robust kernels and forcing linkage\n#define G2O_REGISTER_ROBUST_KERNEL(name, classname) \\\n extern \"C\" void G2O_ROBUST_KERNEL_FACTORY_EXPORT g2o_robust_kernel_##classname(void) {} \\\n static g2o::RegisterRobustKernelProxy g_robust_kernel_proxy_##classname(#name);\n\n#define G2O_USE_ROBUST_KERNEL(classname) \\\n extern \"C\" void G2O_ROBUST_KERNEL_FACTORY_IMPORT g2o_robust_kernel_##classname(void); \\\n static g2o::TypeFunctionProxy proxy_##classname(g2o_robust_kernel_##classname);\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"robust_kernel_impl.h\"\n#include \"robust_kernel_factory.h\"\n\n#include \n\nnamespace g2o {\n\nRobustKernelScaleDelta::RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta) :\n RobustKernel(delta),\n _kernel(kernel)\n{\n}\n\nRobustKernelScaleDelta::RobustKernelScaleDelta(double delta) :\n RobustKernel(delta)\n{\n}\n\nvoid RobustKernelScaleDelta::setKernel(const RobustKernelPtr& ptr)\n{\n _kernel = ptr;\n}\n\nvoid RobustKernelScaleDelta::robustify(double error, Eigen::Vector3d& rho) const\n{\n if (_kernel.get()) {\n double dsqr = _delta * _delta;\n double dsqrReci = 1. / dsqr;\n _kernel->robustify(dsqrReci * error, rho);\n rho[0] *= dsqr;\n rho[2] *= dsqrReci;\n } else { // no robustification\n rho[0] = error;\n rho[1] = 1.;\n rho[2] = 0.;\n }\n}\n\nvoid RobustKernelHuber::setDelta(double delta)\n{\n\tdsqr = delta*delta;\n\t_delta = delta;\n}\n\n\nvoid RobustKernelHuber::setDeltaSqr(const double &delta, const double &deltaSqr)\n{\n\tdsqr = deltaSqr;\n\t_delta = delta;\n}\n\nvoid RobustKernelHuber::robustify(double e, Eigen::Vector3d& rho) const\n{\n //dsqr = _delta * _delta;\n if (e <= dsqr) { // inlier\n rho[0] = e;\n rho[1] = 1.;\n rho[2] = 0.;\n } else { // outlier\n double sqrte = sqrt(e); // absolut value of the error\n rho[0] = 2*sqrte*_delta - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2\n rho[1] = _delta / sqrte; // rho'(e) = delta / sqrt(e)\n rho[2] = - 0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e\n }\n}\n\nvoid RobustKernelTukey::setDeltaSqr(const double &deltaSqr, const double &inv)\n{\n _deltaSqr = deltaSqr;\n _invDeltaSqr = inv;\n \n}\n\nvoid RobustKernelTukey::robustify(double e, Eigen::Vector3d& rho) const\n{\n if (e <= _deltaSqr) { // inlier\n double factor = e*_invDeltaSqr;\n double d = 1-factor;\n double dd = d*d;\n rho[0] = _deltaSqr*(1-dd*d);\n rho[1] = 3*dd;\n rho[2] = -6*_invDeltaSqr*d;\n } else { // outlier\n rho[0] = _deltaSqr; // rho(e) = delta^2\n rho[1] = 0.;\n rho[2] = 0.; \n }\n}\n\nvoid RobustKernelPseudoHuber::robustify(double e2, Eigen::Vector3d& rho) const\n{\n double dsqr = _delta * _delta;\n double dsqrReci = 1. / dsqr;\n double aux1 = dsqrReci * e2 + 1.0;\n double aux2 = sqrt(aux1);\n rho[0] = 2 * dsqr * (aux2 - 1);\n rho[1] = 1. / aux2;\n rho[2] = -0.5 * dsqrReci * rho[1] / aux1;\n}\n\nvoid RobustKernelCauchy::robustify(double e2, Eigen::Vector3d& rho) const\n{\n double dsqr = _delta * _delta;\n double dsqrReci = 1. / dsqr;\n double aux = dsqrReci * e2 + 1.0;\n rho[0] = dsqr * log(aux);\n rho[1] = 1. / aux;\n rho[2] = -dsqrReci * std::pow(rho[1], 2); \n}\n\nvoid RobustKernelSaturated::robustify(double e2, Eigen::Vector3d& rho) const\n{\n double dsqr = _delta * _delta;\n if (e2 <= dsqr) { // inlier\n rho[0] = e2;\n rho[1] = 1.;\n rho[2] = 0.;\n } else { // outlier\n rho[0] = dsqr;\n rho[1] = 0.;\n rho[2] = 0.;\n }\n}\n\n//delta is used as $phi$\nvoid RobustKernelDCS::robustify(double e2, Eigen::Vector3d& rho) const\n{\n const double& phi = _delta;\n double scale = (2.0*phi)/(phi+e2);\n if(scale>=1.0)\n scale = 1.0;\n\n rho[0] = scale*e2*scale;\n rho[1] = (scale*scale);\n rho[2] = 0; \n}\n\n\n// register the kernel to their factory\nG2O_REGISTER_ROBUST_KERNEL(Huber, RobustKernelHuber)\nG2O_REGISTER_ROBUST_KERNEL(Tukey, RobustKernelTukey)\nG2O_REGISTER_ROBUST_KERNEL(PseudoHuber, RobustKernelPseudoHuber)\nG2O_REGISTER_ROBUST_KERNEL(Cauchy, RobustKernelCauchy)\nG2O_REGISTER_ROBUST_KERNEL(Saturated, RobustKernelSaturated)\nG2O_REGISTER_ROBUST_KERNEL(DCS, RobustKernelDCS)\n\n} // end namespace g2o\n" }, { "path": "Thirdparty/g2o/g2o/core/robust_kernel_impl.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_ROBUST_KERNEL_IMPL_H\n#define G2O_ROBUST_KERNEL_IMPL_H\n\n#include \"robust_kernel.h\"\n\nnamespace g2o {\n\n /**\n * \\brief scale a robust kernel to another delta (window size)\n *\n * Scales a robust kernel to another window size. Useful, in case if\n * one implements a kernel which only is designed for a fixed window\n * size.\n */\n class RobustKernelScaleDelta : public RobustKernel\n {\n public:\n /**\n * construct the scaled kernel ontop of another kernel which might be shared accross\n * several scaled kernels\n */\n explicit RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta = 1.);\n explicit RobustKernelScaleDelta(double delta = 1.);\n\n //! return the underlying kernel\n const RobustKernelPtr kernel() const { return _kernel;}\n //! use another kernel for the underlying operation\n void setKernel(const RobustKernelPtr& ptr);\n\n void robustify(double error, Eigen::Vector3d& rho) const;\n\n protected:\n RobustKernelPtr _kernel;\n };\n\n /**\n * \\brief Huber Cost Function\n *\n * Loss function as described by Huber\n * See http://en.wikipedia.org/wiki/Huber_loss_function\n *\n * If e^(1/2) < d\n * rho(e) = e\n *\n * else\n *\n * 1/2 2\n * rho(e) = 2 d e - d\n */\n class RobustKernelHuber : public RobustKernel\n {\n public:\n virtual void setDelta(double delta);\n virtual void setDeltaSqr(const double &delta, const double &deltaSqr);\n virtual void robustify(double e2, Eigen::Vector3d& rho) const;\n\n private:\n float dsqr;\n };\n\n /**\n * \\brief Tukey Cost Function\n *\n *\n * If e^(1/2) < d\n * rho(e) = delta2(1-(1-e/delta2)^3)\n *\n * else\n * \n * rho(e) = delta2\n */\n class RobustKernelTukey : public RobustKernel\n {\n public:\n\n virtual void setDeltaSqr(const double &deltaSqr, const double &inv);\n virtual void robustify(double e2, Eigen::Vector3d& rho) const;\n private:\n float _deltaSqr;\n float _invDeltaSqr;\n };\n\n\n /**\n * \\brief Pseudo Huber Cost Function\n *\n * The smooth pseudo huber cost function:\n * See http://en.wikipedia.org/wiki/Huber_loss_function\n *\n * 2 e\n * 2 d (sqrt(-- + 1) - 1)\n * 2\n * d\n */\n class RobustKernelPseudoHuber : public RobustKernel\n {\n public:\n virtual void robustify(double e2, Eigen::Vector3d& rho) const;\n };\n\n /**\n * \\brief Cauchy cost function\n *\n * 2 e\n * d log(-- + 1)\n * 2\n * d\n */\n class RobustKernelCauchy : public RobustKernel\n {\n public:\n virtual void robustify(double e2, Eigen::Vector3d& rho) const;\n };\n\n /**\n * \\brief Saturated cost function.\n *\n * The error is at most delta^2\n */\n class RobustKernelSaturated : public RobustKernel\n {\n public:\n virtual void robustify(double e2, Eigen::Vector3d& rho) const;\n };\n\n /**\n * \\brief Dynamic covariance scaling - DCS\n * \n * See paper Robust Map Optimization from Agarwal et al. ICRA 2013\n *\n * delta is used as $phi$\n */\n class RobustKernelDCS : public RobustKernel\n {\n public:\n virtual void robustify(double e2, Eigen::Vector3d& rho) const;\n };\n\n} // end namespace g2o\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/solver.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"solver.h\"\n\n#include \n#include \n\nnamespace g2o {\n\nSolver::Solver() :\n _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0),\n _isLevenberg(false), _additionalVectorSpace(0)\n{\n}\n\nSolver::~Solver()\n{\n delete[] _x;\n delete[] _b;\n}\n\nvoid Solver::resizeVector(size_t sx)\n{\n size_t oldSize = _xSize;\n _xSize = sx;\n sx += _additionalVectorSpace; // allocate some additional space if requested\n if (_maxXSize < sx) {\n _maxXSize = 2*sx;\n delete[] _x;\n _x = new double[_maxXSize];\n#ifndef NDEBUG\n memset(_x, 0, _maxXSize * sizeof(double));\n#endif\n if (_b) { // backup the former b, might still be needed for online processing\n memcpy(_x, _b, oldSize * sizeof(double));\n delete[] _b;\n _b = new double[_maxXSize];\n std::swap(_b, _x);\n } else {\n _b = new double[_maxXSize];\n#ifndef NDEBUG\n memset(_b, 0, _maxXSize * sizeof(double));\n#endif\n }\n }\n}\n\nvoid Solver::setOptimizer(SparseOptimizer* optimizer)\n{\n _optimizer = optimizer;\n}\n\nvoid Solver::setLevenberg(bool levenberg)\n{\n _isLevenberg = levenberg;\n}\n\nvoid Solver::setAdditionalVectorSpace(size_t s)\n{\n _additionalVectorSpace = s;\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/solver.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SOLVER_H\n#define G2O_SOLVER_H\n\n#include \"hyper_graph.h\"\n#include \"batch_stats.h\"\n#include \"sparse_block_matrix.h\"\n#include \n\nnamespace g2o {\n\n\n class SparseOptimizer;\n\n /**\n * \\brief Generic interface for a sparse solver operating on a graph which solves one iteration of the linearized objective function\n */\n class Solver\n {\n public:\n Solver();\n virtual ~Solver();\n\n public:\n /**\n * initialize the solver, called once before the first iteration\n */\n virtual bool init(SparseOptimizer* optimizer, bool online = false) = 0;\n\n /**\n * build the structure of the system\n */\n virtual bool buildStructure(bool zeroBlocks = false) = 0;\n /**\n * update the structures for online processing\n */\n virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges) = 0;\n /**\n * build the current system\n */\n virtual bool buildSystem() = 0;\n\n /**\n * solve Ax = b\n */\n virtual bool solve() = 0;\n\n /**\n * computes the block diagonal elements of the pattern specified in the input\n * and stores them in given SparseBlockMatrix\n */\n virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices) = 0;\n\n /**\n * update the system while performing Levenberg, i.e., modifying the diagonal\n * components of A by doing += lambda along the main diagonal of the Matrix.\n * Note that this function may be called with a positive and a negative lambda.\n * The latter is used to undo a former modification.\n * If backup is true, then the solver should store a backup of the diagonal, which\n * can be restored by restoreDiagonal()\n */\n virtual bool setLambda(double lambda, bool backup = false) = 0;\n\n /**\n * restore a previosly made backup of the diagonal\n */\n virtual void restoreDiagonal() = 0;\n\n //! return x, the solution vector\n double* x() { return _x;}\n const double* x() const { return _x;}\n //! return b, the right hand side of the system\n double* b() { return _b;}\n const double* b() const { return _b;}\n\n //! return the size of the solution vector (x) and b\n size_t vectorSize() const { return _xSize;}\n\n //! the optimizer (graph) on which the solver works\n SparseOptimizer* optimizer() const { return _optimizer;}\n void setOptimizer(SparseOptimizer* optimizer);\n\n //! the system is Levenberg-Marquardt\n bool levenberg() const { return _isLevenberg;}\n void setLevenberg(bool levenberg);\n\n /**\n * does this solver support the Schur complement for solving a system consisting of poses and\n * landmarks. Re-implemement in a derived solver, if your solver supports it.\n */\n virtual bool supportsSchur() {return false;}\n\n //! should the solver perform the schur complement or not\n virtual bool schur()=0;\n virtual void setSchur(bool s)=0;\n\n size_t additionalVectorSpace() const { return _additionalVectorSpace;}\n void setAdditionalVectorSpace(size_t s);\n\n /**\n * write debug output of the Hessian if system is not positive definite\n */\n virtual void setWriteDebug(bool) = 0;\n virtual bool writeDebug() const = 0;\n\n //! write the hessian to disk using the specified file name\n virtual bool saveHessian(const std::string& /*fileName*/) const = 0;\n\n protected:\n SparseOptimizer* _optimizer;\n double* _x;\n double* _b;\n size_t _xSize, _maxXSize;\n bool _isLevenberg; ///< the system we gonna solve is a Levenberg-Marquardt system\n size_t _additionalVectorSpace;\n\n void resizeVector(size_t sx);\n\n private:\n // Disable the copy constructor and assignment operator\n Solver(const Solver&) { }\n Solver& operator= (const Solver&) { return *this; }\n };\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/sparse_block_matrix.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SPARSE_BLOCK_MATRIX_\n#define G2O_SPARSE_BLOCK_MATRIX_\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"sparse_block_matrix_ccs.h\"\n#include \"matrix_structure.h\"\n#include \"matrix_operations.h\"\n#include \"../../config.h\"\n\nnamespace g2o {\n using namespace Eigen;\n/**\n * \\brief Sparse matrix which uses blocks\n *\n * Template class that specifies a sparse block matrix. A block\n * matrix is a sparse matrix made of dense blocks. These blocks\n * cannot have a random pattern, but follow a (variable) grid\n * structure. This structure is specified by a partition of the rows\n * and the columns of the matrix. The blocks are represented by the\n * Eigen::Matrix structure, thus they can be statically or dynamically\n * allocated. For efficiency reasons it is convenient to allocate them\n * statically, when possible. A static block matrix has all blocks of\n * the same size, and the size of the block is specified by the\n * template argument. If this is not the case, and you have different\n * block sizes than you have to use a dynamic-block matrix (default\n * template argument). \n */\ntemplate \nclass SparseBlockMatrix {\n\n public:\n //! this is the type of the elementary block, it is an Eigen::Matrix.\n typedef MatrixType SparseMatrixBlock;\n\n //! columns of the matrix\n inline int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}\n //! rows of the matrix\n inline int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}\n\n typedef std::map IntBlockMap;\n\n /**\n * constructs a sparse block matrix having a specific layout\n * @param rbi: array of int containing the row layout of the blocks. \n * the component i of the array should contain the index of the first row of the block i+1.\n * @param rbi: array of int containing the column layout of the blocks. \n * the component i of the array should contain the index of the first col of the block i+1.\n * @param rb: number of row blocks\n * @param cb: number of col blocks\n * @param hasStorage: set it to true if the matrix \"owns\" the blocks, thus it deletes it on destruction.\n * if false the matrix is only a \"view\" over an existing structure.\n */\n SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true);\n\n SparseBlockMatrix();\n\n ~SparseBlockMatrix();\n\n \n //! this zeroes all the blocks. If dealloc=true the blocks are removed from memory\n void clear(bool dealloc=false) ;\n\n //! returns the block at location r,c. if alloc=true he block is created if it does not exist\n SparseMatrixBlock* block(int r, int c, bool alloc=false);\n //! returns the block at location r,c\n const SparseMatrixBlock* block(int r, int c) const;\n\n //! how many rows does the block at block-row r has?\n inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }\n\n //! how many cols does the block at block-col c has?\n inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }\n\n //! where does the row at block-row r starts?\n inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }\n\n //! where does the col at block-col r starts?\n inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }\n\n //! number of non-zero elements\n size_t nonZeros() const; \n //! number of allocated blocks\n size_t nonZeroBlocks() const; \n\n //! deep copy of a sparse-block-matrix;\n SparseBlockMatrix* clone() const ;\n\n /**\n * returns a view or a copy of the block matrix\n * @param rmin: starting block row\n * @param rmax: ending block row\n * @param cmin: starting block col\n * @param cmax: ending block col\n * @param alloc: if true it makes a deep copy, if false it creates a view.\n */\n SparseBlockMatrix* slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const;\n\n //! transposes a block matrix, The transposed type should match the argument false on failure\n template \n bool transpose(SparseBlockMatrix*& dest) const;\n\n //! adds the current matrix to the destination\n bool add(SparseBlockMatrix*& dest) const ;\n\n //! dest = (*this) * M\n template \n bool multiply(SparseBlockMatrix *& dest, const SparseBlockMatrix* M) const;\n\n //! dest = (*this) * src\n void multiply(double*& dest, const double* src) const;\n\n /**\n * compute dest = (*this) * src\n * However, assuming that this is a symmetric matrix where only the upper triangle is stored\n */\n void multiplySymmetricUpperTriangle(double*& dest, const double* src) const;\n\n //! dest = M * (*this)\n void rightMultiply(double*& dest, const double* src) const;\n\n //! *this *= a\n void scale( double a);\n\n /**\n * writes in dest a block permutaton specified by pinv.\n * @param pinv: array such that new_block[i] = old_block[pinv[i]]\n */\n bool symmPermutation(SparseBlockMatrix*& dest, const int* pinv, bool onlyUpper=false) const;\n\n /**\n * fill the CCS arrays of a matrix, arrays have to be allocated beforehand\n */\n int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const;\n\n /**\n * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes\n * the values and assumes that column and row structures have already been written.\n */\n int fillCCS(double* Cx, bool upperTriangle = false) const;\n\n //! exports the non zero blocks in the structure matrix ms\n void fillBlockStructure(MatrixStructure& ms) const;\n\n //! the block matrices per block-column\n const std::vector& blockCols() const { return _blockCols;}\n std::vector& blockCols() { return _blockCols;}\n\n //! indices of the row blocks\n const std::vector& rowBlockIndices() const { return _rowBlockIndices;}\n std::vector& rowBlockIndices() { return _rowBlockIndices;}\n\n //! indices of the column blocks\n const std::vector& colBlockIndices() const { return _colBlockIndices;}\n std::vector& colBlockIndices() { return _colBlockIndices;}\n\n /**\n * write the content of this matrix to a stream loadable by Octave\n * @param upperTriangle does this matrix store only the upper triangular blocks\n */\n bool writeOctave(const char* filename, bool upperTriangle = true) const;\n\n /**\n * copy into CCS structure\n * @return number of processed blocks, -1 on error\n */\n int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const;\n\n /**\n * copy as transposed into a CCS structure\n * @return number of processed blocks, -1 on error\n */\n int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS& blockCCS) const;\n\n /**\n * take over the memory and matrix pattern from a hash matrix.\n * The structure of the hash matrix will be cleared.\n */\n void takePatternFromHash(SparseBlockMatrixHashMap& hashMatrix);\n\n protected:\n std::vector _rowBlockIndices; ///< vector of the indices of the blocks along the rows.\n std::vector _colBlockIndices; ///< vector of the indices of the blocks along the cols\n //! array of maps of blocks. The index of the array represent a block column of the matrix\n //! and the block column is stored as a map row_block -> matrix_block_ptr.\n std::vector _blockCols;\n bool _hasStorage;\n};\n\ntemplate < class MatrixType >\nstd::ostream& operator << (std::ostream&, const SparseBlockMatrix& m);\n\n typedef SparseBlockMatrix SparseBlockMatrixXd; \n\n} //end namespace\n\n#include \"sparse_block_matrix.hpp\"\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\nnamespace g2o {\n using namespace Eigen;\n\n namespace {\n struct TripletEntry\n {\n int r, c;\n double x;\n TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {}\n };\n struct TripletColSort\n {\n bool operator()(const TripletEntry& e1, const TripletEntry& e2) const\n {\n return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r);\n }\n };\n /** Helper class to sort pair based on first elem */\n template >\n struct CmpPairFirst {\n bool operator()(const std::pair& left, const std::pair& right) {\n return Pred()(left.first, right.first);\n }\n };\n }\n\n template \n SparseBlockMatrix::SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage):\n _rowBlockIndices(rbi,rbi+rb),\n _colBlockIndices(cbi,cbi+cb),\n _blockCols(cb), _hasStorage(hasStorage) \n {\n }\n\n template \n SparseBlockMatrix::SparseBlockMatrix( ):\n _blockCols(0), _hasStorage(true) \n {\n }\n\n template \n void SparseBlockMatrix::clear(bool dealloc) {\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) if (_blockCols.size() > 100)\n# endif\n for (int i=0; i < static_cast(_blockCols.size()); ++i) {\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n typename SparseBlockMatrix::SparseMatrixBlock* b=it->second;\n if (_hasStorage && dealloc)\n delete b;\n else\n b->setZero();\n }\n if (_hasStorage && dealloc)\n _blockCols[i].clear();\n }\n }\n\n template \n SparseBlockMatrix::~SparseBlockMatrix(){\n if (_hasStorage)\n clear(true);\n }\n\n template \n typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::block(int r, int c, bool alloc) {\n typename SparseBlockMatrix::IntBlockMap::iterator it =_blockCols[c].find(r);\n typename SparseBlockMatrix::SparseMatrixBlock* _block=0;\n if (it==_blockCols[c].end()){\n if (!_hasStorage && ! alloc )\n return 0;\n else {\n int rb=rowsOfBlock(r);\n int cb=colsOfBlock(c);\n _block=new typename SparseBlockMatrix::SparseMatrixBlock(rb,cb);\n _block->setZero();\n std::pair < typename SparseBlockMatrix::IntBlockMap::iterator, bool> result\n =_blockCols[c].insert(std::make_pair(r,_block)); (void) result;\n assert (result.second);\n }\n } else {\n _block=it->second;\n }\n return _block;\n }\n\n template \n const typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::block(int r, int c) const {\n typename SparseBlockMatrix::IntBlockMap::const_iterator it =_blockCols[c].find(r);\n if (it==_blockCols[c].end())\n return 0;\n return it->second;\n }\n\n\n template \n SparseBlockMatrix* SparseBlockMatrix::clone() const {\n SparseBlockMatrix* ret= new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());\n for (size_t i=0; i<_blockCols.size(); ++i){\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n typename SparseBlockMatrix::SparseMatrixBlock* b=new typename SparseBlockMatrix::SparseMatrixBlock(*it->second);\n ret->_blockCols[i].insert(std::make_pair(it->first, b));\n }\n }\n ret->_hasStorage=true;\n return ret;\n }\n\n\n template \n template \n bool SparseBlockMatrix::transpose(SparseBlockMatrix*& dest) const {\n if (! dest){\n dest=new SparseBlockMatrix(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size());\n } else {\n if (! dest->_hasStorage)\n return false;\n if(_rowBlockIndices.size()!=dest->_colBlockIndices.size())\n return false;\n if (_colBlockIndices.size()!=dest->_rowBlockIndices.size())\n return false;\n for (size_t i=0; i<_rowBlockIndices.size(); ++i){\n if(_rowBlockIndices[i]!=dest->_colBlockIndices[i])\n return false;\n }\n for (size_t i=0; i<_colBlockIndices.size(); ++i){\n if(_colBlockIndices[i]!=dest->_rowBlockIndices[i])\n return false;\n }\n }\n\n for (size_t i=0; i<_blockCols.size(); ++i){\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n typename SparseBlockMatrix::SparseMatrixBlock* s=it->second;\n typename SparseBlockMatrix::SparseMatrixBlock* d=dest->block(i,it->first,true);\n *d = s->transpose();\n }\n }\n return true;\n }\n\n template \n bool SparseBlockMatrix::add(SparseBlockMatrix*& dest) const {\n if (! dest){\n dest=new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());\n } else {\n if (! dest->_hasStorage)\n return false;\n if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size())\n return false;\n if (_colBlockIndices.size()!=dest->_colBlockIndices.size())\n return false;\n for (size_t i=0; i<_rowBlockIndices.size(); ++i){\n if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i])\n return false;\n }\n for (size_t i=0; i<_colBlockIndices.size(); ++i){\n if(_colBlockIndices[i]!=dest->_colBlockIndices[i])\n return false;\n }\n }\n for (size_t i=0; i<_blockCols.size(); ++i){\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n typename SparseBlockMatrix::SparseMatrixBlock* s=it->second;\n typename SparseBlockMatrix::SparseMatrixBlock* d=dest->block(it->first,i,true);\n (*d)+=*s;\n }\n }\n return true;\n }\n\n template \n template < class MatrixResultType, class MatrixFactorType >\n bool SparseBlockMatrix::multiply(SparseBlockMatrix*& dest, const SparseBlockMatrix * M) const {\n // sanity check\n if (_colBlockIndices.size()!=M->_rowBlockIndices.size())\n return false;\n for (size_t i=0; i<_colBlockIndices.size(); ++i){\n if (_colBlockIndices[i]!=M->_rowBlockIndices[i])\n return false;\n }\n if (! dest) {\n dest=new SparseBlockMatrix(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() );\n }\n if (! dest->_hasStorage)\n return false;\n for (size_t i=0; i_blockCols.size(); ++i){\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){\n // look for a non-zero block in a row of column it\n int colM=i;\n const typename SparseBlockMatrix::SparseMatrixBlock *b=it->second;\n typename SparseBlockMatrix::IntBlockMap::const_iterator rbt=_blockCols[it->first].begin();\n while(rbt!=_blockCols[it->first].end()){\n //int colA=it->first;\n int rowA=rbt->first;\n typename SparseBlockMatrix::SparseMatrixBlock *a=rbt->second;\n typename SparseBlockMatrix::SparseMatrixBlock *c=dest->block(rowA,colM,true);\n assert (c->rows()==a->rows());\n assert (c->cols()==b->cols());\n ++rbt;\n (*c)+=(*a)*(*b);\n }\n }\n }\n return false;\n }\n\n template \n void SparseBlockMatrix::multiply(double*& dest, const double* src) const {\n if (! dest){\n dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];\n memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));\n }\n\n // map the memory by Eigen\n Eigen::Map destVec(dest, rows());\n const Eigen::Map srcVec(src, cols());\n\n for (size_t i=0; i<_blockCols.size(); ++i){\n int srcOffset = i ? _colBlockIndices[i-1] : 0;\n\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second;\n int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;\n // destVec += *a * srcVec (according to the sub-vector parts)\n internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);\n }\n }\n }\n\n template \n void SparseBlockMatrix::multiplySymmetricUpperTriangle(double*& dest, const double* src) const\n {\n if (! dest){\n dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];\n memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));\n }\n\n // map the memory by Eigen\n Eigen::Map destVec(dest, rows());\n const Eigen::Map srcVec(src, cols());\n\n for (size_t i=0; i<_blockCols.size(); ++i){\n int srcOffset = colBaseOfBlock(i);\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second;\n int destOffset = rowBaseOfBlock(it->first);\n if (destOffset > srcOffset) // only upper triangle\n break;\n // destVec += *a * srcVec (according to the sub-vector parts)\n internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);\n if (destOffset < srcOffset)\n internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset);\n }\n }\n }\n\n template \n void SparseBlockMatrix::rightMultiply(double*& dest, const double* src) const {\n int destSize=cols();\n\n if (! dest){\n dest=new double [ destSize ];\n memset(dest,0, destSize*sizeof(double));\n }\n\n // map the memory by Eigen\n Eigen::Map destVec(dest, destSize);\n Eigen::Map srcVec(src, rows());\n\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) schedule(dynamic, 10)\n# endif\n for (int i=0; i < static_cast(_blockCols.size()); ++i){\n int destOffset = colBaseOfBlock(i);\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); \n it!=_blockCols[i].end(); \n ++it){\n const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second;\n int srcOffset = rowBaseOfBlock(it->first);\n // destVec += *a.transpose() * srcVec (according to the sub-vector parts)\n internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);\n }\n }\n \n }\n\n template \n void SparseBlockMatrix::scale(double a_) {\n for (size_t i=0; i<_blockCols.size(); ++i){\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n typename SparseBlockMatrix::SparseMatrixBlock* a=it->second;\n *a *= a_;\n }\n }\n }\n\n template \n SparseBlockMatrix* SparseBlockMatrix::slice(int rmin, int rmax, int cmin, int cmax, bool alloc) const {\n int m=rmax-rmin;\n int n=cmax-cmin;\n int rowIdx [m];\n rowIdx[0] = rowsOfBlock(rmin);\n for (int i=1; i::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);\n for (int i=0; i::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){\n if (it->first >= rmin && it->first < rmax){\n typename SparseBlockMatrix::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix::SparseMatrixBlock (* (it->second) ) : it->second;\n s->_blockCols[i].insert(std::make_pair(it->first-rmin, b));\n }\n }\n }\n s->_hasStorage=alloc;\n return s;\n }\n\n template \n size_t SparseBlockMatrix::nonZeroBlocks() const {\n size_t count=0;\n for (size_t i=0; i<_blockCols.size(); ++i)\n count+=_blockCols[i].size();\n return count;\n }\n\n template \n size_t SparseBlockMatrix::nonZeros() const{\n if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) {\n size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime;\n return nnz;\n } else {\n size_t count=0;\n for (size_t i=0; i<_blockCols.size(); ++i){\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n const typename SparseBlockMatrix::SparseMatrixBlock* a=it->second;\n count += a->cols()*a->rows();\n }\n }\n return count;\n }\n }\n\n template \n std::ostream& operator << (std::ostream& os, const SparseBlockMatrix& m){\n os << \"RBI: \" << m.rowBlockIndices().size();\n for (size_t i=0; i::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){\n const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second;\n os << \"BLOCK: \" << it->first << \" \" << i << std::endl;\n os << *b << std::endl;\n }\n }\n return os;\n }\n\n template \n bool SparseBlockMatrix::symmPermutation(SparseBlockMatrix*& dest, const int* pinv, bool upperTriangle) const{\n // compute the permuted version of the new row/column layout\n size_t n=_rowBlockIndices.size();\n // computed the block sizes\n int blockSizes[_rowBlockIndices.size()];\n blockSizes[0]=_rowBlockIndices[0];\n for (size_t i=1; i_rowBlockIndices.size()!=n)\n return false;\n if (dest->_colBlockIndices.size()!=n)\n return false;\n for (size_t i=0; i_rowBlockIndices[i]!=pBlockIndices[i])\n return false;\n if (dest->_colBlockIndices[i]!=pBlockIndices[i])\n return false;\n }\n dest->clear();\n }\n // now ready to permute the columns\n for (size_t i=0; i::IntBlockMap::const_iterator it=_blockCols[i].begin(); \n it!=_blockCols[i].end(); ++it){\n int pj=pinv[it->first];\n\n const typename SparseBlockMatrix::SparseMatrixBlock* s=it->second;\n\n typename SparseBlockMatrix::SparseMatrixBlock* b=0;\n if (! upperTriangle || pj<=pi) {\n b=dest->block(pj,pi,true);\n assert(b->cols()==s->cols());\n assert(b->rows()==s->rows());\n *b=*s;\n } else {\n b=dest->block(pi,pj,true);\n assert(b);\n assert(b->rows()==s->cols());\n assert(b->cols()==s->rows());\n *b=s->transpose();\n }\n }\n //cerr << endl;\n // within each row, \n }\n return true;\n \n }\n\n template \n int SparseBlockMatrix::fillCCS(double* Cx, bool upperTriangle) const\n {\n assert(Cx && \"Target destination is NULL\");\n double* CxStart = Cx;\n for (size_t i=0; i<_blockCols.size(); ++i){\n int cstart=i ? _colBlockIndices[i-1] : 0;\n int csize=colsOfBlock(i);\n for (int c=0; c::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second;\n int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;\n\n int elemsToCopy = b->rows();\n if (upperTriangle && rstart == cstart)\n elemsToCopy = c + 1;\n memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));\n Cx += elemsToCopy;\n\n }\n }\n }\n return Cx - CxStart;\n }\n\n template \n int SparseBlockMatrix::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const\n {\n assert(Cp && Ci && Cx && \"Target destination is NULL\");\n int nz=0;\n for (size_t i=0; i<_blockCols.size(); ++i){\n int cstart=i ? _colBlockIndices[i-1] : 0;\n int csize=colsOfBlock(i);\n for (int c=0; c::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){\n const typename SparseBlockMatrix::SparseMatrixBlock* b=it->second;\n int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;\n\n int elemsToCopy = b->rows();\n if (upperTriangle && rstart == cstart)\n elemsToCopy = c + 1;\n for (int r=0; r\n void SparseBlockMatrix::fillBlockStructure(MatrixStructure& ms) const\n {\n int n = _colBlockIndices.size();\n int nzMax = (int)nonZeroBlocks();\n\n ms.alloc(n, nzMax);\n ms.m = _rowBlockIndices.size();\n\n int nz = 0;\n int* Cp = ms.Ap;\n int* Ci = ms.Aii;\n for (int i = 0; i < static_cast(_blockCols.size()); ++i){\n *Cp = nz;\n const int& c = i;\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {\n const int& r = it->first;\n if (r <= c) {\n *Ci++ = r;\n ++nz;\n }\n }\n Cp++;\n }\n *Cp=nz;\n assert(nz <= nzMax);\n }\n\n template \n bool SparseBlockMatrix::writeOctave(const char* filename, bool upperTriangle) const\n {\n std::string name = filename;\n std::string::size_type lastDot = name.find_last_of('.');\n if (lastDot != std::string::npos) \n name = name.substr(0, lastDot);\n\n std::vector entries;\n for (size_t i = 0; i<_blockCols.size(); ++i){\n const int& c = i;\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {\n const int& r = it->first;\n const MatrixType& m = *(it->second);\n for (int cc = 0; cc < m.cols(); ++cc)\n for (int rr = 0; rr < m.rows(); ++rr) {\n int aux_r = rowBaseOfBlock(r) + rr;\n int aux_c = colBaseOfBlock(c) + cc;\n entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc)));\n if (upperTriangle && r != c) {\n entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc)));\n }\n }\n }\n }\n\n int nz = entries.size();\n std::sort(entries.begin(), entries.end(), TripletColSort());\n\n std::ofstream fout(filename);\n fout << \"# name: \" << name << std::endl;\n fout << \"# type: sparse matrix\" << std::endl;\n fout << \"# nnz: \" << nz << std::endl;\n fout << \"# rows: \" << rows() << std::endl;\n fout << \"# columns: \" << cols() << std::endl;\n fout << std::setprecision(9) << std::fixed << std::endl;\n\n for (std::vector::const_iterator it = entries.begin(); it != entries.end(); ++it) {\n const TripletEntry& entry = *it;\n fout << entry.r+1 << \" \" << entry.c+1 << \" \" << entry.x << std::endl;\n }\n return fout.good();\n }\n\n template \n int SparseBlockMatrix::fillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const\n {\n blockCCS.blockCols().resize(blockCols().size());\n int numblocks = 0;\n for (size_t i = 0; i < blockCols().size(); ++i) {\n const IntBlockMap& row = blockCols()[i];\n typename SparseBlockMatrixCCS::SparseColumn& dest = blockCCS.blockCols()[i];\n dest.clear();\n dest.reserve(row.size());\n for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {\n dest.push_back(typename SparseBlockMatrixCCS::RowBlock(it->first, it->second));\n ++numblocks;\n }\n }\n return numblocks;\n }\n\n template \n int SparseBlockMatrix::fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS& blockCCS) const\n {\n blockCCS.blockCols().clear();\n blockCCS.blockCols().resize(_rowBlockIndices.size());\n int numblocks = 0;\n for (size_t i = 0; i < blockCols().size(); ++i) {\n const IntBlockMap& row = blockCols()[i];\n for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {\n typename SparseBlockMatrixCCS::SparseColumn& dest = blockCCS.blockCols()[it->first];\n dest.push_back(typename SparseBlockMatrixCCS::RowBlock(i, it->second));\n ++numblocks;\n }\n }\n return numblocks;\n }\n\n template \n void SparseBlockMatrix::takePatternFromHash(SparseBlockMatrixHashMap& hashMatrix)\n {\n // sort the sparse columns and add them to the map structures by\n // exploiting that we are inserting a sorted structure\n typedef std::pair SparseColumnPair;\n typedef typename SparseBlockMatrixHashMap::SparseColumn HashSparseColumn;\n for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) {\n // prepare a temporary vector for sorting\n HashSparseColumn& column = hashMatrix.blockCols()[i];\n if (column.size() == 0)\n continue;\n std::vector sparseRowSorted; // temporary structure\n sparseRowSorted.reserve(column.size());\n for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it)\n sparseRowSorted.push_back(*it);\n std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst());\n // try to free some memory early\n HashSparseColumn aux;\n swap(aux, column);\n // now insert sorted vector to the std::map structure\n IntBlockMap& destColumnMap = blockCols()[i];\n destColumnMap.insert(sparseRowSorted[0]);\n for (size_t j = 1; j < sparseRowSorted.size(); ++j) {\n typename SparseBlockMatrix::IntBlockMap::iterator hint = destColumnMap.end();\n --hint; // cppreference says the element goes after the hint (until C++11)\n destColumnMap.insert(hint, sparseRowSorted[j]);\n }\n }\n }\n\n}// end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SPARSE_BLOCK_MATRIX_CCS_H\n#define G2O_SPARSE_BLOCK_MATRIX_CCS_H\n\n#include \n#include \n#include \n\n#include \"../../config.h\"\n#include \"matrix_operations.h\"\n\n#ifdef _MSC_VER\n#include \n#else\n#include \n#endif\n\nnamespace g2o {\n\n /**\n * \\brief Sparse matrix which uses blocks\n *\n * This class is used as a const view on a SparseBlockMatrix\n * which allows a faster iteration over the elements of the\n * matrix.\n */\n template \n class SparseBlockMatrixCCS\n {\n public:\n //! this is the type of the elementary block, it is an Eigen::Matrix.\n typedef MatrixType SparseMatrixBlock;\n\n //! columns of the matrix\n int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}\n //! rows of the matrix\n int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}\n\n /**\n * \\brief A block within a column\n */\n struct RowBlock\n {\n int row; ///< row of the block\n MatrixType* block; ///< matrix pointer for the block\n RowBlock() : row(-1), block(0) {}\n RowBlock(int r, MatrixType* b) : row(r), block(b) {}\n bool operator<(const RowBlock& other) const { return row < other.row;}\n };\n typedef std::vector SparseColumn;\n\n SparseBlockMatrixCCS(const std::vector& rowIndices, const std::vector& colIndices) :\n _rowBlockIndices(rowIndices), _colBlockIndices(colIndices)\n {}\n\n //! how many rows does the block at block-row r has?\n int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }\n\n //! how many cols does the block at block-col c has?\n int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }\n\n //! where does the row at block-row r start?\n int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }\n\n //! where does the col at block-col r start?\n int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }\n\n //! the block matrices per block-column\n const std::vector& blockCols() const { return _blockCols;}\n std::vector& blockCols() { return _blockCols;}\n\n //! indices of the row blocks\n const std::vector& rowBlockIndices() const { return _rowBlockIndices;}\n\n //! indices of the column blocks\n const std::vector& colBlockIndices() const { return _colBlockIndices;}\n\n void rightMultiply(double*& dest, const double* src) const\n {\n int destSize=cols();\n\n if (! dest){\n dest=new double [ destSize ];\n memset(dest,0, destSize*sizeof(double));\n }\n\n // map the memory by Eigen\n Eigen::Map destVec(dest, destSize);\n Eigen::Map srcVec(src, rows());\n\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) schedule(dynamic, 10)\n# endif\n for (int i=0; i < static_cast(_blockCols.size()); ++i){\n int destOffset = colBaseOfBlock(i);\n for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {\n const SparseMatrixBlock* a = it->block;\n int srcOffset = rowBaseOfBlock(it->row);\n // destVec += *a.transpose() * srcVec (according to the sub-vector parts)\n internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);\n }\n }\n }\n\n /**\n * sort the blocks in each column\n */\n void sortColumns()\n {\n for (int i=0; i < static_cast(_blockCols.size()); ++i){\n std::sort(_blockCols[i].begin(), _blockCols[i].end());\n }\n }\n\n /**\n * fill the CCS arrays of a matrix, arrays have to be allocated beforehand\n */\n int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const\n {\n assert(Cp && Ci && Cx && \"Target destination is NULL\");\n int nz=0;\n for (size_t i=0; i<_blockCols.size(); ++i){\n int cstart=i ? _colBlockIndices[i-1] : 0;\n int csize=colsOfBlock(i);\n for (int c=0; cblock;\n int rstart=it->row ? _rowBlockIndices[it->row-1] : 0;\n\n int elemsToCopy = b->rows();\n if (upperTriangle && rstart == cstart)\n elemsToCopy = c + 1;\n for (int r=0; rblock;\n int rstart = it->row ? _rowBlockIndices[it->row-1] : 0;\n\n int elemsToCopy = b->rows();\n if (upperTriangle && rstart == cstart)\n elemsToCopy = c + 1;\n memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));\n Cx += elemsToCopy;\n\n }\n }\n cstart = _colBlockIndices[i];\n }\n return Cx - CxStart;\n }\n\n protected:\n const std::vector& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.\n const std::vector& _colBlockIndices; ///< vector of the indices of the blocks along the cols\n std::vector _blockCols; ///< the matrices stored in CCS order\n };\n\n\n\n /**\n * \\brief Sparse matrix which uses blocks based on hash structures\n *\n * This class is used to construct the pattern of a sparse block matrix \n */\n template \n class SparseBlockMatrixHashMap\n {\n public:\n //! this is the type of the elementary block, it is an Eigen::Matrix.\n typedef MatrixType SparseMatrixBlock;\n\n //! columns of the matrix\n int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}\n //! rows of the matrix\n int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}\n\n typedef std::tr1::unordered_map SparseColumn;\n\n SparseBlockMatrixHashMap(const std::vector& rowIndices, const std::vector& colIndices) :\n _rowBlockIndices(rowIndices), _colBlockIndices(colIndices)\n {}\n\n //! how many rows does the block at block-row r has?\n int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }\n\n //! how many cols does the block at block-col c has?\n int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }\n\n //! where does the row at block-row r start?\n int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }\n\n //! where does the col at block-col r start?\n int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }\n\n //! the block matrices per block-column\n const std::vector& blockCols() const { return _blockCols;}\n std::vector& blockCols() { return _blockCols;}\n\n //! indices of the row blocks\n const std::vector& rowBlockIndices() const { return _rowBlockIndices;}\n\n //! indices of the column blocks\n const std::vector& colBlockIndices() const { return _colBlockIndices;}\n\n /**\n * add a block to the pattern, return a pointer to the added block\n */\n MatrixType* addBlock(int r, int c, bool zeroBlock = false)\n {\n assert(c <(int)_blockCols.size() && \"accessing column which is not available\");\n SparseColumn& sparseColumn = _blockCols[c];\n typename SparseColumn::iterator foundIt = sparseColumn.find(r);\n if (foundIt == sparseColumn.end()) {\n int rb = rowsOfBlock(r);\n int cb = colsOfBlock(c);\n MatrixType* m = new MatrixType(rb, cb);\n if (zeroBlock)\n m->setZero();\n sparseColumn[r] = m;\n return m;\n }\n return foundIt->second;\n }\n\n protected:\n const std::vector& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.\n const std::vector& _colBlockIndices; ///< vector of the indices of the blocks along the cols\n std::vector _blockCols; ///< the matrices stored in CCS order\n };\n\n} //end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H\n#define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H\n\n#include \n#include \n#include \n\n#include \"../../config.h\"\n#include \"matrix_operations.h\"\n\nnamespace g2o {\n\n /**\n * \\brief Sparse matrix which uses blocks on the diagonal\n *\n * This class is used as a const view on a SparseBlockMatrix\n * which allows a faster iteration over the elements of the\n * matrix.\n */\n template \n class SparseBlockMatrixDiagonal\n {\n public:\n //! this is the type of the elementary block, it is an Eigen::Matrix.\n typedef MatrixType SparseMatrixBlock;\n\n //! columns of the matrix\n int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;}\n //! rows of the matrix\n int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;}\n\n typedef std::vector > DiagonalVector;\n\n SparseBlockMatrixDiagonal(const std::vector& blockIndices) :\n _blockIndices(blockIndices)\n {}\n\n //! how many rows/cols does the block at block-row / block-column r has?\n inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; }\n\n //! where does the row /col at block-row / block-column r starts?\n inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; }\n\n //! the block matrices per block-column\n const DiagonalVector& diagonal() const { return _diagonal;}\n DiagonalVector& diagonal() { return _diagonal;}\n\n //! indices of the row blocks\n const std::vector& blockIndices() const { return _blockIndices;}\n\n void multiply(double*& dest, const double* src) const\n {\n int destSize=cols();\n if (! dest) {\n dest=new double[destSize];\n memset(dest,0, destSize*sizeof(double));\n }\n\n // map the memory by Eigen\n Eigen::Map destVec(dest, destSize);\n Eigen::Map srcVec(src, rows());\n\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) schedule(dynamic, 10)\n# endif\n for (int i=0; i < static_cast(_diagonal.size()); ++i){\n int destOffset = baseOfBlock(i);\n int srcOffset = destOffset;\n const SparseMatrixBlock& A = _diagonal[i];\n // destVec += *A.transpose() * srcVec (according to the sub-vector parts)\n internal::axpy(A, srcVec, srcOffset, destVec, destOffset);\n }\n }\n\n protected:\n const std::vector& _blockIndices; ///< vector of the indices of the blocks along the diagonal\n DiagonalVector _diagonal;\n };\n\n} //end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"sparse_block_matrix.h\"\n#include \n\nusing namespace std;\nusing namespace g2o;\nusing namespace Eigen;\n\ntypedef SparseBlockMatrix< MatrixXd >\nSparseBlockMatrixX;\n\nstd::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) {\n for (int i=0; iblock(0,0, true);\n cerr << b->rows() << \" \" << b->cols() << endl;\n for (int i=0; irows(); ++i)\n for (int j=0; jcols(); ++j){\n (*b)(i,j)=i*b->cols()+j;\n }\n\n\n cerr << \"block access 2\" << endl;\n b=M->block(0,2, true);\n cerr << b->rows() << \" \" << b->cols() << endl;\n for (int i=0; irows(); ++i)\n for (int j=0; jcols(); ++j){\n (*b)(i,j)=i*b->cols()+j;\n }\n\n b=M->block(3,2, true);\n cerr << b->rows() << \" \" << b->cols() << endl;\n for (int i=0; irows(); ++i)\n for (int j=0; jcols(); ++j){\n (*b)(i,j)=i*b->cols()+j;\n }\n\n cerr << *M << endl;\n\n cerr << \"SUM\" << endl;\n\n SparseBlockMatrixX* Ms=0;\n M->add(Ms);\n M->add(Ms);\n cerr << *Ms;\n \n SparseBlockMatrixX* Mt=0;\n M->transpose(Mt);\n cerr << *Mt << endl;\n\n SparseBlockMatrixX* Mp=0;\n M->multiply(Mp, Mt);\n cerr << *Mp << endl;\n \n int iperm[]={3,2,1,0};\n SparseBlockMatrixX* PMp=0;\n\n Mp->symmPermutation(PMp,iperm, false);\n cerr << *PMp << endl;\n\n PMp->clear(true);\n Mp->block(3,0)->fill(0.);\n Mp->symmPermutation(PMp,iperm, true);\n cerr << *PMp << endl;\n \n \n \n}\n" }, { "path": "Thirdparty/g2o/g2o/core/sparse_optimizer.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"sparse_optimizer.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"estimate_propagator.h\"\n#include \"optimization_algorithm.h\"\n#include \"batch_stats.h\"\n#include \"hyper_graph_action.h\"\n#include \"robust_kernel.h\"\n#include \"../stuff/timeutil.h\"\n#include \"../stuff/macros.h\"\n#include \"../stuff/misc.h\"\n#include \"../../config.h\"\n\nnamespace g2o{\n using namespace std;\n\n\n SparseOptimizer::SparseOptimizer() :\n _forceStopFlag(0), _verbose(false), _algorithm(0), _computeBatchStatistics(false)\n {\n _graphActions.resize(AT_NUM_ELEMENTS);\n }\n\n SparseOptimizer::~SparseOptimizer(){\n delete _algorithm;\n G2OBatchStatistics::setGlobalStats(0);\n }\n\n void SparseOptimizer::computeActiveErrors()\n {\n // call the callbacks in case there is something registered\n HyperGraphActionSet& actions = _graphActions[AT_COMPUTEACTIVERROR];\n if (actions.size() > 0) {\n for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it)\n (*(*it))(this);\n }\n\n# ifdef G2O_OPENMP\n# pragma omp parallel for default (shared) if (_activeEdges.size() > 50)\n# endif\n for (int k = 0; k < static_cast(_activeEdges.size()); ++k) {\n OptimizableGraph::Edge* e = _activeEdges[k];\n e->computeError();\n }\n\n# ifndef NDEBUG\n for (int k = 0; k < static_cast(_activeEdges.size()); ++k) {\n OptimizableGraph::Edge* e = _activeEdges[k];\n bool hasNan = arrayHasNaN(e->errorData(), e->dimension());\n if (hasNan) {\n cerr << \"computeActiveErrors(): found NaN in error for edge \" << e << endl;\n }\n }\n# endif\n\n }\n\n double SparseOptimizer::activeChi2( ) const\n {\n double chi = 0.0;\n for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {\n const OptimizableGraph::Edge* e = *it;\n chi += e->chi2();\n }\n return chi;\n }\n\n double SparseOptimizer::activeRobustChi2() const\n {\n Eigen::Vector3d rho;\n double chi = 0.0;\n for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {\n const OptimizableGraph::Edge* e = *it;\n if (e->robustKernel()) {\n e->robustKernel()->robustify(e->chi2(), rho);\n chi += rho[0];\n }\n else\n chi += e->chi2();\n }\n return chi;\n }\n\n OptimizableGraph::Vertex* SparseOptimizer::findGauge(){\n if (vertices().empty())\n return 0;\n\n int maxDim=0;\n for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){\n OptimizableGraph::Vertex* v=static_cast(it->second); \n maxDim=std::max(maxDim,v->dimension());\n }\n \n OptimizableGraph::Vertex* rut=0;\n for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){\n OptimizableGraph::Vertex* v=static_cast(it->second);\n if (v->dimension()==maxDim){\n rut=v;\n break;\n }\n }\n return rut;\n }\n\n bool SparseOptimizer::gaugeFreedom()\n {\n if (vertices().empty())\n return false;\n\n int maxDim=0;\n for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){\n OptimizableGraph::Vertex* v=static_cast(it->second); \n maxDim = std::max(maxDim,v->dimension());\n }\n\n for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){\n OptimizableGraph::Vertex* v=static_cast(it->second);\n if (v->dimension() == maxDim) {\n // test for fixed vertex\n if (v->fixed()) {\n return false;\n }\n // test for full dimension prior\n for (HyperGraph::EdgeSet::const_iterator eit = v->edges().begin(); eit != v->edges().end(); ++eit) {\n OptimizableGraph::Edge* e = static_cast(*eit);\n if (e->vertices().size() == 1 && e->dimension() == maxDim)\n return false;\n }\n }\n }\n return true;\n }\n\n bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){\n if (! vlist.size()){\n _ivMap.clear();\n return false;\n }\n\n _ivMap.resize(vlist.size());\n size_t i = 0;\n for (int k=0; k<2; k++)\n for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){\n OptimizableGraph::Vertex* v = *it;\n if (! v->fixed()){\n if (static_cast(v->marginalized()) == k){\n v->setHessianIndex(i);\n _ivMap[i]=v;\n i++;\n }\n }\n else {\n v->setHessianIndex(-1);\n }\n }\n _ivMap.resize(i);\n return true;\n }\n\n void SparseOptimizer::clearIndexMapping(){\n for (size_t i=0; i<_ivMap.size(); ++i){\n _ivMap[i]->setHessianIndex(-1);\n _ivMap[i]=0;\n }\n }\n\n bool SparseOptimizer::initializeOptimization(int level){\n HyperGraph::VertexSet vset;\n for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it)\n vset.insert(it->second);\n return initializeOptimization(vset,level);\n }\n\n bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){\n if (edges().size() == 0) {\n cerr << __PRETTY_FUNCTION__ << \": Attempt to initialize an empty graph\" << endl;\n return false;\n }\n bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;\n assert(workspaceAllocated && \"Error while allocating memory for the Jacobians\");\n clearIndexMapping();\n _activeVertices.clear();\n _activeVertices.reserve(vset.size());\n _activeEdges.clear();\n set auxEdgeSet; // temporary structure to avoid duplicates\n for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){\n OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;\n const OptimizableGraph::EdgeSet& vEdges=v->edges();\n // count if there are edges in that level. If not remove from the pool\n int levelEdges=0;\n for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){\n OptimizableGraph::Edge* e=reinterpret_cast(*it);\n if (level < 0 || e->level() == level) {\n\n bool allVerticesOK = true;\n for (vector::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {\n if (vset.find(*vit) == vset.end()) {\n allVerticesOK = false;\n break;\n }\n }\n if (allVerticesOK && !e->allVerticesFixed()) {\n auxEdgeSet.insert(e);\n levelEdges++;\n }\n\n }\n }\n if (levelEdges){\n _activeVertices.push_back(v);\n\n // test for NANs in the current estimate if we are debugging\n# ifndef NDEBUG\n int estimateDim = v->estimateDimension();\n if (estimateDim > 0) {\n Eigen::VectorXd estimateData(estimateDim);\n if (v->getEstimateData(estimateData.data()) == true) {\n int k;\n bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);\n if (hasNan)\n cerr << __PRETTY_FUNCTION__ << \": Vertex \" << v->id() << \" contains a nan entry at index \" << k << endl;\n }\n }\n# endif\n\n }\n }\n\n _activeEdges.reserve(auxEdgeSet.size());\n for (set::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)\n _activeEdges.push_back(*it);\n\n sortVectorContainers();\n return buildIndexMapping(_activeVertices);\n }\n\n bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset){\n bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;\n assert(workspaceAllocated && \"Error while allocating memory for the Jacobians\");\n clearIndexMapping();\n _activeVertices.clear();\n _activeEdges.clear();\n _activeEdges.reserve(eset.size());\n set auxVertexSet; // temporary structure to avoid duplicates\n for (HyperGraph::EdgeSet::iterator it=eset.begin(); it!=eset.end(); ++it){\n OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it);\n for (vector::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {\n auxVertexSet.insert(static_cast(*vit));\n }\n _activeEdges.push_back(reinterpret_cast(*it));\n }\n\n _activeVertices.reserve(auxVertexSet.size());\n for (set::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it)\n _activeVertices.push_back(*it);\n\n sortVectorContainers();\n return buildIndexMapping(_activeVertices);\n }\n\n void SparseOptimizer::setToOrigin(){\n for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) {\n OptimizableGraph::Vertex* v = static_cast(it->second);\n v->setToOrigin();\n }\n }\n\n void SparseOptimizer::computeInitialGuess()\n {\n EstimatePropagator::PropagateCost costFunction(this);\n computeInitialGuess(costFunction);\n }\n\n void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction)\n {\n OptimizableGraph::VertexSet emptySet;\n std::set backupVertices;\n HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization\n for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {\n OptimizableGraph::Edge* e = *it;\n for (size_t i = 0; i < e->vertices().size(); ++i) {\n OptimizableGraph::Vertex* v = static_cast(e->vertex(i));\n if (v->fixed())\n fixedVertices.insert(v);\n else { // check for having a prior which is able to fully initialize a vertex\n for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) {\n OptimizableGraph::Edge* vedge = static_cast(*vedgeIt);\n if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) {\n //cerr << \"Initialize with prior for \" << v->id() << endl;\n vedge->initialEstimate(emptySet, v);\n fixedVertices.insert(v);\n }\n }\n }\n if (v->hessianIndex() == -1) {\n std::set::const_iterator foundIt = backupVertices.find(v);\n if (foundIt == backupVertices.end()) {\n v->push();\n backupVertices.insert(v);\n }\n }\n }\n }\n\n EstimatePropagator estimatePropagator(this);\n estimatePropagator.propagate(fixedVertices, costFunction);\n\n // restoring the vertices that should not be initialized\n for (std::set::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) {\n Vertex* v = *it;\n v->pop();\n }\n if (verbose()) {\n computeActiveErrors();\n cerr << \"iteration= -1\\t chi2= \" << activeChi2()\n << \"\\t time= 0.0\"\n << \"\\t cumTime= 0.0\"\n << \"\\t (using initial guess from \" << costFunction.name() << \")\" << endl;\n }\n }\n\n int SparseOptimizer::optimize(int iterations, bool online)\n {\n if (_ivMap.size() == 0) {\n cerr << __PRETTY_FUNCTION__ << \": 0 vertices to optimize, maybe forgot to call initializeOptimization()\" << endl;\n return -1;\n }\n\n int cjIterations=0;\n double cumTime=0;\n bool ok=true;\n\n ok = _algorithm->init(online);\n if (! ok) {\n cerr << __PRETTY_FUNCTION__ << \" Error while initializing\" << endl;\n return -1;\n }\n\n _batchStatistics.clear();\n if (_computeBatchStatistics)\n _batchStatistics.resize(iterations);\n \n OptimizationAlgorithm::SolverResult result = OptimizationAlgorithm::OK;\n for (int i=0; isolve(i, online);\n ok = ( result == OptimizationAlgorithm::OK );\n\n bool errorComputed = false;\n if (_computeBatchStatistics) {\n computeActiveErrors();\n errorComputed = true;\n _batchStatistics[i].chi2 = activeRobustChi2();\n _batchStatistics[i].timeIteration = get_monotonic_time()-ts;\n }\n\n if (verbose()){\n double dts = get_monotonic_time()-ts;\n cumTime += dts;\n if (! errorComputed)\n computeActiveErrors();\n cerr << \"iteration= \" << i\n << \"\\t chi2= \" << FIXED(activeRobustChi2())\n << \"\\t time= \" << dts\n << \"\\t cumTime= \" << cumTime\n << \"\\t edges= \" << _activeEdges.size();\n _algorithm->printVerbose(cerr);\n cerr << endl;\n }\n ++cjIterations; \n postIteration(i);\n }\n if (result == OptimizationAlgorithm::Fail) {\n return 0;\n }\n return cjIterations;\n }\n\n\n void SparseOptimizer::update(const double* update)\n {\n // update the graph by calling oplus on the vertices\n for (size_t i=0; i < _ivMap.size(); ++i) {\n OptimizableGraph::Vertex* v= _ivMap[i];\n#ifndef NDEBUG\n bool hasNan = arrayHasNaN(update, v->dimension());\n if (hasNan)\n cerr << __PRETTY_FUNCTION__ << \": Update contains a nan for vertex \" << v->id() << endl;\n#endif\n v->oplus(update);\n update += v->dimension();\n }\n }\n\n void SparseOptimizer::setComputeBatchStatistics(bool computeBatchStatistics)\n {\n if ((_computeBatchStatistics == true) && (computeBatchStatistics == false)) {\n G2OBatchStatistics::setGlobalStats(0);\n _batchStatistics.clear();\n }\n _computeBatchStatistics = computeBatchStatistics;\n }\n\n bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)\n {\n std::vector newVertices;\n newVertices.reserve(vset.size());\n _activeVertices.reserve(_activeVertices.size() + vset.size());\n _activeEdges.reserve(_activeEdges.size() + eset.size());\n for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {\n OptimizableGraph::Edge* e = static_cast(*it);\n if (!e->allVerticesFixed()) _activeEdges.push_back(e);\n }\n \n // update the index mapping\n size_t next = _ivMap.size();\n for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {\n OptimizableGraph::Vertex* v=static_cast(*it);\n if (! v->fixed()){\n if (! v->marginalized()){\n v->setHessianIndex(next);\n _ivMap.push_back(v);\n newVertices.push_back(v);\n _activeVertices.push_back(v);\n next++;\n } \n else // not supported right now\n abort();\n }\n else {\n v->setHessianIndex(-1);\n }\n }\n\n //if (newVertices.size() != vset.size())\n //cerr << __PRETTY_FUNCTION__ << \": something went wrong \" << PVAR(vset.size()) << \" \" << PVAR(newVertices.size()) << endl;\n return _algorithm->updateStructure(newVertices, eset);\n }\n\n void SparseOptimizer::sortVectorContainers()\n {\n // sort vector structures to get deterministic ordering based on IDs\n sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare());\n sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare());\n }\n\n void SparseOptimizer::clear() {\n _ivMap.clear();\n _activeVertices.clear();\n _activeEdges.clear();\n OptimizableGraph::clear();\n }\n\n SparseOptimizer::VertexContainer::const_iterator SparseOptimizer::findActiveVertex(const OptimizableGraph::Vertex* v) const\n {\n VertexContainer::const_iterator lower = lower_bound(_activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare());\n if (lower == _activeVertices.end())\n return _activeVertices.end();\n if ((*lower) == v)\n return lower;\n return _activeVertices.end();\n }\n\n SparseOptimizer::EdgeContainer::const_iterator SparseOptimizer::findActiveEdge(const OptimizableGraph::Edge* e) const\n {\n EdgeContainer::const_iterator lower = lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare());\n if (lower == _activeEdges.end())\n return _activeEdges.end();\n if ((*lower) == e)\n return lower;\n return _activeEdges.end();\n }\n\n void SparseOptimizer::push(SparseOptimizer::VertexContainer& vlist)\n {\n for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)\n (*it)->push();\n }\n\n void SparseOptimizer::pop(SparseOptimizer::VertexContainer& vlist)\n {\n for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)\n (*it)->pop();\n }\n\n void SparseOptimizer::push(HyperGraph::VertexSet& vlist)\n {\n for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) {\n OptimizableGraph::Vertex* v = dynamic_cast(*it);\n if (v)\n\tv->push();\n else \n\tcerr << __FUNCTION__ << \": FATAL PUSH SET\" << endl;\n }\n }\n\n void SparseOptimizer::pop(HyperGraph::VertexSet& vlist)\n {\n for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it){\n OptimizableGraph::Vertex* v = dynamic_cast (*it);\n if (v)\n\tv->pop();\n else \n\tcerr << __FUNCTION__ << \": FATAL POP SET\" << endl;\n }\n }\n\n void SparseOptimizer::discardTop(SparseOptimizer::VertexContainer& vlist)\n {\n for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)\n (*it)->discardTop();\n }\n\n void SparseOptimizer::setVerbose(bool verbose)\n {\n _verbose = verbose;\n }\n\n void SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)\n {\n if (_algorithm) // reset the optimizer for the formerly used solver\n _algorithm->setOptimizer(0);\n _algorithm = algorithm;\n if (_algorithm)\n _algorithm->setOptimizer(this);\n }\n\n bool SparseOptimizer::computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices){\n return _algorithm->computeMarginals(spinv, blockIndices);\n }\n\n void SparseOptimizer::setForceStopFlag(bool* flag)\n {\n _forceStopFlag=flag;\n }\n\n bool SparseOptimizer::removeVertex(HyperGraph::Vertex* v)\n {\n OptimizableGraph::Vertex* vv = static_cast(v);\n if (vv->hessianIndex() >= 0) {\n clearIndexMapping();\n _ivMap.clear();\n }\n return HyperGraph::removeVertex(v);\n }\n\n bool SparseOptimizer::addComputeErrorAction(HyperGraphAction* action)\n {\n std::pair insertResult = _graphActions[AT_COMPUTEACTIVERROR].insert(action);\n return insertResult.second;\n }\n\n bool SparseOptimizer::removeComputeErrorAction(HyperGraphAction* action)\n {\n return _graphActions[AT_COMPUTEACTIVERROR].erase(action) > 0;\n }\n\n void SparseOptimizer::push()\n {\n push(_activeVertices);\n }\n\n void SparseOptimizer::pop()\n {\n pop(_activeVertices);\n }\n\n void SparseOptimizer::discardTop()\n {\n discardTop(_activeVertices);\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/core/sparse_optimizer.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_GRAPH_OPTIMIZER_CHOL_H_\n#define G2O_GRAPH_OPTIMIZER_CHOL_H_\n\n#include \"../stuff/macros.h\"\n\n#include \"optimizable_graph.h\"\n#include \"sparse_block_matrix.h\"\n#include \"batch_stats.h\"\n\n#include \n\nnamespace g2o {\n\n // forward declaration\n class ActivePathCostFunction;\n class OptimizationAlgorithm;\n class EstimatePropagatorCost;\n\n class SparseOptimizer : public OptimizableGraph {\n\n public:\n enum {\n AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS,\n AT_NUM_ELEMENTS, // keep as last element\n };\n\n friend class ActivePathCostFunction;\n\n // Attention: _solver & _statistics is own by SparseOptimizer and will be\n // deleted in its destructor.\n SparseOptimizer();\n virtual ~SparseOptimizer();\n\n // new interface for the optimizer\n // the old functions will be dropped\n /**\n * Initializes the structures for optimizing a portion of the graph specified by a subset of edges.\n * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the \n * schur complement or to set as fixed during the optimization.\n * @param eset: the subgraph to be optimized.\n * @returns false if somethings goes wrong\n */\n virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);\n\n /**\n * Initializes the structures for optimizing a portion of the graph specified by a subset of vertices.\n * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the \n * schur complement or to set as fixed during the optimization.\n * @param vset: the subgraph to be optimized.\n * @param level: is the level (in multilevel optimization)\n * @returns false if somethings goes wrong\n */\n virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0);\n\n /**\n * Initializes the structures for optimizing the whole graph.\n * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the \n * schur complement or to set as fixed during the optimization.\n * @param level: is the level (in multilevel optimization)\n * @returns false if somethings goes wrong\n */\n virtual bool initializeOptimization(int level=0);\n\n /**\n * HACK updating the internal structures for online processing\n */\n virtual bool updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset);\n \n /**\n * Propagates an initial guess from the vertex specified as origin.\n * It should be called after initializeOptimization(...), as it relies on the _activeVertices/_edges structures.\n * It constructs a set of trees starting from the nodes in the graph which are fixed and eligible for preforming optimization.\n * The trees are constructed by utlizing a cost-function specified.\n * @param costFunction: the cost function used\n * @patam maxDistance: the distance where to stop the search\n */\n virtual void computeInitialGuess();\n\n /**\n * Same as above but using a specific propagator\n */\n virtual void computeInitialGuess(EstimatePropagatorCost& propagator);\n\n /**\n * sets all vertices to their origin.\n */\n virtual void setToOrigin();\n\n\n /**\n * starts one optimization run given the current configuration of the graph, \n * and the current settings stored in the class instance.\n * It can be called only after initializeOptimization\n */\n int optimize(int iterations, bool online = false);\n\n /**\n * computes the blocks of the inverse of the specified pattern.\n * the pattern is given via pairs of the blocks in the hessian\n * @param blockIndices: the pattern\n * @param spinv: the sparse block matrix with the result\n * @returns false if the operation is not supported by the solver\n */\n bool computeMarginals(SparseBlockMatrix& spinv, const std::vector >& blockIndices);\n\n /**\n * computes the inverse of the specified vertex.\n * @param vertex: the vertex whose state is to be marginalised\n * @param spinv: the sparse block matrix with the result\n * @returns false if the operation is not supported by the solver\n */\n bool computeMarginals(SparseBlockMatrix& spinv, const Vertex* vertex) {\n if (vertex->hessianIndex() < 0) {\n return false;\n }\n std::vector > index;\n index.push_back(std::pair(vertex->hessianIndex(), vertex->hessianIndex()));\n return computeMarginals(spinv, index);\n }\n\n /**\n * computes the inverse of the set specified vertices, assembled into a single covariance matrix.\n * @param vertex: the pattern\n * @param spinv: the sparse block matrix with the result\n * @returns false if the operation is not supported by the solver\n */\n bool computeMarginals(SparseBlockMatrix& spinv, const VertexContainer& vertices) {\n std::vector > indices;\n for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {\n indices.push_back(std::pair((*it)->hessianIndex(),(*it)->hessianIndex()));\n }\n return computeMarginals(spinv, indices);\n }\n\n //! finds a gauge in the graph to remove the undefined dof.\n // The gauge should be fixed() and then the optimization can work (if no additional dof are in\n // the system. The default implementation returns a node with maximum dimension.\n virtual Vertex* findGauge();\n\n bool gaugeFreedom();\n\n /**returns the cached chi2 of the active portion of the graph*/\n double activeChi2() const;\n /**\n * returns the cached chi2 of the active portion of the graph.\n * In contrast to activeChi2() this functions considers the weighting\n * of the error according to the robustification of the error functions.\n */\n double activeRobustChi2() const;\n\n //! verbose information during optimization\n bool verbose() const {return _verbose;}\n void setVerbose(bool verbose);\n\n /**\n * sets a variable checked at every iteration to force a user stop. The iteration exits when the variable is true;\n */\n void setForceStopFlag(bool* flag);\n bool* forceStopFlag() const { return _forceStopFlag;};\n\n //! if external stop flag is given, return its state. False otherwise\n bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; }\n\n //! the index mapping of the vertices\n const VertexContainer& indexMapping() const {return _ivMap;}\n //! the vertices active in the current optimization\n const VertexContainer& activeVertices() const { return _activeVertices;}\n //! the edges active in the current optimization\n const EdgeContainer& activeEdges() const { return _activeEdges;}\n\n /**\n * Remove a vertex. If the vertex is contained in the currently active set\n * of vertices, then the internal temporary structures are cleaned, e.g., the index\n * mapping is erased. In case you need the index mapping for manipulating the\n * graph, you have to store it in your own copy.\n */\n virtual bool removeVertex(HyperGraph::Vertex* v);\n\n /**\n * search for an edge in _activeVertices and return the iterator pointing to it\n * getActiveVertices().end() if not found\n */\n VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const;\n /**\n * search for an edge in _activeEdges and return the iterator pointing to it\n * getActiveEdges().end() if not found\n */\n EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const;\n\n //! the solver used by the optimizer\n const OptimizationAlgorithm* algorithm() const { return _algorithm;}\n OptimizationAlgorithm* solver() { return _algorithm;}\n void setAlgorithm(OptimizationAlgorithm* algorithm);\n\n //! push the estimate of a subset of the variables onto a stack\n void push(SparseOptimizer::VertexContainer& vlist);\n //! push the estimate of a subset of the variables onto a stack\n void push(HyperGraph::VertexSet& vlist);\n //! push all the active vertices onto a stack\n void push();\n //! pop (restore) the estimate a subset of the variables from the stack\n void pop(SparseOptimizer::VertexContainer& vlist);\n //! pop (restore) the estimate a subset of the variables from the stack\n void pop(HyperGraph::VertexSet& vlist);\n //! pop (restore) the estimate of the active vertices from the stack\n void pop();\n\n //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate\n void discardTop(SparseOptimizer::VertexContainer& vlist);\n //! same as above, but for the active vertices\n void discardTop();\n using OptimizableGraph::discardTop;\n\n /**\n * clears the graph, and polishes some intermediate structures\n * Note that this only removes nodes / edges. Parameters can be removed\n * with clearParameters().\n */\n virtual void clear();\n\n /**\n * computes the error vectors of all edges in the activeSet, and caches them\n */\n void computeActiveErrors();\n\n /**\n * Linearizes the system by computing the Jacobians for the nodes\n * and edges in the graph\n */\n G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem())\n {\n // nothing needed, linearization is now done inside the solver\n }\n\n /**\n * update the estimate of the active vertices \n * @param update: the double vector containing the stacked\n * elements of the increments on the vertices.\n */\n void update(const double* update);\n\n /**\n returns the set of batch statistics about the optimisation\n */\n const BatchStatisticsContainer& batchStatistics() const { return _batchStatistics;}\n /**\n returns the set of batch statistics about the optimisation\n */\n BatchStatisticsContainer& batchStatistics() { return _batchStatistics;}\n \n void setComputeBatchStatistics(bool computeBatchStatistics);\n \n bool computeBatchStatistics() const { return _computeBatchStatistics;}\n\n /**** callbacks ****/\n //! add an action to be executed before the error vectors are computed\n bool addComputeErrorAction(HyperGraphAction* action);\n //! remove an action that should no longer be execured before computing the error vectors\n bool removeComputeErrorAction(HyperGraphAction* action);\n\n \n\n protected:\n bool* _forceStopFlag;\n bool _verbose;\n\n VertexContainer _ivMap;\n VertexContainer _activeVertices; ///< sorted according to VertexIDCompare\n EdgeContainer _activeEdges; ///< sorted according to EdgeIDCompare\n\n void sortVectorContainers();\n \n OptimizationAlgorithm* _algorithm;\n\n /**\n * builds the mapping of the active vertices to the (block) row / column in the Hessian\n */\n bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist);\n void clearIndexMapping();\n\n BatchStatisticsContainer _batchStatistics; ///< global statistics of the optimizer, e.g., timing, num-non-zeros\n bool _computeBatchStatistics;\n };\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// Copyright (C) 2012 R. Kümmerle\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_LINEAR_SOLVER_DENSE_H\n#define G2O_LINEAR_SOLVER_DENSE_H\n\n#include \"../core/linear_solver.h\"\n#include \"../core/batch_stats.h\"\n\n#include \n#include \n#include\n#include\n\n\nnamespace g2o {\n\n /**\n * \\brief linear solver using dense cholesky decomposition\n */\n template \n class LinearSolverDense : public LinearSolver\n {\n public:\n LinearSolverDense() :\n LinearSolver(),\n _reset(true)\n {\n }\n\n virtual ~LinearSolverDense()\n {\n }\n\n virtual bool init()\n {\n _reset = true;\n return true;\n }\n\n bool solve(const SparseBlockMatrix& A, double* x, double* b)\n {\n int n = A.cols();\n int m = A.cols();\n\n Eigen::MatrixXd& H = _H;\n if (H.cols() != n) {\n H.resize(n, m);\n _reset = true;\n }\n if (_reset) {\n _reset = false;\n H.setZero();\n }\n\n // copy the sparse block matrix into a dense matrix\n int c_idx = 0;\n for (size_t i = 0; i < A.blockCols().size(); ++i) {\n int c_size = A.colsOfBlock(i);\n assert(c_idx == A.colBaseOfBlock(i) && \"mismatch in block indices\");\n\n const typename SparseBlockMatrix::IntBlockMap& col = A.blockCols()[i];\n if (col.size() > 0) {\n typename SparseBlockMatrix::IntBlockMap::const_iterator it;\n for (it = col.begin(); it != col.end(); ++it) {\n int r_idx = A.rowBaseOfBlock(it->first);\n // only the upper triangular block is processed\n if (it->first <= (int)i) {\n int r_size = A.rowsOfBlock(it->first);\n H.block(r_idx, c_idx, r_size, c_size) = *(it->second);\n if (r_idx != c_idx) // write the lower triangular block\n H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose();\n }\n }\n }\n\n c_idx += c_size;\n }\n\n // solving via Cholesky decomposition\n Eigen::VectorXd::MapType xvec(x, m);\n Eigen::VectorXd::ConstMapType bvec(b, n);\n _cholesky.compute(H);\n if (_cholesky.isPositive()) {\n xvec = _cholesky.solve(bvec);\n return true;\n }\n return false;\n }\n\n protected:\n bool _reset;\n Eigen::MatrixXd _H;\n Eigen::LDLT _cholesky;\n\n };\n\n\n}// end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_LINEAR_SOLVER_EIGEN_H\n#define G2O_LINEAR_SOLVER_EIGEN_H\n\n#include \n#include \n\n#include \"../core/linear_solver.h\"\n#include \"../core/batch_stats.h\"\n#include \"../stuff/timeutil.h\"\n\n#include \"../core/eigen_types.h\"\n\n#include \n#include \n\nnamespace g2o {\n\n/**\n * \\brief linear solver which uses the sparse Cholesky solver from Eigen\n *\n * Has no dependencies except Eigen. Hence, should compile almost everywhere\n * without to much issues. Performance should be similar to CSparse, I guess.\n */\ntemplate \nclass LinearSolverEigen: public LinearSolver\n{\n public:\n typedef Eigen::SparseMatrix SparseMatrix;\n typedef Eigen::Triplet Triplet;\n typedef Eigen::PermutationMatrix PermutationMatrix;\n /**\n * \\brief Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering\n */\n class CholeskyDecomposition : public Eigen::SimplicialLDLT\n {\n public:\n CholeskyDecomposition() : Eigen::SimplicialLDLT() {}\n using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered;\n\n void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation)\n {\n m_Pinv = permutation;\n m_P = permutation.inverse();\n int size = a.cols();\n SparseMatrix ap(size, size);\n ap.selfadjointView() = a.selfadjointView().twistedBy(m_P);\n analyzePattern_preordered(ap, true);\n }\n };\n\n public:\n LinearSolverEigen() :\n LinearSolver(),\n _init(true), _blockOrdering(false), _writeDebug(false)\n {\n }\n\n virtual ~LinearSolverEigen()\n {\n }\n\n virtual bool init()\n {\n _init = true;\n return true;\n }\n\n bool solve(const SparseBlockMatrix& A, double* x, double* b)\n {\n if (_init)\n _sparseMatrix.resize(A.rows(), A.cols());\n fillSparseMatrix(A, !_init);\n if (_init) // compute the symbolic composition once\n computeSymbolicDecomposition(A);\n _init = false;\n\n double t=get_monotonic_time();\n _cholesky.factorize(_sparseMatrix);\n if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite\n if (_writeDebug) {\n std::cerr << \"Cholesky failure, writing debug.txt (Hessian loadable by Octave)\" << std::endl;\n A.writeOctave(\"debug.txt\");\n }\n return false;\n }\n\n // Solving the system\n VectorXD::MapType xx(x, _sparseMatrix.cols());\n VectorXD::ConstMapType bb(b, _sparseMatrix.cols());\n xx = _cholesky.solve(bb);\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats) {\n globalStats->timeNumericDecomposition = get_monotonic_time() - t;\n globalStats->choleskyNNZ = _cholesky.matrixL().nestedExpression().nonZeros() + _sparseMatrix.cols(); // the elements of D\n }\n\n return true;\n }\n\n //! do the AMD ordering on the blocks or on the scalar matrix\n bool blockOrdering() const { return _blockOrdering;}\n void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering;}\n\n //! write a debug dump of the system matrix if it is not SPD in solve\n virtual bool writeDebug() const { return _writeDebug;}\n virtual void setWriteDebug(bool b) { _writeDebug = b;}\n\n protected:\n bool _init;\n bool _blockOrdering;\n bool _writeDebug;\n SparseMatrix _sparseMatrix;\n CholeskyDecomposition _cholesky;\n\n /**\n * compute the symbolic decompostion of the matrix only once.\n * Since A has the same pattern in all the iterations, we only\n * compute the fill-in reducing ordering once and re-use for all\n * the following iterations.\n */\n void computeSymbolicDecomposition(const SparseBlockMatrix& A)\n {\n double t=get_monotonic_time();\n if (! _blockOrdering) {\n _cholesky.analyzePattern(_sparseMatrix);\n } else {\n // block ordering with the Eigen Interface\n // This is really ugly currently, as it calls internal functions from Eigen\n // and modifies the SparseMatrix class\n Eigen::PermutationMatrix blockP;\n {\n // prepare a block structure matrix for calling AMD\n std::vector triplets;\n for (size_t c = 0; c < A.blockCols().size(); ++c){\n const typename SparseBlockMatrix::IntBlockMap& column = A.blockCols()[c];\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {\n const int& r = it->first;\n if (r > static_cast(c)) // only upper triangle\n break;\n triplets.push_back(Triplet(r, c, 0.));\n }\n }\n\n // call the AMD ordering on the block matrix.\n // Relies on Eigen's internal stuff, probably bad idea\n SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size());\n auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end());\n typename CholeskyDecomposition::CholMatrixType C;\n C = auxBlockMatrix.selfadjointView();\n Eigen::internal::minimum_degree_ordering(C, blockP);\n }\n\n int rows = A.rows();\n assert(rows == A.cols() && \"Matrix A is not square\");\n\n // Adapt the block permutation to the scalar matrix\n PermutationMatrix scalarP;\n scalarP.resize(rows);\n int scalarIdx = 0;\n for (int i = 0; i < blockP.size(); ++i) {\n const int& p = blockP.indices()(i);\n int base = A.colBaseOfBlock(p);\n int nCols = A.colsOfBlock(p);\n for (int j = 0; j < nCols; ++j)\n scalarP.indices()(scalarIdx++) = base++;\n }\n assert(scalarIdx == rows && \"did not completely fill the permutation matrix\");\n // analyze with the scalar permutation\n _cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP);\n\n }\n G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();\n if (globalStats)\n globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;\n }\n\n void fillSparseMatrix(const SparseBlockMatrix& A, bool onlyValues)\n {\n if (onlyValues) {\n A.fillCCS(_sparseMatrix.valuePtr(), true);\n } else {\n\n // create from triplet structure\n std::vector triplets;\n triplets.reserve(A.nonZeros());\n for (size_t c = 0; c < A.blockCols().size(); ++c) {\n int colBaseOfBlock = A.colBaseOfBlock(c);\n const typename SparseBlockMatrix::IntBlockMap& column = A.blockCols()[c];\n for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {\n int rowBaseOfBlock = A.rowBaseOfBlock(it->first);\n const MatrixType& m = *(it->second);\n for (int cc = 0; cc < m.cols(); ++cc) {\n int aux_c = colBaseOfBlock + cc;\n for (int rr = 0; rr < m.rows(); ++rr) {\n int aux_r = rowBaseOfBlock + rr;\n if (aux_r > aux_c)\n break;\n triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc)));\n }\n }\n }\n }\n _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end());\n\n }\n }\n};\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/color_macros.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_COLOR_MACROS_H\n#define G2O_COLOR_MACROS_H\n\n// font attributes\n#define FT_BOLD \"\\033[1m\"\n#define FT_UNDERLINE \"\\033[4m\"\n\n//background color\n#define BG_BLACK \"\\033[40m\"\n#define BG_RED \"\\033[41m\"\n#define BG_GREEN \"\\033[42m\"\n#define BG_YELLOW \"\\033[43m\"\n#define BG_LIGHTBLUE \"\\033[44m\"\n#define BG_MAGENTA \"\\033[45m\"\n#define BG_BLUE \"\\033[46m\"\n#define BG_WHITE \"\\033[47m\"\n\n// font color\n#define CL_BLACK(s) \"\\033[30m\" << s << \"\\033[0m\"\n#define CL_RED(s) \"\\033[31m\" << s << \"\\033[0m\"\n#define CL_GREEN(s) \"\\033[32m\" << s << \"\\033[0m\"\n#define CL_YELLOW(s) \"\\033[33m\" << s << \"\\033[0m\"\n#define CL_LIGHTBLUE(s) \"\\033[34m\" << s << \"\\033[0m\"\n#define CL_MAGENTA(s) \"\\033[35m\" << s << \"\\033[0m\"\n#define CL_BLUE(s) \"\\033[36m\" << s << \"\\033[0m\"\n#define CL_WHITE(s) \"\\033[37m\" << s << \"\\033[0m\"\n\n#define FG_BLACK \"\\033[30m\"\n#define FG_RED \"\\033[31m\"\n#define FG_GREEN \"\\033[32m\"\n#define FG_YELLOW \"\\033[33m\"\n#define FG_LIGHTBLUE \"\\033[34m\"\n#define FG_MAGENTA \"\\033[35m\"\n#define FG_BLUE \"\\033[36m\"\n#define FG_WHITE \"\\033[37m\"\n\n#define FG_NORM \"\\033[0m\"\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/macros.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_MACROS_H\n#define G2O_MACROS_H\n\n#ifndef DEG2RAD\n#define DEG2RAD(x) ((x) * 0.01745329251994329575)\n#endif\n\n#ifndef RAD2DEG\n#define RAD2DEG(x) ((x) * 57.29577951308232087721)\n#endif\n\n// GCC the one and only\n#if defined(__GNUC__)\n# define G2O_ATTRIBUTE_CONSTRUCTOR(func) \\\n static void func(void)__attribute__ ((constructor)); \\\n static void func(void)\n\n# define G2O_ATTRIBUTE_UNUSED __attribute__((unused))\n# define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2)))\n# define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3)))\n# define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning))\n# define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated))\n\n#ifdef ANDROID\n# define g2o_isnan(x) isnan(x)\n# define g2o_isinf(x) isinf(x)\n# define g2o_isfinite(x) isfinite(x)\n#else\n# define g2o_isnan(x) std::isnan(x)\n# define g2o_isinf(x) std::isinf(x)\n# define g2o_isfinite(x) std::isfinite(x)\n#endif\n\n// MSVC on Windows\n#elif defined _MSC_VER\n# define __PRETTY_FUNCTION__ __FUNCTION__\n\n/**\nModified by Mark Pupilli from:\n\n\t\"Initializer/finalizer sample for MSVC and GCC.\n 2010 Joe Lowe. Released into the public domain.\"\n\n\t\"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.\"\n\n\t(As posted on Stack OVerflow)\n*/\n# define G2O_ATTRIBUTE_CONSTRUCTOR(f) \\\n __pragma(section(\".CRT$XCU\",read)) \\\n static void __cdecl f(void); \\\n __declspec(allocate(\".CRT$XCU\")) void (__cdecl*f##_)(void) = f; \\\n static void __cdecl f(void)\n\n# define G2O_ATTRIBUTE_UNUSED\n# define G2O_ATTRIBUTE_FORMAT12\n# define G2O_ATTRIBUTE_FORMAT23\n# define G2O_ATTRIBUTE_WARNING(func) func\n# define G2O_ATTRIBUTE_DEPRECATED(func) func\n\n#include \n\n# define g2o_isnan(x) _isnan(x)\n# define g2o_isinf(x) (_finite(x) == 0)\n# define g2o_isfinite(x) (_finite(x) != 0)\n\n// unknown compiler\n#else\n# ifndef __PRETTY_FUNCTION__\n# define __PRETTY_FUNCTION__ \"\"\n# endif\n# define G2O_ATTRIBUTE_CONSTRUCTOR(func) func\n# define G2O_ATTRIBUTE_UNUSED\n# define G2O_ATTRIBUTE_FORMAT12\n# define G2O_ATTRIBUTE_FORMAT23\n# define G2O_ATTRIBUTE_WARNING(func) func\n# define G2O_ATTRIBUTE_DEPRECATED(func) func\n\n#include \n#define g2o_isnan(x) isnan(x)\n#define g2o_isinf(x) isinf(x)\n#define g2o_isfinite(x) isfinite(x)\n\n#endif\n\n// some macros that are only useful for c++\n#ifdef __cplusplus\n\n#define G2O_FSKIP_LINE(f) \\\n {char c=' ';while(c != '\\n' && f.good() && !(f).eof()) (f).get(c);}\n\n#ifndef PVAR\n #define PVAR(s) \\\n #s << \" = \" << (s) << std::flush\n#endif\n\n#ifndef PVARA\n#define PVARA(s) \\\n #s << \" = \" << RAD2DEG(s) << \"deg\" << std::flush\n#endif\n\n#ifndef FIXED\n#define FIXED(s) \\\n std::fixed << s << std::resetiosflags(std::ios_base::fixed)\n#endif\n\n#endif // __cplusplus\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/misc.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_STUFF_MISC_H\n#define G2O_STUFF_MISC_H\n\n#include \"macros.h\"\n#include \n\n#ifndef M_PI\n#define M_PI 3.14159265358979323846\n#endif\n\n/** @addtogroup utils **/\n// @{\n\n/** \\file misc.h\n * \\brief some general case utility functions\n *\n * This file specifies some general case utility functions\n **/\n\nnamespace g2o {\n\n/**\n * return the square value\n */\ntemplate \ninline T square(T x)\n{\n return x*x;\n}\n\n/**\n * return the hypot of x and y\n */\ntemplate \ninline T hypot(T x, T y)\n{\n return (T) (sqrt(x*x + y*y));\n}\n\n/**\n * return the squared hypot of x and y\n */\ntemplate \ninline T hypot_sqr(T x, T y)\n{\n return x*x + y*y;\n}\n\n/**\n * convert from degree to radian\n */\ninline double deg2rad(double degree)\n{\n return degree * 0.01745329251994329576;\n}\n\n/**\n * convert from radian to degree\n */\ninline double rad2deg(double rad)\n{\n return rad * 57.29577951308232087721;\n}\n\n/**\n * normalize the angle\n */\ninline double normalize_theta(double theta)\n{\n if (theta >= -M_PI && theta < M_PI)\n return theta;\n \n double multiplier = floor(theta / (2*M_PI));\n theta = theta - multiplier*2*M_PI;\n if (theta >= M_PI)\n theta -= 2*M_PI;\n if (theta < -M_PI)\n theta += 2*M_PI;\n\n return theta;\n}\n\n/**\n * inverse of an angle, i.e., +180 degree\n */\ninline double inverse_theta(double th)\n{\n return normalize_theta(th + M_PI);\n}\n\n/**\n * average two angles\n */\ninline double average_angle(double theta1, double theta2)\n{\n double x, y;\n\n x = cos(theta1) + cos(theta2);\n y = sin(theta1) + sin(theta2);\n if(x == 0 && y == 0)\n return 0;\n else\n return std::atan2(y, x);\n}\n\n/**\n * sign function.\n * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0\n */\ntemplate \ninline int sign(T x)\n{\n if (x > 0)\n return 1;\n else if (x < 0)\n return -1;\n else\n return 0;\n}\n\n/**\n * clamp x to the interval [l, u]\n */\ntemplate \ninline T clamp(T l, T x, T u) \n{\n if (x < l)\n return l;\n if (x > u)\n return u;\n return x;\n}\n\n/**\n * wrap x to be in the interval [l, u]\n */\ntemplate \ninline T wrap(T l, T x, T u) \n{\n T intervalWidth = u - l;\n while (x < l)\n x += intervalWidth;\n while (x > u)\n x -= intervalWidth;\n return x;\n}\n\n/**\n * tests whether there is a NaN in the array\n */\ninline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0)\n{\n for (int i = 0; i < size; ++i)\n if (g2o_isnan(array[i])) {\n if (nanIndex)\n *nanIndex = i;\n return true;\n }\n return false;\n}\n\n/**\n * The following two functions are used to force linkage with static libraries.\n */\nextern \"C\"\n{\n typedef void (* ForceLinkFunction) (void);\n}\n\nstruct ForceLinker\n{\n ForceLinker(ForceLinkFunction function) { (function)(); }\n};\n\n\n} // end namespace\n\n// @}\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/os_specific.c", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"os_specific.h\"\n\n#ifdef WINDOWS\n\nint vasprintf(char** strp, const char* fmt, va_list ap)\n{\n int n;\n int size = 100;\n char* p;\n char* np;\n\n if ((p = (char*)malloc(size * sizeof(char))) == NULL)\n return -1;\n\n while (1) {\n#ifdef _MSC_VER\n n = vsnprintf_s(p, size, size - 1, fmt, ap);\n#else\n n = vsnprintf(p, size, fmt, ap);\n#endif\n if (n > -1 && n < size) {\n *strp = p;\n return n;\n }\n if (n > -1)\n size = n+1;\n else\n size *= 2;\n if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) {\n free(p);\n return -1;\n } else\n p = np;\n }\n}\n\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/os_specific.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_OS_SPECIFIC_HH_\n#define G2O_OS_SPECIFIC_HH_\n\n#ifdef WINDOWS\n#include \n#include \n#include \n#ifndef _WINDOWS\n#include \n#endif\n#define drand48() ((double) rand()/(double)RAND_MAX)\n\n#ifdef __cplusplus\nextern \"C\" {\n#endif\n\nint vasprintf(char** strp, const char* fmt, va_list ap);\n\n#ifdef __cplusplus\n}\n#endif\n\n#endif\n\n#ifdef UNIX\n#include \n// nothing to do on real operating systems\n#endif\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/property.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"property.h\"\n\n#include \n#include \n\n#include \"macros.h\"\n\n#include \"string_tools.h\"\nusing namespace std;\n\nnamespace g2o {\n\n BaseProperty::BaseProperty(const std::string name_) :_name(name_){\n }\n\n BaseProperty::~BaseProperty(){}\n\n bool PropertyMap::addProperty(BaseProperty* p) {\n std::pair result = insert(make_pair(p->name(), p));\n return result.second;\n }\n\n bool PropertyMap::eraseProperty(const std::string& name) {\n PropertyMapIterator it=find(name);\n if (it==end())\n return false;\n delete it->second;\n erase(it);\n return true;\n }\n\n PropertyMap::~PropertyMap() {\n for (PropertyMapIterator it=begin(); it!=end(); it++){\n if (it->second)\n delete it->second;\n }\n }\n\n bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value)\n {\n PropertyMapIterator it = find(name);\n if (it == end())\n return false;\n it->second->fromString(value);\n return true;\n }\n\n void PropertyMap::writeToCSV(std::ostream& os) const\n {\n for (PropertyMapConstIterator it=begin(); it!=end(); it++){\n BaseProperty* p =it->second;\n os << p->name() << \", \";\n }\n os << std::endl;\n for (PropertyMapConstIterator it=begin(); it!=end(); it++){\n BaseProperty* p =it->second;\n os << p->toString() << \", \";\n }\n os << std::endl;\n }\n\n bool PropertyMap::updateMapFromString(const std::string& values)\n {\n bool status = true;\n vector valuesMap = strSplit(values, \",\");\n for (size_t i = 0; i < valuesMap.size(); ++i) {\n vector m = strSplit(valuesMap[i], \"=\");\n if (m.size() != 2) {\n cerr << __PRETTY_FUNCTION__ << \": unable to extract name=value pair from \" << valuesMap[i] << endl;\n continue;\n }\n string name = trim(m[0]);\n string value = trim(m[1]);\n status = status && updatePropertyFromString(name, value);\n }\n return status;\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/stuff/property.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_PROPERTY_H_\n#define G2O_PROPERTY_H_\n\n#include \n#include \n#include \n\n#include \"string_tools.h\"\n\nnamespace g2o {\n\n class BaseProperty {\n public:\n BaseProperty(const std::string name_);\n virtual ~BaseProperty();\n const std::string& name() {return _name;}\n virtual std::string toString() const = 0;\n virtual bool fromString(const std::string& s) = 0;\n protected:\n std::string _name;\n };\n\n template \n class Property: public BaseProperty {\n public:\n typedef T ValueType;\n Property(const std::string& name_): BaseProperty(name_){}\n Property(const std::string& name_, const T& v): BaseProperty(name_), _value(v){}\n void setValue(const T& v) {_value = v; }\n const T& value() const {return _value;}\n virtual std::string toString() const\n {\n std::stringstream sstr;\n sstr << _value;\n return sstr.str();\n }\n virtual bool fromString(const std::string& s)\n {\n bool status = convertString(s, _value);\n return status;\n }\n protected:\n T _value;\n };\n\n /**\n * \\brief a collection of properties mapping from name to the property itself\n */\n class PropertyMap : protected std::map\n {\n public:\n typedef std::map BaseClass;\n typedef BaseClass::iterator PropertyMapIterator;\n typedef BaseClass::const_iterator PropertyMapConstIterator;\n\n ~PropertyMap();\n\n /**\n * add a property to the map\n */\n bool addProperty(BaseProperty* p);\n\n /**\n * remove a property from the map\n */\n bool eraseProperty(const std::string& name_);\n\n /**\n * return a property by its name\n */\n template \n P* getProperty(const std::string& name_)\n {\n PropertyMapIterator it=find(name_);\n if (it==end())\n return 0;\n return dynamic_cast(it->second);\n }\n template \n const P* getProperty(const std::string& name_) const\n {\n PropertyMapConstIterator it=find(name_);\n if (it==end())\n return 0;\n return dynamic_cast(it->second);\n }\n\n /**\n * create a property and insert it\n */\n template \n P* makeProperty(const std::string& name_, const typename P::ValueType& v)\n {\n PropertyMapIterator it=find(name_);\n if (it==end()){\n P* p=new P(name_, v);\n addProperty(p);\n return p;\n } else \n return dynamic_cast(it->second);\n }\n\n /**\n * update a specfic property with a new value\n * @return true if the params is stored and update was carried out\n */\n bool updatePropertyFromString(const std::string& name, const std::string& value);\n\n /**\n * update the map based on a name=value string, e.g., name1=val1,name2=val2\n * @return true, if it was possible to update all parameters\n */\n bool updateMapFromString(const std::string& values);\n\n void writeToCSV(std::ostream& os) const;\n\n using BaseClass::size;\n using BaseClass::begin;\n using BaseClass::end;\n using BaseClass::iterator;\n using BaseClass::const_iterator;\n\n };\n\n typedef Property IntProperty;\n typedef Property BoolProperty;\n typedef Property FloatProperty;\n typedef Property DoubleProperty;\n typedef Property StringProperty;\n\n} // end namespace\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/string_tools.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"string_tools.h\"\n#include \"os_specific.h\"\n#include \"macros.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID)\n#include \n#endif\n\nnamespace g2o {\n\nusing namespace std;\n\nstd::string trim(const std::string& s)\n{\n if(s.length() == 0)\n return s;\n string::size_type b = s.find_first_not_of(\" \\t\\n\");\n string::size_type e = s.find_last_not_of(\" \\t\\n\");\n if(b == string::npos)\n return \"\";\n return std::string(s, b, e - b + 1);\n}\n\nstd::string trimLeft(const std::string& s)\n{\n if(s.length() == 0)\n return s;\n string::size_type b = s.find_first_not_of(\" \\t\\n\");\n string::size_type e = s.length() - 1;\n if(b == string::npos)\n return \"\";\n return std::string(s, b, e - b + 1);\n}\n\nstd::string trimRight(const std::string& s)\n{\n if(s.length() == 0)\n return s;\n string::size_type b = 0;\n string::size_type e = s.find_last_not_of(\" \\t\\n\");\n if(b == string::npos)\n return \"\";\n return std::string(s, b, e - b + 1);\n}\n\nstd::string strToLower(const std::string& s)\n{\n string ret;\n std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::tolower);\n return ret;\n}\n\nstd::string strToUpper(const std::string& s)\n{\n string ret;\n std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::toupper);\n return ret;\n}\n\nstd::string formatString(const char* fmt, ...)\n{\n char* auxPtr = NULL;\n va_list arg_list;\n va_start(arg_list, fmt);\n int numChar = vasprintf(&auxPtr, fmt, arg_list);\n va_end(arg_list);\n string retString;\n if (numChar != -1)\n retString = auxPtr;\n else {\n cerr << __PRETTY_FUNCTION__ << \": Error while allocating memory\" << endl;\n }\n free(auxPtr);\n return retString;\n}\n\nint strPrintf(std::string& str, const char* fmt, ...)\n{\n char* auxPtr = NULL;\n va_list arg_list;\n va_start(arg_list, fmt);\n int numChars = vasprintf(&auxPtr, fmt, arg_list);\n va_end(arg_list);\n str = auxPtr;\n free(auxPtr);\n return numChars;\n}\n\nstd::string strExpandFilename(const std::string& filename)\n{\n#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID)\n string result = filename;\n wordexp_t p;\n\n wordexp(filename.c_str(), &p, 0);\n if(p.we_wordc > 0) {\n result = p.we_wordv[0];\n }\n wordfree(&p);\n return result;\n#else\n (void) filename;\n std::cerr << \"WARNING: \" << __PRETTY_FUNCTION__ << \" not implemented\" << std::endl;\n return std::string();\n#endif\n}\n\nstd::vector strSplit(const std::string& str, const std::string& delimiters)\n{\n std::vector tokens;\n string::size_type lastPos = 0;\n string::size_type pos = 0;\n\n do {\n pos = str.find_first_of(delimiters, lastPos);\n tokens.push_back(str.substr(lastPos, pos - lastPos));\n lastPos = pos + 1;\n } while (string::npos != pos);\n\n return tokens;\n}\n\nbool strStartsWith(const std::string& s, const std::string& start)\n{\n if (s.size() < start.size())\n return false;\n return equal(start.begin(), start.end(), s.begin());\n}\n\nbool strEndsWith(const std::string& s, const std::string& end)\n{\n if (s.size() < end.size())\n return false;\n return equal(end.rbegin(), end.rend(), s.rbegin());\n}\n\nint readLine(std::istream& is, std::stringstream& currentLine)\n{\n if (is.eof())\n return -1;\n currentLine.str(\"\");\n currentLine.clear();\n is.get(*currentLine.rdbuf());\n if (is.fail()) // fail is set on empty lines\n is.clear();\n G2O_FSKIP_LINE(is); // read \\n not read by get()\n return static_cast(currentLine.str().size());\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/stuff/string_tools.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_STRING_TOOLS_H\n#define G2O_STRING_TOOLS_H\n\n#include \n#include \n#include \n#include \n\n#include \"macros.h\"\n\nnamespace g2o {\n\n/** @addtogroup utils **/\n// @{\n\n/** \\file stringTools.h\n * \\brief utility functions for handling strings\n */\n\n/**\n * remove whitespaces from the start/end of a string\n */\n std::string trim(const std::string& s);\n\n/**\n * remove whitespaces from the left side of the string\n */\n std::string trimLeft(const std::string& s);\n\n/**\n * remove whitespaced from the right side of the string\n */\n std::string trimRight(const std::string& s);\n\n/**\n * convert the string to lower case\n */\n std::string strToLower(const std::string& s);\n\n/**\n * convert a string to upper case\n */\n std::string strToUpper(const std::string& s);\n\n/**\n * read integer values (seperated by spaces) from a string and store\n * them in the given OutputIterator.\n */\ntemplate \nOutputIterator readInts(const char* str, OutputIterator out)\n{\n char* cl = (char*)str;\n char* cle = cl;\n while (1) {\n int id = strtol(cl, &cle, 10);\n if (cl == cle)\n break;\n *out++ = id;\n cl = cle;\n }\n return out;\n}\n\n/**\n * read float values (seperated by spaces) from a string and store\n * them in the given OutputIterator.\n */\ntemplate \nOutputIterator readFloats(const char* str, OutputIterator out)\n{\n char* cl = (char*)str;\n char* cle = cl;\n while (1) {\n double val = strtod(cl, &cle);\n if (cl == cle)\n break;\n *out++ = val;\n cl = cle;\n }\n return out;\n}\n\n/**\n * format a string and return a std::string.\n * Format is just like printf, see man 3 printf\n */\n std::string formatString(const char* fmt, ...) G2O_ATTRIBUTE_FORMAT12;\n\n/**\n * replacement function for sprintf which fills a std::string instead of a char*\n */\n int strPrintf(std::string& str, const char* fmt, ...) G2O_ATTRIBUTE_FORMAT23;\n\n/**\n * convert a string into an other type.\n */\ntemplate\nbool convertString(const std::string& s, T& x, bool failIfLeftoverChars = true)\n{\n std::istringstream i(s);\n char c;\n if (!(i >> x) || (failIfLeftoverChars && i.get(c)))\n return false;\n return true;\n}\n\n/**\n * convert a string into an other type.\n * Return the converted value. Throw error if parsing is wrong.\n */\ntemplate\nT stringToType(const std::string& s, bool failIfLeftoverChars = true)\n{\n T x;\n convertString(s, x, failIfLeftoverChars);\n return x;\n}\n\n/**\n * return true, if str starts with substr\n */\n bool strStartsWith(const std::string & str, const std::string& substr);\n\n/**\n * return true, if str ends with substr\n */\n bool strEndsWith(const std::string & str, const std::string& substr);\n\n/**\n * expand the given filename like a posix shell, e.g., ~ $CARMEN_HOME and other will get expanded.\n * Also command substitution, e.g. `pwd` will give the current directory.\n */\n std::string strExpandFilename(const std::string& filename);\n\n/**\n * split a string into token based on the characters given in delim\n */\n std::vector strSplit(const std::string& s, const std::string& delim);\n\n/**\n * read a line from is into currentLine.\n * @return the number of characters read into currentLine (excluding newline), -1 on eof()\n */\n int readLine(std::istream& is, std::stringstream& currentLine);\n\n// @}\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/stuff/timeutil.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"timeutil.h\"\n#include \n\n#ifdef _WINDOWS\n#include \n#include \n#endif\n\n#ifdef UNIX\n#include \n#endif\n\nnamespace g2o {\n\n#ifdef _WINDOWS\n#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)\n #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64\n#else\n #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL\n#endif\n\nstruct timezone\n{\n int tz_minuteswest; /* minutes W of Greenwich */\n int tz_dsttime; /* type of dst correction */\n};\n\nint gettimeofday(struct timeval *tv, struct timezone *tz)\n{\n// Define a structure to receive the current Windows filetime\n FILETIME ft;\n \n// Initialize the present time to 0 and the timezone to UTC\n unsigned __int64 tmpres = 0;\n static int tzflag = 0;\n \n if (NULL != tv)\n {\n GetSystemTimeAsFileTime(&ft);\n \n// The GetSystemTimeAsFileTime returns the number of 100 nanosecond \n// intervals since Jan 1, 1601 in a structure. Copy the high bits to \n// the 64 bit tmpres, shift it left by 32 then or in the low 32 bits.\n tmpres |= ft.dwHighDateTime;\n tmpres <<= 32;\n tmpres |= ft.dwLowDateTime;\n \n// Convert to microseconds by dividing by 10\n tmpres /= 10;\n \n// The Unix epoch starts on Jan 1 1970. Need to subtract the difference \n// in seconds from Jan 1 1601.\n tmpres -= DELTA_EPOCH_IN_MICROSECS;\n \n// Finally change microseconds to seconds and place in the seconds value. \n// The modulus picks up the microseconds.\n tv->tv_sec = (long)(tmpres / 1000000UL);\n tv->tv_usec = (long)(tmpres % 1000000UL);\n }\n \n if (NULL != tz) {\n if (!tzflag) {\n _tzset();\n tzflag++;\n }\n\n long sec;\n int hours;\n _get_timezone(&sec);\n _get_daylight(&hours);\n \n// Adjust for the timezone west of Greenwich\n tz->tz_minuteswest = sec / 60;\n tz->tz_dsttime = hours;\n }\n \n return 0;\n}\n#endif\n\nScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {}\n\nScopeTime::~ScopeTime() {\n std::cerr << _title<<\" took \"<<1000*(get_monotonic_time()-_startTime)<<\"ms.\\n\";\n}\n\ndouble get_monotonic_time()\n{\n#if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK))\n struct timespec ts;\n clock_gettime(CLOCK_MONOTONIC, &ts);\n return ts.tv_sec + ts.tv_nsec*1e-9;\n#else\n return get_time();\n#endif\n}\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/stuff/timeutil.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_TIMEUTIL_H\n#define G2O_TIMEUTIL_H\n\n#ifdef _WINDOWS\n#include \n#else\n#include \n#endif\n\n#include \n\n\n/** @addtogroup utils **/\n// @{\n\n/** \\file timeutil.h\n * \\brief utility functions for handling time related stuff\n */\n\n/// Executes code, only if secs are gone since last exec.\n/// extended version, in which the current time is given, e.g., timestamp of IPC message\n#ifndef DO_EVERY_TS\n#define DO_EVERY_TS(secs, currentTime, code) \\\nif (1) {\\\n static double s_lastDone_ = (currentTime); \\\n double s_now_ = (currentTime); \\\n if (s_lastDone_ > s_now_) \\\n s_lastDone_ = s_now_; \\\n if (s_now_ - s_lastDone_ > (secs)) { \\\n code; \\\n s_lastDone_ = s_now_; \\\n }\\\n} else \\\n (void)0\n#endif\n\n/// Executes code, only if secs are gone since last exec.\n#ifndef DO_EVERY\n#define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code)\n#endif\n\n#ifndef MEASURE_TIME\n#define MEASURE_TIME(text, code) \\\n if(1) { \\\n double _start_time_ = g2o::get_time(); \\\n code; \\\n fprintf(stderr, \"%s took %f sec\\n\", text, g2o::get_time() - _start_time_); \\\n } else \\\n (void) 0\n#endif\n\nnamespace g2o {\n\n#ifdef _WINDOWS\ntypedef struct timeval {\n long tv_sec;\n long tv_usec;\n} timeval;\n int gettimeofday(struct timeval *tv, struct timezone *tz);\n#endif\n\n/**\n * return the current time in seconds since 1. Jan 1970\n */\ninline double get_time() \n{\n struct timeval ts;\n gettimeofday(&ts,0);\n return ts.tv_sec + ts.tv_usec*1e-6;\n}\n\n/**\n * return a monotonic increasing time which basically does not need to\n * have a reference point. Consider this for measuring how long some\n * code fragments required to execute.\n *\n * On Linux we call clock_gettime() on other systems we currently\n * call get_time().\n */\n double get_monotonic_time();\n\n/**\n * \\brief Class to measure the time spent in a scope\n *\n * To use this class, e.g. to measure the time spent in a function,\n * just create and instance at the beginning of the function.\n */\nclass ScopeTime {\n public: \n ScopeTime(const char* title);\n ~ScopeTime();\n private:\n std::string _title;\n double _startTime;\n};\n\n} // end namespace\n\n#ifndef MEASURE_FUNCTION_TIME\n#define MEASURE_FUNCTION_TIME \\\n g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__)\n#endif\n\n\n// @}\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/types/CMakeLists.txt", "content": "ADD_LIBRARY(types ${G2O_LIB_TYPE}\n types_sba.h\n types_six_dof_expmap.h\n types_sba.cpp\n types_six_dof_expmap.cpp\n types_seven_dof_expmap.cpp\n types_seven_dof_expmap.h\n se3quat.h\n se3_ops.h\n se3_ops.hpp\n)\n\nSET_TARGET_PROPERTIES(types PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types)\n\nTARGET_LINK_LIBRARIES(types core)\n" }, { "path": "Thirdparty/g2o/g2o/types/se3_ops.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_MATH_STUFF\n#define G2O_MATH_STUFF\n\n#include \n#include \n\nnamespace g2o {\n using namespace Eigen;\n\n inline Matrix3d skew(const Vector3d&v);\n inline Vector3d deltaR(const Matrix3d& R);\n inline Vector2d project(const Vector3d&);\n inline Vector3d project(const Vector4d&);\n inline Vector3d unproject(const Vector2d&);\n inline Vector4d unproject(const Vector3d&);\n inline Eigen::Quaterniond zyx_euler_to_quat(const double &roll, const double &pitch, const double &yaw);\n inline void quat_to_euler_zyx(const Eigen::Quaterniond q, double& roll, double& pitch, double& yaw);\n\n#include \"se3_ops.hpp\"\n\n}\n\n#endif //MATH_STUFF\n" }, { "path": "Thirdparty/g2o/g2o/types/se3_ops.hpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n#pragma once\n\n Matrix3d skew(const Vector3d&v)\n {\n Matrix3d m;\n m.fill(0.);\n m(0,1) = -v(2);\n m(0,2) = v(1);\n m(1,2) = -v(0);\n m(1,0) = v(2);\n m(2,0) = -v(1);\n m(2,1) = v(0);\n return m;\n }\n\n Vector3d deltaR(const Matrix3d& R)\n {\n Vector3d v;\n v(0)=R(2,1)-R(1,2);\n v(1)=R(0,2)-R(2,0);\n v(2)=R(1,0)-R(0,1);\n return v;\n }\n\n Vector2d project(const Vector3d& v)\n {\n Vector2d res;\n res(0) = v(0)/v(2);\n res(1) = v(1)/v(2);\n return res;\n }\n\n Vector3d project(const Vector4d& v)\n {\n Vector3d res;\n res(0) = v(0)/v(3);\n res(1) = v(1)/v(3);\n res(2) = v(2)/v(3);\n return res;\n }\n\n Vector3d unproject(const Vector2d& v)\n {\n Vector3d res;\n res(0) = v(0);\n res(1) = v(1);\n res(2) = 1;\n return res;\n }\n\n Vector4d unproject(const Vector3d& v)\n {\n Vector4d res;\n res(0) = v(0);\n res(1) = v(1);\n res(2) = v(2);\n res(3) = 1;\n return res;\n }\n \nEigen::Quaterniond zyx_euler_to_quat(const double &roll, const double &pitch, const double &yaw)\n{\n double sy = sin(yaw*0.5);\n double cy = cos(yaw*0.5);\n double sp = sin(pitch*0.5);\n double cp = cos(pitch*0.5);\n double sr = sin(roll*0.5);\n double cr = cos(roll*0.5);\n double w = cr*cp*cy + sr*sp*sy;\n double x = sr*cp*cy - cr*sp*sy;\n double y = cr*sp*cy + sr*cp*sy;\n double z = cr*cp*sy - sr*sp*cy;\n return Eigen::Quaterniond(w,x,y,z);\n}\n\nvoid quat_to_euler_zyx(const Eigen::Quaterniond q, double& roll, double& pitch, double& yaw)\n{\n const double qw = q.w();\n const double qx = q.x();\n const double qy = q.y();\n const double qz = q.z();\n \n roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy));\n pitch = asin(2*(qw*qy-qz*qx));\n yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz));\n}\n" }, { "path": "Thirdparty/g2o/g2o/types/se3quat.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SE3QUAT_H_\n#define G2O_SE3QUAT_H_\n\n#include \"se3_ops.h\"\n\n#include \n#include \n\nnamespace g2o {\n using namespace Eigen;\n\n typedef Matrix Vector6d;\n typedef Matrix Vector7d;\n\n class SE3Quat {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n\n\n protected:\n\n Quaterniond _r;\n Vector3d _t;\n\n\n public:\n SE3Quat(){\n _r.setIdentity();\n _t.setZero();\n }\n\n SE3Quat(const Matrix3d& R, const Vector3d& t):_r(Quaterniond(R)),_t(t){ \n normalizeRotation();\n }\n\n SE3Quat(const Quaterniond& q, const Vector3d& t):_r(q),_t(t){\n normalizeRotation();\n }\n \n // x y z qx qy qz qw\n SE3Quat(const Vector7d& v){\n\t fromVector(v);\n\t normalizeRotation();\n }\n \n /**\n * templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6d or Map\n */\n template \n explicit SE3Quat(const MatrixBase& v)\n {\n assert((v.size() == 6 || v.size() == 7) && \"Vector dimension does not match\");\n if (v.size() == 6) {\n for (int i=0; i<3; i++){\n _t[i]=v[i];\n _r.coeffs()(i)=v[i+3];\n }\n _r.w() = 0.; // recover the positive w\n if (_r.norm()>1.){\n _r.normalize();\n } else {\n double w2=1.-_r.squaredNorm();\n _r.w()= (w2<0.) ? 0. : sqrt(w2);\n }\n }\n else if (v.size() == 7) {\n int idx = 0;\n for (int i=0; i<3; ++i, ++idx)\n _t(i) = v(idx);\n for (int i=0; i<4; ++i, ++idx)\n _r.coeffs()(i) = v(idx);\n normalizeRotation();\n }\n }\n\n inline const Vector3d& translation() const {return _t;}\n\n inline void setTranslation(const Vector3d& t_) {_t = t_;}\n\n inline const Quaterniond& rotation() const {return _r;}\n\n void setRotation(const Quaterniond& r_) {_r=r_;}\n\n inline SE3Quat operator* (const SE3Quat& tr2) const{\n SE3Quat result(*this);\n result._t += _r*tr2._t;\n result._r*=tr2._r;\n result.normalizeRotation();\n return result;\n }\n\n inline SE3Quat& operator*= (const SE3Quat& tr2){\n _t+=_r*tr2._t;\n _r*=tr2._r;\n normalizeRotation();\n return *this;\n }\n\n inline Vector3d operator* (const Vector3d& v) const {\n return _t+_r*v;\n }\n\n inline SE3Quat inverse() const{\n SE3Quat ret;\n ret._r=_r.conjugate();\n ret._t=ret._r*(_t*-1.);\n return ret;\n }\n\n inline double operator [](int i) const {\n assert(i<7);\n if (i<3)\n return _t[i];\n return _r.coeffs()[i-3];\n }\n\n // x y z qx qy qz qw\n inline Vector7d toVector() const{\n Vector7d v;\n v[0]=_t(0);\n v[1]=_t(1);\n v[2]=_t(2);\n v[3]=_r.x();\n v[4]=_r.y();\n v[5]=_r.z();\n v[6]=_r.w();\n return v;\n }\n\n // x y z qx qy qz qw\n inline void fromVector(const Vector7d& v){\n _r=Quaterniond(v[6], v[3], v[4], v[5]);\n _t=Vector3d(v[0], v[1], v[2]);\n }\n // x y z qx qy qz\n inline Vector6d toMinimalVector() const{\n Vector6d v;\n v[0]=_t(0);\n v[1]=_t(1);\n v[2]=_t(2);\n v[3]=_r.x();\n v[4]=_r.y();\n v[5]=_r.z();\n return v;\n }\n // x y z qx qy qz\n inline void fromMinimalVector(const Vector6d& v){\n double w = 1.-v[3]*v[3]-v[4]*v[4]-v[5]*v[5];\n if (w>0){\n _r=Quaterniond(sqrt(w), v[3], v[4], v[5]);\n } else {\n _r=Quaterniond(0, -v[3], -v[4], -v[5]);\n }\n _t=Vector3d(v[0], v[1], v[2]);\n } \n \n // copied from my quat to euler\n void quat_to_euler_zyx_infuc(const Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) const\n {\n\t const double qw = q.w();\n\t const double qx = q.x();\n\t const double qy = q.y();\n\t const double qz = q.z();\n\t \n\t roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy));\n\t pitch = asin(2*(qw*qy-qz*qx));\n\t yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz));\n }\n \n inline Vector6d toXYZPRYVector() const{\n\tdouble yaw, pitch, roll;\n\tquat_to_euler_zyx_infuc(_r, roll, pitch, yaw);\n Vector6d v;\n v[0]=_t(0);\n v[1]=_t(1);\n v[2]=_t(2);\n v[3]=roll;\n v[4]=pitch;\n v[5]=yaw;\n return v;\n }\n \n Eigen::Quaterniond zyx_euler_to_quat(const double &roll, const double &pitch, const double &yaw)\n {\n\t double sy = sin(yaw*0.5);\n\t double cy = cos(yaw*0.5);\n\t double sp = sin(pitch*0.5);\n\t double cp = cos(pitch*0.5);\n\t double sr = sin(roll*0.5);\n\t double cr = cos(roll*0.5);\n\t double w = cr*cp*cy + sr*sp*sy;\n\t double x = sr*cp*cy - cr*sp*sy;\n\t double y = cr*sp*cy + sr*cp*sy;\n\t double z = cr*cp*sy - sr*sp*cy;\n\t return Eigen::Quaterniond(w,x,y,z);\n }\n inline void fromXYZPRYVector(const Vector6d& v){\n\t_r = zyx_euler_to_quat(v[3], v[4], v[5]);\n\t_t=Vector3d(v[0], v[1], v[2]);\n }\n\n \n Vector6d log() const {\n Vector6d res;\n Matrix3d _R = _r.toRotationMatrix();\n double d = 0.5*(_R(0,0)+_R(1,1)+_R(2,2)-1);\n Vector3d omega;\n Vector3d upsilon;\n\n\t\n Vector3d dR = deltaR(_R);\n Matrix3d V_inv;\n\n if (d>0.99999)\n {\n\t \n omega=0.5*dR;\n Matrix3d Omega = skew(omega);\n V_inv = Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);\n }\n else\n {\n double theta = acos(d);\n omega = theta/(2*sqrt(1-d*d))*dR;\n Matrix3d Omega = skew(omega);\n V_inv = ( Matrix3d::Identity() - 0.5*Omega\n + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );\n }\n\n upsilon = V_inv*_t;\n for (int i=0; i<3;i++){\n res[i]=omega[i];\n }\n for (int i=0; i<3;i++){\n res[i+3]=upsilon[i];\n }\n\n return res;\n\n }\n\n Vector3d map(const Vector3d & xyz) const\n {\n return _r*xyz + _t;\n }\n \n // angle, translation\n static SE3Quat exp(const Vector6d & update) // doesn't multiply into current...\n {\n Vector3d omega;\n for (int i=0; i<3; i++)\n omega[i]=update[i];\n Vector3d upsilon;\n for (int i=0; i<3; i++)\n upsilon[i]=update[i+3];\n\n double theta = omega.norm();\n Matrix3d Omega = skew(omega);\n\n Matrix3d R;\n Matrix3d V;\n\tQuaterniond q;\n\n if (theta<0.00001)\n {\n //TODO: CHECK WHETHER THIS IS CORRECT!!!\n R = (Matrix3d::Identity() + Omega + Omega*Omega);// must add Omega... though it is small\n\t \n// \t double theta_sq=theta*theta;double theta_po4=theta_sq*theta_sq; // from LSD sophus\n// \t q.w() = 1-0.5*theta_sq + (1.0/384.0)*theta_po4; \n// \t q.vec() = (0.5-1.0/48.0*theta_sq + 1.0/3840.0*theta_po4)*omega;\n// \t q.normalize();\n\t \n V = R;\n }\n else\n {\n Matrix3d Omega2 = Omega*Omega;\n\n R = (Matrix3d::Identity()\n + sin(theta)/theta *Omega\n + (1-cos(theta))/(theta*theta)*Omega2);\n\t \n// \t q.vec() = omega/theta*sin(theta/2); // from LSD sophus\n// \t q.w() = cos(theta/2);\n// \t q.normalize();\n\t \n V = (Matrix3d::Identity()\n + (1-cos(theta))/(theta*theta)*Omega\n + (theta-sin(theta))/(pow(theta,3))*Omega2);\n }\n// if (q.w()<1)\n// \t std::cout<<\"Quaterniond(R) \"< adj() const\n {\n Matrix3d R = _r.toRotationMatrix();\n Matrix res;\n res.block(0,0,3,3) = R;\n res.block(3,3,3,3) = R;\n res.block(3,0,3,3) = skew(_t)*R;\n res.block(0,3,3,3) = Matrix3d::Zero(3,3);\n return res;\n }\n\n Matrix to_homogeneous_matrix() const\n {\n Matrix homogeneous_matrix;\n homogeneous_matrix.setIdentity();\n homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();\n homogeneous_matrix.col(3).head(3) = translation();\n\n return homogeneous_matrix;\n }\n\n void normalizeRotation(){\n if (_r.w()<0){\n _r.coeffs() *= -1;\n }\n _r.normalize();\n }\n\n /**\n * cast SE3Quat into an Eigen::Isometry3d\n */\n operator Eigen::Isometry3d() const\n {\n Eigen::Isometry3d result = (Eigen::Isometry3d) rotation();\n result.translation() = translation();\n return result;\n }\n };\n\n inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3)\n {\n out_str << se3.to_homogeneous_matrix() << std::endl;\n return out_str;\n }\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/types/sim3.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SIM_3\n#define G2O_SIM_3\n\n#include \"se3_ops.h\"\n#include \n\nnamespace g2o\n{\n using namespace Eigen;\n\n typedef Matrix Vector7d;\n typedef Matrix Matrix7d;\n \n\n struct Sim3\n {\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n protected:\n Quaterniond r;\n Vector3d t;\n double s;\n\n\npublic:\n Sim3()\n {\n r.setIdentity();\n t.fill(0.);\n s=1.;\n }\n\n Sim3(const Quaterniond & r, const Vector3d & t, double s)\n : r(r),t(t),s(s)\n {\n }\n\n Sim3(const Matrix3d & R, const Vector3d & t, double s)\n : r(Quaterniond(R)),t(t),s(s)\n {\n }\n\n\n Sim3(const Vector7d & update) // exponential update\n {\n\n Vector3d omega;\n for (int i=0; i<3; i++)\n omega[i]=update[i];\n\n Vector3d upsilon;\n for (int i=0; i<3; i++)\n upsilon[i]=update[i+3];\n\n double sigma = update[6];\n double theta = omega.norm();\n Matrix3d Omega = skew(omega);\n s = std::exp(sigma);\n Matrix3d Omega2 = Omega*Omega;\n Matrix3d I;\n I.setIdentity();\n Matrix3d R;\n\n double eps = 0.00001;\n double A,B,C;\n if (fabs(sigma)1-eps)\n {\n omega=0.5*deltaR(R);\n Omega = skew(omega);\n A = 1./2.;\n B = 1./6.;\n }\n else\n {\n double theta = acos(d);\n double theta2 = theta*theta;\n omega = theta/(2*sqrt(1-d*d))*deltaR(R);\n Omega = skew(omega);\n A = (1-cos(theta))/(theta2);\n B = (theta-sin(theta))/(theta2*theta);\n }\n }\n else\n {\n C=(s-1)/sigma;\n if (d>1-eps)\n {\n\n double sigma2 = sigma*sigma;\n omega=0.5*deltaR(R);\n Omega = skew(omega);\n A = ((sigma-1)*s+1)/(sigma2);\n B = ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);\n }\n else\n {\n double theta = acos(d);\n omega = theta/(2*sqrt(1-d*d))*deltaR(R);\n Omega = skew(omega);\n double theta2 = theta*theta;\n double a=s*sin(theta);\n double b=s*cos(theta);\n double c=theta2 + sigma*sigma;\n A = (a*sigma+ (1-b)*theta)/(theta*c);\n B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);\n }\n }\n\n Matrix3d W = A*Omega + B*Omega*Omega + C*I;\n\n upsilon = W.lu().solve(t);\n\n\n for (int i=0; i<3; i++)\n res[i] = omega[i];\n\n for (int i=0; i<3; i++)\n res[i+3] = upsilon[i];\n\n res[6] = sigma;\n\n return res;\n \n }\n\n\n Sim3 inverse() const\n {\n return Sim3(r.conjugate(), r.conjugate()*((-1./s)*t), 1./s);\n }\n \n\n double operator[](int i) const\n {\n assert(i<8);\n if (i<4){\n\n return r.coeffs()[i];\n }\n if (i<7){\n return t[i-4];\n }\n return s;\n }\n\n double& operator[](int i)\n {\n assert(i<8);\n if (i<4){\n\n return r.coeffs()[i];\n }\n if (i<7)\n {\n return t[i-4];\n }\n return s;\n }\n\n Sim3 operator *(const Sim3& other) const {\n Sim3 ret;\n ret.r = r*other.r;\n ret.t=s*(r*other.t)+t;\n ret.s=s*other.s;\n return ret;\n }\n\n Sim3& operator *=(const Sim3& other){\n Sim3 ret=(*this)*other;\n *this=ret;\n return *this;\n }\n\n inline const Vector3d& translation() const {return t;}\n\n inline Vector3d& translation() {return t;}\n\n inline const Quaterniond& rotation() const {return r;}\n\n inline Quaterniond& rotation() {return r;}\n\n inline const double& scale() const {return s;}\n\n inline double& scale() {return s;}\n\n };\n\n inline std::ostream& operator <<(std::ostream& out_str,\n const Sim3& sim3)\n {\n out_str << sim3.rotation().coeffs() << std::endl;\n out_str << sim3.translation() << std::endl;\n out_str << sim3.scale() << std::endl;\n\n return out_str;\n }\n\n} // end namespace\n\n\n#endif\n" }, { "path": "Thirdparty/g2o/g2o/types/types_sba.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 Kurt Konolige\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"types_sba.h\"\n#include \n\nnamespace g2o {\n\n using namespace std;\n\n\n VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>()\n {\n }\n\n bool VertexSBAPointXYZ::read(std::istream& is)\n {\n Vector3d lv;\n for (int i=0; i<3; i++)\n is >> _estimate[i];\n return true;\n }\n\n bool VertexSBAPointXYZ::write(std::ostream& os) const\n {\n Vector3d lv=estimate();\n for (int i=0; i<3; i++){\n os << lv[i] << \" \";\n }\n return os.good();\n }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/types/types_sba.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 Kurt Konolige\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#ifndef G2O_SBA_TYPES\n#define G2O_SBA_TYPES\n\n#include \"../core/base_vertex.h\"\n\n#include \n#include \n\nnamespace g2o {\n\n/**\n * \\brief Point vertex, XYZ\n */\n class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>\n{\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW \n VertexSBAPointXYZ();\n virtual bool read(std::istream& is);\n virtual bool write(std::ostream& os) const;\n\n virtual void setToOriginImpl() {\n _estimate.fill(0.);\n }\n\n virtual void oplusImpl(const double* update)\n {\n Eigen::Map v(update);\n _estimate += v;\n }\n};\n\n} // end namespace\n\n#endif // SBA_TYPES\n" }, { "path": "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"types_seven_dof_expmap.h\"\n\nnamespace g2o {\n\n VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, Sim3>()\n {\n _marginalized=false;\n _fix_scale = false;\n }\n\n\n EdgeSim3::EdgeSim3() :\n BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>()\n {\n }\n\n\n bool VertexSim3Expmap::read(std::istream& is)\n {\n Vector7d cam2world;\n for (int i=0; i<6; i++){\n is >> cam2world[i];\n }\n is >> cam2world[6];\n// if (! is) {\n// // if the scale is not specified we set it to 1;\n// std::cerr << \"!s\";\n// cam2world[6]=0.;\n// }\n\n for (int i=0; i<2; i++)\n {\n is >> _focal_length1[i];\n }\n for (int i=0; i<2; i++)\n {\n is >> _principle_point1[i];\n }\n\n setEstimate(Sim3(cam2world).inverse());\n return true;\n }\n\n bool VertexSim3Expmap::write(std::ostream& os) const\n {\n Sim3 cam2world(estimate().inverse());\n Vector7d lv=cam2world.log();\n for (int i=0; i<7; i++){\n os << lv[i] << \" \";\n }\n for (int i=0; i<2; i++)\n {\n os << _focal_length1[i] << \" \";\n }\n for (int i=0; i<2; i++)\n {\n os << _principle_point1[i] << \" \";\n }\n return os.good();\n }\n\n bool EdgeSim3::read(std::istream& is)\n {\n Vector7d v7;\n for (int i=0; i<7; i++){\n is >> v7[i];\n }\n\n Sim3 cam2world(v7);\n setMeasurement(cam2world.inverse());\n\n for (int i=0; i<7; i++)\n for (int j=i; j<7; j++)\n {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n }\n\n bool EdgeSim3::write(std::ostream& os) const\n {\n Sim3 cam2world(measurement().inverse());\n Vector7d v7 = cam2world.log();\n for (int i=0; i<7; i++)\n {\n os << v7[i] << \" \";\n }\n for (int i=0; i<7; i++)\n for (int j=i; j<7; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n }\n\n /**Sim3ProjectXYZ*/\n\n EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() :\n BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>()\n {\n }\n\n bool EdgeSim3ProjectXYZ::read(std::istream& is)\n {\n for (int i=0; i<2; i++)\n {\n is >> _measurement[i];\n }\n\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++) {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n }\n\n bool EdgeSim3ProjectXYZ::write(std::ostream& os) const\n {\n for (int i=0; i<2; i++){\n os << _measurement[i] << \" \";\n }\n\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n }\n\n/**InverseSim3ProjectXYZ*/\n\n EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() :\n BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>()\n {\n }\n\n bool EdgeInverseSim3ProjectXYZ::read(std::istream& is)\n {\n for (int i=0; i<2; i++)\n {\n is >> _measurement[i];\n }\n\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++) {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n }\n\n bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const\n {\n for (int i=0; i<2; i++){\n os << _measurement[i] << \" \";\n }\n\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n }\n\n\n// void EdgeSim3ProjectXYZ::linearizeOplus()\n// {\n// VertexSim3Expmap * vj = static_cast(_vertices[1]);\n// Sim3 T = vj->estimate();\n\n// VertexPointXYZ* vi = static_cast(_vertices[0]);\n// Vector3d xyz = vi->estimate();\n// Vector3d xyz_trans = T.map(xyz);\n\n// double x = xyz_trans[0];\n// double y = xyz_trans[1];\n// double z = xyz_trans[2];\n// double z_2 = z*z;\n\n// Matrix tmp;\n// tmp(0,0) = _focal_length(0);\n// tmp(0,1) = 0;\n// tmp(0,2) = -x/z*_focal_length(0);\n\n// tmp(1,0) = 0;\n// tmp(1,1) = _focal_length(1);\n// tmp(1,2) = -y/z*_focal_length(1);\n\n// _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();\n\n// _jacobianOplusXj(0,0) = x*y/z_2 * _focal_length(0);\n// _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *_focal_length(0);\n// _jacobianOplusXj(0,2) = y/z *_focal_length(0);\n// _jacobianOplusXj(0,3) = -1./z *_focal_length(0);\n// _jacobianOplusXj(0,4) = 0;\n// _jacobianOplusXj(0,5) = x/z_2 *_focal_length(0);\n// _jacobianOplusXj(0,6) = 0; // scale is ignored\n\n\n// _jacobianOplusXj(1,0) = (1+y*y/z_2) *_focal_length(1);\n// _jacobianOplusXj(1,1) = -x*y/z_2 *_focal_length(1);\n// _jacobianOplusXj(1,2) = -x/z *_focal_length(1);\n// _jacobianOplusXj(1,3) = 0;\n// _jacobianOplusXj(1,4) = -1./z *_focal_length(1);\n// _jacobianOplusXj(1,5) = y/z_2 *_focal_length(1);\n// _jacobianOplusXj(1,6) = 0; // scale is ignored\n// }\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n// Modified by Raúl Mur Artal (2014)\n// - Added EdgeInverseSim3ProjectXYZ \n// - Modified VertexSim3Expmap to represent relative transformation between two cameras. Includes calibration of both cameras.\n\n#ifndef G2O_SEVEN_DOF_EXPMAP_TYPES\n#define G2O_SEVEN_DOF_EXPMAP_TYPES\n\n#include \"../core/base_vertex.h\"\n#include \"../core/base_binary_edge.h\"\n#include \"types_six_dof_expmap.h\"\n#include \"sim3.h\"\n\nnamespace g2o {\n\n using namespace Eigen;\n\n /**\n * \\brief Sim3 Vertex, (x,y,z,qw,qx,qy,qz)\n * the parameterization for the increments constructed is a 7d vector\n * (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.\n */\n class VertexSim3Expmap : public BaseVertex<7, Sim3>\n {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n VertexSim3Expmap();\n virtual bool read(std::istream& is);\n virtual bool write(std::ostream& os) const;\n\n virtual void setToOriginImpl() {\n _estimate = Sim3();\n }\n\n virtual void oplusImpl(const double* update_)\n {\n Eigen::Map update(const_cast(update_));\n\n if (_fix_scale)\n update[6] = 0;\n\n Sim3 s(update); // exp map\n setEstimate(s*estimate());\n }\n\n Vector2d _principle_point1, _principle_point2;\n Vector2d _focal_length1, _focal_length2;\n\n Vector2d cam_map1(const Vector2d & v) const\n {\n Vector2d res;\n res[0] = v[0]*_focal_length1[0] + _principle_point1[0];\n res[1] = v[1]*_focal_length1[1] + _principle_point1[1];\n return res;\n }\n\n Vector2d cam_map2(const Vector2d & v) const\n {\n Vector2d res;\n res[0] = v[0]*_focal_length2[0] + _principle_point2[0];\n res[1] = v[1]*_focal_length2[1] + _principle_point2[1];\n return res;\n }\n\n bool _fix_scale;\n\n\n protected:\n };\n\n /**\n * \\brief 7D edge between two Vertex7\n */\n class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>\n {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n EdgeSim3();\n virtual bool read(std::istream& is);\n virtual bool write(std::ostream& os) const;\n void computeError()\n {\n const VertexSim3Expmap* v1 = static_cast(_vertices[0]);\n const VertexSim3Expmap* v2 = static_cast(_vertices[1]);\n\n Sim3 C(_measurement);\n Sim3 error_=C*v1->estimate()*v2->estimate().inverse();\n _error = error_.log();\n }\n\n virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}\n virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)\n {\n VertexSim3Expmap* v1 = static_cast(_vertices[0]);\n VertexSim3Expmap* v2 = static_cast(_vertices[1]);\n if (from.count(v1) > 0)\n\t v2->setEstimate(measurement()*v1->estimate());\n else\n\t v1->setEstimate(measurement().inverse()*v2->estimate());\n }\n };\n\n\n/**/\nclass EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>\n{\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n EdgeSim3ProjectXYZ();\n virtual bool read(std::istream& is);\n virtual bool write(std::ostream& os) const;\n\n void computeError()\n {\n const VertexSim3Expmap* v1 = static_cast(_vertices[1]);\n const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);\n\n Vector2d obs(_measurement);\n _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));\n }\n\n // virtual void linearizeOplus();\n\n};\n\n/**/\nclass EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>\n{\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n EdgeInverseSim3ProjectXYZ();\n virtual bool read(std::istream& is);\n virtual bool write(std::ostream& os) const;\n\n void computeError()\n {\n const VertexSim3Expmap* v1 = static_cast(_vertices[1]);\n const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);\n\n Vector2d obs(_measurement);\n _error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate())));\n }\n\n // virtual void linearizeOplus();\n\n};\n\n} // end namespace\n\n#endif\n\n" }, { "path": "Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n#include \"types_six_dof_expmap.h\"\n\n#include \"../core/factory.h\"\n#include \"../stuff/macros.h\"\n\nnamespace g2o {\n\nusing namespace std;\n\n\nVector2d project2d(const Vector3d& v) {\n Vector2d res;\n res(0) = v(0)/v(2);\n res(1) = v(1)/v(2);\n return res;\n}\n\nVector3d unproject2d(const Vector2d& v) {\n Vector3d res;\n res(0) = v(0);\n res(1) = v(1);\n res(2) = 1;\n return res;\n}\n\nVertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() {\n}\n\nbool VertexSE3Expmap::read(std::istream& is) {\n Vector7d est;\n for (int i=0; i<7; i++)\n is >> est[i];\n SE3Quat cam2world;\n cam2world.fromVector(est);\n setEstimate(cam2world.inverse());\n return true;\n}\n\nbool VertexSE3Expmap::write(std::ostream& os) const {\n SE3Quat cam2world(estimate().inverse());\n for (int i=0; i<7; i++)\n os << cam2world[i] << \" \";\n return os.good();\n}\n\nbool EdgeSE3Expmap::read(std::istream& is) {\n Vector7d meas;\n for (int i=0; i<7; i++)\n is >> meas[i];\n SE3Quat cam2world;\n cam2world.fromVector(meas);\n setMeasurement(cam2world.inverse());\n //TODO: Convert information matrix!!\n for (int i=0; i<6; i++)\n for (int j=i; j<6; j++) {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n}\n\nbool EdgeSE3Expmap::write(std::ostream& os) const {\n SE3Quat cam2world(measurement().inverse());\n for (int i=0; i<7; i++)\n os << cam2world[i] << \" \";\n for (int i=0; i<6; i++)\n for (int j=i; j<6; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n}\n\n\n// void EdgeSE3Expmap::linearizeOplus() {\n// VertexSE3Expmap * vi = static_cast(_vertices[0]);\n// SE3Quat Ti(vi->estimate());\n// \n// VertexSE3Expmap * vj = static_cast(_vertices[1]);\n// SE3Quat Tj(vj->estimate());\n// \n// const SE3Quat & Tij = _measurement;\n// SE3Quat invTij = Tij.inverse();\n// \n// SE3Quat invTj_Tij = Tj.inverse()*Tij;\n// SE3Quat infTi_invTij = Ti.inverse()*invTij;\n// \n// _jacobianOplusXi = invTj_Tij.adj();\n// _jacobianOplusXj = -infTi_invTij.adj();\n// }\n\nEdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() {\n}\n\nbool EdgeSE3ProjectXYZ::read(std::istream& is){\n for (int i=0; i<2; i++){\n is >> _measurement[i];\n }\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++) {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n}\n\nbool EdgeSE3ProjectXYZ::write(std::ostream& os) const {\n\n for (int i=0; i<2; i++){\n os << measurement()[i] << \" \";\n }\n\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n}\n\n\nvoid EdgeSE3ProjectXYZ::linearizeOplus() {\n VertexSE3Expmap * vj = static_cast(_vertices[1]);\n SE3Quat T(vj->estimate());\n VertexSBAPointXYZ* vi = static_cast(_vertices[0]);\n Vector3d xyz = vi->estimate();\n Vector3d xyz_trans = T.map(xyz);\n\n double x = xyz_trans[0];\n double y = xyz_trans[1];\n double z = xyz_trans[2];\n double z_2 = z*z;\n\n Matrix tmp;\n tmp(0,0) = fx;\n tmp(0,1) = 0;\n tmp(0,2) = -x/z*fx;\n\n tmp(1,0) = 0;\n tmp(1,1) = fy;\n tmp(1,2) = -y/z*fy;\n\n _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();\n\n _jacobianOplusXj(0,0) = x*y/z_2 *fx;\n _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;\n _jacobianOplusXj(0,2) = y/z *fx;\n _jacobianOplusXj(0,3) = -1./z *fx;\n _jacobianOplusXj(0,4) = 0;\n _jacobianOplusXj(0,5) = x/z_2 *fx;\n\n _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;\n _jacobianOplusXj(1,1) = -x*y/z_2 *fy;\n _jacobianOplusXj(1,2) = -x/z *fy;\n _jacobianOplusXj(1,3) = 0;\n _jacobianOplusXj(1,4) = -1./z *fy;\n _jacobianOplusXj(1,5) = y/z_2 *fy;\n}\n\nVector2d EdgeSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz) const{\n Vector2d proj = project2d(trans_xyz);\n Vector2d res;\n res[0] = proj[0]*fx + cx;\n res[1] = proj[1]*fy + cy;\n return res;\n}\n\n\nVector3d EdgeStereoSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz, const float &bf) const{\n const float invz = 1.0f/trans_xyz[2];\n Vector3d res;\n res[0] = trans_xyz[0]*invz*fx + cx;\n res[1] = trans_xyz[1]*invz*fy + cy;\n res[2] = res[0] - bf*invz;\n return res;\n}\n\nEdgeStereoSE3ProjectXYZ::EdgeStereoSE3ProjectXYZ() : BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>() {\n}\n\nbool EdgeStereoSE3ProjectXYZ::read(std::istream& is){\n for (int i=0; i<=3; i++){\n is >> _measurement[i];\n }\n for (int i=0; i<=2; i++)\n for (int j=i; j<=2; j++) {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n}\n\nbool EdgeStereoSE3ProjectXYZ::write(std::ostream& os) const {\n\n for (int i=0; i<=3; i++){\n os << measurement()[i] << \" \";\n }\n\n for (int i=0; i<=2; i++)\n for (int j=i; j<=2; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n}\n\nvoid EdgeStereoSE3ProjectXYZ::linearizeOplus() {\n VertexSE3Expmap * vj = static_cast(_vertices[1]);\n SE3Quat T(vj->estimate());\n VertexSBAPointXYZ* vi = static_cast(_vertices[0]);\n Vector3d xyz = vi->estimate();\n Vector3d xyz_trans = T.map(xyz);\n\n const Matrix3d R = T.rotation().toRotationMatrix();\n\n double x = xyz_trans[0];\n double y = xyz_trans[1];\n double z = xyz_trans[2];\n double z_2 = z*z;\n\n _jacobianOplusXi(0,0) = -fx*R(0,0)/z+fx*x*R(2,0)/z_2;\n _jacobianOplusXi(0,1) = -fx*R(0,1)/z+fx*x*R(2,1)/z_2;\n _jacobianOplusXi(0,2) = -fx*R(0,2)/z+fx*x*R(2,2)/z_2;\n\n _jacobianOplusXi(1,0) = -fy*R(1,0)/z+fy*y*R(2,0)/z_2;\n _jacobianOplusXi(1,1) = -fy*R(1,1)/z+fy*y*R(2,1)/z_2;\n _jacobianOplusXi(1,2) = -fy*R(1,2)/z+fy*y*R(2,2)/z_2;\n\n _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*R(2,0)/z_2;\n _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)-bf*R(2,1)/z_2;\n _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*R(2,2)/z_2;\n\n _jacobianOplusXj(0,0) = x*y/z_2 *fx;\n _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;\n _jacobianOplusXj(0,2) = y/z *fx;\n _jacobianOplusXj(0,3) = -1./z *fx;\n _jacobianOplusXj(0,4) = 0;\n _jacobianOplusXj(0,5) = x/z_2 *fx;\n\n _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;\n _jacobianOplusXj(1,1) = -x*y/z_2 *fy;\n _jacobianOplusXj(1,2) = -x/z *fy;\n _jacobianOplusXj(1,3) = 0;\n _jacobianOplusXj(1,4) = -1./z *fy;\n _jacobianOplusXj(1,5) = y/z_2 *fy;\n\n _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2;\n _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2;\n _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2);\n _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3);\n _jacobianOplusXj(2,4) = 0;\n _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2;\n}\n\n\n//Only Pose\n\nbool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is){\n for (int i=0; i<2; i++){\n is >> _measurement[i];\n }\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++) {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n}\n\nbool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const {\n\n for (int i=0; i<2; i++){\n os << measurement()[i] << \" \";\n }\n\n for (int i=0; i<2; i++)\n for (int j=i; j<2; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n}\n\n\nvoid EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {\n VertexSE3Expmap * vi = static_cast(_vertices[0]);\n Vector3d xyz_trans = vi->estimate().map(Xw);\n\n double x = xyz_trans[0];\n double y = xyz_trans[1];\n double invz = 1.0/xyz_trans[2];\n double invz_2 = invz*invz;\n\n _jacobianOplusXi(0,0) = x*y*invz_2 *fx;\n _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;\n _jacobianOplusXi(0,2) = y*invz *fx;\n _jacobianOplusXi(0,3) = -invz *fx;\n _jacobianOplusXi(0,4) = 0;\n _jacobianOplusXi(0,5) = x*invz_2 *fx;\n\n _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;\n _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;\n _jacobianOplusXi(1,2) = -x*invz *fy;\n _jacobianOplusXi(1,3) = 0;\n _jacobianOplusXi(1,4) = -invz *fy;\n _jacobianOplusXi(1,5) = y*invz_2 *fy;\n}\n\nVector2d EdgeSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{\n Vector2d proj = project2d(trans_xyz);\n Vector2d res;\n res[0] = proj[0]*fx + cx;\n res[1] = proj[1]*fy + cy;\n return res;\n}\n\n\nVector3d EdgeStereoSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{\n const float invz = 1.0f/trans_xyz[2];\n Vector3d res;\n res[0] = trans_xyz[0]*invz*fx + cx;\n res[1] = trans_xyz[1]*invz*fy + cy;\n res[2] = res[0] - bf*invz;\n return res;\n}\n\n\nbool EdgeStereoSE3ProjectXYZOnlyPose::read(std::istream& is){\n for (int i=0; i<=3; i++){\n is >> _measurement[i];\n }\n for (int i=0; i<=2; i++)\n for (int j=i; j<=2; j++) {\n is >> information()(i,j);\n if (i!=j)\n information()(j,i)=information()(i,j);\n }\n return true;\n}\n\nbool EdgeStereoSE3ProjectXYZOnlyPose::write(std::ostream& os) const {\n\n for (int i=0; i<=3; i++){\n os << measurement()[i] << \" \";\n }\n\n for (int i=0; i<=2; i++)\n for (int j=i; j<=2; j++){\n os << \" \" << information()(i,j);\n }\n return os.good();\n}\n\nvoid EdgeStereoSE3ProjectXYZOnlyPose::linearizeOplus() {\n VertexSE3Expmap * vi = static_cast(_vertices[0]);\n Vector3d xyz_trans = vi->estimate().map(Xw);\n\n double x = xyz_trans[0];\n double y = xyz_trans[1];\n double invz = 1.0/xyz_trans[2];\n double invz_2 = invz*invz;\n\n _jacobianOplusXi(0,0) = x*y*invz_2 *fx;\n _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;\n _jacobianOplusXi(0,2) = y*invz *fx;\n _jacobianOplusXi(0,3) = -invz *fx;\n _jacobianOplusXi(0,4) = 0;\n _jacobianOplusXi(0,5) = x*invz_2 *fx;\n\n _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;\n _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;\n _jacobianOplusXi(1,2) = -x*invz *fy;\n _jacobianOplusXi(1,3) = 0;\n _jacobianOplusXi(1,4) = -invz *fy;\n _jacobianOplusXi(1,5) = y*invz_2 *fy;\n\n _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*y*invz_2;\n _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)+bf*x*invz_2;\n _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2);\n _jacobianOplusXi(2,3) = _jacobianOplusXi(0,3);\n _jacobianOplusXi(2,4) = 0;\n _jacobianOplusXi(2,5) = _jacobianOplusXi(0,5)-bf*invz_2;\n}\n\n\n} // end namespace\n" }, { "path": "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h", "content": "// g2o - General Graph Optimization\n// Copyright (C) 2011 H. Strasdat\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are\n// met:\n//\n// * Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// * Redistributions in binary form must reproduce the above copyright\n// notice, this list of conditions and the following disclaimer in the\n// documentation and/or other materials provided with the distribution.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\n// IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\n// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\n// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\n// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\n// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\n// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\n// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\n// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\n// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n// Modified by Raúl Mur Artal (2014)\n// Added EdgeSE3ProjectXYZ (project using focal_length in x,y directions)\n// Modified by Raúl Mur Artal (2016)\n// Added EdgeStereoSE3ProjectXYZ (project using focal_length in x,y directions)\n// Added EdgeSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)\n// Added EdgeStereoSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)\n\n#ifndef G2O_SIX_DOF_TYPES_EXPMAP\n#define G2O_SIX_DOF_TYPES_EXPMAP\n\n#include \"../core/base_vertex.h\"\n#include \"../core/base_binary_edge.h\"\n#include \"../core/base_unary_edge.h\"\n#include \"se3_ops.h\"\n#include \"se3quat.h\"\n#include \"types_sba.h\"\n#include \n\nnamespace g2o {\nnamespace types_six_dof_expmap {\nvoid init();\n}\n\nusing namespace Eigen;\n\ntypedef Matrix Matrix6d;\n\n\n/**\n * \\brief SE3 Vertex parameterized internally with a transformation matrix\n and externally with its exponential map stores world to cam\n */\nclass VertexSE3Expmap : public BaseVertex<6, SE3Quat>{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n VertexSE3Expmap();\n\n virtual bool read(std::istream& is);\n\n virtual bool write(std::ostream& os) const;\n\n virtual void setToOriginImpl() {\n _estimate = SE3Quat();\n }\n\n virtual void oplusImpl(const double* update_) {\n Eigen::Map update(update_);\n setEstimate(SE3Quat::exp(update)*estimate());\n }\n};\n\n\n/**\n* \\brief 6D edge between two Vertex6\n*/\nclass EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n EdgeSE3Expmap(){};\n bool read(std::istream& is);\n bool write(std::ostream& os) const;\n void computeError()\n {\n const VertexSE3Expmap* v1 = static_cast(_vertices[0]);\n const VertexSE3Expmap* v2 = static_cast(_vertices[1]);\n\n SE3Quat C(_measurement);\n SE3Quat error_= C*v1->estimate()*v2->estimate().inverse(); // from sim3 orbslam vertex 保存的是 world to cam. measurent 是 from 1 to 2\n// SE3Quat error_=C*v2->estimate().inverse()*v1->estimate(); // similar as LSD \n _error = error_.log();\n }\n \n g2o::Vector6d showError()\n {\n const VertexSE3Expmap* v1 = static_cast(_vertices[0]);\n const VertexSE3Expmap* v2 = static_cast(_vertices[1]);\n \n SE3Quat C(_measurement);\n SE3Quat error_= C*v1->estimate()*v2->estimate().inverse(); // from sim3 orbslam\n// SE3Quat error_=C*v2->estimate().inverse()*v1->estimate(); // similar as LSD\n \n std::cout<<\"error_ \"<(_vertices[0]);\n const VertexSE3Expmap* v2 = static_cast(_vertices[1]);\n\n SE3Quat C(_measurement);\n SE3Quat error_= C*v1->estimate()*v2->estimate().inverse(); // from sim3 orbslam\n// SE3Quat error_=C*v2->estimate().inverse()*v1->estimate(); // similar as LSD\n return error_.log().norm();\n }\n \n virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}\n virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)\n {\n VertexSE3Expmap* v1 = static_cast(_vertices[0]);\n VertexSE3Expmap* v2 = static_cast(_vertices[1]);\n if (from.count(v1) > 0)\n\tv2->setEstimate(measurement()*v1->estimate());\n// \tv2->setEstimate(v1->estimate()*measurement());\n else\n\tv1->setEstimate(measurement().inverse()*v2->estimate());\n// \tv1->setEstimate(v2->estimate()*measurement().inverse());\n }\n void setMeasurement(const SE3Quat& m){\n _measurement = m;\n }\n// void linearizeOplus(); // compute exact jacobian LSD and raw g2o has this.\n};\n\n\nclass EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n EdgeSE3ProjectXYZ();\n\n bool read(std::istream& is);\n\n bool write(std::ostream& os) const;\n\n void computeError() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[1]);\n const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);\n Vector2d obs(_measurement);\n _error = obs-cam_project(v1->estimate().map(v2->estimate()));\n }\n\n bool isDepthPositive() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[1]);\n const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);\n return (v1->estimate().map(v2->estimate()))(2)>0.0;\n }\n \n\n virtual void linearizeOplus();\n\n Vector2d cam_project(const Vector3d & trans_xyz) const;\n\n double fx, fy, cx, cy;\n};\n\n\nclass EdgeStereoSE3ProjectXYZ: public BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n EdgeStereoSE3ProjectXYZ();\n\n bool read(std::istream& is);\n\n bool write(std::ostream& os) const;\n\n void computeError() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[1]);\n const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);\n Vector3d obs(_measurement);\n _error = obs - cam_project(v1->estimate().map(v2->estimate()),bf);\n }\n\n bool isDepthPositive() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[1]);\n const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);\n return (v1->estimate().map(v2->estimate()))(2)>0.0;\n }\n\n\n virtual void linearizeOplus();\n\n Vector3d cam_project(const Vector3d & trans_xyz, const float &bf) const;\n\n double fx, fy, cx, cy, bf;\n};\n\nclass EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n EdgeSE3ProjectXYZOnlyPose(){}\n\n bool read(std::istream& is);\n\n bool write(std::ostream& os) const;\n\n void computeError() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[0]);\n Vector2d obs(_measurement);\n _error = obs-cam_project(v1->estimate().map(Xw));\n }\n\n bool isDepthPositive() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[0]);\n return (v1->estimate().map(Xw))(2)>0.0;\n }\n\n\n virtual void linearizeOplus();\n\n Vector2d cam_project(const Vector3d & trans_xyz) const;\n\n Vector3d Xw;\n double fx, fy, cx, cy;\n};\n\n\nclass EdgeStereoSE3ProjectXYZOnlyPose: public BaseUnaryEdge<3, Vector3d, VertexSE3Expmap>{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n EdgeStereoSE3ProjectXYZOnlyPose(){}\n\n bool read(std::istream& is);\n\n bool write(std::ostream& os) const;\n\n void computeError() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[0]);\n Vector3d obs(_measurement);\n _error = obs - cam_project(v1->estimate().map(Xw));\n }\n\n bool isDepthPositive() {\n const VertexSE3Expmap* v1 = static_cast(_vertices[0]);\n return (v1->estimate().map(Xw))(2)>0.0;\n }\n\n\n virtual void linearizeOplus();\n\n Vector3d cam_project(const Vector3d & trans_xyz) const;\n\n Vector3d Xw;\n double fx, fy, cx, cy, bf;\n};\n\n\n\n} // end namespace\n\n#endif\n" }, { "path": "Thirdparty/g2o/license-bsd.txt", "content": "g2o - General Graph Optimization\nCopyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat,\nKurt Konolige, and Wolfram Burgard\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are\nmet:\n\n* Redistributions of source code must retain the above copyright notice,\n this list of conditions and the following disclaimer.\n* Redistributions in binary form must reproduce the above copyright\n notice, this list of conditions and the following disclaimer in the\n documentation and/or other materials provided with the distribution.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS\nIS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED\nTO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A\nPARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT\nHOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,\nSPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED\nTO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR\nPROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF\nLIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING\nNEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\nSOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n" }, { "path": "cmake_modules/FindEigen3.cmake", "content": "# - Try to find Eigen3 lib\n#\n# This module supports requiring a minimum version, e.g. you can do\n# find_package(Eigen3 3.1.2)\n# to require version 3.1.2 or newer of Eigen3.\n#\n# Once done this will define\n#\n# EIGEN3_FOUND - system has eigen lib with correct version\n# EIGEN3_INCLUDE_DIR - the eigen include directory\n# EIGEN3_VERSION - eigen version\n\n# Copyright (c) 2006, 2007 Montel Laurent, \n# Copyright (c) 2008, 2009 Gael Guennebaud, \n# Copyright (c) 2009 Benoit Jacob \n# Redistribution and use is allowed according to the terms of the 2-clause BSD license.\n\nif(NOT Eigen3_FIND_VERSION)\n if(NOT Eigen3_FIND_VERSION_MAJOR)\n set(Eigen3_FIND_VERSION_MAJOR 2)\n endif(NOT Eigen3_FIND_VERSION_MAJOR)\n if(NOT Eigen3_FIND_VERSION_MINOR)\n set(Eigen3_FIND_VERSION_MINOR 91)\n endif(NOT Eigen3_FIND_VERSION_MINOR)\n if(NOT Eigen3_FIND_VERSION_PATCH)\n set(Eigen3_FIND_VERSION_PATCH 0)\n endif(NOT Eigen3_FIND_VERSION_PATCH)\n\n set(Eigen3_FIND_VERSION \"${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}\")\nendif(NOT Eigen3_FIND_VERSION)\n\nmacro(_eigen3_check_version)\n file(READ \"${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h\" _eigen3_version_header)\n\n string(REGEX MATCH \"define[ \\t]+EIGEN_WORLD_VERSION[ \\t]+([0-9]+)\" _eigen3_world_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_WORLD_VERSION \"${CMAKE_MATCH_1}\")\n string(REGEX MATCH \"define[ \\t]+EIGEN_MAJOR_VERSION[ \\t]+([0-9]+)\" _eigen3_major_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_MAJOR_VERSION \"${CMAKE_MATCH_1}\")\n string(REGEX MATCH \"define[ \\t]+EIGEN_MINOR_VERSION[ \\t]+([0-9]+)\" _eigen3_minor_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_MINOR_VERSION \"${CMAKE_MATCH_1}\")\n\n set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})\n if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})\n set(EIGEN3_VERSION_OK FALSE)\n else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})\n set(EIGEN3_VERSION_OK TRUE)\n endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})\n\n if(NOT EIGEN3_VERSION_OK)\n\n message(STATUS \"Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, \"\n \"but at least version ${Eigen3_FIND_VERSION} is required\")\n endif(NOT EIGEN3_VERSION_OK)\nendmacro(_eigen3_check_version)\n\nif (EIGEN3_INCLUDE_DIR)\n\n # in cache already\n _eigen3_check_version()\n set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})\n\nelse (EIGEN3_INCLUDE_DIR)\n\n # specific additional paths for some OS\n if (WIN32)\n set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} \"C:/Program Files/Eigen/include\" \"C:/Program Files (x86)/Eigen/include\")\n endif(WIN32)\n\n find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library\n PATHS\n ${CMAKE_INSTALL_PREFIX}/include\n ${EIGEN_ADDITIONAL_SEARCH_PATHS}\n ${KDE4_INCLUDE_DIR}\n PATH_SUFFIXES eigen3 eigen\n )\n\n if(EIGEN3_INCLUDE_DIR)\n _eigen3_check_version()\n endif(EIGEN3_INCLUDE_DIR)\n\n include(FindPackageHandleStandardArgs)\n find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)\n\n mark_as_advanced(EIGEN3_INCLUDE_DIR)\n\nendif(EIGEN3_INCLUDE_DIR)\n\n" }, { "path": "include/core/BasicEllipsoidEdges.h", "content": "#include \"include/core/Ellipsoid.h\"\n#include \"Thirdparty/g2o/g2o/core/base_multi_edge.h\"\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h\"\n#include \n#include \n#include \n\nnamespace g2o\n{\n\nclass VertexEllipsoid:public BaseVertex<9,ellipsoid> \n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n VertexEllipsoid(){};\n\n virtual void setToOriginImpl();\n\n virtual void oplusImpl(const double* update_);\n\n virtual bool read(std::istream& is) ;\n\n virtual bool write(std::ostream& os) const ;\n};\n\n// 6 degrees\nclass VertexEllipsoidXYZABC:public BaseVertex<6,ellipsoid> \n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n VertexEllipsoidXYZABC(){};\n\n virtual void setToOriginImpl();\n\n virtual void oplusImpl(const double* update_);\n\n virtual bool read(std::istream& is) ;\n\n virtual bool write(std::ostream& os) const ;\n};\n\n// camera-object 3D error\nclass EdgeSE3Ellipsoid9DOF:public BaseBinaryEdge<9,ellipsoid, VertexSE3Expmap, VertexEllipsoid>\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n EdgeSE3Ellipsoid9DOF(){};\n\n virtual bool read(std::istream& is);\n\n virtual bool write(std::ostream& os) const;\n\n void computeError();\n};\n\n// camera -object 2D projection error, rectangle difference of two vertices (left-top and right-bottom points)\nclass EdgeSE3EllipsoidProj:public BaseBinaryEdge<4,Vector4d, VertexSE3Expmap, VertexEllipsoid>\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n EdgeSE3EllipsoidProj(){};\n\n virtual bool read(std::istream& is);\n\n virtual bool write(std::ostream& os) const;\n\n void computeError();\n\n Vector4d getProject();\n\n void setKalib(Matrix3d& K);\n\n Matrix3d Kalib;\n};\n\n// object supporting prior: the Z axis of the object must align with the normal direction of the supporting plane\nclass EdgeEllipsoidGravityPlanePrior:public BaseUnaryEdge<1,Vector4d, VertexEllipsoid>\n{\npublic:\n virtual bool read(std::istream& is);\n\n virtual bool write(std::ostream& os) const;\n\n void computeError();\n};\n\n}" }, { "path": "include/core/DataAssociation.h", "content": "#ifndef ELLIPSOIDSLAM_DATAASSOCIATION_H\n#define ELLIPSOIDSLAM_DATAASSOCIATION_H\n\n#include \n#include \n#include \n\nnamespace EllipsoidSLAM\n{\n \nclass DataAssociationSolver\n{\n\npublic:\n DataAssociationSolver(Map* pMap); // the solver need the ellipsoids in the map\n\n std::vector Solve(EllipsoidSLAM::Frame* pFrame, bool mb3D); \nprivate:\n std::vector SolveCostMat(MatrixXd& mat);\n\n int CreateInstance(); // return the new instance ID\n\n Map* mpMap;\n\n bool mbWithAssociation;\n\n int miInstanceNum;\n};\n\n}\n\n#endif // ELLIPSOIDSLAM_DATAASSOCIATION_H" }, { "path": "include/core/Ellipsoid.h", "content": "#pragma once\n\n#include \"Thirdparty/g2o/g2o/core/base_multi_edge.h\"\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h\"\n#include \n#include \n#include \n\n#include \"include/utils/matrix_utils.h\"\n \ntypedef Eigen::Matrix Vector9d;\ntypedef Eigen::Matrix Matrix9d;\ntypedef Eigen::Matrix Matrix5d;\ntypedef Eigen::Matrix Matrix38d;\ntypedef Eigen::Matrix Vector10d;\ntypedef Eigen::Matrix Vector6d;\ntypedef Eigen::Matrix Vector5d;\ntypedef Eigen::Matrix Vector2d;\ntypedef Eigen::Matrix Vector4d;\ntypedef Eigen::Matrix Matrix6d;\n\nnamespace g2o\n{\n\n\nclass ellipsoid{\n\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n SE3Quat pose; // rigid body transformation, object in world coordinate\n Vector3d scale; // a,b,c : half length of axis x,y,z\n\n Vector9d vec_minimal; // x,y,z,roll,pitch,yaw,a,b,c\n\n double prob; // probability from single-frame ellipsoid estimation\n\n int miLabel; // semantic label.\n int miInstanceID; // instance id.\n\n ellipsoid();\n // Copy constructor.\n ellipsoid(const ellipsoid &e);\n const ellipsoid& operator=(const ellipsoid& in);\n\n // xyz roll pitch yaw half_scale\n void fromMinimalVector(const Vector9d& v);\n\n // xyz quaternion, half_scale\n void fromVector(const Vector10d& v);\n\n const Vector3d& translation() const;\n void setTranslation(const Vector3d& t_);\n void setRotation(const Quaterniond& r_);\n void setRotation(const Matrix3d& R);\n void setScale(const Vector3d &scale_);\n\n // apply update to current ellipsoid. exponential map\n ellipsoid exp_update(const Vector9d& update);\n ellipsoid exp_update_XYZABC(const Vector6d& update);\n\n // actual error between two ellipsoid.\n Vector9d ellipsoid_log_error_9dof(const ellipsoid& newone) const;\n\n // change front face by rotate along current body z axis. another way of representing cuboid. representing same cuboid (IOU always 1)\n ellipsoid rotate_ellipsoid(double yaw_angle) const;\n\n // function called by g2o.\n Vector9d min_log_error_9dof(const ellipsoid& newone, bool print_details=false) const;\n\n // transform a local cuboid to global cuboid Twc is camera pose. from camera to world\n ellipsoid transform_from(const SE3Quat& Twc) const;\n\n // transform a global cuboid to local cuboid Twc is camera pose. from camera to world\n ellipsoid transform_to(const SE3Quat& Twc) const;\n\n // xyz roll pitch yaw half_scale\n Vector9d toMinimalVector() const;\n\n // xyz quaternion, half_scale\n Vector10d toVector() const;\n\n Matrix4d similarityTransform() const;\n\n // Get the projected point of ellipsoid center on image plane\n Vector2d projectCenterIntoImagePoint(const SE3Quat& campose_cw, const Matrix3d& Kalib);\n\n // The ellipsoid is projected onto the image plane to get an ellipse\n Vector5d projectOntoImageEllipse(const SE3Quat& campose_cw, const Matrix3d& Kalib) const;\n\n // Get projection matrix P = K [ R | t ]\n Matrix3Xd generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const;\n\n // Get Q^*\n Matrix4d generateQuadric() const;\n\n // Get the bounding box from ellipse in image plane\n Vector4d getBoundingBoxFromEllipse(Vector5d &ellipse) const;\n\n // Get the projected bounding box in the image plane of the ellipsoid using a camera pose and a calibration matrix.\n Vector4d getBoundingBoxFromProjection(const SE3Quat& campose_cw, const Matrix3d& Kalib) const;\n\n // *** The following 4 functions treat the ellipsoid as external cube\n // 3x8 matrix storing 8 corners; each row is x y z\n Matrix3Xd compute3D_BoxCorner() const;\n\n Matrix2Xd projectOntoImageBoxCorner(const SE3Quat& campose_cw, const Matrix3d& Kalib) const;\n\n // get rectangles after projection [topleft, bottomright]\n Vector4d projectOntoImageRect(const SE3Quat& campose_cw, const Matrix3d& Kalib) const;\n\n // get rectangles after projection [center, width, height]\n Vector4d projectOntoImageBbox(const SE3Quat& campose_cw, const Matrix3d& Kalib) const;\n\n\n // calculate the IoU error with another ellipsoid\n double calculateMIoU(const g2o::ellipsoid& e) const;\n double calculateIntersectionOnZ(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const;\n double calculateArea(const g2o::ellipsoid& e) const;\n double calculateIntersectionOnXY(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const;\n double calculateIntersectionError(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const;\n\n void setColor(const Vector3d &color, double alpha = 1.0);\n Vector3d getColor();\n Vector4d getColorWithAlpha();\n bool isColorSet();\n\n // whether the camera could see the ellipsoid\n bool CheckObservability(const SE3Quat& campose_cw);\n\nprivate:\n bool mbColor;\n Vector4d mvColor;\n\n void UpdateValueFrom(const g2o::ellipsoid& e); // update the basic parameters from the given ellipsoid\n};\n\n} // g2o\n" }, { "path": "include/core/Frame.h", "content": "#ifndef ELLIPSOIDSLAM_FRAME_H\n#define ELLIPSOIDSLAM_FRAME_H\n\n#include \n#include \n#include \"Ellipsoid.h\"\n\nnamespace EllipsoidSLAM{\n \nclass SymmetryOutputData;\n\nclass Frame{\n\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Frame(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &im);\n Frame(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB);\n Frame(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB);\n\n int static total_frame;\n\n int frame_seq_id; // image topic sequence id, fixed\n cv::Mat frame_img; // depth img for processing\n cv::Mat rgb_img; // rgb img for visualization.\n cv::Mat ellipsoids_2d_img;\n\n double timestamp;\n\n g2o::VertexSE3Expmap* pose_vertex;\n\n g2o::SE3Quat cam_pose_Tcw;\t // optimized pose world to cam\n g2o::SE3Quat cam_pose_Twc;\t // optimized pose cam to world\n\n Eigen::MatrixXd mmObservations; // id x1 y1 x2 y2 label rate instanceID\n std::vector mvbOutlier;\n\n // For depth ellipsoid extraction.\n bool mbHaveLocalObject;\n std::vector mpLocalObjects; // local 3d ellipsoid\n};\n\n}\n#endif //ELLIPSOIDSLAM_FRAME_H\n" }, { "path": "include/core/FrameDrawer.h", "content": "#ifndef ELLIPSOIDSLAM_FRAMEDRAWER_H\n#define ELLIPSOIDSLAM_FRAMEDRAWER_H\n\n#include \"Map.h\"\n#include \"Tracking.h\"\n\nnamespace EllipsoidSLAM{\n\nclass Map;\nclass Tracking;\n\nclass FrameDrawer {\npublic:\n FrameDrawer(Map* pMap);\n\n void setTracker(Tracking* pTracker);\n\n cv::Mat drawFrame();\n cv::Mat drawFrameOnImage(cv::Mat &im);\n\n cv::Mat drawDepthFrame();\n\n cv::Mat getCurrentFrameImage();\n cv::Mat getCurrentDepthFrameImage();\n\nprivate:\n\n Map* mpMap;\n Tracking* mpTracking;\n\n cv::Mat drawProjectionOnImage(cv::Mat &im);\n cv::Mat drawObservationOnImage(cv::Mat &im);\n\n cv::Mat mmRGB;\n cv::Mat mmDepth;\n};\n\n}\n#endif //ELLIPSOIDSLAM_FRAMEDRAWER_H\n" }, { "path": "include/core/Geometry.h", "content": "#ifndef ELLIPSOIDSLAM_GEOMETRY_H\n#define ELLIPSOIDSLAM_GEOMETRY_H\n\n#include \n#include \n#include \n\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h\"\n\nusing namespace std;\nusing namespace Eigen;\n\nnamespace EllipsoidSLAM\n{\n struct PointXYZRGB\n {\n double x;\n double y;\n double z;\n unsigned char r; // 0-255\n unsigned char g;\n unsigned char b;\n int size = 1;\n };\n typedef vector PointCloud;\n\n struct camera_intrinsic{\n double fx;\n double fy;\n double cx;\n double cy;\n double scale;\n };\n\n PointCloud getPointCloud(cv::Mat &depth, cv::Mat &rgb, VectorXd &detect, camera_intrinsic &camera);\n PointCloud* transformPointCloud(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc); // generate a new cloud\n void transformPointCloudSelf(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc); // change the points of the origin cloud\n\n PointCloud loadPointsToPointVector(Eigen::MatrixXd &pMat);\n\n void SetPointCloudProperty(PointCloud* pCloud, uchar r, uchar g, uchar b, int size);\n\n Matrix3d getCalibFromCamera(camera_intrinsic &camera);\n\n void SavePointCloudToTxt(const string& path, PointCloud* pCloud);\n\n Vector3d GetPointcloudCenter(PointCloud* pCloud);\n Vector3d TransformPoint(Vector3d &point, const Eigen::Matrix4d &T);\n}\n\n#endif //ELLIPSOIDSLAM_GEOMETRY_H\n" }, { "path": "include/core/Initializer.h", "content": "#ifndef ELLIPSOIDSLAM_INITIALIZER_H\n#define ELLIPSOIDSLAM_INITIALIZER_H\n\n#include \"Ellipsoid.h\"\n#include \"Frame.h\"\n\n#include \n\nusing namespace Eigen;\nusing namespace g2o;\n\nnamespace EllipsoidSLAM\n{\n class Observation {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n int label;\n Vector4d bbox; // left-top x1 y1, right-down x2 y2\n double rate; // accuracy: 0 - 1.0\n Frame* pFrame; // which frame is the observation from\n\n int instance; // useless , for debugging\n };\n typedef std::vector Observations;\n\n class Observation3D {\n public:\n int label;\n g2o::ellipsoid* pObj;\n double rate; // prob: 0 - 1.0\n Frame* pFrame; \n };\n typedef std::vector Observation3Ds;\n\n class Initializer\n {\n\n public:\n Initializer(int rows, int cols); // image rows and cols\n\n /*\n * pose_mat: x y z qx qy qz qw\n * detection_mat: x1 y1 x2 y2 (accuracy)\n */\n g2o::ellipsoid initializeQuadric(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib);\n g2o::ellipsoid initializeQuadric(Observations &obs, Matrix3d &calib);\n\n // calculate the error between a given ellipsoid and planes\n double quadricErrorWithPlanes(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib, g2o::ellipsoid &e);\n\n // solve pose and scale from a matrix Q^* and construct an Ellipsoid class\n g2o::ellipsoid getEllipsoidFromQStar(Matrix4d &QStar);\n\n // get the initialization result.\n bool getInitializeResult();\n private:\n\n // get the homogeneous expressions of planes\n MatrixXd getPlanesHomo(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib);\n // get the vector form of plane constraints to prepare a linear equation system\n MatrixXd getVectorFromPlanesHomo(MatrixXd &planes);\n // solve a least square solution from SVD\n Matrix4d getQStarFromVectors(MatrixXd &planeVecs);\n\n Matrix3Xd generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const;\n MatrixXd fromDetectionsToLines(VectorXd &detection_mat);\n\n void getDetectionAndPoseMatFromObservations(EllipsoidSLAM::Observations &obs,\n MatrixXd &pose_mat, MatrixXd &detection_mat);\n\n // sort eigen values in ascending order.\n void sortEigenValues(VectorXcd &eigens, MatrixXcd &V);\n\n bool mbResult;\n\n int miImageRows, miImageCols;\n\n };\n}\n\n#endif //ELLIPSOIDSLAM_INITIALIZER_H\n" }, { "path": "include/core/Map.h", "content": "#ifndef ELLIPSOIDSLAM_MAP_H\n#define ELLIPSOIDSLAM_MAP_H\n\n#include \"Ellipsoid.h\"\n#include \"Geometry.h\"\n#include \"Plane.h\"\n#include \n#include \n\n#include \n\nusing namespace g2o;\n\nnamespace EllipsoidSLAM\n{\n class Map\n {\n public:\n Map();\n\n void addEllipsoid(ellipsoid* pObj);\n std::vector GetAllEllipsoids();\n\n void addPlane(plane* pPlane);\n std::vector GetAllPlanes();\n void clearPlanes();\n\n void setCameraState(g2o::SE3Quat* state);\n g2o::SE3Quat* getCameraState();\n\n void addCameraStateToTrajectory(g2o::SE3Quat* state);\n std::vector getCameraStateTrajectory();\n\n void addPoint(PointXYZRGB* pPoint);\n void addPointCloud(PointCloud* pPointCloud);\n void clearPointCloud();\n std::vector GetAllPoints();\n\n std::vector getEllipsoidsUsingLabel(int label);\n\n std::map GetAllEllipsoidsMap();\n\n bool AddPointCloudList(const string& name, PointCloud* pCloud, int type = 0); // type 0: replace when exist, type 1: add when exist\n bool DeletePointCloudList(const string& name, int type = 0); // type 0: complete matching, 1: partial matching\n bool ClearPointCloudLists();\n\n std::map GetPointCloudList();\n\n protected:\n std::vector mspEllipsoids;\n std::set mspPlanes;\n\n std::mutex mMutexMap;\n\n g2o::SE3Quat* mCameraState; // Twc\n std::vector mvCameraStates; // Twc camera in world\n\n std::set mspPoints; \n std::map mmPointCloudLists; // name-> pClouds\n\n public:\n // those visual ellipsoids are for visualization only and DO NOT join the optimization\n void addEllipsoidVisual(ellipsoid* pObj);\n std::vector GetAllEllipsoidsVisual();\n void ClearEllipsoidsVisual();\n\n protected:\n std::vector mspEllipsoidsVisual;\n\n };\n}\n\n#endif //ELLIPSOIDSLAM_MAP_H\n" }, { "path": "include/core/MapDrawer.h", "content": "#ifndef ELLIPSOIDSLAM_MAPDRAWER_H\n#define ELLIPSOIDSLAM_MAPDRAWER_H\n\n#include \"Map.h\"\n#include\n\n#include\n#include \n#include\n\nusing namespace std;\n\nnamespace EllipsoidSLAM{\n\nclass Map;\n\nclass MapDrawer\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n MapDrawer(const string &strSettingPath, Map* pMap);\n\n bool updateObjects();\n bool updateCameraState();\n\n\n bool drawObjects();\n bool drawCameraState();\n bool drawEllipsoids();\n\n bool drawPlanes();\n\n bool drawPoints();\n\n void setCalib(Eigen::Matrix3d& calib);\n\n bool drawTrajectory();\n\n void SE3ToOpenGLCameraMatrix(g2o::SE3Quat &matIn, pangolin::OpenGlMatrix &M); // inverse matIn\n void SE3ToOpenGLCameraMatrixOrigin(g2o::SE3Quat &matIn, pangolin::OpenGlMatrix &M); // don't inverse matIn\n void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M);\n\n void drawPointCloudLists(); // draw all the point cloud lists \n void drawPointCloudWithOptions(const std::map &options); // draw the point cloud lists with options opened\n\nprivate:\n\n float mKeyFrameSize;\n float mKeyFrameLineWidth;\n float mGraphLineWidth;\n float mPointSize;\n float mCameraSize;\n float mCameraLineWidth;\n\n cv::Mat mCameraPose;\n\n std::mutex mMutexCamera;\n\n Map* mpMap;\n\n Eigen::Matrix3d mCalib; \n\n void drawPlaneWithEquation(plane* p);\n\n pangolin::OpenGlMatrix getGLMatrixFromCenterAndNormal(Vector3f& center, Vector3f& normal);\n};\n}\n\n#endif //ELLIPSOIDSLAM_MAPDRAWER_H\n" }, { "path": "include/core/Optimizer.h", "content": "#ifndef ELLIPSOIDSLAM_OPTIMIZER_H\n#define ELLIPSOIDSLAM_OPTIMIZER_H\n\n#include \"Frame.h\"\n#include \"Map.h\"\n#include \"Initializer.h\"\n\n#include \"src/symmetry/Symmetry.h\"\n\nnamespace EllipsoidSLAM {\n class Frame;\n class Map;\n class Optimizer {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Optimizer();\n\n void GlobalObjectGraphOptimization(std::vector &pFrames, Map *pMap,\n int rows, int cols, Matrix3d &mCalib, std::map& objectObservations,\n bool save_graph = false, bool withAssociation = false, bool check_visibility = false );\n\n void SetGroundPlane(Vector4d& normal);\n\n private:\n std::map> mMapObjectConstrain;\n\n bool mbGroundPlaneSet;\n Vector4d mGroundPlaneNormal;\n };\n\n}\n\n#endif //ELLIPSOIDSLAM_OPTIMIZER_H\n" }, { "path": "include/core/Plane.h", "content": "#pragma once\n\n#include \n#include \n#include \n\n#include \"include/utils/matrix_utils.h\"\n\n#include \"Ellipsoid.h\"\n\nnamespace g2o\n{\nclass plane {\n\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n plane();\n\n plane(Vector4d param, Vector3d color=Vector3d(1.0,0.0,0.0));\n\n // Copy constructor.\n plane(const plane &p);\n\n const plane& operator=(const plane& p);\n\n // initialize a plane from a point and a normal vector\n void fromPointAndNormal(const Vector3d &point, const Vector3d &normal);\n\n void fromDisAndAngle(double dis, double angle);\n void fromDisAngleTrans(double dis, double angle, double trans);\n\n // update plane parameters in 3 degrees\n plane exp_update(const Vector3d& update);\n \n // update plane parameters in 2 degrees\n plane exp_update2DOF(const Vector2d& update);\n\n // distance from a point to plane\n double distanceToPoint(const Vector3d& point, bool keep_flag = false);\n void transform(g2o::SE3Quat& Twc);\n\n // init visualization for a finite plane given a point and a size\n void InitFinitePlane(const Vector3d& center, double size);\n\n // update function for optimization\n inline void oplus(const Vector3d& v){\n //construct a normal from azimuth and evelation;\n double _azimuth=v[0];\n double _elevation=v[1];\n double s=std::sin(_elevation), c=std::cos(_elevation);\n Vector3d n (c*std::cos(_azimuth), c*std::sin(_azimuth), s) ;\n \n // rotate the normal\n Matrix3d R=rotation(normal());\n double d=distance()+v[2]; // why is plus?\n param.head<3>() = R*n;\n param(3) = -d;\n normalize(param);\n }\n\n // update function for optimization\n inline void oplus_dual(const Vector3d& v){\n //construct a normal from azimuth and evelation;\n double _azimuth=v[0];\n double _elevation=0;\n double s=std::sin(_elevation), c=std::cos(_elevation);\n Vector3d n (c*std::cos(_azimuth), c*std::sin(_azimuth), s) ;\n \n // rotate the normal\n Matrix3d R=rotation(normal());\n double d=distance()+v[1]; // why is plus?\n param.head<3>() = R*n;\n param(3) = -d;\n normalize(param);\n\n mdDualDis += v[2];\n }\n\n static inline void normalize(Vector4d& coeffs) {\n double n=coeffs.head<3>().norm();\n coeffs = coeffs * (1./n);\n }\n\n Vector3d normal() const {\n return param.head<3>();\n }\n\n static Matrix3d rotation(const Vector3d& v) {\n double _azimuth = azimuth(v);\n double _elevation = elevation(v); \n return (AngleAxisd(_azimuth, Vector3d::UnitZ())* AngleAxisd(- _elevation, Vector3d::UnitY())).toRotationMatrix();\n }\n\n // self\n double azimuth() const {\n return atan2(param[1], param[0]);\n }\n\n static double azimuth(const Vector3d& v) {\n return std::atan2(v(1),v(0));\n }\n\n static double elevation(const Vector3d& v) {\n return std::atan2(v(2), v.head<2>().norm());\n }\n\n double distance() const {\n return -param(3);\n }\n\n Eigen::Vector4d GeneratePlaneVec();\n Eigen::Vector4d GenerateAnotherPlaneVec();\n\n // finite plane parameters; treat as a square\n double mdPlaneSize; // side length (meter)\n bool mbLimited;\n\n // dual plane parameter\n double mdDualDis;\n\n Vector4d param; // A B C : AX+BY+CZ+D=0\n Vector3d color; // r g b , [0,1.0]\n Vector3d mvPlaneCenter; // the center of the square. roughly defined. \nprivate:\n\n Eigen::Vector3d GetLineFromCenterAngle(const Eigen::Vector2d center, double angle);\n Eigen::Vector4d LineToPlane(const Eigen::Vector3d line);\n\n};\n\n\n} // namespace g2o\n\n" }, { "path": "include/core/System.h", "content": "#ifndef ELLIPSOIDSLAM_SYSTEM_H\n#define ELLIPSOIDSLAM_SYSTEM_H\n\n#include \"Frame.h\"\n#include \"Map.h\"\n#include \"MapDrawer.h\"\n#include \"Viewer.h\"\n#include \"Tracking.h\"\n#include \"FrameDrawer.h\"\n\n#include \n#include \n#include \n\nnamespace EllipsoidSLAM{\n\nclass Viewer;\nclass Frame;\nclass Map;\nclass Tracking;\nclass FrameDrawer;\n\nclass System {\n\n\npublic:\n System(const string &strSettingsFile, const bool bUseViewer = true);\n\n // Interface.\n // timestamp: sec\n // pose: camera pose in the world coordinate ; x y z qx qy qz qw\n // imDepth: depth image; CV16UC1\n // imRGB: rgb image for visualization; CV8UC3, BGR\n // bboxMat: id x1 y1 x2 y2 label prob [Instance]\n // withAssociation: \n // true, use [instance] as association result; false, use single-frame ellipsoid estimation to solve data association\n bool TrackWithObjects(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd & bboxMat, const cv::Mat &imDepth, const cv::Mat &imRGB = cv::Mat(),\n bool withAssociation = false);\n\n Map* getMap();\n MapDrawer* getMapDrawer();\n FrameDrawer* getFrameDrawer();\n Viewer* getViewer();\n Tracking* getTracker();\n\n // save objects to file\n void SaveObjectsToFile(string &path);\n\n void OpenDepthEllipsoid();\n\n void OpenOptimization();\n void CloseOptimization();\n\nprivate:\n\n Map* mpMap;\n\n Viewer* mpViewer;\n\n MapDrawer* mpMapDrawer;\n\n Tracking* mpTracker;\n FrameDrawer* mpFrameDrawer;\n\n std::thread* mptViewer;\n\n};\n\n} // namespace EllipsoidSLAM\n\n#endif //ELLIPSOIDSLAM_SYSTEM_H\n" }, { "path": "include/core/Tracking.h", "content": "#ifndef ELLIPSOIDSLAM_TRACKING_H\n#define ELLIPSOIDSLAM_TRACKING_H\n\n#include \"System.h\"\n#include \"FrameDrawer.h\"\n#include \"Viewer.h\"\n#include \"Map.h\"\n#include \"MapDrawer.h\"\n#include \"Initializer.h\"\n#include \"Optimizer.h\"\n\n#include \n#include \n#include \n#include \"DataAssociation.h\"\n#include \n\nnamespace EllipsoidSLAM{\n\nclass System;\nclass FrameDrawer;\nclass MapDrawer;\nclass Map;\nclass Viewer;\nclass Frame;\nclass Initializer;\nclass Optimizer;\nclass Symmetry;\n\nclass Tracking {\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Tracking(System* pSys, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,\n const string &strSettingPath);\n\n bool GrabPoseAndSingleObjectAnnotation(const Eigen::VectorXd &pose, const Eigen::VectorXd &detection);\n 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);\n bool GrabPoseAndObjects(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB = cv::Mat(), bool withAssociation = false);\n\n Frame* mCurrFrame;\n\n Eigen::Matrix3d mCalib;\n\n void outputBboxMatWithAssociation();\n\n void SaveObjectHistory(const string& path);\n\n void OpenOptimization();\n void CloseOptimization();\n\n bool SavePointCloudMap(const string& path);\n\n std::vector checkKeyFrameForInstances(std::vector& associations);\n\n // Single-frame Ellipsoid Extraction\n void OpenDepthEllipsoid();\n\n // Groundplane Estimation\n void OpenGroundPlaneEstimation();\n void CloseGroundPlaneEstimation();\n int GetGroundPlaneEstimationState();\n\nprivate:\n\n void ProcessCurrentFrame(bool withAssociation);\n\n void UpdateObjectObservation(Frame* pFrame, bool withAssociation = false);\n\n void JudgeInitialization();\n\n bool isKeyFrameForVisualization(); \n\n void ProcessVisualization();\n\n void RefreshObjectHistory();\n void ProcessGroundPlaneEstimation();\n void Update3DObservationDataAssociation(EllipsoidSLAM::Frame* pFrame, std::vector& associations, std::vector& KeyFrameChecks);\n void UpdateDepthEllipsoidEstimation(EllipsoidSLAM::Frame* pFrame, bool withAssociation);\n\n std::vector GetMannualAssociation(Eigen::MatrixXd &obsMat);\n\nprotected:\n\n g2o::ellipsoid* getObjectDataAssociation(const Eigen::VectorXd &pose, const Eigen::VectorXd &detection);\n\n // System\n System* mpSystem;\n\n //Drawers\n Viewer* mpViewer;\n FrameDrawer* mpFrameDrawer;\n MapDrawer* mpMapDrawer;\n Initializer* mpInitializer;\n\n //Map\n Map* mpMap;\n\n // Optimizer\n Optimizer* mpOptimizer;\n\n //Calibration matrix\n cv::Mat mK;\n cv::Mat mDistCoef;\n float mbf;\n camera_intrinsic mCamera;\n\n int mRows, mCols;\n\n std::vector mvpFrames;\n\n // Store observations in a map with instance id.\n // In the future, storing observations under Ellipsoid class separately would make it clearer.\n std::map mmObjectObservations;\n\n Builder* mpBuilder; // a dense pointcloud builder from visualization\n\n std::map mmObjectHistory;\n\n bool mbOpenOptimization;\n\n bool mbDepthEllipsoidOpened;\n EllipsoidExtractor* mpEllipsoidExtractor;\n std::map mmObjectObservations3D; // 3d observations indexed by instance ID\n\n DataAssociationSolver* pDASolver;\n\n int miGroundPlaneState; // 0: Closed 1: estimating 2: estimated\n g2o::plane mGroundPlane;\n PlaneExtractor* pPlaneExtractor;\n};\n\n}\n\n#endif //ELLIPSOIDSLAM_TRACKING_H\n" }, { "path": "include/core/Viewer.h", "content": "#ifndef ELLIPSOIDSLAM_VIEWER_H\n#define ELLIPSOIDSLAM_VIEWER_H\n\n#include \n#include \n\n#include \"MapDrawer.h\"\n#include \"System.h\"\n#include \"FrameDrawer.h\"\n#include \n\n\nusing namespace std;\n\nstruct MenuStruct\n{\n double min;\n double max;\n double def;\n string name;\n};\n\nnamespace EllipsoidSLAM\n{\n class MapDrawer;\n class System;\n class FrameDrawer;\n\n class Viewer\n {\n public:\n Viewer(const string &strSettingPath, MapDrawer* pMapDrawer);\n Viewer(System* pSystem, const string &strSettingPath, MapDrawer* pMapDrawer);\n\n void SetFrameDrawer(FrameDrawer* p);\n\n void run();\n\n bool updateObject();\n bool updateCamera();\n\n void RequestFinish();\n bool isFinished();\n\n // dynamic menu.\n int addDoubleMenu(string name, double min, double max, double def);\n bool getValueDoubleMenu(int id, double &value);\n\n private:\n float mViewpointX, mViewpointY, mViewpointZ, mViewpointF;\n\n bool CheckFinish();\n void SetFinish();\n bool mbFinishRequested;\n bool mbFinished;\n std::mutex mMutexFinish;\n\n MapDrawer* mpMapDrawer;\n System* mpSystem;\n FrameDrawer* mpFrameDrawer;\n\n // menu lists\n vector*> mvDoubleMenus;\n vector mvMenuStruct;\n\n // Manual control of the point cloud visualization\n map*> mmPointCloudOptionMenus;\n\n std::map mmPointCloudOptionMap;\n void RefreshPointCloudOptions();\n \n void RefreshMenu();\n\n int miRows;\n int miCols;\n\n\n };\n\n\n}\n\n#endif //ELLIPSOIDSLAM_VIEWER_H\n" }, { "path": "include/utils/dataprocess_utils.h", "content": "#ifndef ELLIPSOIDSLAM_DATAPROCESS_UTILS_H\n#define ELLIPSOIDSLAM_DATAPROCESS_UTILS_H\n\n// Eigen\n#include \n#include \n\n#include \n#include \n\nusing namespace std;\n\n// read data from a txt file and store as an eigen matrix\nEigen::MatrixXd readDataFromFile(const char* fileName, bool dropFirstline = false);\n\nstd::vector> readStringFromFile(const char* fileName, int dropLines = 0);\n\n// save eigen matrix data to a text file\nbool saveMatToFile(Eigen::MatrixXd &matIn, const char* fileName);\n\n// get all the file names under a dir. \nvoid GetFileNamesUnderDir(string path,vector& filenames);\nstring splitFileNameFromFullDir(string &s, bool bare = false);\n\n// sort file names in ascending order \nvoid sortFileNames(vector& filenames, vector& filenamesSorted);\n\nbool calibrateMeasurement(Eigen::Vector4d &measure , int rows, int cols, int config_boarder = 10, int config_size = 100);\n\n#endif //ELLIPSOIDSLAM_DATAPROCESS_UTILS_H\n" }, { "path": "include/utils/matrix_utils.h", "content": "#ifndef ELLIPSOIDSLAM_MATRIX_UTILS_H\n#define ELLIPSOIDSLAM_MATRIX_UTILS_H\n\n#include \n// Eigen\n#include \n#include \n\nEigen::Matrix4d getTransformFromVector(Eigen::VectorXd& pose);\nvoid addVecToMatirx(Eigen::MatrixXd &mat, Eigen::VectorXd &vec);\n\ntemplate \nEigen::Quaternion zyx_euler_to_quat(const T &roll, const T &pitch, const T &yaw);\n\ntemplate \nvoid quat_to_euler_zyx(const Eigen::Quaternion& q, T& roll, T& pitch, T& yaw);\n\ntemplate \nvoid rot_to_euler_zyx(const Eigen::Matrix& R, T& roll, T& pitch, T& yaw);\n\ntemplate \nEigen::Matrix euler_zyx_to_rot(const T& roll,const T& pitch,const T& yaw);\n\n// input is 3*n (or 2*n) output is 4*n (or 3*n)\ntemplate \nEigen::Matrix real_to_homo_coord(const Eigen::Matrix& pts_in);\ntemplate \nvoid real_to_homo_coord(const Eigen::Matrix& pts_in, Eigen::Matrix& pts_homo_out);\ntemplate // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function.\nEigen::Matrix real_to_homo_coord_vec(const Eigen::Matrix& pts_in);\n\n\n// input is 3*n (or 4*n) output is 2*n(or 3*n)\ntemplate \nEigen::Matrix homo_to_real_coord(const Eigen::Matrix& pts_homo_in);\ntemplate \nvoid homo_to_real_coord(const Eigen::Matrix& pts_homo_in, Eigen::Matrix& pts_out);\n\ntemplate // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function.\nEigen::Matrix homo_to_real_coord_vec(const Eigen::Matrix& pts_homo_in);\n\n\n//vertically stack a matrix to a matrix, increase rows, keep columns\nvoid vert_stack_m(const Eigen::MatrixXd &a_in, const Eigen::MatrixXd &b_in, Eigen::MatrixXd &combined_out);\nvoid vert_stack_m_self(Eigen::MatrixXf &a_in, const Eigen::MatrixXf &b_in);\n\nvoid fast_RemoveRow(Eigen::MatrixXd& matrix,int rowToRemove, int& total_line_number);\n\n// make sure column size is given. not check here. row will be adjusted automatically. if more cols given, will be zero.\ntemplate \nbool read_all_number_txt(const std::string txt_file_name, Eigen::Matrix& read_number_mat);\n\n// each line: one string, several numbers. make sure column size is correct.\nbool read_obj_detection_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& strings);\n// each line several numbers then one string . make sure column size is correct.\nbool read_obj_detection2_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& strings);\n\n// (partial) sort a vector, only need top_K element, result is stored in idx by default increasing\nvoid sort_indexes(const Eigen::VectorXd &vec, std::vector& idx, int top_k );\nvoid sort_indexes(const Eigen::VectorXd &vec, std::vector& idx);\n\n// change [-180 180] to [-90 90] by +-90\ntemplate \nT normalize_to_pi(T angle);\n\ntemplate \nvoid print_vector(const std::vector& vec);\n\n\n//TODO could be replaced by eigen linespace\ntemplate \nvoid linespace(T starting, T ending, T step, std::vector& res);\n\n\n#endif //ELLIPSOIDSLAM_MATRIX_UTILS_H\n" }, { "path": "install_g2o.sh", "content": "#!/bin/bash\n\ncd Thirdparty/g2o\nmkdir build\ncd build\ncmake .. -DCMAKE_BUILD_TYPE=Release\nmake -j2\n" }, { "path": "src/Polygon/CMakeLists.txt", "content": "add_library(Polygon SHARED\n Polygon.cpp)\ntarget_link_libraries(Polygon ${OpenCV_LIBS})\n" }, { "path": "src/Polygon/Polygon.cpp", "content": "#include \"Polygon.hpp\"\n\nnamespace EllipsoidSLAM\n{\nfloat distPoint(cv::Point2d v,cv::Point2d w) { \n\treturn sqrtf((v.x - w.x)*(v.x - w.x) + (v.y - w.y)*(v.y - w.y)) ;\n}\n\nbool segementIntersection(cv::Point2d p0,cv::Point2d p1,cv::Point2d p2,cv::Point2d p3,cv::Point2d * intersection) {\n\tcv::Point2d s1, s2;\n\ts1 = cv::Point2d(p1.x - p0.x, p1.y - p0.y);\n\ts2 = cv::Point2d(p3.x - p2.x, p3.y - p2.y);\n\n\tfloat s10_x = p1.x - p0.x;\n\tfloat s10_y = p1.y - p0.y;\n\tfloat s32_x = p3.x - p2.x;\n\tfloat s32_y = p3.y - p2.y;\n\tfloat denom = s10_x * s32_y - s32_x * s10_y;\n\n\tif(denom == 0) {\n\t\treturn false;\n\t}\n\n\tbool denom_positive = denom > 0;\n\n\tfloat s02_x = p0.x - p2.x;\n\tfloat s02_y = p0.y - p2.y;\n\tfloat s_numer = s10_x * s02_y - s10_y * s02_x;\n\n\tif((s_numer < 0.f) == denom_positive) {\n\t\treturn false;\n\t}\n\n\tfloat t_numer = s32_x * s02_y - s32_y * s02_x;\n\tif((t_numer < 0) == denom_positive) {\n\t\treturn false;\n\t}\n\n\tif((s_numer > denom) == denom_positive || (t_numer > denom) == denom_positive) {\n\t\treturn false;\n\t}\n\n\tfloat t = t_numer / denom;\n\n\t*intersection = cv::Point2d(p0.x + (t * s10_x), p0.y + (t * s10_y) );\n\treturn true;\n}\n\nbool pointInPolygon(cv::Point2d p,const cv::Point2d * points,int n) {\n\tint i, j ;\n\tbool c = false;\n\tfor(i = 0, j = n - 1; i < n; j = i++) {\n\t\tif( ( (points[i].y >= p.y ) != (points[j].y >= p.y) ) &&\n\t\t\t(p.x <= (points[j].x - points[i].x) * (p.y - points[i].y) / (points[j].y - points[i].y) + points[i].x)\n\t\t\t)\n\t\t\tc = !c;\n\t}\n\treturn c;\n}\n\n\nfloat computeArea(const cv::Point2d * pt,int n ) {\n\tfloat area0 = 0.f;\n\tfor (int i = 0 ; i < n ; i++ ) {\n\t\tint j = (i+1)%n;\n\t\tarea0 += pt[i].x * pt[j].y;\n\t\tarea0 -= pt[i].y * pt[j].x;\n\t}\n\treturn 0.5f * fabs(area0);\n}\n\nstruct PointAngle{\n\tcv::Point2d p;\n\tfloat angle;\n};\n\ncv::Point2d Polygon::getCenter() const {\n\tcv::Point2d center;\n\tcenter.x = 0;\n\tcenter.y = 0;\n\tfor (int i = 0 ; i < n ; i++ ) {\n\t\tcenter.x += pt[i].x;\n\t\tcenter.y += pt[i].y;\n\t}\n\tcenter.x /= n;\n\tcenter.y /= n;\n\treturn center;\n}\n\nstatic int comp_point_with_angle(const void * a, const void * b) {\n\tif ( ((PointAngle*)a)->angle < ((PointAngle*)b)->angle ) return -1;\n\telse if ( ((PointAngle*)a)->angle > ((PointAngle*)b)->angle ) return 1;\n\telse //if ( ((PointAngle*)a)->angle == ((PointAngle*)b)->angle ) return 0;\n\t\treturn 0;\n}\n\nfloat Polygon::area() const {\n\treturn computeArea(pt,n);\n}\n\nvoid Polygon::pointsOrdered() {\n\tif( n <= 0) return;\n\tcv::Point2d center = getCenter();\n\tPointAngle pc[MAX_POINT_POLYGON];\n\tfor (int i = 0 ; i < n ; i++ ) {\n\t\tpc[i].p.x = pt[i].x;\n\t\tpc[i].p.y = pt[i].y;\n\t\tpc[i].angle = atan2f((float)(pt[i].y - center.y),(float)(pt[i].x - center.x));\n\t}\n\tqsort(pc,n,sizeof(PointAngle),comp_point_with_angle);\n\tfor (int i = 0 ; i < n ; i++ ) {\n\t\tpt[i].x = pc[i].p.x;\n\t\tpt[i].y = pc[i].p.y;\n\t}\n}\n\nbool Polygon::pointIsInPolygon(cv::Point2d p) const {\n\treturn pointInPolygon(p,pt,n);\n}\n\nvoid intersectPolygon( const cv::Point2d * poly0, int n0,const cv::Point2d * poly1,int n1, Polygon & inter ) {\n\tinter.clear();\n\tfor (int i = 0 ; i < n0 ;i++) {\n\t\tif( pointInPolygon(poly0[i],poly1,n1) ) {\n\t\t\tinter.add(poly0[i]);\n\t\t}\n\t}\n\n\tfor (int i = 0 ; i < n1 ;i++) { \n\t\tif( pointInPolygon(poly1[i],poly0,n0) ) \n\t\t\tinter.add(poly1[i]);\n\t}\n\n\tfor (int i = 0 ; i < n0 ;i++) {\n\t\tcv::Point2d p0,p1,p2,p3;\n\t\tp0 = poly0[i];\n\t\tp1 = poly0[(i+1)%n0];\n\t\tfor (int j = 0 ; j < n1 ;j++) {\n\t\t\tp2 = poly1[j];\n\t\t\tp3 = poly1[(j+1)%n1];\n\t\t\tcv::Point2d pinter;\n\t\t\tif(segementIntersection(p0,p1,p2,p3,&pinter)) {\n\t\t\t\tinter.add(pinter);\n\t\t\t}\n\t\t}\n\t}\n\tinter.pointsOrdered();\n}\n\nvoid intersectPolygon( const Polygon & poly0, const Polygon & poly1, Polygon & inter ) {\n\tintersectPolygon(&poly0[0],poly0.size(),&poly1[0],poly1.size(),inter);\n}\n\n//------------------------------------------------------------------------------------------------------------\n// SHPC : Sutherland-Hodgeman-Polygon-Clipping Algorihtm\n//------------------------------------------------------------------------------------------------------------\nstatic inline int cross(const cv::Point2d* a,const cv::Point2d* b) {\n\treturn a->x * b->y - a->y * b->x;\n}\n\nstatic inline cv::Point2d* vsub(const cv::Point2d* a,const cv::Point2d* b, cv::Point2d* res) {\n\tres->x = a->x - b->x;\n\tres->y = a->y - b->y;\n\treturn res;\n}\n\nstatic int line_sect(const cv::Point2d* x0,const cv::Point2d* x1,const cv::Point2d* y0,const cv::Point2d* y1, cv::Point2d* res) {\n\tcv::Point2d dx, dy, d;\n\tvsub(x1, x0, &dx);\n\tvsub(y1, y0, &dy);\n\tvsub(x0, y0, &d);\n\tfloat dyx = (float)cross(&dy, &dx);\n\tif (!dyx) return 0;\n\tdyx = cross(&d, &dx) / dyx;\n\tif (dyx <= 0 || dyx >= 1) return 0;\n\tres->x = int(y0->x + dyx * dy.x);\n\tres->y = int(y0->y + dyx * dy.y);\n\treturn 1;\n}\n\nstatic int left_of(const cv::Point2d* a,const cv::Point2d* b,const cv::Point2d* c) {\n\tcv::Point2d tmp1, tmp2;\n\tint x;\n\tvsub(b, a, &tmp1);\n\tvsub(c, b, &tmp2);\n\tx = cross(&tmp1, &tmp2);\n\treturn x < 0 ? -1 : x > 0;\n}\n\nstatic void poly_edge_clip(const Polygon* sub,const cv::Point2d* x0,const cv::Point2d* x1, int left, Polygon* res) {\n\tint i, side0, side1;\n\tcv::Point2d tmp;\n\tconst cv::Point2d* v0 = sub->pt+ sub->n - 1;\n\tconst cv::Point2d* v1;\n\tres->clear();\n\n\tside0 = left_of(x0, x1, v0);\n\tif (side0 != -left) res->add(*v0);\n\n\tfor (i = 0; i < sub->n; i++) {\n\t\tv1 = sub->pt + i;\n\t\tside1 = left_of(x0, x1, v1);\n\t\tif (side0 + side1 == 0 && side0)\n\t\t\t/* last point and current straddle the edge */\n\t\t\tif (line_sect(x0, x1, v0, v1, &tmp))\n\t\t\t\tres->add(tmp);\n\t\tif (i == sub->n - 1) break;\n\t\tif (side1 != -left) res->add(*v1);\n\t\tv0 = v1;\n\t\tside0 = side1;\n\t}\n}\n\nstatic int poly_winding(const Polygon* p) {\n\treturn left_of(p->pt, p->pt + 1, p->pt + 2);\n}\n\nvoid intersectPolygonSHPC(const Polygon * sub,const Polygon* clip,Polygon* res) {\n\tint i;\n\tPolygon P1,P2;\n\tPolygon * p1 = &P1;\n\tPolygon * p2 = &P2;\n\tPolygon * tmp ;\n\n\tint dir = poly_winding(clip);\n\tpoly_edge_clip(sub, clip->pt + clip->n - 1, clip->pt, dir, p2);\n\tfor (i = 0; i < clip->n - 1; i++) {\n\t\ttmp = p2; p2 = p1; p1 = tmp;\n\t\tif(p1->n == 0) {\n\t\t\tp2->n = 0;\n\t\t\tbreak;\n\t\t}\n\t\tpoly_edge_clip(p1, clip->pt + i, clip->pt + i + 1, dir, p2);\n\t}\n\tres->clear();\n\tfor (i = 0 ; i < p2->n ; i++) res->add(p2->pt[i]);\n}\n\nvoid intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Polygon& res) {\n\tintersectPolygonSHPC(&sub,&clip,&res);\n}\n}" }, { "path": "src/Polygon/Polygon.hpp", "content": "/*\n*\tThis file is dependent on github open-source. All rights are perserved by original authors.\n* \tWeb: https://github.com/abreheret/polygon-intersection\n*/\n// Update: Add the API for EllipsoidSLAM.\n\n#ifndef __EllipsoidSLAM_POLYGON_HPP__\n#define __EllipsoidSLAM_POLYGON_HPP__\n\n// #include // not recoginized by OpenCV 4.x\n#include \n\nnamespace EllipsoidSLAM\n{\nfloat distPoint( cv::Point2d p1, cv::Point2d p2 ) ;\nbool segementIntersection(cv::Point2d p0_seg0,cv::Point2d p1_seg0,cv::Point2d p0_seg1,cv::Point2d p1_seg1,cv::Point2d * intersection) ;\n\nbool pointInPolygon(cv::Point2d p,const cv::Point2d * points,int n) ;\n\n\n#define MAX_POINT_POLYGON 64\nstruct Polygon {\n\tcv::Point2d pt[MAX_POINT_POLYGON];\n\tint n;\n\n\tPolygon(int n_ = 0 ) { assert(n_>= 0 && n_ < MAX_POINT_POLYGON); n = n_;}\n\tvirtual ~Polygon() {}\n\n\tvoid clear() { n = 0; }\n\tvoid add(const cv::Point2d &p) {if(n < MAX_POINT_POLYGON) pt[n++] = p;}\n\tvoid push_back(const cv::Point2d &p) {add(p);}\n\tint size() const { return n;}\n\tcv::Point2d getCenter() const ;\n\tconst cv::Point2d & operator[] (int index) const { assert(index >= 0 && index < n); return pt[index];}\n\tcv::Point2d& operator[] (int index) { assert(index >= 0 && index < n); return pt[index]; }\n\tvoid pointsOrdered() ;\n\tfloat area() const ;\n\tbool pointIsInPolygon(cv::Point2d p) const ;\n};\n\n\nvoid intersectPolygon( const cv::Point2d * poly0, int n0,const cv::Point2d * poly1,int n1, Polygon & inter ) ;\nvoid intersectPolygon( const Polygon & poly0, const Polygon & poly1, Polygon & inter ) ;\nvoid intersectPolygonSHPC(const Polygon * sub,const Polygon* clip,Polygon* res) ;\nvoid intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Polygon& res) ;\n\n}\n\n#endif //" }, { "path": "src/config/CMakeLists.txt", "content": "\nadd_library(Config SHARED\n Config.cpp\n)\n\ntarget_link_libraries(Config\n ${OpenCV_LIBS}\n )\n" }, { "path": "src/config/Config.cpp", "content": "\n#include \"Config.h\"\n\nnamespace EllipsoidSLAM{\n void Config::SetParameterFile( const std::string& filename )\n {\n if ( mConfig == nullptr )\n mConfig = shared_ptr(new Config);\n mConfig->mFile.open( filename.c_str(), cv::FileStorage::READ );\n if ( !mConfig->mFile.isOpened())\n {\n std::cerr<<\"parameter file \"<< filename <<\" does not exist.\"<mFile.release();\n return;\n }\n }\n\n Config::~Config()\n {\n if ( mFile.isOpened() )\n mFile.release();\n }\n\n void Config::Init(){\n if ( mConfig == nullptr ){\n mConfig = shared_ptr(new Config);\n\n // ************ default parameters here *************\n mConfig->SetValue(\"Tracking_MINIMUM_INITIALIZATION_FRAME\", 15); // The minimum frame number required for ellipsoid initialization using 2d observations.\n mConfig->SetValue(\"EllipsoidExtractor_DEPTH_RANGE\", 6); // the valid depth range (m)\n\n }\n }\n\n shared_ptr Config::mConfig = nullptr;\n}" }, { "path": "src/config/Config.h", "content": "// A parameter file supporting global dynamic changes\n\n#ifndef ELLIPSOIDSLAM_CONFIG_H\n#define ELLIPSOIDSLAM_CONFIG_H\n\n#include \n#include \n#include \n#include \n\nusing namespace std;\n\nnamespace EllipsoidSLAM {\n class Config{\n public:\n static void SetParameterFile( const string& filename );\n\n // Get function reads value from parameter file (.yaml)\n template \n static T Get(const string& key){\n return T(Config::mConfig->mFile[key]);\n }\n\n template \n static void Set(const string& key, T value){\n Config::mConfig->mFile << key << value; // tobe test\n }\n\n ~Config();\n\n static void Init();\n\n // set the parameter value, if do not exist, create a new one\n template \n static void SetValue(const string& key, T value)\n {\n auto pos = Config::mConfig->mKeyValueMap.find(key);\n if( pos == Config::mConfig->mKeyValueMap.end() ) // do not exist\n {\n Config::mConfig->mKeyValueMap.insert(make_pair(key, double(value)));\n }\n else\n {\n pos->second = double(value);\n } \n }\n\n // ReadValue function reads parameter value set by SetValue function, and return default value if it does not exist\n template \n static T ReadValue(const string& key, T default_value = 0)\n {\n auto pos = Config::mConfig->mKeyValueMap.find(key);\n if( pos == Config::mConfig->mKeyValueMap.end() ) // do not exist\n {\n return Get(key); // return value in the parameter file\n }\n else\n {\n return T(pos->second);\n } \n }\n\n private:\n Config(){};\n static std::shared_ptr mConfig;\n cv::FileStorage mFile;\n\n std::map mKeyValueMap;\n\n };\n}\n#endif //ELLIPSOIDSLAM_CONFIG_H" }, { "path": "src/core/BasicEllipsoidEdges.cpp", "content": "#include \"include/core/BasicEllipsoidEdges.h\"\n\nnamespace g2o\n{\n// ---- VertexEllipsoid\n void VertexEllipsoid::setToOriginImpl() { _estimate = ellipsoid(); }\n\n void VertexEllipsoid::oplusImpl(const double* update_) {\n Eigen::Map update(update_);\n setEstimate(_estimate.exp_update(update));\n }\n\n bool VertexEllipsoid::read(std::istream& is) {\n Vector9d est;\n for (int i=0; i<9; i++)\n is >> est[i];\n ellipsoid Onecube;\n Onecube.fromMinimalVector(est);\n setEstimate(Onecube);\n return true;\n }\n\n bool VertexEllipsoid::write(std::ostream& os) const {\n Vector9d lv=_estimate.toMinimalVector();\n for (int i=0; i update(update_);\n setEstimate(_estimate.exp_update_XYZABC(update));\n }\n\n bool VertexEllipsoidXYZABC::read(std::istream& is) {\n Vector9d est;\n for (int i=0; i<9; i++)\n is >> est[i];\n ellipsoid Onecube;\n Onecube.fromMinimalVector(est);\n setEstimate(Onecube);\n return true;\n }\n\n bool VertexEllipsoidXYZABC::write(std::ostream& os) const {\n Vector9d lv=_estimate.toMinimalVector();\n for (int i=0; i(_vertices[0]); // world to camera pose\n const VertexEllipsoid* cuboidVertex = static_cast(_vertices[1]); // object pose to world\n\n SE3Quat cam_pose_Twc = SE3Vertex->estimate().inverse();\n ellipsoid global_cube = cuboidVertex->estimate();\n ellipsoid esti_global_cube = _measurement.transform_from(cam_pose_Twc);\n _error = global_cube.min_log_error_9dof(esti_global_cube);\n }\n\n\n// EdgeSE3EllipsoidProj\n bool EdgeSE3EllipsoidProj::read(std::istream& is){\n return true;\n };\n\n bool EdgeSE3EllipsoidProj::write(std::ostream& os) const\n {\n return os.good();\n };\n\n Vector4d EdgeSE3EllipsoidProj::getProject(){\n const VertexSE3Expmap* SE3Vertex = static_cast(_vertices[0]); // world to camera pose\n const VertexEllipsoid* cuboidVertex = static_cast(_vertices[1]); // object pose to world\n\n SE3Quat cam_pose_Tcw = SE3Vertex->estimate();\n ellipsoid global_cube = cuboidVertex->estimate();\n\n Vector4d proj = global_cube.getBoundingBoxFromProjection(cam_pose_Tcw, Kalib); // topleft, rightdown\n\n return proj;\n }\n\n void EdgeSE3EllipsoidProj::computeError()\n {\n Vector4d rect_project = getProject();\n\n _error = Vector4d(0,0,0,0);\n for(int i=0;i<4;i++)\n {\n if( _measurement[i] >= 5 ) // the invalid measurement has been set to -1.\n _error[i] = rect_project[i] - _measurement[i];\n }\n }\n\n void EdgeSE3EllipsoidProj::setKalib(Matrix3d& K)\n {\n Kalib = K;\n }\n\n // Prior: objects should be vertical to their supporting planes\n bool EdgeEllipsoidGravityPlanePrior::read(std::istream& is){\n return true;\n };\n\n bool EdgeEllipsoidGravityPlanePrior::write(std::ostream& os) const\n {\n return os.good();\n };\n\n void EdgeEllipsoidGravityPlanePrior::computeError()\n {\n const VertexEllipsoid* cuboidVertex = static_cast(_vertices[0]);\n ellipsoid global_cube = cuboidVertex->estimate();\n\n Eigen::Matrix3d rotMat = global_cube.pose.to_homogeneous_matrix().topLeftCorner(3,3);\n Vector3d zAxis = rotMat.col(2);\n\n Vector3d goundPlaneNormalAxis = _measurement.head(3);\n\n double cos_angle = zAxis.dot(goundPlaneNormalAxis)/zAxis.norm()/goundPlaneNormalAxis.norm();\n \n // prevent NaN\n if( cos_angle > 1 )\n cos_angle = cos_angle - 0.0001;\n else if( cos_angle < -1 )\n cos_angle = cos_angle + 0.0001;\n\n double angle = acos(cos_angle);\n\n double diff_angle = angle - 0;\n\n _error = Eigen::Matrix(diff_angle);\n }\n}// g2o" }, { "path": "src/core/DataAssociation.cpp", "content": "#include \"core/DataAssociation.h\"\n\n#include \n#include \n\nnamespace EllipsoidSLAM\n{\n\n DataAssociationSolver::DataAssociationSolver(Map* pMap)\n {\n mpMap = pMap;\n\n miInstanceNum = 0;\n }\n\n std::vector DataAssociationSolver::Solve(EllipsoidSLAM::Frame* pFrame, bool mb3D)\n {\n Eigen::MatrixXd &obsMat = pFrame->mmObservations;\n g2o::SE3Quat campose_wc = pFrame->cam_pose_Twc;\n\n auto instanceObjMap = mpMap->GetAllEllipsoidsMap();\n int num_object = instanceObjMap.size();\n\n int num_obs = obsMat.rows();\n\n std::map valid3DObsMap;\n\n std::vector associations; associations.resize(num_obs);\n if( mb3D ) // only support 3d observations\n {\n // filter invalid 3d observaions\n for( int i=0;impLocalObjects[i];\n if(pObjObserved != NULL ) \n {\n valid3DObsMap.insert(make_pair(i, pObjObserved));\n }\n else\n associations[i] = -1;\n }\n\n // construct a new list for those valid observations\n int validObNum = valid3DObsMap.size();\n\n if( validObNum < 1 ) return associations;\n\n // calculate the costMat\n MatrixXd costMat; costMat.resize(validObNum, num_object); // row : observations , col : objects in the map\n\n int objObservedId = 0;\n for( auto& validIdObPair : valid3DObsMap )\n {\n g2o::ellipsoid* pObjObserved = validIdObPair.second;\n g2o::ellipsoid pObjObservedWorld = pObjObserved->transform_from(campose_wc);\n VectorXd costVec; costVec.resize(num_object);\n\n int objInMapId = 0;\n for( auto& instanceObj : instanceObjMap )\n {\n g2o::ellipsoid* pObj = instanceObj.second;\n // calculate the distance of between their centers\n double distance = (pObj->pose.translation() - pObjObservedWorld.pose.translation()).norm();\n\n double cost = distance; \n costVec[objInMapId] = cost;\n objInMapId ++;\n }\n\n costMat.row(objObservedId) = costVec.transpose();\n objObservedId++;\n }\n\n // solve results from costMat\n std::vector matAssociations = SolveCostMat(costMat);\n\n // save the association results of the valid observations to output\n objObservedId = 0;\n for( auto& validIdObPair : valid3DObsMap )\n {\n associations[validIdObPair.first] = matAssociations[objObservedId];\n objObservedId++;\n }\n return associations;\n }\n }\n\n // Solve associations from every rows to every cols separately.\n // If an observation doesn't belong to any object in the map, it will get a new instance ID.\n std::vector DataAssociationSolver::SolveCostMat(MatrixXd& mat){\n // Method : Iterate through each row in turn, taking the column with minimum cost value if the value is also less than the thresh,\n // and delete the column from the cost matrix. Repeat the process for every row until there are no columns left.\n // ----- Settings ---\n double SETTING_DIS_THRESH = 1.0; // the minimum cost value must be less than the thresh to be considered valid.\n\n MatrixXd costMat = mat;\n int rows = costMat.rows();\n\n std::vector matAssocitions; matAssocitions.resize(rows);\n\n for( int i=0; i\n#include \n#include \n\nnamespace g2o\n{\n ellipsoid::ellipsoid()\n {\n }\n\n // xyz roll pitch yaw half_scale\n void ellipsoid::fromMinimalVector(const Vector9d& v){\n Eigen::Quaterniond posequat = zyx_euler_to_quat(v(3),v(4),v(5));\n pose = SE3Quat(posequat, v.head<3>());\n scale = v.tail<3>();\n\n vec_minimal = v;\n }\n\n // xyz quaternion, half_scale\n void ellipsoid::fromVector(const Vector10d& v){\n pose.fromVector(v.head<7>());\n scale = v.tail<3>();\n vec_minimal = toMinimalVector();\n }\n\n const Vector3d& ellipsoid::translation() const {return pose.translation();}\n void ellipsoid::setTranslation(const Vector3d& t_) {pose.setTranslation(t_);}\n void ellipsoid::setRotation(const Quaterniond& r_) {pose.setRotation(r_);}\n void ellipsoid::setRotation(const Matrix3d& R) {pose.setRotation(Quaterniond(R));}\n void ellipsoid::setScale(const Vector3d &scale_) {scale=scale_;}\n\n // apply update to current ellipsoid. exponential map\n ellipsoid ellipsoid::exp_update(const Vector9d& update)\n {\n ellipsoid res;\n res.pose = this->pose*SE3Quat::exp(update.head<6>());\n res.scale = this->scale + update.tail<3>();\n\n res.UpdateValueFrom(*this);\n res.vec_minimal = res.toMinimalVector();\n return res;\n }\n\n // TOBE DELETED.\n ellipsoid ellipsoid::exp_update_XYZABC(const Vector6d& update)\n {\n ellipsoid res;\n\n Vector6d pose_vec; pose_vec << 0, 0, 0, update[0], update[1], update[2];\n res.pose = this->pose*SE3Quat::exp(pose_vec); \n res.scale = this->scale + update.tail<3>();\n\n res.UpdateValueFrom(*this);\n res.vec_minimal = res.toMinimalVector();\n return res;\n }\n\n Vector9d ellipsoid::ellipsoid_log_error_9dof(const ellipsoid& newone) const\n {\n Vector9d res;\n SE3Quat pose_diff = newone.pose.inverse()*this->pose;\n\n res.head<6>() = pose_diff.log(); \n res.tail<3>() = this->scale - newone.scale; \n return res; \n }\n\n // change front face by rotate along current body z axis. \n // another way of representing cuboid. representing same cuboid (IOU always 1)\n ellipsoid ellipsoid::rotate_ellipsoid(double yaw_angle) const // to deal with different front surface of cuboids\n {\n ellipsoid res;\n SE3Quat rot(Eigen::Quaterniond(cos(yaw_angle*0.5),0,0,sin(yaw_angle*0.5)),Vector3d(0,0,0)); // change yaw to rotation.\n res.pose = this->pose*rot;\n res.scale = this->scale;\n \n res.UpdateValueFrom(*this);\n res.vec_minimal = res.toMinimalVector();\n\n const double eps = 1e-6;\n 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))\n std::swap(res.scale(0),res.scale(1)); \n\n return res;\n }\n\n Vector9d ellipsoid::min_log_error_9dof(const ellipsoid& newone, bool print_details) const\n {\n bool whether_rotate_ellipsoid=true; // whether rotate cube to find smallest error\n if (!whether_rotate_ellipsoid)\n return ellipsoid_log_error_9dof(newone);\n\n // NOTE rotating ellipsoid... since we cannot determine the front face consistenly, different front faces indicate different yaw, scale representation.\n // need to rotate all 360 degrees (global cube might be quite different from local cube)\n // 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\n // here to adjust the yaw.\n Vector4d rotate_errors_norm; Vector4d rotate_angles(-1,0,1,2); // rotate -90 0 90 180\n Eigen::Matrix rotate_errors;\n for (int i=0;iellipsoid_log_error_9dof(rotated_cuboid);\n rotate_errors_norm(i) = cuboid_error.norm();\n rotate_errors.col(i) = cuboid_error;\n }\n int min_label;\n rotate_errors_norm.minCoeff(&min_label);\n if (print_details)\n if (min_label!=1)\n std::cout<<\"Rotate ellipsoid \"<pose;\n res.scale = this->scale;\n \n res.UpdateValueFrom(*this);\n res.vec_minimal = res.toMinimalVector();\n\n return res;\n }\n\n // transform a global cuboid to local cuboid Twc is camera pose. from camera to world\n ellipsoid ellipsoid::transform_to(const SE3Quat& Twc) const{\n ellipsoid res;\n res.pose = Twc.inverse()*this->pose;\n res.scale = this->scale;\n\n res.UpdateValueFrom(*this);\n res.vec_minimal = res.toMinimalVector();\n \n return res;\n }\n\n // xyz roll pitch yaw half_scale\n Vector9d ellipsoid::toMinimalVector() const{\n Vector9d v;\n v.head<6>() = pose.toXYZPRYVector();\n v.tail<3>() = scale;\n return v;\n }\n\n // xyz quaternion, half_scale\n Vector10d ellipsoid::toVector() const{\n Vector10d v;\n v.head<7>() = pose.toVector();\n v.tail<3>() = scale;\n return v;\n }\n\n Matrix4d ellipsoid::similarityTransform() const\n {\n Matrix4d res = pose.to_homogeneous_matrix(); // 4x4 transform matrix\n Matrix3d scale_mat = scale.asDiagonal();\n res.topLeftCorner<3,3>() = res.topLeftCorner<3,3>()*scale_mat;\n return res;\n }\n\n\n void ellipsoid::UpdateValueFrom(const g2o::ellipsoid& e){\n this->miLabel = e.miLabel;\n this->mbColor = e.mbColor;\n this->mvColor = e.mvColor;\n this->miInstanceID = e.miInstanceID;\n\n this->prob = e.prob;\n }\n\n ellipsoid::ellipsoid(const g2o::ellipsoid &e) {\n pose = e.pose;\n scale = e.scale;\n vec_minimal = e.vec_minimal;\n\n UpdateValueFrom(e);\n }\n\n const ellipsoid& ellipsoid::operator=(const g2o::ellipsoid &e) {\n pose = e.pose;\n scale = e.scale;\n vec_minimal = e.vec_minimal;\n\n UpdateValueFrom(e);\n return e;\n }\n\n // ************* Functions As Ellipsoids ***************\n Vector2d ellipsoid::projectCenterIntoImagePoint(const SE3Quat& campose_cw, const Matrix3d& Kalib)\n {\n Matrix3Xd P = generateProjectionMatrix(campose_cw, Kalib);\n\n Vector3d center_pos = pose.translation();\n Vector4d center_homo = real_to_homo_coord(center_pos);\n Vector3d u_homo = P * center_homo;\n Vector2d u = homo_to_real_coord_vec(u_homo);\n\n return u;\n }\n\n // project the ellipsoid into the image plane, and get an ellipse represented by a Vector5d.\n // Ellipse: x_c, y_c, theta, axis1, axis2\n Vector5d ellipsoid::projectOntoImageEllipse(const SE3Quat& campose_cw, const Matrix3d& Kalib) const \n {\n Matrix4d Q_star = generateQuadric();\n Matrix3Xd P = generateProjectionMatrix(campose_cw, Kalib);\n Matrix3d C_star = P * Q_star * P.transpose();\n Matrix3d C = C_star.inverse(); \n C = C / C(2,2); // normalize\n\n SelfAdjointEigenSolver es(C); // ascending sort by default\n VectorXd eigens = es.eigenvalues();\n\n // If it is an ellipse, the sign of eigen values must be : 1 1 -1 \n // Ref book : Multiple View Geometry in Computer Vision\n int num_pos = int(eigens(0)>0) +int(eigens(1)>0) +int(eigens(2)>0);\n int num_neg = int(eigens(0)<0) +int(eigens(1)<0) +int(eigens(2)<0);\n\n // matrix to equation coefficients: ax^2+bxy+cy^2+dx+ey+f=0\n double a = C(0,0);\n double b = C(0,1)*2;\n double c = C(1,1);\n double d = C(0,2)*2;\n double e = C(2,1)*2;\n double f = C(2,2);\n\n // get x_c, y_c, theta, axis1, axis2 from coefficients\n double delta = c*c - 4.0*a*b;\n double k = (a*f-e*e/4.0) - pow((2*a*e-c*d),2)/(4*(4*a*b-c*c));\n double theta = 1/2.0*atan2(b,(a-c));\n double x_c = (b*e-2*c*d)/(4*a*c-b*b);\n double y_c = (b*d-2*a*e)/(4*a*c-b*b);\n 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));\n 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));\n\n double axis1= sqrt(a_2);\n double axis2= sqrt(b_2);\n\n Vector5d output;\n output << x_c, y_c, theta, axis1, axis2;\n\n return output;\n }\n\n // Get the bounding box from ellipse in image plane\n Vector4d ellipsoid::getBoundingBoxFromEllipse(Vector5d &ellipse) const\n {\n double a = ellipse[3];\n double b = ellipse[4];\n double theta = ellipse[2];\n double x = ellipse[0];\n double y = ellipse[1];\n \n double cos_theta_2 = cos(theta)*cos(theta);\n double sin_theta_2 = 1- cos_theta_2;\n\n double x_limit = sqrt(a*a*cos_theta_2+b*b*sin_theta_2);\n double y_limit = sqrt(a*a*sin_theta_2+b*b*cos_theta_2);\n\n Vector4d output;\n output[0] = x-x_limit; // left up\n output[1] = y-y_limit;\n output[2] = x+x_limit; // right down\n output[3] = y+y_limit;\n\n return output;\n }\n\n // Get projection matrix P = K [ R | t ]\n Matrix3Xd ellipsoid::generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const\n {\n Matrix3Xd identity_lefttop;\n identity_lefttop.resize(3, 4);\n identity_lefttop.col(3)=Vector3d(0,0,0);\n identity_lefttop.topLeftCorner<3,3>() = Matrix3d::Identity(3,3);\n\n Matrix3Xd proj_mat = Kalib * identity_lefttop;\n proj_mat = proj_mat * campose_cw.to_homogeneous_matrix();\n\n return proj_mat;\n }\n\n // Get Q^*\n Matrix4d ellipsoid::generateQuadric() const\n {\n Vector4d axisVec;\n axisVec << 1/(scale[0]*scale[0]), 1/(scale[1]*scale[1]), 1/(scale[2]*scale[2]), -1;\n Matrix4d Q_c = axisVec.asDiagonal(); \n Matrix4d Q_c_star = Q_c.inverse(); \n Matrix4d Q_pose_matrix = pose.to_homogeneous_matrix(); // Twm model in world, world to model\n Matrix4d Q_c_star_trans = Q_pose_matrix * Q_c_star * Q_pose_matrix.transpose(); \n\n return Q_c_star_trans;\n }\n\n // Get the projected bounding box in the image plane of the ellipsoid using a camera pose and a calibration matrix.\n Vector4d ellipsoid::getBoundingBoxFromProjection(const SE3Quat& campose_cw, const Matrix3d& Kalib) const\n {\n Vector5d ellipse = projectOntoImageEllipse(campose_cw, Kalib);\n return getBoundingBoxFromEllipse(ellipse);\n }\n\n Vector3d ellipsoid::getColor(){\n return mvColor.head(3);\n }\n\n Vector4d ellipsoid::getColorWithAlpha(){\n return mvColor;\n }\n\n void ellipsoid::setColor(const Vector3d &color_, double alpha){\n mbColor = true;\n mvColor.head<3>() = color_;\n mvColor[3] = alpha;\n\n }\n\n bool ellipsoid::isColorSet(){\n return mbColor;\n }\n\n bool ellipsoid::CheckObservability(const SE3Quat& campose_cw)\n {\n Vector3d ellipsoid_center = toMinimalVector().head(3); // Pwo\n Vector4d center_homo = real_to_homo_coord_vec(ellipsoid_center);\n\n Eigen::Matrix4d projMat = campose_cw.to_homogeneous_matrix(); // Tcw\n Vector4d center_inCameraAxis_homo = projMat * center_homo; // Pco = Tcw * Pwo\n Vector3d center_inCameraAxis = homo_to_real_coord_vec(center_inCameraAxis_homo);\n\n if( center_inCameraAxis_homo(2) < 0) // if the center is behind the camera ; z<0\n {\n return false;\n }\n else\n return true;\n }\n\n // calculate the IoU Error between two axis-aligned ellipsoid\n double ellipsoid::calculateMIoU(const g2o::ellipsoid& e) const\n {\n return calculateIntersectionError(*this, e);\n }\n\n double ellipsoid::calculateIntersectionOnZ(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const\n {\n g2o::SE3Quat pose_diff = e1.pose.inverse() * e2.pose;\n double z1 = 0; double z2 = pose_diff.translation()[2];\n\n bool flag_oneBigger = false;\n if( z1 > z2 )\n flag_oneBigger = true;\n\n double length;\n if( flag_oneBigger )\n {\n length = (z2 + e2.scale[2]) - (z1 - e1.scale[2]); \n }\n else \n length = (z1 + e1.scale[2]) - (z2 - e2.scale[2]); \n\n if( length < 0 )\n length = 0; // if they are not intersected\n \n return length;\n }\n\n double ellipsoid::calculateArea(const g2o::ellipsoid& e) const\n {\n return e.scale[0]*e.scale[1]*e.scale[2]*8;\n }\n\n void OutputPolygon(EllipsoidSLAM::Polygon& polygon, double resolution)\n {\n int num = polygon.n;\n for( int i=0;i(similarityTransform()*real_to_homo_coord(corners_body));\n return corners_world;\n }\n\n Matrix2Xd ellipsoid::projectOntoImageBoxCorner(const SE3Quat& campose_cw, const Matrix3d& Kalib) const\n {\n Matrix3Xd corners_3d_world = compute3D_BoxCorner();\n Matrix2Xd corner_2d = homo_to_real_coord(Kalib*homo_to_real_coord(campose_cw.to_homogeneous_matrix()*real_to_homo_coord(corners_3d_world)));\n\n return corner_2d;\n }\n\n // get rectangles after projection [topleft, bottomright]\n Vector4d ellipsoid::projectOntoImageRect(const SE3Quat& campose_cw, const Matrix3d& Kalib) const\n {\n Matrix2Xd corner_2d = projectOntoImageBoxCorner(campose_cw, Kalib);\n Vector2d bottomright = corner_2d.rowwise().maxCoeff(); // x y\n Vector2d topleft = corner_2d.rowwise().minCoeff();\n return Vector4d(topleft(0),topleft(1),bottomright(0),bottomright(1));\n }\n\n // get rectangles after projection [center, width, height]\n Vector4d ellipsoid::projectOntoImageBbox(const SE3Quat& campose_cw, const Matrix3d& Kalib) const\n {\n Vector4d rect_project = projectOntoImageRect(campose_cw, Kalib); // top_left, bottom_right x1 y1 x2 y2\n Vector2d rect_center = (rect_project.tail<2>()+rect_project.head<2>())/2;\n Vector2d widthheight = rect_project.tail<2>()-rect_project.head<2>();\n return Vector4d(rect_center(0),rect_center(1),widthheight(0),widthheight(1));\n }\n\n} // g2o" }, { "path": "src/core/Frame.cpp", "content": "#include \n\nnamespace EllipsoidSLAM\n{\n int Frame::total_frame=0;\n\n Frame::Frame(double timestamp_, const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap, const cv::Mat &imDepth, const cv::Mat &imRGB)\n {\n timestamp = timestamp_;\n rgb_img = imRGB.clone();\n frame_img = imDepth.clone();\n\n cam_pose_Twc.fromVector(pose.tail(7));\n cam_pose_Tcw = cam_pose_Twc.inverse();\n\n frame_seq_id = total_frame++;\n\n mvbOutlier.resize(bboxMap.rows());\n fill(mvbOutlier.begin(), mvbOutlier.end(), false); \n\n mmObservations = bboxMap;\n\n std::cout << \"--------> New Frame : \" << frame_seq_id << \" Timestamp: \" << std::to_string(timestamp) << std::endl;\n std::cout << \"[Frame.cpp] bboxMap : \" << std::endl << bboxMap << std::endl << std::endl;\n\n mbHaveLocalObject = false;\n }\n\n\n}\n" }, { "path": "src/core/FrameDrawer.cpp", "content": "#include \"include/core/FrameDrawer.h\"\n#include \"src/config/Config.h\"\n#include \"utils/dataprocess_utils.h\"\n\nusing namespace cv;\n\nnamespace EllipsoidSLAM\n{\n FrameDrawer::FrameDrawer(EllipsoidSLAM::Map *pMap) {\n\n mpMap = pMap;\n mmRGB = cv::Mat();\n mmDepth = cv::Mat();\n }\n\n void FrameDrawer::setTracker(EllipsoidSLAM::Tracking *pTracker) {\n mpTracking = pTracker;\n }\n\n cv::Mat FrameDrawer::drawFrame() {\n Frame * frame = mpTracking->mCurrFrame;\n if( frame == NULL ) return cv::Mat();\n cv::Mat im;\n if(!frame->rgb_img.empty()) // use rgb image if it exists, or use depth image instead.\n im = frame->rgb_img;\n else \n im = frame->frame_img;\n\n cv::Mat out = drawFrameOnImage(im);\n\n mmRGB = out.clone();\n\n return mmRGB;\n }\n\n cv::Mat FrameDrawer::drawDepthFrame() {\n Frame * frame = mpTracking->mCurrFrame;\n cv::Mat I = frame->frame_img; // U16C1 , ushort\n cv::Mat im,R,G,B;\n\n // Vector3d color1(51,25,0);\n // Vector3d color2(255,229,204);\n // r = color1 + value/255*(color2-color1)\n // (255-51)/255 51\n // 220-25\n\n I.convertTo(R, CV_8UC1, 0.028, 51);\n I.convertTo(G, CV_8UC1, 0.028, 25);\n I.convertTo(B, CV_8UC1, 0.028, 0);\n std::vector array_to_merge;\n array_to_merge.push_back(B);\n array_to_merge.push_back(G);\n array_to_merge.push_back(R);\n cv::merge(array_to_merge, im);\n\n cv::Mat out = drawObservationOnImage(im);\n\n mmDepth = out.clone();\n\n return mmDepth;\n }\n\n cv::Mat FrameDrawer::drawProjectionOnImage(cv::Mat &im) {\n std::map pEllipsoidsMapWithLabel = mpMap->GetAllEllipsoidsMap();\n\n // draw projected bounding boxes of the ellipsoids in the map\n cv::Mat imageProj = im.clone();\n for(auto iter=pEllipsoidsMapWithLabel.begin(); iter!=pEllipsoidsMapWithLabel.end();iter++)\n {\n ellipsoid* e = iter->second;\n\n // check whether it could be seen\n if( e->CheckObservability(mpTracking->mCurrFrame->cam_pose_Tcw) )\n {\n Vector4d rect = e->getBoundingBoxFromProjection(mpTracking->mCurrFrame->cam_pose_Tcw, mpTracking->mCalib); // center, width, height\n cv::rectangle(imageProj, cv::Rect(cv::Point(rect[0],rect[1]),cv::Point(rect[2],rect[3])), cv::Scalar(0,0,255), 4);\n }\n }\n\n return imageProj.clone();\n }\n\n cv::Mat FrameDrawer::drawFrameOnImage(cv::Mat &in) {\n cv::Mat out = in.clone();\n out = drawObservationOnImage(out);\n out = drawProjectionOnImage(out);\n return out.clone();\n }\n\n cv::Mat FrameDrawer::drawObservationOnImage(cv::Mat &in) {\n cv::Mat im = in.clone();\n\n Frame * frame = mpTracking->mCurrFrame;\n // draw observation\n Eigen::MatrixXd mat_det = frame->mmObservations;\n int obs = mat_det.rows();\n for(int r=0;r(\"Measurement.Border.Pixels\"), Config::Get(\"Measurement.LengthLimit.Pixels\"));\n\n int labelId = int(vDet(5));\n\n Rect rec(Point(vDet(1), vDet(2)), Point(vDet(3), vDet(4)));\n\n if( !is_border )\n {\n rectangle(im, rec, Scalar(255,0,0), 3);\n putText(im, to_string(labelId), Point(vDet(1), vDet(2)), cv::FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0,255,0), 2);\n }\n }\n\n return im.clone();\n }\n\n cv::Mat FrameDrawer::getCurrentFrameImage(){\n return mmRGB;\n }\n\n cv::Mat FrameDrawer::getCurrentDepthFrameImage(){\n return mmDepth;\n }\n}" }, { "path": "src/core/Geometry.cpp", "content": "#include \"core/Geometry.h\"\n#include \"utils/matrix_utils.h\"\n#include \"utils/dataprocess_utils.h\"\nnamespace EllipsoidSLAM\n{\n\n // Generates all points in the depth image as point cloud.\n PointCloud getPointCloud(cv::Mat &depth, cv::Mat &rgb, Eigen::VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera) {\n PointCloud cloud;\n\n int x1 = 0;\n int y1 = 0;\n int x2 = depth.cols;\n int y2 = depth.rows;\n\n for (int y = y1; y < y2; y++){\n for (int x = x1; x < x2; x++) {\n ushort *ptd = depth.ptr(y);\n ushort d = ptd[x];\n\n PointXYZRGB p;\n p.z = d / camera.scale;\n if (p.z <= 0.1 || p.z > 100) // limit the depth range in [0.1,100]m\n continue;\n\n p.x = (x - camera.cx) * p.z / camera.fx;\n p.y = (y - camera.cy) * p.z / camera.fy;\n\n // get rgb color\n p.b = rgb.ptr(y)[x * 3];\n p.g = rgb.ptr(y)[x * 3 + 1];\n p.r = rgb.ptr(y)[x * 3 + 2];\n\n p.size = 1;\n\n cloud.push_back(p);\n }\n }\n\n return cloud;\n }\n\n // apply transformation on every points in the pointcloud\n PointCloud* transformPointCloud(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc)\n {\n Eigen::Matrix4d Twc = pCampose_wc->to_homogeneous_matrix();\n\n auto * pPoints_global = new PointCloud();\n for (auto &p : *pPoints_local) {\n Eigen::Vector3d xyz(p.x, p.y, p.z);\n Eigen::Vector3d xyz_w = TransformPoint(xyz, Twc);\n \n PointXYZRGB p_w;\n p_w.x = xyz_w[0];\n p_w.y = xyz_w[1];\n p_w.z = xyz_w[2];\n p_w.r = p.r;\n p_w.g = p.g;\n p_w.b = p.b;\n p_w.size = p.size;\n \n pPoints_global->push_back(p_w);\n }\n\n return pPoints_global;\n }\n\n void transformPointCloudSelf(PointCloud *pPoints_local, g2o::SE3Quat* pCampose_wc)\n {\n Eigen::Matrix4d Twc = pCampose_wc->to_homogeneous_matrix();\n\n for (auto &p : *pPoints_local) {\n Eigen::Vector3d xyz(p.x, p.y, p.z);\n Eigen::Vector3d xyz_w = TransformPoint(xyz, Twc);\n \n p.x = xyz_w[0];\n p.y = xyz_w[1];\n p.z = xyz_w[2];\n }\n\n return;\n }\n\n PointCloud loadPointsToPointVector(Eigen::MatrixXd &pMat){\n int total_num = pMat.rows();\n PointCloud points;\n\n for( int i=0; isize();\n MatrixXd outMat; outMat.resize(num, 3);\n for( int i=0;isize();\n Vector3d total(0,0,0);\n for( int i=0; i(point);\n Eigen::Vector4d Xw = T * Xc;\n Eigen::Vector3d xyz_w = homo_to_real_coord(Xw);\n return xyz_w;\n }\n\n\n}\n" }, { "path": "src/core/Initializer.cpp", "content": "#include \"core/Initializer.h\"\n\n#include \n#include \n#include \n\nusing namespace Eigen;\nusing namespace std;\n\nnamespace EllipsoidSLAM\n{\n Initializer::Initializer(int rows, int cols) {\n miImageRows = rows;\n miImageCols = cols;\n }\n\n // ascending sort\n bool cmp(paira, pairb)\n {\n return a.second > b.second;\n }\n\n // Initialize an ellipsoid from several bounding boxes and camera poses.\n g2o::ellipsoid Initializer::initializeQuadric(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib) {\n mbResult = false;\n\n int input_size = pose_mat.rows();\n\n // 1) Generate tangent planes from bounding boxes and camera cneter. \n // Those invalid observations are ignored.\n MatrixXd planesHomo = getPlanesHomo(pose_mat, detection_mat, calib); \n\n int plane_size = planesHomo.cols();\n int invalid_plane = input_size*4 - plane_size;\n if( invalid_plane > 0)\n std::cout << \" * invalid_plane: \" << invalid_plane << std::endl;\n\n if(plane_size < 9) // at least 9 planes are needed\n {\n g2o::ellipsoid e_bad;\n mbResult = false;\n return e_bad;\n }\n\n // Using SVD to generate a quadric\n MatrixXd planesVector = getVectorFromPlanesHomo(planesHomo);\n Matrix4d QStar = getQStarFromVectors(planesVector);\n\n // generate ellipsoid from the quadric\n g2o::ellipsoid e = getEllipsoidFromQStar(QStar);\n\n // Set color : blue.\n Eigen::Vector3d blueScalar(0,0,255);\n e.setColor(blueScalar);\n return e;\n }\n\n MatrixXd Initializer::getPlanesHomo(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib) {\n assert( pose_mat.rows() == detection_mat.rows() && \" Two matrices should match. \" );\n assert( pose_mat.rows() > 2 && \" At least 3 measurements are required. \" );\n\n MatrixXd planes_all(4,0);\n\n int rows = pose_mat.rows();\n for(int i=0;i() = Matrix3d::Identity(3,3);\n\n Matrix3Xd proj_mat = Kalib * identity_lefttop;\n\n proj_mat = proj_mat * campose_cw.to_homogeneous_matrix();\n\n return proj_mat;\n }\n\n MatrixXd Initializer::fromDetectionsToLines(VectorXd &detections) {\n bool flag_openFilter = true; // filter those lines lying on the image boundary\n\n double x1 = detections(0);\n double y1 = detections(1);\n double x2 = detections(2);\n double y2 = detections(3);\n\n Vector3d line1 (1, 0, -x1);\n Vector3d line2 (0, 1, -y1);\n Vector3d line3 (1, 0, -x2);\n Vector3d line4 (0, 1, -y2);\n\n // those lying on the image boundary have been marked -1 \n MatrixXd line_selected(3, 0);\n MatrixXd line_selected_none(3, 0);\n if( !flag_openFilter || ( x1>0 && x10 && y10 && x20 && y2 svd(A, ComputeThinU | ComputeThinV );\n MatrixXd V = svd.matrixV();\n\n VectorXd qj_hat = V.col(V.cols()-1);\n\n // Get QStar\n Matrix4d QStar;\n QStar <<\n qj_hat(0),qj_hat(1),qj_hat(2),qj_hat(3),\n qj_hat(1),qj_hat(4),qj_hat(5),qj_hat(6),\n qj_hat(2),qj_hat(5),qj_hat(7),qj_hat(8),\n qj_hat(3),qj_hat(6),qj_hat(8),qj_hat(9);\n \n return QStar;\n }\n\n g2o::ellipsoid Initializer::getEllipsoidFromQStar(Matrix4d &QStar) {\n g2o::ellipsoid e;\n\n Matrix4d Q = QStar.inverse() * cbrt(QStar.determinant());\n\n SelfAdjointEigenSolver es(Q); // ascending order by default\n MatrixXd D = es.eigenvalues().asDiagonal();\n MatrixXd V = es.eigenvectors();\n\n VectorXd eigens = es.eigenvalues();\n\n // For an ellipsoid, the signs of the eigenvalues must be ---+ or +++-\n int num_pos = int(eigens(0)>0) +int(eigens(1)>0) +int(eigens(2)>0) +int(eigens(3)>0);\n int num_neg = int(eigens(0)<0) +int(eigens(1)<0) +int(eigens(2)<0) +int(eigens(3)<0);\n if( !(num_pos ==3 && num_neg == 1) && !(num_pos ==1 && num_neg == 3) ){\n cout << \" Not Ellipsoid : pos/neg \" << num_pos << \" / \" << num_neg << endl;\n cout << \"eigens :\" << eigens.transpose() << endl;\n mbResult = false;\n return e;\n }\n else\n mbResult = true;\n\n if( eigens(3) > 0 ) // normalize to - - - + \n {\n Q=-Q;\n SelfAdjointEigenSolver es_2(Q); \n D = es_2.eigenvalues().asDiagonal();\n V = es_2.eigenvectors();\n\n eigens = es_2.eigenvalues();\n }\n\n // Solve ellipsoid parameters from matrix Q\n Vector3d lambda_mat = eigens.head(3).array().inverse();\n\n Matrix3d Q33 = Q.block(0,0,3,3);\n\n double k = Q.determinant()/Q33.determinant();\n\n Vector3d value = -k*(lambda_mat);\n Vector3d s = value.array().abs().sqrt();\n\n Vector4d t = QStar.col(3);\n t = t/t(3);\n Vector3d translation = t.head(3);\n\n SelfAdjointEigenSolver es2(Q33); \n MatrixXd D_Q33 = es2.eigenvalues().asDiagonal();\n MatrixXd rot = es2.eigenvectors();\n\n double r,p,y;\n rot_to_euler_zyx(rot, r, p, y);\n Vector3d rpy(r,p,y);\n\n // generate ellipsoid\n Vector9d objectVec;\n objectVec << t(0),t(1),t(2),rpy(0),rpy(1),rpy(2),s(0),s(1),s(2);\n e.fromMinimalVector(objectVec);\n\n return e;\n\n }\n\n void Initializer::sortEigenValues(VectorXcd &eigens, MatrixXcd &V) {\n vector> pairs;\n for(int i=0;i<4;i++)\n pairs.push_back( make_pair( i, eigens(i).real() ) );\n sort(pairs.begin(), pairs.end(), cmp);\n\n // Construct a new matrix in order\n MatrixXcd V_new(4,4);\n Vector4cd eigens_new;\n\n for(int i=0;i<4;i++)\n {\n int oldID = pairs[i].first;\n V_new.col(i) = V.col(oldID);\n eigens_new(i) = complex(pairs[i].second, 0);\n }\n\n eigens = eigens_new;\n V = V_new;\n }\n\n double Initializer::quadricErrorWithPlanes(MatrixXd &pose_mat, MatrixXd &detection_mat, Matrix3d &calib,\n g2o::ellipsoid &e) {\n Matrix4d QStar = e.generateQuadric(); // get Q^*\n Vector10d qj_hat;\n 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);\n\n // Get planes vector\n MatrixXd planesHomo = getPlanesHomo(pose_mat, detection_mat, calib);\n MatrixXd planesVector = getVectorFromPlanesHomo(planesHomo);\n\n // Get error\n VectorXd result = planesVector.transpose() * qj_hat;\n return result.transpose() * result;\n }\n\n bool Initializer::getInitializeResult() {\n return mbResult;\n }\n\n g2o::ellipsoid Initializer::initializeQuadric(EllipsoidSLAM::Observations &obs, Matrix3d &calib) {\n MatrixXd pose_mat;\n MatrixXd detection_mat;\n\n // get pose matrix and detection matrix from observations\n getDetectionAndPoseMatFromObservations(obs, pose_mat, detection_mat);\n\n g2o::ellipsoid e = initializeQuadric(pose_mat, detection_mat, calib);\n\n if( getInitializeResult() )\n {\n e.miLabel = obs[0]->label;\n }\n\n return e;\n }\n\n void Initializer::getDetectionAndPoseMatFromObservations(EllipsoidSLAM::Observations &obs, MatrixXd &pose_mat,\n MatrixXd &detection_mat) {\n int frameSize = obs.size();\n pose_mat.resize(frameSize, 7); // x y z qx qy qz qw\n detection_mat.resize(frameSize, 5); // x1 y1 x2 y2 accuracy\n\n int id = 0;\n for (auto iter = obs.begin(); iter!=obs.end(); iter++)\n {\n Vector5d det_vec;\n det_vec << (*iter)->bbox, (*iter)->rate;\n Vector7d pos_vec = (*iter)->pFrame->cam_pose_Twc.toVector();\n\n pose_mat.row(id) = pos_vec;\n detection_mat.row(id) = det_vec;\n\n id ++;\n }\n }\n}\n" }, { "path": "src/core/Map.cpp", "content": "#include \"include/core/Map.h\"\n\nusing namespace std;\n\nnamespace EllipsoidSLAM\n{\n\nMap::Map() {\n mCameraState = new g2o::SE3Quat();\n}\n\nvoid Map::addPoint(EllipsoidSLAM::PointXYZRGB *pPoint) {\n unique_lock lock(mMutexMap);\n mspPoints.insert(pPoint);\n}\n\nvoid Map::addPointCloud(EllipsoidSLAM::PointCloud *pPointCloud) {\n unique_lock lock(mMutexMap);\n\n for(auto iter=pPointCloud->begin();iter!=pPointCloud->end();++iter){\n mspPoints.insert(&(*iter));\n }\n}\n\nvoid Map::clearPointCloud() {\n unique_lock lock(mMutexMap);\n\n mspPoints.clear();\n}\n\nvoid Map::addEllipsoid(ellipsoid *pObj)\n{\n unique_lock lock(mMutexMap);\n mspEllipsoids.push_back(pObj);\n}\n\n\nvector Map::GetAllEllipsoids()\n{\n unique_lock lock(mMutexMap);\n return mspEllipsoids;\n}\n\nstd::vector Map::GetAllPoints() {\n unique_lock lock(mMutexMap);\n return vector(mspPoints.begin(),mspPoints.end());\n}\n\nvoid Map::addPlane(plane *pPlane)\n{\n unique_lock lock(mMutexMap);\n mspPlanes.insert(pPlane);\n}\n\n\nvector Map::GetAllPlanes()\n{\n unique_lock lock(mMutexMap);\n return vector(mspPlanes.begin(),mspPlanes.end());\n}\n\nvoid Map::setCameraState(g2o::SE3Quat* state) {\n unique_lock lock(mMutexMap);\n mCameraState = state;\n}\n\nvoid Map::addCameraStateToTrajectory(g2o::SE3Quat* state) {\n unique_lock lock(mMutexMap);\n mvCameraStates.push_back(state);\n}\n\ng2o::SE3Quat* Map::getCameraState() {\n unique_lock lock(mMutexMap);\n return mCameraState;\n}\n\nstd::vector Map::getCameraStateTrajectory() {\n unique_lock lock(mMutexMap);\n return mvCameraStates;\n}\n\nstd::vector Map::getEllipsoidsUsingLabel(int label) {\n unique_lock lock(mMutexMap);\n\n std::vector mvpObjects;\n auto iter = mspEllipsoids.begin();\n for(; iter!=mspEllipsoids.end(); iter++)\n {\n\n if( (*iter)->miLabel == label )\n mvpObjects.push_back(*iter);\n\n }\n\n return mvpObjects;\n}\n\nstd::map Map::GetAllEllipsoidsMap() {\n std::map maps;\n for(auto iter= mspEllipsoids.begin(); iter!=mspEllipsoids.end();iter++)\n {\n maps.insert(make_pair((*iter)->miInstanceID, *iter));\n }\n return maps;\n}\n\nvoid Map::clearPlanes(){\n unique_lock lock(mMutexMap);\n mspPlanes.clear();\n}\n\nvoid Map::addEllipsoidVisual(ellipsoid *pObj)\n{\n unique_lock lock(mMutexMap);\n mspEllipsoidsVisual.push_back(pObj);\n}\n\n\nvector Map::GetAllEllipsoidsVisual()\n{\n unique_lock lock(mMutexMap);\n return mspEllipsoidsVisual;\n}\n\nvoid Map::ClearEllipsoidsVisual()\n{\n unique_lock lock(mMutexMap);\n mspEllipsoidsVisual.clear();\n}\n\nbool Map::AddPointCloudList(const string& name, PointCloud* pCloud, int type){\n unique_lock lock(mMutexMap);\n\n // Check repetition\n if(mmPointCloudLists.find(name) != mmPointCloudLists.end() )\n {\n // Exist\n \n if( type == 0){\n // replace it.\n mmPointCloudLists[name]->clear(); // release it\n mmPointCloudLists[name] = pCloud;\n }\n else if( type == 1 )\n {\n // add together\n for( auto &p : *pCloud )\n mmPointCloudLists[name]->push_back(p);\n }\n\n return false;\n }\n else{\n mmPointCloudLists.insert(make_pair(name, pCloud));\n return true;\n }\n \n}\n\nbool Map::DeletePointCloudList(const string& name, int type){\n unique_lock lock(mMutexMap);\n\n if( type == 0 ) // complete matching: the name must be the same\n {\n auto iter = mmPointCloudLists.find(name);\n if (iter != mmPointCloudLists.end() )\n {\n mmPointCloudLists.erase(iter);\n return true;\n }\n else{\n std::cerr << \"PointCloud name \" << name << \" doesn't exsit. Can't delete it.\" << std::endl;\n return false;\n }\n }\n else if ( type == 1 ) // partial matching\n {\n bool deleteSome = false;\n for( auto iter = mmPointCloudLists.begin();iter!=mmPointCloudLists.end();)\n {\n auto strPoints = *iter;\n if( strPoints.first.find(name) != strPoints.first.npos )\n {\n iter = mmPointCloudLists.erase(iter);\n deleteSome = true;\n }\n else \n iter++;\n }\n return deleteSome;\n }\n \n}\n\nbool Map::ClearPointCloudLists(){\n unique_lock lock(mMutexMap);\n\n mmPointCloudLists.clear();\n}\n\nstd::map Map::GetPointCloudList(){\n unique_lock lock(mMutexMap);\n return mmPointCloudLists;\n}\n\n} // namespace " }, { "path": "src/core/MapDrawer.cpp", "content": "#include \"include/core/MapDrawer.h\"\n\n#include \n#include \n\n#include \n#include \n\nnamespace EllipsoidSLAM\n{\n // draw axis for ellipsoids\n void drawAxisNormal()\n {\n float length = 2.0;\n \n // x\n glColor3f(1.0,0.0,0.0); // red x\n glBegin(GL_LINES);\n glVertex3f(0.0, 0.0f, 0.0f);\n glVertex3f(length, 0.0f, 0.0f);\n glEnd();\n \n // y \n glColor3f(0.0,1.0,0.0); // green y\n glBegin(GL_LINES);\n glVertex3f(0.0, 0.0f, 0.0f);\n glVertex3f(0.0, length, 0.0f);\n \n glEnd();\n \n // z \n glColor3f(0.0,0.0,1.0); // blue z\n glBegin(GL_LINES);\n glVertex3f(0.0, 0.0f ,0.0f );\n glVertex3f(0.0, 0.0f ,length );\n \n glEnd();\n }\n\n MapDrawer::MapDrawer(const string &strSettingPath, Map* pMap):mpMap(pMap)\n {\n cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);\n\n mKeyFrameSize = fSettings[\"Viewer.KeyFrameSize\"];\n mKeyFrameLineWidth = fSettings[\"Viewer.KeyFrameLineWidth\"];\n mGraphLineWidth = fSettings[\"Viewer.GraphLineWidth\"];\n mPointSize = fSettings[\"Viewer.PointSize\"];\n mCameraSize = fSettings[\"Viewer.CameraSize\"];\n mCameraLineWidth = fSettings[\"Viewer.CameraLineWidth\"];\n\n float fx = fSettings[\"Camera.fx\"];\n float fy = fSettings[\"Camera.fy\"];\n float cx = fSettings[\"Camera.cx\"];\n float cy = fSettings[\"Camera.cy\"];\n\n mCalib << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n }\n\n // draw external cubes.\n bool MapDrawer::drawObjects() {\n std::vector ellipsoids = mpMap->GetAllEllipsoids();\n\n std::vector ellipsoidsVisual = mpMap->GetAllEllipsoidsVisual();\n ellipsoids.insert(ellipsoids.end(), ellipsoidsVisual.begin(), ellipsoidsVisual.end());\n\n for( size_t i=0; icompute3D_BoxCorner();\n\n glPushMatrix();\n\n glLineWidth(mCameraLineWidth);\n\n if(ellipsoids[i]->isColorSet()){\n Vector3d color = ellipsoids[i]->getColor();\n glColor3f(1.0f,0.0f,0.0f); // red color.\n }\n else\n glColor3f(0.0f,0.0f,1.0f);\n \n glBegin(GL_LINES);\n\n // draw cube lines. \n for(int m=0;m ellipsoids = mpMap->GetAllEllipsoids();\n\n std::vector ellipsoidsVisual = mpMap->GetAllEllipsoidsVisual();\n ellipsoids.insert(ellipsoids.end(), ellipsoidsVisual.begin(), ellipsoidsVisual.end());\n for( size_t i=0; ipose.inverse();\n Vector3d scale = ellipsoids[i]->scale;\n\n glPushMatrix();\n\n glLineWidth(mCameraLineWidth*3/4.0);\n\n if(ellipsoids[i]->isColorSet()){\n Vector4d color = ellipsoids[i]->getColorWithAlpha();\n glColor4f(color(0),color(1),color(2),color(3));\n }\n else\n glColor3f(0.0f,0.0f,1.0f);\n\n GLUquadricObj *pObj;\n pObj = gluNewQuadric();\n gluQuadricDrawStyle(pObj, GLU_LINE);\n\n pangolin::OpenGlMatrix Twm; // model to world\n SE3ToOpenGLCameraMatrix(TmwSE3, Twm);\n glMultMatrixd(Twm.m); \n glScaled(scale[0],scale[1],scale[2]);\n\n gluSphere(pObj, 1.0, 26, 13); // draw a sphere with radius 1.0, center (0,0,0), slices 26, and stacks 13.\n drawAxisNormal();\n\n glPopMatrix();\n }\n\n return true;\n }\n\n // draw all the planes\n bool MapDrawer::drawPlanes() {\n std::vector planes = mpMap->GetAllPlanes();\n for( size_t i=0; i x,y,z;\n if( p->mbLimited ) // draw a finite plane.\n {\n pieces = 100;\n \n double area_range = p->mdPlaneSize;\n double step = area_range/pieces;\n\n // ----- x\n x.clear();\n x.reserve(pieces+2);\n\n starting = p->mvPlaneCenter[0] - area_range/2;\n ending = p->mvPlaneCenter[0] + area_range/2;\n\n while(starting <= ending) {\n x.push_back(starting);\n starting += step;\n }\n\n // ----- y\n y.clear();\n y.reserve(pieces+2);\n\n starting = p->mvPlaneCenter[1] - area_range/2;\n ending = p->mvPlaneCenter[1] + area_range/2;\n\n while(starting <= ending) {\n y.push_back(starting);\n starting += step;\n }\n\n // ----- z\n z.clear();\n z.reserve(pieces+2);\n\n starting = p->mvPlaneCenter[2] - area_range/2;\n ending = p->mvPlaneCenter[2] + area_range/2;\n\n while(starting <= ending) {\n z.push_back(starting);\n starting += step;\n } \n }\n else // draw an infinite plane, make it big enough\n {\n starting = -5;\n ending = 5;\n\n x.clear();\n double step = (ending-starting)/pieces;\n x.reserve(pieces+2);\n while(starting <= ending) {\n x.push_back(starting);\n starting += step;\n }\n y=x;\n z=x;\n }\n \n Vector4d param = p->param;\n Vector3d color = p->color;\n\n glPushMatrix();\n glBegin(GL_POINTS);\n glColor3f(color[0], color[1], color[2]);\n\n double param_abs_x = std::abs(param[0]);\n double param_abs_y = std::abs(param[1]);\n double param_abs_z = std::abs(param[2]);\n\n if( param_abs_z > param_abs_x && param_abs_z > param_abs_y ){\n // if the plane is extending toward x axis, use x,y to calculate z.\n for(int i=0; i Z = (-D-BY-AX)/C\n double z_ = (-param[3]-param[1]*y[j]-param[0]*x[i])/param[2];\n\n glVertex3f(float(x[i]), float(y[j]), float(z_));\n }\n }\n }\n else if( param_abs_x > param_abs_z && param_abs_x > param_abs_y )\n {\n // if the plane is extending toward z axis, use y,z to calculate x.\n for(int i=0; i X = (-D-BY-CZ)/A\n double x_ = (-param[3]-param[1]*y[j]-param[2]*z[i])/param[0];\n\n glVertex3f(float(x_), float(y[j]), float(z[i]));\n }\n }\n }\n else\n {\n // if the plane is extending toward y axis, use x,z to calculate y.\n for(int i=0; i Y = (-D-AX-CZ)/B\n double y_ = (-param[3]-param[0]*x[j]-param[2]*z[i])/param[1];\n\n glVertex3f(float(x[j]), float(y_), float(z[i]));\n }\n }\n }\n \n glEnd();\n glPopMatrix();\n }\n\n bool MapDrawer::drawCameraState() {\n g2o::SE3Quat* cameraState = mpMap->getCameraState(); // Twc\n pangolin::OpenGlMatrix Twc;\n if( cameraState!=NULL )\n SE3ToOpenGLCameraMatrixOrigin(*cameraState, Twc);\n else\n {\n std::cerr << \"Can't load camera state.\" << std::endl;\n Twc.SetIdentity();\n }\n\n const float &w = mCameraSize*1.5;\n const float h = w*0.75;\n const float z = w*0.6;\n\n glPushMatrix();\n\n#ifdef HAVE_GLES\n glMultMatrixf(Twc.m); \n#else\n glMultMatrixd(Twc.m);\n#endif\n\n glLineWidth(mCameraLineWidth);\n glColor3f(1.0f,0.0f,0.0f);\n glBegin(GL_LINES);\n glVertex3f(0,0,0);\n glVertex3f(w,h,z);\n glVertex3f(0,0,0);\n glVertex3f(w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,-h,z);\n glVertex3f(0,0,0);\n glVertex3f(-w,h,z);\n\n glVertex3f(w,h,z);\n glVertex3f(w,-h,z);\n\n glVertex3f(-w,h,z);\n glVertex3f(-w,-h,z);\n\n glVertex3f(-w,h,z);\n glVertex3f(w,h,z);\n\n glVertex3f(-w,-h,z);\n glVertex3f(w,-h,z);\n glEnd();\n\n glPopMatrix();\n\n return true;\n }\n\n bool MapDrawer::drawTrajectory() {\n std::vector traj = mpMap->getCameraStateTrajectory();\n for(int i=0; i pPoints = mpMap->GetAllPoints();\n glPushMatrix();\n\n for(int i=0; i lock(mMutexCamera);\n Rwc = matIn.rowRange(0,3).colRange(0,3).t();\n twc = -Rwc*matIn.rowRange(0,3).col(3);\n }\n\n M.m[0] = Rwc.at(0,0);\n M.m[1] = Rwc.at(1,0);\n M.m[2] = Rwc.at(2,0);\n M.m[3] = 0.0;\n\n M.m[4] = Rwc.at(0,1);\n M.m[5] = Rwc.at(1,1);\n M.m[6] = Rwc.at(2,1);\n M.m[7] = 0.0;\n\n M.m[8] = Rwc.at(0,2);\n M.m[9] = Rwc.at(1,2);\n M.m[10] = Rwc.at(2,2);\n M.m[11] = 0.0;\n\n M.m[12] = twc.at(0);\n M.m[13] = twc.at(1);\n M.m[14] = twc.at(2);\n M.m[15] = 1.0;\n }\n else\n M.SetIdentity();\n }\n\n // not inverse, keep origin\n void MapDrawer::SE3ToOpenGLCameraMatrixOrigin(g2o::SE3Quat &matInSe3, pangolin::OpenGlMatrix &M)\n {\n // eigen to cv\n Eigen::Matrix4d matEigen = matInSe3.to_homogeneous_matrix();\n cv::Mat matIn;\n eigen2cv(matEigen, matIn);\n\n if(!matIn.empty())\n {\n cv::Mat Rwc(3,3,CV_64F);\n cv::Mat twc(3,1,CV_64F);\n {\n unique_lock lock(mMutexCamera);\n Rwc = matIn.rowRange(0,3).colRange(0,3);\n twc = matIn.rowRange(0,3).col(3);\n }\n\n M.m[0] = Rwc.at(0,0);\n M.m[1] = Rwc.at(1,0);\n M.m[2] = Rwc.at(2,0);\n M.m[3] = 0.0;\n\n M.m[4] = Rwc.at(0,1);\n M.m[5] = Rwc.at(1,1);\n M.m[6] = Rwc.at(2,1);\n M.m[7] = 0.0;\n\n M.m[8] = Rwc.at(0,2);\n M.m[9] = Rwc.at(1,2);\n M.m[10] = Rwc.at(2,2);\n M.m[11] = 0.0;\n\n M.m[12] = twc.at(0);\n M.m[13] = twc.at(1);\n M.m[14] = twc.at(2);\n M.m[15] = 1.0;\n }\n else\n M.SetIdentity();\n }\n\n void MapDrawer::setCalib(Eigen::Matrix3d& calib)\n {\n mCalib = calib;\n }\n\n void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M) {\n g2o::SE3Quat *cameraState = mpMap->getCameraState(); // Twc\n\n if (cameraState != NULL)\n SE3ToOpenGLCameraMatrixOrigin(*cameraState, M);\n else {\n M.SetIdentity();\n }\n }\n\n void MapDrawer::drawPointCloudLists()\n {\n auto pointLists = mpMap->GetPointCloudList();\n\n glPushMatrix();\n\n for(auto pair:pointLists){\n auto pPoints = pair.second;\n if( pPoints == NULL ) continue;\n for(int i=0; isize(); i=i+1)\n {\n PointXYZRGB &p = (*pPoints)[i];\n glPointSize( p.size );\n glBegin(GL_POINTS);\n glColor3d(p.r/255.0, p.g/255.0, p.b/255.0);\n glVertex3d(p.x, p.y, p.z);\n glEnd();\n\n }\n }\n glPointSize( 1 );\n\n glPopMatrix();\n }\n\n void MapDrawer::drawPointCloudWithOptions(const std::map &options)\n {\n auto pointLists = mpMap->GetPointCloudList();\n glPushMatrix();\n\n for(auto pair:pointLists){\n auto pPoints = pair.second;\n if( pPoints == NULL ) continue;\n\n auto iter = options.find(pair.first);\n if(iter == options.end()) {\n continue; // not exist\n }\n if(iter->second == false) continue; // menu is closed\n\n for(int i=0; isize(); i=i+1)\n {\n PointXYZRGB &p = (*pPoints)[i];\n glPointSize( p.size );\n glBegin(GL_POINTS);\n glColor3d(p.r/255.0, p.g/255.0, p.b/255.0);\n glVertex3d(p.x, p.y, p.z);\n glEnd();\n\n }\n }\n glPointSize( 1 );\n glPopMatrix(); \n }\n\n\n}\n" }, { "path": "src/core/Optimizer.cpp", "content": "#include \"include/core/Optimizer.h\"\n#include \"include/core/Ellipsoid.h\"\n#include \"include/core/BasicEllipsoidEdges.h\"\n\n#include \"Thirdparty/g2o/g2o/core/block_solver.h\"\n#include \"Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h\"\n#include \"Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h\"\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h\"\n#include \"Thirdparty/g2o/g2o/core/robust_kernel_impl.h\"\n#include \"Thirdparty/g2o/g2o/solvers/linear_solver_dense.h\"\n#include \"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h\"\n\n#include \n\nusing namespace cv;\n\nnamespace EllipsoidSLAM\n{\n\nbool isInImage(Eigen::Vector2d& uv, int rows, int cols){\n \n if( uv(0) >0 && uv(0) < cols )\n if( uv(1) >0 && uv(1) < rows )\n return true;\n\n return false;\n}\n\n// [ Unused ]\n// The function checks several conditions to decide whether the edge will be activated:\n// 1) if the object is behind the camera\n// 2) if the camera is inside the elipsoid, which generates an ill condition\n// 3) if the projected ellipse lies in the image width and height\n// if one of the boundingbox vertces lies in the image, active it too.\nbool checkVisibility(g2o::EdgeSE3EllipsoidProj *edge, g2o::VertexSE3Expmap *vSE3, \n g2o::VertexEllipsoid *vEllipsoid, Eigen::Matrix3d &mCalib, int rows, int cols)\n{\n g2o::ellipsoid e = vEllipsoid->estimate();\n Vector3d ellipsoid_center = e.toMinimalVector().head(3); // Pwo\n Vector4d center_homo = real_to_homo_coord_vec(ellipsoid_center);\n\n g2o::SE3Quat campose_Tcw = vSE3->estimate();\n Eigen::Matrix4d projMat = campose_Tcw.to_homogeneous_matrix(); // Tcw\n \n // project to image plane.\n Vector4d center_inCameraAxis_homo = projMat * center_homo; // Pco = Tcw * Pwo\n Vector3d center_inCameraAxis = homo_to_real_coord_vec(center_inCameraAxis_homo);\n\n if( center_inCameraAxis_homo(2) < 0) // if the object is behind the camera. z< 0\n {\n return false;\n }\n\n // check if the camera is inside the elipsoid, which generates an ill condition\n g2o::SE3Quat campose_Twc = campose_Tcw.inverse();\n Eigen::Vector3d X_cam = campose_Twc.translation();\n Eigen::Vector4d X_homo = real_to_homo_coord_vec(X_cam);\n Eigen::Matrix4d Q_star = e.generateQuadric();\n Eigen::Matrix4d Q = Q_star.inverse();\n double point_in_Q = X_homo.transpose() * Q * X_homo;\n if(point_in_Q < 0) // the center of the camera is inside the ellipsoid\n return false;\n\n // check if the projected ellipse lies in the image width and height\n Eigen::Matrix3Xd P = e.generateProjectionMatrix(campose_Tcw, mCalib);\n Eigen::Vector3d uv = P * center_homo;\n uv = uv/uv(2);\n Eigen::Vector2d uv_2d(uv(0), uv(1));\n if( isInImage(uv_2d, rows, cols) )\n return true;\n\n // if one of the boundingbox vertces lies in the image, active it too.\n Vector4d vec_proj = edge->getProject();\n Vector2d point_lu(vec_proj(0), vec_proj(1));\n Vector2d point_rd(vec_proj(2), vec_proj(3));\n if( isInImage(point_lu,rows,cols) || isInImage(point_rd, rows, cols) )\n return true;\n\n return false;\n\n}\n\nvoid Optimizer::GlobalObjectGraphOptimization(std::vector &pFrames, Map *pMap,\n int rows, int cols, Matrix3d &mCalib, std::map& objectObservations, bool save_graph, bool withAssociation, bool check_visibility) {\n // ************************ LOAD CONFIGURATION ************************\n double config_ellipsoid_3d_scale = Config::Get(\"Optimizer.Edges.3DEllipsoid.Scale\");\n bool mbSetGravityPrior = Config::Get(\"Optimizer.Edges.GravityPrior.Open\") == 1; \n double dGravityPriorScale = Config::Get(\"Optimizer.Edges.GravityPrior.Scale\");\n bool mbOpen3DProb = true; \n\n // OUTPUT\n std::cout << \" -- Optimization parameters : \" << std::endl;\n if(mbGroundPlaneSet)\n std::cout << \" [ Using Ground Plane: \" << mGroundPlaneNormal.transpose() << \" ] \" << std::endl;\n\n if(!mbGroundPlaneSet || !mbSetGravityPrior ) \n std::cout << \" * Gravity Prior : closed.\" << std::endl;\n else\n std::cout << \" * Gravity Prior : Open.\" << std::endl;\n \n cout<<\" * Scale_3dedge: \" << config_ellipsoid_3d_scale << endl;\n cout<<\" * Scale_GravityPrior: \" << dGravityPriorScale << endl;\n // ************************************************************************\n\n // Initialize variables.\n int total_frame_number = int(pFrames.size());\n std::map pEllipsoidsMapWithInstance = pMap->GetAllEllipsoidsMap();\n int objects_num = int(pEllipsoidsMapWithInstance.size());\n\n // initialize graph optimization.\n g2o::SparseOptimizer graph;\n g2o::BlockSolverX::LinearSolverType* linearSolver;\n linearSolver = new g2o::LinearSolverDense();\n g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);\n g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);\n graph.setAlgorithm(solver);\n graph.setVerbose(false); // Set output.\n\n std::map vEllipsoidVertexMaps;\n std::vector edges, edgesValid, edgesInValid;\n std::vector validVec; validVec.resize(total_frame_number);\n std::vector edgesEllipsoidGravityPlanePrior; // Gravity prior\n std::vector vSE3Vertex;\n\n // Add SE3 vertices for camera poses\n bool bSLAM_mode = false; // Mapping Mode : Fix camera poses and mapping ellipsoids only\n for( int frame_index=0; frame_index< total_frame_number ; frame_index++) {\n g2o::SE3Quat curr_cam_pose_Twc = pFrames[frame_index]->cam_pose_Twc;\n Eigen::MatrixXd det_mat = pFrames[frame_index]->mmObservations;\n\n g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();\n vSE3->setId(graph.vertices().size());\n graph.addVertex(vSE3);\n vSE3->setEstimate(pFrames[frame_index]->cam_pose_Tcw); // Tcw\n if(!bSLAM_mode)\n vSE3->setFixed(true); // Fix all the poses in mapping mode.\n else \n vSE3->setFixed(frame_index == 0); \n vSE3Vertex.push_back(vSE3);\n\n // Add odom edges if in SLAM Mode\n if(bSLAM_mode && frame_index > 0){\n g2o::SE3Quat prev_cam_pose_Tcw = pFrames[frame_index-1]->cam_pose_Twc.inverse();\n g2o::SE3Quat curr_cam_pose_Tcw = curr_cam_pose_Twc.inverse();\n g2o::SE3Quat odom_val = curr_cam_pose_Tcw*prev_cam_pose_Tcw.inverse();;\n\n g2o::EdgeSE3Expmap* e = new g2o::EdgeSE3Expmap();\n e->setVertex(0, dynamic_cast( vSE3Vertex[frame_index-1] ));\n e->setVertex(1, dynamic_cast( vSE3Vertex[frame_index] ));\n e->setMeasurement(odom_val);\n\n e->setId(graph.edges().size());\n Vector6d inv_sigma;inv_sigma<<1,1,1,1,1,1;\n inv_sigma = inv_sigma*1.0;\n Matrix6d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();\n e->setInformation(info);\n graph.addEdge(e);\n }\n }\n\n // Initialize objects vertices and add edges of camera-objects 2d observations \n int objects_ob_num = objectObservations.size();\n int objectid_in_edge = 0;\n int current_ob_id = 0; \n int symplaneid_in_edge = 0;\n for(auto instanceObs : objectObservations ) \n {\n int instance = instanceObs.first;\n if (pEllipsoidsMapWithInstance.find(instance) == pEllipsoidsMapWithInstance.end()) // if the instance has not been initialized yet\n continue;\n\n Observations obs = instanceObs.second;\n\n // Add objects vertices\n g2o::VertexEllipsoid *vEllipsoid = new g2o::VertexEllipsoid();\n vEllipsoid->setEstimate(*pEllipsoidsMapWithInstance[instance]);\n vEllipsoid->setId(graph.vertices().size());\n vEllipsoid->setFixed(false);\n graph.addVertex(vEllipsoid);\n vEllipsoidVertexMaps.insert(make_pair(instance, vEllipsoid));\n\n // Add gravity prior\n if(mbGroundPlaneSet && mbSetGravityPrior ){\n g2o::EdgeEllipsoidGravityPlanePrior *vGravityPriorEdge = new g2o::EdgeEllipsoidGravityPlanePrior;\n vGravityPriorEdge->setVertex(0, dynamic_cast( vEllipsoid ));\n\n vGravityPriorEdge->setMeasurement(mGroundPlaneNormal); \n Matrix inv_sigma;\n inv_sigma << 1 * dGravityPriorScale;\n MatrixXd info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();\n vGravityPriorEdge->setInformation(info);\n \n graph.addEdge(vGravityPriorEdge);\n edgesEllipsoidGravityPlanePrior.push_back(vGravityPriorEdge);\n \n }\n\n // Add camera-objects 2d constraints \n // At least 3 observations are needed to activiate 2d edges. Since one ellipsoid has 9 degrees,\n // and every observation offers 4 constrains, only at least 3 observations could fully constrain an ellipsoid.\n bool bVvalid_2d_constraints = (obs.size() > 2);\n if( bVvalid_2d_constraints ){\n for( int i=0; ilabel;\n Vector4d measure_bbox = vOb->bbox;\n double measure_prob = vOb->rate;\n\n // find the corresponding frame vertex.\n int frame_index = vOb->pFrame->frame_seq_id;\n g2o::VertexSE3Expmap *vSE3 = vSE3Vertex[frame_index]; \n // create 2d edge\n g2o::EdgeSE3EllipsoidProj *e = new g2o::EdgeSE3EllipsoidProj();\n e->setVertex(0, dynamic_cast( vSE3 ));\n e->setVertex(1, dynamic_cast( vEllipsoidVertexMaps[instance] ));\n e->setMeasurement(measure_bbox); \n e->setId(graph.edges().size()); \n Vector4d inv_sigma;\n inv_sigma << 1, 1, 1, 1; \n Matrix4d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();\n info = info * measure_prob;\n e->setInformation(info);\n // e->setRobustKernel(new g2o::RobustKernelHuber()); // Huber Kernel\n e->setKalib(mCalib);\n e->setLevel(0);\n edges.push_back(e);\n\n // Two conditions for valid 2d edges:\n // 1) [closed] visibility check: see the comments of function checkVisibility for detail.\n // 2) NaN check\n bool c1 = (!check_visibility) || checkVisibility(e, vSE3, vEllipsoidVertexMaps[instance], mCalib, rows, cols);\n\n e->computeError();\n double e_error = e->chi2();\n bool c2 = !isnan(e_error); // NaN check\n\n if( c1 && c2 ){\n graph.addEdge(e);\n edgesValid.push_back(e); // valid edges and invalid edges are for debug output\n }\n else\n edgesInValid.push_back(e); \n }\n }\n }\n\n // Add 3d constraints\n std::vector vEllipsoid3DEdges;\n for( int frame_index=0; frame_index< total_frame_number ; frame_index++) {\n auto &ObjectsInFrame = pFrames[frame_index]->mpLocalObjects;\n for( auto obj : ObjectsInFrame )\n {\n if( obj == NULL ) continue;\n int instance = obj->miInstanceID;\n if(vEllipsoidVertexMaps.find(instance) == vEllipsoidVertexMaps.end())\n continue; // if the instance has not been initialized\n \n auto vEllipsoid = vEllipsoidVertexMaps[instance]; \n auto vSE3 = vSE3Vertex[frame_index];\n\n // create 3d edges\n g2o::EdgeSE3Ellipsoid9DOF* vEllipsoid3D = new g2o::EdgeSE3Ellipsoid9DOF; \n vEllipsoid3D->setId(graph.edges().size()); \n vEllipsoid3D->setVertex(0, dynamic_cast( vSE3 ));\n vEllipsoid3D->setVertex(1, dynamic_cast( vEllipsoidVertexMaps[instance] )); \n vEllipsoid3D->setMeasurement(*obj); \n\n Vector9d inv_sigma;\n inv_sigma << 1,1,1,1,1,1,1,1,1;\n if(mbOpen3DProb)\n inv_sigma = inv_sigma * sqrt(obj->prob);\n Matrix9d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal() * config_ellipsoid_3d_scale;\n vEllipsoid3D->setInformation(info);\n\n graph.addEdge(vEllipsoid3D);\n vEllipsoid3DEdges.push_back(vEllipsoid3D);\n }\n }\n\n // output \n std::cout << \" -- GRAPH INFORMATION : \" << std::endl;\n cout << \" * Object Num : \" << objects_num << endl;\n cout<<\" * Vertices: \"<estimate();\n }\n else\n {\n cerr << \"Find Error in ellipsoid storage. \" << endl;\n cout << \"Map: instance\" << (*iter).first << endl;\n }\n }\n\n // Output optimization information.\n // object list\n ofstream out_obj(\"./object_list.txt\");\n auto iter = vEllipsoidVertexMaps.begin();\n for(;iter!=vEllipsoidVertexMaps.end();iter++)\n {\n out_obj << iter->first << \"\\t\" << iter->second->estimate().toMinimalVector().transpose() \n << \"\\t\" << iter->second->estimate().miLabel << std::endl;\n }\n out_obj.close();\n}\n\nOptimizer::Optimizer()\n{\n mbGroundPlaneSet = false;\n}\n\nvoid Optimizer::SetGroundPlane(Vector4d& normal){\n mbGroundPlaneSet = true;\n mGroundPlaneNormal = normal;\n}\n\n}\n" }, { "path": "src/core/Plane.cpp", "content": "#include \"include/core/Plane.h\"\n\nnamespace g2o\n{\n plane::plane() {\n param = Vector4d(1,0,0,0);\n color = Vector3d(1,0,0);\n mdDualDis = 0;\n\n mbLimited = false;\n }\n\n plane::plane(Vector4d param_, Eigen::Vector3d color_) {\n param = param_;\n color = color_;\n mdDualDis = 0;\n\n mbLimited = false;\n }\n\n plane plane::exp_update(const Vector3d& update)\n {\n g2o::plane plane_update;\n\n plane_update.param[0] = param[0] + update[0]; // A\n plane_update.param[1] = param[1] + update[1]; // B\n plane_update.param[2] = 0; // C \n plane_update.param[3] = param[3] + update[2]; // D\n plane_update.color = color;\n\n return plane_update;\n }\n\n plane plane::exp_update2DOF(const Vector2d& update)\n {\n double k_update = update[0];\n double b_update = update[1];\n\n double k_current = -param[0]/param[1];\n double b_current = -param[3]/param[1];\n\n double k = k_current + k_update;\n double b = b_current + b_update;\n \n double B = -1;\n double A = k;\n double C = 0;\n double D = b;\n\n g2o::plane plane_update;\n plane_update.param[0] = A; // A\n plane_update.param[1] = B; // B\n plane_update.param[2] = C; // C ; z remains unchanged\n plane_update.param[3] = D; // D\n plane_update.color = color;\n\n return plane_update;\n }\n\n plane::plane(const plane &p){\n param = p.param;\n color = p.color;\n\n mbLimited = p.mbLimited;\n mvPlaneCenter = p.mvPlaneCenter;\n mdPlaneSize = p.mdPlaneSize;\n\n mdDualDis = p.mdDualDis;\n }\n\n const plane& plane::operator=(const plane& p){\n param = p.param;\n color = p.color;\n\n mbLimited = p.mbLimited;\n mvPlaneCenter = p.mvPlaneCenter;\n mdPlaneSize = p.mdPlaneSize;\n\n mdDualDis = p.mdDualDis;\n return p;\n }\n\n void plane::fromPointAndNormal(const Vector3d &point, const Vector3d &normal)\n {\n param.head(3) = normal; // normal : [ a, b, c]^T\n param[3] = -point.transpose() * normal; // X^T * pi = 0 ; ax0+by0+cz0+d=0\n color = Vector3d(1,0,0);\n\n mdDualDis = 0;\n }\n\n void plane::fromDisAndAngle(double dis, double angle)\n {\n fromDisAngleTrans(dis, angle, 0);\n }\n\n void plane::fromDisAngleTrans(double dis, double angle, double trans)\n {\n param[0] = sin(angle);\n param[1] = -cos(angle);\n param[2] = 0;\n param[3] = -dis;\n\n mdDualDis = trans;\n }\n\n\n double plane::distanceToPoint(const Vector3d& point, bool keep_flag){\n double fenzi = param(0)*point(0) + param(1)*point(1) +param(2)*point(2) + param(3);\n double fenmu = std::sqrt ( param(0)*param(0)+param(1)*param(1)+param(2)*param(2) );\n double value = fenzi/fenmu;\n\n if(keep_flag) return value;\n else return std::abs(value);\n }\n\n void plane::transform(g2o::SE3Quat& Twc){\n Matrix4d matTwc = Twc.to_homogeneous_matrix();\n Matrix4d matTwc_trans = matTwc.transpose();\n Matrix4d matTwc_trans_inv = matTwc_trans.inverse();\n param = matTwc_trans_inv * param;\n }\n\n // for the visualization of symmetry planes\n void plane::InitFinitePlane(const Vector3d& center, double size)\n {\n mdPlaneSize = size;\n mvPlaneCenter = center;\n mbLimited = true;\n }\n\n Eigen::Vector4d plane::GeneratePlaneVec()\n {\n return param;\n }\n\n Eigen::Vector4d plane::GenerateAnotherPlaneVec()\n {\n // azimuth : angle of the normal\n g2o::plane p2;\n p2.fromDisAndAngle(mdDualDis, azimuth());\n\n return p2.param;\n }\n\n Vector3d plane::GetLineFromCenterAngle(const Vector2d center, double angle)\n {\n // x = center[0] + t * cos(theta)\n // y = center[1] + t * sin(theta)\n // goal : \n // AX + BY + C = 0 ; get A,B,C\n\n // get rid of t:\n // sin(theta) * x - cos(theta) * y = \n // sin(theta) * center[0] - cos(theta) * center[1]\n \n // so: \n // sint * x + (- cost) * y + (cost*c1 - sint*c0) = 0\n\n Vector3d param;\n param[0] = sin(angle);\n param[1] = -cos(angle);\n param[2] = cos(angle) * center[1] - sin(angle) * center[0];\n\n return param;\n }\n\n Vector4d plane::LineToPlane(const Vector3d line)\n {\n Vector4d plane;\n plane << line[0], line[1], 0, line[2];\n return plane;\n }\n\n} // g2o namespace" }, { "path": "src/core/System.cpp", "content": "#include \"include/core/System.h\"\n#include \"include/utils/dataprocess_utils.h\"\n\n#include \"src/config/Config.h\"\n\nnamespace EllipsoidSLAM\n{\n\n System::System(const string &strSettingsFile, const bool bUseViewer) {\n\n cout << endl <<\n \"EllipsoidSLAM Project 2019, Beihang University.\" << endl;\n cout << \" Input Sensor: RGB-D \" << endl;\n\n cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);\n if(!fsSettings.isOpened())\n {\n cerr << \"Failed to open settings file at: \" << strSettingsFile << endl;\n exit(-1);\n }\n \n // Initialize global settings.\n Config::Init();\n Config::SetParameterFile(strSettingsFile);\n\n //Create the Map\n mpMap = new Map();\n\n mpFrameDrawer = new FrameDrawer(mpMap);\n\n //Create Drawers. These are used by the Viewer\n mpMapDrawer = new MapDrawer(strSettingsFile, mpMap);\n\n mpTracker = new Tracking(this, mpFrameDrawer, mpMapDrawer, mpMap, strSettingsFile);\n mpFrameDrawer->setTracker(mpTracker);\n \n //Initialize the Viewer thread and launch\n if(bUseViewer)\n {\n mpViewer = new Viewer(this, strSettingsFile, mpMapDrawer);\n mptViewer = new thread(&Viewer::run, mpViewer);\n mpViewer->SetFrameDrawer(mpFrameDrawer);\n }\n\n OpenDepthEllipsoid(); // Open Single-Frame Ellipsoid Extraction\n mpTracker->OpenGroundPlaneEstimation(); // Open Groundplane Estimation.\n }\n\n bool System::TrackWithObjects(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd & bboxMat, const cv::Mat &imDepth, const cv::Mat &imRGB,\n bool withAssociation)\n {\n return mpTracker->GrabPoseAndObjects(timestamp, pose, bboxMat, imDepth, imRGB, withAssociation);\n }\n\n Map* System::getMap() {\n return mpMap;\n }\n\n MapDrawer* System::getMapDrawer() {\n return mpMapDrawer;\n }\n\n FrameDrawer* System::getFrameDrawer() {\n return mpFrameDrawer;\n }\n\n Viewer* System::getViewer() {\n return mpViewer;\n }\n\n Tracking* System::getTracker() {\n return mpTracker;\n }\n\n void System::SaveObjectsToFile(string &path){\n auto ellipsoids = mpMap->GetAllEllipsoids();\n \n MatrixXd objMat;objMat.resize(ellipsoids.size(), 11);\n int i=0;\n for(auto e : ellipsoids)\n {\n Vector10d vec = e->toVector();\n Eigen::Matrix vec_instance;\n vec_instance << e->miInstanceID, vec;\n objMat.row(i++) = vec_instance;\n }\n\n saveMatToFile(objMat, path.c_str());\n\n std::cout << \"Save \" << ellipsoids.size() << \" objects to \" << path << std::endl;\n }\n\n void System::OpenDepthEllipsoid()\n {\n mpTracker->OpenDepthEllipsoid();\n }\n\n void System::OpenOptimization()\n {\n mpTracker->OpenOptimization();\n }\n\n void System::CloseOptimization()\n {\n mpTracker->CloseOptimization();\n }\n}\n\n\n\n" }, { "path": "src/core/Tracking.cpp", "content": "#include \"include/core/Tracking.h\"\n#include \"src/config/Config.h\"\n#include \"utils/dataprocess_utils.h\"\n\nEigen::MatrixXd matSymPlanes;\n\nnamespace EllipsoidSLAM\n{\n void outputObjectObservations(std::map &mmObjectObservations)\n {\n ofstream out(\"./log_mmObjectObservations.txt\");\n \n out << \" --------- ObjectObservations : \" << std::endl;\n for ( auto obPair: mmObjectObservations)\n {\n out << \" ---- Instance \" << obPair.first << \" (\" << obPair.second.size() << \") :\" << std::endl;\n\n for( auto ob : obPair.second )\n {\n out << \" -- ob : \" << ob->pFrame->frame_seq_id << \" | \" << ob->bbox.transpose() << \" | \" << ob->label << \" | \" << ob->rate << std::endl;\n }\n\n out << std::endl;\n }\n\n out.close();\n std::cout << \"Save to log_mmObjectObservations.txt...\" << std::endl;\n }\n\n void Tracking::outputBboxMatWithAssociation()\n {\n std::map mapTimestampToObservations;\n\n for ( auto obPair: mmObjectObservations)\n {\n int instance = obPair.first;\n for( auto ob : obPair.second )\n {\n ob->instance = instance;\n\n // save with timestamp\n if(mapTimestampToObservations.find(ob->pFrame->timestamp)!=mapTimestampToObservations.end())\n mapTimestampToObservations[ob->pFrame->timestamp].push_back(ob);\n else {\n mapTimestampToObservations.insert(make_pair(ob->pFrame->timestamp, Observations()));\n mapTimestampToObservations[ob->pFrame->timestamp].push_back(ob);\n }\n }\n }\n\n for( auto frameObsPair : mapTimestampToObservations ){\n string str_timestamp = to_string(frameObsPair.first);\n\n string filename = string(\"./bbox/\") + str_timestamp + \".txt\";\n ofstream out(filename.c_str());\n \n int num = 0;\n for ( auto ob: frameObsPair.second)\n {\n \n out << num++ << \" \" << ob->bbox.transpose() << \" \" << ob->label << \" \" << ob->rate << \" \" << ob->instance << std::endl;\n }\n\n out.close();\n std::cout << \"Save to \" << filename << std::endl;\n }\n\n std::cout << \"Finish... \" << std::endl;\n \n }\n\n Tracking::Tracking(EllipsoidSLAM::System *pSys, EllipsoidSLAM::FrameDrawer *pFrameDrawer,\n EllipsoidSLAM::MapDrawer *pMapDrawer, EllipsoidSLAM::Map *pMap, const string &strSettingPath)\n :mpMap(pMap), mpSystem(pSys), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer)\n {\n cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);\n float fx = fSettings[\"Camera.fx\"];\n float fy = fSettings[\"Camera.fy\"];\n float cx = fSettings[\"Camera.cx\"];\n float cy = fSettings[\"Camera.cy\"];\n\n cv::Mat K = cv::Mat::eye(3,3,CV_32F);\n K.at(0,0) = fx;\n K.at(1,1) = fy;\n K.at(0,2) = cx;\n K.at(1,2) = cy;\n K.copyTo(mK);\n\n cv::Mat DistCoef(4,1,CV_32F);\n DistCoef.at(0) = fSettings[\"Camera.k1\"];\n DistCoef.at(1) = fSettings[\"Camera.k2\"];\n DistCoef.at(2) = fSettings[\"Camera.p1\"];\n DistCoef.at(3) = fSettings[\"Camera.p2\"];\n const float k3 = fSettings[\"Camera.k3\"];\n if(k3!=0)\n {\n DistCoef.resize(5);\n DistCoef.at(4) = k3;\n }\n DistCoef.copyTo(mDistCoef);\n\n mbf = fSettings[\"Camera.bf\"];\n\n float fps = fSettings[\"Camera.fps\"];\n if(fps==0)\n fps=30;\n\n int rows = fSettings[\"Camera.height\"];\n int cols = fSettings[\"Camera.width\"];\n\n mCalib << fx, 0, cx,\n 0, fy, cy,\n 0, 0, 1;\n\n mCamera.cx = cx;\n mCamera.cy = cy;\n mCamera.fx = fx;\n mCamera.fy = fy;\n mCamera.scale = fSettings[\"Camera.scale\"];\n\n mpInitializer = new Initializer(rows, cols);\n mpOptimizer = new Optimizer;\n mRows = rows;\n mCols = cols;\n\n mbDepthEllipsoidOpened = false;\n\n mbOpenOptimization = true;\n\n pDASolver = new DataAssociationSolver(mpMap);\n mpBuilder = new Builder();\n mpBuilder->setCameraIntrinsic(mCalib, mCamera.scale);\n\n mCurrFrame = NULL;\n\n // output\n cout << endl << \"Camera Parameters: \" << endl;\n cout << \"- fx: \" << fx << endl;\n cout << \"- fy: \" << fy << endl;\n cout << \"- cx: \" << cx << endl;\n cout << \"- cy: \" << cy << endl;\n cout << \"- k1: \" << DistCoef.at(0) << endl;\n cout << \"- k2: \" << DistCoef.at(1) << endl;\n if(DistCoef.rows==5)\n cout << \"- k3: \" << DistCoef.at(4) << endl;\n cout << \"- p1: \" << DistCoef.at(2) << endl;\n cout << \"- p2: \" << DistCoef.at(3) << endl;\n cout << \"- fps: \" << fps << endl;\n cout << \"- rows: \" << rows << endl;\n cout << \"- cols: \" << cols << endl;\n cout << \"- Scale: \" << mCamera.scale << endl;\n\n // ********** DEBUG ***********\n matSymPlanes.resize(0, 5);\n }\n\n g2o::ellipsoid* Tracking::getObjectDataAssociation(const Eigen::VectorXd &pose, const Eigen::VectorXd &detection) {\n auto objects = mpMap->GetAllEllipsoids();\n if( objects.size() > 0 )\n return objects[0];\n else\n return NULL;\n }\n\n bool Tracking::GrabPoseAndObjects(const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap,\n const cv::Mat &imDepth, const cv::Mat &imRGB, bool withAssociation) {\n return GrabPoseAndObjects(0, pose, bboxMap, imDepth, imRGB, withAssociation);\n }\n\n bool Tracking::GrabPoseAndObjects(double timestamp, const Eigen::VectorXd &pose, const Eigen::MatrixXd &bboxMap,\n const cv::Mat &imDepth, const cv::Mat &imRGB, bool withAssociation) {\n\n clock_t time_start = clock(); \n\n Frame *pF = new Frame(timestamp, pose, bboxMap, imDepth, imRGB);\n mvpFrames.push_back(pF);\n mCurrFrame = pF;\n\n clock_t time_init_frame_process = clock();\n\n UpdateObjectObservation(mCurrFrame, withAssociation); // Store object observation in a specific data structure.\n clock_t time_UpdateObjectObservation = clock();\n\n JudgeInitialization();\n\n if(mbOpenOptimization){\n ProcessCurrentFrame(withAssociation);\n }\n clock_t time_ProcessCurrentFrame = clock();\n\n // Visualization\n ProcessVisualization();\n clock_t time_Visualization = clock();\n\n // // Output running time\n // cout << \" - System Time: \" << endl;\n // cout << \" -- time_init_frame_process: \" <<(double)(time_init_frame_process - time_start) / CLOCKS_PER_SEC << \"s\" << endl; \n // cout << \" -- time_UpdateObjectObservation: \" <<(double)(time_UpdateObjectObservation - time_init_frame_process) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \" -- time_ProcessCurrentFrame: \" <<(double)(time_ProcessCurrentFrame - time_UpdateObjectObservation) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \" -- time_Visualization: \" <<(double)(time_Visualization - time_ProcessCurrentFrame) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \" - [ total_frame: \" <<(double)(time_Visualization - time_start) / CLOCKS_PER_SEC << \"s ]\" << endl;\n\n return true;\n }\n\n void Tracking::ProcessVisualization()\n {\n // Visualize frames with intervals\n if( isKeyFrameForVisualization() )\n mpMap->addCameraStateToTrajectory(&mCurrFrame->cam_pose_Twc);\n mpMap->setCameraState(&mCurrFrame->cam_pose_Twc);\n\n // Render rgb images and depth images for visualization.\n cv::Mat imForShow = mpFrameDrawer->drawFrame();\n cv::Mat imForShowDepth = mpFrameDrawer->drawDepthFrame();\n \n }\n\n void Tracking::ProcessCurrentFrame(bool withAssociation){\n clock_t time_start = clock(); \n\n double depth_range = Config::ReadValue(\"EllipsoidExtractor_DEPTH_RANGE\"); // Only consider pointcloud within depth_range\n\n // Begin Global Optimization when there are objects in map.\n if(mpMap->GetAllEllipsoids().size()>0){\n mpOptimizer->GlobalObjectGraphOptimization(mvpFrames, mpMap, mRows, mCols, mCalib, mmObjectObservations, true, withAssociation); \n\n RefreshObjectHistory();\n }\n clock_t time_optimization = clock(); \n\n // [A visualization tool] When Builder is opened, it generates local pointcloud from depth and rgb images of current frame,\n // and global pointcloud by simply adding local pointcloud in world coordinate and then downsampling them for visualization. \n bool mbOpenBuilder = Config::Get(\"Visualization.Builder.Open\") == 1;\n if(mbOpenBuilder)\n {\n if(!mCurrFrame->rgb_img.empty()){ // RGB images are needed.\n Eigen::VectorXd pose = mCurrFrame->cam_pose_Twc.toVector();\n mpBuilder->processFrame(mCurrFrame->rgb_img, mCurrFrame->frame_img, pose, depth_range);\n\n mpBuilder->voxelFilter(0.01); // Down sample threshold; smaller the finer; depend on the hardware.\n PointCloudPCL::Ptr pCloudPCL = mpBuilder->getMap();\n PointCloudPCL::Ptr pCurrentCloudPCL = mpBuilder->getCurrentMap();\n\n auto pCloud = pclToQuadricPointCloudPtr(pCloudPCL);\n auto pCloudLocal = pclToQuadricPointCloudPtr(pCurrentCloudPCL);\n mpMap->AddPointCloudList(\"Builder.Global Points\", pCloud);\n mpMap->AddPointCloudList(\"Builder.Local Points\", pCloudLocal);\n }\n }\n clock_t time_builder = clock(); \n\n // // Output running time\n // cout << \" -- ProcessCurrentFrame Time: \" << endl;\n // cout << \" --- time_optimization: \" <<(double)(time_optimization - time_start) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \" --- time_builder: \" <<(double)(time_builder - time_optimization) / CLOCKS_PER_SEC << \"s\" << endl;\n }\n\n void AddSegCloudsToQuadricStorage(std::vector::Ptr>& segClouds, EllipsoidSLAM::PointCloud* pSegCloud){\n int cloud_num = segClouds.size();\n srand(time(0));\n for(int i=0;ipoints.size();\n int r = rand()%155;\n int g = rand()%155;\n int b = rand()%155;\n for( int n=0;npoints[n].x;\n point.y = segClouds[i]->points[n].y;\n point.z = segClouds[i]->points[n].z;\n point.r = r;\n point.g = g;\n point.b = b;\n\n point.size = 2;\n pSegCloud->push_back(point);\n }\n \n }\n\n return;\n\n }\n\n // Process Ellipsoid Estimation for every boundingboxes in current frame.\n // Finally, store 3d Ellipsoids into the membter variable mpLocalObjects of pFrame.\n void Tracking::UpdateDepthEllipsoidEstimation(EllipsoidSLAM::Frame* pFrame, bool withAssociation)\n {\n Eigen::MatrixXd &obs_mat = pFrame->mmObservations;\n int rows = obs_mat.rows();\n\n Eigen::VectorXd pose = pFrame->cam_pose_Twc.toVector();\n EllipsoidSLAM::PointCloud* pCenterCloud = new EllipsoidSLAM::PointCloud;\n EllipsoidSLAM::PointCloud* pSegCloud = new EllipsoidSLAM::PointCloud;\n\n mpEllipsoidExtractor->ClearPointCloudList(); // clear point cloud visualization\n\n bool bPlaneNotClear = true;\n for(int i=0;i(\"Measurement.Border.Pixels\"), Config::Get(\"Measurement.LengthLimit.Pixels\"));\n\n g2o::ellipsoid* pE_extracted = NULL;\n // 2 conditions must meet to start ellipsoid extraction:\n // C1 : the bounding box is not on border\n bool c1 = !is_border;\n\n // C2 : the groundplane has been estimated successfully\n bool c2 = miGroundPlaneState == 2;\n \n // in condition 3, it will not start\n // C3 : under with association mode, and the association is invalid, no need to extract ellipsoids again.\n bool c3 = false;\n if( withAssociation )\n {\n int instance = round(det_vec(7));\n if ( instance < 0 ) c3 = true; // invalid instance\n }\n \n if( c1 && c2 && !c3 ){ \n g2o::ellipsoid e_extractByFitting_newSym = mpEllipsoidExtractor->EstimateLocalEllipsoid(pFrame->frame_img, measurement, label, pose, mCamera);\n\n if(bPlaneNotClear){\n mpMap->clearPlanes();\n if(miGroundPlaneState == 2) // if the groundplane has been estimated\n mpMap->addPlane(&mGroundPlane);\n bPlaneNotClear = false;\n }\n\n if( mpEllipsoidExtractor->GetResult() )\n {\n g2o::ellipsoid *pE_extractByFitting = new g2o::ellipsoid(e_extractByFitting_newSym);\n // std::cout << \"[Tracking.cpp] Get an Ellipsoid [fixSym]: \" << e_extractByFitting_newSym.toMinimalVector().transpose() << std::endl;\n\n pE_extracted = pE_extractByFitting; // Store result to pE_extracted.\n\n // Visualize estimated ellipsoid\n g2o::ellipsoid* pObjByFitting = new g2o::ellipsoid(e_extractByFitting_newSym.transform_from(pFrame->cam_pose_Twc));\n pObjByFitting->setColor(Vector3d(0.8,0.0,0.0), 1); // Set green color\n mpMap->addEllipsoidVisual(pObjByFitting);\n \n // Visualize symmetry plane\n SymmetryOutputData symOutputData = mpEllipsoidExtractor->GetSymmetryOutputData();\n if(symOutputData.result){\n Vector3d planeCenter = pObjByFitting->pose.translation();\n if(symOutputData.symmetryType == 1){\n g2o::plane* pSymPlane = new g2o::plane(symOutputData.planeVec, Vector3d(1.0,0,0.0));\n pSymPlane->InitFinitePlane(planeCenter, 1); \n mpMap->addPlane(pSymPlane);\n }\n else if(symOutputData.symmetryType == 2) // dual reflection \n {\n g2o::plane* pSymPlane = new g2o::plane(symOutputData.planeVec, Vector3d(0.0,0.8,0.0));\n pSymPlane->InitFinitePlane(planeCenter, 1); \n mpMap->addPlane(pSymPlane);\n\n g2o::plane* pSymPlane2 = new g2o::plane(symOutputData.planeVec2, Vector3d(0.0,0.8,0));\n pSymPlane2->InitFinitePlane(planeCenter, 1); \n mpMap->addPlane(pSymPlane2);\n }\n\n // output ; Save every estimated symmetry parameter for every frame.\n VectorXd symPlaneVec; symPlaneVec.resize(5); // Param(4) ; error\n symPlaneVec << symOutputData.planeVec, symOutputData.prob;\n addVecToMatirx(matSymPlanes, symPlaneVec);\n }\n\n } // successful estimation.\n\n }\n pFrame->mpLocalObjects.push_back(pE_extracted);\n }\n }\n\n void Tracking::Update3DObservationDataAssociation(EllipsoidSLAM::Frame* pFrame, std::vector& associations, std::vector& KeyFrameChecks)\n {\n int num = associations.size();\n\n if( mbDepthEllipsoidOpened )\n {\n std::vector pLocalObjects = pFrame->mpLocalObjects;\n\n for( int i=0; impLocalObjects[i] = NULL;\n continue;\n }\n\n // Save 3D observations\n Observation3D* pOb3d = new Observation3D;\n pOb3d->pFrame = pFrame;\n pOb3d->pObj = pLocalObjects[i];\n mmObjectObservations3D[instance].push_back(pOb3d);\n\n // Set instance to the ellipsoid according to the associations\n pLocalObjects[i]->miInstanceID = instance;\n }\n }\n\n return;\n }\n\n // Consider key observations for every object instances.\n // key observations: two valid observations for the same instance should have enough intervals( distance or angles between the two poses ).\n std::vector Tracking::checkKeyFrameForInstances(std::vector& associations)\n {\n double CONFIG_KEYFRAME_DIS;\n double CONFIG_KEYFRAME_ANGLE;\n\n if( Config::Get(\"Tracking.KeyFrameCheck.Close\") == 1)\n {\n CONFIG_KEYFRAME_DIS = 0; \n CONFIG_KEYFRAME_ANGLE = 0; \n }\n else\n {\n CONFIG_KEYFRAME_DIS = 0.4; \n CONFIG_KEYFRAME_ANGLE = CV_PI/180.0*15; \n }\n\n int num =associations.size();\n std::vector checks; checks.resize(num);\n fill(checks.begin(), checks.end(), false);\n for( int i=0;ipFrame->cam_pose_Twc;\n g2o::SE3Quat &pose_curr_wc = mCurrFrame->cam_pose_Twc;\n\n g2o::SE3Quat pose_diff = pose_curr_wc.inverse() * pose_last_wc;\n double dis = pose_diff.translation().norm();\n\n Eigen::Quaterniond quat = pose_diff.rotation();\n Eigen::AngleAxisd axis(quat);\n double angle = axis.angle();\n\n if( dis > CONFIG_KEYFRAME_DIS || angle > CONFIG_KEYFRAME_ANGLE)\n checks[i] = true;\n else\n checks[i] = false;\n }\n }\n }\n return checks;\n }\n\n // for the mannual data association, \n // this function will directly return the results of [instance] in the object detection matrix\n // PS. one row of detection matrix is : id x1 y1 x2 y2 label rate instanceID\n std::vector Tracking::GetMannualAssociation(Eigen::MatrixXd &obsMat)\n {\n int num = obsMat.rows();\n std::vector associations; associations.resize(num);\n for( int i=0; iClearEllipsoidsVisual(); // Clear the Visual Ellipsoids in the map\n\n // [1] Process 3d observations\n // 1.1 process groundplane estimation\n if(miGroundPlaneState == 1) // State 1: Groundplane estimation opened, and not done yet.\n ProcessGroundPlaneEstimation();\n\n // 1.2 process single-frame ellipsoid estimation\n if( mbDepthEllipsoidOpened )\n UpdateDepthEllipsoidEstimation(pFrame, withAssociation);\n\n // 1.3 process data association\n // if data association is given, directly pass on; \n // if not, the DASolver will solve it automatically by comparing the estimated ellipsoid with those ellipsoids in the map. \n // This function is not steady and still under Test.\n std::vector associations;\n if(withAssociation)\n {\n associations = GetMannualAssociation(pFrame->mmObservations);\n }\n else\n {\n assert( mbDepthEllipsoidOpened && \"ATTENTION: ONLY 3D MODE IS SUPPORTED FOR AUTOMATIC DATA ASSOCIATION NOW.\" );\n associations = pDASolver->Solve(pFrame, mbDepthEllipsoidOpened);\n } \n \n std::vector KeyFrameChecks = checkKeyFrameForInstances(associations); // Check whether they are key observations\n\n // Save data associations to the member variable of pFrame\n Update3DObservationDataAssociation(pFrame, associations, KeyFrameChecks);\n \n // [2] Process 2D observations\n Eigen::MatrixXd &obs_mat = pFrame->mmObservations;\n int rows = obs_mat.rows();\n\n // load parameters\n int config_border_pixels = Config::Get(\"Measurement.Border.Pixels\");\n int config_lengthlimit_pixels = Config::Get(\"Measurement.LengthLimit.Pixels\");\n for(int i=0;ilabel = label;\n pOb->bbox = measurement;\n pOb->rate = det_vec(6);\n pOb->pFrame = pFrame;\n\n // Save 2d observations in mmObjectObservations, which will be loaded into the Optimization.\n if( mmObjectObservations.find(instance) != mmObjectObservations.end())\n mmObjectObservations[instance].push_back(pOb);\n else{\n Observations obs;\n obs.push_back(pOb);\n mmObjectObservations.insert(make_pair(instance, obs));\n }\n }\n }\n\n // Initilize those instances that have enough observations.\n void Tracking::JudgeInitialization() {\n\n // 1. Consider 2d initialization\n int CONFIG_MINIMUM_INITIALIZATION_FRAME = Config::ReadValue(\"Tracking_MINIMUM_INITIALIZATION_FRAME\");\n\n std::vector objects = mpMap->GetAllEllipsoids();\n\n std::set existInstances; // Save the existed instances in the map\n for(auto iter=objects.begin(); iter!=objects.end(); iter++)\n {\n existInstances.insert((*iter)->miInstanceID); \n }\n\n // outputObjectObservations(mmObjectObservations);\n for(auto iter=mmObjectObservations.begin(); iter!=mmObjectObservations.end(); iter++)\n {\n if( existInstances.find(iter->first) == existInstances.end() )\n {\n // if the instance has not been initialized\n Observations obs = iter->second;\n int config_frame_num = obs.size();\n if(config_frame_num < CONFIG_MINIMUM_INITIALIZATION_FRAME) continue; // if there are enough observations\n\n ellipsoid e = mpInitializer->initializeQuadric(obs, mCalib); // initialize quadrics by SVD\n e.miInstanceID = iter->first; \n\n if( mpInitializer->getInitializeResult() ) {\n ellipsoid* pBox = new ellipsoid(e);\n mpMap->addEllipsoid(pBox);\n\n // output\n cout << std::endl;\n cout << \"-------- INITIALIZE NEW OBJECT BY SVD ---------\" << endl;\n cout << \"Label id: \" << pBox->miLabel << endl;\n cout << \"Instance id: \" << pBox->miInstanceID << endl;\n cout << \"Initialization box: \" << pBox->vec_minimal.transpose() << endl;\n cout << std::endl;\n\n continue;\n }\n }\n }\n\n // 2. Consider 3d initilization\n\n // Refresh exsited instance id.\n objects = mpMap->GetAllEllipsoids();\n existInstances.clear();\n for(auto iter=objects.begin(); iter!=objects.end(); iter++)\n {\n existInstances.insert((*iter)->miInstanceID); \n }\n\n // Try initialization using single-frame ellipsoid estimation.\n if( mbDepthEllipsoidOpened ){ \n srand(time(0));\n for(auto objPair : mmObjectObservations3D)\n {\n int instance = objPair.first;\n if( existInstances.find(instance) == existInstances.end() ) // if the instance has not been initialized yet\n {\n Observation3D* pOb3D = objPair.second.back();\n g2o::ellipsoid* pE = pOb3D->pObj; \n Frame* pFrame = pOb3D->pFrame;\n\n g2o::ellipsoid* pBox = new ellipsoid(pE->transform_from(pFrame->cam_pose_Twc));\n pBox->setColor(Vector3d(0, 0, 1));\n mpMap->addEllipsoid(pBox);\n }\n }\n }\n }\n\n void Tracking::OpenDepthEllipsoid(){\n mbDepthEllipsoidOpened = true;\n\n mpEllipsoidExtractor = new EllipsoidExtractor;\n\n // Open visualization during the estimation process\n mpEllipsoidExtractor->OpenVisualization(mpMap);\n\n // Open symmetry\n if(Config::Get(\"EllipsoidExtraction.Symmetry.Open\") == 1)\n mpEllipsoidExtractor->OpenSymmetry();\n\n std::cout << std::endl;\n cout << \" * Open Single-Frame Ellipsoid Estimation. \" << std::endl;\n std::cout << std::endl;\n }\n\n bool Tracking::isKeyFrameForVisualization()\n {\n static Frame* lastVisualizedFrame;\n if( mvpFrames.size() < 2 ) \n {\n lastVisualizedFrame = mCurrFrame;\n return true; \n }\n\n auto lastPose = lastVisualizedFrame->cam_pose_Twc;\n auto currPose = mCurrFrame->cam_pose_Twc;\n auto diffPose = lastPose.inverse() * currPose;\n Vector6d vec = diffPose.toXYZPRYVector();\n\n if( (vec.head(3).norm() > 0.4) || (vec.tail(3).norm() > M_PI/180.0*15) ) // Visualization param for camera poses\n {\n lastVisualizedFrame = mCurrFrame;\n return true;\n }\n else\n return false;\n }\n\n void Tracking::OpenOptimization(){\n mbOpenOptimization = true;\n std::cout << std::endl << \"Optimization Opens.\" << std::endl << std::endl ;\n }\n\n void Tracking::CloseOptimization(){\n mbOpenOptimization = false;\n std::cout << std::endl << \"Optimization Closes.\" << std::endl << std::endl ;\n }\n\n void Tracking::OpenGroundPlaneEstimation(){\n miGroundPlaneState = 1;\n pPlaneExtractor = new PlaneExtractor;\n PlaneExtractorParam param;\n param.fx = mK.at(0,0);\n param.fy = mK.at(1,1);\n param.cx = mK.at(0,2);\n param.cy = mK.at(1,2);\n param.scale = Config::Get(\"Camera.scale\");\n pPlaneExtractor->SetParam(param);\n\n std::cout << \" * Open Groundplane Estimation\" << std::endl;\n std::cout << std::endl;\n }\n\n void Tracking::CloseGroundPlaneEstimation(){\n miGroundPlaneState = 0;\n std::cout << std::endl;\n std::cout << \" * Close Groundplane Estimation* \" << std::endl;\n std::cout << std::endl;\n }\n\n int Tracking::GetGroundPlaneEstimationState(){\n return miGroundPlaneState;\n }\n\n void Tracking::ProcessGroundPlaneEstimation()\n {\n cv::Mat depth = mCurrFrame->frame_img;\n g2o::plane groundPlane;\n bool result = pPlaneExtractor->extractGroundPlane(depth, groundPlane);\n if( result )\n {\n g2o::SE3Quat& Twc = mCurrFrame->cam_pose_Twc; \n groundPlane.transform(Twc); // transform to the world coordinate.\n mGroundPlane = groundPlane;\n\n miGroundPlaneState = 2; \n\n std::cout << \" * Estimate Ground Plane Succeeds: \" << mGroundPlane.param.transpose() << std::endl;\n\n mGroundPlane.color = Vector3d(0.0,0.8,0.0); \n mpMap->addPlane(&mGroundPlane);\n\n // Visualize the pointcloud during ground plane extraction\n PointCloudPCL::Ptr pCloud = pPlaneExtractor->GetCloudDense();\n EllipsoidSLAM::PointCloud cloudQuadr = pclToQuadricPointCloud(pCloud);\n EllipsoidSLAM::PointCloud* pCloudQuadr = new EllipsoidSLAM::PointCloud(cloudQuadr);\n EllipsoidSLAM::PointCloud* pCloudQuadrGlobal = transformPointCloud(pCloudQuadr, &mCurrFrame->cam_pose_Twc);\n SetPointCloudProperty(pCloudQuadrGlobal, 0,255,100,2);\n mpMap->clearPointCloud();\n\n auto vPotentialGroundplanePoints = pPlaneExtractor->GetPotentialGroundPlanePoints();\n\n srand(time(0));\n for( auto& cloud : vPotentialGroundplanePoints )\n {\n PointCloudPCL::Ptr pCloudPCL(new PointCloudPCL(cloud));\n EllipsoidSLAM::PointCloud cloudQuadri = pclToQuadricPointCloud(pCloudPCL);\n EllipsoidSLAM::PointCloud* pCloudGlobal = transformPointCloud(&cloudQuadri, &Twc);\n \n int r = rand()%155;\n int g = 155;\n int b = rand()%155;\n SetPointCloudProperty(pCloudGlobal, r, g, b, 4);\n mpMap->AddPointCloudList(string(\"pPlaneExtractor.PotentialGround\"), pCloudGlobal, 1);\n }\n\n // Active the mannual check of groundplane estimation.\n int active_mannual_groundplane_check = Config::Get(\"Plane.MannualCheck.Open\");\n int key = -1;\n bool open_mannual_check = active_mannual_groundplane_check==1;\n bool result_mannual_check = false;\n if(open_mannual_check)\n {\n std::cout << \"Estimate Groundplane Done.\" << std::endl;\n std::cout << \"As Groundplane estimation is a simple implementation, please mannually check its correctness.\" << std::endl;\n std::cout << \"Enter Key \\'Y\\' to confirm, and any other key to cancel this estimation: \" << std::endl;\n\n key = getchar();\n }\n\n result_mannual_check = (key == 'Y' || key == 'y'); \n\n if( !open_mannual_check || (open_mannual_check && result_mannual_check) )\n {\n // Set groundplane to EllipsoidExtractor\n if( mbDepthEllipsoidOpened ){\n std::cout << \" * Add supporting plane to Ellipsoid Extractor.\" << std::endl;\n mpEllipsoidExtractor->SetSupportingPlane(&mGroundPlane);\n }\n\n // Set groundplane to Optimizer\n std::cout << \" * Set groundplane param to optimizer. \" << std::endl;\n mpOptimizer->SetGroundPlane(mGroundPlane.param);\n }\n else\n {\n std::cout << \" * Cancel this Estimation. \" << std::endl;\n miGroundPlaneState = 1;\n }\n\n }\n else\n {\n std::cout << \" * Estimate Ground Plane Fails \" << std::endl;\n }\n\n\n }\n\n bool Tracking::SavePointCloudMap(const string& path)\n {\n std::cout << \"Save pointcloud Map to : \" << path << std::endl;\n mpBuilder->saveMap(path);\n\n return true;\n }\n\n // This function saves the object history, which stores all the optimized object vector after every new observations.\n void Tracking::RefreshObjectHistory()\n {\n // Object Vector[11]: optimized_time[1] | Valid/inValid(1/0)[1] | minimal_vec[9] \n std::map pEllipsoidsMapWithInstance = mpMap->GetAllEllipsoidsMap();\n for( auto pairInsPEllipsoid : pEllipsoidsMapWithInstance )\n {\n int instance = pairInsPEllipsoid.first;\n if( mmObjectHistory.find(instance) == mmObjectHistory.end() ) // when the instance has no record in the history\n {\n MatrixXd obHistory; obHistory.resize(0, 11);\n mmObjectHistory.insert(make_pair(instance, obHistory));\n }\n\n // Add new history\n VectorXd hisVec; hisVec.resize(11);\n assert(mmObjectObservations.find(instance)!=mmObjectObservations.end() && \"How does the ellipsoid get into the map without observations?\");\n\n int currentObs = mmObjectObservations[instance].size();\n hisVec[0] = currentObs; // observation num.\n hisVec[1] = 1;\n\n Vector9d vec = pairInsPEllipsoid.second->toMinimalVector(); \n hisVec.tail<9>() = vec;\n \n // Get the observation num of the last history, add new row if the current observation num is newer.\n MatrixXd &obHisMat = mmObjectHistory[instance];\n if( obHisMat.rows() == 0)\n { \n // Save to the matrix\n addVecToMatirx(obHisMat, hisVec);\n }\n else {\n int lastObNum = round(obHisMat.row(obHisMat.rows()-1)[0]);\n if( lastObNum == currentObs ) // Compare with last observation\n {\n // Cover it and remain the same\n obHisMat.row(obHisMat.rows()-1) = hisVec;\n }\n else\n addVecToMatirx(obHisMat, hisVec); // Add a new row.\n }\n }\n }\n\n // Save the object history into a text file.\n void Tracking::SaveObjectHistory(const string& path)\n {\n /*\n * TotalInstanceNum\n * instanceID1 historyNum\n * 0 Valid(1/0) minimalVec\n * 1 Valid(1/0) minimalVec\n * 2 Valid(1/0) minimalVec\n * ...\n * instanceID2 historyNum\n * 0 Valid(1/0) minimalVec\n * 1 Valid(1/0) minimalVec\n * ...\n * \n */ \n ofstream out(path.c_str());\n int total_num = mmObjectHistory.size();\n\n out << total_num << std::endl;\n for( auto obPair : mmObjectHistory )\n {\n int instance = obPair.first;\n MatrixXd &hisMat = obPair.second;\n\n int hisNum = hisMat.rows();\n out << instance << \" \" << hisNum << std::endl;\n for( int n=0;n\n\n#include \n\nnamespace EllipsoidSLAM {\n\n Viewer::Viewer(System *pSystem, const string &strSettingPath, EllipsoidSLAM::MapDrawer *pMapDrawer){\n cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);\n\n mViewpointX = fSettings[\"Viewer.ViewpointX\"];\n mViewpointY = fSettings[\"Viewer.ViewpointY\"];\n mViewpointZ = fSettings[\"Viewer.ViewpointZ\"];\n mViewpointF = fSettings[\"Viewer.ViewpointF\"];\n\n mbFinishRequested=false;\n mpSystem = pSystem;\n mpMapDrawer = pMapDrawer;\n\n miRows = fSettings[\"Camera.height\"];\n miCols = fSettings[\"Camera.width\"];\n\n mvMenuStruct.clear();\n }\n\n Viewer::Viewer(const string &strSettingPath, MapDrawer* pMapDrawer):mpMapDrawer(pMapDrawer) {\n\n cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);\n\n mViewpointX = fSettings[\"Viewer.ViewpointX\"];\n mViewpointY = fSettings[\"Viewer.ViewpointY\"];\n mViewpointZ = fSettings[\"Viewer.ViewpointZ\"];\n mViewpointF = fSettings[\"Viewer.ViewpointF\"];\n\n mbFinishRequested=false;\n\n miRows = fSettings[\"Camera.height\"];\n miCols = fSettings[\"Camera.width\"];\n\n mvMenuStruct.clear();\n }\n\n void Viewer::SetFrameDrawer(FrameDrawer* pFrameDrawer)\n {\n mpFrameDrawer = pFrameDrawer;\n }\n\n void Viewer::run() {\n mbFinished = false;\n\n pangolin::CreateWindowAndBind(\"EllipsoidSLAM: Map Viewer\", 1024, 768);\n\n // 3D Mouse handler requires depth testing to be enabled\n glEnable(GL_DEPTH_TEST);\n\n // Issue specific OpenGl we might need\n glEnable(GL_BLEND);\n glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);\n\n pangolin::CreatePanel(\"menu\").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175));\n pangolin::Var menuFollowCamera(\"menu.Follow Camera\", true, true);\n pangolin::Var menuShowKeyFrames(\"menu.Show KeyFrames\", true, true);\n pangolin::Var menuShowEllipsoids(\"menu.Show Ellipsoids\", true, true);\n pangolin::Var menuShowPlanes(\"menu.Show Planes\", true, true);\n pangolin::Var menuShowCuboids(\"menu.Show Cuboids\", false, true);\n\n // Define Camera Render Object (for view / scene browsing)\n pangolin::OpenGlRenderState s_cam(\n pangolin::ProjectionMatrix(1024, 768, mViewpointF, mViewpointF, 512, 389, 0.1, 1000),\n pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0)\n );\n\n // Add named OpenGL viewport to window and provide 3D Handler\n pangolin::View &d_cam = pangolin::Display(\"cam\")\n .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -float(miCols) / float(miRows))\n .SetHandler(new pangolin::Handler3D(s_cam));\n \n // Add view for images\n pangolin::View& rgb_image = pangolin::Display(\"rgb\")\n .SetBounds(0,0.3,0.2,0.5,float(miCols) / float(miRows))\n .SetLock(pangolin::LockLeft, pangolin::LockBottom);\n\n pangolin::View& depth_image = pangolin::Display(\"depth\")\n .SetBounds(0,0.3,0.5,0.8,float(miCols) / float(miRows))\n .SetLock(pangolin::LockLeft, pangolin::LockBottom);\n\n pangolin::OpenGlMatrix Twc;\n Twc.SetIdentity();\n\n bool bFollow = true;\n\n pangolin::GlTexture imageTexture(miCols,miRows,GL_RGB,false,0,GL_BGR,GL_UNSIGNED_BYTE);\n\n while (1) {\n RefreshMenu(); // Deal with dynamic menu bars\n\n glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);\n\n mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc); // get current camera pose\n \n if(menuFollowCamera && bFollow) // Follow camera\n {\n s_cam.Follow(Twc);\n }\n else if(menuFollowCamera && !bFollow)\n {\n s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0));\n s_cam.Follow(Twc);\n bFollow = true;\n }\n else if(!menuFollowCamera && bFollow)\n {\n bFollow = false;\n }\n\n d_cam.Activate(s_cam);\n glClearColor(1.0f,1.0f,1.0f,1.0f);\n\n pangolin::glDrawAxis(3); // draw world coordintates\n\n if(menuShowKeyFrames)\n {\n mpMapDrawer->drawCameraState();\n mpMapDrawer->drawTrajectory();\n }\n\n // draw external cubes of ellipsoids \n if(menuShowCuboids)\n mpMapDrawer->drawObjects();\n\n // draw ellipsoids\n if(menuShowEllipsoids)\n mpMapDrawer->drawEllipsoids();\n\n // draw planes, including grounplanes and symmetry planes\n if(menuShowPlanes)\n mpMapDrawer->drawPlanes();\n\n mpMapDrawer->drawPoints(); // draw point clouds\n\n // draw pointclouds with names\n RefreshPointCloudOptions();\n mpMapDrawer->drawPointCloudWithOptions(mmPointCloudOptionMap);\n // mpMapDrawer->drawPointCloudLists();\n\n // draw images : rgb\n cv::Mat rgb = mpFrameDrawer->getCurrentFrameImage();\n if(!rgb.empty())\n {\n imageTexture.Upload(rgb.data,GL_BGR,GL_UNSIGNED_BYTE);\n //display the image\n rgb_image.Activate();\n glColor3f(1.0,1.0,1.0);\n imageTexture.RenderToViewportFlipY();\n }\n\n // draw images : depth\n cv::Mat depth = mpFrameDrawer->getCurrentDepthFrameImage();\n if(!depth.empty())\n {\n imageTexture.Upload(depth.data,GL_BGR,GL_UNSIGNED_BYTE);\n //display the image\n depth_image.Activate();\n glColor3f(1.0,1.0,1.0);\n imageTexture.RenderToViewportFlipY();\n }\n\n pangolin::FinishFrame();\n\n if (CheckFinish())\n break;\n }\n\n SetFinish();\n }\n\n\n bool Viewer::CheckFinish()\n {\n unique_lock lock(mMutexFinish);\n return mbFinishRequested;\n }\n\n void Viewer::SetFinish()\n {\n unique_lock lock(mMutexFinish);\n mbFinished = true;\n }\n\n void Viewer::RequestFinish()\n {\n unique_lock lock(mMutexFinish);\n mbFinishRequested = true;\n }\n\n\n bool Viewer::isFinished()\n {\n unique_lock lock(mMutexFinish);\n return mbFinished;\n }\n\n int Viewer::addDoubleMenu(string name, double min, double max, double def){\n unique_lock lock(mMutexFinish);\n\n MenuStruct menu;\n menu.min = min;\n menu.max = max;\n menu.def = def;\n menu.name = name;\n mvMenuStruct.push_back(menu);\n\n return mvMenuStruct.size()-1;\n }\n\n bool Viewer::getValueDoubleMenu(int id, double &value){\n unique_lock lock(mMutexFinish);\n if( 0 <= id && 0< mvDoubleMenus.size())\n {\n value = mvDoubleMenus[id]->Get();\n return true;\n }\n else\n {\n return false;\n }\n \n }\n\n void Viewer::RefreshPointCloudOptions()\n {\n // generate options from mmPointCloudOptionMenus, pointclouds with names will only be drawn when their options are activated.\n std::map options;\n for( auto pair : mmPointCloudOptionMenus)\n options.insert(make_pair(pair.first, pair.second->Get()));\n \n mmPointCloudOptionMap.clear();\n mmPointCloudOptionMap = options;\n }\n\n void Viewer::RefreshMenu(){\n unique_lock lock(mMutexFinish);\n\n // Generate menu bar for every pointcloud in pointcloud list.\n auto pointLists = mpSystem->getMap()->GetPointCloudList();\n\n // Iterate over the menu and delete the menu if the corresponding clouds are no longer available\n for( auto menuPair = mmPointCloudOptionMenus.begin(); menuPair!=mmPointCloudOptionMenus.end(); )\n {\n if(pointLists.find(menuPair->first) == pointLists.end())\n {\n delete menuPair->second; // destroy the dynamic menu \n menuPair = mmPointCloudOptionMenus.erase(menuPair); \n }\n else \n menuPair++;\n }\n\n // Iterate over the cloud lists to add new menu.\n for( auto cloudPair: pointLists )\n {\n if(mmPointCloudOptionMenus.find(cloudPair.first) == mmPointCloudOptionMenus.end())\n {\n pangolin::Var* pMenu = new pangolin::Var(string(\"menu.\") + cloudPair.first, true, true);\n mmPointCloudOptionMenus.insert(make_pair(cloudPair.first, pMenu)); \n }\n }\n\n // refresh double bars\n int doubleBarNum = mvDoubleMenus.size();\n int structNum = mvMenuStruct.size();\n if( structNum > 0 && structNum > doubleBarNum )\n {\n for(int i = doubleBarNum; i < structNum; i++)\n {\n pangolin::Var* pMenu = new pangolin::Var(string(\"menu.\")+mvMenuStruct[i].name, mvMenuStruct[i].def, mvMenuStruct[i].min, mvMenuStruct[i].max);\n mvDoubleMenus.push_back(pMenu);\n }\n }\n\n }\n}\n\n" }, { "path": "src/dense_builder/CMakeLists.txt", "content": "add_library(dense_builder SHARED\n builder.cpp\n)\n\ntarget_link_libraries(dense_builder\n ${OpenCV_LIBS}\n ${PCL_LIBRARIES}\n utils\n )\n" }, { "path": "src/dense_builder/builder.cpp", "content": "#include \"builder.h\"\n#include \"include/utils/matrix_utils.h\"\n\n#include \n#include \n\n#include // for make_shared\n\n#include \n\n// //PCL\n#include \n#include \n\n\nusing namespace Eigen;\nusing namespace std;\n\nvoid Builder::processFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose, double depth_thresh){\n if(!mbInitialized) {\n std::cerr << \"Please initialize the builder first.\" << std::endl;\n return;\n }\n PointCloudPCL::Ptr pLocalPointCloud = image2PointCloud(rgb, depth, depth_thresh);\n\n PointCloudPCL::Ptr pGlobalPointCloud = transformToWolrd(pLocalPointCloud, pose); // pose: Twc\n mpCurrentMap = pGlobalPointCloud;\n\n // add to map\n addPointCloudToMap(pGlobalPointCloud);\n\n return;\n}\n\nvoid Builder::setCameraIntrinsic(Matrix3d &calibMat, double scale){\n mmCalib = calibMat;\n mdScale = scale;\n\n mbInitialized = true;\n}\n\nBuilder::Builder():mbInitialized(false), mpMap(new PointCloudPCL){\n\n}\n\nPointCloudPCL::Ptr Builder::image2PointCloud( cv::Mat& rgb, cv::Mat& depth, double depth_thresh)\n{\n PointCloudPCL::Ptr temp_cloud ( new PointCloudPCL );\n \n Camera camera;\n getCameraParam(mmCalib, camera);\n for (int m = 0; m < depth.rows; m++)\n for (int n=0; n < depth.cols; n++)\n {\n ushort * ptd = depth.ptr(m);\n ushort d = ptd[n];\n if (d == 0)\n continue;\n\n PointT p;\n p.z = double(d) / mdScale;\n\n if( p.z < 0.5 || p.z > depth_thresh )\n continue; \n\n p.x = (n - camera.cx) * p.z / camera.fx;\n p.y = (m - camera.cy) * p.z / camera.fy;\n\n // B G R\n p.b = rgb.ptr(m)[n*3];\n p.g = rgb.ptr(m)[n*3+1];\n p.r = rgb.ptr(m)[n*3+2];\n\n temp_cloud->points.push_back( p );\n }\n\n temp_cloud->height = 1;\n temp_cloud->width = temp_cloud->points.size();\n temp_cloud->is_dense = true;\n return temp_cloud;\n}\n\nvoid Builder::getCameraParam(Matrix3d &calib, Camera &cam){\n // kalib matrix:\n // fx s cx\n // 0 fy cy\n // 0 0 1\n cam.fx = calib(0,0);\n cam.fy = calib(1,1);\n cam.cx = calib(0,2);\n cam.cy = calib(1,2);\n}\n\nPointCloudPCL::Ptr Builder::getMap()\n{\n return mpMap;\n}\n\nPointCloudPCL::Ptr Builder::getCurrentMap()\n{\n return mpCurrentMap;\n}\n\nPointCloudPCL::Ptr Builder::transformToWolrd( PointCloudPCL::Ptr& pPointCloud, VectorXd &pose){\n // pose: Tcw \n // tx ty tz (3 floats) give the position of \n // the optical center of the color camera \n // with respect to the world origin as defined by the motion capture system.\n PointCloudPCL::Ptr transformed_cloud(new PointCloudPCL);\n Matrix4d transform = getTransformFromVector(pose);\n std::cout << \"transform \" << transform << std::endl;\n pcl::transformPointCloud (*pPointCloud, *transformed_cloud, transform);\n \n return transformed_cloud;\n}\n\nvoid Builder::addPointCloudToMap(PointCloudPCL::Ptr pPointCloud){\n *mpMap = *mpMap + *pPointCloud;\n}\n\nvoid Builder::saveMap(const string& path){\n if(mpMap->points.size() < 1) return;\n pcl::io::savePCDFileASCII(path, *mpMap);\n std::cerr << \"Saved \" << mpMap->points.size () << \" data points to \" << path << std::endl;\n}\n\nvoid Builder::voxelFilter(double grid_size){\n // Voxel grid downsample\n static pcl::VoxelGrid voxel;\n\n double gridsize = grid_size;\n voxel.setLeafSize( gridsize, gridsize, gridsize );\n voxel.setInputCloud( mpMap );\n PointCloudPCL::Ptr tmp( new PointCloudPCL() );\n voxel.filter( *tmp );\n pcl::copyPointCloud(*tmp, *mpMap);\n}" }, { "path": "src/dense_builder/builder.h", "content": "// Builder generates dense point cloud from RGB-D data and groundtruth camera poses.\n\n#include \n#include \n\n//PCL\n#include \n#include \n#include \n\ntypedef pcl::PointXYZRGB PointT;\ntypedef pcl::PointCloud PointCloudPCL;\n\n#include \n\nusing namespace Eigen;\nusing namespace std;\n\nstruct Camera\n{\n double fx;\n double fy;\n double cx;\n double cy;\n};\n\nclass Builder\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n Builder();\n\n void setCameraIntrinsic(Matrix3d &calibMat, double scale);\n\n void processFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose, double depth_thresh = 1000); \n PointCloudPCL::Ptr getMap();\n PointCloudPCL::Ptr getCurrentMap();\n void saveMap(const string& path);\n\n void voxelFilter(double grid_size);\n\nprivate:\n void getCameraParam(Matrix3d &calib, Camera &cam);\n PointCloudPCL::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, double depth_thresh); \n PointCloudPCL::Ptr transformToWolrd( PointCloudPCL::Ptr& pPointCloud, VectorXd &pose);\n\n void addPointCloudToMap(PointCloudPCL::Ptr pPointCloud);\n\nprivate:\n\n Matrix3d mmCalib; // calibration mat\n double mdScale; // scale for depth\n\n bool mbInitialized;\n\n PointCloudPCL::Ptr mpMap;\n PointCloudPCL::Ptr mpCurrentMap;\n\n};" }, { "path": "src/pca/CMakeLists.txt", "content": "# set(CMAKE_BUILD_TYPE Debug)\n\nadd_library(EllipsoidExtractor SHARED\n EllipsoidExtractor.cpp\n)\n\ntarget_link_libraries(EllipsoidExtractor\n ${OpenCV_LIBS}\n symmetry\n utils\n )\n" }, { "path": "src/pca/EllipsoidExtractor.cpp", "content": "#include \"EllipsoidExtractor.h\"\n\n#include \n\n// For Euclidean Cluster Extraction\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n// g2o\n#include \"Thirdparty/g2o/g2o/core/block_solver.h\"\n#include \"Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h\"\n#include \"Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h\"\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h\"\n#include \"Thirdparty/g2o/g2o/solvers/linear_solver_dense.h\"\n\n#include \"Thirdparty/g2o/g2o/core/robust_kernel.h\"\n#include \"Thirdparty/g2o/g2o/core/robust_kernel_impl.h\"\n\n#include \n#include \n\nnamespace EllipsoidSLAM\n{\n\nbool compare_cloud_with_z(EllipsoidSLAM::PointXYZRGB &p1, EllipsoidSLAM::PointXYZRGB &p2)\n{\n return p1.z < p2.z;\n}\n\nEllipsoidExtractor::EllipsoidExtractor()\n{\n mResult = false;\n mbSetPlane = false;\n\n mDebugCenterCloud = NULL;\n\n mbOpenVisualization = false;\n miExtractCount = 0;\n\n mbOpenSymmetry = false;\n}\n\nvoid EllipsoidExtractor::LoadSymmetryPrior()\n{\n // the semantic label comes from coco names, which is suitable for YOLO.\n // *************************************\n // Object-type LabelID SymmetryType\n // potted plant | 58 | None\n // bed | 59 | Reflection\n // tvmonitor | 62 | Reflection\n // sofa | 57 | Reflection\n // keyboard | 66 | Dual Reflection\n // laptop | 63 | Reflection\n // mouse | 64 | Reflection\n // cup | 41 | Reflection\n // suitcase| 28 | Dual Reflection\n\n std::map labelSymmetry;\n labelSymmetry.insert(make_pair(58,0));\n labelSymmetry.insert(make_pair(59,1));\n labelSymmetry.insert(make_pair(62,1));\n labelSymmetry.insert(make_pair(57,1));\n labelSymmetry.insert(make_pair(66,1));\n labelSymmetry.insert(make_pair(63,1));\n labelSymmetry.insert(make_pair(64,1));\n labelSymmetry.insert(make_pair(41,1));\n labelSymmetry.insert(make_pair(28,2));\n\n mmLabelSymmetry = labelSymmetry; // load to the config\n}\n\nbool EllipsoidExtractor::GetResult()\n{\n return mResult;\n}\n\npcl::PointCloud::Ptr EllipsoidExtractor::ExtractPointCloud(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera)\n{ \n clock_t time_1_start = clock();\n\n assert( mbSetPlane && \"Please set the supporting plane first.\");\n\n double depth_range = Config::ReadValue(\"EllipsoidExtractor_DEPTH_RANGE\", 6); \n\n PointCloud* pPoints_local = new PointCloud(getPointCloudInRect(depth, bbox, camera, depth_range));\n\n // downsample points with small grid\n PointCloud* pPoints_local_downsample = new PointCloud;\n DownSamplePointCloudOnly(*pPoints_local, *pPoints_local_downsample, 0.01);\n \n // transform to the world coordinate.\n g2o::SE3Quat campose_wc; campose_wc.fromVector(pose);\n PointCloud* pPoints_global = transformPointCloud(pPoints_local_downsample, &campose_wc);\n \n mpPointsDebug = pPoints_global;\n clock_t time_2_getPointsDownsampleTransToWorld = clock();\n\n PointCloud* pPoints_planeFiltered = ApplySupportingPlaneFilter(pPoints_global);\n clock_t time_3_SupportingPlaneFilter = clock();\n\n // VisualizePointCloud(\"OriginObjectPoints\", pPoints_planeFiltered, Vector3d(1.0,0,0), 2);;\n clock_t time_4_VisualizePointCloud = clock();\n\n if( pPoints_planeFiltered->size() < 1 )\n {\n std::cout << \"No point left.\" << std::endl;\n std::cout << \"pPoints_global points: \" << pPoints_global->size() << std::endl; \n std::cout << \"No enough point cloud after Filtering. Num: \" << pPoints_planeFiltered->size() << std::endl;\n miSystemState = 4;\n return NULL;\n }\n\n PointCloud* pPoints_sampled = pPoints_planeFiltered;\n\n if( pPoints_sampled->size() < 1 ) \n {\n std::cout << \"No enough point cloud after sampling. Num: \" << pPoints_sampled->size() << std::endl;\n miSystemState = 3;\n return NULL;\n }\n\n Vector3d center;\n bool bCenter = GetCenter(depth, bbox, pose, camera, center);\n if(!bCenter) {\n miSystemState = 1;\n\n std::cout << \"Can't Find Center. Bbox: \" << bbox.transpose() << std::endl;\n return NULL; \n }\n clock_t time_5_GetCenter = clock();\n\n mDebugCenter = center;\n PointCloud* pPointsEuFiltered = ApplyEuclideanFilter(pPoints_sampled, center);\n\n if( miEuclideanFilterState > 0 )\n {\n miSystemState = 2; // fail to filter\n return NULL;\n }\n clock_t time_6_ApplyEuclideanFilter = clock();\n\n // we have gotten the object points in the world coordinate\n pcl::PointCloud::Ptr clear_cloud_ptr = QuadricPointCloudToPclXYZ(*pPointsEuFiltered);\n\n mpPoints = pPointsEuFiltered;\n VisualizePointCloud(\"Object Points\", mpPoints, Vector3d(0.4,0,1.0), 2);;\n clock_t time_7_VisualizePointCloud = clock();\n\n // output: time efficiency\n // cout << \"****** System Time [ExtractPoints.cpp] ******\" << endl ;\n // cout << \"time_2_getPointsDownsampleTransToWorld: \" <<(double)(time_2_getPointsDownsampleTransToWorld - time_1_start) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_3_SupportingPlaneFilter: \" <<(double)(time_3_SupportingPlaneFilter - time_2_getPointsDownsampleTransToWorld) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_4_VisualizePointCloud: \" <<(double)(time_4_VisualizePointCloud - time_3_SupportingPlaneFilter) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_5_GetCenter: \" <<(double)(time_5_GetCenter - time_4_VisualizePointCloud) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_6_ApplyEuclideanFilter: \" <<(double)(time_6_ApplyEuclideanFilter - time_5_GetCenter) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_7_VisualizePointCloud: \" <<(double)(time_7_VisualizePointCloud - time_6_ApplyEuclideanFilter) / CLOCKS_PER_SEC << \"s\" << endl;\n\n return clear_cloud_ptr;\n}\n\nPCAResult EllipsoidExtractor::ProcessPCA(pcl::PointCloud::Ptr &pCloudPCL)\n{\n pcl::PointCloud::Ptr cloud = pCloudPCL;\n\tpcl::PointCloud::Ptr cloud_normal(new pcl::PointCloud());\n\n Eigen::Vector4d pcaCentroid;\n\tpcl::compute3DCentroid(*cloud, pcaCentroid);\n\tEigen::Matrix3d covariance;\n\tpcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);\n\tEigen::SelfAdjointEigenSolver eigen_solver(covariance, Eigen::ComputeEigenvectors);\n\tEigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors();\n\tEigen::Vector3d eigenValuesPCA = eigen_solver.eigenvalues();\n\n\t// center point\n\tPointType c;\n\tc.x = pcaCentroid(0);\n\tc.y = pcaCentroid(1);\n\tc.z = pcaCentroid(2);\n\n PCAResult output;\n output.result = true;\n output.center = Eigen::Vector3d(c.x,c.y,c.z);\n output.rotMat = eigenVectorsPCA; // Rotation matrix(in world coordinate)\n output.covariance = Eigen::Vector3d(eigenValuesPCA(0),eigenValuesPCA(1),eigenValuesPCA(2)); // covariance used for scale estimation of ellipsoid\n\n return output;\n}\n\nvoid EllipsoidExtractor::ApplyGravityPrior(PCAResult &data)\n{\n assert( mbSetPlane && \"Please set the supporting plane first.\");\n \n Eigen::Matrix3d matRot = calibRotMatAccordingToGroundPlane( data.rotMat, mpPlane->param.head(3));\n data.rotMat = matRot;\n return;\n}\n\nvoid EllipsoidExtractor::AlignZAxisToGravity(PCAResult &data){\n // the goal for aligning Z axis :\n // make sure the z axis of the ellipsoid is along the direction of the groundplane normal,\n // 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\n\n // First, get the z axis\n double max_cos_theta_abs = 0;\n bool max_flag_pos;\n int max_id = -1;\n\n Vector3d z_axis;\n if( mbSetPlane )\n z_axis = mpPlane->param.head(3).normalized();\n else\n z_axis = Vector3d(0,0,1);\n\n // find which axis in rotMat has minimum angle difference with z_axis\n for( int i=0;i<3;i++ )\n {\n Vector3d axis = data.rotMat.col(i);\n double cos_theta = axis.dot(z_axis); // a*b = |a||b|cos(theta)\n\n bool flag_pos = cos_theta > 0;\n double cos_theta_abs = std::abs(cos_theta);\n\n if( cos_theta_abs > max_cos_theta_abs )\n {\n max_cos_theta_abs = cos_theta_abs;\n max_flag_pos = flag_pos;\n max_id = i;\n }\n }\n \n assert( max_id >= 0 && \"Must find a biggest one.\" );\n\n // swap the rotMat to get the correct z axis\n Matrix3d rotMatSwap; Vector3d covarianceSwap;\n Vector3d z_axis_vec;\n\n // invert the direction\n if(max_flag_pos) \n z_axis_vec = data.rotMat.col(max_id);\n else\n z_axis_vec = -data.rotMat.col(max_id);\n rotMatSwap.col(2) = z_axis_vec;\n covarianceSwap(2) = data.covariance[max_id];\n\n // get other two axes.\n int x_axis_id = (max_id+1)%3; // next axis.\n rotMatSwap.col(0) = data.rotMat.col(x_axis_id);\n covarianceSwap(0) = data.covariance[x_axis_id];\n\n int y_axis_id = (max_id+2)%3;\n rotMatSwap.col(1) = rotMatSwap.col(2).cross(rotMatSwap.col(0)); \n covarianceSwap(1) = data.covariance[y_axis_id];\n\n data.rotMat = rotMatSwap;\n data.covariance = covarianceSwap;\n\n return ;\n}\n\n// This function is a simple implementation for constructing an ellipsoid from rgb-d data, which differs from the paper.\ng2o::ellipsoid EllipsoidExtractor::ConstructEllipsoid(PCAResult &data)\n{\n g2o::ellipsoid e;\n\n // 1) use the calibrated rotation matrix from PCA as the orientation of the ellipsoid.\n // the z axis has been adjusted to be along the normal of the groundplane.\n Eigen::Quaterniond quat(data.rotMat);\n\n // 2) the scale will be decided using an approximation: the max x,y,z value of the object points.\n Eigen::Vector3d scale = data.scale;\n\n // x y z qx qy qz qw a b c\n Vector10d ellip_vec;\n ellip_vec << data.center[0], data.center[1], data.center[2], \n (quat.x()), (quat.y()), (quat.z()), (quat.w()), \n scale[0], scale[1], scale[2];\n e.fromVector(ellip_vec);\n\n return e;\n\n}\n\ng2o::ellipsoid EllipsoidExtractor::EstimateLocalEllipsoid(cv::Mat& depth, Eigen::Vector4d& bbox, int label, Eigen::VectorXd &pose, camera_intrinsic& camera){\n miExtractCount ++; // the total extraction times for naming point clouds.\n\n g2o::ellipsoid e;\n miSystemState = 0; // reset the state\n mSymmetryOutputData.result = false; // reset\n mResult = false;\n\n clock_t time_start = clock();\n // 1. Get the object points after supporting plane filter and euclidean filter in the world coordinate\n pcl::PointCloud::Ptr pCloudPCL = ExtractPointCloud(depth,bbox,pose,camera);\n if(miSystemState > 0 )\n return e;\n clock_t time_1_ExtractPointCloud = clock();\n\n // process the principle components analysis to get the rotation matrix, scale, and center point of the point cloud\n PCAResult data = ProcessPCA(pCloudPCL);\n \n // adjust the rotation matrix to be right-handed \n AdjustChirality(data); \n // adjust the x,y,z order\n AlignZAxisToGravity(data);\n // align z axis with the normal of the supporting plane\n ApplyGravityPrior(data);\n\n Vector3d center = data.center; // center point of the object points from PCA.\n EllipsoidSLAM::PointCloud* pObjectClearCloud = pclXYZToQuadricPointCloudPtr(pCloudPCL);\n // EllipsoidSLAM::PointCloud* pObjectCloud = pObjectClearCloud; // world coordinate\n\n // downsample to estimate symmetry.\n double grid_size_for_symmetry = Config::ReadValue(\"EllipsoidExtraction.Symmetry.GridSize\");\n EllipsoidSLAM::PointCloud* pObjectCloud = new EllipsoidSLAM::PointCloud;\n DownSamplePointCloudOnly(*pObjectClearCloud, *pObjectCloud, grid_size_for_symmetry);\n VisualizePointCloud(\"Points For Sym\", pObjectCloud, Vector3d(0.5,0.5,0.0), 6);\n\n // construct a normalized rotation matrix using the normal of the supporting plane and the normal of the symmetry plane.\n Vector3d rot_vec_z;\n if( mbSetPlane )\n rot_vec_z = mpPlane->param.head(3).normalized();\n else\n rot_vec_z = Vector3d(0,0,1);\n Vector3d rot_vec_x = data.rotMat.col(0).normalized(); // or use the origin PCA result.\n Vector3d rot_vec_y = rot_vec_z.cross(rot_vec_x);\n\n Matrix3d rotMat_wo; // object in world\n rotMat_wo.col(0) = rot_vec_x;\n rotMat_wo.col(1) = rot_vec_y;\n rotMat_wo.col(2) = rot_vec_z;\n\n // transform to the normalized coordinate\n g2o::SE3Quat* pSE3Two = new g2o::SE3Quat;\n Eigen::Quaterniond quat_wo(rotMat_wo);\n pSE3Two->setRotation(quat_wo);\n pSE3Two->setTranslation(center); // it is the center of the old object points; it's better to use the center of the new complete points\n g2o::SE3Quat SE3Tow(pSE3Two->inverse());\n EllipsoidSLAM::PointCloud* pObjectCloudNormalized = transformPointCloud(pObjectCloud, &SE3Tow); // normalized coordinate\n // VisualizePointCloud(\"normalizedPoints\", pObjectCloudNormalized, Vector3d(0,0.4,0), 2);\n\n clock_t time_2_partPCA = clock();\n\n // begin symmetry plane estimation.\n SymmetryOutputData dataSymOutput;\n dataSymOutput.result = false;\n bool runSymmetry = false;\n if( mbOpenSymmetry )\n {\n // 1. Check symmetry type\n bool hasSymmetry = (mmLabelSymmetry.find(label) != mmLabelSymmetry.end());\n int symmetryType = -1;\n if(hasSymmetry)\n {\n symmetryType = mmLabelSymmetry[label];\n if(symmetryType > 0) runSymmetry = true; // have valid symmetry type\n }\n if(runSymmetry)\n {\n // 3. initialize the symmetry solver\n Symmetry ext;\n\n // Get a depth map whose values store the straight distances between the 3d points and camera center\n cv::Mat projDepth = ext.getProjDepthMat(depth, camera);\n g2o::SE3Quat campose_wc; campose_wc.fromVector(pose.tail(7));\n g2o::SE3Quat campose_oc = SE3Tow * campose_wc;\n Eigen::VectorXd poseNormalized = campose_oc.toVector();\n\n SymmetrySolverData result = ext.estimateSymmetry(bbox, pObjectCloudNormalized, poseNormalized, projDepth, camera, symmetryType);\n\n // store the output \n dataSymOutput.pCloud = pObjectCloudNormalized;\n dataSymOutput.prob = result.prob;\n dataSymOutput.result = result.result;\n dataSymOutput.center = center;\n dataSymOutput.symmetryType = symmetryType;\n\n // store the sym plane\n g2o::plane symPlaneWorld(*result.pPlane);\n symPlaneWorld.transform(*pSE3Two);\n dataSymOutput.planeVec = symPlaneWorld.param; // store the symmetry plane in the world coordinate\n \n if(symmetryType == 2) // another symmetry plane for dual reflection\n {\n g2o::plane symPlaneWorld2(*result.pPlane2);\n symPlaneWorld2.transform(*pSE3Two);\n dataSymOutput.planeVec2 = symPlaneWorld2.param;\n }\n\n // complete the pointcloud using the symmetry plane if the estimation is successful\n if(dataSymOutput.result)\n {\n PointCloud* pSymCloudNormalized = SymmetrySolver::GetSymmetryPointCloud(pObjectCloudNormalized, *result.pPlane); // the plane in result is in normalized coordinate.\n\n if( symmetryType == 2)\n {\n PointCloud* pSymCloudNormalized2_1 = SymmetrySolver::GetSymmetryPointCloud(pObjectCloudNormalized, *result.pPlane2);\n PointCloud* pSymCloudNormalized2_2 = SymmetrySolver::GetSymmetryPointCloud(pSymCloudNormalized, *result.pPlane2);\n CombinePointCloud(pSymCloudNormalized, pSymCloudNormalized2_1);\n CombinePointCloud(pSymCloudNormalized, pSymCloudNormalized2_2);\n }\n\n // add the mirrored points to the object points\n int sym_num = pSymCloudNormalized->size();\n for( int i=0;ipush_back((*pSymCloudNormalized)[i]);\n\n // here, mirrwoed points have changed the center and rotation of the object points,\n // so we need a new transformation to move back the object points to the normalized coordinate\n\n // get the new center of the objects with mirrored points\n Vector3d centerCombined = GetPointcloudCenter(pObjectCloudNormalized);\n\n // to world coordinate\n Vector3d centerCombinedWorld = TransformPoint(centerCombined, pSE3Two->to_homogeneous_matrix());\n dataSymOutput.center = centerCombinedWorld;\n\n g2o::SE3Quat Tom; // mirror objects in normalized coordinate\n Tom.setTranslation(centerCombined);\n Matrix3d rotMat_om; // object in world\n rotMat_om.col(0) = result.pPlane->param.head(3); // x: the normal of symmetry plane 1\n rotMat_om.col(0) = rotMat_om.col(0)/(rotMat_om.col(0).norm()); // normalize\n rotMat_om.col(2) = Vector3d(0,0,1); // z: the normal of the groundplane, which is Z(0,0,1)\n rotMat_om.col(1) = rotMat_om.col(2).cross(rotMat_om.col(0)); // y: z cross x\n\n Eigen::Quaterniond quat_om(rotMat_om);\n Tom.setRotation(quat_om);\n\n // transform points\n g2o::SE3Quat Tmo = Tom.inverse();\n transformPointCloudSelf(pObjectCloudNormalized, &Tmo); // po->pm\n\n // change T\n g2o::SE3Quat* pSE3Twm = new g2o::SE3Quat();\n (*pSE3Twm) = (*pSE3Two) * Tom;\n pSE3Two = pSE3Twm;\n\n }\n mSymmetryOutputData = dataSymOutput; \n\n // VisualizePointCloud(\"WithSymNormalized\", pObjectCloudNormalized, Vector3d(0,0,0.4), 2);\n\n // transform to the world\n EllipsoidSLAM::PointCloud* pObjectCloudWithSymWorld = EllipsoidSLAM::transformPointCloud(pObjectCloudNormalized, pSE3Two);\n VisualizePointCloud(\"Mirrored Points\", pObjectCloudWithSymWorld, Vector3d(0,1.0,0.2), 8);\n\n } // end of symmetry type\n } // end of the symmetry.\n clock_t time_3_symmetryEstimation = clock();\n\n // Estimate an ellipsoid from the complete object points\n // calculate the covariance along three main axes\n PCAResult dataCenterPCA = ProcessPCANormalized(pObjectCloudNormalized); \n g2o::ellipsoid e_zero_normalized = ConstructEllipsoid(dataCenterPCA);\n\n // transform back to the world coordinate\n g2o::ellipsoid e_global_normalized = e_zero_normalized.transform_from(*pSE3Two);\n\n // transform to the local coordinate.\n g2o::SE3Quat campose_wc; campose_wc.fromVector(pose);\n g2o::ellipsoid e_local_normalized = e_global_normalized.transform_from(campose_wc.inverse());\n clock_t time_5_zeroPCA = clock();\n\n // output the main running time\n // cout << \"****** System Time [EllipsoidExtractor.cpp] ******\" << endl ;\n // cout << \"time_ExtractPointCloud: \" <<(double)(time_1_ExtractPointCloud - time_start) / CLOCKS_PER_SEC << \"s\" << endl;\n // // cout << \"time_partPCA: \" <<(double)(time_2_partPCA - time_1_ExtractPointCloud) / CLOCKS_PER_SEC << \"s\" << endl; // too small\n // cout << \"time_symmetryEstimation: \" <<(double)(time_3_symmetryEstimation - time_2_partPCA) / CLOCKS_PER_SEC << \"s\" << endl;\n // // cout << \"time_zeroPCA: \" <<(double)(time_5_zeroPCA - time_3_symmetryEstimation) / CLOCKS_PER_SEC << \"s\" << endl << endl; // too small\n // cout << \"total_ellipsoidExtraction: \" <<(double)(time_5_zeroPCA - time_start) / CLOCKS_PER_SEC << \"s\" << endl;\n\n g2o::ellipsoid e_local = e_local_normalized;\n\n // calculate the probability of the single-frame ellipsoid estimation\n double prob_symmetry;\n if(runSymmetry)\n prob_symmetry = dataSymOutput.prob; // when the symmetry is opened\n else\n prob_symmetry = 1.0; // when the symmetry is closed, set it to a constant\n \n e_local.prob = prob_symmetry;\n\n mResult = true;\n return e_local;\n}\n\nPCAResult EllipsoidExtractor::ProcessPCANormalized(EllipsoidSLAM::PointCloud* pObject)\n{\n PCAResult data;\n double x,y,z;\n x=0;y=0;z=0;\n int num = pObject->size();\n\n double max_x = 0;\n double max_y = 0;\n double max_z = 0;\n for( int i=0;i max_x ) max_x = std::abs(px);\n if( std::abs(py) > max_y ) max_y = std::abs(py);\n if( std::abs(pz) > max_z ) max_z = std::abs(pz);\n }\n\n x /= num;\n y /= num;\n z /= num;\n\n data.covariance = Vector3d(x,y,z);\n data.center = Vector3d(0,0,0);\n data.rotMat = Matrix3d::Identity();\n data.scale << max_x, max_y, max_z;\n\n return data;\n\n}\n\nSymmetryOutputData EllipsoidExtractor::GetSymmetryOutputData()\n{\n return mSymmetryOutputData;\n}\n\n EllipsoidSLAM::PointCloud* EllipsoidExtractor::GetPointCloudInProcess()\n {\n return mpPoints;\n }\n\n EllipsoidSLAM::PointCloud* EllipsoidExtractor::GetPointCloudDebug()\n {\n return mpPointsDebug;\n }\n \n // pointcloud process\nEllipsoidSLAM::PointCloud* EllipsoidExtractor::ApplyPlaneFilter(EllipsoidSLAM::PointCloud* pCloud, double z)\n{\n EllipsoidSLAM::PointCloud *pCloudFiltered = new EllipsoidSLAM::PointCloud;\n int num = pCloud->size();\n for(auto p: (*pCloud))\n {\n if(p.z > z)\n pCloudFiltered->push_back(p);\n }\n\n return pCloudFiltered;\n}\n\nEllipsoidSLAM::PointCloud* EllipsoidExtractor::ApplySupportingPlaneFilter(EllipsoidSLAM::PointCloud* pCloud)\n{\n EllipsoidSLAM::PointCloud *pCloudFiltered = new EllipsoidSLAM::PointCloud;\n int num = pCloud->size();\n\n int i=0;\n for(auto p: (*pCloud))\n {\n 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.\n bool ok = dis>0.05; \n\n if(ok)\n pCloudFiltered->push_back(p);\n }\n\n return pCloudFiltered;\n}\n\n// 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.\n// Considering the robustness, several points will be sampled around the center point of the bounding box,\n// and their average 3D positions will be taken as the output.\nbool EllipsoidExtractor::GetCenter(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera, Vector3d& center){\n double depth_range = Config::ReadValue(\"EllipsoidExtractor_DEPTH_RANGE\");\n // get the center of the bounding box\n int x = int((bbox(0)+bbox(2))/2.0);\n int y = int((bbox(1)+bbox(3))/2.0); \n\n int point_num = 10; // sample 10 * 10 points\n int x_delta = std::abs(bbox(0) - bbox(2))/4.0 / point_num;\n int y_delta = std::abs(bbox(1) - bbox(3))/4.0 / point_num;\n\n PointCloudPCL::Ptr pCloud (new PointCloudPCL);\n PointCloudPCL& cloud = *pCloud;\n for( int x_id = -point_num/2; x_id(y_);\n ushort d = ptd[x_];\n\n PointT p;\n p.z = d / camera.scale;\n // if the depth value is invalid, ignore this point\n if (p.z <= 0.1 || p.z > depth_range) \n continue;\n\n p.x = (x_ - camera.cx) * p.z / camera.fx;\n p.y = (y_ - camera.cy) * p.z / camera.fy;\n cloud.points.push_back(p);\n }\n }\n mDebugCenterCloud = pCloud; // store for visualization\n \n if( cloud.size() < 2 ) return false; // we need at least 2 valid points\n\n Eigen::Vector4d centroid;\n pcl::compute3DCentroid(cloud, centroid); // get their centroid\n\n EllipsoidSLAM::PointXYZRGB p;\n p.x = centroid[0];\n p.y = centroid[1];\n p.z = centroid[2];\n\n // transform to the world coordintate\n g2o::SE3Quat campose_wc;campose_wc.fromVector(pose);\n Eigen::Matrix4d Twc = campose_wc.to_homogeneous_matrix();\n\n Eigen::Vector3d xyz(p.x, p.y, p.z);\n Eigen::Vector4d Xc = real_to_homo_coord(xyz);\n \n Eigen::Vector4d Xw = Twc * Xc;\n Eigen::Vector3d xyz_w = homo_to_real_coord(Xw);\n \n\n center = xyz_w;\n\n return true;\n\n\n}\n\n\nEllipsoidSLAM::PointCloud* EllipsoidExtractor::ApplyEuclideanFilter(EllipsoidSLAM::PointCloud* pCloud, Vector3d ¢er)\n{\n clock_t time_1_start = clock();\n\n // load config parameters\n int CONFIG_MinClusterSize = Config::Get( \"EllipsoidExtraction.Euclidean.MinClusterSize\"); \n double CONFIG_ClusterTolerance = Config::Get( \"EllipsoidExtraction.Euclidean.ClusterTolerance\"); \n double CONFIG_CENTER_DIS = Config::Get( \"EllipsoidExtraction.Euclidean.CenterDis\"); \n\n assert( CONFIG_MinClusterSize>0&&CONFIG_ClusterTolerance>0&&CONFIG_CENTER_DIS>0 && \"Forge to set param. \" );\n\n pcl::PointCloud::Ptr pCloudPCL = QuadricPointCloudToPclXYZ(*pCloud);\n\n // Creating the KdTree object for the search method of the extraction\n pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); // use kdTree to speed up\n tree->setInputCloud (pCloudPCL);\n\n int point_num = pCloudPCL->size();\n\n std::vector cluster_indices;\n pcl::EuclideanClusterExtraction ec;\n ec.setClusterTolerance (CONFIG_ClusterTolerance); // an important parameter. it must be larger than the grid size in the down sampling process\n ec.setMinClusterSize (CONFIG_MinClusterSize);\n ec.setMaxClusterSize (point_num);\n\n ec.setSearchMethod (tree);\n ec.setInputCloud (pCloudPCL);\n ec.extract (cluster_indices);\n\n bool bFindCluster = false;\n pcl::PointCloud::Ptr pFinalPoints;\n\n clock_t time_2_extractClusters = clock();\n\n int cluster_size = cluster_indices.size();\n\n // store the point clouds\n std::vector::Ptr> cloud_cluster_vector;\n for(auto it = cluster_indices.begin(); it!=cluster_indices.end();it++)\n { \n pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud);\n for (std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)\n cloud_cluster->points.push_back (pCloudPCL->points[*pit]); \n cloud_cluster_vector.push_back(cloud_cluster);\n }\n mDebugEuclideanFilterClouds = cloud_cluster_vector; // store for debugging\n\n clock_t time_3_getClusterPoints = clock();\n\n // select the cluster that has a distance below a threshold with the 3D center point projected near the center of the 2D bounding box\n for(int i=0; i::Ptr cloud_cluster = cloud_cluster_vector[i];\n\n if( cluster_size == 1) {\n pFinalPoints = cloud_cluster;\n bFindCluster = true;\n }\n\n double dis = getDistanceFromPointToCloud(center, cloud_cluster);\n bool c2 = false;\n if( dis < CONFIG_CENTER_DIS ) c2 = true; // the distance should be small enough\n if(c2)\n {\n pFinalPoints = cloud_cluster;\n bFindCluster = true;\n break;\n }\n }\n\n EllipsoidSLAM::PointCloud* pCloudFiltered;\n if(!bFindCluster)\n {\n miEuclideanFilterState = 3;\n return pCloudFiltered;\n }\n\n clock_t time_4_selectTheBestCluster = clock();\n\n pCloudFiltered = new EllipsoidSLAM::PointCloud(pclXYZToQuadricPointCloud(pFinalPoints)); \n miEuclideanFilterState = 0;\n\n clock_t time_5_copyNewCloud = clock();\n\n // cout << \"****** System Time [Euclidean Filter] ******\" << endl ;\n // cout << \"time_2_extractClusters: \" <<(double)(time_2_extractClusters - time_1_start) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_3_getClusterPoints: \" <<(double)(time_3_getClusterPoints - time_2_extractClusters) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_4_selectTheBestCluster: \" <<(double)(time_4_selectTheBestCluster - time_3_getClusterPoints) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"time_5_copyNewCloud: \" <<(double)(time_5_copyNewCloud - time_4_selectTheBestCluster) / CLOCKS_PER_SEC << \"s\" << endl; \n\n return pCloudFiltered;\n}\n\ndouble EllipsoidExtractor::getDistanceFromPointToCloud(Vector3d& point, pcl::PointCloud::Ptr pCloud){\n double mini_dis = 999999;\n if(pCloud->size() < 1) return -1;\n\n for(auto p : *pCloud) \n {\n Vector3d p_(p.x, p.y, p.z);\n double dis = (point - p_).norm();\n if( dis < mini_dis )\n mini_dis = dis;\n\n }\n return mini_dis;\n}\n\nvoid EllipsoidExtractor::SetSupportingPlane(g2o::plane* pPlane){\n mpPlane = pPlane;\n mbSetPlane = true;\n\n}\n\nvoid EllipsoidExtractor::AdjustChirality(PCAResult &data){\n // using cross operation to generate a new axis to make the coordinate right-handed\n data.rotMat.col(2) = data.rotMat.col(0).cross(data.rotMat.col(1));\n return;\n}\n\nEigen::Matrix3d EllipsoidExtractor::calibRotMatAccordingToGroundPlane(Matrix3d& rotMat, const Vector3d& normal){\n // in order to apply a small rotation to align the z axis of the object and the normal vector of the groundplane,\n // we need calculate the rotation axis and its angle.\n\n // first get the rotation axis\n Vector3d ellipsoid_zAxis = rotMat.col(2);\n Vector3d rot_axis = ellipsoid_zAxis.cross(normal); \n rot_axis.normalize();\n\n // then get the angle between the normal of the groundplane and the z axis of the object\n double norm1 = normal.norm();\n double norm2 = ellipsoid_zAxis.norm();\n double vec_dot = normal.transpose() * ellipsoid_zAxis;\n double cos_theta = vec_dot/norm1/norm2;\n double theta = acos(cos_theta); \n\n // generate the rotation vector\n AngleAxisd rot_angleAxis(theta,rot_axis);\n\n Matrix3d rotMat_calibrated = rot_angleAxis * rotMat;\n\n return rotMat_calibrated;\n}\n\nvoid EllipsoidExtractor::OpenVisualization(Map* pMap)\n{\n mbOpenVisualization = true;\n mpMap = pMap;\n}\n\nvoid EllipsoidExtractor::ClearPointCloudList()\n{\n if( mbOpenVisualization )\n {\n mpMap->DeletePointCloudList(\"EllipsoidExtractor\", 1); // partial martching \n }\n}\n\nvoid EllipsoidExtractor::VisualizePointCloud(const string& name, EllipsoidSLAM::PointCloud* pCloud, const Vector3d &color, int point_size){\n if( mbOpenVisualization ) \n {\n // if the color is not set, use random color\n uchar r,g,b;\n if( color[0] < -0.01 || color[1] < -0.01 || color[2] < -0.01)\n {\n srand(time(0));\n r = rand()%155;\n g = rand()%155;\n b = rand()%155;\n }\n else\n {\n r = 255 * color[0];\n g = 255 * color[1];\n b = 255 * color[2];\n }\n \n SetPointCloudProperty(pCloud, r,g,b,point_size);\n\n string full_name = string(\"EllipsoidExtractor.\") + name;\n\n mpMap->AddPointCloudList(full_name, pCloud, 1);\n }\n\n}\n\nvoid EllipsoidExtractor::VisualizeEllipsoid(const string& name, g2o::ellipsoid* pObj)\n{\n if( mbOpenVisualization ) \n {\n mpMap->addEllipsoidVisual(pObj);\n } \n}\n\nvoid EllipsoidExtractor::OpenSymmetry()\n{\n std::cout << std::endl;\n std::cout << \" * Open Symmetry Estimation. \" << std::endl;\n\n LoadSymmetryPrior();\n\n mbOpenSymmetry = true;\n\n}\n\n} // namespace: EllipsoidSLAM\n" }, { "path": "src/pca/EllipsoidExtractor.h", "content": "// Single-frame ellipsoid extraction from RGB-D data\n\n#ifndef ELLIPSOIDSLAM_ELLIPSOIDEXTRACTOR_H\n#define ELLIPSOIDSLAM_ELLIPSOIDEXTRACTOR_H\n\n#include \n\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\n#include \n#include \n#include \n#include \n#include \t\t \n\ntypedef pcl::PointXYZ PointType;\ntypedef pcl::Normal NormalType;\n\nnamespace EllipsoidSLAM\n{\n\nstruct PCAResult\n{\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n \n bool result;\n Eigen::Vector3d center;\n Eigen::Matrix3d rotMat;\n Eigen::Vector3d covariance;\n Eigen::Vector3d scale;\n};\n\nclass EllipsoidExtractor\n{\n\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n EllipsoidExtractor(); \n\n // open symmetry plane estimation to finish point cloud completion\n void OpenSymmetry();\n\n // The supporting plane is used to segment object points and estimate the object orientation\n void SetSupportingPlane(g2o::plane* pPlane);\n \n // API: estimate a 3d ellipsoid from RGB-D data and a bounding box, the ellipsoid is in global coordinate\n g2o::ellipsoid EstimateLocalEllipsoid(cv::Mat& depth, Eigen::Vector4d& bbox, int label, Eigen::VectorXd &pose, camera_intrinsic& camera); \n\n void OpenVisualization(Map* pMap); // if opened, the pointcloud during the process will be visualized \n void ClearPointCloudList(); // clear the visualized point cloud \n\n\n bool GetResult(); // get extraction result.\n SymmetryOutputData GetSymmetryOutputData(); // get the detail of symmetry estimation\n EllipsoidSLAM::PointCloud* GetPointCloudInProcess(); // get the object point cloud\n EllipsoidSLAM::PointCloud* GetPointCloudDebug(); // get the debug point cloud before Eucliden filter\n\nprivate:\n void LoadSymmetryPrior(); // define object symmetry prior\n\n pcl::PointCloud::Ptr ExtractPointCloud(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera); // extract point cloud from depth image.\n PCAResult ProcessPCA(pcl::PointCloud::Ptr &pCloudPCL); // apply principal component analysis \n g2o::ellipsoid ConstructEllipsoid(PCAResult &data); // generate a sparse ellipsoid estimation from PCA result.\n\n void ApplyGravityPrior(PCAResult &data); // add the supporting groundplane prior to calibrate the rotation matrix\n\n EllipsoidSLAM::PointCloud* ApplyEuclideanFilter(EllipsoidSLAM::PointCloud* pCloud, Vector3d ¢er); // apply euclidean filter to get the object points\n EllipsoidSLAM::PointCloud* ApplyPlaneFilter(EllipsoidSLAM::PointCloud* pCloud, double z); // filter points lying under the supporting plane\n\n EllipsoidSLAM::PointCloud* ApplySupportingPlaneFilter(EllipsoidSLAM::PointCloud* pCloud); \n\n bool GetCenter(cv::Mat& depth, Eigen::Vector4d& bbox, Eigen::VectorXd &pose, camera_intrinsic& camera, Vector3d& center); // get a coarse 3d center of the object\n double getDistanceFromPointToCloud(Vector3d& point, pcl::PointCloud::Ptr pCloud); // get the minimum distance between a point and a pointcloud\n\n // make sure the rotation matrix is right-handed\n void AdjustChirality(PCAResult &data);\n\n // adjust the axis order to make z axis near the normal of the ground plane\n void AlignZAxisToGravity(PCAResult &data);\n // this function will be called if groundplane is set when aligning Z axis\n Eigen::Matrix3d calibRotMatAccordingToGroundPlane( Matrix3d& rotMat, const Vector3d& normal);\n\n void VisualizePointCloud(const string& name, EllipsoidSLAM::PointCloud* pCloud, const Vector3d &color = Vector3d(-1,-1,-1), int point_size = 2);\n void VisualizeEllipsoid(const string& name, g2o::ellipsoid* pObj);\n\n PCAResult ProcessPCANormalized(EllipsoidSLAM::PointCloud* pObject);\n\nprivate:\n bool mResult; // estimation result.\n\n int miEuclideanFilterState; // Euclidean filter result: 1 no clusters 2: not the biggest cluster 3: fail to find valid cluster 0: success\n int miSystemState; // 0: normal 1: no depth value for center point 2: fail to filter. 3: no point left after downsample\n EllipsoidSLAM::PointCloud* mpPoints; // store object points\n EllipsoidSLAM::PointCloud* mpPointsDebug; // store points for debugging ( points before Euclidean filter)\n\n // supporting plane\n bool mbSetPlane; \n g2o::plane* mpPlane;\n\n // symmetry prior\n std::map mmLabelSymmetry;\n \npublic: \n // data generated in the process\n Vector3d mDebugCenter;\n std::vector::Ptr> mDebugEuclideanFilterClouds;\n PointCloudPCL::Ptr mDebugCenterCloud;\n\nprivate:\n SymmetrySolver* mpSymSolver;\n\n SymmetryOutputData mSymmetryOutputData;\n\n bool mbOpenVisualization;\n Map* mpMap;\n int miExtractCount;\n\n bool mbOpenSymmetry;\n};\n\n\n}\n\n#endif // ELLIPSOIDSLAM_ELLIPSOIDEXTRACTOR_H" }, { "path": "src/plane/CMakeLists.txt", "content": "\n\n# SET(CMAKE_BUILD_TYPE Debug)\n\nadd_library(PlaneExtractor SHARED\nPlaneExtractor.cpp\n)\n\ntarget_link_libraries(PlaneExtractor\n ${OpenCV_LIBS}\n ${PCL_LIBRARIES}\n Config\n )\n" }, { "path": "src/plane/PlaneExtractor.cpp", "content": "#include \"PlaneExtractor.h\"\n\n#include \"src/symmetry/PointCloudFilter.h\"\n\n#include \n\nnamespace EllipsoidSLAM\n{\n bool compare_func_plane_dis(g2o::plane* &p1, g2o::plane* &p2)\n {\n Vector3d cam_pose(0,0,0);\n double dis1 = p1->distanceToPoint(cam_pose); \n double dis2 = p2->distanceToPoint(cam_pose); \n return dis1 < dis2;\n }\n\n bool compare_func_plane_size(std::pair &p1, std::pair &p2)\n {\n return p1.second > p2.second;\n }\n\n void PlaneExtractor::extractPlanes(const cv::Mat &imDepth) {\n mvPlaneCoefficients.clear();\n mvPlanePoints.clear();\n\n int row_start;\n if( mParam.RangeOpen ) // if this flag is opened, only the bottom half part of the depth image is considered\n row_start = imDepth.rows - mParam.RangeHeight; // it normally starts from the half height of the image \n else\n row_start = 0;\n PointCloudPCL::Ptr inputCloud( new PointCloudPCL() );\n for ( int m=row_start; m(m)[n];\n \n double d = depthValue/ mParam.scale;\n PointT p;\n p.z = d;\n p.x = ( n - mParam.cx) * p.z / mParam.fx;\n p.y = ( m - mParam.cy) * p.z / mParam.fy;\n p.r = 0;\n p.g = 0;\n p.b = 250;\n\n inputCloud->points.push_back(p);\n }\n }\n inputCloud->height = ceil(imDepth.rows-row_start);\n inputCloud->width = ceil(imDepth.cols);\n\n mpCloudDense = inputCloud;\n\n // estimate the normal\n pcl::IntegralImageNormalEstimation ne;\n pcl::PointCloud::Ptr cloud_normals(new pcl::PointCloud);\n ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);\n ne.setMaxDepthChangeFactor(0.05f);\n ne.setNormalSmoothingSize(10.0f);\n ne.setInputCloud(inputCloud);\n ne.compute(*cloud_normals);\n\n // load config parameters\n int min_plane = Config::Get(\"Plane.MinSize\");\n float AngTh = Config::Get(\"Plane.AngleThreshold\");\n float DisTh = Config::Get(\"Plane.DistanceThreshold\");\n\n vector coefficients;\n vector inliers;\n pcl::PointCloud::Ptr labels ( new pcl::PointCloud );\n vector label_indices;\n vector boundary;\n\n pcl::OrganizedMultiPlaneSegmentation< PointT, pcl::Normal, pcl::Label > mps;\n mps.setMinInliers (100);\n mps.setAngularThreshold (0.017453 * AngTh); // deg to rad\n mps.setDistanceThreshold (DisTh);\n mps.setInputNormals (cloud_normals);\n mps.setInputCloud (inputCloud);\n std::vector, Eigen::aligned_allocator>> regions;\n mps.segmentAndRefine (regions, coefficients, inliers, labels, label_indices, boundary);\n\n pcl::ExtractIndices extract;\n extract.setInputCloud(inputCloud);\n extract.setNegative(false);\n\n // save the points belonging to each planes, and the coefficients\n for (int i = 0; i < inliers.size(); ++i) {\n int planeSize = inliers[i].indices.size();\n if( planeSize < min_plane ) continue;\n\n PointCloudPCL::Ptr planeCloud(new PointCloudPCL());\n cv::Mat coef = (cv::Mat_(4,1) << coefficients[i].values[0],\n coefficients[i].values[1],\n coefficients[i].values[2],\n coefficients[i].values[3]);\n if(coef.at(3) < 0)\n coef = -coef;\n\n extract.setIndices(boost::make_shared(inliers[i]));\n extract.filter(*planeCloud);\n\n mvPlanePoints.push_back(*planeCloud);\n mvPlaneCoefficients.push_back(coef); \n }\n }\n\n void PlaneExtractor::SetParam(PlaneExtractorParam& param){\n mParam = param;\n }\n\n\n std::vector PlaneExtractor::GetPoints(){\n return mvPlanePoints;\n }\n\n std::vector PlaneExtractor::GetPotentialGroundPlanePoints(){\n return mvPotentialGroundPlanePoints;\n }\n\n std::vector PlaneExtractor::GetCoefficients(){\n return mvPlaneCoefficients;\n }\n\n bool PlaneExtractor::extractGroundPlane(const cv::Mat &depth, g2o::plane& plane){\n mParam.RangeOpen = false;\n mParam.RangeHeight = depth.rows/2;\n\n extractPlanes(depth); // first extract all the potential planes\n \n if( mvPlaneCoefficients.size() < 1) // there should be more than 1 potential planes \n return false;\n\n // the groundplane should meet two criteria:\n // 1) its normal vector should have a small angle difference with the gravity direction. ( filter the walls )\n // 2) its distance with the center of the camera is the smallest among all the extracted planes.\n\n Vector3d cam_pose(0,0,0); \n std::vector vpPlanes;\n std::vector> mapPlaneSize;\n for( int i=0; i(0)), double(coeff.at(1)), double(coeff.at(2)), double(coeff.at(3));\n\n // 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].\n // it always meets the criteria when the camera is located on a mobile robot.\n // it is a loose criteria only for filtering the walls.\n Eigen::Vector3d axisY(0,1,0); \n Eigen::Vector3d axisNorm = vec.head(3);\n double cos_theta = axisNorm.transpose() * axisY;\n cos_theta = cos_theta / axisNorm.norm() / axisY.norm();\n\n double theta = acos( cos_theta ); // acos : [0,pi]\n if( theta > M_PI/4 && theta < 3*M_PI/4 ) continue; \n\n g2o::plane* pPlane = new g2o::plane();\n pPlane->param= vec;\n vpPlanes.push_back(pPlane);\n mvPotentialGroundPlanePoints.push_back(mvPlanePoints[i]);\n mapPlaneSize.push_back(make_pair(pPlane, mvPlanePoints[i].size()));\n }\n\n if( vpPlanes.size() < 1) { // there should be more than 1 valid planes \n std::cout << \"Please let the camera be parallel to the ground for initialization.\" << std::endl;\n return false;\n }\n\n // sort according to the size of planes\n sort(mapPlaneSize.begin(), mapPlaneSize.end(), compare_func_plane_size);\n g2o::plane* pPlaneGround = mapPlaneSize[0].first;\n\n // adjust the flag of the plane coefficients to make the distance between the camera center and the plane positive\n double value = pPlaneGround->distanceToPoint(Vector3d(0,0,0), true);\n if( value < 0 ) \n pPlaneGround->param = -pPlaneGround->param;\n\n plane = *pPlaneGround;\n\n return true;\n }\n\n PlaneExtractor::PlaneExtractor(const string& settings){\n std::cout << \"Init plane extractor using : \" << settings << std::endl; \n Config::Init();\n Config::SetParameterFile(settings);\n mParam.fx = Config::Get(\"Camera.fx\"); \n mParam.fy = Config::Get(\"Camera.fy\"); ; \n mParam.cx = Config::Get(\"Camera.cx\"); ; \n mParam.cy = Config::Get(\"Camera.cy\"); ; \n mParam.scale = Config::Get(\"DepthMapFactor\"); ; \n }\n\n PointCloudPCL::Ptr PlaneExtractor::GetCloudDense(){\n return mpCloudDense;\n }\n}" }, { "path": "src/plane/PlaneExtractor.h", "content": "// this class offers a simple demo for extracting the groundplane from a single RGB-D frame\n\n#ifndef PLANEEXTRACTOR_H\n#define PLANEEXTRACTOR_H\n\n#include \n#include \n\n#include \n\n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\ntypedef pcl::PointCloud PointCloudPCL;\nusing namespace std;\n\nnamespace EllipsoidSLAM\n{\n\nstruct PlaneExtractorParam\n{\n double fx,fy,cx,cy;\n double scale;\n\n bool RangeOpen = false; // if this flag is opened, only the bottom half part of the depth image is considered\n int RangeHeight;\n};\n\nclass PlaneExtractor\n{\n\npublic:\n PlaneExtractor(){};\n PlaneExtractor(const string& settings);\n bool extractGroundPlane(const cv::Mat &depth, g2o::plane& plane);\n\n void extractPlanes(const cv::Mat &depth);\n\n void SetParam(PlaneExtractorParam& param);\n\n std::vector GetPoints();\n std::vector GetPotentialGroundPlanePoints();\n\n std::vector GetCoefficients();\n PointCloudPCL::Ptr GetCloudDense();\nprivate:\n // params\n int mParamRangeHeight; // whether to consider the bottom half part only\n \n PlaneExtractorParam mParam;\n\n std::vector mvPlanePoints;\n std::vector mvPotentialGroundPlanePoints;\n std::vector mvPlaneCoefficients;\n\n PointCloudPCL::Ptr mpCloudDense;\n};\n\n\n} // EllipsoidSLAM\n\n#endif // PLANEEXTRACTOR_H" }, { "path": "src/symmetry/BorderExtractor.cpp", "content": "#include \"BorderExtractor.h\"\n\nnamespace EllipsoidSLAM\n{\npcl::PointCloud::Ptr BorderExtractor::ToPointTCloud(pcl::PointCloud::Ptr pCloud)\n{\n pcl::PointCloud::Ptr pCloudXYZ(new pcl::PointCloud);\n for( auto p : pCloud->points )\n {\n PointType pXYZ;\n pXYZ.x = p.x;\n pXYZ.y = p.y;\n pXYZ.z = p.z;\n\n pCloudXYZ->points.push_back(pXYZ);\n \n }\n\n return pCloudXYZ;\n}\n\npcl::PointCloud::Ptr BorderExtractor::CombineCloud(pcl::PointCloud::Ptr& pCloud1, pcl::PointCloud::Ptr& pCloud2, pcl::PointIndices& indices1)\n{\n pcl::PointCloud::Ptr pComposite(new pcl::PointCloud);\n \n int i=0;\n for( auto p: pCloud1->points ){\n pComposite->points.push_back(p);\n indices1.indices.push_back(i++);\n }\n for( auto p: pCloud2->points )\n pComposite->points.push_back(p);\n return pComposite;\n}\n\npcl::PointCloud::Ptr \nBorderExtractor::FilterBordersBasedOnPointCloud(pcl::PointCloud::Ptr& pBordersNoisy, pcl::PointCloud::Ptr &point_cloud_ptr){\n\n pcl::PointCloud::Ptr pBorders_PointTCloud = ToPointTCloud(pBordersNoisy);\n\n pcl::PointIndices indices;\n pcl::PointCloud::Ptr pComposite = CombineCloud(pBorders_PointTCloud, point_cloud_ptr, indices);\n\n // filter the outliers of the borders.\n pcl::PointCloud::Ptr borderCloudFiltered(new pcl::PointCloud());\n\n pcl::RadiusOutlierRemoval outrem;\n outrem.setInputCloud(pComposite);\n outrem.setIndices(boost::make_shared(indices));\n outrem.setRadiusSearch(0.05); \n outrem.setMinNeighborsInRadius(6);\n outrem.filter(*borderCloudFiltered);\n\n return borderCloudFiltered;\n\n}\n\npcl::PointCloud::Ptr BorderExtractor::extractBordersFromPointCloud(pcl::PointCloud::Ptr& point_cloud_ptr, double resolution)\n{\n clock_t startTime = clock();\n\n float angular_resolution = pcl::deg2rad( float(resolution) );\n pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;\n bool setUnseenToMaxRange = true;\n\n Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());\n\n pcl::PointCloud& point_cloud = *point_cloud_ptr;\n\n pcl::PointCloud far_ranges;\n\n scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],\n point_cloud.sensor_origin_[1],\n point_cloud.sensor_origin_[2])) *\n Eigen::Affine3f (point_cloud.sensor_orientation_);\n \n // -----------------------------------------------\n // -----Create RangeImage from the PointCloud-----\n // -----------------------------------------------\n float noise_level = 0.0;\n float min_range = 0.0f;\n int border_size = 1;\n pcl::RangeImage::Ptr range_image_ptr (new pcl::RangeImage);\n pcl::RangeImage& range_image = *range_image_ptr; \n range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),\n scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);\n range_image.integrateFarRanges (far_ranges);\n\n if (setUnseenToMaxRange)\n range_image.setUnseenToMaxRange ();\n // -------------------------\n // -----Extract borders-----\n // -------------------------\n pcl::RangeImageBorderExtractor border_extractor (&range_image);\n pcl::PointCloud border_descriptions;\n\n // set parameter\n pcl::RangeImageBorderExtractor::Parameters& param = border_extractor.getParameters();\n // param.pixel_radius_borders = 1; // default : 3\n // param.minimum_border_probability = 0.99; // defualt: 0.8\n border_extractor.compute (border_descriptions);\n \n // ----------------------------------\n // -----Show points in 3D viewer-----\n // ----------------------------------\n pcl::PointCloud::Ptr border_points_ptr(new pcl::PointCloud),\n veil_points_ptr(new pcl::PointCloud),\n shadow_points_ptr(new pcl::PointCloud);\n pcl::PointCloud& border_points = *border_points_ptr,\n & veil_points = * veil_points_ptr,\n & shadow_points = *shadow_points_ptr;\n for (int y=0; y< (int)range_image.height; ++y)\n {\n for (int x=0; x< (int)range_image.width; ++x)\n {\n if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])\n border_points.points.push_back (range_image.points[y*range_image.width + x]);\n if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])\n veil_points.points.push_back (range_image.points[y*range_image.width + x]);\n if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])\n shadow_points.points.push_back (range_image.points[y*range_image.width + x]);\n }\n }\n\n std::cout << \"border_points: \" << border_points.size() << std::endl;\n std::cout << \"veil_points: \" << veil_points.size() << std::endl;\n std::cout << \"shadow_points: \" << shadow_points.size() << std::endl;\n cout << \"Border Extraction - No Floor: \" <<(double)(clock() - startTime) / CLOCKS_PER_SEC << \"s\" << endl;\n\n return border_points_ptr;\n \n}\n}" }, { "path": "src/symmetry/BorderExtractor.h", "content": "// extract borders from point objects to speed up the symmetry plane estimation\n#ifndef ELLIPSOIDSLAM_BORDEREXTRACTOR_H\n#define ELLIPSOIDSLAM_BORDEREXTRACTOR_H\n\n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"src/symmetry/PointCloudFilter.h\"\n\n#include \n\n#include \n\ntypedef pcl::PointXYZ PointType;\n\nnamespace EllipsoidSLAM\n{\n\nclass BorderExtractor\n{\npublic:\n\npcl::PointCloud::Ptr ToPointTCloud(pcl::PointCloud::Ptr pCloud);\npcl::PointCloud::Ptr CombineCloud(pcl::PointCloud::Ptr& pCloud1, pcl::PointCloud::Ptr& pCloud2, pcl::PointIndices& indices1);\npcl::PointCloud::Ptr FilterBordersBasedOnPointCloud(pcl::PointCloud::Ptr& pBordersNoisy, pcl::PointCloud::Ptr &point_cloud_ptr);\npcl::PointCloud::Ptr extractBordersFromPointCloud(pcl::PointCloud::Ptr& point_cloud_ptr, double resolution);\n\n}; // class\n\n}// namespace\n\n#endif //ELLIPSOIDSLAM_BORDEREXTRACTOR_H" }, { "path": "src/symmetry/CMakeLists.txt", "content": "# set(CMAKE_BUILD_TYPE Debug)\n\nadd_library(symmetry SHARED\n Symmetry.cpp\n SymmetrySolver.cpp\n BorderExtractor.cpp\n PointCloudFilter.cpp\n)\n\nmessage (\"source dir of symmetry: \" ${PROJECT_SOURCE_DIR})\ntarget_link_libraries(symmetry\n ${OpenCV_LIBS}\n ${PCL_LIBRARIES}\n utils\n ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so\n )\n" }, { "path": "src/symmetry/PointCloudFilter.cpp", "content": "#include \"PointCloudFilter.h\"\n\nVector2d getXYCenterOfPointCloud(EllipsoidSLAM::PointCloud* pPoints)\n{\n double x=0;\n double y=0;\n int num=0;\n for( auto p:(*pPoints))\n {\n x+=p.x;\n y+=p.y;\n num++;\n }\n x = x / double(num);\n y = y / double(num);\n return Vector2d(x,y);\n}\n\n\n// coordinates : x -> right, y-> down, z->front \nEllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, cv::Mat &rgb, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range) {\n // detect : x1 y1 x2 y2 \n EllipsoidSLAM::PointCloud cloud;\n\n // scan the points in the bounding box \n int x1 = int(detect(0));\n int y1 = int(detect(1));\n int x2 = int(detect(2));\n int y2 = int(detect(3));\n\n for (int y = y1; y < y2; y = y+3){\n for (int x = x1; x < x2; x = x+3) {\n ushort *ptd = depth.ptr(y);\n ushort d = ptd[x];\n\n EllipsoidSLAM::PointXYZRGB p;\n p.z = d / camera.scale;\n if (p.z <= 0.1 || p.z > range) // if the depth is valid\n continue;\n\n p.x = (x - camera.cx) * p.z / camera.fx;\n p.y = (y - camera.cy) * p.z / camera.fy;\n\n p.b = rgb.ptr(y)[x * 3];\n p.g = rgb.ptr(y)[x * 3 + 1];\n p.r = rgb.ptr(y)[x * 3 + 2];\n\n p.size = 1;\n\n cloud.push_back(p);\n }\n }\n\n return cloud;\n\n}\n\nEllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range) {\n cv::Mat rgb = cv::Mat(depth.rows, depth.cols, CV_8UC3, cv::Scalar(0,0,0)); \n return getPointCloudInRect(depth, rgb, detect, camera, range);\n}\n\nvoid filterGround(EllipsoidSLAM::PointCloud** ppCloud)\n{\n EllipsoidSLAM::PointCloud *pCloudFiltered = new EllipsoidSLAM::PointCloud;\n EllipsoidSLAM::PointCloud *pCloud = *ppCloud;\n int num = pCloud->size();\n for(auto p: (*pCloud))\n {\n if(p.z > 0.05)\n pCloudFiltered->push_back(p);\n }\n\n delete pCloud;\n (*ppCloud) = pCloudFiltered;\n}\n\nvoid outputCloud(EllipsoidSLAM::PointCloud *pCloud, int num )\n{\n int total_num = pCloud->size();\n\n cout << \"===== Point Cloud ====\" << endl;\n int count = 0;\n for(auto p: *pCloud)\n {\n cout << count << \": \" << p.x <<\",\"<< p.y <<\",\"<< p.z << endl ;\n if(count++ > num)\n break;\n }\n}\n\nEllipsoidSLAM::PointCloud pclToQuadricPointCloud(PointCloudPCL::Ptr &pCloud)\n{\n EllipsoidSLAM::PointCloud cloud;\n int num = pCloud->points.size();\n for(int i=0;ipoints[i];\n p.r = pT.r;\n p.g = pT.g;\n p.b = pT.b;\n\n p.x = pT.x;\n p.y = pT.y;\n p.z = pT.z;\n cloud.push_back(p);\n }\n\n return cloud;\n}\n\nEllipsoidSLAM::PointCloud* pclToQuadricPointCloudPtr(PointCloudPCL::Ptr &pCloud)\n{\n EllipsoidSLAM::PointCloud* cloudPtr = new EllipsoidSLAM::PointCloud;\n EllipsoidSLAM::PointCloud& cloud = *cloudPtr;\n int num = pCloud->points.size();\n for(int i=0;ipoints[i];\n p.r = pT.r;\n p.g = pT.g;\n p.b = pT.b;\n\n p.x = pT.x;\n p.y = pT.y;\n p.z = pT.z;\n cloud.push_back(p);\n }\n\n return cloudPtr;\n}\n\nPointCloudPCL::Ptr QuadricPointCloudToPcl(EllipsoidSLAM::PointCloud &cloud)\n{\n PointCloudPCL::Ptr pCloud(new PointCloudPCL);\n \n int num = cloud.size();\n for(int i=0;ipoints.push_back(p);\n }\n\n return pCloud;\n}\n\npcl::PointCloud::Ptr QuadricPointCloudToPclXYZ(EllipsoidSLAM::PointCloud &cloud)\n{\n pcl::PointCloud::Ptr pCloud(new pcl::PointCloud);\n \n int num = cloud.size();\n for(int i=0;ipoints.push_back(p);\n }\n pCloud->width = (int) pCloud->points.size (); \n pCloud->height = 1;\n\n return pCloud;\n}\n\nEllipsoidSLAM::PointCloud pclXYZToQuadricPointCloud(pcl::PointCloud::Ptr &pCloud)\n{\n EllipsoidSLAM::PointCloud cloud;\n int num = pCloud->points.size();\n for(int i=0;ipoints[i];\n p.x = pT.x;\n p.y = pT.y;\n p.z = pT.z;\n cloud.push_back(p);\n }\n\n return cloud;\n}\n\nEllipsoidSLAM::PointCloud* pclXYZToQuadricPointCloudPtr(pcl::PointCloud::Ptr &pCloud)\n{\n EllipsoidSLAM::PointCloud* cloudPtr = new EllipsoidSLAM::PointCloud;\n EllipsoidSLAM::PointCloud &cloud = *cloudPtr;\n int num = pCloud->points.size();\n for(int i=0;ipoints[i];\n p.x = pT.x;\n p.y = pT.y;\n p.z = pT.z;\n cloud.push_back(p);\n }\n\n return cloudPtr;\n}\n\n\nvoid \nDownSamplePointCloud(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int param_num)\n{\n clock_t startTime,endTime;\n startTime = clock();\n PointCloudPCL::Ptr pPclCloud = QuadricPointCloudToPcl(cloudIn);\n endTime = clock();\n\n clock_t startTime_downsample = clock();\n\n pcl::VoxelGrid voxel;\n double gridsize = 0.02;\n voxel.setLeafSize( gridsize, gridsize, gridsize );\n voxel.setInputCloud( pPclCloud );\n PointCloudPCL::Ptr tmp( new PointCloudPCL() );\n voxel.filter( *tmp );\n clock_t endTime_downsample = clock();\n\n clock_t startTime_outlier = clock();\n PointCloudPCL::Ptr pPclCloudFiltered(new PointCloudPCL);\n \n pcl::StatisticalOutlierRemoval sor; \n sor.setInputCloud (tmp); \n sor.setMeanK (param_num); \n sor.setStddevMulThresh (1.0); \n sor.filter (*pPclCloudFiltered); \n\n clock_t endTime_outlier = clock();\n\n\n clock_t startTime2,endTime2;\n startTime2 = clock();\n cloudOut = pclToQuadricPointCloud(pPclCloudFiltered);\n endTime2 = clock();\n\n // cout << \"Time diff QuadricPointCloudToPcl: \" <<(double)(endTime - startTime) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"Time diff downsample: \" <<(double)(endTime_downsample - startTime_downsample) / CLOCKS_PER_SEC << \"s\" << endl;\n // cout << \"Time diff outlier: \" <<(double)(endTime_outlier - startTime_outlier) / CLOCKS_PER_SEC << \"s\" << endl;\n}\n\nvoid DownSamplePointCloudOnly(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, double grid)\n{\n PointCloudPCL::Ptr pPclCloud = QuadricPointCloudToPcl(cloudIn);\n\n pcl::VoxelGrid voxel;\n double gridsize = grid; \n voxel.setLeafSize( gridsize, gridsize, gridsize );\n voxel.setInputCloud( pPclCloud );\n PointCloudPCL::Ptr tmp( new PointCloudPCL() );\n voxel.filter( *tmp );\n\n cloudOut = pclToQuadricPointCloud(tmp);\n}\n\nvoid FiltOutliers(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int num_neighbor)\n{\n PointCloudPCL::Ptr pPclCloud = QuadricPointCloudToPcl(cloudIn);\n\n PointCloudPCL::Ptr pPclCloudFiltered(new PointCloudPCL);\n \n pcl::StatisticalOutlierRemoval sor; \n sor.setInputCloud (pPclCloud); \n sor.setMeanK (num_neighbor); \n sor.setStddevMulThresh (1.0); \n sor.filter (*pPclCloudFiltered); \n\n cloudOut = pclToQuadricPointCloud(pPclCloudFiltered);\n}\n\nvoid FiltPointsInBox(EllipsoidSLAM::PointCloud* pPoints_global, EllipsoidSLAM::PointCloud* pPoints_global_inBox, g2o::ellipsoid &e){\n double radius = MAX(MAX(e.scale[0], e.scale[1]), e.scale[2]);\n Vector3d center = e.toVector().head(3);\n \n pPoints_global_inBox->clear();\n\n Eigen::Matrix4d Q_star = e.generateQuadric();\n Eigen::Matrix4d Q = Q_star.inverse();\n for(auto p : (*pPoints_global) ) \n {\n Vector3d xyz(p.x, p.y, p.z);\n\n Eigen::Vector4d X = real_to_homo_coord_vec(xyz);\n\n double abstract_dis = X.transpose() * Q * X;\n bool isInside = (abstract_dis <0);\n\n if( isInside )\n pPoints_global_inBox->push_back(p);\n }\n\n return;\n}\n\n// add the points in point cloud p2 to p1\nvoid CombinePointCloud(EllipsoidSLAM::PointCloud *p1, EllipsoidSLAM::PointCloud *p2){\n if( p1 ==NULL || p2==NULL){\n cerr<< \" point cloud is NULL. \" << endl;\n return;\n }\n for(auto point : *p2)\n p1->push_back(point);\n\n return ; \n}" }, { "path": "src/symmetry/PointCloudFilter.h", "content": "// process point cloud: segmentation, downsample, filter...\n\n#ifndef ELLIPSOIDSLAM_POINTCLOUDFILTER_H\n#define ELLIPSOIDSLAM_POINTCLOUDFILTER_H\n\n// pcl\n#include \n#include \n#include \n#include \n#include \n\ntypedef pcl::PointXYZRGB PointT;\ntypedef pcl::PointCloud PointCloudPCL;\n\n#include \n#include \n\n#include \n\n#include \n#include \n\n#include \n\nVector2d getXYCenterOfPointCloud(EllipsoidSLAM::PointCloud* pPoints);\nEllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, cv::Mat &rgb, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range=100);\nEllipsoidSLAM::PointCloud getPointCloudInRect(cv::Mat &depth, const VectorXd &detect, EllipsoidSLAM::camera_intrinsic &camera, double range=100);\nvoid filterGround(EllipsoidSLAM::PointCloud** ppCloud);\nvoid outputCloud(EllipsoidSLAM::PointCloud *pCloud, int num = 10);\nEllipsoidSLAM::PointCloud pclToQuadricPointCloud(PointCloudPCL::Ptr &pCloud);\nPointCloudPCL::Ptr QuadricPointCloudToPcl(EllipsoidSLAM::PointCloud &cloud);\npcl::PointCloud::Ptr QuadricPointCloudToPclXYZ(EllipsoidSLAM::PointCloud &cloud);\n\nEllipsoidSLAM::PointCloud* pclToQuadricPointCloudPtr(PointCloudPCL::Ptr &pCloud);\n\nEllipsoidSLAM::PointCloud pclXYZToQuadricPointCloud(pcl::PointCloud::Ptr &pCloud);\nEllipsoidSLAM::PointCloud* pclXYZToQuadricPointCloudPtr(pcl::PointCloud::Ptr &pCloud);\n\n// downsample and outlier filter\nvoid DownSamplePointCloud(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int param_num = 100);\n\n// only downsample\nvoid DownSamplePointCloudOnly(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, double grid=0.02);\n\n// filter outliers\nvoid FiltOutliers(EllipsoidSLAM::PointCloud& cloudIn, EllipsoidSLAM::PointCloud& cloudOut, int num_neighbor = 100);\n\n// filter points and keep those in the ellipsoid\nvoid FiltPointsInBox(EllipsoidSLAM::PointCloud* pPoints_global, EllipsoidSLAM::PointCloud* pPoints_global_inBox, g2o::ellipsoid &e);\n\nvoid CombinePointCloud(EllipsoidSLAM::PointCloud *p1, EllipsoidSLAM::PointCloud *p2);\n\n#endif // POINTCLOUDFILTER" }, { "path": "src/symmetry/Symmetry.cpp", "content": "#include \"PointCloudFilter.h\"\n#include \"SymmetrySolver.h\"\n#include \"Symmetry.h\"\n\n#include \n\nnamespace EllipsoidSLAM\n{\n // ---- compare functions\n bool comp_func_mapPlane(const pair& t1, const pair& t2)\n { \n return t1.first > t2.first; \n } \n\n bool comp_func_mapProbData(const pair& t1, const pair& t2)\n { \n return t1.first > t2.first; \n } \n\n void SaveSymmetryResultToText(std::vector> &mapProbData, \n const string& path)\n {\n ofstream out(path.c_str());\n\n for( auto pair : mapProbData )\n {\n double prob = pair.first;\n auto data = pair.second;\n\n out <<\n data.pInitPlane->azimuth() << \"\\t\" <<\n data.pInitPlane->distance() << \"\\t\" <<\n data.init_error << \"\\t\" <<\n\n data.pPlane->azimuth() << \"\\t\" <<\n data.pPlane->distance() << \"\\t\" <<\n data.final_error << std::endl;\n\n }\n\n std::cout << \"[ Save symmetry result to : \" << path << \" ]\" << std::endl;\n }\n\n std::vector GenerateInitPlanes(int symType = 1)\n {\n /*\n * sample new planes along two degrees: angle and distance.\n * on each of them sample (2*step_num+1) planes.\n * totally, sample (step_num*2+1)*(step_num*2+1) planes around the init planes.\n */\n bool open_multiple_samples = true;\n\n if(open_multiple_samples)\n {\n // CONFIGURATION\n int step_num = 1;\n double diff_dis = 0.2; // 0.2m\n double diff_angle = M_PI/180.0*5; // 5deg\n\n double start_dis = -diff_dis * step_num;\n double start_angle = -diff_angle * step_num;\n\n int loop_num = 2 * step_num + 1;\n // ------------\n std::vector initPlanes;\n for(int i = 0; ifromDisAngleTrans(dis, angle, 0);\n initPlanes.push_back(pPlane);\n }\n }\n return initPlanes;\n }\n else\n {\n std::vector initPlanes;\n g2o::plane *pPlane = new g2o::plane;\n pPlane->fromDisAngleTrans(0, 0, 0);\n initPlanes.push_back(pPlane);\n return initPlanes;\n }\n }\n\n SymmetrySolverData Symmetry::estimateSymmetry(Vector4d &bbox, PointCloud* pCloud, VectorXd& pose, cv::Mat& projDepth, camera_intrinsic& camera, \n int symType)\n {\n SymmetrySolverData output; output.result = false;\n if( pCloud->size() < 1 ) \n {\n std::cerr << \" Point Cloud Size has some problems: \" << pCloud->size() << std::endl;\n return output;\n }\n\n Matrix3d calib = getCalibFromCamera(camera);\n \n SymmetrySolver solver;\n solver.SetCameraParam(camera);\n // set the border\n if( mbOpenSparseEstimation )\n solver.SetBorders(mpBorders);\n std::vector> mapProbData;\n\n std::vector initPlanes = GenerateInitPlanes(symType);\n\n int opt_time = initPlanes.size();\n for(int i=0; i< opt_time ; i ++){ \n g2o::plane* pPlane= initPlanes[i];\n SymmetrySolverData data;\n if(symType == 1)\n data = solver.OptimizeSymmetryPlane(bbox, *pPlane, pCloud, projDepth, pose, calib, camera.scale, symType);\n else if(symType == 2)\n data = solver.OptimizeSymmetryDualPlane(bbox, *pPlane, pCloud, projDepth, pose, calib, camera.scale, symType);\n double prob = data.prob;\n mapProbData.push_back(make_pair(prob, data));\n }\n\n\n sort(mapProbData.begin(), mapProbData.end(), comp_func_mapProbData);\n assert(mapProbData.size() > 0 && \"Wrong size in mapProbData.\");\n auto pair = mapProbData[0];\n\n output = pair.second;\n return output;\n }\n\n void Symmetry::releaseData(SymmetryOutputData& data)\n {\n if(data.pCloud != NULL)\n delete data.pCloud;\n }\n\n // change the distance between a point to the image plane to the distance between a point to the camera center\n double calculateProjZ(double f, double d, double xi, double yi){\n return d*sqrt(xi*xi+f*f+yi*yi)/f;\n }\n\n cv::Mat Symmetry::getProjDepthMat(cv::Mat& depth, camera_intrinsic& camera){\n int rows = depth.rows;\n int cols = depth.cols;\n\n cv::Mat depthProj = cv::Mat(rows, cols, CV_16UC1, cv::Scalar(0));\n\n double fx = camera.fx;\n double cx = camera.cx;\n double cy = camera.cy;\n for( int x=0;x(y,x);\n\n double realz = calculateProjZ(fx, double(d), (x - cx), (y - cy));\n \n depthProj.at(y, x) = ushort(realz);\n }\n \n return depthProj;\n }\n\n void Symmetry::SetBorders(EllipsoidSLAM::PointCloud* pBorders)\n {\n mbOpenSparseEstimation = true;\n mpBorders = pBorders;\n }\n\n Symmetry::Symmetry()\n {\n mbOpenSparseEstimation = false;\n\n mpExtractor = new BorderExtractor;\n\n miParamFilterPointNum = 100;\n\n mbGroundPlaneSet = false;\n\n mbObjInitialGuessSet = false;\n\n mbInitPlanesSet = false;\n }\n\n}" }, { "path": "src/symmetry/Symmetry.h", "content": "#ifndef ELLIPSOIDSLAM_SYMMETRY_H\n#define ELLIPSOIDSLAM_SYMMETRY_H\n\n#include \n#include \n\n#include \nusing namespace Eigen;\n\n#include \n#include \"SymmetrySolver.h\"\n#include \"BorderExtractor.h\"\n\nnamespace EllipsoidSLAM {\n\nclass SymmetryOutputData\n{\npublic:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n bool result;\n \n PointCloud* pCloud; // object point cloud\n Vector4d planeVec;\n Vector4d planeVec2; // the second plane of dual reflection\n\n double prob;\n PointCloud* pBorders; // object borders, used in the sparse mode; invalid now.\n\n Vector3d center; // for visualizing the symmetry plane as an finite plane\n int symmetryType;\n};\n\nclass Symmetry {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n Symmetry();\n\n // API: estimate a symmetry plane from a point cloud using all the points\n // symType: 1 reflection symmetry; 2 dual reflection symmetry\n SymmetrySolverData estimateSymmetry(Vector4d& bbox, PointCloud* pCloud, VectorXd& pose, cv::Mat& projDepth, camera_intrinsic& camera,\n int symType=1);\n \n void static releaseData(SymmetryOutputData& data);\n\n cv::Mat static getProjDepthMat(cv::Mat& depth, camera_intrinsic& camera);\n\n public:\n void SetGroundPlane(Vector4d& normal);\n\n void SetBorders(EllipsoidSLAM::PointCloud* pBorders); \n void SetConfigResolution(double res);\n double GetConfigResolution();\n void SetConfigFilterPointNum(int num);\n private: \n bool mbOpenSparseEstimation;\n PointCloud* mpBorders;\n\n BorderExtractor* mpExtractor;\n\n int miParamFilterPointNum;\n\n Vector4d mGroundPlaneNormal;\n bool mbGroundPlaneSet;\n\n g2o::ellipsoid mObjInitialGuess;\n bool mbObjInitialGuessSet;\n\n std::vector mvpInitPlanes;\n bool mbInitPlanesSet;\n};\n\n}\n\n#endif //ELLIPSOIDSLAM_SYMMETRY_H\n" }, { "path": "src/symmetry/SymmetrySolver.cpp", "content": "#include \"core/Ellipsoid.h\"\n#include \"SymmetrySolver.h\"\n#include \"PointCloudFilter.h\"\n#include \"utils/matrix_utils.h\"\n// g2o\n#include \"Thirdparty/g2o/g2o/core/block_solver.h\"\n#include \"Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h\"\n#include \"Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h\"\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h\"\n#include \"Thirdparty/g2o/g2o/solvers/linear_solver_dense.h\"\n\n#include \"Thirdparty/g2o/g2o/core/robust_kernel.h\"\n#include \"Thirdparty/g2o/g2o/core/robust_kernel_impl.h\"\n\n#include \n#include \n\nusing namespace EllipsoidSLAM;\n\nstatic int calcu_time = 0;\n\nbool isInRange(int x, int y, int range_x, int range_x2, int range_y, int range_y2)\n{\n if( range_x < x && x < range_x2 )\n if( range_y& kdTree)\n{\n g2o::ellipsoid e;\n g2o::SE3Quat campose_wc, campose_cw; \n campose_wc.fromVector(pose.tail(7));\n campose_cw = campose_wc.inverse();\n Matrix3Xd projMat = e.generateProjectionMatrix(campose_cw, calib); \n \n int num = pCloudSym->size();\n\n double ln_total_P = 0;\n\n int num_invalid = 0;\n for( int i=0; i(uvHomo);\n \n int x = int(uv[0]);\n int y = int(uv[1]);\n\n double dis_diff = 0;\n\n if( isInRange(x,y,bbox[0],bbox[2],bbox[1],bbox[3]) ) \n {\n ushort *ptd = depth.ptr(int(uv[1])); \n ushort d = ptd[int(uv[0])];\n\n if( d == 0 ) // occluded area\n {\n dis_diff = 0;\n } \n else \n {\n double depth = d / scale;\n\n Vector3d cam_c = campose_wc.toVector().head(3);\n Vector3d point(p.x,p.y,p.z);\n double dis_cam_point = (cam_c-point).norm();\n\n if( dis_cam_point > depth ) // occluded area\n {\n dis_diff = 0;\n }\n else // observable area\n {\n if(!checkValidPoint(p)) {\n dis_diff = 0;\n num_invalid++;\n }\n else\n dis_diff = findMinimalDistanceWithKdTree(p, kdTree);\n }\n }\n\n }\n else // observable area\n {\n if(!checkValidPoint(p)) {\n dis_diff = 0;\n num_invalid++;\n }\n else\n dis_diff = findMinimalDistanceWithKdTree(p, kdTree);\n }\n\n // Sigma is used for probability calculation. it's useless in the optimization process.\n double Sigma = Config::ReadValue(\"SymmetrySolver.Sigma\"); \n double Sigma_inv = 1.0 / Sigma; \n\n double ln_P = -0.5*Sigma_inv*Sigma_inv*dis_diff*dis_diff;\n ln_total_P += ln_P; \n }\n\n int num_valid = num - num_invalid;\n double aver_ln_P;\n if(num_valid > 0)\n aver_ln_P = ln_total_P / double(num_valid); \n else\n aver_ln_P = -INFINITY;\n mData.pSymetryCloud = pCloudSym;\n\n if(num_invalid>0)\n std::cout << \" - Invalid/Valid SymPoints Num : \" << num_invalid << \" / \" << num_valid << std::endl;\n\n return aver_ln_P;\n}\n\ndouble SymmetrySolver::findMinimalDistanceWithKdTree(PointXYZRGB &p, pcl::KdTreeFLANN& kdTree)\n{\n pcl::PointXYZ searchPoint;\n\n searchPoint.x = p.x;\n searchPoint.y = p.y;\n searchPoint.z = p.z;\n\n // K nearest neighbor search\n int K = 1;\n\n std::vector pointIdxNKNSearch(K);\n std::vector pointNKNSquaredDistance(K);\n\n kdTree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);\n \n assert( pointIdxNKNSearch.size() == 1 && \"Error in KdTree Search.\" );\n\n int nearest_id = pointIdxNKNSearch[0];\n pcl::PointXYZ nearest_point = kdTree.getInputCloud()->points[nearest_id];\n\n Vector3d pos1(p.x,p.y,p.z);\n Vector3d pos2(nearest_point.x,nearest_point.y,nearest_point.z);\n double dis = (pos1-pos2).norm();\n\n return dis;\n}\n\nPointCloud* SymmetrySolver::GetSymmetryPointCloud(PointCloud* pCloud, g2o::plane &plane, int symType)\n{\n PointCloud* pCloudSym = new PointCloud;\n\n int num = pCloud->size();\n for(int i=0;i(point);\n Vector4d pointSymHomo = GetSymmetryPointOfPlane(point_homo, plane);\n Vector3d pointSym = homo_to_real_coord_vec(pointSymHomo);\n \n auto pSym = p;\n\n pSym.g = 255;\n pSym.x = pointSym[0];\n pSym.y = pointSym[1];\n pSym.z = pointSym[2];\n pCloudSym->push_back(pSym);\n\n }\n return pCloudSym;\n}\n\nVector4d SymmetrySolver::GetSymmetryPointOfPlane(Vector4d& point, g2o::plane &plane)\n{\n // get the plane normal vector\n Vector3d normal = plane.param.head(3);\n normal.normalize();\n\n double up_equation = std::abs(plane.param.transpose()*point);\n double down_equation = std::sqrt(plane.param.head(3).transpose()*plane.param.head(3));\n double dis = up_equation/down_equation;\n\n Vector3d point_3 = homo_to_real_coord_vec(point);\n\n\n double symbol;\n double symbol_value = point.transpose()*plane.param;\n if(symbol_value > 0) symbol =-1 ;\n else symbol = 1;\n\n Vector3d symPoint = point_3 + 2*symbol*dis*normal;\n Vector4d symPointHomo = real_to_homo_coord_vec(symPoint);\n return symPointHomo;\n}\n\n// Given an initial symmetry plane, output an optimized symmetry plane\nSymmetrySolverData SymmetrySolver::OptimizeSymmetryPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, \n int symType)\n{\n g2o::SparseOptimizer graph;\n g2o::BlockSolverX::LinearSolverType* linearSolver;\n linearSolver = new g2o::LinearSolverDense();\n g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);\n g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);\n graph.setAlgorithm(solver);\n graph.setVerbose(false); // output\n\n // add vertices\n g2o::plane plane = initPlane;\n g2o::VertexPlane* vpVertexPlane = new g2o::VertexPlane;\n vpVertexPlane->setEstimate(plane);\n vpVertexPlane->setId(0);\n vpVertexPlane->setFixed(false);\n graph.addVertex(vpVertexPlane);\n\n // add edges\n g2o::EdgeSymmetryPlane* e = new g2o::EdgeSymmetryPlane();\n e->setVertex(0, dynamic_cast( vpVertexPlane ));\n e->setMeasurement(1); \n e->setId(0);\n double inv_sigma;inv_sigma = 1; \n inv_sigma = inv_sigma*1; \n Matrix info; info(0,0) = inv_sigma;\n e->setInformation(info);\n\n // initialize edge parameter\n e->initializeParam(bbox, pCloud, depth_origin, pose, calib, scale);\n e->initializeKDTree(); \n\n if( mbOpenSparseEstimation )\n e->setBordersPointCloud( mpBorders );\n\n graph.addEdge(e);\n\n // save \n mData.pInitPlane = new g2o::plane(vpVertexPlane->estimate());\n\n e->computeError();\n Matrix errorMat_init = e->error();\n mData.init_error = errorMat_init(0,0);\n\n graph.initializeOptimization();\n graph.optimize(5); // the number could be adjusted according to your time-efficiency trade\n\n Matrix errorMat = e->error();\n\n // save result.\n mData.prob = exp(-errorMat(0,0)); // probability calculation could be adjusted here.\n mData.pPlane = new g2o::plane(vpVertexPlane->estimate());\n mData.pPlane->color = Vector3d(1,0,0);\n mData.symType = symType;\n mData.final_error = errorMat(0,0);\n mData.result = true;\n return mData;\n \n}\n\n// this one is the dual plane version.\nSymmetrySolverData SymmetrySolver::OptimizeSymmetryDualPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, \n int symType)\n{\n g2o::SparseOptimizer graph;\n g2o::BlockSolverX::LinearSolverType* linearSolver;\n linearSolver = new g2o::LinearSolverDense();\n g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);\n g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);\n graph.setAlgorithm(solver);\n graph.setVerbose(false); \n \n g2o::VertexDualPlane* vpVertexDualPlane = new g2o::VertexDualPlane;\n vpVertexDualPlane->setEstimate(initPlane);\n vpVertexDualPlane->setId(0);\n vpVertexDualPlane->setFixed(false);\n graph.addVertex(vpVertexDualPlane);\n\n g2o::EdgeSymmetryDualPlane* e = new g2o::EdgeSymmetryDualPlane();\n e->setVertex(0, dynamic_cast( vpVertexDualPlane ));\n e->setMeasurement(1); \n e->setId(0);\n double inv_sigma;inv_sigma = 1; \n Matrix info; info(0,0) = inv_sigma;\n e->setInformation(info);\n\n e->initializeParam(bbox, pCloud, depth_origin, pose, calib, scale);\n e->initializeKDTree(); \n\n if( mbOpenSparseEstimation )\n e->setBordersPointCloud( mpBorders );\n\n graph.addEdge(e);\n\n mData.pInitPlane = new g2o::plane(initPlane.GeneratePlaneVec());\n mData.pInitPlane2 = new g2o::plane(initPlane.GenerateAnotherPlaneVec());\n e->computeError();\n Matrix errorMat_init = e->error();\n mData.init_error = errorMat_init(0,0);\n\n graph.initializeOptimization();\n graph.optimize(5); \n\n Matrix errorMat = e->error();\n\n mData.prob = exp(-errorMat(0,0));\n g2o::plane dualplaneOptimized = vpVertexDualPlane->estimate();\n mData.pPlane = new g2o::plane(dualplaneOptimized.GeneratePlaneVec());\n mData.pPlane2 = new g2o::plane(dualplaneOptimized.GenerateAnotherPlaneVec());\n mData.pPlane->color = Vector3d(1.0,0.3,0);\n mData.pPlane2->color = Vector3d(1.0,0.3,0);\n mData.symType = symType;\n mData.result = true;\n mData.final_error = errorMat(0,0);\n return mData;\n}\n\nvoid SymmetrySolver::SetCameraParam(camera_intrinsic& camera){\n mCamera = camera;\n}\n\nvoid SymmetrySolver::SetInitPlane(g2o::plane* plane){\n mpInitPlane = plane;\n}\n\nSymmetrySolverData SymmetrySolver::getResult()\n{\n return mData;\n}\n\nvoid SymmetrySolver::SetBorders(EllipsoidSLAM::PointCloud* pBorders){\n mpBorders = pBorders;\n mbOpenSparseEstimation = true;\n}\n\nSymmetrySolverData SymmetrySolver::mData;\n\nnamespace g2o\n{\n EdgeSymmetryPlane::EdgeSymmetryPlane()\n {\n mbInitilized = false;\n mbKdTreeInitialized = false;\n\n mpBorders = NULL; \n miSymmetryType = 1; // default: reflection symmetry\n }\n\n bool EdgeSymmetryPlane::read(std::istream& is){\n return true;\n };\n\n bool EdgeSymmetryPlane::write(std::ostream& os) const\n {\n return os.good();\n };\n\n void EdgeSymmetryPlane::computeError()\n {\n if(!mbInitilized)\n cerr << \"Symmetry Edge has not been initialized yet.\" << endl;\n const VertexPlane* planeVertex = static_cast(_vertices[0]);\n\n assert( mpCloud != NULL && \"Point cloud shouldn't be NULL.\");\n\n g2o::plane plane = planeVertex->estimate();\n\n // generate mirrored pointcloud\n PointCloud* pObjectCloud;\n if(mpBorders == NULL ) pObjectCloud = mpCloud;\n else pObjectCloud = mpBorders;\n PointCloud* pCloudSym = SymmetrySolver::GetSymmetryPointCloud(pObjectCloud, plane);\n\n double cost;\n assert( mbKdTreeInitialized == true && \"Please initialize kdTree.\");\n cost = SymmetrySolver::GetPointCloudProb(mBbox, pCloudSym, mpCloud, mDepth, mPose, mCalib, mScale, mKdtree);\n \n _error = Matrix( - cost);\n }\n\n void EdgeSymmetryPlane::initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale)\n {\n mpCloud = pCloud;\n mDepth = depth;\n mPose = pose;\n mCalib = calib;\n mScale = scale;\n\n mBbox = bbox;\n\n mbInitilized = true;\n }\n\n void EdgeSymmetryPlane::initializeKDTree()\n {\n if(mpCloud == NULL )\n {\n cerr << \"Point cloud is NULL.\" << endl;\n return;\n }\n\n pcl::PointCloud::Ptr cloudPCL (new pcl::PointCloud); \n int num = mpCloud->size();\n for(int i=0;ipoints.push_back(p);\n }\n\n mKdtree.setInputCloud (cloudPCL);\n mbKdTreeInitialized = true;\n }\n\n void EdgeSymmetryPlane::setBordersPointCloud(PointCloud* pBorders)\n {\n mpBorders = pBorders;\n }\n\n EdgeSymmetryDualPlane::EdgeSymmetryDualPlane()\n {\n mbInitilized = false;\n mbKdTreeInitialized = false;\n\n mpBorders = NULL; \n miSymmetryType = 1; // default: reflection symmetry\n }\n\n bool EdgeSymmetryDualPlane::read(std::istream& is){\n return true;\n };\n\n bool EdgeSymmetryDualPlane::write(std::ostream& os) const\n {\n return os.good();\n };\n\n void EdgeSymmetryDualPlane::computeError()\n {\n if(!mbInitilized)\n cerr << \"Symmetry Edge has not been initialized yet.\" << endl;\n const VertexDualPlane* dualplaneVertex = static_cast(_vertices[0]);\n\n assert( mpCloud != NULL && \"Point cloud shouldn't be NULL.\");\n\n g2o::plane dualplane = dualplaneVertex->estimate();\n g2o::plane plane1(dualplane.GeneratePlaneVec());\n g2o::plane plane2(dualplane.GenerateAnotherPlaneVec());\n\n PointCloud* pObjectCloud;\n if(mpBorders == NULL ) pObjectCloud = mpCloud;\n else pObjectCloud = mpBorders;\n\n PointCloud* pCloudSym1 = SymmetrySolver::GetSymmetryPointCloud(pObjectCloud, plane1);\n PointCloud* pCloudSym2 = SymmetrySolver::GetSymmetryPointCloud(pObjectCloud, plane2);\n CombinePointCloud(pCloudSym1, pCloudSym2);\n assert( mbKdTreeInitialized && \" Please initialize the kdtree \" );\n double cost = SymmetrySolver::GetPointCloudProb(mBbox, pCloudSym1, mpCloud, mDepth, mPose, mCalib, mScale, mKdtree);\n \n _error = Matrix( - cost);\n }\n\n void EdgeSymmetryDualPlane::initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale)\n {\n mpCloud = pCloud;\n mDepth = depth;\n mPose = pose;\n mCalib = calib;\n mScale = scale;\n\n mBbox = bbox;\n\n mbInitilized = true;\n }\n\n void EdgeSymmetryDualPlane::initializeKDTree()\n {\n if(mpCloud == NULL )\n {\n cerr << \"Point cloud is NULL.\" << endl;\n return;\n }\n\n pcl::PointCloud::Ptr cloudPCL (new pcl::PointCloud); \n int num = mpCloud->size();\n for(int i=0;ipoints.push_back(p);\n }\n\n mKdtree.setInputCloud (cloudPCL);\n mbKdTreeInitialized = true;\n }\n\n void EdgeSymmetryDualPlane::setBordersPointCloud(PointCloud* pBorders)\n {\n mpBorders = pBorders;\n }\n\n void VertexPlane::setToOriginImpl() { _estimate = g2o::plane(); }\n\n void VertexPlane::oplusImpl(const double* update_) {\n Eigen::Map update(update_);\n Vector3d update2DOF(update[0], 0, update[1]); // yaw, 0, dis\n\n _estimate.oplus(update2DOF); \n }\n\n bool VertexPlane::read(std::istream& is) {\n return true;\n }\n\n bool VertexPlane::write(std::ostream& os) const {\n return os.good();\n }\n\n void VertexDualPlane::setToOriginImpl() { _estimate = g2o::plane(); }\n\n void VertexDualPlane::oplusImpl(const double* update_) {\n Eigen::Map update(update_);\n _estimate.oplus_dual(update);\n }\n\n bool VertexDualPlane::read(std::istream& is) {\n return true;\n }\n\n bool VertexDualPlane::write(std::ostream& os) const {\n return os.good();\n }\n\n} // namespace g2o\n" }, { "path": "src/symmetry/SymmetrySolver.h", "content": "#ifndef ELLIPSOIDSLAM_SYMMETRYSOLVER_H\n#define ELLIPSOIDSLAM_SYMMETRYSOLVER_H\n\n#include \n#include \n#include \n\n#include \n\n#include \n#include \n\nusing namespace Eigen;\nusing namespace EllipsoidSLAM;\n\nclass SymmetrySolverData\n{\npublic:\n PointCloud* pSymetryCloud;\n g2o::plane* pPlane;\n g2o::plane* pPlane2; \n\n double prob;\n int symType;\n bool result;\n \n // save the initial data\n g2o::plane* pInitPlane;\n g2o::plane* pInitPlane2;\n double init_error;\n double final_error;\n};\n\nclass SymmetrySolver\n{\n\npublic:\n SymmetrySolver();\n \n SymmetrySolverData OptimizeSymmetryPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, int symType);\n SymmetrySolverData OptimizeSymmetryDualPlane(Vector4d &bbox, g2o::plane& initPlane, PointCloud* pCloud, cv::Mat &depth_origin, VectorXd &pose, Matrix3d &calib, double scale, int symType);\n \n static double GetPlaneProbWithKdTree(g2o::plane &plane, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale, pcl::KdTreeFLANN& kdTree, \n PointCloud* pBorders = NULL);\n\n static double GetPointCloudProb(Vector4d &bbox, PointCloud* pCloudSym, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale, pcl::KdTreeFLANN& kdTree);\n static double GetPlaneProbWithKdTreeUsingAdvanceProbEquation(g2o::plane &plane, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale, pcl::KdTreeFLANN& kdTree, \n PointCloud* pBorders = NULL);\n\n static double calculatePlaneCost(g2o::plane &plane, PointCloud* pCloud);\n\n static PointCloud* GetSymmetryPointCloud(PointCloud* pCloud, g2o::plane &p, int symType = 1);\n\npublic:\n void SetBorders(EllipsoidSLAM::PointCloud* pBorders);\n\nprivate:\n static double calculateOcclProbOfPoints(PointCloud *pCloudSym, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale);\n static double calculateMatchProbOfPoints(PointCloud* pCloud, PointCloud* pCloudSym);\n static double findMinimalDistance(PointXYZRGB &p, PointCloud* pCloud);\n static Vector4d GetSymmetryPointOfPlane(Vector4d& point, g2o::plane &plane);\n\n static double calculateMatchProbOfPointsWithKdTree(PointCloud* pCloud, PointCloud* pCloudSym, pcl::KdTreeFLANN& kdTree);\n static double findMinimalDistanceWithKdTree(PointXYZRGB &p, pcl::KdTreeFLANN& kdTree);\n\npublic:\n\n static SymmetrySolverData mData;\n\nprivate:\n bool mbOpenSparseEstimation;\n PointCloud* mpBorders;\n\n std::map mmLabelSymmetry;\n\npublic:\n void SetCameraParam(camera_intrinsic& camera);\n void SetInitPlane(g2o::plane* plane);\n SymmetrySolverData getResult();\nprivate:\n camera_intrinsic mCamera;\n g2o::plane* mpInitPlane;\n\n};\n\n/*\n* This part contains Vertex, Edges in symmetry optimization using g2o.\n*/\nnamespace g2o\n{\n class VertexPlane:public BaseVertex<2,g2o::plane> \n {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n VertexPlane(){};\n\n virtual void setToOriginImpl();\n\n virtual void oplusImpl(const double* update_);\n\n virtual bool read(std::istream& is) ;\n\n virtual bool write(std::ostream& os) const ;\n\n };\n\n // Dual Plane \n class VertexDualPlane:public BaseVertex<3,g2o::plane> \n {\n public:\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW;\n VertexDualPlane(){};\n\n virtual void setToOriginImpl();\n\n virtual void oplusImpl(const double* update_);\n\n virtual bool read(std::istream& is) ;\n\n virtual bool write(std::ostream& os) const ;\n\n };\n\n class EdgeSymmetryPlane:public BaseUnaryEdge<1,double, VertexPlane>\n {\n public:\n EdgeSymmetryPlane();\n\n virtual bool read(std::istream& is);\n\n virtual bool write(std::ostream& os) const;\n\n void computeError();\n\n void initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale);\n void initializeKDTree();\n\n void setBordersPointCloud(PointCloud* pBorders);\n private:\n EllipsoidSLAM::PointCloud* mpCloud;\n cv::Mat mDepth;\n VectorXd mPose;\n Matrix3d mCalib;\n double mScale;\n\n Vector4d mBbox; //bounding box.\n\n bool mbInitilized;\n\n pcl::KdTreeFLANN mKdtree;\n bool mbKdTreeInitialized;\n\n EllipsoidSLAM::PointCloud* mpBorders;\n\n int miSymmetryType;\n\n };\n\n\n class EdgeSymmetryDualPlane:public BaseUnaryEdge<1,double, VertexDualPlane>\n {\n public:\n EdgeSymmetryDualPlane();\n\n virtual bool read(std::istream& is);\n\n virtual bool write(std::ostream& os) const;\n\n void computeError();\n\n void initializeParam(Vector4d &bbox, PointCloud* pCloud, cv::Mat& depth, VectorXd &pose, Matrix3d &calib, double scale);\n void initializeKDTree();\n\n void setBordersPointCloud(PointCloud* pBorders);\n\n void setSymmetryType(int type);\n\n private:\n EllipsoidSLAM::PointCloud* mpCloud;\n cv::Mat mDepth;\n VectorXd mPose;\n Matrix3d mCalib;\n double mScale;\n\n Vector4d mBbox; //bounding box.\n\n bool mbInitilized;\n\n pcl::KdTreeFLANN mKdtree;\n bool mbKdTreeInitialized;\n\n EllipsoidSLAM::PointCloud* mpBorders;\n\n int miSymmetryType;\n\n };\n}\n\n\n#endif" }, { "path": "src/tum_rgbd/CMakeLists.txt", "content": "# set(CMAKE_BUILD_TYPE Debug)\nadd_library(tum_rgbd SHARED\n io.cpp\n)\n\ntarget_link_libraries(tum_rgbd\n ${OpenCV_LIBS}\n utils\n )\n" }, { "path": "src/tum_rgbd/io.cpp", "content": "// Basic input and output of the TUM-RGBD dataset.\n\n#include \"io.h\"\n#include \"utils/dataprocess_utils.h\"\n\n#include \n#include \n\n\nusing namespace std;\nnamespace TUMRGBD\n{\n\n void Dataset::loadDataset(string &path){\n cout << \"Load dataset from: \" << path << endl;\n \n msDatasetDir = path;\n msRGBDir = msDatasetDir + \"rgb/\";\n msDepthDir = msDatasetDir + \"depth/\";\n msGroundtruthPath = msDatasetDir + \"groundtruth.txt\";\n msAssociatePath = msDatasetDir + \"associate.txt\";\n msAssociateGroundtruthPath = msDatasetDir + \"associateGroundtruth.txt\";\n\n // get all the file names under the directory\n GetFileNamesUnderDir(msRGBDir, mvRGBFileNames);\n sortFileNames(mvRGBFileNames, mvRGBFileNames);\n miTotalNum = mvRGBFileNames.size();\n\n // generate the map between the timestamps to the depth images\n loadGroundTruthToMap(msGroundtruthPath);\n\n // generate the index from ID to the timestamps of rgb images\n generateIndexIdToRGBTimeStamp(); \n // generate the associations from rgb Timestamp to depth Timestamp\n LoadAssociationRGBToDepth(msAssociatePath);\n // generate the associations from rgb timestamp to the groundtruth\n LoadAssociationRGBToGroundtruth(msAssociateGroundtruthPath);\n\n // get debug file num:\n std::cout << \"mmTimeStampToPose: \" << mmTimeStampToPose.size() << std::endl;\n std::cout << \"mvIdToDepthImagePath: \" << mvIdToDepthImagePath.size() << std::endl;\n std::cout << \"mvIdToGroundtruthTimeStamp: \" << mvIdToGroundtruthTimeStamp.size() << std::endl;\n \n\n miCurrentID = 0;\n mbDetectionLoaded = false;\n mbOdomSet = false; \n }\n\n bool Dataset::readFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose){\n if(miCurrentID < miTotalNum) {\n miCurrentID++;\n bool result = findFrameUsingID(miCurrentID-1, rgb, depth, pose);\n return result;\n }\n else\n {\n std::cout << \"[Dataset] no data left.\" << std::endl;\n return false;\n }\n }\n\n bool Dataset::findFrameUsingID(int id, cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose){\n if( id <0 || id>=miTotalNum) return false;\n \n int currentID = id;\n string depthTimeStampAssociated = mvIdToDepthTimeStamp[currentID];\n if(depthTimeStampAssociated == \"\"){\n std::cout << \"[Dataset] fail to load the depth timestamp.\" << std::endl;\n return false; \n }\n\n string depthPath = msDatasetDir + \"/\" + mvIdToDepthImagePath[currentID];\n\n string gtTimeStamp = mvIdToGroundtruthTimeStamp[currentID];\n\n bool bFindPose = false;\n \n if( !mbOdomSet ) // if the odometry is set, return the pose of the odometry instead of the groundtruth\n bFindPose = getPoseFromTimeStamp(gtTimeStamp, pose);\n else\n bFindPose = getPoseFromRGBTimeStamp(mvIdToRGBTimeStamp[currentID], pose); \n\n if(bFindPose){\n rgb = cv::imread(mvRGBFileNames[currentID], IMREAD_UNCHANGED);\n depth = cv::imread(depthPath, IMREAD_UNCHANGED);\n return true;\n }\n else\n {\n std::cout << \"[Dataset] fail to find the pose .\" << std::endl;\n return false; \n }\n }\n\n\n map::const_iterator AssociateWithNumber(const map& map, const string ×tamp){\n auto iter = map.begin();\n for( ; iter != map.end(); iter++ )\n {\n if( iter->first == \"\" || timestamp == \"\" ) continue;\n if( std::abs(atof(iter->first.c_str()) - atof(timestamp.c_str())) < 0.001 )\n {\n // get it\n return iter;\n }\n }\n return iter;\n }\n\n map::const_iterator AssociateWithNumber(const map& map, const string ×tamp){\n auto iter = map.begin();\n for( ; iter != map.end(); iter++ )\n {\n if( iter->first == \"\" || timestamp == \"\" ) continue;\n if( std::abs(atof(iter->first.c_str()) - atof(timestamp.c_str())) < 0.001 )\n {\n // get it\n return iter;\n }\n }\n return iter;\n }\n\n // get pose from the map using the timestamp as index.\n // attention: this version uses string type of timestamp to search, which means their timestamp MUST be totally the same.\n // it will be updated to double type in the future.\n bool Dataset::getPoseFromTimeStamp(string ×tamp, VectorXd &pose){\n for( auto iter : mmTimeStampToPose )\n {\n if( std::abs(std::atof(iter.first.c_str()) - std::atof(timestamp.c_str())) < 0.001 )\n {\n pose = iter.second;\n return true;\n }\n }\n\n return false; \n }\n\n bool Dataset::getPoseFromRGBTimeStamp(string ×tamp, VectorXd &pose){\n auto iter = AssociateWithNumber(mmOdomRGBStampToPose, timestamp);\n // auto iter = mmOdomRGBStampToPose.find(timestamp);\n if( iter != mmOdomRGBStampToPose.end() )\n {\n pose = iter->second;\n return true;\n }\n else\n {\n return false;\n }\n \n }\n\n void Dataset::loadGroundTruthToMap(string &path){\n std::vector> strMat = readStringFromFile(path.c_str(), 0);\n\n int totalPose = strMat.size();\n for(int i=0;i strVec = strMat[i];\n string timestamp = strVec[0];\n\n VectorXd pose; pose.resize(7);\n for(int p=1;p<8;p++)\n pose(p-1) = stod(strVec[p]);\n \n mmTimeStampToPose.insert(make_pair(timestamp, pose));\n }\n\n }\n\n void Dataset::LoadAssociationRGBToDepth(string &path)\n {\n std::vector> associateMat = readStringFromFile(msAssociatePath.c_str());\n\n map mapRGBToDepth;\n map mapRGBToDepthImagePath;\n int associationNum = associateMat.size();\n for(int i=0;i lineVec = associateMat[i];\n string rgbTS = lineVec[0];\n string depthTS = lineVec[2];\n mapRGBToDepth.insert(make_pair(rgbTS, depthTS));\n mapRGBToDepthImagePath.insert(make_pair(rgbTS, lineVec[3]));\n }\n\n // for every rgb timestamp, find an associated depth timestamp\n mvIdToDepthTimeStamp.resize(miTotalNum);\n mvIdToDepthImagePath.resize(miTotalNum);\n for( int p=0;psecond;\n }\n else\n {\n mvIdToDepthTimeStamp[p] = \"\"; // empty stands for null\n }\n mvIdToDepthImagePath[p] = mapRGBToDepthImagePath[iter->first];\n }\n }\n\n void Dataset::LoadAssociationRGBToGroundtruth(string &path)\n {\n std::vector> associateMat = readStringFromFile(msAssociateGroundtruthPath.c_str());\n\n map mapRGBToGt;\n int associationNum = associateMat.size();\n for(int i=0;i lineVec = associateMat[i];\n string rgbTS = lineVec[0];\n string gtTS = lineVec[2];\n\n // Considering the precision of the timestamps of the result from the associate.py in TUM-RGB-D dataset,\n // we need to eliminate two zeros in the tails to make the groundtruth and the association have the same precision\n gtTS = gtTS.substr(0, gtTS.length()-2);\n\n mapRGBToGt.insert(make_pair(rgbTS, gtTS));\n }\n\n // for every rgb timestamp, find an associated groundtruth timestamp\n mvIdToGroundtruthTimeStamp.resize(miTotalNum);\n for( int p=0;psecond;\n }\n else\n {\n mvIdToGroundtruthTimeStamp[p] = \"\"; // empty stands for null\n }\n \n }\n }\n\n void Dataset::generateIndexIdToRGBTimeStamp(){\n // extract the bare name from a full path\n mvIdToRGBTimeStamp.clear();\n for(auto s:mvRGBFileNames)\n {\n string bareName = splitFileNameFromFullDir(s, true);\n mvIdToRGBTimeStamp.push_back(bareName);\n }\n }\n\n bool Dataset::empty(){\n return miCurrentID >= miTotalNum;\n }\n\n int Dataset::getCurrentID(){\n return miCurrentID;\n }\n\n int Dataset::getTotalNum()\n {\n return miTotalNum;\n }\n\n bool Dataset::loadDetectionDir(string &path)\n {\n msDetectionDir = path;\n mbDetectionLoaded = true;\n\n return true;\n }\n\n Eigen::MatrixXd Dataset::getDetectionMat(){\n Eigen::MatrixXd detMat;\n if(!mbDetectionLoaded){\n std::cerr << \"Detection dir has not loaded yet.\" << std::endl;\n return detMat;\n }\n // get the RGB timestamp as the name of the object detection file\n string bairName = mvIdToRGBTimeStamp[miCurrentID-1];\n string fullPath = msDetectionDir + bairName + \".txt\";\n\n detMat = readDataFromFile(fullPath.c_str());\n return detMat;\n \n }\n\n vector Dataset::generateValidVector(){\n vector validVec;\n for(int i=0; i=miTotalNum) return false;\n \n int currentID = id;\n \n string depthTimeStampAssociated = mvIdToDepthTimeStamp[currentID];\n if(depthTimeStampAssociated == \"\"){\n std::cout << \"No depthTimeStampAssociated. \" << std::endl;\n return false; \n }\n\n string gtTimeStamp = mvIdToGroundtruthTimeStamp[currentID];\n\n VectorXd pose;\n if(getPoseFromTimeStamp(gtTimeStamp, pose))\n return true;\n else\n {\n return false; \n }\n \n }\n\n bool Dataset::SetOdometry(const string& dir_odom, bool calibrate){\n\n std::vector> strMat = readStringFromFile(dir_odom.c_str(), 0);\n\n if(strMat.size() == 0) {\n std::cerr << \" Odometry dir error! Keep gt. \" << std::endl;\n return false;\n }\n\n mmOdomRGBStampToPose.clear();\n int totalPose = strMat.size();\n\n for(int i=0;i strVec = strMat[i];\n string timestamp = strVec[0];\n\n VectorXd pose; pose.resize(7);\n for(int p=1;p<8;p++)\n pose(p-1) = stod(strVec[p]);\n \n mmOdomRGBStampToPose.insert(make_pair(timestamp, pose));\n }\n std::cout << \"Setting odometry succeeds.\";\n\n\n if( calibrate )\n {\n std::cout << \"Get calibrate transform... \" << std::endl;\n\n // find the corresponding groundtruth of the first timestamp of the odometry\n bool findCalibTrans = false;\n int transId = 0;\n for(auto timestampOdomPair: mmOdomRGBStampToPose)\n {\n string timestamp_gt = mvIdToGroundtruthTimeStamp[transId]; // assume that all the rgb images have corresponding odometry values\n \n assert( mvIdToRGBTimeStamp[transId] == timestampOdomPair.first && \"Odom should start from the first rgb frame.\" );\n\n VectorXd gtPose;\n if( getPoseFromTimeStamp(timestamp_gt, gtPose) )\n {\n g2o::SE3Quat pose_wc; pose_wc.fromVector(gtPose);\n g2o::SE3Quat pose_oc; pose_oc.fromVector(timestampOdomPair.second);\n mTransGtCalibrate = new g2o::SE3Quat;\n *mTransGtCalibrate = pose_wc * pose_oc.inverse();\n\n findCalibTrans = true;\n\n break;\n }\n transId ++ ;\n }\n\n if( !findCalibTrans)\n {\n std::cerr << \"Can't find calibrate transformation... Close calibraton!\"<< std::endl;\n calibrate = false;\n }\n else\n {\n std::cout << \"Find calibration trans ID: \" << transId << std::endl;\n\n \n for(auto timestampOdomPair: mmOdomRGBStampToPose)\n {\n // calibrate all\n VectorXd pose = timestampOdomPair.second;\n VectorXd pose_processed; \n if(calibrate)\n pose_processed = calibratePose(pose);\n else\n pose_processed = pose; \n\n mmOdomRGBStampToPose[timestampOdomPair.first] = pose_processed;\n \n }\n \n }\n \n }\n\n mbOdomSet = true;\n }\n\n VectorXd Dataset::calibratePose(VectorXd& pose)\n {\n g2o::SE3Quat pose_c; pose_c.fromVector(pose.tail(7));\n g2o::SE3Quat pose_w = (*mTransGtCalibrate) * pose_c;\n\n return pose_w.toVector();\n }\n\n void Dataset::SetCurrentID(int id)\n {\n if ( id >= miTotalNum ){\n std::cout << \"Fail. id is larger than totalNum : \" << miTotalNum << std::endl;\n return;\n }\n\n miCurrentID = id;\n\n return;\n \n }\n\n double Dataset::GetCurrentTimestamp()\n {\n return GetTimestamp(miCurrentID-1);\n }\n\n double Dataset::GetTimestamp(int id)\n {\n return stod(mvIdToRGBTimeStamp[id]);\n }\n}" }, { "path": "src/tum_rgbd/io.h", "content": "// Basic input and output of the TUM-RGBD dataset.\n\n#include \n#include \n#include \n\n#include \n#include \n\n#include \n\n#include \n\nusing namespace std;\nusing namespace cv;\nusing namespace Eigen;\n\nnamespace TUMRGBD\n{\n\n/*\n* All the timestamp is based on RGB images.\n*/\nclass Dataset\n{\n public:\n // Load the next RGB-D frame containing a RGB image, a depth image and a camera pose.\n // pose: x y z qx qy qz qw\n bool readFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose);\n void loadDataset(string &path);\n std::vector generateValidVector();\n\n // load object detections\n bool loadDetectionDir(string &path);\n Eigen::MatrixXd getDetectionMat();\n\n bool empty();\n\n int getCurrentID();\n int getTotalNum();\n\n // load a specified frame using its ID\n bool findFrameUsingID(int id, cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose);\n \n // load an odometry data generated from wheels or visual odometry algorithm like ORB-SLAM.\n // the system will automatically calibrate its coordinate by aligning the pose of the first frame to the corresponding ground truth.\n bool SetOdometry(const string& dir_odom, bool calibrate = true);\n\n // jump to a specified frame using its ID\n void SetCurrentID(int id);\n\n // Get the timestamp of the current frame\n double GetCurrentTimestamp();\n double GetTimestamp(int id);\n\n bool getPoseFromTimeStamp(string ×tamp, VectorXd &pose);\n\n private:\n void associateRGBWithGroundtruth();\n\n void loadGroundTruthToMap(string &path);\n\n // load the associations from the timestamps of rgb images to the timestamps of depth images\n void LoadAssociationRGBToDepth(string &path);\n void LoadAssociationRGBToGroundtruth(string &path);\n\n void generateIndexIdToRGBTimeStamp();\n\n bool judgeValid(int id); // the frame with the id is valid when it contains valid depth and rgb images ...\n\n bool getPoseFromRGBTimeStamp(string ×tamp, VectorXd &pose);\n VectorXd calibratePose(VectorXd& pose); // calibrate the odom data by aligning to the first frame of the groundtruth \n private:\n string msDatasetDir; // the root directory of the dataset\n\n string msRGBDir; \n string msDepthDir;\n string msGroundtruthPath;\n\n string msAssociatePath;\n string msAssociateGroundtruthPath;\n\n string msDetectionDir;\n\n vector mvRGBFileNames; // store the full paths of all the rgb images \n\n vector mvPoses;\n\n int miCurrentID;\n int miTotalNum;\n\n vector mvIdToGroundtruthTimeStamp; \n vector mvIdToDepthTimeStamp; \n vector mvIdToDepthImagePath; \n vector mvIdToRGBTimeStamp;\n\n map mmTimeStampToPose;\n map mmOdomRGBStampToPose;\n\n bool mbDetectionLoaded;\n\n bool mbOdomSet;\n string msOdomDir;\n g2o::SE3Quat* mTransGtCalibrate;\n};\n\n}" }, { "path": "src/utils/dataprocess_utils.cpp", "content": "#include \"utils/dataprocess_utils.h\"\n\n#include \n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\n#include // for setprecision\n\n#include \"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h\"\n\nusing namespace std;\nusing namespace Eigen;\ntypedef Matrix Vector7d;\ntypedef Matrix Vector6d;\n\nbool compare_func_stringasdouble(string &s1, string &s2)\n{\n string bareS1 = splitFileNameFromFullDir(s1, true);\n string bareS2 = splitFileNameFromFullDir(s2, true);\n return stod(bareS1) < stod(bareS2);\n}\n\nvoid GetFileNamesUnderDir(string path,vector& filenames)\n{\n filenames.clear();\n DIR *pDir;\n struct dirent* ptr;\n if(!(pDir = opendir(path.c_str()))){\n cout<<\"Folder doesn't Exist!\"<d_name, \".\") != 0 && strcmp(ptr->d_name, \"..\") != 0){\n filenames.push_back(path + \"/\" + ptr->d_name);\n }\n }\n closedir(pDir);\n}\n\nstring splitFileNameFromFullDir(string &s, bool bare)\n{\n\n int pos = s.find_last_of('/');\n string name(s.substr(pos+1));\n\n if( bare )\n {\n int posBare = name.find_last_of('.');\n string bare_name(name.substr(0, posBare));\n return bare_name;\n }\n\n return name;\n\n}\n\nvoid sortFileNames(vector& filenames, vector& filenamesSorted)\n{\n filenamesSorted = filenames;\n sort(filenamesSorted.begin(), filenamesSorted.end(), compare_func_stringasdouble);\n}\n\nEigen::MatrixXd readDataFromFile(const char* fileName, bool dropFirstline){\n ifstream fin(fileName);\n string line;\n\n if(dropFirstline)\n getline(fin, line); // drop this line\n\n MatrixXd mat;\n int line_num = 0;\n while( getline(fin, line) )\n {\n vector s;\n boost::split( s, line, boost::is_any_of( \" \\t,\" ), boost::token_compress_on );\n\n VectorXd lineVector(s.size());\n for (int i=0;i> readStringFromFile(const char* fileName, int dropLines){\n ifstream fin(fileName);\n string line;\n\n for(int i=0; i> strMat;\n while( getline(fin, line) )\n {\n vector s;\n boost::split( s, line, boost::is_any_of( \" \\t,\" ), boost::token_compress_on );\n\n strMat.push_back(s);\n }\n fin.close();\n\n return strMat;\n}\n\n// return True if the measure lies on the image border or does not meet the size requirement\nbool calibrateMeasurement(Vector4d &measure , int rows, int cols, int config_boarder, int config_size){\n\n int x_length = measure[2] - measure[0];\n int y_length = measure[3] - measure[1];\n if( x_length < config_size || y_length < config_size ){\n std::cout << \" [small detection \" << config_size << \"] invalid. \" << std::endl;\n return true;\n }\n\n Vector4d measure_calibrated(-1,-1,-1,-1);\n Vector4d measure_uncalibrated(-1,-1,-1,-1);\n\n int correct_num = 0;\n if( measure[0]>config_boarder && measure[0]config_boarder && measure[2]config_boarder && measure[1]config_boarder && measure[3]\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\nusing namespace Eigen;\n\nMatrix4d getTransformFromVector(VectorXd& pose)\n{\n if( pose.rows() == 7)\n {\n // x y z qx qy qz qw\n Matrix4d homogeneous_matrix;\n Quaterniond _r(pose(6),pose(3),pose(4),pose(5)); // w x y z\n homogeneous_matrix.setIdentity();\n homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();\n homogeneous_matrix.col(3).head(3) = pose.head(3);\n\n return homogeneous_matrix;\n }\n}\n\nvoid addVecToMatirx(Eigen::MatrixXd &mat, Eigen::VectorXd &vec)\n{\n assert( mat.cols() == vec.rows() && \"the size of vec and mat must match.\" );\n mat.conservativeResize(mat.rows()+1, mat.cols());\n mat.row(mat.rows()-1) = vec;\n \n}\n\ntemplate \nEigen::Quaternion zyx_euler_to_quat(const T &roll, const T &pitch, const T &yaw)\n{\n T sy = sin(yaw*0.5);\n T cy = cos(yaw*0.5);\n T sp = sin(pitch*0.5);\n T cp = cos(pitch*0.5);\n T sr = sin(roll*0.5);\n T cr = cos(roll*0.5);\n T w = cr*cp*cy + sr*sp*sy;\n T x = sr*cp*cy - cr*sp*sy;\n T y = cr*sp*cy + sr*cp*sy;\n T z = cr*cp*sy - sr*sp*cy;\n return Eigen::Quaternion(w,x,y,z);\n}\ntemplate Eigen::Quaterniond zyx_euler_to_quat(const double&, const double&, const double&);\ntemplate Eigen::Quaternionf zyx_euler_to_quat(const float&, const float&, const float&);\n\n\ntemplate \nvoid quat_to_euler_zyx(const Eigen::Quaternion& q, T& roll, T& pitch, T& yaw)\n{\n T qw = q.w();\n T qx = q.x();\n T qy = q.y();\n T qz = q.z();\n\n roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy));\n pitch = asin(2*(qw*qy-qz*qx));\n yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz));\n}\ntemplate void quat_to_euler_zyx(const Eigen::Quaterniond&, double&, double&, double&);\ntemplate void quat_to_euler_zyx(const Eigen::Quaternionf&, float&, float&, float&);\n\n\ntemplate \nvoid rot_to_euler_zyx(const Eigen::Matrix& R, T& roll, T& pitch, T& yaw)\n{\n pitch = asin(-R(2,0));\n\n if (fabs(pitch - M_PI/2.0) < 1.0e-3)\n {\n roll = 0.0;\n yaw = atan2(R(1,2) - R(0,1), R(0,2) + R(1,1)) + roll;\n }\n else if (fabs(pitch + M_PI/2.0) < 1.0e-3)\n {\n roll = 0.0;\n yaw = atan2(R(1,2) - R(0,1), R(0,2) + R(1,1)) - roll;\n }\n else\n {\n roll = atan2(R(2,1), R(2,2));\n yaw = atan2(R(1,0), R(0,0));\n }\n}\ntemplate void rot_to_euler_zyx(const Matrix3d&, double&, double&, double&);\ntemplate void rot_to_euler_zyx(const Matrix3f&, float&, float&, float&);\n\n\n\ntemplate \nEigen::Matrix euler_zyx_to_rot(const T& roll,const T& pitch,const T& yaw)\n{\n T cp = cos(pitch);\n T sp = sin(pitch);\n T sr = sin(roll);\n T cr = cos(roll);\n T sy = sin(yaw);\n T cy = cos(yaw);\n\n Eigen::Matrix R;\n R<< cp*cy, (sr*sp*cy)-(cr*sy), (cr*sp*cy)+(sr*sy),\n cp*sy, (sr*sp*sy)+(cr*cy), (cr*sp*sy)-(sr*cy),\n -sp, sr*cp, cr * cp;\n return R;\n}\ntemplate Matrix3d euler_zyx_to_rot(const double&, const double&, const double&);\ntemplate Matrix3f euler_zyx_to_rot(const float&, const float&, const float&);\n\n\ntemplate \nEigen::Matrix real_to_homo_coord(const Eigen::Matrix& pts_in)\n{\n Eigen::Matrix pts_homo_out;\n int raw_rows=pts_in.rows(); int raw_cols=pts_in.cols();\n\n pts_homo_out.resize(raw_rows+1,raw_cols);\n pts_homo_out<::Ones(raw_cols);\n return pts_homo_out;\n}\ntemplate MatrixXd real_to_homo_coord(const MatrixXd&);template MatrixXf real_to_homo_coord(const MatrixXf&);\n\n\ntemplate \nvoid real_to_homo_coord(const Eigen::Matrix& pts_in, Eigen::Matrix& pts_homo_out)\n{\n int raw_rows=pts_in.rows(); int raw_cols=pts_in.cols();\n\n pts_homo_out.resize(raw_rows+1,raw_cols);\n pts_homo_out<::Ones(raw_cols);\n}\ntemplate void real_to_homo_coord(const MatrixXd&,MatrixXd&);template void real_to_homo_coord(const MatrixXf&,MatrixXf&);\n\n\ntemplate // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function.\nEigen::Matrix real_to_homo_coord_vec(const Eigen::Matrix& pts_in)\n{\n Eigen::Matrix pts_homo_out;\n int raw_rows=pts_in.rows();;\n\n pts_homo_out.resize(raw_rows+1);\n pts_homo_out<(const VectorXd&); template VectorXf real_to_homo_coord_vec(const VectorXf&);\n\n\ntemplate \nEigen::Matrix homo_to_real_coord(const Eigen::Matrix& pts_homo_in)\n{\n Eigen::Matrix pts_out(pts_homo_in.rows()-1,pts_homo_in.cols());\n for (int i=0;i(const MatrixXd&);template MatrixXf homo_to_real_coord(const MatrixXf&);\n\n\ntemplate \nvoid homo_to_real_coord(const Eigen::Matrix& pts_homo_in, Eigen::Matrix& pts_out)\n{\n pts_out.resize(pts_homo_in.rows()-1,pts_homo_in.cols());\n for (int i=0;i(const MatrixXd&,MatrixXd&);template void homo_to_real_coord(const MatrixXf&,MatrixXf&);\n\n\ntemplate // though vector can be casted into matrix, to make output clear to be vector, it is better to define a new function.\nEigen::Matrix homo_to_real_coord_vec(const Eigen::Matrix& pts_homo_in)\n{\n Eigen::Matrix pt_out;\n if (pts_homo_in.rows()==4)\n pt_out=pts_homo_in.head(3)/pts_homo_in(3);\n else if (pts_homo_in.rows()==3)\n pt_out=pts_homo_in.head(2)/pts_homo_in(2);\n\n return pt_out;\n}\ntemplate VectorXd homo_to_real_coord_vec(const VectorXd&); template VectorXf homo_to_real_coord_vec(const VectorXf&);\n\n\nvoid fast_RemoveRow(MatrixXd& matrix,int rowToRemove, int& total_line_number)\n{\n matrix.row(rowToRemove) = matrix.row(total_line_number-1);\n total_line_number--;\n}\n\nvoid vert_stack_m(const MatrixXd &a_in, const MatrixXd &b_in, MatrixXd &combined_out)\n{\n assert (a_in.cols() == b_in.cols());\n combined_out.resize(a_in.rows()+b_in.rows(),a_in.cols());\n combined_out<\nbool read_all_number_txt(const std::string txt_file_name, Eigen::Matrix& read_number_mat)\n{\n if (!std::ifstream(txt_file_name))\n {\n std::cout<<\"ERROR!!! Cannot read txt file \"<> t)\n {\n read_number_mat(row_counter,colu)=t;\n colu++;\n }\n row_counter++;\n if (row_counter>=read_number_mat.rows()) // if matrix row is not enough, make more space.\n read_number_mat.conservativeResize(read_number_mat.rows()*2,read_number_mat.cols());\n }\n }\n filetxt.close();\n\n read_number_mat.conservativeResize(row_counter,read_number_mat.cols()); // cut into actual rows\n\n return true;\n}\ntemplate bool read_all_number_txt(const std::string, MatrixXd&); template bool read_all_number_txt(const std::string, MatrixXi&);\n\n\nbool read_obj_detection_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& all_strings)\n{\n if (!std::ifstream(txt_file_name))\n {\n std::cout<<\"ERROR!!! Cannot read txt file \"<>classname;\n all_strings.push_back(classname);\n\n int colu=0;\n while (ss >> t)\n {\n read_number_mat(row_counter,colu)=t;\n colu++;\n }\n row_counter++;\n if (row_counter>=read_number_mat.rows()) // if matrix row is not enough, make more space.\n read_number_mat.conservativeResize(read_number_mat.rows()*2,read_number_mat.cols());\n }\n }\n filetxt.close();\n read_number_mat.conservativeResize(row_counter,read_number_mat.cols()); // cut into actual rows\n}\n\n\nbool read_obj_detection2_txt(const std::string txt_file_name, Eigen::MatrixXd& read_number_mat, std::vector& all_strings)\n{\n if (!std::ifstream(txt_file_name))\n {\n std::cout<<\"ERROR!!! Cannot read txt file \"<> t)\n {\n read_number_mat(row_counter,colu)=t;\n colu++;\n if (colu>read_number_mat.cols()-1)\n break;\n }\n\n std::string classname; ss>>classname;\n all_strings.push_back(classname);\n\n row_counter++;\n if (row_counter>=read_number_mat.rows()) // if matrix row is not enough, make more space.\n read_number_mat.conservativeResize(read_number_mat.rows()*2,read_number_mat.cols());\n }\n }\n filetxt.close();\n read_number_mat.conservativeResize(row_counter,read_number_mat.cols()); // cut into actual rows\n\n}\n\nvoid sort_indexes(const Eigen::VectorXd &vec, std::vector& idx, int top_k )\n{\n std::partial_sort(idx.begin(),idx.begin()+top_k, idx.end(), [&vec](int i1, int i2){return vec(i1) < vec(i2);} );\n}\n\nvoid sort_indexes(const Eigen::VectorXd &vec, std::vector& idx)\n{\n sort(idx.begin(), idx.end(), [&vec](int i1, int i2){return vec(i1) < vec(i2);} );\n}\n\n\n\ntemplate \nT normalize_to_pi(T angle)\n{\n if (angle > M_PI/2.0)\n return angle-M_PI; // # change to -90 ~90\n else if (angle<-M_PI/2.0)\n return angle+M_PI;\n else\n return angle;\n}\ntemplate double normalize_to_pi(double);\n\n\n\ntemplate \nvoid print_vector(const std::vector& vec)\n{\n for (size_t i=0;i&);template void print_vector(const std::vector&);template void print_vector(const std::vector&);\n\n\ntemplate \nvoid linespace(T starting, T ending, T step, std::vector& res) {\n res.reserve((ending-starting)/step+2);\n while(starting <= ending) {\n res.push_back(starting);\n starting += step; // TODO could recode to better handle rounding errors\n if (res.size()>1000)\n {\n std::cout<<\"Linespace too large size!!!!\"<&);\ntemplate void linespace(double, double, double, std::vector&);\n" } ]