├── karto_slam.png ├── slam_karto ├── maps │ └── willow-full-0.05.pgm ├── config │ ├── recovery_behaviors.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ ├── base_local_planner_params.yaml │ ├── explore_costmap_params.yaml │ ├── costmap_common_params.yaml │ ├── move_base.xml │ └── mapper_params.yaml ├── launch │ ├── build_map_w_params.launch │ ├── karto_slam.launch │ ├── karto_stage.launch │ └── karto_slam.rviz ├── CMakeLists.txt ├── package.xml ├── worlds │ └── willow-pr2-5cm.world ├── CHANGELOG.rst └── src │ ├── spa_solver.h │ └── spa_solver.cpp ├── sparse_bundle_adjustment ├── README.txt ├── include │ └── sparse_bundle_adjustment │ │ ├── visualization.h │ │ ├── sba_setup.h │ │ ├── read_spa.h │ │ ├── sba_file_io.h │ │ ├── node.h │ │ ├── proj.h │ │ ├── csparse.h │ │ ├── spa2d.h │ │ ├── bpcg │ │ └── bpcg.h │ │ └── sba.h ├── package.xml ├── CMakeLists.txt ├── CHANGELOG.rst └── src │ ├── cholmod_timing.cpp │ ├── visualization.cpp │ ├── node.cpp │ ├── nodes │ └── sba_node.cpp │ └── proj.cpp ├── open_karto ├── Authors ├── samples │ ├── README.txt │ ├── SpaSolver.h │ ├── CMakeLists.txt │ ├── SpaSolver.cpp │ ├── tutorial1.cpp │ └── tutorial2.cpp ├── README.md ├── package.xml ├── CMakeLists.txt ├── include │ └── open_karto │ │ ├── Types.h │ │ ├── Macros.h │ │ └── Math.h ├── CHANGELOG.rst ├── LICENSE └── src │ └── Karto.cpp └── README.md /karto_slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lausen001/Karto-Note/HEAD/karto_slam.png -------------------------------------------------------------------------------- /slam_karto/maps/willow-full-0.05.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lausen001/Karto-Note/HEAD/slam_karto/maps/willow-full-0.05.pgm -------------------------------------------------------------------------------- /sparse_bundle_adjustment/README.txt: -------------------------------------------------------------------------------- 1 | Sparse Bundle Adjustment Library 2 | ================================ 3 | 4 | Originally developed at Willow Garage as part of the vslam stack. 5 | -------------------------------------------------------------------------------- /open_karto/Authors: -------------------------------------------------------------------------------- 1 | Karto Open Source Library 1.0 2 | 3 | Contributors: 4 | ------------------------------- 5 | Michael A. Eriksen (eriksen@ai.sri.com) 6 | Benson Limketkai (bensonl@ai.sri.com) 7 | -------------------------------------------------------------------------------- /slam_karto/config/recovery_behaviors.yaml: -------------------------------------------------------------------------------- 1 | recovery_behaviors: [{name: conservative_clear, type: ClearCostmapRecovery}, {name: aggressive_clear, type: ClearCostmapRecovery}] 2 | 3 | conservative_clear: 4 | reset_distance: 3.0 5 | 6 | aggressive_clear: 7 | reset_distance: 1.84 8 | 9 | -------------------------------------------------------------------------------- /slam_karto/launch/build_map_w_params.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /open_karto/samples/README.txt: -------------------------------------------------------------------------------- 1 | The CMake file does not work out of the box, because you will need a few 2 | external libraries (all available on ROS) if you want to use the SPA solver for 3 | closing loops: 4 | 5 | CSparse 6 | eigen 7 | sba 8 | 9 | 10 | Alternatively, you can just not compile Tutorial2. 11 | -------------------------------------------------------------------------------- /slam_karto/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | NavfnROS: 2 | allow_unknown: false 3 | #Independent settings for the planner's costmap 4 | global_costmap: 5 | global_frame: /map 6 | robot_base_frame: base_link 7 | update_frequency: 5.0 8 | publish_frequency: 0.0 9 | static_map: true 10 | rolling_window: false 11 | -------------------------------------------------------------------------------- /slam_karto/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | publish_voxel_map: true 3 | global_frame: /map 4 | robot_base_frame: base_link 5 | update_frequency: 5.0 6 | publish_frequency: 2.0 7 | static_map: false 8 | rolling_window: true 9 | width: 6.0 10 | height: 6.0 11 | resolution: 0.025 12 | origin_x: 0.0 13 | origin_y: 0.0 14 | -------------------------------------------------------------------------------- /open_karto/README.md: -------------------------------------------------------------------------------- 1 | # Open Karto 2 | 3 | Catkinized ROS Package of the OpenKarto Library (LGPL3) 4 | 5 | # Status 6 | 7 | * Devel Job Status: [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__open_karto__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__open_karto__ubuntu_bionic_amd64/) 8 | * AMD64 Debian Job Status: [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__open_karto__ubuntu_bionic_amd64__binary)](http://build.ros.org/job/Mbin_uB64__open_karto__ubuntu_bionic_amd64__binary/) 9 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/visualization.h: -------------------------------------------------------------------------------- 1 | #ifndef VSLAM_SYSTEM_VISUALIZATION_H 2 | #define VSLAM_SYSTEM_VISUALIZATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace sba { 9 | 10 | // draw the graph on rviz 11 | void drawGraph(const SysSBA &sba, const ros::Publisher &camera_pub, 12 | const ros::Publisher &point_pub, int decimation = 1, int bicolor = 0); 13 | 14 | } // namespace sba 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /slam_karto/launch/karto_slam.launch: -------------------------------------------------------------------------------- 1 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /slam_karto/launch/karto_stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /open_karto/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | open_karto 4 | 1.2.3 5 | Catkinized ROS packaging of the OpenKarto library 6 | 7 | Michael Ferguson 8 | Russell Toris 9 | Luc Bettaieb 10 | 11 | LGPLv3 12 | 13 | catkin 14 | 15 | boost 16 | 17 | boost 18 | 19 | 20 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/package.xml: -------------------------------------------------------------------------------- 1 | 2 | sparse_bundle_adjustment 3 | ROS wrapper for the sparse bundle adjustment (sba) library (needed for slam_karto) 4 | 0.4.4 5 | BSD 6 | Michael Ferguson 7 | Luc Bettaieb 8 | 9 | catkin 10 | 11 | roscpp 12 | eigen 13 | liblapack-dev 14 | libblas-dev 15 | suitesparse 16 | 17 | 18 | -------------------------------------------------------------------------------- /slam_karto/config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | #Independent settings for the local costmap 3 | transform_tolerance: 0.3 4 | sim_time: 1.7 5 | sim_granularity: 0.025 6 | dwa: true 7 | vx_samples: 3 8 | vtheta_samples: 20 9 | max_vel_x: 0.65 10 | min_vel_x: 0.1 11 | max_rotational_vel: 1.0 12 | min_in_place_rotational_vel: 0.4 13 | xy_goal_tolerance: 0.4 14 | yaw_goal_tolerance: 0.05 15 | goal_distance_bias: 0.8 16 | path_distance_bias: 0.6 17 | occdist_scale: 0.01 18 | heading_lookahead: 0.325 19 | oscillation_reset_dist: 0.05 20 | holonomic_robot: true 21 | acc_lim_th: 3.2 22 | acc_lim_x: 2.5 23 | acc_lim_y: 2.5 24 | -------------------------------------------------------------------------------- /slam_karto/config/explore_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | loop_closure_planner: 2 | allow_unknown: false 3 | explore_planner: 4 | allow_unknown: false 5 | #Independent settings for the planner's costmap 6 | global_costmap: 7 | global_frame: /map 8 | robot_base_frame: base_link 9 | update_frequency: 5.0 10 | publish_frequency: 0.0 11 | static_map: true 12 | rolling_window: false 13 | # global_frame: /map 14 | # robot_base_frame: base_link 15 | # update_frequency: 1.0 16 | # publish_frequency: 0.0 17 | # raytrace_range: 5.0 18 | # obstacle_range: 5.0 19 | # static_map: false 20 | # rolling_window: false 21 | # width: 60.0 22 | # height: 60.0 23 | # resolution: 0.2 24 | # origin_x: -30.0 25 | # origin_y: -30.0 26 | # track_unknown_space: true 27 | -------------------------------------------------------------------------------- /slam_karto/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | #START VOXEL STUFF 2 | map_type: voxel 3 | origin_z: 0.0 4 | z_resolution: 0.2 5 | z_voxels: 10 6 | unknown_threshold: 9 7 | mark_threshold: 0 8 | #END VOXEL STUFF 9 | transform_tolerance: 0.3 10 | obstacle_range: 2.5 11 | max_obstacle_height: 2.0 12 | raytrace_range: 3.0 13 | footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 14 | #robot_radius: 0.46 15 | footprint_padding: 0.01 16 | inflation_radius: 0.55 17 | cost_scaling_factor: 10.0 18 | lethal_cost_threshold: 100 19 | unknown_cost_value: 255 20 | observation_sources: base_scan 21 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, 22 | observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 23 | -------------------------------------------------------------------------------- /slam_karto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(slam_karto) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 11) 6 | endif() 7 | 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | message_filters 11 | nav_msgs 12 | open_karto 13 | rosconsole 14 | roscpp 15 | sensor_msgs 16 | sparse_bundle_adjustment 17 | tf 18 | visualization_msgs 19 | ) 20 | find_package(Eigen3 REQUIRED) 21 | 22 | include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) 23 | add_definitions(${EIGEN3_DEFINITIONS}) 24 | 25 | catkin_package() 26 | 27 | add_executable(slam_karto src/slam_karto.cpp src/spa_solver.cpp) 28 | target_link_libraries(slam_karto ${catkin_LIBRARIES}) 29 | 30 | install(TARGETS slam_karto 31 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 32 | ) 33 | -------------------------------------------------------------------------------- /open_karto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(open_karto) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | find_package(Boost REQUIRED COMPONENTS thread) 7 | 8 | catkin_package( 9 | DEPENDS Boost 10 | INCLUDE_DIRS 11 | include 12 | LIBRARIES 13 | karto 14 | ) 15 | 16 | if(BUILD_SHARED_LIBS) 17 | add_definitions(-DKARTO_DYNAMIC) 18 | endif() 19 | 20 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 21 | add_library(karto SHARED src/Karto.cpp src/Mapper.cpp) 22 | target_link_libraries(karto ${Boost_LIBRARIES}) 23 | 24 | install(DIRECTORY include/ DESTINATION include) 25 | install(TARGETS karto 26 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 27 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 28 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 29 | ) 30 | -------------------------------------------------------------------------------- /slam_karto/config/move_base.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(sparse_bundle_adjustment) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 11) 6 | endif() 7 | 8 | find_package(catkin REQUIRED COMPONENTS roscpp) 9 | find_package(Eigen3 REQUIRED) 10 | find_package(suitesparse QUIET) 11 | if(NOT SuiteSparse_FOUND) 12 | set(SuiteSparse_LIBRARIES blas lapack cholmod cxsparse) 13 | endif() 14 | 15 | include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${SuiteSparse_INCLUDE_DIRS}) 16 | add_definitions(${EIGEN3_DEFINITIONS}) 17 | INCLUDE_DIRECTORIES(include) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS include 21 | LIBRARIES sba 22 | ) 23 | 24 | add_definitions(-DSBA_CHOLMOD) 25 | 26 | # SBA library 27 | add_library(sba src/sba.cpp src/spa.cpp src/spa2d.cpp src/csparse.cpp src/proj.cpp src/node.cpp src/sba_file_io.cpp) 28 | #rosbuild_add_compile_flags(sba ${SSE_FLAGS}) 29 | target_link_libraries(sba ${SuiteSparse_LIBRARIES}) 30 | 31 | install(DIRECTORY include/sparse_bundle_adjustment/ 32 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 33 | 34 | install(TARGETS sba 35 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 36 | -------------------------------------------------------------------------------- /slam_karto/package.xml: -------------------------------------------------------------------------------- 1 | 2 | slam_karto 3 | 4 | This package pulls in the Karto mapping library, and provides a ROS 5 | wrapper for using it. 6 | 7 | 0.8.1 8 | Michael Ferguson 9 | Luc Bettaieb 10 | Russell Toris 11 | Brian Gerkey 12 | LGPL 13 | 14 | catkin 15 | 16 | eigen 17 | message_filters 18 | nav_msgs 19 | open_karto 20 | rosconsole 21 | roscpp 22 | sparse_bundle_adjustment 23 | sensor_msgs 24 | tf 25 | visualization_msgs 26 | 27 | eigen 28 | message_filters 29 | nav_msgs 30 | open_karto 31 | rosconsole 32 | roscpp 33 | sparse_bundle_adjustment 34 | sensor_msgs 35 | tf 36 | visualization_msgs 37 | 38 | 39 | -------------------------------------------------------------------------------- /slam_karto/worlds/willow-pr2-5cm.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.5 0.5 0.75] 4 | gui_nose 0 5 | ) 6 | 7 | define topurg ranger 8 | ( 9 | sensor( 10 | range_max 30.0 11 | fov 270.25 12 | samples 1081 13 | ) 14 | # generic model properties 15 | color "black" 16 | size [ 0.05 0.05 0.1 ] 17 | ) 18 | 19 | define pr2 position 20 | ( 21 | size [0.65 0.65 0.25] 22 | origin [-0.05 0 0 0] 23 | gui_nose 1 24 | drive "omni" 25 | topurg(pose [ 0.275 0.000 0 0.000 ]) 26 | ) 27 | 28 | define floorplan model 29 | ( 30 | # sombre, sensible, artistic 31 | color "gray30" 32 | 33 | # most maps will need a bounding box 34 | boundary 1 35 | 36 | gui_nose 0 37 | gui_grid 0 38 | 39 | gui_outline 0 40 | gripper_return 0 41 | fiducial_return 0 42 | ranger_return 1 43 | ) 44 | 45 | # set the resolution of the underlying raytrace model in meters 46 | resolution 0.02 47 | 48 | interval_sim 100 # simulation timestep in milliseconds 49 | 50 | 51 | window 52 | ( 53 | size [ 745.000 448.000 ] 54 | 55 | rotate [ 0.000 -1.560 ] 56 | scale 18.806 57 | ) 58 | 59 | # load an environment bitmap 60 | floorplan 61 | ( 62 | name "willow" 63 | bitmap "../maps/willow-full-0.05.pgm" 64 | size [58.25 47.25 1.0] 65 | pose [ -23.625 29.125 0 90.000 ] 66 | ) 67 | 68 | # throw in a robot 69 | pr2( pose [ -28.610 13.562 0 99.786 ] name "pr2" color "blue") 70 | block( pose [ -25.062 12.909 0 180.000 ] color "red") 71 | block( pose [ -25.062 12.909 0 180.000 ] color "red") 72 | block( pose [ -25.062 12.909 0 180.000 ] color "red") 73 | -------------------------------------------------------------------------------- /slam_karto/config/mapper_params.yaml: -------------------------------------------------------------------------------- 1 | # General Parameters 2 | use_scan_matching: true 3 | use_scan_barycenter: true 4 | minimum_time_interval: 3600 5 | minimum_travel_distance: 0.2 6 | minimum_travel_heading: 0.174 #in radians 7 | scan_buffer_size: 70 8 | scan_buffer_maximum_scan_distance: 20.0 9 | link_match_minimum_response_fine: 0.8 10 | link_scan_maximum_distance: 10.0 11 | loop_search_maximum_distance: 4.0 12 | do_loop_closing: true 13 | loop_match_minimum_chain_size: 10 14 | loop_match_maximum_variance_coarse: 0.4 # gets squared later 15 | loop_match_minimum_response_coarse: 0.8 16 | loop_match_minimum_response_fine: 0.8 17 | 18 | # Correlation Parameters - Correlation Parameters 19 | correlation_search_space_dimension: 0.3 20 | correlation_search_space_resolution: 0.01 21 | correlation_search_space_smear_deviation: 0.03 22 | 23 | # Correlation Parameters - Loop Closure Parameters 24 | loop_search_space_dimension: 8.0 25 | loop_search_space_resolution: 0.05 26 | loop_search_space_smear_deviation: 0.03 27 | 28 | # Scan Matcher Parameters 29 | distance_variance_penalty: 0.3 # gets squared later 30 | angle_variance_penalty: 0.349 # in degrees (gets converted to radians then squared) 31 | fine_search_angle_offset: 0.00349 # in degrees (gets converted to radians) 32 | coarse_search_angle_offset: 0.349 # in degrees (gets converted to radians) 33 | coarse_angle_resolution: 0.0349 # in degrees (gets converted to radians) 34 | minimum_angle_penalty: 0.9 35 | minimum_distance_penalty: 0.5 36 | use_response_expansion: false 37 | -------------------------------------------------------------------------------- /open_karto/samples/SpaSolver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef __SPASOLVER__ 19 | #define __SPASOLVER__ 20 | 21 | #include 22 | 23 | #ifndef EIGEN_USE_NEW_STDVECTOR 24 | #define EIGEN_USE_NEW_STDVECTOR 25 | #endif // EIGEN_USE_NEW_STDVECTOR 26 | 27 | #define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10) 28 | #include 29 | 30 | #include "sba/spa2d.h" 31 | 32 | typedef std::vector CovarianceVector; 33 | 34 | class SpaSolver : public karto::ScanSolver 35 | { 36 | public: 37 | SpaSolver(); 38 | virtual ~SpaSolver(); 39 | 40 | public: 41 | virtual void Clear(); 42 | virtual void Compute(); 43 | virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; 44 | 45 | virtual void AddNode(karto::Vertex* pVertex); 46 | virtual void AddConstraint(karto::Edge* pEdge); 47 | 48 | private: 49 | karto::ScanSolver::IdPoseVector corrections; 50 | 51 | sba::SysSPA2d m_Spa; 52 | }; 53 | 54 | #endif // __SPASOLVER__ 55 | 56 | -------------------------------------------------------------------------------- /slam_karto/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package slam_karto 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.8.1 (2018-09-11) 6 | ------------------ 7 | * set C++11 if std not specified 8 | This is mainly for building on Lunar 9 | * Contributors: Michael Ferguson 10 | 11 | 0.8.0 (2018-08-21) 12 | ------------------ 13 | * update maintainer email 14 | * Merge pull request `#19 `_ from ros-perception/maintainer-add 15 | Adding myself as a maintainer for slam_karto 16 | * Adding myself as a maintainer for slam_karto 17 | * Merge pull request `#15 `_ from jasonimercer/lock-fix 18 | Fixed locks so they stay in scope until the end of the method. 19 | * Fixed locks so they stay in scope until the end of the method. 20 | * Merge pull request `#14 `_ from xpharry/indigo-devel 21 | modify for stage simulation 22 | * modify for stage simulation 23 | * Merge pull request `#9 `_ from hvpandya/patch-1 24 | Update karto_slam.launch 25 | * Update karto_slam.launch 26 | package name must be slam_karto 27 | * Contributors: Harsh Pandya, Jason Mercer, Luc Bettaieb, Michael Ferguson, xpharry 28 | 29 | 0.7.3 (2016-02-04) 30 | ------------------ 31 | * fixed the upside-down detection 32 | * update maintainer email 33 | * Contributors: Michael Ferguson, mgerdzhev 34 | 35 | 0.7.2 (2015-07-18) 36 | ------------------ 37 | * Added in parameter server settings for Mapper within slam_karto 38 | * Contributors: Luc Bettaieb, Michael Ferguson 39 | 40 | 0.7.1 (2014-06-17) 41 | ------------------ 42 | * build updates for sba, fix install 43 | * Contributors: Michael Ferguson 44 | 45 | 0.7.0 (2014-06-15) 46 | ------------------ 47 | * First release in a very, very long time. 48 | * Catkinized, updated to work with catkinized open_karto and sba 49 | * Contributors: Jon Binney, Michael Ferguson 50 | -------------------------------------------------------------------------------- /open_karto/include/open_karto/Types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef OPEN_KARTO_TYPES_H 19 | #define OPEN_KARTO_TYPES_H 20 | 21 | #include 22 | 23 | /** 24 | * Karto type definition 25 | * Ensures platform independent types for windows, linux and mac 26 | */ 27 | #if defined(_MSC_VER) 28 | 29 | typedef signed __int8 kt_int8s; 30 | typedef unsigned __int8 kt_int8u; 31 | 32 | typedef signed __int16 kt_int16s; 33 | typedef unsigned __int16 kt_int16u; 34 | 35 | typedef signed __int32 kt_int32s; 36 | typedef unsigned __int32 kt_int32u; 37 | 38 | typedef signed __int64 kt_int64s; 39 | typedef unsigned __int64 kt_int64u; 40 | 41 | #else 42 | 43 | #include 44 | 45 | typedef int8_t kt_int8s; 46 | typedef uint8_t kt_int8u; 47 | 48 | typedef int16_t kt_int16s; 49 | typedef uint16_t kt_int16u; 50 | 51 | typedef int32_t kt_int32s; 52 | typedef uint32_t kt_int32u; 53 | 54 | #if defined(__LP64__) 55 | typedef signed long kt_int64s; 56 | typedef unsigned long kt_int64u; 57 | #else 58 | typedef signed long long kt_int64s; 59 | typedef unsigned long long kt_int64u; 60 | #endif 61 | 62 | #endif 63 | 64 | typedef bool kt_bool; 65 | typedef char kt_char; 66 | typedef float kt_float; 67 | typedef double kt_double; 68 | 69 | #endif // OPEN_KARTO_TYPES_H 70 | -------------------------------------------------------------------------------- /slam_karto/src/spa_solver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef KARTO_SPASOLVER_H 19 | #define KARTO_SPASOLVER_H 20 | 21 | #include 22 | 23 | #ifndef EIGEN_USE_NEW_STDVECTOR 24 | #define EIGEN_USE_NEW_STDVECTOR 25 | #endif // EIGEN_USE_NEW_STDVECTOR 26 | 27 | #define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10) 28 | #include 29 | 30 | #include 31 | 32 | typedef std::vector CovarianceVector; 33 | 34 | class SpaSolver : public karto::ScanSolver 35 | { 36 | public: 37 | SpaSolver(); 38 | virtual ~SpaSolver(); 39 | 40 | public: 41 | virtual void Clear(); 42 | virtual void Compute(); 43 | virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; 44 | 45 | virtual void AddNode(karto::Vertex* pVertex); 46 | virtual void AddConstraint(karto::Edge* pEdge); 47 | 48 | // Get the underlying graph from SBA 49 | // return the graph of constraints 50 | /// x,y -> x',y' 4 floats per connection 51 | void getGraph(std::vector &g) { m_Spa.getGraph(g); } 52 | 53 | private: 54 | karto::ScanSolver::IdPoseVector corrections; 55 | 56 | sba::SysSPA2d m_Spa; 57 | }; 58 | 59 | #endif // KARTO_SPASOLVER_H 60 | 61 | -------------------------------------------------------------------------------- /open_karto/samples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2010 SRI International 3 | # 4 | # This program is free software: you can redistribute it and/or modify 5 | # it under the terms of the GNU Lesser General Public License as published by 6 | # the Free Software Foundation, either version 3 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU Lesser General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU Lesser General Public License 15 | # along with this program. If not, see . 16 | # 17 | 18 | ADD_DEFINITIONS(-DUSE_SPA) 19 | 20 | if(WIN32) 21 | ADD_DEFINITIONS(-DEIGEN_DONT_ALIGN) 22 | endif() 23 | 24 | if(WIN32) 25 | # turn off various warnings 26 | foreach(warning 4251) 27 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd${warning}") 28 | endforeach(warning) 29 | endif() 30 | 31 | include_directories(../source) 32 | 33 | ############################################################################ 34 | # tutorial 1 35 | add_executable( 36 | Karto_Tutorial1 37 | tutorial1.cpp 38 | ) 39 | 40 | target_link_libraries(Karto_Tutorial1 OpenKarto) 41 | 42 | if (UNIX) 43 | target_link_libraries(Karto_Tutorial1 "pthread") 44 | endif() 45 | 46 | ############################################################################ 47 | # tutorial 2 48 | 49 | include_directories(${CSPARSE_DIR}/CSparse/Include) 50 | include_directories(${SBA_DIR}/sba/include) 51 | include_directories(${EIGEN_DIR}/eigen) 52 | 53 | add_executable( 54 | Karto_Tutorial2 55 | tutorial2.cpp 56 | SpaSolver.h 57 | SpaSolver.cpp 58 | ) 59 | 60 | target_link_libraries(Karto_Tutorial2 OpenKarto) 61 | target_link_libraries(Karto_Tutorial2 spa2d) 62 | target_link_libraries(Karto_Tutorial2 csparse) 63 | 64 | if(PNG_FOUND) 65 | target_link_libraries(Karto_Tutorial2 ${ZLIB_LIBRARIES}) 66 | target_link_libraries(Karto_Tutorial2 ${PNG_LIBRARIES}) 67 | endif() 68 | 69 | if (UNIX) 70 | target_link_libraries(Karto_Tutorial2 "pthread") 71 | endif() 72 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sparse_bundle_adjustment 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.4.4 (2020-03-28) 6 | ------------------ 7 | * fix unitialized variable causing crashes (`#10 `_) 8 | * Contributors: Michael Ferguson 9 | 10 | 0.4.3 (2020-01-31) 11 | ------------------ 12 | * remove bouncing email from maintainers 13 | * Merge pull request `#9 `_ from seanyen/modern_cpp 14 | [windows] use modern cpp & more portable fixes 15 | * modernize cpp code & CMake files. 16 | * Changing maintainer email 17 | * Contributors: Luc Bettaieb, Michael Ferguson, seanyen 18 | 19 | 0.4.2 (2018-08-23) 20 | ------------------ 21 | * rework how we set the C++ standard 22 | * Merge pull request `#6 `_ from moriarty/set-cpp-11 23 | Set C++ 11 24 | * [Maintainers] Add myself as a maintainer 25 | Mostly so that I can see build failures. 26 | * [CMake][C++11] compile with -std=c++11 27 | * Contributors: Alexander Moriarty, Michael Ferguson 28 | 29 | 0.4.1 (2018-08-21) 30 | ------------------ 31 | * Merge pull request `#4 `_ from moriarty/eigen-and-pkg-fmt-2 32 | Fixes Eigen3 warnings and bumps to package.xml format 2 33 | * Update email address so I see build failures 34 | * [REP-140] Package.xml format 2 35 | * fix deprecated eigen3 cmake warning 36 | * Contributors: Alexnader Moriarty, Michael Ferguson, Steffen Fuchs 37 | 38 | 0.4.0 (2018-08-18) 39 | ------------------ 40 | * Merge pull request `#3 `_ from moriarty/melodic-devel 41 | [melodic-devel][18.04] fix compile errors 42 | * [melodic-devel][18.04] fix compile errors 43 | fix compile errors for newer gcc 44 | * Merge pull request `#2 `_ from ros-perception/maintainer-add 45 | Adding myself as a maintainer for sparse_bundle_adjustment 46 | * Adding myself as a maintainer for sparse_bundle_adjustment 47 | * Contributors: Alexander Moriarty, Luc Bettaieb, Michael Ferguson 48 | 49 | 0.3.2 (2014-06-17) 50 | ------------------ 51 | * major build/install fixes for the farm 52 | * Contributors: Michael Ferguson 53 | 54 | 0.3.1 (2014-06-17) 55 | ------------------ 56 | * add depend on eigen 57 | * Contributors: Michael Ferguson 58 | 59 | 0.3.0 (2014-06-16) 60 | ------------------ 61 | * import cleaned up sba (this is the version from wg) 62 | * Contributors: Michael Ferguson 63 | -------------------------------------------------------------------------------- /open_karto/samples/SpaSolver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #include "SpaSolver.h" 19 | #include 20 | 21 | SpaSolver::SpaSolver() 22 | { 23 | 24 | } 25 | 26 | SpaSolver::~SpaSolver() 27 | { 28 | 29 | } 30 | 31 | void SpaSolver::Clear() 32 | { 33 | corrections.clear(); 34 | } 35 | 36 | const karto::ScanSolver::IdPoseVector& SpaSolver::GetCorrections() const 37 | { 38 | return corrections; 39 | } 40 | 41 | void SpaSolver::Compute() 42 | { 43 | corrections.clear(); 44 | 45 | typedef std::vector > NodeVector; 46 | 47 | printf("DO SPA BEGIN\n"); 48 | m_Spa.doSPA(40); 49 | printf("DO SPA END\n"); 50 | NodeVector nodes = m_Spa.getNodes(); 51 | forEach(NodeVector, &nodes) 52 | { 53 | karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot); 54 | corrections.push_back(std::make_pair(iter->nodeId, pose)); 55 | } 56 | } 57 | 58 | void SpaSolver::AddNode(karto::Vertex* pVertex) 59 | { 60 | karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose(); 61 | Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading()); 62 | m_Spa.addNode2d(vector, pVertex->GetObject()->GetUniqueId()); 63 | } 64 | 65 | void SpaSolver::AddConstraint(karto::Edge* pEdge) 66 | { 67 | karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject(); 68 | karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject(); 69 | karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); 70 | 71 | karto::Pose2 diff = pLinkInfo->GetPoseDifference(); 72 | Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading()); 73 | 74 | karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); 75 | Eigen::Matrix m; 76 | m(0,0) = precisionMatrix(0,0); 77 | m(0,1) = m(1,0) = precisionMatrix(0,1); 78 | m(0,2) = m(2,0) = precisionMatrix(0,2); 79 | m(1,1) = precisionMatrix(1,1); 80 | m(1,2) = m(2,1) = precisionMatrix(1,2); 81 | m(2,2) = precisionMatrix(2,2); 82 | 83 | m_Spa.addConstraint2d(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m); 84 | } 85 | -------------------------------------------------------------------------------- /slam_karto/src/spa_solver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #include "spa_solver.h" 19 | #include 20 | 21 | #include "ros/console.h" 22 | 23 | SpaSolver::SpaSolver() 24 | { 25 | 26 | } 27 | 28 | SpaSolver::~SpaSolver() 29 | { 30 | 31 | } 32 | 33 | void SpaSolver::Clear() 34 | { 35 | corrections.clear(); 36 | } 37 | 38 | const karto::ScanSolver::IdPoseVector& SpaSolver::GetCorrections() const 39 | { 40 | return corrections; 41 | } 42 | 43 | void SpaSolver::Compute() 44 | { 45 | corrections.clear(); 46 | 47 | typedef std::vector > NodeVector; 48 | 49 | ROS_INFO("Calling doSPA for loop closure"); 50 | m_Spa.doSPA(40); 51 | ROS_INFO("Finished doSPA for loop closure"); 52 | NodeVector nodes = m_Spa.getNodes(); 53 | forEach(NodeVector, &nodes) 54 | { 55 | karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot); 56 | corrections.push_back(std::make_pair(iter->nodeId, pose)); 57 | } 58 | } 59 | 60 | void SpaSolver::AddNode(karto::Vertex* pVertex) 61 | { 62 | karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose(); 63 | Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading()); 64 | m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId()); 65 | } 66 | 67 | void SpaSolver::AddConstraint(karto::Edge* pEdge) 68 | { 69 | karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject(); 70 | karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject(); 71 | karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); 72 | 73 | karto::Pose2 diff = pLinkInfo->GetPoseDifference(); 74 | Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading()); 75 | 76 | karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); 77 | Eigen::Matrix m; 78 | m(0,0) = precisionMatrix(0,0); 79 | m(0,1) = m(1,0) = precisionMatrix(0,1); 80 | m(0,2) = m(2,0) = precisionMatrix(0,2); 81 | m(1,1) = precisionMatrix(1,1); 82 | m(1,2) = m(2,1) = precisionMatrix(1,2); 83 | m(2,2) = precisionMatrix(2,2); 84 | 85 | m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m); 86 | } 87 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/src/cholmod_timing.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "cholmod.h" 36 | #include "sparse_bundle_adjustment/sba.h" 37 | #include 38 | #define CPUTIME ((double) (clock ( )) / CLOCKS_PER_SEC) 39 | 40 | int main (void) 41 | { 42 | cholmod_sparse *A ; 43 | cholmod_dense *x, *b, *r ; 44 | cholmod_factor *L ; 45 | double one [2] = {1,0}, m1 [2] = {-1,0} ; /* basic scalars */ 46 | cholmod_common c ; 47 | cholmod_start (&c) ; /* start CHOLMOD */ 48 | A = cholmod_read_sparse (stdin, &c) ; /* read in a matrix */ 49 | cholmod_print_sparse (A, "A", &c) ; /* print the matrix */ 50 | if (A == NULL || A->stype == 0) /* A must be symmetric */ 51 | { 52 | cholmod_free_sparse (&A, &c) ; 53 | cholmod_finish (&c) ; 54 | return (0) ; 55 | } 56 | b = cholmod_ones (A->nrow, 1, A->xtype, &c) ; /* b = ones(n,1) */ 57 | double t0 = CPUTIME; 58 | L = cholmod_analyze (A, &c) ; /* analyze */ 59 | cholmod_factorize (A, L, &c) ; /* factorize */ 60 | x = cholmod_solve (CHOLMOD_A, L, b, &c) ; /* solve Ax=b */ 61 | double t1 = CPUTIME; 62 | printf("Time: %12.4f \n", t1-t0); 63 | r = cholmod_copy_dense (b, &c) ; /* r = b */ 64 | cholmod_sdmult (A, 0, m1, one, x, r, &c) ; /* r = r-Ax */ 65 | printf ("norm(b-Ax) %8.1e\n", 66 | cholmod_norm_dense (r, 0, &c)) ; /* print norm(r) */ 67 | cholmod_free_factor (&L, &c) ; /* free matrices */ 68 | cholmod_free_sparse (&A, &c) ; 69 | cholmod_free_dense (&r, &c) ; 70 | cholmod_free_dense (&x, &c) ; 71 | cholmod_free_dense (&b, &c) ; 72 | cholmod_finish (&c) ; /* finish CHOLMOD */ 73 | return (0) ; 74 | } 75 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Karto-Note 2 | Karto Slam源码详细注释。 3 | 4 | Karto是一种2D激光SLAM解决方案，它是基于稀疏图优化的方法，带闭环检测。 5 | 6 | 本文对Karto的主流程代码进行分析，主要对局部map的维护、基于相关方法的scan-to-map匹配、闭环检测、BA图构建、计数法更新栅格地图等几个方面进行讲解。希望可以帮到有需要的同学，错误的地方请您批评指正。 7 | 8 | :) 如果对您有帮助，帮我点个star呦~ 9 | 10 | ## 目录（知乎） 11 | - [Karto源码解析(一)：工程运行](https://zhuanlan.zhihu.com/p/350852337) 12 | - [Karto源码解析(二)：代码解析](https://zhuanlan.zhihu.com/p/352388229) 13 | 14 | ## 一、主流程 15 | 16 | 先放一个自己画的Karto流程简图。 17 | 18 | ![Image](https://github.com/smilefacehh/Karto-Note/blob/main/karto_slam.png) 19 | 20 | 每当新来一帧数据，都会执行一次完整的主流程。 21 | 22 | 1. 首先判断当前帧与前一帧是否间隔足够大，不够大则直接丢弃，可以理解为只处理关键帧； 23 | 24 | 2. 接下来要对当前帧位姿进行校正，采用scan-to-map的配准方法。map是选择当前帧时空维度上相邻的帧组成的集合，我们称之为局部map。在当前帧位姿搜索空间中，对当前帧激光点进行变换，与局部map的激光点集合进行匹配，计算响应值，选择响应值最高的匹配位姿，作为当前帧位姿； 25 | 26 | 3. 前面两步相当于SLAM的前端，得到了当前帧的优化位姿。接着，构建BA优化图，添加当前帧为新的顶点，然后添加一些边。一共要添加四种边： 27 | 28 | - 与前一帧建立边； 29 | - 与局部map中最近的一帧建立边； 30 | - 与当前帧的广搜邻近帧的chain，建立边，后边再解释是啥； 31 | - 与闭环候选帧中最接近的一帧建立边； 32 | 33 | 4. 闭环检测。在历史帧中查找当前帧时间上相隔较远，空间上相隔较近的帧，作为候选闭环帧，与当前帧进行scan-to-map匹配。如果匹配较好，认为找到闭环了； 34 | 35 | 5. 如果找到闭环，就执行BA图优化，更新所有顶点位姿。 36 | 37 | ## 二、局部map 38 | 39 | 局部map是指与当前帧在时间、空间维度上邻近的帧集合。用于与当前帧进行scan-to-map匹配，优化当前帧位姿。局部map中的激光点需要过滤一下，不能直接全部拿来匹配。因为即便局部帧与当前帧很近，但仍有可能局部帧与当前帧处在一个物体的两面，比如穿过一堵墙，那么局部帧看到墙这一面的点，当前帧已经到了墙另一面，这些点是看不到的。这样的点需要剔除，如何辨识这些点，可以看看FindValidPoints方法。 40 | 41 | 相关函数包括：AddRunningScan、AddScans、AddScan、FindValidPoints。 42 | 43 | ## 三、基于相关方法的scan-to-map匹配 44 | 45 | scan-to-map匹配是激光雷达中一个基础的概念，scan是当前帧，map是当前帧时空维度上临近的帧组成的集合。map已经经过不断地优化，有一定精度了，而当前帧只有一个初始的估计位姿。至于怎么来的，可以是用前一帧位姿+运动计算，也可以是有外部传感器IMU、轮式里程计等得到的，总之它是不准确的。那么我们可以将当前帧的激光点，随意变换一下，再与map的激光点进行匹配。如果匹配的特别好，我们认为这个随意变换的pose就是我们需要的校正位姿。当然，随意变换是有个限制的，不能变换的特别特别大吧，所以定义了一个变化范围，称之为搜索空间。包括平移（x、y）、旋转（theta）维度上的搜索。 46 | 47 | 伪代码大致是这样的： 48 | 49 | ``` 50 | scan, map 51 | bestPose = (x,y,theta) 52 | bestScore = Score(scan, map) 53 | for dx in (-dx, dx): 54 | for dy in (-dy, dy): 55 | for dtheta in (-dtheta, dtheta): 56 | T = (x + dx, y + dy, theta + dtheta) 57 | scan = T * scan 58 | score = Score(scan, map) 59 | if score > bestScore: 60 | bestScore = score 61 | bestPose = T 62 | ``` 63 | 64 | 如何判断scan与map匹配的好不好呢？很简单，scan的激光点变换之后的位置，有map点接盘，不管是什么点，只要有个点就算匹配上了。统计一下匹配上的比例，用它来评判匹配好不好。为啥有效呢？可以想想。 65 | 66 | 另外要提一点，搜索空间中的角度问题。如果每平移一个（dx, dy）之后，再计算旋转dtheta角度对应的激光点位置，那计算DX*DY*DTheta次。能不能精简一下呢，可以的。我们知道旋转这个东西跟在哪没有关系，所以一开始就先计算好旋转一个dtheta角之后，激光点的偏移量，注意是偏移量。后面对于任何平移（dx, dy）之后的位置，加上这个偏移量就可以了。整个计算量就是DX*DY + DTheta次，少了很多。 67 | 68 | scan-to-map匹配在很多地方都会用到。当前帧与局部map匹配，当前帧与闭环候选帧集合匹配，当前帧与广搜临近帧的chain匹配，都会调用这个方法。 69 | 70 | 相关函数包括：MatchScan、CorrelateScan、ComputeOffsets、GetResponse、ComputePositionalCovariance、ComputeAngularCovariance。 71 | 72 | ## 四、闭环检测 73 | 74 | 闭环就是机器人走了一圈（也有可能是好多圈），我们要在历史帧中找当前帧位置的帧，构建闭环。闭环帧跟当前帧的位置要很接近，时间呢又要求隔得比较远，不然就是相邻帧了。找到这些候选帧，称之为闭环map，再与当前帧进行scan-to-map匹配，优化当前帧的位姿。 75 | 76 | 相关函数包括：TryCloseLoop、FindPossibleLoopClosure。 77 | 78 | ## 五、BA图构建 79 | 80 | 这里属于SLAM后端部分了，BA图的顶点为历史帧，边则有很多情况。每当新来一帧，需要从图中挑出来以下这些顶点，与之建立一条边。 81 | 82 | - 与前一帧建立边； 83 | - 与局部map中最接近的一帧建立边； 84 | - 与闭环map中最接近的一帧建立边，可以理解为闭环边； 85 | - 这个复杂一点，首先在BA图中当前帧顶点位置处，广搜得到一些距离较近的帧，然后对于每一帧截取前后时间段范围内的帧，作为该邻近帧的chain。再在chain里面找一帧最近的帧，与当前帧建立边。目的都是尽可能多的把距离相近的帧关联起来，不然图过于稀疏。 86 | 87 | 相关函数包括：AddEdges、LinkNearChains、LinkChainToScan、FindNearChains、FindNearLinkedScans。 88 | 89 | ## 六、计数法更新栅格地图 90 | 91 | 每当新增一帧激光点数据，如何融入已有的地图中呢？我们为栅格定义了三种状态 92 | 93 | ``` 94 | typedef enum 95 | { 96 | GridStates_Unknown = 0, 97 | GridStates_Occupied = 100, 98 | GridStates_Free = 255 99 | } GridStates; 100 | ``` 101 | 102 | 从激光发射器位置发射一束激光，打在物体表面，得到一个激光点数据。那么激光点位置记为占用，激光束穿过的的位置都是free，物体后面的区域是未知的。每当新增一帧激光点数据，对激光束路径上的点累加一次free或者occupied计数，用occupied/free来判断是否占用。 103 | 104 | 相关函数包括：CreateFromScans、AddScan、RayTrace、UpdateCell。 105 | 106 | 如有错误请您批评指正，希望内容对您有帮助，更多细节可以查看代码注释~ 107 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/sba_setup.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // 36 | // Functions defined in various test setup files 37 | // 38 | 39 | #ifndef _SBA_SETUP_H_ 40 | #define _SBA_SETUP_H_ 41 | 42 | #include "sparse_bundle_adjustment/sba.h" 43 | #include "sparse_bundle_adjustment/spa2d.h" 44 | using namespace Eigen; 45 | using namespace sba; 46 | using namespace frame_common; 47 | 48 | #include 49 | #include 50 | #include 51 | 52 | using namespace std; 53 | 54 | // elapsed time in microseconds 55 | long long utime(); 56 | // set up spiral system 57 | void 58 | spiral_setup(SysSBA &sba, CamParams &cpars, vector,Eigen::aligned_allocator > > &cps, 59 | double s_near, double s_far, 60 | double ptsize, double kfang, double initang, double cycles, 61 | double inoise, double pnoise, double qnoise); 62 | 63 | void 64 | sphere_setup(SysSBA &sba, CamParams &cpars, vector, Eigen::aligned_allocator > > &cps, 65 | int ncams, // number of cameras 66 | int ncpts, // number of new pts per camera 67 | int nccs, // number of camera connections per camera 68 | double inoise, double pnoise, double qnoise); 69 | 70 | 71 | void 72 | spa_spiral_setup(SysSPA &spa, bool use_cross_links, 73 | vector, Eigen::aligned_allocator > > &cps, 74 | Matrix prec, Matrix vprec, 75 | Matrix a10prec, Matrix a15prec, 76 | double kfang, double initang, double cycles, 77 | double pnoise, double qnoise, double snoise, // measurement noise (m,deg), scale noise (percent) 78 | double mpnoise, double mqnoise); // initial displacement (m,deg) 79 | 80 | 81 | void 82 | spa2d_spiral_setup(SysSPA2d &spa, 83 | vector, Eigen::aligned_allocator > > &cps, 84 | Matrix prec, Matrix vprec, 85 | Matrix a10prec, Matrix a15prec, 86 | double kfang, double initang, double cycles, 87 | double pnoise, double qnoise, double snoise, double dpnoise, double dqnoise); 88 | 89 | 90 | #endif // _VO_SETUP_H_ 91 | -------------------------------------------------------------------------------- /open_karto/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package open_karto 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.2.3 (2020-11-10) 6 | ------------------ 7 | * update destructor to not use c11 feature (`#26 `_) 8 | * fix parameter passing bug of first link to other robots (`#23 `_) 9 | * Fix -Wdelete-non-virtual-dtor and -Wreorder warnings (`#24 `_) 10 | * Contributors: Michael Ferguson, Survy Vaish, Zezhou.Sun 11 | 12 | 1.2.2 (2020-08-07) 13 | ------------------ 14 | * Move the C++ classes and definitions of inline functions from the Mapper.cpp file to the Mapper.h file (`#22 `_) 15 | * add Hokuyo configuration in `SetAngularResolution` function (`#20 `_) 16 | Karto has five kinds of lidar parameter configurations (LMS100, LMS200, LMS291, UTM_30LX, URG_04LX), but there are only the first three configurations in the `SetAngularResolution` function, so I added the parameter configuration of UTM_30LX and URG_04LX. 17 | * Contributors: Jian Wen, Zezhou.Sun 18 | 19 | 1.2.1 (2020-04-24) 20 | ------------------ 21 | * bump version to silence CMP0048 22 | * Merge pull request `#19 `_ from moriarty/patch-1 23 | Melodic: build status badges Indigo is EOL. 24 | * Melodic: build status badges Indigo is EOL. 25 | * Merge pull request `#18 `_ from ms-iot/windows_port 26 | [Windows][melodic] Fix Windows build break 27 | * Changing maintainer email 28 | * Fix windows build. 29 | * Contributors: Alex Moriarty, Luc Bettaieb, Michael Ferguson, Sean Yen 30 | 31 | 1.2.0 (2018-07-11) 32 | ------------------ 33 | * Adds maintainer and branches for melodic 34 | * Merge pull request `#14 `_ from ros-perception/maintainer-add 35 | Adding myself as a maintainer for open_karto 36 | * Adding myself as a maintainer for open_karto 37 | * Merge pull request `#11 `_ from Maidbot/minimum_time_interval 38 | Process scan if enough time has elapsed since the previous one 39 | * Merge pull request `#10 `_ from mikepurvis/fix-cpp11 40 | Use std::isnan/isinf, for C++11 compatibility. 41 | * [Mapper] Take time into account in HasMoveEnough 42 | The function HasMovedEnough now also returns true if more than MinimumTimeInterval time has elapsed since the previously processed laser scan 43 | * [Mapper] Add new parameter, MinimumTimeInterval 44 | * Use std::isnan/isinf, for C++11 compatibility. 45 | * Merge pull request `#8 `_ from mig-em/feature/invalidScans1.1.4 46 | Add invalid scan detection for scanmatcher 47 | * Merge pull request `#9 `_ from Maidbot/ignore_min_readings 48 | Ignore readings less than the sensor's minimum range 49 | * [Karto] Also ignore readings less than the minimum range 50 | * Add invalid scan detection for scanmatcher 51 | * Contributors: Luc Bettaieb, Michael Ferguson, Mike Purvis, Russell Toris, Spyros Maniatopoulos, mig-em 52 | 53 | 1.1.4 (2016-03-02) 54 | ------------------ 55 | * update build status badges 56 | * Adds LocalizedRangeScanWithPoints range scan 57 | * Contributors: Michael Ferguson, Russell Toris 58 | 59 | 1.1.3 (2016-02-16) 60 | ------------------ 61 | * Link against, and export depend on, boost 62 | * Contributors: Hai Nguyen, Michael Ferguson 63 | 64 | 1.1.2 (2015-07-13) 65 | ------------------ 66 | * Added getters and setters for parameters inside Mapper so they can be changed via the ROS param server. 67 | * Contributors: Luc Bettaieb, Michael Ferguson 68 | 69 | 1.1.1 (2015-05-07) 70 | ------------------ 71 | * Makes FindValidPoints robust to the first point in the scan being a NaN 72 | * Bump minimum cmake version requirement 73 | * Fix cppcheck warnings 74 | * Fix newlines (dos2unix) & superfluous whitespace 75 | * Protect functions that throw away const-ness (check dirty flag) with mutex 76 | * Don't modify scan during loop closure check - work on a copy of it 77 | * removed useless return to avoid cppcheck error 78 | * Add Mapper::SetUseScanMatching 79 | * Remove html entities from log output 80 | * Fix NANs cause raytracing to take forever 81 | * Contributors: Daniel Pinyol, Michael Ferguson, Paul Mathieu, Siegfried-A. Gevatter Pujals, liz-murphy 82 | 83 | 1.1.0 (2014-06-15) 84 | ------------------ 85 | * Release as a pure catkin ROS package 86 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/read_spa.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // convert a file in Freiburg's VERTEX / EDGE format into a set of constraints 36 | 37 | #ifndef SBA_READ_SPA_H 38 | #define SBA_READ_SPA_H 39 | 40 | #ifndef EIGEN_USE_NEW_STDVECTOR 41 | #define EIGEN_USE_NEW_STDVECTOR 42 | #endif // EIGEN_USE_NEW_STDVECTOR 43 | 44 | #define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10) 45 | #include 46 | #include 47 | 48 | struct tinfo 49 | { 50 | int pn; // index of point 51 | int fn0, fn1; // index of frames 52 | int fpn0, fpn1; // local point index in frame 53 | double x0,y0,z0,u0,v0; // local XYZ coords and projection 54 | double x1,y1,z1,u1,v1; // local XYZ coords and projection 55 | }; 56 | 57 | int 58 | ReadSPAFile(char *fin, // input file 59 | // node translation 60 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &ntrans, 61 | // node rotation 62 | std::vector< Eigen::Vector4d, Eigen::aligned_allocator > &nqrot, 63 | // constraint indices 64 | std::vector< Eigen::Vector2i, Eigen::aligned_allocator > &cind, 65 | // constraint local translation 66 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &ctrans, 67 | // constraint local rotation as quaternion 68 | std::vector< Eigen::Vector4d, Eigen::aligned_allocator > &cqrot, 69 | // constraint covariance 70 | std::vector< Eigen::Matrix, Eigen::aligned_allocator > > &cvar, 71 | // track info: point projections, see format description in IntelSeattle files 72 | std::vector &tracks 73 | ); 74 | 75 | int 76 | ReadSPA2dFile(char *fin, // input file 77 | // node translation 78 | std::vector< Eigen::Vector2d, Eigen::aligned_allocator > &ntrans, 79 | // node rotation 80 | std::vector< double > &nqrot, 81 | // constraint indices 82 | std::vector< Eigen::Vector2i, Eigen::aligned_allocator > &cind, 83 | // constraint local translation 84 | std::vector< Eigen::Vector2d, Eigen::aligned_allocator > &ctrans, 85 | // constraint local rotation as quaternion 86 | std::vector< double > &cqrot, 87 | // constraint covariance 88 | std::vector< Eigen::Matrix, Eigen::aligned_allocator > > &cvar, 89 | // scan points 90 | std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator > > &scans 91 | ); 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /slam_karto/launch/karto_slam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 549 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.699999988079071 56 | Class: rviz/Map 57 | Color Scheme: map 58 | Draw Behind: false 59 | Enabled: true 60 | Name: Map 61 | Topic: /map 62 | Unreliable: false 63 | Use Timestamp: false 64 | Value: true 65 | - Class: rviz/MarkerArray 66 | Enabled: true 67 | Marker Topic: /visualization_marker_array 68 | Name: MarkerArray 69 | Namespaces: 70 | {} 71 | Queue Size: 100 72 | Value: true 73 | Enabled: true 74 | Global Options: 75 | Background Color: 48; 48; 48 76 | Default Light: true 77 | Fixed Frame: map 78 | Frame Rate: 30 79 | Name: root 80 | Tools: 81 | - Class: rviz/Interact 82 | Hide Inactive Objects: true 83 | - Class: rviz/MoveCamera 84 | - Class: rviz/Select 85 | - Class: rviz/FocusCamera 86 | - Class: rviz/Measure 87 | - Class: rviz/SetInitialPose 88 | Theta std deviation: 0.2617993950843811 89 | Topic: /initialpose 90 | X std deviation: 0.5 91 | Y std deviation: 0.5 92 | - Class: rviz/SetGoal 93 | Topic: /move_base_simple/goal 94 | - Class: rviz/PublishPoint 95 | Single click: true 96 | Topic: /clicked_point 97 | Value: true 98 | Views: 99 | Current: 100 | Class: rviz/Orbit 101 | Distance: 10 102 | Enable Stereo Rendering: 103 | Stereo Eye Separation: 0.05999999865889549 104 | Stereo Focal Distance: 1 105 | Swap Stereo Eyes: false 106 | Value: false 107 | Field of View: 0.7853981852531433 108 | Focal Point: 109 | X: 0 110 | Y: 0 111 | Z: 0 112 | Focal Shape Fixed Size: true 113 | Focal Shape Size: 0.05000000074505806 114 | Invert Z Axis: false 115 | Name: Current View 116 | Near Clip Distance: 0.009999999776482582 117 | Pitch: 0.785398006439209 118 | Target Frame: 119 | Yaw: 0.785398006439209 120 | Saved: ~ 121 | Window Geometry: 122 | Displays: 123 | collapsed: false 124 | Height: 846 125 | Hide Left Dock: false 126 | Hide Right Dock: false 127 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 128 | Selection: 129 | collapsed: false 130 | Time: 131 | collapsed: false 132 | Tool Properties: 133 | collapsed: false 134 | Views: 135 | collapsed: false 136 | Width: 1200 137 | X: 597 138 | Y: 466 139 | -------------------------------------------------------------------------------- /open_karto/include/open_karto/Macros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef OPEN_KARTO_MACROS_H 19 | #define OPEN_KARTO_MACROS_H 20 | 21 | //////////////////////////////////////////////////////////////////////////////////////// 22 | //////////////////////////////////////////////////////////////////////////////////////// 23 | //////////////////////////////////////////////////////////////////////////////////////// 24 | 25 | /** 26 | * Karto defines for handling deprecated code 27 | */ 28 | #ifndef KARTO_DEPRECATED 29 | # if defined(__GNUC__) && (__GNUC__ >= 4 || (__GNUC__ == 3 && __GNUC_MINOR__>=1)) 30 | # define KARTO_DEPRECATED __attribute__((deprecated)) 31 | # elif defined(__INTEL) || defined(_MSC_VER) 32 | # define KARTO_DEPRECATED __declspec(deprecated) 33 | # else 34 | # define KARTO_DEPRECATED 35 | # endif 36 | #endif 37 | 38 | //////////////////////////////////////////////////////////////////////////////////////// 39 | //////////////////////////////////////////////////////////////////////////////////////// 40 | //////////////////////////////////////////////////////////////////////////////////////// 41 | 42 | /** 43 | * Karto defines for windows dynamic build 44 | */ 45 | #if defined(_MSC_VER) || defined(__CYGWIN__) || defined(__MINGW32__) || defined( __BCPLUSPLUS__) || defined( __MWERKS__) 46 | # if defined( _LIB ) || defined( KARTO_STATIC ) || defined( STATIC_BUILD ) 47 | # define KARTO_EXPORT 48 | # else 49 | # ifdef KARTO_DYNAMIC 50 | # define KARTO_EXPORT __declspec(dllexport) 51 | # else 52 | # define KARTO_EXPORT __declspec(dllimport) 53 | # endif // KARTO_DYNAMIC 54 | # endif 55 | #else 56 | # define KARTO_EXPORT 57 | #endif 58 | 59 | //////////////////////////////////////////////////////////////////////////////////////// 60 | //////////////////////////////////////////////////////////////////////////////////////// 61 | //////////////////////////////////////////////////////////////////////////////////////// 62 | 63 | /** 64 | * Helper defines for std iterator loops 65 | */ 66 | #define forEach( listtype, list ) \ 67 | for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) 68 | 69 | #define forEachAs( listtype, list, iter ) \ 70 | for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) 71 | 72 | #define const_forEach( listtype, list ) \ 73 | for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) 74 | 75 | #define const_forEachAs( listtype, list, iter ) \ 76 | for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) 77 | 78 | #define forEachR( listtype, list ) \ 79 | for ( listtype::reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter ) 80 | 81 | #define const_forEachR( listtype, list ) \ 82 | for ( listtype::const_reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter ) 83 | 84 | 85 | //////////////////////////////////////////////////////////////////////////////////////// 86 | //////////////////////////////////////////////////////////////////////////////////////// 87 | //////////////////////////////////////////////////////////////////////////////////////// 88 | 89 | /** 90 | * Disable annoying compiler warnings 91 | */ 92 | 93 | #if defined(__INTEL) || defined(_MSC_VER) 94 | 95 | // Disable the warning: 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2' 96 | #pragma warning(disable:4251) 97 | 98 | #endif 99 | 100 | #ifdef __INTEL_COMPILER 101 | 102 | // Disable the warning: conditional expression is constant 103 | #pragma warning(disable:4127) 104 | 105 | // Disable the warning: 'identifier' : unreferenced formal parameter 106 | #pragma warning(disable:4100) 107 | 108 | // remark #383: value copied to temporary, reference to temporary used 109 | #pragma warning(disable:383) 110 | 111 | // remark #981: operands are evaluated in unspecified order 112 | // disabled -> completely pointless if the functions do not have side effects 113 | #pragma warning(disable:981) 114 | 115 | // remark #1418: external function definition with no prior declaration 116 | #pragma warning(disable:1418) 117 | 118 | // remark #1572: floating-point equality and inequality comparisons are unreliable 119 | // disabled -> everyone knows it, the parser passes this problem deliberately to the user 120 | #pragma warning(disable:1572) 121 | 122 | // remark #10121: 123 | #pragma warning(disable:10121) 124 | 125 | #endif // __INTEL_COMPILER 126 | 127 | #endif // OPEN_KARTO_MACROS_H 128 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/src/visualization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sba { 4 | 5 | void drawGraph(const SysSBA &sba, const ros::Publisher &camera_pub, 6 | const ros::Publisher &point_pub, int decimation, int bicolor) 7 | { 8 | int num_points = sba.tracks.size(); 9 | int num_cameras = sba.nodes.size(); 10 | if (num_points == 0 && num_cameras == 0) return; 11 | 12 | visualization_msgs::Marker camera_marker, point_marker; 13 | camera_marker.header.frame_id = "/pgraph"; 14 | camera_marker.header.stamp = ros::Time::now(); 15 | camera_marker.ns = "pgraph"; 16 | camera_marker.id = 0; 17 | camera_marker.action = visualization_msgs::Marker::ADD; 18 | camera_marker.pose.position.x = 0; 19 | camera_marker.pose.position.y = 0; 20 | camera_marker.pose.position.z = 0; 21 | camera_marker.pose.orientation.x = 0.0; 22 | camera_marker.pose.orientation.y = 0.0; 23 | camera_marker.pose.orientation.z = 0.0; 24 | camera_marker.pose.orientation.w = 1.0; 25 | camera_marker.scale.x = 0.02; 26 | camera_marker.scale.y = 0.02; 27 | camera_marker.scale.z = 0.02; 28 | camera_marker.color.r = 0.0f; 29 | camera_marker.color.g = 1.0f; 30 | camera_marker.color.b = 1.0f; 31 | camera_marker.color.a = 1.0f; 32 | camera_marker.lifetime = ros::Duration(); 33 | camera_marker.type = visualization_msgs::Marker::LINE_LIST; 34 | 35 | point_marker = camera_marker; 36 | point_marker.color.r = 1.0f; 37 | point_marker.color.g = 0.0f; 38 | point_marker.color.b = 0.0f; 39 | point_marker.color.a = 0.5f; 40 | point_marker.scale.x = 0.02; 41 | point_marker.scale.y = 0.02; 42 | point_marker.scale.z = 0.02; 43 | point_marker.type = visualization_msgs::Marker::POINTS; 44 | 45 | // draw points, decimated 46 | point_marker.points.resize((int)(num_points/(double)decimation + 0.5)); 47 | point_marker.colors.resize((int)(num_points/(double)decimation + 0.5)); 48 | for (int i=0, ii=0; i < num_points; i += decimation, ii++) 49 | { 50 | const Vector4d &pt = sba.tracks[i].point; 51 | point_marker.colors[ii].r = 1.0f; 52 | if (bicolor > 0 && i >= bicolor) 53 | point_marker.colors[ii].g = 1.0f; 54 | else 55 | point_marker.colors[ii].g = 0.0f; 56 | point_marker.colors[ii].b = 0.0f; 57 | point_marker.points[ii].x = pt(2); 58 | point_marker.points[ii].y = -pt(0); 59 | point_marker.points[ii].z = -pt(1); 60 | } 61 | 62 | // draw cameras 63 | camera_marker.points.resize(num_cameras*6); 64 | for (int i=0, ii=0; i < num_cameras; i++) 65 | { 66 | const Node &nd = sba.nodes[i]; 67 | Vector3d opt; 68 | Matrix tr; 69 | transformF2W(tr,nd.trans,nd.qrot); 70 | 71 | camera_marker.points[ii].x = nd.trans.z(); 72 | camera_marker.points[ii].y = -nd.trans.x(); 73 | camera_marker.points[ii++].z = -nd.trans.y(); 74 | opt = tr*Vector4d(0,0,0.3,1); 75 | camera_marker.points[ii].x = opt.z(); 76 | camera_marker.points[ii].y = -opt.x(); 77 | camera_marker.points[ii++].z = -opt.y(); 78 | 79 | camera_marker.points[ii].x = nd.trans.z(); 80 | camera_marker.points[ii].y = -nd.trans.x(); 81 | camera_marker.points[ii++].z = -nd.trans.y(); 82 | opt = tr*Vector4d(0.2,0,0,1); 83 | camera_marker.points[ii].x = opt.z(); 84 | camera_marker.points[ii].y = -opt.x(); 85 | camera_marker.points[ii++].z = -opt.y(); 86 | 87 | camera_marker.points[ii].x = nd.trans.z(); 88 | camera_marker.points[ii].y = -nd.trans.x(); 89 | camera_marker.points[ii++].z = -nd.trans.y(); 90 | opt = tr*Vector4d(0,0.1,0,1); 91 | camera_marker.points[ii].x = opt.z(); 92 | camera_marker.points[ii].y = -opt.x(); 93 | camera_marker.points[ii++].z = -opt.y(); 94 | } 95 | 96 | // draw point-plane projections 97 | int num_tracks = sba.tracks.size(); 98 | int ii = camera_marker.points.size(); 99 | 100 | for (int i=0; i < num_tracks; i++) 101 | { 102 | const ProjMap &prjs = sba.tracks[i].projections; 103 | for (ProjMap::const_iterator itr = prjs.begin(); itr != prjs.end(); itr++) 104 | { 105 | const Proj &prj = (*itr).second; 106 | if (prj.pointPlane) // have a ptp projection 107 | { 108 | camera_marker.points.resize(ii+2); 109 | Point pt0 = sba.tracks[i].point; 110 | Vector3d plane_point = prj.plane_point; 111 | Vector3d plane_normal = prj.plane_normal; 112 | Eigen::Vector3d w = pt0.head<3>()-plane_point; 113 | // Eigen::Vector3d projpt = plane_point+(w.dot(plane_normal))*plane_normal; 114 | Eigen::Vector3d projpt = pt0.head<3>() - (w.dot(plane_normal))*plane_normal; 115 | // Vector3d pt1 = pt0.head<3>()+0.1*plane_normal; 116 | Vector3d pt1 = projpt; 117 | 118 | camera_marker.points[ii].x = pt0.z(); 119 | camera_marker.points[ii].y = -pt0.x(); 120 | camera_marker.points[ii++].z = -pt0.y(); 121 | camera_marker.points[ii].x = pt1.z(); 122 | camera_marker.points[ii].y = -pt1.x(); 123 | camera_marker.points[ii++].z = -pt1.y(); 124 | } 125 | } 126 | } 127 | 128 | camera_pub.publish(camera_marker); 129 | point_pub.publish(point_marker); 130 | } 131 | 132 | } // namespace sba 133 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/sba_file_io.h: -------------------------------------------------------------------------------- 1 | #ifndef SBA_BUNDLER_FILE_H 2 | #define SBA_BUNDLER_FILE_H 3 | 4 | #ifndef EIGEN_USE_NEW_STDVECTOR 5 | #define EIGEN_USE_NEW_STDVECTOR 6 | #endif // EIGEN_USE_NEW_STDVECTOR 7 | 8 | //#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10) 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "sparse_bundle_adjustment/sba.h" 17 | 18 | namespace Eigen 19 | { 20 | typedef Matrix Vector11d; 21 | typedef Matrix Vector5d; 22 | } 23 | 24 | namespace sba 25 | { 26 | 27 | /** \brief Reads bundle adjustment data from a Bundler file to an instance of SysSBA. 28 | * 29 | * \param filename The name of the bundler-formatted file to read from. 30 | * \param sbaout An instance of SBA that the file will be written to. 31 | * It should be noted that the bundler format does not support stereo points; 32 | * all points read in will be monocular. 33 | * Note: documentation of the Bundler format can be found at 34 | * http://phototour.cs.washington.edu/bundler/bundler-v0.3-manual.html . 35 | */ 36 | int readBundlerFile(const char *filename, sba::SysSBA& sbaout); 37 | 38 | /** \brief Writes bundle adjustment data from an instance of SysSBA to a Bundler file. 39 | * 40 | * \param filename The name of the bundler-formatted file to write to. 41 | * The file is created, or if it already exists, truncated. 42 | * \param sbain An instance of SBA that the file will be written from. 43 | * It should be noted that the bundler format does not support stereo points; 44 | * all points written will be monocular. Also, since SBA does not store point 45 | * color information, all points will be colored white. 46 | * Note: documentation of the Bundler format can be found at 47 | * http://phototour.cs.washington.edu/bundler/bundler-v0.3-manual.html . 48 | */ 49 | int writeBundlerFile(const char *filename, sba::SysSBA& sbain); 50 | 51 | /** \brief A low-level parser for bundler files. */ 52 | int 53 | ParseBundlerFile(const char *fin, // input file 54 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &camp, // cam params 55 | std::vector< Eigen::Matrix3d, Eigen::aligned_allocator > &camR, // cam rotation matrix 56 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &camt, // cam translation 57 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &ptp, // point position 58 | std::vector< Eigen::Vector3i, Eigen::aligned_allocator > &ptc, // point color 59 | std::vector< std::vector< Eigen::Vector4d, Eigen::aligned_allocator > > &ptt // point tracks - each vector is 60 | ); 61 | 62 | /** \brief Reads bundle adjustment data from a graph-type file to an instance of SysSBA. 63 | * 64 | * \param filename The name of the bundler-formatted file to read from. 65 | * \param sbaout An instance of SBA that the file will be written to. 66 | * Note: where is the documentation for this format? 67 | */ 68 | int readGraphFile(const char *filename, sba::SysSBA& sbaout); 69 | 70 | /** \brief A low-level parser for graph files. */ 71 | int 72 | ParseGraphFile(const char *fin, // input file 73 | std::vector< Eigen::Vector5d, Eigen::aligned_allocator > &camps, // cam params 74 | std::vector< Eigen::Vector4d, Eigen::aligned_allocator > &camqs, // cam rotation matrix 75 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &camts, // cam translation 76 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &ptps, // point position 77 | std::vector< std::vector< Eigen::Vector11d, Eigen::aligned_allocator > > &ptts // point tracks - each vector is 78 | ); 79 | 80 | 81 | /// write out system in SBA form 82 | void writeLourakisFile(const char *fname, SysSBA& sba); 83 | void writeA(const char *fname, SysSBA& sba); // save precision matrix 84 | void writeSparseA(const char *fname, SysSBA& sba); // save precision matrix in CSPARSE format 85 | 86 | /** 87 | * \brief Writes out the current SBA system as an ascii graph file 88 | * suitable to be read in by the Freiburg HChol system. 89 | * is true if only monocular projections are desired 90 | */ 91 | int writeGraphFile(const char *filename, SysSBA& sba, bool mono=false); 92 | 93 | 94 | /** \brief Reads 3D pose graph data from a graph-type file to an instance of SysSPA. 95 | * 96 | * \param filename The name of the bundler-formatted file to read from. 97 | * \param spaout An instance of SPA that the file will be written to. 98 | * Note: where is the documentation for this format? 99 | */ 100 | 101 | int readSPAGraphFile(const char *filename, SysSPA& spaout); 102 | 103 | /** \brief A low-level parser for graph files. */ 104 | int 105 | ParseSPAGraphFile(const char *fin, // input file 106 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &ntrans, // node translation 107 | std::vector< Eigen::Vector4d, Eigen::aligned_allocator > &nqrot, // node rotation 108 | std::vector< Eigen::Vector2i, Eigen::aligned_allocator > &cind, // constraint indices 109 | std::vector< Eigen::Vector3d, Eigen::aligned_allocator > &ctrans, // constraint local translation 110 | std::vector< Eigen::Vector4d, Eigen::aligned_allocator > &cqrot, // constraint local rotation as quaternion 111 | std::vector< Eigen::Matrix, Eigen::aligned_allocator > > &cvar); // constraint covariance 112 | 113 | }; // namespace SBA 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/src/node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace std; 4 | using namespace Eigen; 5 | 6 | namespace sba 7 | { 8 | void Node::setTransform() 9 | { transformW2F(w2n,trans,qrot); } 10 | 11 | // 12 | // sets incremental angle derivatives 13 | // 14 | void Node::setDri() 15 | { 16 | setDr(true); 17 | } 18 | 19 | // constant derivative matrices 20 | // these are the derivatives of the *inverse* rotation 21 | Eigen::Matrix3d Node::dRidx, Node::dRidy, Node::dRidz; 22 | 23 | void Node::initDr() 24 | { 25 | dRidx << 0.0,0.0,0.0, 26 | 0.0,0.0,2.0, 27 | 0.0,-2.0,0.0; 28 | dRidy << 0.0,0.0,-2.0, 29 | 0.0,0.0,0.0, 30 | 2.0,0.0,0.0; 31 | dRidz << 0.0,2.0,0.0, 32 | -2.0,0.0,0.0, 33 | 0.0,0.0,0.0; 34 | } 35 | 36 | 37 | void Node::normRot() 38 | { 39 | // std::cout << "[NormRot] qrot start = " << qrot.transpose() << std::endl; 40 | if (qrot.w() < 0) qrot.coeffs().head<3>() = -qrot.coeffs().head<3>(); 41 | double sn = qrot.coeffs().head<3>().squaredNorm(); 42 | if (sn >= 0.9999) // too close to high derivatives 43 | qrot.coeffs().head<3>() *= -1.0/(sqrt(sn)*1.0001); // switch sides; 1e-4 seems to work well 44 | qrot.w() = sqrt(1.0 - qrot.coeffs().head<3>().squaredNorm()); 45 | if (std::isnan(qrot.x()) || std::isnan(qrot.y()) || std::isnan(qrot.z()) || std::isnan(qrot.w()) ) 46 | { 47 | printf("[NormRot] Bad quaternion: %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w()); 48 | exit(1); 49 | } 50 | // std::cout << "[NormRot] qrot end = " << qrot.transpose() << std::endl; 51 | } 52 | 53 | // 54 | // sets angle derivatives 55 | // 56 | void Node::setDr(bool local) 57 | { 58 | // for dS'*R', with dS the incremental change 59 | if (local) 60 | { 61 | #if 0 62 | dRdx = w2n.block<3,3>(0,0) * dRidx; 63 | dRdy = w2n.block<3,3>(0,0) * dRidy; 64 | dRdz = w2n.block<3,3>(0,0) * dRidz; 65 | #endif 66 | dRdx = dRidx * w2n.block<3,3>(0,0); 67 | dRdy = dRidy * w2n.block<3,3>(0,0); 68 | dRdz = dRidz * w2n.block<3,3>(0,0); 69 | 70 | } 71 | else 72 | { 73 | double x2 = qrot.x() * 2.0; 74 | double y2 = qrot.y() * 2.0; 75 | double z2 = qrot.z() * 2.0; 76 | double w2 = qrot.w() * 2.0; 77 | double xw = qrot.x()/qrot.w(); // these are problematic for w near zero 78 | double yw = qrot.y()/qrot.w(); 79 | double zw = qrot.z()/qrot.w(); 80 | 81 | // dR/dx 82 | dRdx(0,0) = 0.0; 83 | dRdx(0,1) = y2-zw*x2; 84 | dRdx(0,2) = z2+yw*x2; 85 | 86 | dRdx(1,0) = y2+zw*x2; 87 | dRdx(1,1) = -2.0*x2; 88 | dRdx(1,2) = w2-xw*x2; 89 | 90 | dRdx(2,0) = z2-yw*x2; 91 | dRdx(2,1) = -dRdx(1,2); 92 | dRdx(2,2) = dRdx(1,1); 93 | 94 | // dR/dy 95 | dRdy(0,0) = -2.0*y2; 96 | dRdy(0,1) = x2-zw*y2; 97 | dRdy(0,2) = (-w2)+yw*y2; 98 | 99 | dRdy(1,0) = x2+zw*y2; 100 | dRdy(1,1) = 0.0; 101 | dRdy(1,2) = dRdx(2,0); 102 | 103 | dRdy(2,0) = -dRdy(0,2); 104 | dRdy(2,1) = dRdx(0,2); 105 | dRdy(2,2) = dRdy(0,0); 106 | 107 | // dR/dz 108 | dRdz(0,0) = -2.0*z2; 109 | dRdz(0,1) = w2-zw*z2; 110 | dRdz(0,2) = dRdy(1,0); 111 | 112 | dRdz(1,0) = -dRdz(0,1); 113 | dRdz(1,1) = dRdz(0,0); 114 | dRdz(1,2) = dRdx(0,1); 115 | 116 | dRdz(2,0) = dRdy(0,1); 117 | dRdz(2,1) = dRdx(1,0); 118 | dRdz(2,2) = 0.0; 119 | } 120 | } 121 | 122 | void Node::normRotLocal() 123 | { 124 | qrot.normalize(); 125 | if (qrot.w() < 0) qrot.coeffs().head<3>() = -qrot.coeffs().head<3>(); 126 | if (std::isnan(qrot.x()) || std::isnan(qrot.y()) || std::isnan(qrot.z()) || std::isnan(qrot.w()) ) 127 | { 128 | printf("[NormRot] Bad quaternion in normRotLocal(): %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w()); 129 | exit(1); 130 | } 131 | // std::cout << "[NormRot] qrot end = " << qrot.transpose() << std::endl; 132 | } 133 | 134 | void Node::projectMono(const Point& point, Eigen::Vector3d& proj) 135 | { 136 | Vector2d proj2d; 137 | project2im(proj2d, point); 138 | 139 | proj.head<2>() = proj2d; 140 | } 141 | 142 | void Node::projectStereo(const Point& point, Eigen::Vector3d& proj) 143 | { 144 | Vector2d proj2d; 145 | Vector3d pc, baseline_vect; 146 | project2im(proj2d, point); 147 | 148 | // Camera coords for right camera 149 | baseline_vect << baseline, 0, 0; 150 | pc = Kcam * (w2n*point - baseline_vect); 151 | proj.head<2>() = proj2d; 152 | proj(2) = pc(0)/pc(2); 153 | } 154 | 155 | 156 | // transforms 157 | void transformW2F(Eigen::Matrix &m, 158 | const Eigen::Matrix &trans, 159 | const Eigen::Quaternion &qrot) 160 | { 161 | m.block<3,3>(0,0) = qrot.toRotationMatrix().transpose(); 162 | m.col(3).setZero(); // make sure there's no translation 163 | m.col(3) = -m*trans; 164 | }; 165 | 166 | void transformF2W(Eigen::Matrix &m, 167 | const Eigen::Matrix &trans, 168 | const Eigen::Quaternion &qrot) 169 | { 170 | m.block<3,3>(0,0) = qrot.toRotationMatrix(); 171 | m.col(3) = trans.head(3); 172 | }; 173 | 174 | 175 | // returns the local R,t in nd0 that produces nd1 176 | // NOTE: returns a postfix rotation; should return a prefix 177 | void transformN2N(Eigen::Matrix &trans, 178 | Eigen::Quaternion &qr, 179 | Node &nd0, Node &nd1) 180 | { 181 | Matrix tfm; 182 | Quaterniond q0,q1; 183 | q0 = nd0.qrot; 184 | transformW2F(tfm,nd0.trans,q0); 185 | trans.head(3) = tfm*nd1.trans; 186 | trans(3) = 1.0; 187 | q1 = nd1.qrot; 188 | qr = q0.inverse()*q1; 189 | qr.normalize(); 190 | if (qr.w() < 0) 191 | qr.coeffs() = -qr.coeffs(); 192 | } 193 | 194 | 195 | } // namespace sba 196 | 197 | 198 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/src/nodes/sba_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Messages 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | 17 | using namespace sba; 18 | 19 | class SBANode 20 | { 21 | public: 22 | SysSBA sba; 23 | ros::NodeHandle n; 24 | ros::Subscriber frame_sub; 25 | ros::Publisher cam_marker_pub; 26 | ros::Publisher point_marker_pub; 27 | 28 | ros::Timer timer_sba; 29 | ros::Timer timer_vis; 30 | 31 | // Mapping from external point index to internal (sba) point index 32 | std::map point_indices; 33 | 34 | // Mapping from external node index to internal (sba) node index 35 | std::map node_indices; 36 | 37 | void addFrame(const sba::Frame::ConstPtr& msg); 38 | void addNode(const sba::CameraNode& msg); 39 | void addPoint(const sba::WorldPoint& msg); 40 | void addProj(const sba::Projection& msg); 41 | void doSBA(/*const ros::TimerEvent& event*/); 42 | void publishTopics(/*const ros::TimerEvent& event*/); 43 | SBANode(); 44 | }; 45 | 46 | void SBANode::addFrame(const sba::Frame::ConstPtr& msg) 47 | { 48 | unsigned int i = 0; 49 | 50 | ros::Time beginning = ros::Time::now(); 51 | 52 | // Add all nodes 53 | for (i=0; i < msg->nodes.size(); i++) 54 | { 55 | addNode(msg->nodes[i]); 56 | } 57 | 58 | // Add all points 59 | for (i=0; i < msg->points.size(); i++) 60 | { 61 | addPoint(msg->points[i]); 62 | } 63 | 64 | // Add all projections 65 | for (i=0; i < msg->projections.size(); i++) 66 | { 67 | addProj(msg->projections[i]); 68 | } 69 | 70 | publishTopics(); 71 | 72 | // Do SBA Every 10 frames. 73 | if (sba.nodes.size() % 5 == 0) 74 | { 75 | doSBA(); 76 | } 77 | 78 | ros::Time end = ros::Time::now(); 79 | 80 | printf("[SBA] Added frame with %u nodes, %u points, and %u projections (%f s)\n", 81 | (unsigned int)msg->nodes.size(), (unsigned int)msg->points.size(), 82 | (unsigned int)msg->projections.size(), (end-beginning).toSec()); 83 | } 84 | 85 | void SBANode::addNode(const sba::CameraNode& msg) 86 | { 87 | Vector4d trans(msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z, 1.0); 88 | Quaterniond qrot(msg.transform.rotation.w, msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z); 89 | 90 | frame_common::CamParams cam_params; 91 | cam_params.fx = msg.fx; 92 | cam_params.fy = msg.fy; 93 | cam_params.cx = msg.cx; 94 | cam_params.cy = msg.cy; 95 | cam_params.tx = msg.baseline; 96 | 97 | bool fixed = msg.fixed; 98 | 99 | unsigned int newindex = sba.addNode(trans, qrot, cam_params, fixed); 100 | 101 | node_indices[msg.index] = newindex; 102 | } 103 | 104 | void SBANode::addPoint(const sba::WorldPoint& msg) 105 | { 106 | Vector4d point(msg.x, msg.y, msg.z, msg.w); 107 | unsigned int newindex = sba.addPoint(point); 108 | 109 | point_indices[msg.index] = newindex; 110 | } 111 | 112 | void SBANode::addProj(const sba::Projection& msg) 113 | { 114 | int camindex = node_indices[msg.camindex]; 115 | int pointindex = point_indices[msg.pointindex]; 116 | Vector3d keypoint(msg.u, msg.v, msg.d); 117 | bool stereo = msg.stereo; 118 | bool usecovariance = msg.usecovariance; 119 | Eigen::Matrix3d covariance; 120 | covariance << msg.covariance[0], msg.covariance[1], msg.covariance[2], 121 | msg.covariance[3], msg.covariance[4], msg.covariance[5], 122 | msg.covariance[6], msg.covariance[7], msg.covariance[8]; 123 | 124 | // Make sure it's valid before adding it. 125 | if (pointindex < (int)sba.tracks.size() && camindex < (int)sba.nodes.size()) 126 | { 127 | sba.addProj(camindex, pointindex, keypoint, stereo); 128 | if (usecovariance) 129 | sba.setProjCovariance(camindex, pointindex, covariance); 130 | } 131 | else 132 | { 133 | ROS_INFO("Failed to add projection: C: %d, P: %d, Csize: %d, Psize: %d", 134 | camindex, pointindex,(int)sba.nodes.size(),(int)sba.tracks.size()); 135 | } 136 | } 137 | 138 | void SBANode::doSBA(/*const ros::TimerEvent& event*/) 139 | { 140 | unsigned int projs = 0; 141 | // For debugging. 142 | for (int i = 0; i < (int)sba.tracks.size(); i++) 143 | { 144 | projs += sba.tracks[i].projections.size(); 145 | } 146 | ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sba.nodes.size(), 147 | (int)sba.tracks.size(), projs); 148 | 149 | if (sba.nodes.size() > 0) 150 | { 151 | // Copied from vslam.cpp: refine() 152 | sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY); 153 | 154 | double cost = sba.calcRMSCost(); 155 | 156 | if (isnan(cost) || isinf(cost)) // is NaN? 157 | { 158 | ROS_INFO("NaN cost!"); 159 | } 160 | else 161 | { 162 | if (sba.calcRMSCost() > 4.0) 163 | sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY); // do more 164 | if (sba.calcRMSCost() > 4.0) 165 | sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY); // do more 166 | } 167 | } 168 | } 169 | 170 | void SBANode::publishTopics(/*const ros::TimerEvent& event*/) 171 | { 172 | // Visualization 173 | if (cam_marker_pub.getNumSubscribers() > 0 || point_marker_pub.getNumSubscribers() > 0) 174 | { 175 | drawGraph(sba, cam_marker_pub, point_marker_pub); 176 | } 177 | } 178 | 179 | SBANode::SBANode() 180 | { 181 | // Advertise visualization topics. 182 | cam_marker_pub = n.advertise("/sba/cameras", 1); 183 | point_marker_pub = n.advertise("/sba/points", 1); 184 | 185 | //timer_sba = n.createTimer(ros::Duration(5.0), &SBANode::doSBA, this); 186 | //timer_vis = n.createTimer(ros::Duration(1.0), &SBANode::publishTopics, this); 187 | 188 | // Subscribe to topics. 189 | frame_sub = n.subscribe("/sba/frames", 5000, &SBANode::addFrame, this); 190 | 191 | sba.useCholmod(true); 192 | 193 | printf("[SBA] Initialization complete.\n"); 194 | } 195 | 196 | int main(int argc, char** argv) 197 | { 198 | ros::init(argc, argv, "sba_node"); 199 | SBANode sbanode; 200 | 201 | ros::spin(); 202 | } 203 | 204 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/node.h: -------------------------------------------------------------------------------- 1 | #ifndef _NODE_H_ 2 | #define _NODE_H_ 3 | 4 | // Functions for handling nodes (cameras) within the sba system. 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // needed for camera params 12 | #ifndef _CAMPARAMS_H_ 13 | #define _CAMPARAMS_H_ 14 | 15 | namespace frame_common 16 | { 17 | 18 | /// camera params 19 | /// focal lengths, image center, baseline (for stereo) 20 | /// duplicated in SBA package 21 | struct CamParams 22 | { 23 | double fx, fy, cx, cy, tx; 24 | }; 25 | } // end namespace 26 | 27 | #endif // _CAMPARMS_H_ 28 | 29 | namespace fc = frame_common; 30 | 31 | // put things into a namespace 32 | namespace sba 33 | { 34 | 35 | /// \brief Keypoints - subpixel using floats. 36 | /// u,v are pixel coordinates, d is disparity (if stereo) 37 | typedef Eigen::Vector3d Keypoint; 38 | 39 | 40 | /// \brief Point holds 3D points using in world coordinates. 41 | /// Currently we just represent these as 4-vectors, with a final 42 | /// "1.0". 43 | typedef Eigen::Vector4d Point; 44 | 45 | 46 | /// \brief NODE holds graph nodes corresponding to frames, for use in 47 | /// sparse bundle adjustment. 48 | /// Each node has a 6DOF pose, encoded as a translation vector and 49 | /// rotation unit quaternion (Eigen classes). These represent the 50 | /// pose of the node in the world frame. 51 | /// 52 | /// The pose generates a 3x4 homogenous transform, taking a point 53 | /// in world coordinates into the node coordinates. 54 | /// 55 | /// Additionally a 3x4 homogenous transform is composed from the 56 | /// pose transform and a projection transform to the frame image 57 | /// coordinates. 58 | /// 59 | /// Projections from points to features are in a list. There 60 | /// should also be a reverse index from features to projections 61 | /// and points, so that matched features can tie in to points. 62 | /// 63 | class Node 64 | { 65 | public: 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 67 | 68 | // 6DOF pose as a unit quaternion and translation vector 69 | Eigen::Matrix trans; ///< Translation in homogeneous coordinates, last element is 1.0. 70 | Eigen::Quaternion qrot; ///< Rotation of the node expressed as a Quaternion. 71 | 72 | /// \brief Normalize quaternion to unit. 73 | /// Problem with global derivatives near w=0, solved by a hack for now. 74 | void normRot(); 75 | 76 | /// \brief Normalize quaternion to unit, without w=0 considerations. 77 | void normRotLocal(); 78 | 79 | /// Resultant transform from world to node coordinates. 80 | Eigen::Matrix w2n; 81 | 82 | /// \brief Sets the transform matrices of the node. 83 | void setTransform(); 84 | 85 | // Covariance matrix, 6x6. Variables are [trans,rot], with the 86 | // rotational part being the xyz parameters of the unit 87 | // quaternion 88 | // Eigen::Matrix covar; 89 | 90 | /// \brief Projection to frame image coordinates. 91 | /// pre-multiply by the frame camera matrix. 92 | Eigen::Matrix Kcam; 93 | double baseline; ///< baseline for stereo 94 | 95 | 96 | /// \brief Sets the Kcam and baseline matrices from frame_common::CamParams. 97 | /// \param cp The camera parameters from camera calibration. 98 | void setKcam(const fc::CamParams &cp) 99 | { 100 | Kcam.setZero(); 101 | Kcam(0,0) = cp.fx; 102 | Kcam(1,1) = cp.fy; 103 | Kcam(0,2) = cp.cx; 104 | Kcam(1,2) = cp.cy; 105 | Kcam(2,2) = 1.0; 106 | baseline = cp.tx; 107 | setProjection(); 108 | } 109 | 110 | /// \brief The transform from world to image coordinates. 111 | Eigen::Matrix w2i; 112 | 113 | /// Project a point into image coordinates. 114 | /// \param pi The u, v projection of the point into image coordinates. 115 | /// \param p The 3D point in world coordinates to be projected. 116 | void project2im(Eigen::Vector2d &pi, const Point &p) const 117 | { Eigen::Vector3d p1 = w2i * p; pi = p1.head(2)/p1(2); } 118 | 119 | /// Set up world-to-image projection matrix (w2i), assumes camera parameters 120 | /// are filled. 121 | void setProjection() 122 | { w2i = Kcam * w2n; } 123 | 124 | /// \brief Derivatives of the rotation matrix transpose wrt quaternion xyz, 125 | /// used for calculating Jacobian wrt pose of a projection. 126 | Eigen::Matrix dRdx, dRdy, dRdz; 127 | 128 | /// \brief Constant matrices for derivatives. 129 | static Eigen::Matrix3d dRidx, dRidy, dRidz; // these are constant 130 | 131 | /// \brief Sets up constant matrices for derivatives. 132 | static void initDr(); // call this to set up constant matrices 133 | 134 | /// \brief Set angle derivates. 135 | void setDr(bool local = false); 136 | 137 | /// \brief Set local angle derivatives. 138 | void setDri(); 139 | 140 | /// For SBA, is this camera fixed or free? 141 | bool isFixed; 142 | 143 | /// Previous translation, saved for downdating within LM. 144 | Eigen::Matrix oldtrans; 145 | 146 | /// Previous quaternion rotation, saved for downdating within LM. 147 | Eigen::Quaternion oldqrot; 148 | 149 | /// \brief Project a 3D point into the image frame as a monocular point. 150 | void projectMono(const Point& pt, Eigen::Vector3d& proj); 151 | 152 | /// \brief Project a 3D point into the image frame as a stereo point. 153 | void projectStereo(const Point& pt, Eigen::Vector3d& proj); 154 | }; 155 | 156 | 157 | // Functions to create transformations 158 | 159 | /// Projection matrix from World to Frame coordinates: [R' -R'*t] 160 | /// 161 | /// Based on translation vector and unit quaternion rotation, which 162 | /// gives the frame's pose in world coordinates. Assumes quaternion 163 | /// is normalized (unit). 164 | void transformW2F(Eigen::Matrix &m, 165 | const Eigen::Matrix &trans, 166 | const Eigen::Quaternion &qrot); 167 | 168 | 169 | /// Projection matrix from Frame to World coordinates: [R t] 170 | /// 171 | void transformF2W(Eigen::Matrix &m, 172 | const Eigen::Matrix &trans, 173 | const Eigen::Quaternion &qrot); 174 | 175 | 176 | /// Transform of one node in another's coords 177 | /// 178 | void transformN2N(Eigen::Matrix &trans, 179 | Eigen::Quaternion &qrot, 180 | Node &nd0, Node &nd1); 181 | 182 | } // sba namespace 183 | 184 | #endif // _NODE_H_ 185 | -------------------------------------------------------------------------------- /open_karto/samples/tutorial1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #include 19 | 20 | /** 21 | * Sample code to demonstrate karto map creation 22 | * Create a laser range finder device and three "dummy" range scans. 23 | * Add the device and range scans to a karto Mapper. 24 | */ 25 | karto::Dataset* CreateMap(karto::Mapper* pMapper) 26 | { 27 | karto::Dataset* pDataset = new karto::Dataset(); 28 | 29 | ///////////////////////////////////// 30 | // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory. 31 | karto::Name name("laser0"); 32 | karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name); 33 | pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0)); 34 | pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5)); 35 | pLaserRangeFinder->SetRangeThreshold(12.0); 36 | 37 | pDataset->Add(pLaserRangeFinder); 38 | 39 | ///////////////////////////////////// 40 | // Create three localized range scans, all using the same range readings, but with different poses. 41 | karto::LocalizedRangeScan* pLocalizedRangeScan = NULL; 42 | 43 | // 1. localized range scan 44 | 45 | // Create a vector of range readings. Simple example where all the measurements are the same value. 46 | std::vector readings; 47 | for (int i=0; i<361; i++) 48 | { 49 | readings.push_back(3.0); 50 | } 51 | 52 | // create localized range scan 53 | pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); 54 | pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0)); 55 | pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0)); 56 | 57 | // Add the localized range scan to the mapper 58 | pMapper->Process(pLocalizedRangeScan); 59 | std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; 60 | 61 | // Add the localized range scan to the dataset 62 | pDataset->Add(pLocalizedRangeScan); 63 | 64 | // 2. localized range scan 65 | 66 | // create localized range scan 67 | pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); 68 | pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57)); 69 | pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57)); 70 | 71 | // Add the localized range scan to the mapper 72 | pMapper->Process(pLocalizedRangeScan); 73 | std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; 74 | 75 | // Add the localized range scan to the dataset 76 | pDataset->Add(pLocalizedRangeScan); 77 | 78 | // 3. localized range scan 79 | 80 | // create localized range scan 81 | pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); 82 | pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449)); 83 | pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449)); 84 | 85 | // Add the localized range scan to the mapper 86 | pMapper->Process(pLocalizedRangeScan); 87 | std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; 88 | 89 | // Add the localized range scan to the dataset 90 | pDataset->Add(pLocalizedRangeScan); 91 | 92 | return pDataset; 93 | } 94 | 95 | /** 96 | * Sample code to demonstrate basic occupancy grid creation and print occupancy grid. 97 | */ 98 | karto::OccupancyGrid* CreateOccupancyGrid(karto::Mapper* pMapper, kt_double resolution) 99 | { 100 | std::cout << "Generating map..." << std::endl; 101 | 102 | // Create a map (occupancy grid) - time it 103 | karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution); 104 | 105 | return pOccupancyGrid; 106 | } 107 | 108 | /** 109 | * Sample code to print a basic occupancy grid 110 | */ 111 | void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid) 112 | { 113 | if (pOccupancyGrid != NULL) 114 | { 115 | // Output ASCII representation of map 116 | kt_int32s width = pOccupancyGrid->GetWidth(); 117 | kt_int32s height = pOccupancyGrid->GetHeight(); 118 | karto::Vector2 offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset(); 119 | 120 | std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl; 121 | for (kt_int32s y=height-1; y>=0; y--) 122 | { 123 | for (kt_int32s x=0; xGetValue(karto::Vector2(x, y)); 127 | 128 | switch (value) 129 | { 130 | case karto::GridStates_Unknown: 131 | std::cout << "*"; 132 | break; 133 | case karto::GridStates_Occupied: 134 | std::cout << "X"; 135 | break; 136 | case karto::GridStates_Free: 137 | std::cout << " "; 138 | break; 139 | default: 140 | std::cout << "?"; 141 | } 142 | } 143 | std::cout << std::endl; 144 | } 145 | std::cout << std::endl; 146 | } 147 | } 148 | 149 | int main(int /*argc*/, char /***argv*/) 150 | { 151 | // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. 152 | ///////////////////////////////////// 153 | // Get karto default mapper 154 | karto::Mapper* pMapper = new karto::Mapper(); 155 | if (pMapper != NULL) 156 | { 157 | karto::OccupancyGrid* pOccupancyGrid = NULL; 158 | 159 | ///////////////////////////////////// 160 | // sample code that creates a map from sample device and sample localized range scans 161 | 162 | std::cout << "Tutorial 1 ----------------" << std::endl << std::endl; 163 | 164 | // clear mapper 165 | pMapper->Reset(); 166 | 167 | // create map from created dataset 168 | karto::Dataset* pDataset = CreateMap(pMapper); 169 | 170 | // create occupancy grid at 0.1 resolution and print grid 171 | pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1); 172 | PrintOccupancyGrid(pOccupancyGrid); 173 | delete pOccupancyGrid; 174 | 175 | // delete mapper 176 | delete pMapper; 177 | 178 | delete pDataset; 179 | } 180 | 181 | return 0; 182 | } 183 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/proj.h: -------------------------------------------------------------------------------- 1 | #ifndef _PROJ_H_ 2 | #define _PROJ_H_ 3 | 4 | #ifndef EIGEN_USE_NEW_STDVECTOR 5 | #define EIGEN_USE_NEW_STDVECTOR 6 | #endif // EIGEN_USE_NEW_STDVECTOR 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace sba 17 | { 18 | class JacobProds 19 | { 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 22 | 23 | JacobProds() 24 | { 25 | Hpp.setZero(); 26 | Hpc.setZero(); 27 | Hcc.setZero(); 28 | Bp.setZero(); 29 | JcTE.setZero(); 30 | } 31 | 32 | /// Point-to-point Hessian (JpT*Jp). 33 | Eigen::Matrix Hpp; 34 | 35 | /// Point-to-camera Hessian (JpT*Jc) 36 | Eigen::Matrix Hpc; 37 | 38 | /// Camera-to-camera Hessian (JcT*Jc) 39 | Eigen::Matrix Hcc; 40 | 41 | /// The B matrix with respect to points (JpT*Err) 42 | Eigen::Matrix Bp; 43 | 44 | /// Another matrix with respect to cameras (JcT*Err) 45 | Eigen::Matrix JcTE; 46 | }; 47 | 48 | class Proj; // Forward reference. 49 | 50 | /// Obnoxiously long type def for the map type that holds the point 51 | /// projections in tracks. 52 | typedef std::map, Eigen::aligned_allocator > ProjMap; 53 | 54 | /// \brief Proj holds a projection measurement of a point onto a 55 | /// frame. They are a repository for the link between the frame and 56 | /// the point, with auxillary info such as Jacobians. 57 | class Proj 58 | { 59 | public: 60 | 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 62 | 63 | /// \brief General & stereo constructor. To construct a monocular 64 | /// projection, either use stereo = false or the other constructor. 65 | /// NOTE: sets the projection to be valid. 66 | Proj(int ci, Eigen::Vector3d &q, bool stereo = true); 67 | 68 | /// \brief Monocular constructor. To construct a stereo projection, 69 | /// use other constructor. 70 | /// NOTE: sets the projection to be valid. 71 | Proj(int ci, Eigen::Vector2d &q); 72 | 73 | /// \brief Default constructor. Initializes to default values, 74 | /// kp = <0 0 0> and ndi = <0>. Also sets the projection to be invalid. 75 | Proj(); 76 | 77 | /// Node index, the camera node for this projection. 78 | int ndi; 79 | 80 | /// Keypoint, u,v,u-d vector 81 | Eigen::Vector3d kp; 82 | 83 | /// Reprojection error. 84 | Eigen::Vector3d err; 85 | 86 | /// Whether the projection is Stereo (True) or Monocular (False). 87 | bool stereo; 88 | 89 | /// Calculates re-projection error and stores it in #err. 90 | double calcErr(const Node &nd, const Point &pt, double huber = 0.0); 91 | 92 | /// \brief Get the correct squared norm of the error, depending on 93 | /// whether the projection is monocular or stereo. 94 | double getErrSquaredNorm(); 95 | 96 | /// \brief Get the correct norm of the error, depending on whether the 97 | /// projection is monocular or stereo. 98 | double getErrNorm(); 99 | 100 | /// Sets the jacobians and hessians for the projection to use for SBA. 101 | /** Monocular: 102 | 103 | dpc/dq = dR'/dq [pw-t], in homogeneous form, with q a quaternion param 104 | dpc/dx = -R' * [1 0 0]', in homogeneous form, with x a translation param 105 | d(px/pz)/du = [ pz dpx/du - px dpz/du ] / pz^2, 106 | works for all variables 107 | 108 | Stereo: 109 | pc = R'[pw-t] => left cam 110 | pc = R'[pw-t] + [b 0 0]' => right cam px only 111 | 112 | dpc/dq = dR'/dq [pw-t], in homogeneous form, with q a quaternion param 113 | dpc/dx = -R' * [1 0 0]', in homogeneous form, with x a translation param 114 | d(px/pz)/du = [ pz dpx/du - px dpz/du ] / pz^2, 115 | works for all variables 116 | only change for right cam is px += b */ 117 | void setJacobians(const Node &nd, const Point &pt, JacobProds *jpp); 118 | 119 | /// Jacobian products 120 | JacobProds *jp; 121 | 122 | /// Point-to-camera matrix (HpcT*Hpp^-1) 123 | /// Need to save this 124 | Eigen::Matrix Tpc; 125 | 126 | /// valid or not (could be out of bounds) 127 | bool isValid; 128 | 129 | /// scaling factor for quaternion derivatives relative to translational ones; 130 | /// not sure if this is needed, it's close to 1.0 131 | static constexpr double qScale = 1.0; 132 | 133 | /// Use a covariance matrix? 134 | bool useCovar; 135 | 136 | /// Covariance matrix for cost calculation. 137 | Eigen::Matrix covarmat; 138 | 139 | /// Whether this is a point-plane match (true) or a point-point match (false). 140 | bool pointPlane; 141 | 142 | /// Normal for point-plane projections 143 | Eigen::Vector3d plane_normal; 144 | 145 | /// Point for point-plane projections 146 | Eigen::Vector3d plane_point; 147 | 148 | /// Point-plane match point index in SBA. 149 | int plane_point_index; 150 | 151 | /// Point-plane node index in SBA. 152 | int plane_node_index; 153 | 154 | /// Original normal in #plane_node_index coordinate's frame. 155 | Eigen::Vector3d plane_local_normal; 156 | 157 | /// \brief Set the covariance matrix to use for cost calculation. 158 | /// Without the covariance matrix, cost is calculated by: 159 | /// cost = ||err|| 160 | /// With a covariance matrix, the cost is calculated by: 161 | /// cost = (err)T*covar*(err) 162 | void setCovariance(const Eigen::Matrix3d &covar); 163 | 164 | /// Clear the covariance matrix and no longer use it. 165 | void clearCovariance(); 166 | 167 | 168 | protected: 169 | /// Set monocular jacobians/hessians. 170 | void setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp); 171 | 172 | /// Set stereo jacobians/hessians. 173 | void setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp); 174 | 175 | /// Calculate error function for stereo. 176 | double calcErrMono_(const Node &nd, const Point &pt, double huber); 177 | 178 | /// Calculate error function for stereo. 179 | double calcErrStereo_(const Node &nd, const Point &pt, double huber); 180 | }; 181 | 182 | class Track 183 | { 184 | public: 185 | /// Constructor for a Track at point p. 186 | Track(Point p); 187 | 188 | /// Default constructor for Track. 189 | Track(); 190 | 191 | /// \brief A map of all the projections of the point with camera index 192 | /// as key, based off an STL map. 193 | ProjMap projections; 194 | 195 | /// \brief An Eigen 4-vector containing the coordinates of 196 | /// the point associated with the track. 197 | Point point; 198 | }; 199 | 200 | 201 | } // sba 202 | 203 | #endif // _PROJ_H 204 | -------------------------------------------------------------------------------- /open_karto/samples/tutorial2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #include "SpaSolver.h" 19 | 20 | #include 21 | 22 | /** 23 | * Sample code to demonstrate karto map creation 24 | * Create a laser range finder device and three "dummy" range scans. 25 | * Add the device and range scans to a karto Mapper. 26 | */ 27 | karto::Dataset* CreateMap(karto::Mapper* pMapper) 28 | { 29 | karto::Dataset* pDataset = new karto::Dataset(); 30 | 31 | ///////////////////////////////////// 32 | // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory. 33 | karto::Name name("laser0"); 34 | karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name); 35 | pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0)); 36 | pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5)); 37 | pLaserRangeFinder->SetRangeThreshold(12.0); 38 | 39 | pDataset->Add(pLaserRangeFinder); 40 | 41 | ///////////////////////////////////// 42 | // Create three localized range scans, all using the same range readings, but with different poses. 43 | karto::LocalizedRangeScan* pLocalizedRangeScan = NULL; 44 | 45 | // 1. localized range scan 46 | 47 | // Create a vector of range readings. Simple example where all the measurements are the same value. 48 | std::vector readings; 49 | for (int i=0; i<361; i++) 50 | { 51 | readings.push_back(3.0); 52 | } 53 | 54 | // create localized range scan 55 | pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); 56 | pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0)); 57 | pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0)); 58 | 59 | // Add the localized range scan to the mapper 60 | pMapper->Process(pLocalizedRangeScan); 61 | std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; 62 | 63 | // Add the localized range scan to the dataset 64 | pDataset->Add(pLocalizedRangeScan); 65 | 66 | // 2. localized range scan 67 | 68 | // create localized range scan 69 | pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); 70 | pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57)); 71 | pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57)); 72 | 73 | // Add the localized range scan to the mapper 74 | pMapper->Process(pLocalizedRangeScan); 75 | std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; 76 | 77 | // Add the localized range scan to the dataset 78 | pDataset->Add(pLocalizedRangeScan); 79 | 80 | // 3. localized range scan 81 | 82 | // create localized range scan 83 | pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings); 84 | pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449)); 85 | pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449)); 86 | 87 | // Add the localized range scan to the mapper 88 | pMapper->Process(pLocalizedRangeScan); 89 | std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl; 90 | 91 | // Add the localized range scan to the dataset 92 | pDataset->Add(pLocalizedRangeScan); 93 | 94 | return pDataset; 95 | } 96 | 97 | /** 98 | * Sample code to demonstrate basic occupancy grid creation and print occupancy grid. 99 | */ 100 | karto::OccupancyGrid* CreateOccupancyGrid(karto::Mapper* pMapper, kt_double resolution) 101 | { 102 | std::cout << "Generating map..." << std::endl; 103 | 104 | // Create a map (occupancy grid) - time it 105 | karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution); 106 | 107 | return pOccupancyGrid; 108 | } 109 | 110 | /** 111 | * Sample code to print a basic occupancy grid 112 | */ 113 | void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid) 114 | { 115 | if (pOccupancyGrid != NULL) 116 | { 117 | // Output ASCII representation of map 118 | kt_int32s width = pOccupancyGrid->GetWidth(); 119 | kt_int32s height = pOccupancyGrid->GetHeight(); 120 | karto::Vector2 offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset(); 121 | 122 | std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl; 123 | for (kt_int32s y=height-1; y>=0; y--) 124 | { 125 | for (kt_int32s x=0; xGetValue(karto::Vector2(x, y)); 129 | 130 | switch (value) 131 | { 132 | case karto::GridStates_Unknown: 133 | std::cout << "*"; 134 | break; 135 | case karto::GridStates_Occupied: 136 | std::cout << "X"; 137 | break; 138 | case karto::GridStates_Free: 139 | std::cout << " "; 140 | break; 141 | default: 142 | std::cout << "?"; 143 | } 144 | } 145 | std::cout << std::endl; 146 | } 147 | std::cout << std::endl; 148 | } 149 | } 150 | 151 | int main(int /*argc*/, char /***argv*/) 152 | { 153 | // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. 154 | ///////////////////////////////////// 155 | // Get karto default mapper 156 | karto::Mapper* pMapper = new karto::Mapper(); 157 | if (pMapper != NULL) 158 | { 159 | // set solver 160 | SpaSolver* pSolver = new SpaSolver(); 161 | pMapper->SetScanSolver(pSolver); 162 | 163 | karto::OccupancyGrid* pOccupancyGrid = NULL; 164 | 165 | ///////////////////////////////////// 166 | // sample code that creates a map from sample device and sample localized range scans 167 | 168 | // clear mapper 169 | pMapper->Reset(); 170 | 171 | // create map from created dataset 172 | karto::Dataset* pDataset = CreateMap(pMapper); 173 | 174 | // create occupancy grid at 0.1 resolution and print grid 175 | pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1); 176 | PrintOccupancyGrid(pOccupancyGrid); 177 | delete pOccupancyGrid; 178 | 179 | // delete solver 180 | delete pSolver; 181 | 182 | // delete mapper 183 | delete pMapper; 184 | 185 | // delete dataset and all allocated devices and device states 186 | delete pDataset; 187 | } 188 | 189 | return 0; 190 | } 191 | -------------------------------------------------------------------------------- /open_karto/include/open_karto/Math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #ifndef OPEN_KARTO_MATH_H 19 | #define OPEN_KARTO_MATH_H 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | namespace karto 28 | { 29 | /** 30 | * Platform independent pi definitions 31 | */ 32 | const kt_double KT_PI = 3.14159265358979323846; // The value of PI 33 | const kt_double KT_2PI = 6.28318530717958647692; // 2 * PI 34 | const kt_double KT_PI_2 = 1.57079632679489661923; // PI / 2 35 | const kt_double KT_PI_180 = 0.01745329251994329577; // PI / 180 36 | const kt_double KT_180_PI = 57.29577951308232087685; // 180 / PI 37 | 38 | /** 39 | * Lets define a small number! 40 | */ 41 | const kt_double KT_TOLERANCE = 1e-06; 42 | 43 | /** 44 | * Lets define max value of kt_int32s (int32_t) to use it to mark invalid scans 45 | */ 46 | 47 | const kt_int32s INVALID_SCAN = std::numeric_limits::max(); 48 | 49 | namespace math 50 | { 51 | /** 52 | * Converts degrees into radians 53 | * @param degrees 54 | * @return radian equivalent of degrees 55 | */ 56 | inline kt_double DegreesToRadians(kt_double degrees) 57 | { 58 | return degrees * KT_PI_180; 59 | } 60 | 61 | /** 62 | * Converts radians into degrees 63 | * @param radians 64 | * @return degree equivalent of radians 65 | */ 66 | inline kt_double RadiansToDegrees(kt_double radians) 67 | { 68 | return radians * KT_180_PI; 69 | } 70 | 71 | /** 72 | * Square function 73 | * @param value 74 | * @return square of value 75 | */ 76 | template 77 | inline T Square(T value) 78 | { 79 | return (value * value); 80 | } 81 | 82 | /** 83 | * Round function 84 | * @param value 85 | * @return rounds value to the nearest whole number (as double) 86 | */ 87 | inline kt_double Round(kt_double value) 88 | { 89 | return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5); 90 | } 91 | 92 | /** 93 | * Binary minimum function 94 | * @param value1 95 | * @param value2 96 | * @return the lesser of value1 and value2 97 | */ 98 | template 99 | inline const T& Minimum(const T& value1, const T& value2) 100 | { 101 | return value1 < value2 ? value1 : value2; 102 | } 103 | 104 | /** 105 | * Binary maximum function 106 | * @param value1 107 | * @param value2 108 | * @return the greater of value1 and value2 109 | */ 110 | template 111 | inline const T& Maximum(const T& value1, const T& value2) 112 | { 113 | return value1 > value2 ? value1 : value2; 114 | } 115 | 116 | /** 117 | * Clips a number to the specified minimum and maximum values. 118 | * @param n number to be clipped 119 | * @param minValue minimum value 120 | * @param maxValue maximum value 121 | * @return the clipped value 122 | */ 123 | template 124 | inline const T& Clip(const T& n, const T& minValue, const T& maxValue) 125 | { 126 | return Minimum(Maximum(n, minValue), maxValue); 127 | } 128 | 129 | /** 130 | * Checks whether two numbers are equal within a certain tolerance. 131 | * @param a 132 | * @param b 133 | * @return true if a and b differ by at most a certain tolerance. 134 | */ 135 | inline kt_bool DoubleEqual(kt_double a, kt_double b) 136 | { 137 | double delta = a - b; 138 | return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE; 139 | } 140 | 141 | /** 142 | * Checks whether value is in the range [0;maximum) 143 | * @param value 144 | * @param maximum 145 | */ 146 | template 147 | inline kt_bool IsUpTo(const T& value, const T& maximum) 148 | { 149 | return (value >= 0 && value < maximum); 150 | } 151 | 152 | /** 153 | * Checks whether value is in the range [0;maximum) 154 | * Specialized version for unsigned int (kt_int32u) 155 | * @param value 156 | * @param maximum 157 | */ 158 | template<> 159 | inline kt_bool IsUpTo(const kt_int32u& value, const kt_int32u& maximum) 160 | { 161 | return (value < maximum); 162 | } 163 | 164 | 165 | /** 166 | * Checks whether value is in the range [a;b] 167 | * @param value 168 | * @param a 169 | * @param b 170 | */ 171 | template 172 | inline kt_bool InRange(const T& value, const T& a, const T& b) 173 | { 174 | return (value >= a && value <= b); 175 | } 176 | 177 | /** 178 | * Normalizes angle to be in the range of [-pi, pi] 179 | * @param angle to be normalized 180 | * @return normalized angle 181 | */ 182 | inline kt_double NormalizeAngle(kt_double angle) 183 | { 184 | while (angle < -KT_PI) 185 | { 186 | if (angle < -KT_2PI) 187 | { 188 | angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI; 189 | } 190 | else 191 | { 192 | angle += KT_2PI; 193 | } 194 | } 195 | 196 | while (angle > KT_PI) 197 | { 198 | if (angle > KT_2PI) 199 | { 200 | angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI; 201 | } 202 | else 203 | { 204 | angle -= KT_2PI; 205 | } 206 | } 207 | 208 | assert(math::InRange(angle, -KT_PI, KT_PI)); 209 | 210 | return angle; 211 | } 212 | 213 | /** 214 | * Returns an equivalent angle to the first parameter such that the difference 215 | * when the second parameter is subtracted from this new value is an angle 216 | * in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi. 217 | * @param minuend 218 | * @param subtrahend 219 | * @return normalized angle 220 | */ 221 | inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend) 222 | { 223 | while (minuend - subtrahend < -KT_PI) 224 | { 225 | minuend += KT_2PI; 226 | } 227 | 228 | while (minuend - subtrahend > KT_PI) 229 | { 230 | minuend -= KT_2PI; 231 | } 232 | 233 | return minuend; 234 | } 235 | 236 | /** 237 | * Align a value to the alignValue. 238 | * The alignValue should be the power of two (2, 4, 8, 16, 32 and so on) 239 | * @param value 240 | * @param alignValue 241 | * @return aligned value 242 | */ 243 | template 244 | inline T AlignValue(size_t value, size_t alignValue = 8) 245 | { 246 | return static_cast ((value + (alignValue - 1)) & ~(alignValue - 1)); 247 | } 248 | } // namespace math 249 | 250 | } // namespace karto 251 | 252 | #endif // OPEN_KARTO_MATH_H 253 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/csparse.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // 36 | // Interface to CSparse 37 | // 38 | 39 | #ifndef _CSPARSE_H_ 40 | #define _CSPARSE_H_ 41 | 42 | #ifndef EIGEN_USE_NEW_STDVECTOR 43 | #define EIGEN_USE_NEW_STDVECTOR 44 | #endif // EIGEN_USE_NEW_STDVECTOR 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | // CSparse header 54 | extern "C" { 55 | #include "suitesparse/cs.h" 56 | } 57 | #include 58 | #include 59 | 60 | // Cholmod header, other header files brought in 61 | #ifdef SBA_CHOLMOD 62 | #include "suitesparse/cholmod.h" 63 | #endif 64 | 65 | // these are for SparseLib and IML, testing the Delayed State Filter 66 | #ifdef SBA_DSIF 67 | #include "SparseLib/compcol_double.h" // Compressed column matrix header 68 | #include "SparseLib/mvblasd.h" // MV_Vector level 1 BLAS 69 | #include "SparseLib/icpre_double.h" // Diagonal preconditioner 70 | #include "SparseLib/cg.h" // IML++ CG template 71 | #endif 72 | 73 | // block jacobian PCG 74 | #include "sparse_bundle_adjustment/bpcg/bpcg.h" 75 | 76 | using namespace Eigen; 77 | using namespace std; 78 | 79 | namespace sba 80 | { 81 | class CSparse 82 | { 83 | public: 84 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 85 | 86 | // constructor 87 | CSparse(); 88 | 89 | // destructor 90 | ~CSparse(); 91 | 92 | // storage of diagonal blocks 93 | vector< Matrix, aligned_allocator > > diag; 94 | // compressed column storage of blocks 95 | vector< map, less, 96 | aligned_allocator > > > cols; 97 | 98 | void setupBlockStructure(int n); // size of rows/cols of A (in blocks) 99 | 100 | // add in blocks 101 | inline void addDiagBlock(Matrix &m, int n) 102 | { diag[n]+=m; }; 103 | void incDiagBlocks(double lam); 104 | void addOffdiagBlock(Matrix &m, int ii, int jj); 105 | 106 | // set up compressed column structure; true if first time 107 | // is the diagonal multiplier for LM 108 | void setupCSstructure(double diaginc, bool init=false); 109 | 110 | // write cs structure into a dense Eigen matrix 111 | void uncompress(MatrixXd &m); 112 | 113 | // parameters 114 | int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements) 115 | int nnz; // number of non-zeros in A 116 | 117 | // CSparse structures 118 | cs *A; // linear problem matrix 119 | 120 | // RHS Eigen vector 121 | VectorXd B; 122 | 123 | // which algorithm? 124 | bool useCholmod; 125 | 126 | // doing the Cholesky with CSparse or Cholmod 127 | bool doChol(); // solve in place with RHS B 128 | 129 | // doing the BPCG 130 | // max iterations , ending toleranace , current sba iteration 131 | int doBPCG(int iters, double tol, int sba_iter); 132 | // CG structure for 6x6 matrices 133 | jacobiBPCG<6> bpcg; 134 | 135 | #ifdef SBA_CHOLMOD 136 | // CHOLMOD structures 137 | bool chInited; 138 | cholmod_sparse *chA; // linear problem matrix 139 | cholmod_common *chc; 140 | cholmod_common Common; 141 | #endif 142 | 143 | }; 144 | 145 | 146 | class CSparse2d 147 | { 148 | public: 149 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 150 | 151 | // constructor 152 | CSparse2d(); 153 | 154 | // destructor 155 | ~CSparse2d(); 156 | 157 | // storage of diagonal blocks 158 | vector< Matrix, aligned_allocator > > diag; 159 | // compressed column storage of blocks 160 | vector< map, less, 161 | aligned_allocator > > > cols; 162 | 163 | void setupBlockStructure(int n, bool eraseit = true); // size of rows/cols of A (in blocks) 164 | 165 | // add in blocks 166 | inline void addDiagBlock(Matrix &m, int n) 167 | { diag[n]+=m; }; 168 | void addOffdiagBlock(Matrix &m, int ii, int jj); 169 | void incDiagBlocks(double lam); 170 | 171 | // set up compressed column structure; true if first time 172 | // is the diagonal multiplier for LM 173 | void setupCSstructure(double diaginc, bool init=false); 174 | 175 | // write cs structure into a dense Eigen matrix 176 | void uncompress(MatrixXd &m); 177 | 178 | // parameters 179 | int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements) 180 | int nnz; // number of non-zeros in A 181 | 182 | // CSparse2d structures 183 | cs *A, *AF; // linear problem matrices, A upper diagonal, AF symmetric 184 | 185 | // RHS Eigen vector 186 | VectorXd B, Bprev; 187 | 188 | // which algorithm? 189 | bool useCholmod; 190 | 191 | // doing the Cholesky 192 | bool doChol(); // solve in place with RHS B 193 | 194 | // doing PCG with incomplete Cholesky preconditioner 195 | // returns 0 on success, 1 on not achieving tolerance, >1 on other errors 196 | int doPCG(int iters); 197 | 198 | // doing the BPCG 199 | // max iterations , ending toleranace 200 | int doBPCG(int iters, double tol, int sba_iter); 201 | // CG structure for 3x3 matrices 202 | jacobiBPCG<3> bpcg; 203 | 204 | #ifdef SBA_CHOLMOD 205 | // CHOLMOD structures 206 | bool chInited; 207 | cholmod_sparse *chA; // linear problem matrix 208 | cholmod_common *chc; 209 | cholmod_common Common; 210 | #endif 211 | 212 | }; 213 | 214 | 215 | } // end namespace sba 216 | 217 | #endif // _CSPARSE_H 218 | -------------------------------------------------------------------------------- /open_karto/LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /open_karto/src/Karto.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 SRI International 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU Lesser General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU Lesser General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "open_karto/Karto.h" 27 | 28 | namespace karto 29 | { 30 | 31 | //////////////////////////////////////////////////////////////////////////////////////// 32 | //////////////////////////////////////////////////////////////////////////////////////// 33 | //////////////////////////////////////////////////////////////////////////////////////// 34 | 35 | SensorManager* SensorManager::GetInstance() 36 | { 37 | static Singleton sInstance; 38 | return sInstance.Get(); 39 | } 40 | 41 | //////////////////////////////////////////////////////////////////////////////////////// 42 | //////////////////////////////////////////////////////////////////////////////////////// 43 | //////////////////////////////////////////////////////////////////////////////////////// 44 | 45 | Object::Object() 46 | : m_pParameterManager(new ParameterManager()) 47 | { 48 | } 49 | 50 | Object::Object(const Name& rName) 51 | : m_Name(rName) 52 | , m_pParameterManager(new ParameterManager()) 53 | { 54 | } 55 | 56 | Object::~Object() 57 | { 58 | delete m_pParameterManager; 59 | m_pParameterManager = NULL; 60 | } 61 | 62 | //////////////////////////////////////////////////////////////////////////////////////// 63 | //////////////////////////////////////////////////////////////////////////////////////// 64 | //////////////////////////////////////////////////////////////////////////////////////// 65 | 66 | Module::Module(const std::string& rName) 67 | : Object(rName) 68 | { 69 | } 70 | 71 | Module::~Module() 72 | { 73 | } 74 | 75 | //////////////////////////////////////////////////////////////////////////////////////// 76 | //////////////////////////////////////////////////////////////////////////////////////// 77 | //////////////////////////////////////////////////////////////////////////////////////// 78 | 79 | Sensor::Sensor(const Name& rName) 80 | : Object(rName) 81 | { 82 | m_pOffsetPose = new Parameter("OffsetPose", Pose2(), GetParameterManager()); 83 | } 84 | 85 | Sensor::~Sensor() 86 | { 87 | } 88 | 89 | //////////////////////////////////////////////////////////////////////////////////////// 90 | //////////////////////////////////////////////////////////////////////////////////////// 91 | //////////////////////////////////////////////////////////////////////////////////////// 92 | 93 | SensorData::SensorData(const Name& rSensorName) 94 | : Object() 95 | , m_StateId(-1) 96 | , m_UniqueId(-1) 97 | , m_SensorName(rSensorName) 98 | , m_Time(0.0) 99 | { 100 | } 101 | 102 | SensorData::~SensorData() 103 | { 104 | forEach(CustomDataVector, &m_CustomData) 105 | { 106 | delete *iter; 107 | } 108 | 109 | m_CustomData.clear(); 110 | } 111 | 112 | //////////////////////////////////////////////////////////////////////////////////////// 113 | //////////////////////////////////////////////////////////////////////////////////////// 114 | //////////////////////////////////////////////////////////////////////////////////////// 115 | 116 | void CellUpdater::operator() (kt_int32u index) 117 | { 118 | kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer(); 119 | kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer(); 120 | kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer(); 121 | 122 | m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]); 123 | } 124 | 125 | //////////////////////////////////////////////////////////////////////////////////////// 126 | //////////////////////////////////////////////////////////////////////////////////////// 127 | //////////////////////////////////////////////////////////////////////////////////////// 128 | 129 | std::ostream& operator << (std::ostream& rStream, Exception& rException) 130 | { 131 | rStream << "Error detect: " << std::endl; 132 | rStream << " ==> error code: " << rException.GetErrorCode() << std::endl; 133 | rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl; 134 | 135 | return rStream; 136 | } 137 | 138 | //////////////////////////////////////////////////////////////////////////////////////// 139 | //////////////////////////////////////////////////////////////////////////////////////// 140 | //////////////////////////////////////////////////////////////////////////////////////// 141 | 142 | const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, 143 | CoordinateConverter* pCoordinateConverter, 144 | kt_bool ignoreThresholdPoints, 145 | kt_bool flipY) const 146 | { 147 | PointVectorDouble pointReadings; 148 | 149 | Pose2 scanPose = pLocalizedRangeScan->GetSensorPose(); 150 | 151 | // compute point readings 152 | kt_int32u beamNum = 0; 153 | kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings(); 154 | for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++) 155 | { 156 | kt_double rangeReading = pRangeReadings[i]; 157 | 158 | if (ignoreThresholdPoints) 159 | { 160 | if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold())) 161 | { 162 | continue; 163 | } 164 | } 165 | else 166 | { 167 | rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold()); 168 | } 169 | 170 | kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution(); 171 | 172 | Vector2 point; 173 | 174 | point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); 175 | point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); 176 | 177 | if (pCoordinateConverter != NULL) 178 | { 179 | Vector2 gridPoint = pCoordinateConverter->WorldToGrid(point, flipY); 180 | point.SetX(gridPoint.GetX()); 181 | point.SetY(gridPoint.GetY()); 182 | } 183 | 184 | pointReadings.push_back(point); 185 | } 186 | 187 | return pointReadings; 188 | } 189 | 190 | kt_bool LaserRangeFinder::Validate(SensorData* pSensorData) 191 | { 192 | LaserRangeScan* pLaserRangeScan = dynamic_cast(pSensorData); 193 | 194 | // verify number of range readings in LaserRangeScan matches the number of expected range readings 195 | if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings()) 196 | { 197 | std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings() 198 | << " range readings, expected " << GetNumberOfRangeReadings() << std::endl; 199 | return false; 200 | } 201 | 202 | return true; 203 | } 204 | 205 | //////////////////////////////////////////////////////////////////////////////////////// 206 | //////////////////////////////////////////////////////////////////////////////////////// 207 | //////////////////////////////////////////////////////////////////////////////////////// 208 | 209 | void ParameterManager::Clear() 210 | { 211 | forEach(karto::ParameterVector, &m_Parameters) 212 | { 213 | delete *iter; 214 | } 215 | 216 | m_Parameters.clear(); 217 | 218 | m_ParameterLookup.clear(); 219 | } 220 | 221 | void ParameterManager::Add(AbstractParameter* pParameter) 222 | { 223 | if (pParameter != NULL && pParameter->GetName() != "") 224 | { 225 | if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end()) 226 | { 227 | m_Parameters.push_back(pParameter); 228 | 229 | m_ParameterLookup[pParameter->GetName()] = pParameter; 230 | } 231 | else 232 | { 233 | m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString()); 234 | 235 | assert(false); 236 | } 237 | } 238 | } 239 | 240 | //////////////////////////////////////////////////////////////////////////////////////// 241 | //////////////////////////////////////////////////////////////////////////////////////// 242 | //////////////////////////////////////////////////////////////////////////////////////// 243 | 244 | /* 245 | std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] = 246 | { 247 | "Custom", 248 | 249 | "Sick_LMS100", 250 | "Sick_LMS200", 251 | "Sick_LMS291", 252 | 253 | "Hokuyo_UTM_30LX", 254 | "Hokuyo_URG_04LX" 255 | }; 256 | */ 257 | } // namespace karto 258 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/spa2d.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // 36 | // Sparse pose adjustment, 2d version 37 | // 38 | 39 | #ifndef _SPA2D_H_ 40 | #define _SPA2D_H_ 41 | 42 | #ifndef EIGEN_USE_NEW_STDVECTOR 43 | #define EIGEN_USE_NEW_STDVECTOR 44 | #endif // EIGEN_USE_NEW_STDVECTOR 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #ifndef EIGEN_USE_NEW_STDVECTOR 52 | #define EIGEN_USE_NEW_STDVECTOR 53 | #endif 54 | #include 55 | #include 56 | 57 | // sparse Cholesky 58 | #include 59 | // block jacobian pcg 60 | #include 61 | 62 | // Defines for methods to use with doSBA(). 63 | #define SBA_DENSE_CHOLESKY 0 64 | #define SBA_SPARSE_CHOLESKY 1 65 | #define SBA_GRADIENT 2 66 | #define SBA_BLOCK_JACOBIAN_PCG 3 67 | 68 | 69 | // put things into a namespace 70 | namespace sba 71 | { 72 | 73 | /// NODE2d holds graph nodes corresponding to frames, for use in 74 | /// sparse bundle adjustment 75 | 76 | /// type double must be or 77 | /// 78 | /// Each node has a 6DOF pose, encoded as a translation vector and 79 | /// rotation unit quaternion (Eigen classes). These represent the 80 | /// pose of the node in the world frame. 81 | /// 82 | /// The pose generates a 3x4 homogenous transform, taking a point 83 | /// in world coordinates into the node coordinates. 84 | /// 85 | /// Additionally a 3x4 homogenous transform is composed from the 86 | /// pose transform and a projection transform to the frame image 87 | /// coordinates. 88 | /// 89 | /// Projections from points to features are in a list. There 90 | /// should also be a reverse index from features to projections 91 | /// and points, so that matched features can tie in to points. 92 | /// 93 | 94 | class Node2d 95 | { 96 | public: 97 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 98 | 99 | /// node id - somewhat redundant, but can be useful, e.g., in KARTO links 100 | int nodeId; 101 | 102 | /// 6DOF pose as a unit quaternion and translation vector 103 | Eigen::Matrix trans; // homogeneous coordinates, last element is 1.0 104 | double arot; // angle in radians, normalized to [-pi,+pi] 105 | /// Normalize to [-pi,+pi] 106 | inline void normArot() 107 | { 108 | if (arot > M_PI) arot -= 2.0*M_PI; 109 | if (arot < -M_PI) arot += 2.0*M_PI; 110 | } 111 | 112 | /// Resultant transform from world to node coordinates; 113 | Eigen::Matrix w2n; 114 | void setTransform(); 115 | 116 | /// Covariance matrix, 3x3. Variables are [trans,rot], with the 117 | /// rotational part being the x parameter of the unit 118 | /// quaternion 119 | // Eigen::Matrix covar; 120 | 121 | /// Derivatives of the rotation matrix transpose wrt quaternion xyz, used for 122 | /// calculating Jacobian wrt pose of a projection. 123 | Eigen::Matrix2d dRdx; 124 | 125 | void setDr(); // set local angle derivatives 126 | 127 | /// For SPA, is this camera fixed or free? 128 | bool isFixed; 129 | 130 | /// 3DOF pose as a unit quaternion and translation vector, saving 131 | /// for LM step 132 | Eigen::Matrix oldtrans; // homogeneous coordinates, last element is 1.0 133 | double oldarot; // angle 134 | }; 135 | 136 | 137 | /// CONP2 holds a constraint measurement of a pose to a pose. 138 | /// They are a repository for links between poses within a frame, 139 | /// with aux info such as jacobians 140 | 141 | class Con2dP2 142 | { 143 | public: 144 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 145 | 146 | /// Reference pose index 147 | int ndr; 148 | 149 | /// Node2d index for the second node 150 | int nd1; 151 | 152 | /// Mean vector, quaternion (inverse) and precision matrix for this constraint 153 | Eigen::Vector2d tmean; 154 | double amean; 155 | Eigen::Matrix prec; 156 | 157 | /// error 158 | Eigen::Matrix err; 159 | /// calculates projection error and stores it in 160 | inline double calcErr(const Node2d &nd0, const Node2d &nd1); 161 | 162 | /// calculate error in distance only, no weighting 163 | double calcErrDist(const Node2d &nd0, const Node2d &nd1); 164 | 165 | 166 | /// jacobian with respect to frames; uses dR'/dq from Node2d calculation 167 | Eigen::Matrix J0,J0t,J1,J1t; 168 | 169 | /// scaling factor for quaternion derivatives relative to translational ones; 170 | /// not sure if this is needed, it's close to 1.0 171 | static constexpr double qScale = 1.0; 172 | 173 | /// dpc/dq = dR'/dq [pw-t], in homogeneous form, with q a quaternion param 174 | /// 175 | 176 | /// dpc/dx = -R' * [1 0 0]', in homogeneous form, with x a translation param 177 | /// 178 | 179 | /// d(px/pz)/du = [ pz dpx/du - px dpz/du ] / pz^2, 180 | /// works for all variables 181 | /// 182 | void setJacobians(std::vector > &nodes); 183 | 184 | /// valid or not (could be out of bounds) 185 | bool isValid; 186 | }; 187 | 188 | 189 | 190 | /// SysSPA2d holds a set of nodes and constraints for sparse pose adjustment 191 | 192 | class SysSPA2d 193 | { 194 | public: 195 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 196 | 197 | /// constructor 198 | SysSPA2d() { nFixed = 1; verbose = false; lambda = 1.0e-4, print_iros_stats=false; } 199 | 200 | /// add a node at a pose 201 | /// is x,y,th, with th in radians 202 | int addNode(const Vector3d &pos, int id); 203 | void removeNode(int id); 204 | 205 | // add a constraint 206 | // , are node id's 207 | // is x,y,th, with th in radians 208 | // is a 3x3 precision matrix (inverse covariance 209 | bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec); 210 | bool removeConstraint(int ndi0, int ndi1); 211 | 212 | 213 | /// set of nodes (camera frames) for SPA system, indexed by position; 214 | std::vector > nodes; 215 | std::vector > getNodes() 216 | { return nodes; } 217 | 218 | /// set of point scans, corresponding to nodes 219 | std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator > > scans; 220 | 221 | /// Number of fixed nodes 222 | int nFixed; 223 | 224 | /// Set of P2 constraints 225 | std::vector > p2cons; 226 | 227 | /// calculate the error in the system; 228 | /// if is true, just the distance error without weighting 229 | double calcCost(bool tcost = false); 230 | /// calculate error assuming outliers > dist 231 | double calcCost(double dist); 232 | double errcost; 233 | 234 | 235 | /// print some system stats 236 | void printStats(); 237 | void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format 238 | 239 | /// set up linear system, from RSS submission (konolige 2010) 240 | /// is the diagonal augmentation for the LM step 241 | void setupSys(double sLambda); 242 | void setupSparseSys(double sLambda, int iter, int sparseType); 243 | 244 | /// do LM solution for system; returns number of iterations on 245 | /// finish. Argument is max number of iterations to perform, 246 | /// initial diagonal augmentation, and sparse form of Cholesky. 247 | /// useCSParse = 0 for dense Cholesky, 1 for sparse Cholesky, 2 for BPCG 248 | double lambda; 249 | int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY, double initTol = 1.0e-8, int CGiters = 50); 250 | int doSPAwindowed(int window, int niter, double sLambda, int useCSparse); 251 | 252 | 253 | #ifdef SBA_DSIF 254 | /// Delayed Sparse Information Filter for comparison 255 | void doDSIF(int newnode); 256 | void setupSparseDSIF(int newnode); 257 | #endif 258 | 259 | /// Convergence bound (square of minimum acceptable delta change) 260 | double sqMinDelta; 261 | 262 | /// linear system matrix and vector 263 | Eigen::MatrixXd A; 264 | Eigen::VectorXd B; 265 | 266 | /// sparse matrix object 267 | CSparse2d csp; 268 | 269 | /// use CHOLMOD or CSparse 270 | void useCholmod(bool yes) 271 | { csp.useCholmod = yes; } 272 | 273 | /// if we want statistics 274 | bool verbose; 275 | bool print_iros_stats; 276 | 277 | /// return the graph of constraints 278 | /// x,y -> x',y' 4 floats per connection 279 | void getGraph(std::vector &graph); 280 | }; 281 | 282 | 283 | 284 | 285 | /// constraint files 286 | 287 | /// reads in a file of pose constraints 288 | bool read2dP2File(char *fname, SysSPA2d spa); 289 | 290 | 291 | } // ends namespace sba 292 | 293 | #endif // _SPA2d_H_ 294 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/src/proj.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace sba 4 | { 5 | Proj::Proj(int ci, Eigen::Vector3d &q, bool stereo) 6 | : ndi(ci), kp(q), stereo(stereo), 7 | isValid(true), useCovar(false), pointPlane(false) {} 8 | 9 | Proj::Proj(int ci, Eigen::Vector2d &q) 10 | : ndi(ci), kp(q(0), q(1), 0), 11 | stereo(false), isValid(true), useCovar(false), pointPlane(false) {} 12 | 13 | Proj::Proj() 14 | : ndi(0), kp(0, 0, 0), 15 | stereo(false), isValid(false), useCovar(false), pointPlane(false) {} 16 | 17 | void Proj::setJacobians(const Node &nd, const Point &pt, JacobProds *jpp) 18 | { 19 | if (stereo) 20 | setJacobiansStereo_(nd, pt, jpp); 21 | else 22 | setJacobiansMono_(nd, pt, jpp); 23 | } 24 | 25 | double Proj::calcErr(const Node &nd, const Point &pt, const double huber) 26 | { 27 | if (stereo) 28 | return calcErrStereo_(nd, pt, huber); 29 | else 30 | return calcErrMono_(nd, pt, huber); 31 | } 32 | 33 | double Proj::getErrNorm() 34 | { 35 | if (stereo) 36 | return err.norm(); 37 | else 38 | return err.head<2>().norm(); 39 | } 40 | 41 | double Proj::getErrSquaredNorm() 42 | { 43 | if (stereo) 44 | return err.squaredNorm(); 45 | else 46 | return err.head<2>().squaredNorm(); 47 | } 48 | 49 | void Proj::setCovariance(const Eigen::Matrix3d &covar) 50 | { 51 | useCovar = true; 52 | covarmat = covar; 53 | } 54 | 55 | void Proj::clearCovariance() 56 | { 57 | useCovar = false; 58 | } 59 | 60 | void Proj::setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp) 61 | { 62 | // first get the world point in camera coords 63 | Eigen::Matrix pc = nd.w2n * pt; 64 | 65 | /// jacobian with respect to frame; uses dR'/dq from Node calculation 66 | Eigen::Matrix jacc; 67 | 68 | /// jacobian with respect to point 69 | Eigen::Matrix jacp; 70 | 71 | // Jacobians wrt camera parameters 72 | // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 73 | double px = pc(0); 74 | double py = pc(1); 75 | double pz = pc(2); 76 | double ipz2 = 1.0/(pz*pz); 77 | if (std::isnan(ipz2) ) { printf("[SetJac] infinite jac\n"); *(int *)0x0 = 0; } 78 | 79 | double ipz2fx = ipz2*nd.Kcam(0,0); // Fx 80 | double ipz2fy = ipz2*nd.Kcam(1,1); // Fy 81 | // scale quaternion derivative to match the translational ones 82 | double ipz2fxq = qScale*ipz2fx; 83 | double ipz2fyq = qScale*ipz2fy; 84 | Eigen::Matrix pwt; 85 | 86 | // check for local vars 87 | pwt = (pt-nd.trans).head<3>(); // transform translations, use differential rotation 88 | 89 | // dx 90 | Eigen::Matrix dp = nd.dRdx * pwt; // dR'/dq * [pw - t] 91 | jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq; 92 | jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq; 93 | // dy 94 | dp = nd.dRdy * pwt; // dR'/dq * [pw - t] 95 | jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq; 96 | jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq; 97 | // dz 98 | dp = nd.dRdz * pwt; // dR'/dq * [pw - t] 99 | jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq; 100 | jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq; 101 | 102 | // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 103 | dp = -nd.w2n.col(0); // dpc / dx 104 | jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 105 | jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 106 | dp = -nd.w2n.col(1); // dpc / dy 107 | jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 108 | jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 109 | dp = -nd.w2n.col(2); // dpc / dz 110 | jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 111 | jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 112 | 113 | // Jacobians wrt point parameters 114 | // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 115 | dp = nd.w2n.col(0); // dpc / dx 116 | jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 117 | jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 118 | dp = nd.w2n.col(1); // dpc / dy 119 | jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 120 | jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 121 | dp = nd.w2n.col(2); // dpc / dz 122 | jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 123 | jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 124 | 125 | #ifdef DEBUG 126 | for (int i=0; i<2; i++) 127 | for (int j=0; j<6; j++) 128 | if (std::isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j); *(int *)0x0 = 0; } 129 | #endif 130 | 131 | // Set Hessians + extras. 132 | jpp->Hpp = jacp.transpose() * jacp; 133 | jpp->Hcc = jacc.transpose() * jacc; 134 | jpp->Hpc = jacp.transpose() * jacc; 135 | jpp->JcTE = jacc.transpose() * err.head<2>(); 136 | jpp->Bp = jacp.transpose() * err.head<2>(); 137 | 138 | jp = jpp; 139 | } 140 | 141 | // calculate error of a projection 142 | // we should do something about negative Z 143 | double Proj::calcErrMono_(const Node &nd, const Point &pt, double huber) 144 | { 145 | Eigen::Vector3d p1 = nd.w2i * pt; 146 | err(2) = 0.0; 147 | if (p1(2) <= 0.0) 148 | { 149 | #ifdef DEBUG 150 | printf("[CalcErr] negative Z! Node %d \n",ndi); 151 | if (std::isnan(err[0]) || std::isnan(err[1]) ) printf("[CalcErr] NaN!\n"); 152 | #endif 153 | err = Eigen::Vector3d(0.0,0.0,0.0); 154 | return 0.0; 155 | } 156 | else 157 | err.head<2>() = p1.head<2>()/p1(2); 158 | 159 | err -= kp; 160 | 161 | // pseudo-Huber weighting 162 | // C(e) = 2*s^2*[sqrt(1+(e/s)^2)-1] 163 | // w = sqrt(C(norm(e)))/norm(e) 164 | 165 | if (huber > 0) 166 | { 167 | double b2 = huber*huber; // kernel width 168 | double e2 = err.head<2>().squaredNorm(); 169 | if (e2 > b2) 170 | { 171 | double c = 2.0*huber*sqrt(e2) - b2; 172 | double w = sqrt(c/e2); 173 | err.head<2>() *= w; // weight the error 174 | // std::cout << "Huber weight: " << w << " Err sq: " << e2 << std::endl; 175 | } 176 | 177 | 178 | // double b2 = HUBER*HUBER; // kernel width 179 | // double e2 = err.squaredNorm(); 180 | // e2 = std::max(e2,1e-22); // can't have a zero here 181 | // double w = 2*b2*(sqrt(1+e2/b2)-1); 182 | // w = sqrt(w/e2); 183 | // err.head<2>() *= w; // weight the error 184 | } 185 | 186 | return err.head<2>().squaredNorm(); 187 | } 188 | 189 | 190 | void Proj::setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp) 191 | { 192 | // first get the world point in camera coords 193 | Eigen::Matrix pc = nd.w2n * pt; 194 | 195 | /// jacobian with respect to point 196 | Eigen::Matrix jacp; 197 | 198 | /// jacobian with respect to frame; uses dR'/dq from Node calculation 199 | Eigen::Matrix jacc; 200 | 201 | // Jacobians wrt camera parameters 202 | // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 203 | double px = pc(0); 204 | double py = pc(1); 205 | double pz = pc(2); 206 | double ipz2 = 1.0/(pz*pz); 207 | if (std::isnan(ipz2) ) { printf("[SetJac] infinite jac\n"); *(int *)0x0 = 0; } 208 | 209 | double ipz2fx = ipz2*nd.Kcam(0,0); // Fx 210 | double ipz2fy = ipz2*nd.Kcam(1,1); // Fy 211 | double b = nd.baseline; // stereo baseline 212 | // scale quaternion derivative to match the translational ones 213 | double ipz2fxq = qScale*ipz2fx; 214 | double ipz2fyq = qScale*ipz2fy; 215 | Eigen::Matrix pwt; 216 | 217 | // check for local vars 218 | pwt = (pt-nd.trans).head(3); // transform translations, use differential rotation 219 | 220 | // dx 221 | Eigen::Matrix dp = nd.dRdx * pwt; // dR'/dq * [pw - t] 222 | jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq; 223 | jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq; 224 | jacc(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 225 | // dy 226 | dp = nd.dRdy * pwt; // dR'/dq * [pw - t] 227 | jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq; 228 | jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq; 229 | jacc(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 230 | // dz 231 | dp = nd.dRdz * pwt; // dR'/dq * [pw - t] 232 | jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq; 233 | jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq; 234 | jacc(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 235 | 236 | // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 237 | dp = -nd.w2n.col(0); // dpc / dx 238 | jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq; 239 | jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 240 | jacc(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 241 | dp = -nd.w2n.col(1); // dpc / dy 242 | jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq; 243 | jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 244 | jacc(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 245 | dp = -nd.w2n.col(2); // dpc / dz 246 | jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq; 247 | jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 248 | jacc(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 249 | 250 | // Jacobians wrt point parameters 251 | // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 252 | dp = nd.w2n.col(0); // dpc / dx 253 | jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq; 254 | jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 255 | jacp(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 256 | dp = nd.w2n.col(1); // dpc / dy 257 | jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq; 258 | jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 259 | jacp(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 260 | dp = nd.w2n.col(2); // dpc / dz 261 | jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq; 262 | jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 263 | jacp(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px 264 | 265 | #ifdef DEBUG 266 | for (int i=0; i<2; i++) 267 | for (int j=0; j<6; j++) 268 | if (std::isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j); *(int *)0x0 = 0; } 269 | #endif 270 | if (useCovar) 271 | { 272 | jacc = covarmat * jacc; 273 | jacp = covarmat * jacp; 274 | } 275 | 276 | // Set Hessians + extras. 277 | jpp->Hpp = jacp.transpose() * jacp; 278 | jpp->Hcc = jacc.transpose() * jacc; 279 | jpp->Hpc = jacp.transpose() * jacc; 280 | jpp->JcTE = jacc.transpose() * err; 281 | jpp->Bp = jacp.transpose() * err; 282 | 283 | jp = jpp; 284 | } 285 | 286 | // calculate error of a projection 287 | // we should do something about negative Z 288 | double Proj::calcErrStereo_(const Node &nd, const Point &pt, double huber) 289 | { 290 | Eigen::Vector3d p1 = nd.w2i * pt; 291 | Eigen::Vector3d p2 = nd.w2n * pt; 292 | Eigen::Vector3d pb(nd.baseline,0,0); 293 | 294 | // TODO: Clean this up a bit. 295 | if (pointPlane) 296 | { 297 | // Project point onto plane. 298 | Eigen::Vector3d w = pt.head<3>()-plane_point; 299 | 300 | //printf("w: %f %f %f\n", w.x(), w.y(), w.z()); 301 | //Eigen::Vector3d projpt = pt.head<3>()+(w.dot(plane_normal))*plane_normal; 302 | Eigen::Vector3d projpt = plane_point+(w.dot(plane_normal))*plane_normal; 303 | // Eigen::Vector3d projpt = pt.head<3>()+(w.dot(plane_normal))*plane_normal; 304 | //printf("[Proj] Distance to plane: %f\n", w.dot(plane_normal)); 305 | p1 = nd.w2i*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0); 306 | p2 = nd.w2n*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0); 307 | } 308 | 309 | double invp1 = 1.0/p1(2); 310 | 311 | err.head<2>() = p1.head<2>()*invp1; 312 | // right camera px 313 | p2 = nd.Kcam*(p2-pb); 314 | 315 | err(2) = p2(0)/p2(2); 316 | if (p1(2) <= 0.0) 317 | { 318 | #ifdef DEBUG 319 | printf("[CalcErr] negative Z! Node %d\n",ndi); 320 | if (std::isnan(err[0]) || std::isnan(err[1]) ) printf("[CalcErr] NaN!\n"); 321 | #endif 322 | err.setZero(); 323 | 324 | return 0.0; 325 | } 326 | err -= kp; 327 | 328 | if (abs(err(0)) > 1e6 || abs(err(1)) > 1e6 || abs(err(2)) > 1e6) 329 | { 330 | printf("\n\n[CalcErr] Excessive error.\n"); 331 | 332 | isValid = false; 333 | } 334 | 335 | if (useCovar) 336 | err = covarmat*err; 337 | 338 | // Huber kernel weighting 339 | if (huber > 0.0) 340 | { 341 | double b2 = huber*huber; // kernel width 342 | double e2 = err.squaredNorm(); 343 | if (e2 > b2) 344 | { 345 | double c = 2.0*huber*sqrt(e2) - b2; 346 | double w = sqrt(c/e2); 347 | err *= w; 348 | // std::cout << "Huber weight: " << w << " Err sq: " << e2 << std::endl; 349 | } 350 | } 351 | 352 | return err.squaredNorm(); 353 | } 354 | 355 | // Constructors for track. 356 | Track::Track() : point() { } 357 | Track::Track(Point p) : point(p) { } 358 | 359 | } // sba 360 | 361 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/bpcg/bpcg.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // 36 | // block preconditioned conjugate gradient 37 | // 6x6 blocks at present, should be templatized 38 | // 39 | 40 | #ifndef _BPCG_H_ 41 | #define _BPCG_H_ 42 | 43 | #ifndef EIGEN_USE_NEW_STDVECTOR 44 | #define EIGEN_USE_NEW_STDVECTOR 45 | #endif // EIGEN_USE_NEW_STDVECTOR 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | using namespace Eigen; 55 | using namespace std; 56 | 57 | //typedef Matrix Vector6d; 58 | //typedef Vector6d::AlignedMapType AV6d; 59 | 60 | namespace sba 61 | { 62 | /// Let's try templated versions 63 | 64 | template 65 | class jacobiBPCG 66 | { 67 | public: 68 | jacobiBPCG() { residual = 0.0; }; 69 | int doBPCG(int iters, double tol, 70 | vector< Matrix, aligned_allocator > > &diag, 71 | vector< map, less, aligned_allocator > > > &cols, 72 | VectorXd &x, 73 | VectorXd &b, 74 | bool abstol = false, 75 | bool verbose = false 76 | ); 77 | 78 | // uses internal linear storage for Hessian 79 | int doBPCG2(int iters, double tol, 80 | vector< Matrix, aligned_allocator > > &diag, 81 | vector< map, less, aligned_allocator > > > &cols, 82 | VectorXd &x, 83 | VectorXd &b, 84 | bool abstol = false, 85 | bool verbose = false 86 | ); 87 | 88 | double residual; 89 | 90 | private: 91 | void mMV(vector< Matrix, aligned_allocator > > &diag, 92 | vector< map, less, aligned_allocator > > > &cols, 93 | const VectorXd &vin, 94 | VectorXd &vout); 95 | 96 | // uses internal linear storage 97 | void mMV2(vector< Matrix, aligned_allocator > > &diag, 98 | const VectorXd &vin, 99 | VectorXd &vout); 100 | 101 | void mD(vector< Matrix, aligned_allocator > > &diag, 102 | VectorXd &vin, 103 | VectorXd &vout); 104 | 105 | vector vcind, vrind; 106 | vector< Matrix, aligned_allocator > > vcols; 107 | }; 108 | 109 | template 110 | void jacobiBPCG::mMV(vector< Matrix, aligned_allocator > > &diag, 111 | vector< map, less, aligned_allocator > > > &cols, 112 | const VectorXd &vin, 113 | VectorXd &vout) 114 | { 115 | // loop over all entries 116 | if (cols.size() > 0) 117 | for (int i=0; i<(int)cols.size(); i++) 118 | { 119 | vout.segment(i*N) = diag[i]*vin.segment(i*N); // only works with cols ordering 120 | 121 | map, less, 122 | aligned_allocator > > &col = cols[i]; 123 | if (col.size() > 0) 124 | { 125 | typename map, less, // need "typename" here, barf 126 | aligned_allocator > >::iterator it; 127 | for (it = col.begin(); it != col.end(); it++) 128 | { 129 | int ri = (*it).first; // get row index 130 | const Matrix &M = (*it).second; // matrix 131 | vout.segment(i*N) += M.transpose()*vin.segment(ri*N); 132 | vout.segment(ri*N) += M*vin.segment(i*N); 133 | } 134 | } 135 | } 136 | } 137 | 138 | // 139 | // matrix-vector multiply with linear storage 140 | // 141 | 142 | template 143 | void jacobiBPCG::mMV2(vector< Matrix, aligned_allocator > > &diag, 144 | const VectorXd &vin, 145 | VectorXd &vout) 146 | { 147 | // linear storage for matrices 148 | // loop over off-diag entries 149 | if (diag.size() > 0) 150 | for (int i=0; i<(int)diag.size(); i++) 151 | vout.segment(i*N) = diag[i]*vin.segment(i*N); // only works with cols ordering 152 | 153 | for (int i=0; i<(int)vcind.size(); i++) 154 | { 155 | int ri = vrind[i]; 156 | int ii = vcind[i]; 157 | const Matrix &M = vcols[i]; 158 | vout.segment(ri*N) += M*vin.segment(ii*N); 159 | vout.segment(ii*N) += M.transpose()*vin.segment(ri*N); 160 | } 161 | } 162 | 163 | 164 | 165 | 166 | template 167 | void jacobiBPCG::mD(vector< Matrix, aligned_allocator > > &diag, 168 | VectorXd &vin, 169 | VectorXd &vout) 170 | { 171 | // loop over diag entries 172 | for (int i=0; i<(int)diag.size(); i++) 173 | vout.segment(i*N) = diag[i]*vin.segment(i*N); 174 | } 175 | 176 | 177 | template 178 | int jacobiBPCG::doBPCG(int iters, double tol, 179 | vector< Matrix, aligned_allocator > > &diag, 180 | vector< map, less, aligned_allocator > > > &cols, 181 | VectorXd &x, 182 | VectorXd &b, 183 | bool abstol, 184 | bool verbose) 185 | { 186 | // set up local vars 187 | VectorXd r,d,q,s; 188 | int n = diag.size(); 189 | int n6 = n*N; 190 | r.setZero(n6); 191 | d.setZero(n6); 192 | q.setZero(n6); 193 | s.setZero(n6); 194 | 195 | // set up Jacobi preconditioner 196 | vector< Matrix, aligned_allocator > > J; 197 | J.resize(n); 198 | for (int i=0; i d0) d0 = residual; 209 | } 210 | 211 | for (i=0; i 238 | int jacobiBPCG::doBPCG2(int iters, double tol, 239 | vector< Matrix, aligned_allocator > > &diag, 240 | vector< map, less, aligned_allocator > > > &cols, 241 | VectorXd &x, 242 | VectorXd &b, 243 | bool abstol, 244 | bool verbose) 245 | { 246 | // set up local vars 247 | VectorXd r,d,q,s; 248 | int n = diag.size(); 249 | int n6 = n*N; 250 | r.setZero(n6); 251 | d.setZero(n6); 252 | q.setZero(n6); 253 | s.setZero(n6); 254 | 255 | vcind.clear(); 256 | vrind.clear(); 257 | vcols.clear(); 258 | 259 | // set up alternate rep for sparse matrix 260 | for (int i=0; i<(int)cols.size(); i++) 261 | { 262 | map, less, 263 | aligned_allocator > > &col = cols[i]; 264 | if (col.size() > 0) 265 | { 266 | typename map, less, 267 | aligned_allocator > >::iterator it; 268 | for (it = col.begin(); it != col.end(); it++) 269 | { 270 | int ri = (*it).first; // get row index 271 | vrind.push_back(ri); 272 | vcind.push_back(i); 273 | vcols.push_back((*it).second); 274 | } 275 | } 276 | } 277 | 278 | // set up Jacobi preconditioner 279 | vector< Matrix, aligned_allocator > > J; 280 | J.resize(n); 281 | for (int i=0; i d0) d0 = residual; 292 | } 293 | 294 | for (i=0; i, aligned_allocator > > &diag, 326 | vector< map, less, aligned_allocator > > > &cols, 327 | const VectorXd &vin, 328 | VectorXd &vout); 329 | 330 | // 331 | // jacobi-preconditioned block conjugate gradient 332 | // returns number of iterations 333 | 334 | int 335 | bpcg_jacobi(int iters, double tol, 336 | vector< Matrix, aligned_allocator > > &diag, 337 | vector< map, less, aligned_allocator > > > &cols, 338 | VectorXd &x, 339 | VectorXd &b, 340 | bool abstol = false, 341 | bool verbose = false 342 | ); 343 | 344 | int 345 | bpcg_jacobi_dense(int iters, double tol, 346 | MatrixXd &M, 347 | VectorXd &x, 348 | VectorXd &b); 349 | 350 | // 351 | // jacobi-preconditioned block conjugate gradient 352 | // returns number of iterations 353 | 354 | int 355 | bpcg_jacobi3(int iters, double tol, 356 | vector< Matrix, aligned_allocator > > &diag, 357 | vector< map, less, aligned_allocator > > > &cols, 358 | VectorXd &x, 359 | VectorXd &b, 360 | bool abstol = false, 361 | bool verbose = false 362 | ); 363 | 364 | int 365 | bpcg_jacobi_dense3(int iters, double tol, 366 | MatrixXd &M, 367 | VectorXd &x, 368 | VectorXd &b); 369 | 370 | #endif 371 | 372 | } // end namespace sba 373 | 374 | #endif // BPCG_H 375 | -------------------------------------------------------------------------------- /sparse_bundle_adjustment/include/sparse_bundle_adjustment/sba.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // 36 | // Sparse bundle/pose adjustment classes and functions 37 | // 38 | 39 | #ifndef _SBA_H_ 40 | #define _SBA_H_ 41 | 42 | #ifndef EIGEN_USE_NEW_STDVECTOR 43 | #define EIGEN_USE_NEW_STDVECTOR 44 | #endif // EIGEN_USE_NEW_STDVECTOR 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | #include 57 | #include 58 | 59 | 60 | // sparse Cholesky 61 | #include 62 | // block jacobian pcg 63 | #include 64 | 65 | // Defines for methods to use with doSBA(). 66 | #define SBA_DENSE_CHOLESKY 0 67 | #define SBA_SPARSE_CHOLESKY 1 68 | #define SBA_GRADIENT 2 69 | #define SBA_BLOCK_JACOBIAN_PCG 3 70 | 71 | namespace sba 72 | { 73 | /// SysSBA holds a set of nodes and points for sparse bundle adjustment 74 | 75 | class SysSBA 76 | { 77 | public: 78 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 79 | 80 | /// \brief How much information to print to console. 81 | int verbose; 82 | long long t0, t1, t2, t3, t4; // save timing 83 | 84 | /// \brief Default constructor. 85 | SysSBA() { nFixed = 1; useLocalAngles = true; Node::initDr(); 86 | verbose = 1; huber = 0.0; } 87 | 88 | /// \brief Set of nodes (camera frames) for SBA system, indexed by node number. 89 | std::vector > nodes; 90 | 91 | /// \brief Number of fixed nodes (nodes with known poses) from the first node. 92 | int nFixed; 93 | 94 | /// \brief Set of tracks for each point P (projections onto camera frames). 95 | std::vector > tracks; 96 | 97 | /// \brief Return total number of projections 98 | int countProjs(); 99 | 100 | /// \brief Calculate the total cost of the system by adding the squared 101 | /// error over all projections. 102 | /// \return Total error of the system, in pixels^2. 103 | double calcCost(); 104 | 105 | /// \brief Calculates total cost of the system, not counting projections 106 | /// with errors higher than . 107 | /// \param dist Distance, in pixels^2, above which projections are 108 | /// considered outliers. 109 | /// \return Total error of the system, in pixels^2. 110 | double calcCost(double dist); 111 | 112 | /// \brief Huber parameter; greater than 0.0 for Huber weighting of cost 113 | double huber; 114 | 115 | /// \brief Calculates total RMS cost of the system, not counting 116 | /// projections with errors higher than . 117 | /// \param dist RMS distance, in pixels, above which projections are 118 | /// considered outliers. 119 | /// \return Total RMS error of the system, in pixels. 120 | double calcRMSCost(double dist = 10000.0); 121 | 122 | /// \brief Calculates average cost of the system. 123 | /// \returns Average error of the system over all projections, in pixels. 124 | double calcAvgError(); 125 | 126 | /// \brief Find number of points with Z < 0 (projected behind the camera). 127 | int numBadPoints(); 128 | 129 | /// \brief Find number of projections with errors over a certain value. 130 | int countBad(double dist); 131 | 132 | /// \brief Remove projections with too high of an error. 133 | /// \return Number of projections removed. 134 | int removeBad(double dist); 135 | 136 | /// \brief Reduce tracks by eliminating single tracks and invalid 137 | /// projections. 138 | int reduceTracks(); 139 | 140 | /// \brief Print some system stats. 141 | void printStats(); 142 | 143 | /// local or global angle coordinates 144 | bool useLocalAngles; 145 | 146 | /// set up linear system, from Engels and Nister 2006; 147 | /// is the diagonal augmentation for the LM step 148 | double lambda; // save for continuation 149 | void setupSys(double sLambda); 150 | void setupSparseSys(double sLambda, int iter, int sparseType); 151 | 152 | /// do LM solution for system; returns number of iterations on 153 | /// finish. Argument is max number of iterations to perform. 154 | /// is the LM diagonal factor 155 | /// is one of 156 | /// SBA_DENSE_CHOLESKY, SBA_SPARSE_CHOLESKY, SBA_GRADIENT, SBA_BLOCK_JACOBIAN_PCG 157 | /// initTol is the initial tolerance for CG iterations 158 | int doSBA(int niter, double lambda = 1.0e-4, int useCSparse = 0, double initTol = 1.0e-8, 159 | int maxCGiters = 100); 160 | 161 | /// Convergence bound (square of minimum acceptable delta change) 162 | double sqMinDelta; 163 | 164 | /// \brief Adds a node to the system. 165 | /// \param trans A 4x1 vector of translation of the camera. 166 | /// \param qrot A Quaternion containing the rotatin of the camera. 167 | /// \param cp Camera parameters containing the K parameters, including 168 | /// baseline and other camera parameters. 169 | /// \param isFixed Whether this camera is fixed in space or not, for sba. 170 | /// \return the index of the node added. 171 | int addNode(Eigen::Matrix &trans, 172 | Eigen::Quaternion &qrot, 173 | const fc::CamParams &cp, 174 | bool isFixed = false); 175 | 176 | /// \brief Adds a point to the system. 177 | /// \param p A point (4x1 Eigen Vector). 178 | /// \return the index of the point added. 179 | int addPoint(Point p); 180 | 181 | /// \brief Add a projection between point and camera, in setting up the 182 | /// system. 183 | /// \param ci camera/node index (same as in nodes structure). 184 | /// \param pi point index (same as in tracks structure). 185 | /// \param q the keypoint of the projection in image coordinates as u,v,u-d. 186 | /// \param stereo whether the point is stereo or not (true is stereo, 187 | /// false is monocular). 188 | /// \return whether the projection was added (true) or not (false). 189 | /// Should only fail if the projection is a duplicate of an existing one 190 | /// with a different keypoint (i.e., same point projected to 2 locations 191 | /// in an image). 192 | bool addProj(int ci, int pi, Eigen::Vector3d &q, bool stereo=true); 193 | 194 | /// \brief Add a projection between point and camera, in setting up the 195 | /// system. 196 | /// \param ci camera/node index (same as in nodes structure). 197 | /// \param pi point index (same as in tracks structure). 198 | /// \param q the keypoint of the projection in image coordinates as u,v,u-d. 199 | /// \return whether the projection was added (true) or not (false). 200 | /// Should only fail if the projection is a duplicate of an existing one 201 | /// with a different keypoint (i.e., same point projected to 2 locations 202 | /// in an image). 203 | bool addMonoProj(int ci, int pi, Eigen::Vector2d &q); 204 | 205 | /// \brief Add a projection between point and camera, in setting up the 206 | /// system. 207 | /// \param ci camera/node index (same as in nodes structure). 208 | /// \param pi point index (same as in tracks structure). 209 | /// \param q the keypoint of the projection in image coordinates as u,v,u-d. 210 | /// \return whether the projection was added (true) or not (false). 211 | /// Should only fail if the projection is a duplicate of an existing one 212 | /// with a different keypoint (i.e., same point projected to 2 locations 213 | /// in an image). 214 | bool addStereoProj(int ci, int pi, Eigen::Vector3d &q); 215 | 216 | /// \brief Sets the covariance matrix of a projection. 217 | /// \param ci camera/node index (same as in nodes structure). 218 | /// \param pi point index (same as in tracks structure). 219 | /// \param covar 3x3 covariance matrix that affects the cost of the 220 | /// projection. Instead of the cost being ||err||, the cost is now 221 | /// (err)T*covar*(err). 222 | void setProjCovariance(int ci, int pi, Eigen::Matrix3d &covar); 223 | 224 | /// \brief Adds a pair of point-plane projection matches. 225 | /// Assumes the points have already been added to the system with 226 | /// point-to-point projections; this just takes care of the forward and 227 | /// backward point-to-plane projections. 228 | /// \param ci0 Camera index of the first point in the match. 229 | /// \param pi0 Point index of the first point in the match. 230 | /// \param normal0 3D normal for the first point in camera0's coordinate frame. 231 | /// \param ci1 Camera index of the second point in the match. 232 | /// \param pi1 Point index of the second point in the match. 233 | /// \param normal1 3D normal for the second point in camera1's coordinate frame. 234 | void addPointPlaneMatch(int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1); 235 | 236 | /// \brief Update normals in point-plane matches, if any. 237 | void updateNormals(); 238 | 239 | /// linear system matrix and vector 240 | Eigen::MatrixXd A; 241 | Eigen::VectorXd B; 242 | 243 | /// sparse connectivity matrix 244 | /// for each node, holds vector of connecting nodes 245 | /// "true" for don't use connection 246 | std::vector > connMat; 247 | 248 | /// Sets up the connectivity matrix by clearing connections with 249 | /// less than minpts. 250 | void setConnMat(int minpts); 251 | /// sets up connectivity matrix by greedy spanning tree 252 | void setConnMatReduced(int maxconns); 253 | /// removes tracks that aren't needed 254 | int remExcessTracks(int minpts); 255 | 256 | /// get rid of long tracks 257 | int reduceLongTracks(double pct); 258 | 259 | /// merge tracks based on identity pairs 260 | void mergeTracks(std::vector > &prs); 261 | /// merge two tracks if possible (no conflicts) 262 | int mergeTracksSt(int tr0, int tr1); 263 | 264 | /// use CHOLMOD or CSparse 265 | void useCholmod(bool yes) 266 | { csp.useCholmod = yes; } 267 | 268 | /// sparse matrix object 269 | /// putting this at the end gets rid of alignment errors when making SBA objects 270 | CSparse csp; 271 | 272 | // Private helper functions 273 | protected: 274 | /// Generate a connections map, used for setConnMat() and 275 | /// setConnMatReduced(). 276 | vector > generateConns_(); 277 | 278 | /// Split a track into random tracks. (What is len?) 279 | void tsplit(int tri, int len); 280 | 281 | /// Storage for old points, for checking LM step and reverting 282 | std::vector > oldpoints; 283 | 284 | /// variables for each track 285 | std::vector > tps; 286 | 287 | /// storage for Jacobian products 288 | std::vector > jps; 289 | 290 | }; 291 | 292 | 293 | /// CONP2 holds a constraint measurement of a pose to a pose. 294 | /// They are a repository for links between poses within a frame, 295 | /// with aux info such as jacobians 296 | 297 | class ConP2 298 | { 299 | public: 300 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 301 | 302 | /// Reference pose index 303 | int ndr; 304 | 305 | /// Node index for the second node 306 | int nd1; 307 | 308 | /// Mean vector, quaternion (inverse) and precision matrix for this constraint 309 | Eigen::Vector3d tmean; 310 | Eigen::Quaternion qpmean; 311 | Eigen::Matrix prec; 312 | 313 | 314 | /// error 315 | Eigen::Matrix err; 316 | /// calculates projection error and stores it in 317 | inline double calcErr(const Node &nd0, const Node &nd1); 318 | 319 | /// calculate error in distance only, no weighting 320 | double calcErrDist(const Node &nd0, const Node &nd1); 321 | 322 | 323 | /// jacobian with respect to frames; uses dR'/dq from Node calculation 324 | Eigen::Matrix J0,J0t,J1,J1t; 325 | 326 | /// scaling factor for quaternion derivatives relative to translational ones; 327 | /// not sure if this is needed, it's close to 1.0 328 | static constexpr double qScale = 1.0; 329 | 330 | /// dpc/dq = dR'/dq [pw-t], in homogeneous form, with q a quaternion param 331 | /// 332 | 333 | /// dpc/dx = -R' * [1 0 0]', in homogeneous form, with x a translation param 334 | /// 335 | 336 | /// d(px/pz)/du = [ pz dpx/du - px dpz/du ] / pz^2, 337 | /// works for all variables 338 | /// 339 | void setJacobians(std::vector > &nodes); 340 | 341 | /// valid or not (could be out of bounds) 342 | bool isValid; 343 | }; 344 | 345 | 346 | /// CONSCALE holds a constraint measurement on the scale of 347 | /// a pose-pose constraint, in a scale group 348 | 349 | class ConScale 350 | { 351 | public: 352 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 353 | 354 | /// Reference pose index 355 | int nd0; 356 | 357 | /// Node index for the second node 358 | int nd1; 359 | 360 | /// Scale variable index 361 | int sv; 362 | 363 | /// Scale factor for this constraint 364 | double ks; 365 | 366 | /// Weight for this constraint 367 | double w; 368 | 369 | /// error 370 | double err; 371 | /// calculates projection error and stores it in 372 | inline double calcErr(const Node &nd0, const Node &nd1, double alpha); 373 | 374 | /// jacobian with respect to reference frame: 375 | /// -2(t1 - t0) 376 | Eigen::Vector3d J0; 377 | 378 | /// jacobian with respect to second frame: 379 | /// 2(t1 - t0) 380 | Eigen::Vector3d J1; 381 | 382 | /// jacobians are computed from (ti - tj)^2 - a*kij = 0 383 | void setJacobians(std::vector > &nodes); 384 | 385 | /// valid or not (could be out of bounds) 386 | bool isValid; 387 | }; 388 | 389 | 390 | /// CONP3P holds a constraint measurement between 3 poses. 391 | /// They are a repository for links between poses within a frame, 392 | /// with aux info such as jacobians 393 | 394 | class ConP3P 395 | { 396 | public: 397 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 398 | 399 | /// Reference pose index 400 | int ndr; 401 | 402 | /// Node indices, the constraint for this object. 403 | int nd1, nd2; 404 | 405 | /// Mean vector and precision matrix for this constraint 406 | Eigen::Matrix mean; 407 | Eigen::Matrix prec; 408 | 409 | /// error 410 | Eigen::Matrix err; 411 | /// calculates projection error and stores it in 412 | Eigen::Matrix calcErr(const Node &nd, const Point &pt); 413 | 414 | /// Jacobians of 0p1,0p2 with respect to global p0, p1, p2 415 | Eigen::Matrix J10, J11, J20, J22; 416 | 417 | /// dpc/dq = dR'/dq [pw-t], in homogeneous form, with q a quaternion param 418 | /// 419 | 420 | /// dpc/dx = -R' * [1 0 0]', in homogeneous form, with x a translation param 421 | /// 422 | 423 | /// d(px/pz)/du = [ pz dpx/du - px dpz/du ] / pz^2, 424 | /// works for all variables 425 | /// 426 | void setJacobians(std::vector > nodes); 427 | 428 | /// temp storage for Hpc, Tpc matrices in SBA 429 | Eigen::Matrix Hpc; 430 | Eigen::Matrix Tpc; 431 | 432 | /// valid or not (could be out of bounds) 433 | bool isValid; 434 | }; 435 | 436 | 437 | 438 | /// SysSPA holds a set of nodes and constraints for sparse pose adjustment 439 | 440 | class SysSPA 441 | { 442 | public: 443 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 444 | 445 | /// constructor 446 | SysSPA() { nFixed = 1; useLocalAngles = true; Node::initDr(); lambda = 1.0e-4; 447 | verbose = false;} 448 | 449 | /// print info 450 | bool verbose; 451 | 452 | /// \brief Adds a node to the system. 453 | /// \param trans A 4x1 vector of translation of the camera. 454 | /// \param qrot A Quaternion containing the rotatin of the camera. 455 | /// \param isFixed Whether this camera is fixed in space or not, for spa. 456 | /// \return the index of the node added. 457 | int addNode(Eigen::Matrix &trans, 458 | Eigen::Quaternion &qrot, 459 | bool isFixed = false); 460 | 461 | /// \brief Adds a pose constraint to the system. 462 | /// \param n1 Index of first node of the constraint 463 | /// \param n2 Index of second node of the constraint 464 | /// \param tmean A 3x1 vector, local translation from n1 to n2 465 | /// \param qpmean A Quaternion, local rotation from n1 to n2 466 | /// \param prec A 6x6 matrix, precision matrix for this link 467 | /// \return true if constraint added, false if nd0 or nd1 not found 468 | bool addConstraint(int nd0, int nd1, 469 | Eigen::Vector3d &tmean, 470 | Eigen::Quaterniond &qpmean, 471 | Eigen::Matrix &prec); 472 | 473 | /// set of nodes (camera frames) for SPA system, indexed by position; 474 | std::vector > nodes; 475 | 476 | /// set of scale for SPA system, indexed by position; 477 | std::vector scales; 478 | 479 | /// Number of fixed nodes 480 | int nFixed; 481 | 482 | /// Set of P2 constraints 483 | std::vector > p2cons; 484 | 485 | /// Set of scale constraints 486 | std::vector > scons; 487 | 488 | /// local or global angle coordinates 489 | bool useLocalAngles; 490 | 491 | /// calculate the error in the system; 492 | /// if is true, just the distance error without weighting 493 | double calcCost(bool tcost = false); 494 | /// calculate error assuming outliers > dist 495 | double calcCost(double dist); 496 | 497 | 498 | /// print some system stats 499 | void printStats(); 500 | void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format 501 | 502 | /// set up linear system, from RSS submission (konolige 2010) 503 | /// is the diagonal augmentation for the LM step 504 | double lambda; // save for continuation 505 | void setupSys(double sLambda); 506 | void setupSparseSys(double sLambda, int iter, int sparseType); 507 | 508 | /// do LM solution for system; returns number of iterations on 509 | /// finish. Argument is max number of iterations to perform, 510 | /// initial diagonal augmentation, and sparse form of Cholesky. 511 | int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY, 512 | double initTol = 1.0e-8, int CGiters = 50); 513 | 514 | /// Convergence bound (square of minimum acceptable delta change) 515 | double sqMinDelta; 516 | 517 | /// Spanning tree initialization 518 | void spanningTree(int node=0); 519 | 520 | /// Add a constraint between two poses, in a given pose frame. 521 | /// is reference pose, and are the pose constraints. 522 | void addConstraint(int pr, int p0, int p1, Eigen::Matrix &mean, Eigen::Matrix &prec); 523 | 524 | /// linear system matrix and vector 525 | Eigen::MatrixXd A; 526 | Eigen::VectorXd B; 527 | 528 | /// sparse matrix object 529 | CSparse csp; 530 | 531 | /// 6DOF pose as a unit quaternion and translation vector, saving 532 | /// for LM step 533 | Eigen::Matrix oldtrans; // homogeneous coordinates, last element is 1.0 534 | Eigen::Matrix oldqrot; // this is the quaternion as coefficients, note xyzw order 535 | 536 | }; 537 | 538 | /// constraint files 539 | 540 | /// reads in a file of pose constraints 541 | bool readP2File(char *fname, SysSPA spa); 542 | 543 | 544 | } // ends namespace sba 545 | 546 | #endif // _SBA_H_ 547 | 548 | --------------------------------------------------------------------------------