├── CMakeLists.txt ├── CMakeModules └── FindMKL.cmake ├── LICENSE ├── README.md ├── include └── slam_karto_gtsam │ └── GTSAMSolver.h ├── package.xml └── src ├── GTSAMSolver.cpp └── slam_karto_gtsam.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(slam_karto_gtsam) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cmake_modules 8 | message_filters 9 | nav_msgs 10 | open_karto 11 | rosconsole 12 | roscpp 13 | sensor_msgs 14 | geometry_msgs 15 | tf 16 | visualization_msgs 17 | ) 18 | 19 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 20 | 21 | find_package(MKL REQUIRED) 22 | find_package(PkgConfig REQUIRED) 23 | find_package(Eigen3 REQUIRED) 24 | find_package(GTSAM REQUIRED) 25 | 26 | include_directories( 27 | include/${PROJECT_NAME} 28 | ${catkin_INCLUDE_DIRS} 29 | ${EIGEN_INCLUDE_DIRS} 30 | ${MKL_INCLUDE_DIR} 31 | ${GTSAM_INCLUDE_DIRS} 32 | ) 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | CATKIN_DEPENDS roscpp message_filters nav_msgs open_karto rosconsole roscpp sensor_msgs geometry_msgs tf visualization_msgs 37 | DEPENDS Eigen3 38 | ) 39 | 40 | add_executable(slam_karto_gtsam src/slam_karto_gtsam.cpp src/GTSAMSolver.cpp) 41 | 42 | target_link_libraries(slam_karto_gtsam gtsam ${catkin_LIBRARIES} ${MKL_LIBRARIES}) 43 | 44 | #install(TARGETS slam_karto_gtsam 45 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | #) 47 | -------------------------------------------------------------------------------- /CMakeModules/FindMKL.cmake: -------------------------------------------------------------------------------- 1 | # This file is adapted from the one in OpenMEEG: http://www-sop.inria.fr/athena/software/OpenMEEG/ 2 | # - Try to find the Intel Math Kernel Library 3 | # Once done this will define 4 | # 5 | # MKL_FOUND - system has MKL 6 | # MKL_ROOT_DIR - path to the MKL base directory 7 | # MKL_INCLUDE_DIR - the MKL include directory 8 | # MKL_LIBRARIES - MKL libraries 9 | # 10 | # There are few sets of libraries: 11 | # Array indexes modes: 12 | # LP - 32 bit indexes of arrays 13 | # ILP - 64 bit indexes of arrays 14 | # Threading: 15 | # SEQUENTIAL - no threading 16 | # INTEL - Intel threading library 17 | # GNU - GNU threading library 18 | # MPI support 19 | # NOMPI - no MPI support 20 | # INTEL - Intel MPI library 21 | # OPEN - Open MPI library 22 | # SGI - SGI MPT Library 23 | 24 | # linux 25 | IF(UNIX AND NOT APPLE) 26 | IF(${CMAKE_HOST_SYSTEM_PROCESSOR} STREQUAL "x86_64") 27 | SET(MKL_ARCH_DIR "intel64") 28 | ELSE() 29 | SET(MKL_ARCH_DIR "32") 30 | ENDIF() 31 | ENDIF() 32 | # macos 33 | IF(APPLE) 34 | SET(MKL_ARCH_DIR "intel64") 35 | ENDIF() 36 | 37 | IF(FORCE_BUILD_32BITS) 38 | set(MKL_ARCH_DIR "32") 39 | ENDIF() 40 | # windows 41 | IF(WIN32) 42 | IF(${CMAKE_SIZEOF_VOID_P} EQUAL 8) 43 | SET(MKL_ARCH_DIR "intel64") 44 | ELSE() 45 | SET(MKL_ARCH_DIR "ia32") 46 | ENDIF() 47 | ENDIF() 48 | 49 | SET(MKL_THREAD_VARIANTS SEQUENTIAL GNUTHREAD INTELTHREAD) 50 | SET(MKL_MODE_VARIANTS ILP LP) 51 | SET(MKL_MPI_VARIANTS NOMPI INTELMPI OPENMPI SGIMPT) 52 | 53 | FIND_PATH(MKL_ROOT_DIR 54 | include/mkl_cblas.h 55 | PATHS 56 | $ENV{MKLDIR} 57 | /opt/intel/mkl/ 58 | /opt/intel/mkl/*/ 59 | /opt/intel/cmkl/ 60 | /opt/intel/cmkl/*/ 61 | /opt/intel/*/mkl/ 62 | /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal 63 | "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl" 64 | "C:/Program Files (x86)/Intel/Composer XE 2013/mkl" 65 | "C:/Program Files/Intel/MKL/*/" 66 | "C:/Program Files/Intel/ComposerXE-2011/mkl" 67 | "C:/Program Files/Intel/Composer XE 2013/mkl" 68 | ) 69 | 70 | FIND_PATH(MKL_INCLUDE_DIR 71 | mkl_cblas.h 72 | PATHS 73 | ${MKL_ROOT_DIR}/include 74 | ${INCLUDE_INSTALL_DIR} 75 | ) 76 | 77 | FIND_PATH(MKL_FFTW_INCLUDE_DIR 78 | fftw3.h 79 | PATH_SUFFIXES fftw 80 | PATHS 81 | ${MKL_ROOT_DIR}/include 82 | ${INCLUDE_INSTALL_DIR} 83 | NO_DEFAULT_PATH 84 | ) 85 | 86 | IF(WIN32) 87 | SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}") 88 | 89 | IF (MKL_INCLUDE_DIR MATCHES "10.") 90 | IF(CMAKE_CL_64) 91 | SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64) 92 | ELSE() 93 | SET(MKL_LIBS mkl_solver mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libguide mkl_lapack95 mkl_blas95) 94 | ENDIF() 95 | ELSEIF(MKL_INCLUDE_DIR MATCHES "2013") # version 11 ... 96 | IF(CMAKE_CL_64) 97 | SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_intel_thread libiomp5md mkl_lapack95_lp64 mkl_blas95_lp64) 98 | ELSE() 99 | SET(MKL_LIBS mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libiomp5md mkl_lapack95 mkl_blas95) 100 | ENDIF() 101 | ELSE() # old MKL 9 102 | SET(MKL_LIBS mkl_solver mkl_c libguide mkl_lapack mkl_ia32) 103 | ENDIF() 104 | 105 | IF (MKL_INCLUDE_DIR MATCHES "10.3") 106 | SET(MKL_LIBS ${MKL_LIBS} libiomp5md) 107 | ENDIF() 108 | 109 | FOREACH (LIB ${MKL_LIBS}) 110 | FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH) 111 | IF(${LIB}_PATH) 112 | SET(MKL_LIBRARIES ${MKL_LIBRARIES} ${${LIB}_PATH}) 113 | ELSE() 114 | MESSAGE(STATUS "Could not find ${LIB}: disabling MKL") 115 | ENDIF() 116 | ENDFOREACH() 117 | SET(MKL_FOUND ON) 118 | ELSE() # UNIX and macOS 119 | FIND_LIBRARY(MKL_CORE_LIBRARY 120 | mkl_core 121 | PATHS 122 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 123 | ${MKL_ROOT_DIR}/lib/ 124 | ) 125 | 126 | # Threading libraries 127 | FIND_LIBRARY(MKL_SEQUENTIAL_LIBRARY 128 | mkl_sequential 129 | PATHS 130 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 131 | ${MKL_ROOT_DIR}/lib/ 132 | ) 133 | 134 | FIND_LIBRARY(MKL_INTELTHREAD_LIBRARY 135 | mkl_intel_thread 136 | PATHS 137 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 138 | ${MKL_ROOT_DIR}/lib/ 139 | ) 140 | 141 | # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above) 142 | IF(NOT APPLE) 143 | FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY 144 | mkl_gnu_thread 145 | PATHS 146 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 147 | ${MKL_ROOT_DIR}/lib/ 148 | ) 149 | ENDIF() 150 | 151 | # Intel Libraries 152 | IF("${MKL_ARCH_DIR}" STREQUAL "32") 153 | FIND_LIBRARY(MKL_LP_LIBRARY 154 | mkl_intel 155 | PATHS 156 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 157 | ${MKL_ROOT_DIR}/lib/ 158 | ) 159 | 160 | FIND_LIBRARY(MKL_ILP_LIBRARY 161 | mkl_intel 162 | PATHS 163 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 164 | ${MKL_ROOT_DIR}/lib/ 165 | ) 166 | else() 167 | FIND_LIBRARY(MKL_LP_LIBRARY 168 | mkl_intel_lp64 169 | PATHS 170 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 171 | ${MKL_ROOT_DIR}/lib/ 172 | ) 173 | 174 | FIND_LIBRARY(MKL_ILP_LIBRARY 175 | mkl_intel_ilp64 176 | PATHS 177 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 178 | ${MKL_ROOT_DIR}/lib/ 179 | ) 180 | ENDIF() 181 | 182 | # Lapack 183 | FIND_LIBRARY(MKL_LAPACK_LIBRARY 184 | mkl_lapack 185 | PATHS 186 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 187 | ${MKL_ROOT_DIR}/lib/ 188 | ) 189 | 190 | IF(NOT MKL_LAPACK_LIBRARY) 191 | FIND_LIBRARY(MKL_LAPACK_LIBRARY 192 | mkl_lapack95_lp64 193 | PATHS 194 | ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} 195 | ${MKL_ROOT_DIR}/lib/ 196 | ) 197 | ENDIF() 198 | 199 | # iomp5 200 | IF("${MKL_ARCH_DIR}" STREQUAL "32") 201 | IF(UNIX AND NOT APPLE) 202 | FIND_LIBRARY(MKL_IOMP5_LIBRARY 203 | iomp5 204 | PATHS 205 | ${MKL_ROOT_DIR}/../lib/ia32 206 | ) 207 | ELSE() 208 | SET(MKL_IOMP5_LIBRARY "") # no need for mac 209 | ENDIF() 210 | else() 211 | IF(UNIX AND NOT APPLE) 212 | FIND_LIBRARY(MKL_IOMP5_LIBRARY 213 | iomp5 214 | PATHS 215 | ${MKL_ROOT_DIR}/../lib/intel64 216 | ) 217 | ELSE() 218 | SET(MKL_IOMP5_LIBRARY "") # no need for mac 219 | ENDIF() 220 | ENDIF() 221 | 222 | foreach (MODEVAR ${MKL_MODE_VARIANTS}) 223 | foreach (THREADVAR ${MKL_THREAD_VARIANTS}) 224 | if (MKL_CORE_LIBRARY AND MKL_${MODEVAR}_LIBRARY AND MKL_${THREADVAR}_LIBRARY) 225 | set(MKL_${MODEVAR}_${THREADVAR}_LIBRARIES 226 | ${MKL_${MODEVAR}_LIBRARY} ${MKL_${THREADVAR}_LIBRARY} ${MKL_CORE_LIBRARY} 227 | ${MKL_LAPACK_LIBRARY} ${MKL_IOMP5_LIBRARY}) 228 | # message("${MODEVAR} ${THREADVAR} ${MKL_${MODEVAR}_${THREADVAR}_LIBRARIES}") # for debug 229 | endif() 230 | endforeach() 231 | endforeach() 232 | 233 | IF(APPLE) 234 | SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES}) 235 | ELSE() 236 | SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) 237 | ENDIF() 238 | 239 | MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY 240 | MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY) 241 | ENDIF() 242 | 243 | INCLUDE(FindPackageHandleStandardArgs) 244 | find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES) 245 | 246 | #if(MKL_FOUND) 247 | # LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack 248 | #endif() 249 | 250 | MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Saurav Agarwal 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # slam_karto_gtsam 2 | Pose-graph SLAM with Open Karto as Front-end and GTSAM as Backend 3 | 4 | # Notes 5 | 6 | The optimization routine used is Levenberg-Marquadt provided by gtsam, you can easily change it to something else (Powell's dog leg etc.). This package is currently tested with Ubuntu 16.04 LTS + ROS Kinetic 7 | 8 | # How-To Use 9 | 10 | Use the given launch files. The build_map_w_params launch files uses a given param file, which you can edit to change the behavior of open_karto. Make sure to use the right scan topic. Also, the package will expect an odometry tf to be published to between odom frame and base_link (or base_footprint). 11 | 12 | # Dependencies 13 | 14 | * Download and install Intel MKL from https://software.intel.com/en-us/mkl 15 | 16 | * Download and install GTSAM 4.x, instructions are in GTSAM readme. $ git clone https://bitbucket.org/gtborg/gtsam.git 17 | 18 | * Download and put open karto in your catkin_ws/src from https://github.com/ros-perception/open_karto 19 | 20 | # Support 21 | 22 | Head on over to my blog post http://sauravag.com/2017/07/an-practical-introduction-to-pose-graph-slam/ where you can ask questions. 23 | -------------------------------------------------------------------------------- /include/slam_karto_gtsam/GTSAMSolver.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Copyright (c) 2017, Saurav Agarwal 4 | * All rights reserved. 5 | * 6 | *********************************************************************/ 7 | 8 | /* Authors: Saurav Agarwal */ 9 | 10 | 11 | #ifndef KARTO_GTSAMSolver_H 12 | #define KARTO_GTSAMSolver_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | typedef std::vector CovarianceVector; 22 | 23 | /** 24 | * @brief Wrapper for G2O to interface with Open Karto 25 | */ 26 | class GTSAMSolver : public karto::ScanSolver 27 | { 28 | public: 29 | 30 | GTSAMSolver(); 31 | 32 | virtual ~GTSAMSolver(); 33 | 34 | public: 35 | 36 | /** 37 | * @brief Clear the vector of corrections 38 | * @details Empty out previously computed corrections 39 | */ 40 | virtual void Clear(); 41 | 42 | /** 43 | * @brief Solve the SLAM back-end 44 | * @details Calls G2O to solve the SLAM back-end 45 | */ 46 | virtual void Compute(); 47 | 48 | /** 49 | * @brief Get the vector of corrections 50 | * @details Get the vector of corrections 51 | * @return Vector with corrected poses 52 | */ 53 | virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; 54 | 55 | /** 56 | * @brief Add a node to pose-graph 57 | * @details Add a node which is a robot pose to the pose-graph 58 | * 59 | * @param pVertex the node to be added in 60 | */ 61 | virtual void AddNode(karto::Vertex* pVertex); 62 | 63 | /** 64 | * @brief Add an edge constraint to pose-graph 65 | * @details Adds a relative pose measurement constraint between two poses in the graph 66 | * 67 | * @param pEdge [description] 68 | */ 69 | virtual void AddConstraint(karto::Edge* pEdge); 70 | 71 | /** 72 | * @brief Get the pose-graph 73 | * @details Get the underlying graph from g2o, return the graph of constraints 74 | * 75 | * @param g the graph 76 | */ 77 | void getGraph(std::vector &nodes, std::vector > &edges); 78 | 79 | private: 80 | 81 | karto::ScanSolver::IdPoseVector corrections_; 82 | 83 | gtsam::NonlinearFactorGraph graph_; 84 | 85 | gtsam::Values initialGuess_; 86 | 87 | std::vector graphNodes_; 88 | 89 | }; 90 | 91 | #endif // KARTO_GTSAMSolver_H 92 | 93 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | slam_karto_gtsam 3 | 4 | This package pulls in the Karto mapping library, and provides a ROS 5 | wrapper for using it with gtsam. 6 | 7 | 0.7.3 8 | Saurav Agarwal 9 | Saurav Agarwal 10 | BSD 11 | 12 | catkin 13 | 14 | cmake_modules 15 | eigen 16 | message_filters 17 | nav_msgs 18 | open_karto 19 | rosconsole 20 | roscpp 21 | sensor_msgs 22 | geometry_msgs 23 | tf 24 | visualization_msgs 25 | 26 | eigen 27 | message_filters 28 | nav_msgs 29 | open_karto 30 | rosconsole 31 | roscpp 32 | sensor_msgs 33 | geometry_msgs 34 | tf 35 | visualization_msgs 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/GTSAMSolver.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Copyright (c) 2017, Saurav Agarwal 4 | * All rights reserved. 5 | * 6 | *********************************************************************/ 7 | 8 | /* Authors: Saurav Agarwal */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include "GTSAMSolver.h" 14 | 15 | GTSAMSolver::GTSAMSolver() 16 | { 17 | using namespace gtsam; 18 | 19 | // add the prior on the first node which is known 20 | noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); 21 | 22 | graph_.emplace_shared >(0, Pose2(0, 0, 0), priorNoise); 23 | 24 | } 25 | 26 | GTSAMSolver::~GTSAMSolver() 27 | { 28 | 29 | } 30 | 31 | void GTSAMSolver::Clear() 32 | { 33 | corrections_.clear(); 34 | } 35 | 36 | const karto::ScanSolver::IdPoseVector& GTSAMSolver::GetCorrections() const 37 | { 38 | return corrections_; 39 | } 40 | 41 | void GTSAMSolver::Compute() 42 | { 43 | using namespace gtsam; 44 | 45 | corrections_.clear(); 46 | 47 | graphNodes_.clear(); 48 | 49 | LevenbergMarquardtParams parameters; 50 | 51 | // Stop iterating once the change in error between steps is less than this value 52 | parameters.relativeErrorTol = 1e-5; 53 | 54 | // Do not perform more than N iteration steps 55 | parameters.maxIterations = 500; 56 | 57 | // Create the optimizer ... 58 | LevenbergMarquardtOptimizer optimizer(graph_, initialGuess_, parameters); 59 | 60 | // ... and optimize 61 | Values result = optimizer.optimize(); 62 | 63 | Values::ConstFiltered viewPose2 = result.filter(); 64 | 65 | // put values into corrections container 66 | for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose2) 67 | { 68 | 69 | karto::Pose2 pose(key_value.value.x(), key_value.value.y(), key_value.value.theta()); 70 | 71 | corrections_.push_back(std::make_pair(key_value.key, pose)); 72 | 73 | graphNodes_.push_back(Eigen::Vector2d(key_value.value.x(), key_value.value.y())); 74 | 75 | } 76 | 77 | } 78 | 79 | void GTSAMSolver::AddNode(karto::Vertex* pVertex) 80 | { 81 | 82 | using namespace gtsam; 83 | 84 | karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose(); 85 | 86 | initialGuess_.insert(pVertex->GetObject()->GetUniqueId(), 87 | Pose2( odom.GetX(), odom.GetY(), odom.GetHeading() )); 88 | 89 | graphNodes_.push_back(Eigen::Vector2d(odom.GetX(), odom.GetY())); 90 | 91 | ROS_DEBUG("[gtsam] Adding node %d.", pVertex->GetObject()->GetUniqueId()); 92 | 93 | } 94 | 95 | void GTSAMSolver::AddConstraint(karto::Edge* pEdge) 96 | { 97 | 98 | using namespace gtsam; 99 | 100 | // Set source and target 101 | int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId(); 102 | 103 | int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId(); 104 | 105 | // Set the measurement (poseGraphEdge distance between vertices) 106 | karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); 107 | 108 | karto::Pose2 diff = pLinkInfo->GetPoseDifference(); 109 | 110 | // Set the covariance of the measurement 111 | karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance(); 112 | 113 | Eigen::Matrix cov; 114 | 115 | cov(0,0) = precisionMatrix(0,0); 116 | 117 | cov(0,1) = cov(1,0) = precisionMatrix(0,1); 118 | 119 | cov(0,2) = cov(2,0) = precisionMatrix(0,2); 120 | 121 | cov(1,1) = precisionMatrix(1,1); 122 | 123 | cov(1,2) = cov(2,1) = precisionMatrix(1,2); 124 | 125 | cov(2,2) = precisionMatrix(2,2); 126 | 127 | noiseModel::Gaussian::shared_ptr model = noiseModel::Diagonal::Covariance(cov); 128 | 129 | // Add odometry factors 130 | // Create odometry (Between) factors between consecutive poses 131 | graph_.emplace_shared >(sourceID, targetID, Pose2(diff.GetX(), diff.GetY(), diff.GetHeading()), model); 132 | 133 | // Add the constraint to the optimizer 134 | ROS_DEBUG("[gtsam] Adding Edge from node %d to node %d.", sourceID, targetID); 135 | 136 | } 137 | 138 | void GTSAMSolver::getGraph(std::vector &nodes, std::vector > &edges) 139 | { 140 | using namespace gtsam; 141 | 142 | nodes = graphNodes_; 143 | 144 | // double *data1 = new double[3]; 145 | 146 | // double *data2 = new double[3]; 147 | 148 | // for (SparseOptimizer::EdgeSet::iterator it = optimizer_.edges().begin(); it != optimizer_.edges().end(); ++it) 149 | // { 150 | 151 | // EdgeSE2* e = dynamic_cast(*it); 152 | 153 | // if(e) 154 | // { 155 | 156 | // VertexSE2* v1 = dynamic_cast(e->vertices()[0]); 157 | 158 | // v1->getEstimateData(data1); 159 | 160 | // Eigen::Vector2d poseFrom(data1[0], data1[1]); 161 | 162 | // VertexSE2* v2 = dynamic_cast(e->vertices()[1]); 163 | 164 | // v2->getEstimateData(data2); 165 | 166 | // Eigen::Vector2d poseTo(data2[0], data2[1]); 167 | 168 | // edges.push_back(std::make_pair(poseFrom, poseTo)); 169 | 170 | // } 171 | 172 | // } 173 | 174 | // delete data1; 175 | 176 | // delete data2; 177 | 178 | } -------------------------------------------------------------------------------- /src/slam_karto_gtsam.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_karto 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Brian Gerkey */ 18 | 19 | /********************************************************************* 20 | * 21 | * Copyright (c) 2017, Saurav Agarwal 22 | * All rights reserved. 23 | * 24 | *********************************************************************/ 25 | 26 | /* Authors: Saurav Agarwal */ 27 | 28 | 29 | #include "ros/ros.h" 30 | #include "ros/console.h" 31 | #include "message_filters/subscriber.h" 32 | #include "tf/transform_broadcaster.h" 33 | #include "tf/transform_listener.h" 34 | #include "tf/message_filter.h" 35 | #include "visualization_msgs/MarkerArray.h" 36 | 37 | #include "nav_msgs/MapMetaData.h" 38 | #include "sensor_msgs/LaserScan.h" 39 | #include "nav_msgs/GetMap.h" 40 | 41 | #include "open_karto/Mapper.h" 42 | 43 | #include "GTSAMSolver.h" 44 | 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | // compute linear index for given map coords 52 | #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) 53 | 54 | class SlamKarto 55 | { 56 | public: 57 | SlamKarto(); 58 | ~SlamKarto(); 59 | 60 | void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan); 61 | bool mapCallback(nav_msgs::GetMap::Request &req, 62 | nav_msgs::GetMap::Response &res); 63 | 64 | private: 65 | bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t); 66 | karto::LaserRangeFinder* getLaser(const sensor_msgs::LaserScan::ConstPtr& scan); 67 | bool addScan(karto::LaserRangeFinder* laser, 68 | const sensor_msgs::LaserScan::ConstPtr& scan, 69 | karto::Pose2& karto_pose); 70 | bool updateMap(); 71 | void publishTransform(); 72 | void publishLoop(double transform_publish_period); 73 | void publishGraphVisualization(); 74 | 75 | // ROS handles 76 | ros::NodeHandle node_; 77 | tf::TransformListener tf_; 78 | tf::TransformBroadcaster* tfB_; 79 | message_filters::Subscriber* scan_filter_sub_; 80 | tf::MessageFilter* scan_filter_; 81 | ros::Publisher sst_; 82 | ros::Publisher marker_publisher_; 83 | ros::Publisher sstm_; 84 | ros::ServiceServer ss_; 85 | 86 | // The map that will be published / send to service callers 87 | nav_msgs::GetMap::Response map_; 88 | 89 | // Storage for ROS parameters 90 | std::string odom_frame_; 91 | std::string map_frame_; 92 | std::string base_frame_; 93 | int throttle_scans_; 94 | ros::Duration map_update_interval_; 95 | double resolution_; 96 | boost::mutex map_mutex_; 97 | boost::mutex map_to_odom_mutex_; 98 | 99 | // Karto bookkeeping 100 | karto::Mapper* mapper_; 101 | karto::Dataset* dataset_; 102 | GTSAMSolver* solver_; 103 | std::map lasers_; 104 | std::map lasers_inverted_; 105 | 106 | // Internal state 107 | bool got_map_; 108 | int laser_count_; 109 | boost::thread* transform_thread_; 110 | tf::Transform map_to_odom_; 111 | unsigned marker_count_; 112 | bool inverted_laser_; 113 | }; 114 | 115 | SlamKarto::SlamKarto() : 116 | got_map_(false), 117 | laser_count_(0), 118 | transform_thread_(NULL), 119 | marker_count_(0) 120 | { 121 | map_to_odom_.setIdentity(); 122 | 123 | // Retrieve parameters 124 | ros::NodeHandle private_nh_("~"); 125 | 126 | if(!private_nh_.getParam("odom_frame", odom_frame_)) 127 | odom_frame_ = "odom"; 128 | if(!private_nh_.getParam("map_frame", map_frame_)) 129 | map_frame_ = "map"; 130 | if(!private_nh_.getParam("base_frame", base_frame_)) 131 | base_frame_ = "base_link"; 132 | if(!private_nh_.getParam("throttle_scans", throttle_scans_)) 133 | throttle_scans_ = 1; 134 | double tmp; 135 | if(!private_nh_.getParam("map_update_interval", tmp)) 136 | tmp = 5.0; 137 | map_update_interval_.fromSec(tmp); 138 | if(!private_nh_.getParam("resolution", resolution_)) 139 | { 140 | // Compatibility with slam_gmapping, which uses "delta" to mean 141 | // resolution 142 | if(!private_nh_.getParam("delta", resolution_)) 143 | resolution_ = 0.05; 144 | } 145 | double transform_publish_period; 146 | private_nh_.param("transform_publish_period", transform_publish_period, 0.05); 147 | 148 | // Set up advertisements and subscriptions 149 | tfB_ = new tf::TransformBroadcaster(); 150 | sst_ = node_.advertise("map", 1, true); 151 | sstm_ = node_.advertise("map_metadata", 1, true); 152 | ss_ = node_.advertiseService("dynamic_map", &SlamKarto::mapCallback, this); 153 | scan_filter_sub_ = new message_filters::Subscriber(node_, "scan", 5); 154 | scan_filter_ = new tf::MessageFilter(*scan_filter_sub_, tf_, odom_frame_, 5); 155 | scan_filter_->registerCallback(boost::bind(&SlamKarto::laserCallback, this, _1)); 156 | marker_publisher_ = node_.advertise("visualization_marker_array",1); 157 | 158 | // Create a thread to periodically publish the latest map->odom 159 | // transform; it needs to go out regularly, uninterrupted by potentially 160 | // long periods of computation in our main loop. 161 | transform_thread_ = new boost::thread(boost::bind(&SlamKarto::publishLoop, this, transform_publish_period)); 162 | 163 | // Initialize Karto structures 164 | mapper_ = new karto::Mapper(); 165 | dataset_ = new karto::Dataset(); 166 | 167 | // Setting General Parameters from the Parameter Server 168 | bool use_scan_matching; 169 | if(private_nh_.getParam("use_scan_matching", use_scan_matching)) 170 | mapper_->setParamUseScanMatching(use_scan_matching); 171 | 172 | bool use_scan_barycenter; 173 | if(private_nh_.getParam("use_scan_barycenter", use_scan_barycenter)) 174 | mapper_->setParamUseScanBarycenter(use_scan_barycenter); 175 | 176 | double minimum_travel_distance; 177 | if(private_nh_.getParam("minimum_travel_distance", minimum_travel_distance)) 178 | mapper_->setParamMinimumTravelDistance(minimum_travel_distance); 179 | 180 | double minimum_travel_heading; 181 | if(private_nh_.getParam("minimum_travel_heading", minimum_travel_heading)) 182 | mapper_->setParamMinimumTravelHeading(minimum_travel_heading); 183 | 184 | int scan_buffer_size; 185 | if(private_nh_.getParam("scan_buffer_size", scan_buffer_size)) 186 | mapper_->setParamScanBufferSize(scan_buffer_size); 187 | 188 | double scan_buffer_maximum_scan_distance; 189 | if(private_nh_.getParam("scan_buffer_maximum_scan_distance", scan_buffer_maximum_scan_distance)) 190 | mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance); 191 | 192 | double link_match_minimum_response_fine; 193 | if(private_nh_.getParam("link_match_minimum_response_fine", link_match_minimum_response_fine)) 194 | mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine); 195 | 196 | double link_scan_maximum_distance; 197 | if(private_nh_.getParam("link_scan_maximum_distance", link_scan_maximum_distance)) 198 | mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance); 199 | 200 | double loop_search_maximum_distance; 201 | if(private_nh_.getParam("loop_search_maximum_distance", loop_search_maximum_distance)) 202 | mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance); 203 | 204 | bool do_loop_closing; 205 | if(private_nh_.getParam("do_loop_closing", do_loop_closing)) 206 | mapper_->setParamDoLoopClosing(do_loop_closing); 207 | 208 | int loop_match_minimum_chain_size; 209 | if(private_nh_.getParam("loop_match_minimum_chain_size", loop_match_minimum_chain_size)) 210 | mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size); 211 | 212 | double loop_match_maximum_variance_coarse; 213 | if(private_nh_.getParam("loop_match_maximum_variance_coarse", loop_match_maximum_variance_coarse)) 214 | mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse); 215 | 216 | double loop_match_minimum_response_coarse; 217 | if(private_nh_.getParam("loop_match_minimum_response_coarse", loop_match_minimum_response_coarse)) 218 | mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse); 219 | 220 | double loop_match_minimum_response_fine; 221 | if(private_nh_.getParam("loop_match_minimum_response_fine", loop_match_minimum_response_fine)) 222 | mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine); 223 | 224 | // Setting Correlation Parameters from the Parameter Server 225 | 226 | double correlation_search_space_dimension; 227 | if(private_nh_.getParam("correlation_search_space_dimension", correlation_search_space_dimension)) 228 | mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension); 229 | 230 | double correlation_search_space_resolution; 231 | if(private_nh_.getParam("correlation_search_space_resolution", correlation_search_space_resolution)) 232 | mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution); 233 | 234 | double correlation_search_space_smear_deviation; 235 | if(private_nh_.getParam("correlation_search_space_smear_deviation", correlation_search_space_smear_deviation)) 236 | mapper_->setParamCorrelationSearchSpaceSmearDeviation(correlation_search_space_smear_deviation); 237 | 238 | // Setting Correlation Parameters, Loop Closure Parameters from the Parameter Server 239 | 240 | double loop_search_space_dimension; 241 | if(private_nh_.getParam("loop_search_space_dimension", loop_search_space_dimension)) 242 | mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension); 243 | 244 | double loop_search_space_resolution; 245 | if(private_nh_.getParam("loop_search_space_resolution", loop_search_space_resolution)) 246 | mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution); 247 | 248 | double loop_search_space_smear_deviation; 249 | if(private_nh_.getParam("loop_search_space_smear_deviation", loop_search_space_smear_deviation)) 250 | mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation); 251 | 252 | // Setting Scan Matcher Parameters from the Parameter Server 253 | 254 | double distance_variance_penalty; 255 | if(private_nh_.getParam("distance_variance_penalty", distance_variance_penalty)) 256 | mapper_->setParamDistanceVariancePenalty(distance_variance_penalty); 257 | 258 | double angle_variance_penalty; 259 | if(private_nh_.getParam("angle_variance_penalty", angle_variance_penalty)) 260 | mapper_->setParamAngleVariancePenalty(angle_variance_penalty); 261 | 262 | double fine_search_angle_offset; 263 | if(private_nh_.getParam("fine_search_angle_offset", fine_search_angle_offset)) 264 | mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset); 265 | 266 | double coarse_search_angle_offset; 267 | if(private_nh_.getParam("coarse_search_angle_offset", coarse_search_angle_offset)) 268 | mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset); 269 | 270 | double coarse_angle_resolution; 271 | if(private_nh_.getParam("coarse_angle_resolution", coarse_angle_resolution)) 272 | mapper_->setParamCoarseAngleResolution(coarse_angle_resolution); 273 | 274 | double minimum_angle_penalty; 275 | if(private_nh_.getParam("minimum_angle_penalty", minimum_angle_penalty)) 276 | mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty); 277 | 278 | double minimum_distance_penalty; 279 | if(private_nh_.getParam("minimum_distance_penalty", minimum_distance_penalty)) 280 | mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty); 281 | 282 | bool use_response_expansion; 283 | if(private_nh_.getParam("use_response_expansion", use_response_expansion)) 284 | mapper_->setParamUseResponseExpansion(use_response_expansion); 285 | 286 | // Set solver to be used in loop closure 287 | solver_ = new GTSAMSolver(); 288 | 289 | mapper_->SetScanSolver(solver_); 290 | } 291 | 292 | SlamKarto::~SlamKarto() 293 | { 294 | if(transform_thread_) 295 | { 296 | transform_thread_->join(); 297 | delete transform_thread_; 298 | } 299 | if (scan_filter_) 300 | delete scan_filter_; 301 | if (scan_filter_sub_) 302 | delete scan_filter_sub_; 303 | if (solver_) 304 | delete solver_; 305 | if (mapper_) 306 | delete mapper_; 307 | if (dataset_) 308 | delete dataset_; 309 | // TODO: delete the pointers in the lasers_ map; not sure whether or not 310 | // I'm supposed to do that. 311 | } 312 | 313 | void 314 | SlamKarto::publishLoop(double transform_publish_period) 315 | { 316 | if(transform_publish_period == 0) 317 | return; 318 | 319 | ros::Rate r(1.0 / transform_publish_period); 320 | while(ros::ok()) 321 | { 322 | publishTransform(); 323 | r.sleep(); 324 | } 325 | } 326 | 327 | void 328 | SlamKarto::publishTransform() 329 | { 330 | boost::mutex::scoped_lock lock(map_to_odom_mutex_); 331 | ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.05); 332 | tfB_->sendTransform(tf::StampedTransform (map_to_odom_, ros::Time::now(), map_frame_, odom_frame_)); 333 | } 334 | 335 | karto::LaserRangeFinder* 336 | SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan) 337 | { 338 | // Check whether we know about this laser yet 339 | if(lasers_.find(scan->header.frame_id) == lasers_.end()) 340 | { 341 | // New laser; need to create a Karto device for it. 342 | 343 | // Get the laser's pose, relative to base. 344 | tf::Stamped ident; 345 | tf::Stamped laser_pose; 346 | ident.setIdentity(); 347 | ident.frame_id_ = scan->header.frame_id; 348 | ident.stamp_ = scan->header.stamp; 349 | try 350 | { 351 | tf_.transformPose(base_frame_, ident, laser_pose); 352 | } 353 | catch(tf::TransformException e) 354 | { 355 | ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", 356 | e.what()); 357 | return NULL; 358 | } 359 | 360 | double yaw = tf::getYaw(laser_pose.getRotation()); 361 | 362 | ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f", 363 | scan->header.frame_id.c_str(), 364 | laser_pose.getOrigin().x(), 365 | laser_pose.getOrigin().y(), 366 | yaw); 367 | // To account for lasers that are mounted upside-down, 368 | // we create a point 1m above the laser and transform it into the laser frame 369 | // if the point's z-value is <=0, it is upside-down 370 | 371 | tf::Vector3 v; 372 | v.setValue(0, 0, 1 + laser_pose.getOrigin().z()); 373 | tf::Stamped up(v, scan->header.stamp, base_frame_); 374 | 375 | try 376 | { 377 | tf_.transformPoint(scan->header.frame_id, up, up); 378 | ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z()); 379 | } 380 | catch (tf::TransformException& e) 381 | { 382 | ROS_WARN("Unable to determine orientation of laser: %s", e.what()); 383 | return NULL; 384 | } 385 | 386 | bool inverse = lasers_inverted_[scan->header.frame_id] = up.z() <= 0; 387 | if (inverse) 388 | ROS_INFO("laser is mounted upside-down"); 389 | 390 | 391 | // Create a laser range finder device and copy in data from the first 392 | // scan 393 | std::string name = scan->header.frame_id; 394 | karto::LaserRangeFinder* laser = 395 | karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name)); 396 | laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(), 397 | laser_pose.getOrigin().y(), 398 | yaw)); 399 | laser->SetMinimumRange(scan->range_min); 400 | laser->SetMaximumRange(scan->range_max); 401 | laser->SetMinimumAngle(scan->angle_min); 402 | laser->SetMaximumAngle(scan->angle_max); 403 | laser->SetAngularResolution(scan->angle_increment); 404 | // TODO: expose this, and many other parameters 405 | //laser_->SetRangeThreshold(12.0); 406 | 407 | // Store this laser device for later 408 | lasers_[scan->header.frame_id] = laser; 409 | 410 | // Add it to the dataset, which seems to be necessary 411 | dataset_->Add(laser); 412 | } 413 | 414 | return lasers_[scan->header.frame_id]; 415 | } 416 | 417 | bool 418 | SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t) 419 | { 420 | // Get the robot's pose 421 | tf::Stamped ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0), 422 | tf::Vector3(0,0,0)), t, base_frame_); 423 | tf::Stamped odom_pose; 424 | try 425 | { 426 | tf_.transformPose(odom_frame_, ident, odom_pose); 427 | } 428 | catch(tf::TransformException e) 429 | { 430 | ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); 431 | return false; 432 | } 433 | double yaw = tf::getYaw(odom_pose.getRotation()); 434 | 435 | karto_pose = 436 | karto::Pose2(odom_pose.getOrigin().x(), 437 | odom_pose.getOrigin().y(), 438 | yaw); 439 | return true; 440 | } 441 | 442 | void 443 | SlamKarto::publishGraphVisualization() 444 | { 445 | std::vector graph_nodes; 446 | 447 | std::vector > graph_edges; 448 | 449 | solver_->getGraph(graph_nodes, graph_edges); 450 | 451 | visualization_msgs::MarkerArray marray; 452 | 453 | visualization_msgs::Marker m; 454 | m.header.frame_id = "map"; 455 | m.header.stamp = ros::Time::now(); 456 | m.id = 0; 457 | m.ns = "karto"; 458 | m.type = visualization_msgs::Marker::SPHERE; 459 | m.pose.position.x = 0.0; 460 | m.pose.position.y = 0.0; 461 | m.pose.position.z = 0.0; 462 | m.scale.x = 0.1; 463 | m.scale.y = 0.1; 464 | m.scale.z = 0.1; 465 | m.color.r = 1.0; 466 | m.color.g = 0; 467 | m.color.b = 0.0; 468 | m.color.a = 1.0; 469 | m.lifetime = ros::Duration(0); 470 | 471 | visualization_msgs::Marker edge; 472 | edge.header.frame_id = "map"; 473 | edge.header.stamp = ros::Time::now(); 474 | edge.action = visualization_msgs::Marker::ADD; 475 | edge.ns = "karto"; 476 | edge.id = 0; 477 | edge.type = visualization_msgs::Marker::LINE_STRIP; 478 | edge.scale.x = 0.1; 479 | edge.scale.y = 0.1; 480 | edge.scale.z = 0.1; 481 | edge.color.a = 1.0; 482 | edge.color.r = 0.0; 483 | edge.color.g = 0.0; 484 | edge.color.b = 1.0; 485 | 486 | m.action = visualization_msgs::Marker::ADD; 487 | 488 | uint id = 0; 489 | 490 | for (uint i=0; i < graph_nodes.size(); i++) 491 | { 492 | m.id = id; 493 | m.pose.position.x = graph_nodes[i](0); 494 | m.pose.position.y = graph_nodes[i](1); 495 | marray.markers.push_back(visualization_msgs::Marker(m)); 496 | id++; 497 | } 498 | 499 | for (uint i=0; i < graph_edges.size(); i++) 500 | { 501 | 502 | geometry_msgs::Point p; 503 | 504 | p.x = graph_edges[i].first(0); 505 | 506 | p.y = graph_edges[i].first(1); 507 | 508 | edge.points.push_back(p); 509 | 510 | p.x = graph_edges[i].second(0); 511 | 512 | p.y = graph_edges[i].second(1); 513 | 514 | edge.points.push_back(p); 515 | 516 | edge.id = id; 517 | 518 | marray.markers.push_back(visualization_msgs::Marker(edge)); 519 | 520 | id++; 521 | } 522 | 523 | m.action = visualization_msgs::Marker::DELETE; 524 | 525 | for (; id < marker_count_; id++) 526 | { 527 | m.id = id; 528 | marray.markers.push_back(visualization_msgs::Marker(m)); 529 | } 530 | 531 | marker_count_ = marray.markers.size(); 532 | 533 | marker_publisher_.publish(marray); 534 | 535 | } 536 | 537 | void 538 | SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) 539 | { 540 | laser_count_++; 541 | if ((laser_count_ % throttle_scans_) != 0) 542 | return; 543 | 544 | static ros::Time last_map_update(0,0); 545 | 546 | // Check whether we know about this laser yet 547 | karto::LaserRangeFinder* laser = getLaser(scan); 548 | 549 | if(!laser) 550 | { 551 | ROS_WARN("Failed to create laser device for %s; discarding scan", 552 | scan->header.frame_id.c_str()); 553 | return; 554 | } 555 | 556 | karto::Pose2 odom_pose; 557 | if(addScan(laser, scan, odom_pose)) 558 | { 559 | ROS_DEBUG("added scan at pose: %.3f %.3f %.3f", 560 | odom_pose.GetX(), 561 | odom_pose.GetY(), 562 | odom_pose.GetHeading()); 563 | 564 | publishGraphVisualization(); 565 | 566 | if(!got_map_ || 567 | (scan->header.stamp - last_map_update) > map_update_interval_) 568 | { 569 | if(updateMap()) 570 | { 571 | last_map_update = scan->header.stamp; 572 | got_map_ = true; 573 | ROS_DEBUG("Updated the map"); 574 | } 575 | } 576 | } 577 | } 578 | 579 | bool 580 | SlamKarto::updateMap() 581 | { 582 | boost::mutex::scoped_lock lock(map_mutex_); 583 | 584 | karto::OccupancyGrid* occ_grid = 585 | karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_); 586 | 587 | if(!occ_grid) 588 | return false; 589 | 590 | if(!got_map_) { 591 | map_.map.info.resolution = resolution_; 592 | map_.map.info.origin.position.x = 0.0; 593 | map_.map.info.origin.position.y = 0.0; 594 | map_.map.info.origin.position.z = 0.0; 595 | map_.map.info.origin.orientation.x = 0.0; 596 | map_.map.info.origin.orientation.y = 0.0; 597 | map_.map.info.origin.orientation.z = 0.0; 598 | map_.map.info.origin.orientation.w = 1.0; 599 | } 600 | 601 | // Translate to ROS format 602 | kt_int32s width = occ_grid->GetWidth(); 603 | kt_int32s height = occ_grid->GetHeight(); 604 | karto::Vector2 offset = occ_grid->GetCoordinateConverter()->GetOffset(); 605 | 606 | if(map_.map.info.width != (unsigned int) width || 607 | map_.map.info.height != (unsigned int) height || 608 | map_.map.info.origin.position.x != offset.GetX() || 609 | map_.map.info.origin.position.y != offset.GetY()) 610 | { 611 | map_.map.info.origin.position.x = offset.GetX(); 612 | map_.map.info.origin.position.y = offset.GetY(); 613 | map_.map.info.width = width; 614 | map_.map.info.height = height; 615 | map_.map.data.resize(map_.map.info.width * map_.map.info.height); 616 | } 617 | 618 | for (kt_int32s y=0; yGetValue(karto::Vector2(x, y)); 624 | 625 | switch (value) 626 | { 627 | case karto::GridStates_Unknown: 628 | map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1; 629 | break; 630 | case karto::GridStates_Occupied: 631 | map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100; 632 | break; 633 | case karto::GridStates_Free: 634 | map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0; 635 | break; 636 | default: 637 | ROS_WARN("Encountered unknown cell value at %d, %d", x, y); 638 | break; 639 | } 640 | } 641 | } 642 | 643 | // Set the header information on the map 644 | map_.map.header.stamp = ros::Time::now(); 645 | map_.map.header.frame_id = map_frame_; 646 | 647 | sst_.publish(map_.map); 648 | sstm_.publish(map_.map.info); 649 | 650 | delete occ_grid; 651 | 652 | return true; 653 | } 654 | 655 | bool 656 | SlamKarto::addScan(karto::LaserRangeFinder* laser, 657 | const sensor_msgs::LaserScan::ConstPtr& scan, 658 | karto::Pose2& karto_pose) 659 | { 660 | if(!getOdomPose(karto_pose, scan->header.stamp)) 661 | return false; 662 | 663 | // Create a vector of doubles for karto 664 | std::vector readings; 665 | 666 | if (lasers_inverted_[scan->header.frame_id]) { 667 | for(std::vector::const_reverse_iterator it = scan->ranges.rbegin(); 668 | it != scan->ranges.rend(); 669 | ++it) 670 | { 671 | readings.push_back(*it); 672 | } 673 | } else { 674 | for(std::vector::const_iterator it = scan->ranges.begin(); 675 | it != scan->ranges.end(); 676 | ++it) 677 | { 678 | readings.push_back(*it); 679 | } 680 | } 681 | 682 | // create localized range scan 683 | karto::LocalizedRangeScan* range_scan = 684 | new karto::LocalizedRangeScan(laser->GetName(), readings); 685 | range_scan->SetOdometricPose(karto_pose); 686 | range_scan->SetCorrectedPose(karto_pose); 687 | 688 | // Add the localized range scan to the mapper 689 | bool processed; 690 | if((processed = mapper_->Process(range_scan))) 691 | { 692 | //std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl; 693 | 694 | karto::Pose2 corrected_pose = range_scan->GetCorrectedPose(); 695 | 696 | // Compute the map->odom transform 697 | tf::Stamped odom_to_map; 698 | try 699 | { 700 | tf_.transformPose(odom_frame_,tf::Stamped (tf::Transform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()), 701 | tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(), 702 | scan->header.stamp, base_frame_),odom_to_map); 703 | } 704 | catch(tf::TransformException e) 705 | { 706 | ROS_ERROR("Transform from base_link to odom failed\n"); 707 | odom_to_map.setIdentity(); 708 | } 709 | 710 | map_to_odom_mutex_.lock(); 711 | map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ), 712 | tf::Point( odom_to_map.getOrigin() ) ).inverse(); 713 | map_to_odom_mutex_.unlock(); 714 | 715 | 716 | // Add the localized range scan to the dataset (for memory management) 717 | dataset_->Add(range_scan); 718 | } 719 | else 720 | delete range_scan; 721 | 722 | return processed; 723 | } 724 | 725 | bool 726 | SlamKarto::mapCallback(nav_msgs::GetMap::Request &req, 727 | nav_msgs::GetMap::Response &res) 728 | { 729 | boost::mutex::scoped_lock lock(map_mutex_); 730 | if(got_map_ && map_.map.info.width && map_.map.info.height) 731 | { 732 | res = map_; 733 | return true; 734 | } 735 | else 736 | return false; 737 | } 738 | 739 | int 740 | main(int argc, char** argv) 741 | { 742 | ros::init(argc, argv, "slam_karto_g2o"); 743 | 744 | SlamKarto kn; 745 | 746 | ros::spin(); 747 | 748 | return 0; 749 | } 750 | --------------------------------------------------------------------------------