├── CMakeLists.txt ├── Makefile ├── README ├── cmake ├── FindCholmod.cmake └── FindEigen3.cmake ├── demo_rgbd.launch ├── demo_rgbd.vcg ├── include └── isam │ ├── Anchor.h │ ├── Cholesky.h │ ├── ChowLiuTree.h │ ├── Covariances.h │ ├── Element.h │ ├── Factor.h │ ├── GLCReparam.h │ ├── Graph.h │ ├── Jacobian.h │ ├── Node.h │ ├── Noise.h │ ├── OptimizationInterface.h │ ├── Optimizer.h │ ├── OrderedSparseMatrix.h │ ├── Point2d.h │ ├── Point3d.h │ ├── Point3dh.h │ ├── Pose2d.h │ ├── Pose3d.h │ ├── Properties.h │ ├── Rot3d.h │ ├── Slam.h │ ├── SparseMatrix.h │ ├── SparseSystem.h │ ├── SparseVector.h │ ├── covariance.h │ ├── glc.h │ ├── isam.h │ ├── numericalDiff.h │ ├── robust.h │ ├── slam2d.h │ ├── slam3d.h │ ├── slam_depthmono.h │ ├── slam_monocular.h │ ├── slam_stereo.h │ └── util.h ├── isamlib ├── Anchor.cpp ├── CMakeLists.txt ├── Cholesky.cpp ├── ChowLiuTree.cpp ├── Covariances.cpp ├── GLCReparam.cpp ├── Node.cpp ├── Optimizer.cpp ├── OrderedSparseMatrix.cpp ├── Slam.cpp ├── SparseMatrix.cpp ├── SparseSystem.cpp ├── SparseVector.cpp ├── covariance.cpp ├── glc.cpp ├── numericalDiff.cpp └── util.cpp ├── manifest.xml ├── src ├── bundleAdjust.cpp ├── cameraParameters.h ├── featureTracking.cpp ├── featureTracking_ocl.cpp ├── pointDefinition.h ├── processDepthmap.cpp ├── registerPointCloud.cpp ├── stackDepthPoint.cpp ├── transformMaintenance.cpp └── visualOdometry.cpp └── stack.xml /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 33 | find_package(Eigen3 REQUIRED) 34 | include_directories(${EIGEN3_INCLUDE_DIR}) 35 | include_directories("${PROJECT_SOURCE_DIR}/include") 36 | 37 | add_subdirectory(isamlib) 38 | link_libraries(isamlib) 39 | 40 | rosbuild_add_executable(featureTracking src/featureTracking.cpp) 41 | rosbuild_add_executable(visualOdometry src/visualOdometry.cpp) 42 | rosbuild_add_executable(bundleAdjust src/bundleAdjust.cpp) 43 | rosbuild_add_executable(processDepthmap src/processDepthmap.cpp) 44 | rosbuild_add_executable(stackDepthPoint src/stackDepthPoint.cpp) 45 | rosbuild_add_executable(transformMaintenance src/transformMaintenance.cpp) 46 | rosbuild_add_executable(registerPointCloud src/registerPointCloud.cpp) 47 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | Depth Enhanced Monocular Odometry (Demo) is a monocular visual odometry method assisted by depth maps, and optionally an IMU. The program contains three major nodes. The "featureTracking" node extracts and tracks Harris corners by Kanade Lucas Tomasi (KLT) tracker provided in the OpenCV library. The "visualOdometry" node takes the tracked features and estimates frame to frame motion. Features associated with depth (either from the depth map or triangulated from previously estimated camera motion) are used to solve the 6DOF motion, and features without depth help solve orientation. The "bundleAdjust" node refines the estimated motion with bundle adjustment. It processes sequences of images using Incremental Smoothing and Mapping (iSAM) open source library. 2 | 3 | The program is tested on ROS Fuerte, on a laptop computer with 2.5 GHz quad cores and 6 Gib memory. This version uses an RGBD camera. 4 | 5 | Wiki Webpage: http://wiki.ros.org/demo_rgbd 6 | 7 | A slimmed down version of the program (without the bundle adjustment) is available for running on embedded systems 8 | 9 | GitHub Code: https://github.com/jizhang-cmu/demo_rgbd_slimmed.git 10 | 11 | Also, another version of the program that uses a camera and a 3D lidar is available at 12 | 13 | Wiki Webpage: http://wiki.ros.org/demo_lidar 14 | 15 | GitHub Code: https://github.com/jizhang-cmu/demo_lidar.git 16 | 17 | How to use: 18 | 19 | 1) Install additional packages by "sudo apt-get install libsuitesparse-dev libeigen3-dev libsdl1.2-dev". These packages are required by iSAM library for the bundle adjustment. 20 | 21 | 2) Download the program file to a ROS directory, unpack the file and rename the folder to “demo_rgbd” (GitHub may add "-xxx" to the end of the folder name). Go to the folder and “rosmake”, then “roslaunch demo_rgbd.launch”. The launch file should start the program and rviz. 22 | 23 | 3) Download datasets from the following website. Make sure the data files are for the RGBD camera (not camera and lidar) version. Play the data files with “rosbag play data_file_name.bag”. Note that if a slow computer is used, users can try to play the data files at a lower speed, e.g. “rosbag play data_file_name.bag -r 0.5” plays the data file at half speed. 24 | 25 | Datasets can be downloaded at: http://www.frc.ri.cmu.edu/~jizhang03/projects.htm 26 | 27 | Notes: 28 | 29 | Camera intrinsic parameters (K matrices for rgb and depth images) are defined in the "src/cameraParameters.h" file. The program does not use "/camera_info" messages in the data files. The current parameters are set at the default values for Xtion sensors. 30 | 31 | The current program uses “/camera/rgb/image_rect” and “/camera/depth_registered/image” messages published by the “openni_camera” driver. Users can also remap from other messages such as “/camera/rgb/image” (for rgb images) and “/camera/depth/image” (for depth images). However, if using “/camera/depth/image”, pleases remember to change “kDepth” (K matrix for depth images) in the “cameraParameters.h” file to those in the “/camera/depth/camera_info” messages. 32 | 33 | It is possible to accelerate feature tracking with a GPU. To do this, simply replace the "src/featureTracking.cpp" file with the "src/featureTracking_ocl.cpp" file and recompile. We use OpenCL to communicate with the GPU. Users first need to install OpenCL in a version no earlier than 1.1 with full profile, then install OpenCV using CMake with flag WITH_OPENCL=ON. 34 | -------------------------------------------------------------------------------- /cmake/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDES 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ) 17 | 18 | find_library(CHOLMOD_LIBRARIES cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 19 | 20 | if(CHOLMOD_LIBRARIES) 21 | 22 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 23 | 24 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 25 | if (AMD_LIBRARY) 26 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 27 | else () 28 | set(CHOLMOD_LIBRARIES FALSE) 29 | endif () 30 | 31 | endif(CHOLMOD_LIBRARIES) 32 | 33 | if(CHOLMOD_LIBRARIES) 34 | 35 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 36 | if (COLAMD_LIBRARY) 37 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 38 | else () 39 | set(CHOLMOD_LIBRARIES FALSE) 40 | endif () 41 | 42 | endif(CHOLMOD_LIBRARIES) 43 | 44 | if(CHOLMOD_LIBRARIES) 45 | 46 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 47 | if (CAMD_LIBRARY) 48 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 49 | else () 50 | set(CHOLMOD_LIBRARIES FALSE) 51 | endif () 52 | 53 | endif(CHOLMOD_LIBRARIES) 54 | 55 | if(CHOLMOD_LIBRARIES) 56 | 57 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 58 | if (CCOLAMD_LIBRARY) 59 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 60 | else () 61 | set(CHOLMOD_LIBRARIES FALSE) 62 | endif () 63 | 64 | endif(CHOLMOD_LIBRARIES) 65 | 66 | if(CHOLMOD_LIBRARIES) 67 | 68 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 69 | if (CHOLMOD_METIS_LIBRARY) 70 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 71 | endif () 72 | 73 | endif(CHOLMOD_LIBRARIES) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 77 | CHOLMOD_INCLUDES CHOLMOD_LIBRARIES) 78 | 79 | mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY) 80 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /demo_rgbd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /demo_rgbd.vcg: -------------------------------------------------------------------------------- 1 | AfterMapped.Enabled=0 2 | AfterMapped.Length=0.5 3 | AfterMapped.Radius=0.12 4 | AfterMapped.Reference\ Frame=/aft_ba 5 | Background\ ColorB=0 6 | Background\ ColorG=0 7 | Background\ ColorR=0 8 | Camera\ Config=-0.219797 1.59541 21.6229 0.536626 -0.591043 4.7155 9 | Camera\ Type=rviz::OrbitViewController 10 | Camera2.Enabled=1 11 | Camera2.Length=1 12 | Camera2.Radius=0.1 13 | Camera2.Reference\ Frame=/camera2 14 | Depthmap.Enabled=1 15 | Depthmap.Image\ Topic=/camera/depth_registered/image 16 | Depthmap.Queue\ Size=2 17 | Depthmap.Transport\ Hint=raw 18 | Fixed\ Frame=/camera_init 19 | Image.Enabled=1 20 | Image.Image\ Topic=/image/show_2 21 | Image.Queue\ Size=2 22 | Image.Transport\ Hint=raw 23 | Odometry.Angle\ Tolerance=0.1 24 | Odometry.ColorB=0 25 | Odometry.ColorG=0.1 26 | Odometry.ColorR=1 27 | Odometry.Enabled=1 28 | Odometry.Keep=1000 29 | Odometry.Length=1 30 | Odometry.Position\ Tolerance=0.1 31 | Odometry.Topic=/cam2_to_init 32 | Property\ Grid\ Splitter=662,0 33 | Property\ Grid\ State=expanded=.Global Options;splitterratio=0.5 34 | QMainWindow=000000ff00000000fd000000030000000000000118000004bbfc0200000003fb000000100044006900730070006c006100790073010000002d000002df000000c300fffffffb0000000a0049006d0061006700650100000312000000e90000001400fffffffb0000001000440065007000740068006d006100700100000401000000e70000001400ffffff000000010000013100000477fc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000002d000004770000005100fffffffb0000000a0056006900650077007302000002ae000000000000013100000400fb0000001200530065006c0065006300740069006f006e000000001d0000039b0000005100ffffff00000003000004cf0000003efc0100000001fb0000000800540069006d00650000000000000004cf0000021400ffffff000002b1000004bb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 35 | Target\ Frame=/camera_init 36 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 37 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 38 | camera.Enabled=0 39 | camera.Length=1 40 | camera.Radius=0.1 41 | camera.Reference\ Frame=/camera 42 | cameraInit.Enabled=0 43 | cameraInit.Length=1.5 44 | cameraInit.Radius=0.1 45 | cameraInit.Reference\ Frame=/camera_init 46 | depthCloud..AxisColorAutocompute\ Value\ Bounds=1 47 | depthCloud..AxisColorAxis=2 48 | depthCloud..AxisColorMax\ Value=14.7891 49 | depthCloud..AxisColorMin\ Value=8.66777 50 | depthCloud..AxisColorUse\ Fixed\ Frame=1 51 | depthCloud..FlatColorColorB=0 52 | depthCloud..FlatColorColorG=1 53 | depthCloud..FlatColorColorR=1 54 | depthCloud..IntensityAutocompute\ Intensity\ Bounds=1 55 | depthCloud..IntensityChannel\ Name=intensity 56 | depthCloud..IntensityMax\ ColorB=1 57 | depthCloud..IntensityMax\ ColorG=1 58 | depthCloud..IntensityMax\ ColorR=1 59 | depthCloud..IntensityMax\ Intensity=4096 60 | depthCloud..IntensityMin\ ColorB=0 61 | depthCloud..IntensityMin\ ColorG=0 62 | depthCloud..IntensityMin\ ColorR=0 63 | depthCloud..IntensityMin\ Intensity=0 64 | depthCloud..IntensityUse\ full\ RGB\ spectrum=0 65 | depthCloud.Alpha=1 66 | depthCloud.Billboard\ Size=0.02 67 | depthCloud.Color\ Transformer=FlatColor 68 | depthCloud.Decay\ Time=0 69 | depthCloud.Enabled=0 70 | depthCloud.Position\ Transformer=XYZ 71 | depthCloud.Queue\ Size=10 72 | depthCloud.Selectable=1 73 | depthCloud.Style=1 74 | depthCloud.Topic=/depth_cloud 75 | imageCloud..AxisColorAutocompute\ Value\ Bounds=1 76 | imageCloud..AxisColorAxis=2 77 | imageCloud..AxisColorMax\ Value=4.61162 78 | imageCloud..AxisColorMin\ Value=1.1223 79 | imageCloud..AxisColorUse\ Fixed\ Frame=1 80 | imageCloud..FlatColorColorB=1 81 | imageCloud..FlatColorColorG=1 82 | imageCloud..FlatColorColorR=1 83 | imageCloud..IntensityAutocompute\ Intensity\ Bounds=1 84 | imageCloud..IntensityChannel\ Name=intensity 85 | imageCloud..IntensityMax\ ColorB=1 86 | imageCloud..IntensityMax\ ColorG=1 87 | imageCloud..IntensityMax\ ColorR=1 88 | imageCloud..IntensityMax\ Intensity=4096 89 | imageCloud..IntensityMin\ ColorB=0 90 | imageCloud..IntensityMin\ ColorG=0 91 | imageCloud..IntensityMin\ ColorR=0 92 | imageCloud..IntensityMin\ Intensity=0 93 | imageCloud..IntensityUse\ full\ RGB\ spectrum=0 94 | imageCloud.Alpha=1 95 | imageCloud.Billboard\ Size=0.05 96 | imageCloud.Color\ Transformer=FlatColor 97 | imageCloud.Decay\ Time=0 98 | imageCloud.Enabled=1 99 | imageCloud.Position\ Transformer=XYZ 100 | imageCloud.Queue\ Size=10 101 | imageCloud.Selectable=1 102 | imageCloud.Style=1 103 | imageCloud.Topic=/image_points_proj 104 | surroundCloud..AxisColorAutocompute\ Value\ Bounds=1 105 | surroundCloud..AxisColorAxis=2 106 | surroundCloud..AxisColorMax\ Value=2.81577 107 | surroundCloud..AxisColorMin\ Value=1.33099 108 | surroundCloud..AxisColorUse\ Fixed\ Frame=1 109 | surroundCloud..FlatColorColorB=1 110 | surroundCloud..FlatColorColorG=1 111 | surroundCloud..FlatColorColorR=1 112 | surroundCloud..IntensityAutocompute\ Intensity\ Bounds=1 113 | surroundCloud..IntensityChannel\ Name=intensity 114 | surroundCloud..IntensityMax\ ColorB=1 115 | surroundCloud..IntensityMax\ ColorG=1 116 | surroundCloud..IntensityMax\ ColorR=1 117 | surroundCloud..IntensityMax\ Intensity=10 118 | surroundCloud..IntensityMin\ ColorB=0 119 | surroundCloud..IntensityMin\ ColorG=0 120 | surroundCloud..IntensityMin\ ColorR=0 121 | surroundCloud..IntensityMin\ Intensity=10 122 | surroundCloud..IntensityUse\ full\ RGB\ spectrum=0 123 | surroundCloud.Alpha=1 124 | surroundCloud.Billboard\ Size=0.04 125 | surroundCloud.Color\ Transformer=AxisColor 126 | surroundCloud.Decay\ Time=0 127 | surroundCloud.Enabled=1 128 | surroundCloud.Position\ Transformer=XYZ 129 | surroundCloud.Queue\ Size=10 130 | surroundCloud.Selectable=1 131 | surroundCloud.Style=1 132 | surroundCloud.Topic=/surround_cloud 133 | [Display0] 134 | ClassName=rviz::ImageDisplay 135 | Name=Image 136 | [Display1] 137 | ClassName=rviz::ImageDisplay 138 | Name=Depthmap 139 | [Display2] 140 | ClassName=rviz::PointCloud2Display 141 | Name=depthCloud 142 | [Display3] 143 | ClassName=rviz::PointCloud2Display 144 | Name=imageCloud 145 | [Display4] 146 | ClassName=rviz::PointCloud2Display 147 | Name=surroundCloud 148 | [Display5] 149 | ClassName=rviz::AxesDisplay 150 | Name=cameraInit 151 | [Display6] 152 | ClassName=rviz::AxesDisplay 153 | Name=camera 154 | [Display7] 155 | ClassName=rviz::AxesDisplay 156 | Name=Camera2 157 | [Display8] 158 | ClassName=rviz::AxesDisplay 159 | Name=AfterMapped 160 | [Display9] 161 | ClassName=rviz::OdometryDisplay 162 | Name=Odometry 163 | [Window] 164 | Height=1256 165 | Width=975 166 | X=-947 167 | Y=-1204 168 | -------------------------------------------------------------------------------- /include/isam/Anchor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Anchor.h 3 | * @brief Implementation of the anchor node. 4 | * @author Hordur Johannsson 5 | * @author Michael Kaess 6 | * @version $Id: Anchor.h 6334 2012-03-22 18:53:24Z hordurj $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "Node.h" 32 | #include "Factor.h" 33 | #include "Pose2d.h" 34 | #include "Point2d.h" 35 | #include "Pose3d.h" 36 | #include "Slam.h" 37 | 38 | namespace isam { 39 | 40 | typedef NodeT Pose2d_Node; 41 | typedef NodeT Pose3d_Node; 42 | 43 | /** 44 | * A anchor node for 2d poses 45 | */ 46 | class Anchor2d_Node : public Pose2d_Node { 47 | public: 48 | Anchor2d_Node(Slam* slam); 49 | ~Anchor2d_Node(); 50 | 51 | /** 52 | * Add a prior to the anchor. The prior will be removed 53 | * if this anchor is merged with another anchor's frame. 54 | */ 55 | void set_prior(); 56 | 57 | /** 58 | * Add a new anchor to this frame. 59 | */ 60 | void add_anchor(Anchor2d_Node* a); 61 | 62 | /** 63 | * Merges the node with anchor a. 64 | * 65 | * @param a the node to merge with. 66 | * @param old_origin the pose of this frame in the new frame. 67 | * 68 | * Usage: b.merge(a); 69 | * 70 | * All anchors in a's frame will be merged with the b's frame. 71 | * 72 | */ 73 | void merge(Anchor2d_Node* a, Pose2d old_origin); 74 | 75 | Anchor2d_Node* parent() { return _parent; } 76 | 77 | private: 78 | Anchor2d_Node* _parent; 79 | std::vector _childs; 80 | Factor* _prior; 81 | Slam* _slam; 82 | }; 83 | 84 | /** 85 | * A anchor node for 3d poses 86 | */ 87 | class Anchor3d_Node : public Pose3d_Node { 88 | public: 89 | Anchor3d_Node(Slam* slam); 90 | ~Anchor3d_Node(); 91 | 92 | /** 93 | * Add a prior to the anchor. The prior will be removed 94 | * if this anchor is merged with another anchor's frame. 95 | */ 96 | void set_prior(); 97 | 98 | /** 99 | * Add a new anchor to this frame. 100 | */ 101 | void add_anchor(Anchor3d_Node* a); 102 | 103 | /** 104 | * Merges the node with anchor a. 105 | * 106 | * @param a the node to merge with. 107 | * @param old_origin the pose of this frame in the new frame. 108 | * 109 | * Usage: b.merge(a); 110 | * 111 | * All anchors in a's frame will be merged with the b's frame. 112 | * 113 | */ 114 | void merge(Anchor3d_Node* a, Pose3d old_origin); 115 | 116 | Anchor3d_Node* parent() { return _parent; } 117 | 118 | private: 119 | Anchor3d_Node* _parent; 120 | std::vector _childs; 121 | Factor* _prior; 122 | Slam* _slam; 123 | }; 124 | 125 | } 126 | -------------------------------------------------------------------------------- /include/isam/Cholesky.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Cholesky.h 3 | * @brief Cholesky batch factorization using SuiteSparse by Tim Davis. 4 | * @author Michael Kaess 5 | * @version $Id: Cholesky.h 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "SparseSystem.h" 33 | 34 | namespace isam { 35 | 36 | class Cholesky { 37 | public: 38 | virtual ~Cholesky() {} 39 | 40 | /** 41 | * Factorize a given system Ax=b and optionally solve. 42 | * @param Ab SparseSystem with measurement Jacobian A and right hand side b. 43 | * @param delta Optional parameter to return solution of system. 44 | * @param lambda Adds elements to diagonal of information matrix A'A before 45 | * factorization, used for Levenberg-Marquardt algorithm. 46 | */ 47 | virtual void factorize(const SparseSystem& Ab, Eigen::VectorXd* delta = NULL, double lambda = 0.) = 0; 48 | 49 | /** 50 | * Copy R into a SparseSystem data structure (expensive, so can be 51 | * avoided during batch factorization). 52 | * @param R SparseSystem that upon return will contain the R factor. 53 | */ 54 | virtual void get_R(SparseSystem& R) = 0; 55 | 56 | /** 57 | * Access the variable ordering used for Cholesky factorization. 58 | * @return Pointer to variable ordering. 59 | */ 60 | virtual int* get_order() = 0; 61 | 62 | static Cholesky* Create(); 63 | 64 | protected: 65 | Cholesky() {} 66 | }; 67 | 68 | } 69 | -------------------------------------------------------------------------------- /include/isam/ChowLiuTree.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ChowLiuTree.h 3 | * @brief ChowLiuTree for information form Gaussian distributions 4 | * @author Nicholas Carlevaris-Bianco 5 | * @author Michael Kaess 6 | * @version $Id: covariance.cpp 4975 2011-07-13 17:49:09Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "Node.h" 39 | 40 | 41 | namespace isam { 42 | 43 | 44 | /** 45 | * ChowLiuTreeInfo 46 | * Information for Gaussian distribtuion for chow liu tree 47 | * 48 | */ 49 | class ChowLiuTreeInfo { 50 | 51 | private: 52 | 53 | const Eigen::MatrixXd& _L; 54 | const std::vector& _nodes; 55 | 56 | public: 57 | 58 | /** 59 | * Constructor. 60 | * @param L the information matrix 61 | * @param nodes the nodes in the information matrix 62 | */ 63 | ChowLiuTreeInfo (const Eigen::MatrixXd& L, const std::vector& nodes) 64 | : _L(L), _nodes(nodes) { } 65 | 66 | //const std::vector& nodes() {return _nodes;} 67 | int num_nodes() {return _nodes.size();} 68 | 69 | /** 70 | * Marginal distribution 71 | * @return Information matrix P(node[id]) 72 | */ 73 | Eigen::MatrixXd marginal(int id); 74 | 75 | /** 76 | * Joint distribution 77 | * @return Information matrix P(node[ida], node[idb]) 78 | */ 79 | Eigen::MatrixXd joint(int ida, int idb); // a, b 80 | 81 | /** 82 | * Conditional distribution 83 | * @return Information matrix P(node[ida] | node[idb]) 84 | */ 85 | Eigen::MatrixXd conditional(int ida, int idb); 86 | 87 | }; 88 | 89 | /** 90 | * ChowLiuTreeNode 91 | * A node in the chowliu tree 92 | * 93 | */ 94 | class ChowLiuTreeNode { 95 | 96 | public: 97 | 98 | int id; 99 | int pid; 100 | std::vector cids; 101 | Eigen::MatrixXd marginal; // marginal information 102 | Eigen::MatrixXd conditional; // conditional information with parrent 103 | Eigen::MatrixXd joint; // joint information with parrent 104 | 105 | bool is_root () { return (pid == -1) ? true : false; } 106 | bool is_leaf () { return (cids.size() == 0) ? true : false; } 107 | 108 | }; 109 | 110 | class MI { 111 | 112 | public: 113 | int id1; 114 | int id2; 115 | double mi; 116 | 117 | MI (int id1_, int id2_, double mi_) 118 | : id1(id1_), id2(id2_), mi(mi_){ 119 | } 120 | 121 | }; 122 | 123 | 124 | 125 | /** 126 | * ChowLiuTree 127 | * Chow Liu Tree class for information form gaussian distribtuions 128 | * 129 | */ 130 | class ChowLiuTree { 131 | 132 | ChowLiuTreeInfo _clt_info; 133 | std::list _edges; 134 | 135 | void _calc_edges(); 136 | double _calc_mi(int ida, int idb); 137 | void _max_span_tree(); 138 | void _build_tree_rec(int id, int pid); 139 | 140 | public: 141 | 142 | // the resulting chow-liu tree 143 | std::map tree; 144 | 145 | ChowLiuTree (const Eigen::MatrixXd &L, const std::vector& nodes); 146 | 147 | }; 148 | 149 | 150 | } // namespace isam 151 | -------------------------------------------------------------------------------- /include/isam/Covariances.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Covariances.h 3 | * @brief Providing access to entries of the covariance. 4 | * @author Michael Kaess 5 | * @version $Id: Covariances.h 7858 2013-01-14 03:50:24Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "SparseSystem.h" 35 | #include "Node.h" 36 | #include "covariance.h" 37 | 38 | namespace isam { 39 | 40 | class Slam; 41 | 42 | class Covariances { 43 | private: 44 | // either directly coupled to a Slam object... 45 | Slam* _slam; 46 | 47 | // ...or we operate on a copy of the relevant data from a Slam object 48 | SparseSystem _R; 49 | std::map > _index; 50 | 51 | mutable CovarianceCache _cache; 52 | 53 | // utility function for _index 54 | int get_start(Node* node) const; 55 | int get_dim(Node* node) const; 56 | 57 | // only used for cloning below 58 | Covariances(Slam& slam); 59 | 60 | public: 61 | 62 | /** 63 | * Create an instance based on a Slam object, that always refers to 64 | * the latest state of slam. 65 | */ 66 | Covariances(Slam* slam) : _slam(slam), _R(1,1) {} 67 | 68 | virtual ~Covariances() {}; 69 | 70 | /** 71 | * Create a stand-alone copy, useful for calculating covariances in 72 | * a separate thread. Copies all necessary data structures to work 73 | * independently of the Slam object. 74 | * @return Covariances object that is independent of Slam object. 75 | */ 76 | virtual Covariances clone() const { 77 | return Covariances(*_slam); 78 | } 79 | 80 | typedef std::list > node_lists_t; 81 | typedef std::list > node_pair_list_t; 82 | 83 | /** 84 | * Calculates marginal covariance over a list of 85 | * lists. Significantly more efficient than calling 86 | * marginal_covariance multiple times with separate lists, as 87 | * intermediate results are being reused. 88 | * @param node_lists List of list of nodes. 89 | * @return List of marginal covariance matrices. 90 | */ 91 | virtual std::list marginal(const node_lists_t& node_lists) const; 92 | 93 | /** 94 | * Calculates marginal covariance over a list of nodes. 95 | * @param nodes List of nodes. 96 | * return Marginal covariance matrix. 97 | */ 98 | virtual Eigen::MatrixXd marginal(const std::list& nodes) const; 99 | 100 | /** 101 | * Calculates individual entries of the covariance matrix (as 102 | * opposed to marginal_covariance, which calculates blocks 103 | * containing select variables). Note that a single call with a long 104 | * list of entries is significantly more efficient than repeatedly 105 | * calling this function, as intermediate results are being reused. 106 | * @param entry_list List of pairs of nodes, indexing entries in 107 | * the covariance matrix in (column, row) format. 108 | * @return List of matrices. 109 | */ 110 | virtual std::list access(const node_pair_list_t& node_pair_list) const; 111 | 112 | }; 113 | 114 | } 115 | -------------------------------------------------------------------------------- /include/isam/Element.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Element.h 3 | * @brief Basic functionality for nodes and factors of a graph. 4 | * @author Michael Kaess 5 | * @version $Id: Element.h 5796 2011-12-07 00:39:30Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | namespace isam { 34 | 35 | class Element { 36 | Element(const Element& rhs); // not allowed 37 | const Element& operator= (const Element& rhs); // not allowed 38 | 39 | const char* _name; 40 | 41 | int _start; // needed for Slam::jacobian 42 | 43 | protected: 44 | int _id; 45 | int _dim; 46 | 47 | public: 48 | Element(const char* name, int dim) : _name(name), _dim(dim) {} 49 | virtual ~Element() {}; 50 | 51 | virtual int unique_id() {return _id;} 52 | virtual const char* name() const {return _name;} 53 | inline int dim() const {return _dim;} 54 | inline int start() const {return _start;} 55 | 56 | virtual void write(std::ostream &out) const { 57 | out << name(); 58 | } 59 | 60 | friend class Slam; 61 | friend class Covariances; 62 | }; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /include/isam/Factor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Factor.h 3 | * @brief Graph factor for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: Factor.h 7610 2012-10-25 10:21:12Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include // for sqrt 34 | #include 35 | 36 | #include "util.h" 37 | #include "Jacobian.h" 38 | #include "Element.h" 39 | #include "Node.h" 40 | #include "Noise.h" 41 | #include "numericalDiff.h" 42 | 43 | namespace isam { 44 | 45 | //typedef double (*cost_func_t)(double); 46 | 47 | 48 | // Factor of the graph of measurements between Nodes. 49 | class Factor : public Element, Function { 50 | friend std::ostream& operator<<(std::ostream& output, const Factor& e) { 51 | e.write(output); 52 | return output; 53 | } 54 | 55 | cost_func_t *ptr_cost_func; 56 | 57 | static int _next_id; 58 | bool _deleted; 59 | protected: 60 | 61 | const Noise _noise; 62 | 63 | std::vector _nodes; // list of nodes affected by measurement 64 | 65 | public: 66 | 67 | virtual Eigen::VectorXd error(Selector s = ESTIMATE) const { 68 | Eigen::VectorXd err = _noise.sqrtinf() * basic_error(s); 69 | // optional modified cost function 70 | if (*ptr_cost_func) { 71 | for (int i=0; i=0)?1.:(-1.)) * sqrt((*ptr_cost_func)(val)); 74 | } 75 | } 76 | return err; 77 | } 78 | 79 | std::vector& nodes() {return _nodes;} 80 | 81 | Factor(const char* name, int dim, const Noise& noise) 82 | : Element(name, dim), ptr_cost_func(NULL), _deleted(false), _noise(noise) { 83 | #ifndef NDEBUG 84 | // all lower triagular entries below the diagonal must be 0 85 | for (int r=0; r<_noise.sqrtinf().rows(); r++) { 86 | for (int c=0; cadd_factor(this); 101 | } 102 | initialize(); 103 | } 104 | 105 | virtual void set_cost_function(cost_func_t* ptr) {ptr_cost_func = ptr;} 106 | 107 | virtual Eigen::VectorXd basic_error(Selector s = ESTIMATE) const = 0; 108 | 109 | virtual const Eigen::MatrixXd& sqrtinf() const {return _noise.sqrtinf();} 110 | 111 | Eigen::VectorXd evaluate() const { 112 | return error(LINPOINT); 113 | } 114 | 115 | virtual Jacobian jacobian_internal(bool force_numerical) { 116 | if (force_numerical) { 117 | // ignore any symbolic derivative provided by user 118 | return Factor::jacobian(); 119 | } else { 120 | return jacobian(); 121 | } 122 | } 123 | 124 | // can be replaced by symbolic derivative by user 125 | virtual Jacobian jacobian() { 126 | Eigen::MatrixXd H = numerical_jacobian(); 127 | Eigen::VectorXd r = error(LINPOINT); 128 | Jacobian jac(r); 129 | int position = 0; 130 | int n_measure = dim(); 131 | for (unsigned int i=0; i<_nodes.size(); i++) { 132 | int n_var = _nodes[i]->dim(); 133 | Eigen::MatrixXd Hi = H.block(0, position, n_measure, n_var); 134 | position += n_var; 135 | jac.add_term(_nodes[i], Hi); 136 | } 137 | return jac; 138 | } 139 | 140 | int num_measurements() const { 141 | return dim(); 142 | } 143 | 144 | void mark_deleted() { _deleted = true; } 145 | bool deleted() const { return _deleted; } 146 | 147 | virtual void write(std::ostream &out) const { 148 | Element::write(out); 149 | for (unsigned int i=0; i<_nodes.size(); i++) { 150 | if (_nodes[i]) { 151 | out << " " << _nodes[i]->unique_id(); 152 | } 153 | } 154 | } 155 | 156 | private: 157 | 158 | virtual Eigen::MatrixXd numerical_jacobian() { 159 | return numericalDiff(*this); 160 | } 161 | 162 | }; 163 | 164 | /** 165 | * Convert upper triangular square root information matrix to string. 166 | * @param sqrtinf Upper triangular square matrix. 167 | */ 168 | inline std::string noise_to_string(const Noise& noise) { 169 | int nrows = noise.sqrtinf().rows(); 170 | int ncols = noise.sqrtinf().cols(); 171 | require(nrows==ncols, "slam2d::sqrtinf_to_string: matrix must be square"); 172 | std::stringstream s; 173 | s << "{"; 174 | bool first = true; 175 | for (int r=0; r 191 | class FactorT : public Factor { 192 | 193 | protected: 194 | 195 | const T _measure; 196 | 197 | public: 198 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 199 | 200 | FactorT(const char* name, int dim, const Noise& noise, const T& measure) : Factor(name, dim, noise), _measure(measure) {} 201 | 202 | const T& measurement() const {return _measure;} 203 | 204 | void write(std::ostream &out) const { 205 | Factor::write(out); 206 | out << " " << _measure << " " << noise_to_string(_noise); 207 | } 208 | 209 | }; 210 | 211 | 212 | } 213 | -------------------------------------------------------------------------------- /include/isam/GLCReparam.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GLCReparam.h 3 | * @brief Generic Linear Constraint Reparametrization 4 | * @author Nicholas Carlevaris-Bianco 5 | * @author Michael Kaess 6 | * @version $Id: GLCReparam.h 8578 2013-07-01 00:28:49Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "Node.h" 36 | #include "Factor.h" 37 | #include "Anchor.h" 38 | 39 | namespace isam { 40 | 41 | /** 42 | * interface class to allow for reparametrization by the user 43 | * see GLC_RootShift implementation as an example 44 | */ 45 | class GLC_Reparam { 46 | 47 | public: 48 | 49 | /** 50 | * set_nodes() 51 | * @param nodes Vector of Node pointers which support the factor 52 | */ 53 | virtual void set_nodes (std::vector nodes) = 0; 54 | /** 55 | * clone() 56 | * @return a new instance of the derived class 57 | */ 58 | virtual GLC_Reparam* clone() = 0; 59 | /** 60 | * is_angle() 61 | * @return boolean vector indacating if elemetes reparameterized vector are angles 62 | */ 63 | virtual Eigen::VectorXb is_angle() = 0; 64 | /** 65 | * jacobian() 66 | * @return jacobian of reparameterized output wrt node input 67 | */ 68 | virtual Eigen::MatrixXd jacobian() = 0; 69 | /** 70 | * reparameterize() 71 | * @return reparameterized vector 72 | */ 73 | virtual Eigen::VectorXd reparameterize (Selector s) = 0; 74 | 75 | }; 76 | 77 | /** 78 | * GLC_RootShift 79 | * implementation of GLC_Reparam 80 | * performs root shift operation 81 | */ 82 | class GLC_RootShift : public GLC_Reparam { 83 | 84 | private: 85 | std::vector _nodes; 86 | Eigen::VectorXb _is_angle; 87 | int _dim; // input and output dim 88 | 89 | public: 90 | 91 | void set_nodes (std::vector nodes) { 92 | _nodes = nodes; 93 | _dim = 0; 94 | for (size_t i=0; i<_nodes.size(); i++) { 95 | _dim += _nodes[i]->dim(); 96 | } 97 | _is_angle.resize(_dim); 98 | int ioff = 0; 99 | for (size_t i=0; i<_nodes.size(); i++) { 100 | _is_angle.segment(ioff, _nodes[i]->dim()) = _nodes[i]->is_angle(); 101 | ioff += _nodes[i]->dim(); 102 | } 103 | } 104 | 105 | GLC_RootShift* clone() {return new GLC_RootShift(*this);} 106 | Eigen::VectorXb is_angle() {return _is_angle;} 107 | Eigen::MatrixXd jacobian(); 108 | Eigen::VectorXd reparameterize (Selector s); 109 | Eigen::VectorXd root_shift (Node* node_i, Node* node_j, Selector s); 110 | 111 | }; 112 | 113 | } // namespace isam 114 | -------------------------------------------------------------------------------- /include/isam/Graph.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Graph.h 3 | * @brief Basic graph for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: Graph.h 7610 2012-10-25 10:21:12Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "Node.h" 36 | #include "Factor.h" 37 | 38 | namespace isam { 39 | 40 | class Graph { 41 | Graph(const Graph& rhs); // not allowed 42 | const Graph& operator= (const Graph& rhs); // not allowed 43 | protected: 44 | std::list _nodes; 45 | std::list _factors; 46 | public: 47 | Graph() {} 48 | virtual ~Graph() {} 49 | virtual void add_node(Node* node) { 50 | _nodes.push_back(node); 51 | } 52 | virtual void add_factor(Factor* factor) { 53 | _factors.push_back(factor); 54 | } 55 | virtual void remove_node(Node* node) { 56 | _nodes.remove(node); 57 | } 58 | virtual void remove_factor(Factor* factor) { 59 | _factors.remove(factor); 60 | } 61 | const std::list& get_nodes() const {return _nodes;} 62 | const std::list& get_factors() const {return _factors;} 63 | int num_nodes() const {return _nodes.size();} 64 | int num_factors() const {return _factors.size();} 65 | 66 | void erase_marked(int & variables_deleted, int & measurements_deleted) 67 | { 68 | variables_deleted = 0; 69 | measurements_deleted = 0; 70 | 71 | for (std::list::iterator node = _nodes.begin(); node != _nodes.end(); ) 72 | { 73 | if ((*node)->deleted()) { 74 | variables_deleted += (*node)->dim(); 75 | node = _nodes.erase(node); 76 | } else ++node; 77 | } 78 | std::set nodes_affected; 79 | for (std::list::iterator factor = _factors.begin(); factor != _factors.end();) 80 | { 81 | if ((*factor)->deleted()) { 82 | std::vector & nodes = (*factor)->nodes(); 83 | for (std::vector::iterator node = nodes.begin(); node != nodes.end(); ++node) 84 | nodes_affected.insert(*node); 85 | measurements_deleted += (*factor)->dim(); 86 | factor = _factors.erase(factor); 87 | } else ++factor; 88 | } 89 | for (std::set::iterator node = nodes_affected.begin(); node != nodes_affected.end(); ++node) 90 | { 91 | (*node)->erase_marked_factors(); 92 | } 93 | } 94 | 95 | virtual void print_graph() const { 96 | printf("****GRAPH****:\n"); 97 | printf("**NODES**:\n"); 98 | for(std::list::const_iterator it = _nodes.begin(); it!=_nodes.end(); it++) { 99 | (*it)->write(std::cout); 100 | printf(" Factors: "); 101 | std::list neighbors = (*it)->factors(); 102 | for(std::list::iterator ite = neighbors.begin(); ite!=neighbors.end(); ite++) { 103 | printf("%i ", (*ite)->unique_id()); 104 | } 105 | printf("\n"); 106 | } 107 | printf("**FACTORS**:\n"); 108 | for(std::list::const_iterator it = _factors.begin(); it!=_factors.end(); it++) { 109 | std::cout << (**it); 110 | printf(" Nodes: "); 111 | std::vector neighbors = (*it)->nodes(); 112 | for(std::vector::iterator itn = neighbors.begin(); itn!=neighbors.end(); itn++) { 113 | printf("%i ", (*itn)->unique_id()); 114 | } 115 | printf("\n"); 116 | } 117 | printf("****END OF GRAPH****:\n"); 118 | } 119 | 120 | virtual void write(std::ostream &out) const { 121 | for(std::list::const_iterator it = _factors.begin(); it!=_factors.end(); it++) { 122 | Factor& factor = **it; 123 | out << factor; 124 | out << "\n"; 125 | } 126 | for(std::list::const_iterator it = _nodes.begin(); it!=_nodes.end(); it++) { 127 | Node& node = **it; 128 | out << node; 129 | out << "\n"; 130 | } 131 | } 132 | }; 133 | 134 | } 135 | -------------------------------------------------------------------------------- /include/isam/Jacobian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Jacobian.h 3 | * @brief Jacobian and terms. 4 | * @author Michael Kaess 5 | * @version $Id: Jacobian.h 6536 2012-04-22 16:30:15Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | namespace isam { 33 | 34 | typedef double (*cost_func_t)(double); 35 | 36 | 37 | // Elementary Jacobian for one specific variable/node, potentially containing multiple measurement rows. 38 | class Term { 39 | friend std::ostream& operator<<(std::ostream& output, const Term& t) { 40 | t.write(output); 41 | return output; 42 | } 43 | 44 | Node* _node; 45 | 46 | Eigen::MatrixXd _term; 47 | 48 | public: 49 | 50 | const Node* node() const {return _node;} 51 | 52 | const Eigen::MatrixXd& term() const {return _term;} 53 | 54 | Term(Node* node, const Eigen::MatrixXd& term) : _node(node), _term(term) {} 55 | 56 | Term(Node* node, const double * term, int r, int c) : _node(node), _term(r,c) { 57 | int n = 0; 58 | for (int row=0; row Terms; 71 | 72 | // Jacobian consisting of multiple blocks. 73 | class Jacobian { 74 | friend std::ostream& operator<<(std::ostream& output, const Jacobian& t) { 75 | t.write(output); 76 | return output; 77 | } 78 | 79 | int _dimtotal; 80 | 81 | Terms _terms; 82 | 83 | Eigen::VectorXd _residual; 84 | 85 | public: 86 | 87 | Jacobian() : _dimtotal(0), _residual() {} 88 | 89 | Jacobian(Eigen::VectorXd& residual) : _dimtotal(0), _residual(residual) {} 90 | 91 | inline Jacobian(const double * residual, int size) : _dimtotal(0), _residual(size) { 92 | memcpy(_residual.data(), residual, size*sizeof(double)); 93 | } 94 | 95 | const Terms& terms() const {return _terms;} 96 | 97 | // note: rhs for linear system Ax=b is negative of residual! 98 | Eigen::VectorXd rhs() const {return - _residual;} 99 | 100 | void add_term(Node* node, const Eigen::MatrixXd& term) { 101 | _terms.push_back(Term(node, term)); 102 | _dimtotal += node->dim(); 103 | } 104 | 105 | inline void add_term(Node* node, const double* term, int r, int c) { 106 | _terms.push_back(Term(node, term, r, c)); 107 | _dimtotal += node->dim(); 108 | } 109 | 110 | int dimtotal() const { return _dimtotal; } 111 | 112 | void write(std::ostream &out) const { 113 | int i=1; 114 | for (Terms::const_iterator it = _terms.begin(); it != _terms.end(); it++, i++) { 115 | out << "Term " << i << ":" << std::endl; 116 | out << *it; 117 | } 118 | out << "rhs: " << std::endl << _residual << std::endl; 119 | } 120 | }; 121 | 122 | } 123 | -------------------------------------------------------------------------------- /include/isam/Node.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Node.h 3 | * @brief Graph node for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: Node.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "Element.h" 34 | 35 | namespace Eigen { 36 | typedef Matrix VectorXb; 37 | } 38 | 39 | namespace isam { 40 | 41 | enum Selector {LINPOINT, ESTIMATE}; 42 | 43 | class Factor; // Factor.h not included here to avoid circular dependency 44 | 45 | // Node of the graph also containing measurements (Factor). 46 | class Node : public Element { 47 | friend std::ostream& operator<<(std::ostream& output, const Node& n) { 48 | n.write(output); 49 | return output; 50 | } 51 | 52 | static int _next_id; 53 | bool _deleted; 54 | 55 | protected: 56 | 57 | std::list _factors; // list of adjacent factors 58 | 59 | public: 60 | 61 | Node(const char* name, int dim) : Element(name, dim), _deleted(false) { 62 | _id = _next_id++; 63 | } 64 | 65 | virtual ~Node() {}; 66 | 67 | virtual bool initialized() const = 0; 68 | 69 | virtual Eigen::VectorXd vector(Selector s = ESTIMATE) const = 0; 70 | 71 | virtual Eigen::VectorXd vector0() const = 0; 72 | virtual Eigen::VectorXb is_angle() const {return Eigen::VectorXb::Zero(_dim);} 73 | 74 | virtual void update(const Eigen::VectorXd& v) = 0; 75 | virtual void update0(const Eigen::VectorXd& v) = 0; 76 | 77 | virtual void linpoint_to_estimate() = 0; 78 | virtual void estimate_to_linpoint() = 0; 79 | virtual void swap_estimates() = 0; 80 | 81 | virtual void apply_exmap(const Eigen::VectorXd& v) = 0; 82 | virtual void self_exmap(const Eigen::VectorXd& v) = 0; 83 | 84 | void add_factor(Factor* e) {_factors.push_back(e);} 85 | void remove_factor(Factor* e) {_factors.remove(e);} 86 | 87 | const std::list& factors() {return _factors;} 88 | 89 | bool deleted() const { return _deleted; } 90 | 91 | void mark_deleted(); 92 | void erase_marked_factors(); 93 | 94 | virtual void write(std::ostream &out) const = 0; 95 | }; 96 | 97 | // Generic template for easy instantiation of the multiple node types. 98 | template 99 | class NodeT : public Node { 100 | 101 | protected: 102 | T* _value; // current estimate 103 | T* _value0; // linearization point 104 | 105 | public: 106 | 107 | NodeT() : Node(T::name(), T::dim) { 108 | _value = NULL; 109 | _value0 = NULL; 110 | } 111 | 112 | NodeT(const char* name) : Node(name, T::dim) { 113 | _value = NULL; 114 | _value0 = NULL; 115 | } 116 | 117 | virtual ~NodeT() { 118 | delete _value; 119 | delete _value0; 120 | } 121 | 122 | void init(const T& t) { 123 | delete _value; delete _value0; 124 | _value = new T(t); _value0 = new T(t); 125 | } 126 | 127 | bool initialized() const {return _value != NULL;} 128 | 129 | T value(Selector s = ESTIMATE) const {return (s==ESTIMATE)?*_value:*_value0;} 130 | T value0() const {return *_value0;} 131 | 132 | Eigen::VectorXd vector(Selector s = ESTIMATE) const {return (s==ESTIMATE)?_value->vector():_value0->vector();} 133 | Eigen::VectorXd vector0() const {return _value0->vector();} 134 | Eigen::VectorXb is_angle() const {return _value->is_angle();} 135 | 136 | void update(const Eigen::VectorXd& v) {_value->set(v);} 137 | void update0(const Eigen::VectorXd& v) {_value0->set(v);} 138 | 139 | void linpoint_to_estimate() {*_value = *_value0;} 140 | void estimate_to_linpoint() {*_value0 = *_value;} 141 | void swap_estimates() {T tmp = *_value; *_value = *_value0; *_value0 = tmp;} 142 | 143 | void apply_exmap(const Eigen::VectorXd& v) {*_value = _value0->exmap(v);} 144 | void self_exmap(const Eigen::VectorXd& v) {*_value0 = _value0->exmap(v);} 145 | 146 | void write(std::ostream &out) const { 147 | out << name() << "_Node " << _id; 148 | if (_value != NULL) { 149 | out << " " << value(); 150 | } 151 | } 152 | }; 153 | 154 | } 155 | -------------------------------------------------------------------------------- /include/isam/Noise.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Noise.h 3 | * @brief Various noise models. 4 | * @author Michael Kaess 5 | * @version $Id: Noise.h 5797 2011-12-07 03:50:41Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | namespace isam { 34 | 35 | // general noise model class 36 | class Noise { 37 | public: 38 | Eigen::MatrixXd _sqrtinf; 39 | const Eigen::MatrixXd& sqrtinf() const {return _sqrtinf;} 40 | }; 41 | 42 | // noise model based on square root information matrix 43 | class SqrtInformation : public Noise { 44 | public: 45 | SqrtInformation(const Eigen::MatrixXd& sqrtinf) {_sqrtinf = sqrtinf;} 46 | }; 47 | 48 | // noise model based on information matrix 49 | class Information : public Noise { 50 | public: 51 | Information(const Eigen::MatrixXd& inf) { 52 | _sqrtinf = inf.llt().matrixU(); 53 | } 54 | }; 55 | 56 | // noise model based on covariance matrix 57 | class Covariance : public Noise { 58 | public: 59 | Covariance(const Eigen::MatrixXd& cov) { 60 | _sqrtinf = cov.inverse().llt().matrixU(); 61 | } 62 | }; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /include/isam/OptimizationInterface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OptimizationInterface.h 3 | * @brief Abstract base class for nonlinear optimizer. 4 | * @author Michael Kaess 5 | * @author David Rosen 6 | * @version $Id: OptimizationInterface.h 6371 2012-03-29 22:22:23Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #include "SparseSystem.h" 34 | #include "Node.h" 35 | 36 | 37 | namespace isam { 38 | 39 | /** 40 | * Abstract base class providing an interface between the nonlinear system 41 | * to be optimized (stored in the Nodes of the Graph constructed in the SLAM) 42 | * and the Optimization class that actually performs the optimizations. 43 | */ 44 | class OptimizationInterface { 45 | 46 | protected: 47 | 48 | /** Factored Jacobian about the current linearization point.*/ 49 | SparseSystem _R; 50 | 51 | public: 52 | virtual SparseSystem jacobian() = 0; 53 | virtual void apply_exmap(const Eigen::VectorXd& delta) = 0; 54 | virtual void self_exmap(const Eigen::VectorXd& delta) = 0; 55 | virtual void estimate_to_linpoint() = 0; 56 | virtual void linpoint_to_estimate() = 0; 57 | virtual void swap_estimates() = 0; 58 | virtual Eigen::VectorXd weighted_errors(Selector s = ESTIMATE) = 0; 59 | 60 | OptimizationInterface(): _R(1,1) {} 61 | 62 | virtual ~OptimizationInterface() {} 63 | 64 | friend class Optimizer; 65 | }; 66 | 67 | } 68 | -------------------------------------------------------------------------------- /include/isam/OrderedSparseMatrix.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OrderedSparseMatrix.h 3 | * @brief Adds column ordering to sparse matrix functionality for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: OrderedSparseMatrix.h 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include "SparseVector.h" 31 | #include "SparseMatrix.h" 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace isam { 38 | 39 | class OrderedSparseMatrix : public SparseMatrix { 40 | int* _r_to_a; // column variable order 41 | int* _a_to_r; 42 | 43 | /** 44 | * Allocate memory - private. 45 | * @param init_table Determines if index table is initialized with identity. 46 | */ 47 | void _allocate_OrderedSparseMatrix(bool init_order = true); 48 | 49 | /** 50 | * Copy data from one sparse matrix to a new one - private. 51 | * @param vec Existing sparse vector to copy from. 52 | */ 53 | void _copy_from_OrderedSparseMatrix(const OrderedSparseMatrix& mat); 54 | 55 | /** 56 | * Deallocate memory - private. 57 | */ 58 | void _dealloc_OrderedSparseMatrix(); 59 | 60 | /** 61 | * Calculate reverse ordering for O(1) access. 62 | * @param num Number of entries in ordering 63 | * @param order Input order. 64 | * @param reverse_order Output reverse order. 65 | */ 66 | void _calc_reverse_order(int num, const int* order, int* reverse_order) const; 67 | 68 | /** 69 | * Sets the variable order, for example after batch reordering. 70 | * @param order Pointer to list of integer IDs as used by CSparse. 71 | */ 72 | void _set_order(const int* r_to_a); 73 | 74 | public: 75 | 76 | /** 77 | * Constructor. 78 | * @param num_rows Initial number of rows. 79 | * @param num_cols Initial number of columns. 80 | */ 81 | OrderedSparseMatrix(int num_rows, int num_cols); 82 | 83 | /** 84 | * Copy constructor. 85 | * @param mat Matrix to copy. 86 | */ 87 | OrderedSparseMatrix(const OrderedSparseMatrix& mat); 88 | 89 | /** 90 | * Submatrix copy constructor. 91 | * @param mat Matrix to copy. 92 | * @param num_rows Number of rows to copy. 93 | * @param num_cols Number of columns to copy. 94 | * @param first_row Row offset. 95 | * @param first_col Column offset. 96 | */ 97 | OrderedSparseMatrix(const OrderedSparseMatrix& mat, int num_rows, 98 | int num_cols, int first_row = 0, int first_col = 0); 99 | 100 | OrderedSparseMatrix(int num_rows, int num_cols, SparseVector_p* rows); 101 | 102 | /** 103 | * Destructor. 104 | */ 105 | virtual ~OrderedSparseMatrix(); 106 | 107 | /** 108 | * Assignment operator. 109 | * @param mat Right-hand-side matrix in assignment 110 | * @return self. 111 | */ 112 | const OrderedSparseMatrix& operator= (const OrderedSparseMatrix& mat); 113 | 114 | // overridden functions 115 | 116 | /** 117 | * Replace the given row. Also reorders the new vector according to internal ordering. 118 | * @param row Number of row to replace. 119 | * @param new_row New row vector. 120 | */ 121 | void set_row(int row, const SparseVector& new_row); 122 | 123 | /** 124 | * Import externally allocated rows, and also set the ordering. 125 | * @param num_rows Number of rows of new matrix. 126 | * @param num_cols Number of columns of new matrix. 127 | * @param rows Array of SparseVector of length num_rows. 128 | * @param r_to_a Variable ordering. 129 | */ 130 | void import_rows_ordered(int num_rows, int num_cols, SparseVector_p* rows, int* r_to_a); 131 | 132 | /** 133 | * Expand matrix to include new columns. 134 | * @param num Number of columns to add. 135 | */ 136 | void append_new_cols(int num); 137 | 138 | // new functions 139 | 140 | /** 141 | * Return variable ordering 142 | * @return Array of integers that specifies for each column in A 143 | * which column it now corresponds to in R. 144 | */ 145 | virtual const int* a_to_r() const; 146 | 147 | /** 148 | * Return inverse variable ordering 149 | * @return Array of integers that specifies for each column in R 150 | * which column it originally came from in A. 151 | */ 152 | virtual const int* r_to_a() const; 153 | 154 | }; 155 | 156 | } 157 | -------------------------------------------------------------------------------- /include/isam/Point2d.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Point2d.h 3 | * @brief Simple 2D point class. 4 | * @author Michael Kaess 5 | * @version $Id: Point2d.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | namespace Eigen { 33 | typedef Matrix VectorXb; 34 | } 35 | 36 | namespace isam { 37 | 38 | class Point2d { 39 | friend std::ostream& operator<<(std::ostream& out, const Point2d& p) { 40 | p.write(out); 41 | return out; 42 | } 43 | 44 | double _x; 45 | double _y; 46 | 47 | public: 48 | static const int dim = 2; 49 | static const int size = 2; 50 | static const char* name() { 51 | return "Point2d"; 52 | } 53 | Point2d() : _x(0), _y(0) {} 54 | Point2d(double x, double y) : _x(x), _y(y) {} 55 | Point2d(const Eigen::Vector2d& vec) : _x(vec(0)), _y(vec(1)) {} 56 | 57 | double x() const {return _x;} 58 | double y() const {return _y;} 59 | 60 | void set_x(double x) {_x = x;} 61 | void set_y(double y) {_y = y;} 62 | 63 | Point2d exmap(const Eigen::Vector2d& delta) const { 64 | Point2d res = *this; 65 | res._x += delta(0); 66 | res._y += delta(1); 67 | return res; 68 | } 69 | 70 | Eigen::Vector2d vector() const { 71 | Eigen::Vector2d v(_x, _y); 72 | return v; 73 | } 74 | void set(double x, double y) { 75 | _x = x; 76 | _y = y; 77 | } 78 | void set(const Eigen::Vector2d& v) { 79 | _x = v(0); 80 | _y = v(1); 81 | } 82 | void write(std::ostream &out) const { 83 | out << "(" << _x << ", " << _y << ")"; 84 | } 85 | 86 | Eigen::VectorXb is_angle() const { 87 | return Eigen::VectorXb::Zero(size); 88 | } 89 | 90 | }; 91 | 92 | } 93 | -------------------------------------------------------------------------------- /include/isam/Point3d.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Point3d.h 3 | * @brief Simple 3D point class. 4 | * @author Michael Kaess 5 | * @version $Id: Point3d.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "Point2d.h" 33 | 34 | namespace isam { 35 | 36 | class Point3d { 37 | friend std::ostream& operator<<(std::ostream& out, const Point3d& p) { 38 | p.write(out); 39 | return out; 40 | } 41 | 42 | double _x; 43 | double _y; 44 | double _z; 45 | public: 46 | static const int dim = 3; 47 | static const int size = 3; 48 | static const char* name() { 49 | return "Point3d"; 50 | } 51 | Point3d() : _x(0.), _y(0.), _z(0.) {} 52 | Point3d(double x, double y, double z) : _x(x), _y(y), _z(z) {} 53 | Point3d(const Eigen::Vector3d& vec) : _x(vec(0)), _y(vec(1)), _z(vec(2)) {} 54 | 55 | double x() const {return _x;} 56 | double y() const {return _y;} 57 | double z() const {return _z;} 58 | 59 | void set_x(double x) {_x = x;} 60 | void set_y(double y) {_y = y;} 61 | void set_z(double z) {_z = z;} 62 | 63 | Point3d exmap(const Eigen::Vector3d& delta) const { 64 | Point3d res = *this; 65 | res._x += delta(0); 66 | res._y += delta(1); 67 | res._z += delta(2); 68 | return res; 69 | } 70 | 71 | Eigen::Vector3d vector() const { 72 | Eigen::Vector3d tmp(_x, _y, _z); 73 | return tmp; 74 | } 75 | void set(double x, double y, double z) { 76 | _x = x; 77 | _y = y; 78 | _z = z; 79 | } 80 | void set(const Eigen::Vector3d& v) { 81 | _x = v(0); 82 | _y = v(1); 83 | _z = v(2); 84 | } 85 | 86 | Eigen::VectorXb is_angle() const { 87 | return Eigen::VectorXb::Zero(size); 88 | } 89 | 90 | Point3d to_point3d() { 91 | return *this; 92 | } 93 | 94 | void of_point2d(const Point2d& p) { 95 | _x = p.x(); 96 | _y = p.y(); 97 | _z = 0.; 98 | } 99 | void write(std::ostream &out) const { 100 | out << "(" << _x << ", " << _y << ", " << _z << ")"; 101 | } 102 | }; 103 | 104 | } 105 | -------------------------------------------------------------------------------- /include/isam/Point3dh.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Point3dh.h 3 | * @brief 3D homogeneous point class. 4 | * @author Michael Kaess 5 | * @version $Id: Point3dh.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "Point3d.h" 34 | 35 | namespace isam { 36 | 37 | class Point3dh { 38 | friend std::ostream& operator<<(std::ostream& out, const Point3dh& p) { 39 | p.write(out); 40 | return out; 41 | } 42 | 43 | double _x; 44 | double _y; 45 | double _z; 46 | double _w; 47 | 48 | public: 49 | 50 | static const int dim = 3; 51 | static const int size = 4; 52 | static const char* name() { 53 | return "Point3dh"; 54 | } 55 | 56 | Point3dh() : _x(0.), _y(0.), _z(0.), _w(1.) {} // note: 0,0,0,0 is not allowed! 57 | Point3dh(double x, double y, double z, double w) : _x(x), _y(y), _z(z), _w(w) {} 58 | Point3dh(const Eigen::Vector4d& vec) : _x(vec(0)), _y(vec(1)), _z(vec(2)), _w(vec(3)) {} 59 | Point3dh(const Point3d& p) : _x(p.x()), _y(p.y()), _z(p.z()), _w(1.) {} 60 | 61 | double x() const {return _x;} 62 | double y() const {return _y;} 63 | double z() const {return _z;} 64 | double w() const {return _w;} 65 | 66 | void x(double x) {_x = x;} 67 | void y(double y) {_y = y;} 68 | void z(double z) {_z = z;} 69 | void w(double w) {_w = w;} 70 | 71 | static Eigen::Vector4d delta3_to_homogeneous(const Eigen::Vector3d& delta) { 72 | double theta = delta.norm(); 73 | double S; 74 | if (theta < 0.0001) { // sqrt4(machine precession) 75 | S = 0.5 + theta*theta/48.; 76 | } else { 77 | S = sin(0.5*theta)/theta; 78 | } 79 | double C = cos(0.5*theta); 80 | return Eigen::Vector4d(S*delta(0), S*delta(1), S*delta(2), C); 81 | } 82 | 83 | Point3dh exmap(const Eigen::Vector3d& delta) const { 84 | #if 0 85 | // solution with Householder matrix following HZ second edition 86 | 87 | // std::cout << norm() << std::endl; 88 | assert(norm() == 1); 89 | // make sure not above PI, otherwise needs to be corrected 90 | assert(delta.norm() < 3.14); 91 | 92 | Eigen::Vector4d delta4 = delta3_to_homogeneous(delta); 93 | // multiply with Householder matrix, without explicitly calculating the matrix 94 | Eigen::Vector4d e(0,0,0,1); 95 | Eigen::Vector4d v = vector() + (x()<0.?-1:1) * norm() * e; 96 | 97 | // checking if sign is correct to yield (0,0,0,1), otherwise fix 98 | Eigen::Vector4d test = vector() - 2*v*(v.transpose()*vector())/(v.transpose()*v); 99 | if (test(3)<0) { 100 | v = v - 2 * (x()<0.?-1:1) * norm() * e; 101 | } 102 | 103 | Eigen::Vector4d res = delta4 - 2*v*(v.transpose()*delta4)/(v.transpose()*v); 104 | // std::cout << vector() << " " << res << std::endl; 105 | return Point3dh(res); 106 | #else 107 | #if 1 108 | // solution by mapping to unit Quaternion and back (both are unit spheres!) 109 | Eigen::Quaterniond delta_quat = Rot3d::delta3_to_quat(delta); 110 | Eigen::Quaterniond quat(_w, _x, _y, _z); 111 | // quat.normalize(); // just to be safe... 112 | Eigen::Quaterniond res = quat * delta_quat; 113 | return Point3dh(res.x(), res.y(), res.z(), res.w()); 114 | #else 115 | #if 1 116 | // 3DOF exmap - not recommended 117 | Point3dh res = *this; 118 | res.normalize(); 119 | Eigen::Vector4d::Index pos; 120 | res.vector().cwiseAbs().maxCoeff(&pos); 121 | // std::cout << *this << " -- " << pos << " -- " << res.vector() << std::endl; 122 | // only update 3 out of 4 entries 123 | int idx = 0; 124 | if (pos!=0) {res._x += delta(idx); idx++;} 125 | if (pos!=1) {res._y += delta(idx); idx++;} 126 | if (pos!=2) {res._z += delta(idx); idx++;} 127 | if (pos!=3) {res._w += delta(idx);} 128 | res.normalize(); 129 | return res; 130 | #else 131 | // 4DOF exmap - not feasible without constrained optimization 132 | Point3dh res = *this; 133 | res._x += delta(0); 134 | res._y += delta(1); 135 | res._z += delta(2); 136 | res._w += delta(3); 137 | return res; 138 | #endif 139 | #endif 140 | #endif 141 | } 142 | 143 | Eigen::Vector4d vector() const { 144 | Eigen::VectorXd tmp(4); 145 | tmp << _x, _y, _z, _w; 146 | return tmp; 147 | } 148 | 149 | Eigen::VectorXb is_angle() const { 150 | Eigen::VectorXb isang (4); 151 | isang << false, false, false, false; 152 | return isang; 153 | } 154 | 155 | void set(double x, double y, double z, double w) { 156 | _x = x; 157 | _y = y; 158 | _z = z; 159 | _w = w; 160 | } 161 | 162 | void set(const Eigen::Vector4d& v) { 163 | _x = v(0); 164 | _y = v(1); 165 | _z = v(2); 166 | _w = v(3); 167 | } 168 | 169 | Point3d to_point3d() const { 170 | double w_inv = 1. / _w; 171 | return Point3d(_x*w_inv, _y*w_inv, _z*w_inv); 172 | } 173 | 174 | double norm() const { 175 | return sqrt(_x*_x + _y*_y + _z*_z + _w*_w); 176 | } 177 | 178 | Point3dh normalize() { 179 | double a = 1. / norm(); 180 | _x *= a; 181 | _y *= a; 182 | _z *= a; 183 | _w *= a; 184 | return *this; 185 | } 186 | 187 | void write(std::ostream &out) const { 188 | out << "(" << _x << ", " << _y << ", " << _z << ", " << _w << ")"; 189 | } 190 | }; 191 | 192 | } 193 | -------------------------------------------------------------------------------- /include/isam/Pose2d.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Pose2d.h 3 | * @brief Simple 2D pose class. 4 | * @author Michael Kaess 5 | * @version $Id: Pose2d.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "util.h" 35 | #include "Point2d.h" 36 | 37 | namespace Eigen { 38 | typedef Matrix VectorXb; 39 | } 40 | 41 | namespace isam { 42 | 43 | class Pose2d { 44 | friend std::ostream& operator<<(std::ostream& out, const Pose2d& p) { 45 | p.write(out); 46 | return out; 47 | } 48 | 49 | double _x; 50 | double _y; 51 | double _t; 52 | 53 | public: 54 | // assignment operator and copy constructor implicitly created, which is ok 55 | static const int dim = 3; 56 | static const int size = 3; 57 | static const char* name() { 58 | return "Pose2d"; 59 | } 60 | Pose2d() : _x(0.), _y(0.), _t(0.) {} 61 | Pose2d(double x, double y, double t) : _x(x), _y(y), _t(t) {} 62 | Pose2d(const Eigen::Vector3d& vec) : _x(vec(0)), _y(vec(1)), _t(vec(2)) {} 63 | 64 | double x() const {return _x;} 65 | double y() const {return _y;} 66 | double t() const {return _t;} 67 | 68 | void set_x(double x) {_x = x;} 69 | void set_y(double y) {_y = y;} 70 | void set_t(double t) {_t = t;} 71 | 72 | Pose2d exmap(const Eigen::Vector3d& delta) const { 73 | Pose2d res = *this; 74 | res._x += delta(0); 75 | res._y += delta(1); 76 | res._t = standardRad(res._t + delta(2)); 77 | return res; 78 | } 79 | 80 | Eigen::Vector3d vector() const { 81 | Eigen::Vector3d v(_x, _y, _t); 82 | return v; 83 | } 84 | void set(double x, double y, double t) { 85 | _x = x; 86 | _y = y; 87 | _t = t; 88 | } 89 | void set(const Eigen::Vector3d& v) { 90 | _x = v(0); 91 | _y = v(1); 92 | _t = standardRad(v(2)); 93 | } 94 | void write(std::ostream &out) const { 95 | out << "(" << _x << ", " << _y << ", " << _t << ")"; 96 | } 97 | 98 | Eigen::VectorXb is_angle() const { 99 | Eigen::VectorXb isang (dim); 100 | isang << false, false, true; 101 | return isang; 102 | } 103 | 104 | /** 105 | * Calculate new pose b composed from this pose (a) and the odometry d. 106 | * Follows notation of Lu&Milios 1997. 107 | * \f$ b = a \oplus d \f$ 108 | * @param d Pose difference to add. 109 | * @return d transformed from being local in this frame (a) to the global frame. 110 | */ 111 | Pose2d oplus(const Pose2d& d) const { 112 | double c = cos(t()); 113 | double s = sin(t()); 114 | double px = x() + c*d.x() - s*d.y(); 115 | double py = y() + s*d.x() + c*d.y(); 116 | double pt = t() + d.t(); 117 | return Pose2d(px,py,pt); 118 | } 119 | 120 | /** 121 | * Odometry d from b to this pose (a). Follows notation of 122 | * Lu&Milios 1997. 123 | * \f$ d = a \ominus b \f$ 124 | * @param b Base frame. 125 | * @return Global this (a) expressed in base frame b. 126 | */ 127 | Pose2d ominus(const Pose2d& b) const { 128 | double c = cos(b.t()); 129 | double s = sin(b.t()); 130 | double dx = x() - b.x(); 131 | double dy = y() - b.y(); 132 | double ox = c*dx + s*dy; 133 | double oy = -s*dx + c*dy; 134 | double ot = t() - b.t(); 135 | return Pose2d(ox,oy,ot); 136 | } 137 | 138 | /** 139 | * Project point into this coordinate frame. 140 | * @param p Point to project 141 | * @return Point p locally expressed in this frame. 142 | */ 143 | Point2d transform_to(const Point2d& p) const { 144 | double c = cos(t()); 145 | double s = sin(t()); 146 | double dx = p.x() - x(); 147 | double dy = p.y() - y(); 148 | double x = c*dx + s*dy; 149 | double y = -s*dx + c*dy; 150 | return Point2d(x,y); 151 | } 152 | 153 | Point2d transform_from(const Point2d& p) const { 154 | double c = cos(t()); 155 | double s = sin(t()); 156 | double px = x() + c*p.x() - s*p.y(); 157 | double py = y() + s*p.x() + c*p.y(); 158 | return Point2d(px,py); 159 | } 160 | 161 | }; 162 | 163 | } 164 | -------------------------------------------------------------------------------- /include/isam/Properties.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Properties.h 3 | * @brief Properties class for easy access to internal parameters. 4 | * @author Michael Kaess 5 | * @version $Id: Properties.h 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | namespace isam { 31 | 32 | enum Method {GAUSS_NEWTON, LEVENBERG_MARQUARDT, DOG_LEG}; 33 | 34 | /** 35 | * User changeable default parameters. 36 | */ 37 | class Properties { 38 | // default copy constructor and assignment operator in use 39 | public: 40 | 41 | /** print additional information if true */ 42 | bool verbose; 43 | 44 | /** omit all textual output if true */ 45 | bool quiet; 46 | 47 | /** Ignore any symbolic derivatives provided in factors */ 48 | bool force_numerical_jacobian; 49 | 50 | /** which optimization method to use */ 51 | Method method; 52 | 53 | /** Powell's Dog-Leg termination criteria: stop whenever the infinity-norm 54 | * of the gradient direction vector falls below this threshold. */ 55 | double epsilon1; 56 | /** Powell's Dog-Leg termination criteria: stop whenever the 2-norm of 57 | * the correction step falls below this threshold. */ 58 | double epsilon2; 59 | /** Powell's Dog-Leg termination criteria: stop whenever the infinity-norm 60 | * of the error-residual vector falls below this threshold. */ 61 | double epsilon3; 62 | 63 | /** Termination criterion: stop whenever the absolute sum-of-squares error 64 | * falls below this threshold. */ 65 | double epsilon_abs; 66 | /** Termination criterion: stop whenever the /difference/ in absolute 67 | * sum-of-squares errors between two estimates falls below this fraction 68 | * of the /total/ absolute sum-of-squares errors. */ 69 | double epsilon_rel; 70 | 71 | /** Maximum number of iterations */ 72 | int max_iterations; 73 | /** Starting value for lambda in LM */ 74 | double lm_lambda0; 75 | /** Factor for multiplying (failure) or dividing (success) lambda */ 76 | double lm_lambda_factor; 77 | 78 | /** Only update R matrix/solution/batch every mod_update steps */ 79 | int mod_update; 80 | /** Batch solve with variable reordering and relinearization every mod_batch steps */ 81 | int mod_batch; 82 | /** For incremental steps, solve by backsubstitution every mod_solve steps */ 83 | int mod_solve; 84 | 85 | // default parameters 86 | Properties() : 87 | verbose(false), 88 | quiet(false), 89 | 90 | force_numerical_jacobian(false), 91 | 92 | method(GAUSS_NEWTON), 93 | 94 | epsilon1(1e-2), 95 | epsilon2(1e-2), 96 | epsilon3(1e-2), 97 | 98 | epsilon_abs(1e-3), 99 | epsilon_rel(1e-5), 100 | 101 | max_iterations(500), 102 | 103 | lm_lambda0(1e-6), 104 | lm_lambda_factor(10.), 105 | 106 | mod_update(1), 107 | mod_batch(100), 108 | mod_solve(1) 109 | {} 110 | }; 111 | 112 | } 113 | -------------------------------------------------------------------------------- /include/isam/Slam.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Slam.h 3 | * @brief SLAM implementation using iSAM 4 | * @author Michael Kaess 5 | * @version $Id: Slam.h 6371 2012-03-29 22:22:23Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "SparseSystem.h" 35 | #include "Node.h" 36 | #include "Factor.h" 37 | #include "Graph.h" 38 | #include "Properties.h" 39 | #include "OptimizationInterface.h" 40 | #include "Optimizer.h" 41 | #include "Covariances.h" 42 | 43 | 44 | namespace isam { 45 | 46 | 47 | /** 48 | * Return type of Slam::update() to allow future extensions without 49 | * having to change the interface. 50 | */ 51 | class UpdateStats { 52 | public: 53 | // current step number 54 | int step; 55 | 56 | // was batch performed? 57 | bool batch; 58 | 59 | // was the solution updated? 60 | bool solve; 61 | }; 62 | 63 | /** 64 | * The actual SLAM interface. 65 | */ 66 | class Slam: public Graph, OptimizationInterface { 67 | // Graph prohibits copy construction and assignment operator 68 | 69 | int _step; 70 | 71 | Properties _prop; 72 | 73 | Covariances _covariances; 74 | 75 | public: 76 | 77 | //-- manipulating the graph ----------------------------- 78 | 79 | /** 80 | * Default constructor. 81 | */ 82 | Slam(); 83 | 84 | /** 85 | * Destructor. 86 | */ 87 | virtual ~Slam(); 88 | 89 | /** 90 | * Returns a copy of the current properties. 91 | */ 92 | Properties properties() { 93 | return _prop; 94 | } 95 | 96 | /** 97 | * Sets new properties. 98 | */ 99 | void set_properties(Properties prop) { 100 | _prop = prop; 101 | } 102 | 103 | /** 104 | * Saves the graph (nodes and factors). 105 | * @param fname Filename with optional path to save graph to. 106 | */ 107 | void save(const std::string fname) const; 108 | 109 | /** 110 | * Adds a node (variable) to the graph. 111 | * @param node Pointer to new node. 112 | */ 113 | void add_node(Node* node); 114 | 115 | /** 116 | * Adds a factor (measurement) to the graph. 117 | * @param factor Pointer to new factor. 118 | */ 119 | void add_factor(Factor* factor); 120 | 121 | /** 122 | * Removes a node (variable) and all adjacent factors from the graph. 123 | * Note that the node itself is not deallocated. 124 | * @param node Pointer to node. 125 | */ 126 | void remove_node(Node* node); 127 | 128 | /** 129 | * Removes an factor (measurement) from the graph. 130 | * Note that the factor itself is not deallocated. 131 | * Be careful not to leave unconnected nodes behind. 132 | * @param factor Pointer to factor. 133 | */ 134 | void remove_factor(Factor* factor); 135 | 136 | //-- solving the system ----------------------------- 137 | 138 | /** 139 | * Update the graph by finding new solution; depending on properties 140 | * this might simply be a Givens update, could include a solve step, 141 | * or be a full batch step with reordering. 142 | * @return Update statistics. 143 | */ 144 | virtual UpdateStats update(); 145 | 146 | /** 147 | * Fully solve the system, iterating until convergence. 148 | * @return Number of iterations performed. 149 | */ 150 | virtual int batch_optimization(); 151 | 152 | //-- misc ----------------------------- 153 | 154 | /** 155 | * Sets a cost function different from the default (quadratic). 156 | * @param cost_func Pointer to cost function, see util.h for a list of robust 157 | * cost functions. Instead of cost_squared, use NULL, which avoids calculating square roots. 158 | */ 159 | void set_cost_function(cost_func_t cost_func); 160 | 161 | /** 162 | * Calculates the normalized chi-square value (weighted sum of squared 163 | * errors divided by degrees of freedom [# measurements - # variables]) 164 | * for the estimate x. 165 | */ 166 | double normalized_chi2(); 167 | 168 | /** 169 | * Calculates the chi2 error of the last_n constraints. 170 | */ 171 | double local_chi2(int last_n); 172 | 173 | /** 174 | * Weighted non-squared error vector, by default at current estimate. 175 | */ 176 | Eigen::VectorXd weighted_errors(Selector s = ESTIMATE); 177 | 178 | /** 179 | * Weighted sum of squared errors, by default at the current estimate. 180 | */ 181 | double chi2(Selector s = ESTIMATE); 182 | 183 | /** 184 | * Returns the current factor matrix. 185 | */ 186 | virtual const SparseSystem& get_R() const; 187 | 188 | /** 189 | * Calculate the full Jacobian numerical (fast column-wise procedure). 190 | */ 191 | virtual SparseSystem jacobian_numerical_columnwise(); 192 | 193 | /** 194 | * Returns the last n rows of the measurement Jacobian of the SLAM system. 195 | * @param last_n Only return Jacobians of last n measurements (-1 returns all) 196 | * @return Measurement Jacobian. 197 | */ 198 | virtual SparseSystem jacobian_partial(int last_n); 199 | 200 | /** 201 | * Returns the measurement Jacobian of the SLAM system. 202 | * @return Measurement Jacobian. 203 | */ 204 | virtual SparseSystem jacobian(); 205 | 206 | /** 207 | * Returns the Covariances object associated with this Slam object. 208 | * @return Covariances object for access to estimation covariances. 209 | */ 210 | const Covariances& covariances(); 211 | 212 | /** 213 | * Print statistics for debugging. 214 | */ 215 | virtual void print_stats(); 216 | 217 | private: 218 | 219 | /** 220 | * Apply a delta vector to the linearization point and store result as new estimate. 221 | */ 222 | void apply_exmap(const Eigen::VectorXd& x); 223 | 224 | /** 225 | * Apply a delta vector directly to the linearization point. 226 | */ 227 | void self_exmap(const Eigen::VectorXd& x); 228 | 229 | /** 230 | * Set current estimate to the linearization point (needed in dog leg). 231 | */ 232 | void linpoint_to_estimate(); 233 | 234 | /** 235 | * Set linearization point to current estimate. 236 | */ 237 | void estimate_to_linpoint(); 238 | 239 | /** 240 | * Exchange linearization point and current estimate (needed in dog leg). 241 | */ 242 | void swap_estimates(); 243 | 244 | /** 245 | * Update the system with any newly added measurements. The measurements will be 246 | * appended to the existing factor matrix, and the factor is transformed into 247 | * triangular form again using Givens rotations. 248 | * Very efficient for exploration O(1), but can be more expensive otherwise >O(n). 249 | */ 250 | virtual void incremental_update(); 251 | 252 | /** 253 | * Resolve the system with linearization based on current estimate; 254 | * perform variable reordering for efficiency. 255 | */ 256 | virtual void batch_optimization_step(); 257 | 258 | 259 | // internal variable used for operations such as removing of parts of 260 | // the graph that currently cannot be done incrementally 261 | bool _require_batch; 262 | 263 | cost_func_t _cost_func; 264 | 265 | void update_starts(); 266 | 267 | protected: 268 | int _dim_nodes; 269 | int _dim_measure; 270 | int _num_new_measurements; 271 | int _num_new_rows; 272 | 273 | Optimizer _opt; 274 | 275 | friend class Covariances; 276 | 277 | }; 278 | 279 | } 280 | -------------------------------------------------------------------------------- /include/isam/SparseMatrix.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseMatrix.h 3 | * @brief Sparse matrix functionality for iSAM 4 | * @author Michael Kaess 5 | * @version $Id: SparseMatrix.h 6376 2012-03-30 18:34:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "SparseVector.h" 34 | 35 | namespace isam { 36 | 37 | typedef SparseVector* SparseVector_p; 38 | 39 | class SparseMatrix { 40 | int _num_rows; // current number of rows 41 | int _num_cols; // current number of columns 42 | int _max_num_rows; // allocated number of row indices 43 | int _max_num_cols; // allocated number of columns 44 | SparseVector_p* _rows; // pointers to the actual rows 45 | 46 | /** 47 | * Allocate memory - private. 48 | * @param num_rows Number of active rows. 49 | * @param num_cols Number of active columns. 50 | * @param max_num_rows Number of rows to reserve memory for. 51 | * @param max_num_cols Number of columns to reserve memory for. 52 | */ 53 | void _allocate_SparseMatrix(int num_rows, int num_cols, int max_num_rows, int max_num_cols, bool init_rows = true); 54 | 55 | /** 56 | * Copy data from one sparse matrix to a new one - private. 57 | * @param vec Existing sparse vector to copy from. 58 | */ 59 | void _copy_from_SparseMatrix(const SparseMatrix& mat); 60 | 61 | /** 62 | * Deallocate memory - private. 63 | */ 64 | void _dealloc_SparseMatrix(); 65 | 66 | public: 67 | 68 | /** 69 | * Constructor. 70 | * @param num_rows Initial number of rows. 71 | * @param num_cols Initial number of columns. 72 | */ 73 | SparseMatrix(int num_rows, int num_cols); 74 | 75 | /** 76 | * Copy constructor. 77 | * @param mat Matrix to copy. 78 | */ 79 | SparseMatrix(const SparseMatrix& mat); 80 | 81 | /** 82 | * Submatrix copy constructor 83 | * @param mat Matrix to copy. 84 | * @param num_rows Number of rows to copy. 85 | * @param num_cols Number of columns to copy. 86 | * @param first_row Row offset. 87 | * @param first_col Column offset. 88 | */ 89 | SparseMatrix(const SparseMatrix& mat, int num_rows, int num_cols, int first_row = 0, int first_col = 0); 90 | 91 | SparseMatrix(int num_rows, int num_cols, SparseVector_p* rows); 92 | 93 | /** 94 | * Destructor. 95 | */ 96 | virtual ~SparseMatrix(); 97 | 98 | /** 99 | * Assignment operator. 100 | * @param mat Right-hand-side matrix in assignment 101 | * @return self. 102 | */ 103 | const SparseMatrix& operator= (const SparseMatrix& mat); 104 | 105 | /** 106 | * Read a matrix entry. 107 | * @param row Row of entry. 108 | * @param col Column of entry. 109 | * @return Value of entry. 110 | */ 111 | virtual double operator()(int row, int col) const; 112 | 113 | /** 114 | * Set one entry of the matrix. Non-existing entries are created. 115 | * @param row Row of entry. 116 | * @param col Column of entry. 117 | * @param val New value of entry. 118 | * @param grow_matrix Enlarge matrix if entry outside current size (default: false). 119 | */ 120 | virtual void set(int row, int col, const double val, bool grow_matrix = false); 121 | 122 | /** 123 | * Append a new entry to a row. Allows efficient adding of presorted elements. 124 | * @param row Row of new entry. 125 | * @param col Column of new entry - must be after last existing one in this row. 126 | * @param val Value of new entry. 127 | */ 128 | virtual void append_in_row(int row, int col,const double val); 129 | 130 | /** 131 | * Determine number of non-zero entries in sparse matrix. 132 | * @return Number of non-zero entries. 133 | */ 134 | virtual int nnz() const; 135 | 136 | /** 137 | * Determine maximum number of non-zero entries in any column. 138 | * @return Maximum number of non-zero entries in any column. 139 | */ 140 | virtual int max_nz() const; 141 | 142 | /** 143 | * Print sparse matrix as triples to stream; 144 | * also readable by Matlab: "load R.txt; S=spconvert(R); spy(S)" 145 | * @param out Output stream. 146 | */ 147 | virtual void print(std::ostream& out = std::cout) const; 148 | 149 | /** 150 | * Print sparse matrix as triples to file 151 | * @param file_name File name to write sparse matrix to. 152 | */ 153 | virtual void print(const std::string& file_name) const; 154 | 155 | /** 156 | * Print size of matrix and number of entries for debugging. 157 | */ 158 | virtual void print_stats() const; 159 | 160 | /** 161 | * Prints non-zero pattern to stdout. 162 | */ 163 | virtual void print_pattern() const; 164 | 165 | /** 166 | * Save eps graphics file with sparse matrix patterns. 167 | */ 168 | virtual void save_pattern_eps(const std::string& file_name) const; 169 | 170 | /** 171 | * Return a sparse row. 172 | * @param row Number of row to return. 173 | * @return Sparse row vector. 174 | */ 175 | virtual const SparseVector& get_row(int row) const; 176 | 177 | /** 178 | * Replace the given row. 179 | * @param row Number of row to replace. 180 | * @param new_row New row vector. 181 | */ 182 | virtual void set_row(int row, const SparseVector& new_row); 183 | 184 | /** 185 | * Import externally allocated rows. 186 | * @param num_rows Number of rows of new matrix. 187 | * @param num_cols Number of columns of new matrix. 188 | * @param rows Array of SparseVector of length num_rows. 189 | */ 190 | virtual void import_rows(int num_rows, int num_cols, SparseVector_p* rows); 191 | 192 | /** 193 | * Append new rows to matrix. 194 | * @param num Number of rows to add. 195 | */ 196 | virtual void append_new_rows(int num); 197 | 198 | /** 199 | * Append new columns to matrix. 200 | * @param num Number of columns to add. 201 | */ 202 | virtual void append_new_cols(int num); 203 | 204 | /** 205 | * Grow matrix to given number of rows; ignore if already at least as large. 206 | * @param num_rows Number of rows. 207 | */ 208 | virtual void ensure_num_rows(int num_rows); 209 | 210 | /** 211 | * Grow matrix to given number of columns; ignore if already at least as large. 212 | * @param num_cols Number of columns. 213 | */ 214 | virtual void ensure_num_cols(int num_cols); 215 | 216 | /** 217 | * Removes the last row (used in SparseSystem::add_row_givens). 218 | */ 219 | virtual void remove_row(); 220 | 221 | /** 222 | * Zero out an entry by applying a Givens rotation. Note that both 223 | * sparse rows have to be completely 0 on the left of col. 224 | * @param row The row from which row_bot is taken. 225 | * @param col The column of row_bot that should become 0. 226 | * @param c_givens Returns cosine of givens rotation if not NULL. 227 | * @param s_givens Returns sine of givens rotation if not NULL. 228 | */ 229 | virtual void apply_givens(int row, int col, double* c_givens = NULL, double* s_givens = NULL); 230 | 231 | /** 232 | * Triangulate matrix by applying Givens rotations to all entries below the diagonal. 233 | * @return Number of Givens rotations applied (for analysis). 234 | */ 235 | virtual int triangulate_with_givens(); 236 | 237 | inline int num_rows() const {return _num_rows;} 238 | inline int num_cols() const {return _num_cols;} 239 | 240 | friend class OrderedSparseMatrix; 241 | }; 242 | 243 | const Eigen::VectorXd operator*(const SparseMatrix& lhs, const Eigen::VectorXd& rhs); 244 | Eigen::VectorXd mul_SparseMatrixTrans_Vector(const SparseMatrix& lhs, const Eigen::VectorXd& rhs); 245 | SparseMatrix sparseMatrix_of_matrix(const Eigen::MatrixXd& m); 246 | Eigen::MatrixXd matrix_of_sparseMatrix(const SparseMatrix& s); 247 | 248 | } 249 | -------------------------------------------------------------------------------- /include/isam/SparseSystem.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseSystem.h 3 | * @brief Adds rhs functionality to sparse matrix for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: SparseSystem.h 4133 2011-03-22 20:40:38Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "OrderedSparseMatrix.h" 33 | 34 | namespace isam { 35 | 36 | class SparseSystem : public OrderedSparseMatrix { 37 | Eigen::VectorXd _rhs; 38 | public: 39 | SparseSystem(int num_rows, int num_cols); 40 | SparseSystem(const SparseSystem& mat); 41 | SparseSystem(const SparseSystem& mat, int num_rows, int num_cols, int first_row = 0, int first_col = 0); 42 | SparseSystem(int num_rows, int num_cols, SparseVector_p* rows, const Eigen::VectorXd& rhs); 43 | virtual ~SparseSystem(); 44 | const SparseSystem& operator= (const SparseSystem& mat); 45 | 46 | const Eigen::VectorXd& rhs() const {return _rhs;} 47 | void set_rhs(const Eigen::VectorXd& rhs) {_rhs = rhs;} 48 | 49 | // overridden functions 50 | 51 | /** 52 | * Note: While rows are passed in, the rhs is required to already 53 | * contain the new entry - necessary because we cannot change the 54 | * signature of the function. 55 | */ 56 | void apply_givens(int row, int col, double* c_givens = NULL, double* s_givens = NULL); 57 | 58 | void append_new_rows(int num); 59 | 60 | // new functions 61 | 62 | /** 63 | * Insert a new row 64 | * @param new_row The new sparse measurement row to add. 65 | * @param new_r New right hand side entry. 66 | */ 67 | virtual void add_row(const SparseVector& new_row, double new_r); 68 | 69 | /** 70 | * Insert a new measurement row and triangulate using Givens rotations 71 | * @param new_row The new sparse measurement row to add. 72 | * @param new_r New right hand side entry. 73 | * @return Number of Givens rotations applied (for analysis). 74 | */ 75 | virtual int add_row_givens(const SparseVector& new_row, double new_r); 76 | 77 | /** 78 | * Solve equation system by backsubstitution. 79 | * @return Solution for x in Rx=b' 80 | */ 81 | virtual Eigen::VectorXd solve() const; 82 | 83 | }; 84 | 85 | } 86 | -------------------------------------------------------------------------------- /include/isam/SparseVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseVector.cpp 3 | * @brief part of sparse matrix functionality for iSAM 4 | * @author Michael Kaess 5 | * @version $Id: SparseVector.h 3970 2011-02-16 00:16:54Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "isam/util.h" 33 | 34 | namespace isam { 35 | 36 | class SparseVector { 37 | int _nnz; 38 | int _nnz_max; 39 | int* _indices; 40 | double* _values; 41 | 42 | /** 43 | * Copy data from one sparse vector to a new one - private 44 | * @param vec Existing sparse vector to copy from. 45 | */ 46 | void _copy_from(const SparseVector& vec); 47 | 48 | /** 49 | * Deallocate memory - private. 50 | */ 51 | void _dealloc(); 52 | 53 | /** 54 | * Search for an index - private; 55 | * @return Index of entry, or if not in list, index of preceding entry + 1. 56 | */ 57 | int _search(int idx) const; 58 | 59 | /** 60 | * Resize the sparse vector - dangerous, only used internally - private. 61 | * @param new_nnz_max New size of vector. 62 | */ 63 | void _resize(int new_nnz_max); 64 | 65 | public: 66 | /** 67 | * Standard constructor. 68 | */ 69 | SparseVector(); 70 | 71 | SparseVector(int nnz_max) : _nnz(0), _nnz_max(nnz_max) { 72 | _indices = new int[_nnz_max]; 73 | _values = new double[_nnz_max]; 74 | } 75 | 76 | /** 77 | * Copy constructor - note that overwriting operator= is also necessary! 78 | * @param vec SparseVector to initialize from. 79 | */ 80 | SparseVector(const SparseVector& vec); 81 | 82 | /** 83 | * Subvector copy constructor. 84 | * @param vec SparseVector to copy from. 85 | * @param num Number of entries to copy (note sparse - most will not exist!) 86 | * @param first Index of first entry of new vector. 87 | */ 88 | SparseVector(const SparseVector& vec, int num, int first = 0); 89 | 90 | /** 91 | * Construct from raw data. 92 | * @param indices Array of row indices. 93 | * @param values Array of values. 94 | * @param nnz Length of vector. 95 | */ 96 | SparseVector(int* indices, double* values, int nnz); 97 | 98 | /** 99 | * Destructor. 100 | */ 101 | virtual ~SparseVector(); 102 | 103 | /** 104 | * Assignment operator (see also copy constructor). 105 | * @param vec Right-hand-side vector in assignment 106 | * @return self. 107 | */ 108 | const SparseVector& operator= (const SparseVector& vec); 109 | 110 | /** 111 | * Access value of an entry. 112 | * @param idx Index of entry to access. 113 | * @return Value of entry. 114 | */ 115 | double operator()(int idx) const; 116 | 117 | /** 118 | * Copy raw data, useful for converting to Matlab format. 119 | * @param indices Destination for row indices. 120 | * @param values Destination for values. 121 | */ 122 | void copy_raw(int* indices, double* values) const; 123 | 124 | /** 125 | * Create a new entry at the end of the sparse vector. 126 | * Used for efficient incremental creation of SparseVector in apply_givens(). 127 | * @param idx Index of entry to add, has to be greater than last_idx(). 128 | * @param val Value of new entry. 129 | */ 130 | inline void append(int idx, const double val = 0.) { 131 | requireDebug(_nnz==0 || _indices[_nnz-1] < idx, "SparseVector::append: index has to be after last entry"); 132 | 133 | if (_nnz+1 > _nnz_max) { 134 | // automatic resizing, amortized cost O(1) 135 | int new_nnz_max = _nnz_max*2; 136 | _resize(new_nnz_max); 137 | } 138 | // insert new entry 139 | _indices[_nnz] = idx; 140 | _values[_nnz] = val; 141 | _nnz++; 142 | } 143 | 144 | /** 145 | * Assign a value to a specific entry. 146 | * @param idx Index of entry to assign to. 147 | * @param val Value to assign. 148 | * @return True if new entry had to be created (needed to update row index) 149 | */ 150 | bool set(int idx, const double val = 0.); 151 | 152 | 153 | /** 154 | * Assign a value to a specific entry. 155 | * @param idx Index of entry to assign to. 156 | * @param vals Vector of values to assign. 157 | * @param c Number of values in double array val. 158 | * @return True if new entries had to be created 159 | */ 160 | bool set(int idx, const Eigen::VectorXd& vals); 161 | 162 | /** 163 | * Remove an entry; simply ignores non-existing entries. 164 | * @param idx Index of entry to remove. 165 | */ 166 | void remove(int idx); 167 | 168 | /** 169 | * Find index of first non-zero entry. 170 | * @return Index of first non-zero entry, or -1 if vector is empty. 171 | */ 172 | int first() const; 173 | 174 | /** 175 | * Find index of last non-zero entry 176 | * @return Index of last non-zero entry, or -1 if vector is empty. 177 | */ 178 | int last() const; 179 | 180 | /** 181 | * Shift indices starting at pos by offset. 182 | * @param num Number of new entries. 183 | * @param pos First index that should be shifted. 184 | */ 185 | void add_entries(int num, int pos); 186 | 187 | /** 188 | * Prints contents of vector as a list of tuples. 189 | */ 190 | void print() const; 191 | 192 | inline int nnz() const { 193 | return _nnz; 194 | } 195 | 196 | friend class SparseVectorIter; 197 | }; 198 | 199 | class SparseVectorIter { 200 | const SparseVector& s; 201 | int index; 202 | public: 203 | /** 204 | * Iterator for SparseVector. 205 | * @param sv SparseVector. 206 | */ 207 | inline SparseVectorIter(const SparseVector& sv) : s(sv), index(0) {} 208 | 209 | /** 210 | * Check if current element valid, ie. if we have not reached the end yet. 211 | * @return True if we still have a valid entry. 212 | */ 213 | inline bool valid() const { 214 | return (index < s._nnz); 215 | } 216 | 217 | /** 218 | * Get current element index. 219 | * @return Current element index. 220 | */ 221 | inline int get() const { 222 | requireDebug(index < s._nnz, "SparseVectorIter::get(): Index out of range."); 223 | int tmp = s._indices[index]; 224 | return tmp; 225 | } 226 | 227 | /** 228 | * Get current element index and value. 229 | * @param val Current element value returned. 230 | * @return Current element index. 231 | */ 232 | inline int get(double& val) const { 233 | requireDebug(index < s._nnz, "SparseVectorIter::get(): Index out of range."); 234 | val = s._values[index]; 235 | return s._indices[index]; 236 | } 237 | 238 | /** 239 | * Get current element value. 240 | * @return Current element value returned. 241 | */ 242 | inline double get_val() const { 243 | requireDebug(index < s._nnz, "SparseVectorIter::get_val(): Index out of range."); 244 | return s._values[index]; 245 | } 246 | 247 | /** 248 | * Go to next element. 249 | */ 250 | inline void next() { 251 | if (index < s._nnz) { 252 | index++; 253 | } 254 | } 255 | }; 256 | 257 | } 258 | 259 | -------------------------------------------------------------------------------- /include/isam/covariance.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file covariance.h 3 | * @brief Recovery of marginal covariance matrix. 4 | * @author Michael Kaess 5 | * @author Nicholas Carlevaris-Bianco 6 | * @version $Id: covariance.h 8578 2013-07-01 00:28:49Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include // pair 33 | #include 34 | #include 35 | 36 | #define USE_TR1 37 | #ifdef USE_TR1 38 | #include 39 | #else 40 | #include "boost/unordered_map.hpp" 41 | #endif 42 | 43 | #include "SparseMatrix.h" 44 | 45 | namespace isam { 46 | 47 | #ifdef USE_TR1 48 | #include 49 | typedef std::tr1::unordered_map umap; 50 | #else 51 | typedef boost::unordered_map umap; 52 | #endif 53 | 54 | class CovarianceCache { 55 | public: 56 | umap entries; 57 | // precalculated diagonal inverses 58 | std::vector diag; 59 | // recovering rows is expensive, buffer results 60 | std::vector rows; 61 | // avoid having to cleanup buffers each time by explicitly marking entries as valid 62 | std::vector rows_valid; 63 | // avoid having to cleanup valid entries by using different indices each time 64 | unsigned int current_valid; 65 | // stats 66 | int num_calc; 67 | 68 | CovarianceCache () { 69 | current_valid = 1; 70 | } 71 | }; 72 | 73 | typedef std::vector< std::vector > index_lists_t; 74 | typedef std::vector< std::pair > entry_list_t; 75 | 76 | /** 77 | * Takes a list of variable indices, and returns the marginal covariance matrix. 78 | * @param R Sparse factor matrix. 79 | * @param cache Covariance cache object. 80 | * @param index_lists List of lists of indices; a block will be recovered for each list. 81 | * @param debug Optional parameter to print timing information. 82 | * @param step Optional parameter to print statistics (default=-1, no stats printed). 83 | * @return List of dense marginal covariance matrices. 84 | */ 85 | std::list cov_marginal(const SparseMatrix& R, CovarianceCache& cache, 86 | const index_lists_t& index_lists, 87 | bool debug=false, int step=-1); 88 | 89 | /** 90 | * Takes a list of pairs of integers and returns the corresonding 91 | * entries of the covariance matrix. 92 | * @param R Sparse factor matrix. 93 | * @param cache Covariance cache object. 94 | * @param entry_lists List of pairs of integers refering to covariance matrix entries. 95 | * @param debug Optional parameter to print timing information. 96 | * @param step Optional parameter to print statistics (default=-1, no stats printed). 97 | * @return List of doubles corresponding to the requested covariance matrix entries. 98 | */ 99 | std::list cov_marginal(const SparseMatrix& R, CovarianceCache& cache, 100 | const entry_list_t& entry_list); 101 | 102 | } 103 | -------------------------------------------------------------------------------- /include/isam/glc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file glc.h 3 | * @brief Generic Linear Constraint functions 4 | * @author Nicholas Carlevaris-Bianco 5 | * @author Michael Kaess 6 | * @version $Id: glc.h 8578 2013-07-01 00:28:49Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "Node.h" 36 | #include "Factor.h" 37 | #include "Anchor.h" 38 | #include "GLCReparam.h" 39 | 40 | //#define GLC_DEBUG 41 | 42 | namespace isam { 43 | 44 | /** 45 | * Removes a node (variable) and all factors within in its elimination clique 46 | * replaces these factors with either a dense GLC factor or a set of sparse 47 | * GLC factors 48 | * Note that the node itself and the removed factors are not deallocated. 49 | * 50 | * We usually use GLC with USE_QUATERNIONS turned off in Rot3d.h. This 51 | * avoids having to account for the state transformation between euler and 52 | * angle axis representations. This is currently done numerically 53 | * (see exmap_jacobian()) and produces slightly higher KLD in the GLC results. 54 | * 55 | * @param node Pointer to node to be removed. 56 | * @param sparse Bool flag if new factors should be sparse approximate or dense 57 | * @param rp functor to reparamertize variables before linearization 58 | * @return vector of new factors. 59 | */ 60 | std::vector glc_remove_node(Slam& slam, Node* node, bool sparse = false, 61 | GLC_Reparam* rp = NULL); 62 | 63 | /** 64 | * Find the factors that will be eliminated and replaced when node is removed 65 | * IMPORTANT: must be called before node is removed using glc_remove_node() 66 | * @param node Pointer to node to be removed. 67 | * @return vector of factors that will be eliminated. 68 | */ 69 | std::vector glc_elim_factors(Node* node); 70 | 71 | #ifdef USE_QUATERNIONS 72 | Eigen::MatrixXd exmap_jacobian (const std::vector& nodes); 73 | #endif 74 | 75 | /** 76 | * Generic Linear Constraint 77 | */ 78 | class GLC_Factor : public Factor { 79 | 80 | public: 81 | const Eigen::VectorXd _x; 82 | const Eigen::MatrixXd _G; 83 | GLC_Reparam* _rp; 84 | 85 | /** 86 | * Constructor. 87 | * @param poses Vector of Pose3d_Node pointers which support the factor 88 | * @param x The state vector used as the linearization point for the GLC 89 | * @param G The PCA transformation calculated from the target information 90 | * @param rp Reparametrization object, uses to implement root-shift or user defined reparameterizations 91 | * to be applied before commiting to a linearization point 92 | */ 93 | GLC_Factor(std::vector nodes, const Eigen::VectorXd& x, const Eigen::MatrixXd& G, GLC_Reparam* rp = NULL) 94 | : Factor("GLC_Factor", G.rows(), Information(Eigen::MatrixXd::Identity(G.rows(), G.rows()))), 95 | _x(x), _G(G) 96 | { 97 | _nodes.resize(nodes.size()); 98 | for (size_t i=0; iclone(); 104 | _rp->set_nodes(_nodes); 105 | } 106 | 107 | } 108 | 109 | ~GLC_Factor () { 110 | if (_rp != NULL) 111 | delete _rp; 112 | } 113 | 114 | void initialize() { 115 | } 116 | 117 | Jacobian jacobian () { 118 | 119 | Eigen::MatrixXd H; 120 | if (_rp) { 121 | H = sqrtinf() * _G * _rp->jacobian(); 122 | }else { 123 | H = sqrtinf() * _G; 124 | } 125 | 126 | #ifdef USE_QUATERNIONS 127 | Eigen::MatrixXd Jexmap = exmap_jacobian (_nodes); 128 | H = H * Jexmap; 129 | #endif 130 | 131 | Eigen::VectorXd r = error(LINPOINT); 132 | Jacobian jac(r); 133 | int position = 0; 134 | int n_measure = dim(); 135 | for (size_t i=0; i<_nodes.size(); i++) { 136 | int n_var = _nodes[i]->dim(); 137 | Eigen::MatrixXd Hi = H.block(0, position, n_measure, n_var); 138 | position += n_var; 139 | jac.add_term(_nodes[i], Hi); 140 | } 141 | 142 | #ifdef GLC_DEBUG 143 | // compare with numerical jacobian 144 | Jacobian njac = Factor::jacobian(); 145 | Eigen::MatrixXd nH(n_measure, H.cols()); 146 | int offset = 0; 147 | for (Terms::const_iterator it=njac.terms().begin(); it!=njac.terms().end(); it++) { 148 | int ncols = it->term().cols(); 149 | nH.block(0, offset, n_measure, ncols) = it->term(); 150 | offset += ncols; 151 | } 152 | if ((nH - H).array().abs().matrix().lpNorm() > 1e-6) { 153 | std::cout << "Ja = [" << H << "];" << std::endl; 154 | std::cout << "Jn = [" << nH << "];" << std::endl; 155 | } 156 | #endif 157 | return jac; 158 | } 159 | 160 | Eigen::VectorXd basic_error(Selector s = LINPOINT) const { 161 | int nn = _nodes.size(); 162 | Eigen::VectorXd x_p; 163 | Eigen::VectorXb is_angle; 164 | 165 | if (_rp) { 166 | x_p = _rp->reparameterize(s); 167 | is_angle = _rp->is_angle(); 168 | } else { 169 | x_p.resize(_G.cols()); 170 | is_angle.resize(_G.cols()); 171 | int off = 0; 172 | for (int i=0; idim(); 174 | x_p.segment(off, d) = _nodes[i]->vector(s); 175 | is_angle.segment(off, d) = _nodes[i]->is_angle(); 176 | off += _nodes[i]->dim(); 177 | } 178 | } 179 | 180 | Eigen::VectorXd err_fs = x_p - _x; 181 | for (int i=0; i. 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | // all include files needed by a user of iSAM 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | 45 | // Doxygen documentation follows 46 | 47 | /** @mainpage iSAM: Incremental Smoothing and Mapping (v1.6) 48 | 49 | 50 | @section intro_what What is iSAM? 51 | 52 | iSAM is an optimization library for sparse nonlinear problems as 53 | encountered in simultaneous localization and mapping (SLAM). The iSAM 54 | library provides efficient algorithms for batch and incremental 55 | optimization, recovering the exact least-squares solution. The library 56 | can easily be extended to new problems, and functionality for often 57 | encountered 2D and 3D SLAM problems is already provided. The iSAM 58 | algorithm was originally developed by Michael 60 | Kaess (kaess@mit.edu) and Frank 62 | Dellaert (dellaert@cc.gatech.edu) at Georgia Tech. 63 | 64 | 65 | @section intro_copyright Copyright and License 66 | 67 | Copyright (C) 2009-2012 Massachusetts Institute of Technology.
Michael 69 | Kaess (kaess@mit.edu), Hordur Johannsson (hordurj@mit.edu), David 70 | Rosen (dmrosen@mit.edu) and John Leonard (jleonard@mit.edu) 72 | 73 | iSAM is free software; you can redistribute it and/or modify it under 74 | the terms of the GNU Lesser General Public License as published by the 75 | Free Software Foundation; either version 2.1 of the License, or (at 76 | your option) any later version. 77 | 78 | iSAM is distributed in the hope that it will be useful, but WITHOUT 79 | ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 80 | FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 81 | License for more details. 82 | 83 | You should have received a copy of the GNU Lesser General Public 84 | License along with iSAM. If not, see . 85 | 86 | 87 | @section intro_download Download 88 | 89 | The latest version of iSAM is available at http://people.csail.mit.edu/kaess/isam 92 | 93 | The source code is available from our public subversion repository: 94 | @verbatim 95 | svn co https://svn.csail.mit.edu/isam 96 | @endverbatim 97 | 98 | 99 | @section intro_requirements Requirements 100 | 101 | This software was tested under Ubuntu 9.04-11.04, and Mac OS X 102 | 10.5-10.6. iSAM depends on the following software: 103 | 104 | @li CMake (version 2.8 or higher) 105 | @li CHOLMOD (sparse matrix libraries by Tim Davis, University of Florida) 106 | @li Eigen (version 3) 107 | @li SDL (optional, needed for 3D viewer) 108 | @li doxygen (optional, to generate documentation) 109 | @li graphviz (optional, to generate documentation) 110 | 111 | To install all required packages in Ubuntu 9.10 and later, simply type: 112 | 113 | @verbatim 114 | sudo apt-get install cmake libsuitesparse-dev libeigen3-dev libsdl1.2-dev doxygen graphviz 115 | @endverbatim 116 | 117 | Note that CHOLMOD is contained in SuiteSparse. On Mac OS X, SuiteSparse 118 | and SDL are available through MacPorts: 119 | 120 | @verbatim 121 | sudo port install suitesparse libsdl 122 | @endverbatim 123 | 124 | 125 | @section intro_installation Installation 126 | 127 | Compile with: 128 | @verbatim 129 | make 130 | @endverbatim 131 | 132 | Directory structure: 133 | @li @c isamlib/ contains the source code for the iSAM library 134 | @li @c include/ contains the header files for the iSAM library 135 | @li @c isam/ contains the source code for the iSAM executable 136 | @li @c examples/ contains examples for using different components of the iSAM library 137 | @li @c doc/ contains the documentation (after calling "make doc") 138 | @li @c misc/ contains code referenced from publications 139 | @li @c data/ contains example data files 140 | @li @c lib/ contains the actual library "libisam.a" 141 | @li @c bin/ contains the main executable "isam" 142 | 143 | Usage example: 144 | @verbatim 145 | bin/isam -G data/sphere400.txt 146 | @endverbatim 147 | 148 | For more usage information: 149 | @verbatim 150 | bin/isam -h 151 | @endverbatim 152 | 153 | Install the library in your system with: 154 | @verbatim 155 | make install 156 | @endverbatim 157 | 158 | Note that make just provides a convenient wrapper for running cmake in 159 | a separate "build" directory. Compile options can be changed with 160 | "ccmake build". In particular, support for the 3D viewer can be 161 | disabled by setting USE_GUI to OFF. Library and include paths can be 162 | modified manually in case SuiteSparse/CHOLMOD was installed in a local 163 | directory and cannot be found automatically. 164 | 165 | @section intro_documentation Documentation 166 | 167 | To generate detailed documentation for the source code, type: 168 | @verbatim 169 | make doc 170 | @endverbatim 171 | and open @c doc/html/index.html in your browser. 172 | 173 | 174 | @section intro_references References (please cite when using this software) 175 | 176 | Details of the algorithms used in this software are provided in these 177 | publications (the latex bibliography file @c isam.bib is included for 178 | convenience). 179 | 180 | @li "iSAM: Incremental Smoothing and Mapping" by M. Kaess, 181 | A. Ranganathan, and F. Dellaert, IEEE Trans. on Robotics, TRO, 182 | vol. 24, no. 6, Dec. 2008, pp. 1365-1378, PDF 185 | 186 | @li "Covariance Recovery from a Square Root Information Matrix for 187 | Data Association" by M. Kaess and F. Dellaert, Journal of Robotics and 188 | Autonomous Systems, RAS, vol. 57, Dec. 2009, pp. 1198-1210, PDF 191 | 192 | @li "An Incremental Trust-Region Method for Robust Online Sparse 193 | Least-Squares Estimation" by D.M. Rosen, M. Kaess and J.J. Leonard, 194 | International Conference on Robotics and Automation (ICRA), May 2012, 195 | PDF 197 | 198 | A full list of iSAM-related references in BibTeX format is available 199 | here: \ref Bibliography. 200 | 201 | Newer publications will be available from my web page at http://people.csail.mit.edu/kaess/ 204 | 205 | 206 | @section intro_ack Acknowledgments 207 | 208 | Many thanks to Richard Roberts for his help with this software. Thanks 209 | also to John McDonald, Ayoung Kim, Ryan Eustice, Aisha Walcott, Been 210 | Kim and Abe Bachrach for their feedback and patience with earlier 211 | versions. 212 | 213 | */ 214 | -------------------------------------------------------------------------------- /include/isam/numericalDiff.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file numericalDiff.h 3 | * @brief Numerical differentiation. 4 | * @author Michael Kaess 5 | * @version $Id: numericalDiff.h 4038 2011-02-26 04:31:00Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "isam/Node.h" 34 | 35 | namespace isam { 36 | 37 | // Abstract class to enforce interface for function object. 38 | class Function { 39 | public: 40 | virtual ~Function() {} 41 | virtual int num_measurements() const = 0; 42 | virtual Eigen::VectorXd evaluate() const = 0; 43 | virtual std::vector& nodes() = 0; 44 | }; 45 | 46 | /** 47 | * Takes a general vector valued function and returns the 48 | * Jacobian at the linearization point given by x0. 49 | * @param func Function object with evaluation function that takes and returns vectors. 50 | * @return Matrix containing the Jacobian of func, with 51 | * dim(y) rows and dim(x) columns, where y=func(x). 52 | */ 53 | Eigen::MatrixXd numericalDiff(Function& func); 54 | 55 | } 56 | -------------------------------------------------------------------------------- /include/isam/robust.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robust.h 3 | * @brief Robust estimator functionality. 4 | * @author Michael Kaess 5 | * @version $Id: robust.h 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "util.h" 33 | 34 | namespace isam { 35 | 36 | /* 37 | * robust error functions following Hartley&Zisserman book (2nd edition, pages 616-622) 38 | * summary: 39 | * - squared error is not robust 40 | * - Blake-Zisserman, corrupted Gaussian and Cauchy functions are 41 | * non-convex, and therefore require very good initialization 42 | * - L1 yields a stable minimum, the median, but is non-differentiable at the origin 43 | * - Huber cost function is convex, combining squared near the origin, L1 further out 44 | * - pseudo-Huber is a good alternative to Huber, with continuous derivatives of all orders 45 | */ 46 | 47 | /** 48 | * Standard squared cost function. 49 | * @param d Unmodified cost/distance. 50 | */ 51 | inline double cost_squared(double d) { 52 | return d*d; 53 | } 54 | 55 | /** 56 | * Robust Blake-Zisserman cost function. 57 | * @param d Unmodified cost/distance. 58 | * @param e Approximate crossover between inliers and outliers: d^2 = -log(e). 59 | */ 60 | inline double cost_blake_zisserman(double d, double e) { 61 | return -log(exp(-d*d) + e); 62 | } 63 | 64 | /** 65 | * Robust cost function using corrupted Gaussian. 66 | * @param d Unmodified cost/distance. 67 | * @param w Ratio of standard deviations of the outliers to the inliers. 68 | * @param a Expected fraction of inliers. 69 | */ 70 | inline double cost_corrupted_gaussian(double d, double w, double a) { 71 | double d2 = d*d; 72 | double w2 = w*w; 73 | return -log(a*exp(-d2) + (1.-a)*exp(-d2/w2)/w); 74 | } 75 | 76 | /** 77 | * Robust Cauchy cost function. 78 | * @param d Unmodified cost/distance. 79 | * @param b Determines for which range of d the function closely 80 | * approximates the squared error function. 81 | */ 82 | inline double cost_cauchy(double d, double b = 1.) { 83 | // the first term is a constant and could be omitted 84 | return log(M_PI/b) * log(1. + d*d/(b*b)); 85 | } 86 | 87 | /** 88 | * Robust L1 cost function, the sum of absolute errors. 89 | * @param d Unmodified cost/distance. 90 | * @param b ? 91 | */ 92 | inline double cost_l1(double d, double b = 0.5) { 93 | return 2.*b*fabs(d); 94 | } 95 | 96 | /** 97 | * Robust Huber cost function. 98 | * @param d Unmodified cost/distance. 99 | * @param b Approximately the outlier threshold. 100 | */ 101 | inline double cost_huber(double d, double b) { 102 | double abs_d = fabs(d); 103 | if (abs_d < b) { 104 | return d*d; 105 | } else { 106 | return 2*b*abs_d - b*b; 107 | } 108 | } 109 | 110 | /** 111 | * Robust Pseudo-Huber cost function. 112 | * @param d Unmodified cost/distance. 113 | * @param b ? 114 | */ 115 | inline double cost_pseudo_huber(double d, double b) { 116 | double b2 = b*b; 117 | return 2*b2*(sqrt(1+d*d/b2) - 1); 118 | } 119 | 120 | } 121 | -------------------------------------------------------------------------------- /include/isam/slam_depthmono.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file slam_depthmono.h 3 | * @brief Provides specialized nodes and factors for depthmono vision applications. 4 | * @author Michael Kaess 5 | * @version $Id: slam_depthmono.h 7956 2013-01-23 16:46:03Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "Node.h" 36 | #include "Factor.h" 37 | #include "Pose3d.h" 38 | #include "Point3dh.h" 39 | 40 | namespace isam { 41 | 42 | class DepthmonoMeasurement { 43 | friend std::ostream& operator<<(std::ostream& out, const DepthmonoMeasurement& t) { 44 | t.write(out); 45 | return out; 46 | } 47 | 48 | public: 49 | double u; 50 | double v; 51 | double depth; 52 | bool valid; 53 | 54 | DepthmonoMeasurement(double u, double v, double depth) 55 | : u(u), v(v), depth(depth), valid(true) { 56 | } 57 | DepthmonoMeasurement(double u, double v, double depth, bool valid) 58 | : u(u), v(v), depth(depth), valid(valid) { 59 | } 60 | 61 | Eigen::Vector3d vector() const { 62 | Eigen::Vector3d tmp(u, v, depth); 63 | return tmp; 64 | } 65 | 66 | void write(std::ostream &out) const { 67 | out << "(" << u << ", " << v << ", " << depth << ")"; 68 | } 69 | }; 70 | 71 | class DepthmonoCamera { // for now, camera and robot are identical 72 | double _f; 73 | Eigen::Vector2d _pp; 74 | double _b; 75 | 76 | public: 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | 79 | DepthmonoCamera() : _f(1), _pp(Eigen::Vector2d(0.5,0.5)) {} 80 | DepthmonoCamera(double f, const Eigen::Vector2d& pp) : _f(f), _pp(pp) {} 81 | 82 | inline double focalLength() const {return _f;} 83 | 84 | inline Eigen::Vector2d principalPoint() const {return _pp;} 85 | 86 | DepthmonoMeasurement project(const Pose3d& pose, const Point3dh& Xw) const { 87 | Point3dh X = pose.transform_to(Xw); 88 | // camera system has z pointing forward, instead of x 89 | double x = X.y(); 90 | double y = X.z(); 91 | double z = X.x(); 92 | double w = X.w(); 93 | 94 | double fz = _f / z; 95 | double u = x * fz + _pp(0); 96 | double v = y * fz + _pp(1); 97 | 98 | double depth = 5.; 99 | bool valid = ((w==0&&z>0) || (z/w) > 0.); // infront of camera? 100 | if (valid) { 101 | depth = z / w; 102 | } 103 | // Setting a point valid can make the system (hj 25/10/2012) 104 | // ill conditioned, i.e. if point goes behind all cameras that see it 105 | //valid = true; 106 | 107 | return DepthmonoMeasurement(u, v, depth, valid); 108 | } 109 | 110 | 111 | Point3dh backproject(const Pose3d& pose, const DepthmonoMeasurement& measure) const { 112 | double lx = (measure.u-_pp(0)) * measure.depth / _f; 113 | double ly = (measure.v-_pp(1)) * measure.depth / _f; 114 | double lz = measure.depth; 115 | double lw = 1.; 116 | if (lz<0.) { 117 | std::cout << "Warning: DepthmonoCamera.backproject called with negative disparity\n"; 118 | } 119 | Point3dh X(lz, lx, ly, lw); 120 | 121 | return pose.transform_from(X); 122 | } 123 | 124 | }; 125 | 126 | /** 127 | * Depthmono observation of a 3D homogeneous point; 128 | * projective or Euclidean geometry depending on constructor used. 129 | */ 130 | class Depthmono_Factor : public FactorT { 131 | Pose3d_Node* _pose; 132 | Point3d_Node* _point; 133 | Point3dh_Node* _point_h; 134 | DepthmonoCamera* _camera; 135 | bool _relative; 136 | Pose3d_Node* _base; 137 | 138 | public: 139 | 140 | // constructor for projective geometry 141 | Depthmono_Factor(Pose3d_Node* pose, Point3dh_Node* point, DepthmonoCamera* camera, 142 | const DepthmonoMeasurement& measure, const Noise& noise, bool relative = false) 143 | : FactorT("Depthmono_Factor", 3, noise, measure), 144 | _pose(pose), _point(NULL), _point_h(point), _camera(camera), _relative(relative), _base(NULL) { 145 | // DepthmonoCamera could also be a node later (either with 0 variables, 146 | // or with calibration as variables) 147 | _nodes.resize(2); 148 | _nodes[0] = pose; 149 | _nodes[1] = point; 150 | // for relative parameterization recover base pose 151 | if (_relative) { 152 | if (!point->factors().empty()) { 153 | _nodes.resize(3); 154 | _nodes[2] = point->factors().front()->nodes()[0]; 155 | // todo: first factor might refer to a prior or other type of node... 156 | _base = dynamic_cast(_nodes[2]); 157 | } else { 158 | _base = _pose; 159 | } 160 | point->set_base(_base); 161 | } 162 | } 163 | 164 | // constructor for Euclidean geometry 165 | // WARNING: only use for points at short range 166 | Depthmono_Factor(Pose3d_Node* pose, Point3d_Node* point, DepthmonoCamera* camera, 167 | const DepthmonoMeasurement& measure, const Noise& noise, bool relative = false) 168 | : FactorT("Depthmono_Factor", 3, noise, measure), 169 | _pose(pose), _point(point), _point_h(NULL), _camera(camera), _relative(relative), _base(NULL) { 170 | _nodes.resize(2); 171 | _nodes[0] = pose; 172 | _nodes[1] = point; 173 | // for relative parameterization recover base pose 174 | if (_relative) { 175 | if (!point->factors().empty()) { 176 | _nodes.resize(3); 177 | _nodes[2] = point->factors().front()->nodes()[0]; 178 | // todo: first factor might refer to a prior or other type of node... 179 | _base = dynamic_cast(_nodes[2]); 180 | } else { 181 | _base = _pose; 182 | } 183 | point->set_base(_base); 184 | } 185 | } 186 | 187 | void initialize() { 188 | require(_pose->initialized(), "Depthmono_Factor requires pose to be initialized"); 189 | bool initialized = (_point_h!=NULL) ? _point_h->initialized() : _point->initialized(); 190 | if (!initialized) { 191 | Point3dh predict; 192 | if (_relative) { 193 | predict = _camera->backproject(Pose3d(), _measure); 194 | } else { 195 | predict = _camera->backproject(_pose->value(), _measure); 196 | } 197 | // normalize homogeneous vector 198 | predict = Point3dh(predict.vector()).normalize(); 199 | if (_point_h!=NULL) { 200 | _point_h->init(predict); 201 | } else { 202 | _point->init(predict.to_point3d()); 203 | } 204 | } 205 | } 206 | 207 | Eigen::VectorXd basic_error(Selector s = ESTIMATE) const { 208 | Point3dh point = (_point_h!=NULL) ? _point_h->value(s) : _point->value(s); 209 | Pose3d pose = _pose->value(s); 210 | if (_base) { 211 | // pose of camera relative to base camera (which might be this one!) 212 | pose = pose.ominus(_base->value(s)); 213 | } 214 | DepthmonoMeasurement predicted = _camera->project(pose, point); 215 | if (_point_h!=NULL || predicted.valid == true) { 216 | return (predicted.vector() - _measure.vector()); 217 | } else { 218 | //std::cout << "Warning - DepthmonoFactor.basic_error: point behind camera, dropping term.\n"; 219 | std::cout << "Warning - DepthmonoFactor.basic_error: " << this 220 | << " #: " << _nodes[1]->factors().size() << " " << _nodes[0]->factors().size() 221 | << " \npoint: " << point << " ptr: " << _point 222 | << " pose: " << pose << " ptr: " << _pose 223 | << " \npredicted: " << predicted.vector().transpose() 224 | << " measure: " << _measure.vector().transpose() << std::endl; 225 | return Eigen::Vector3d::Zero(); 226 | } 227 | } 228 | 229 | }; 230 | 231 | } 232 | -------------------------------------------------------------------------------- /include/isam/slam_monocular.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file slam_monocular.h 3 | * @brief Provides nodes and factors for monocular vision applications. 4 | * @author Michael Kaess 5 | * @version $Id: slam_monocular.h 8429 2013-05-15 02:22:17Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | // WARNING: The monocular factor has not been tested extensively... 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "Node.h" 38 | #include "Factor.h" 39 | #include "Pose3d.h" 40 | #include "Point3dh.h" 41 | 42 | namespace isam { 43 | 44 | class MonocularMeasurement { 45 | friend std::ostream& operator<<(std::ostream& out, const MonocularMeasurement& t) { 46 | t.write(out); 47 | return out; 48 | } 49 | 50 | public: 51 | double u; 52 | double v; 53 | bool valid; 54 | 55 | MonocularMeasurement(double u, double v) : u(u), v(v), valid(true) {} 56 | MonocularMeasurement(double u, double v, bool valid) : u(u), v(v), valid(valid) {} 57 | 58 | Eigen::Vector3d vector() const { 59 | Eigen::Vector3d tmp(u, v); 60 | return tmp; 61 | } 62 | 63 | void write(std::ostream &out) const { 64 | out << "(" << u << ", " << v << ")"; 65 | } 66 | }; 67 | 68 | class MonocularCamera { // for now, camera and robot are identical 69 | double _f; 70 | Eigen::Vector2d _pp; 71 | double _b; 72 | 73 | public: 74 | 75 | MonocularCamera() : _f(1), _pp(Eigen::Vector2d(0.5,0.5)) {} 76 | MonocularCamera(double f, const Eigen::Vector2d& pp) : _f(f), _pp(pp) {} 77 | 78 | inline double focalLength() const {return _f;} 79 | 80 | inline Eigen::Vector2d principalPoint() const {return _pp;} 81 | 82 | MonocularMeasurement project(const Pose3d& pose, const Point3dh& Xw) const { 83 | Point3dh X = pose.transform_to(Xw); 84 | // camera system has z pointing forward, instead of x 85 | double x = X.y(); 86 | double y = X.z(); 87 | double z = X.x(); 88 | double w = X.w(); 89 | double fz = _f / z; 90 | double u = x * fz + _pp(0); 91 | double v = y * fz + _pp(1); 92 | bool valid = ((w==0&&z>0) || (z/w) > 0.); // infront of camera? 93 | // Setting a point valid can make the system (hj 25/10/2012) 94 | // ill conditioned, i.e. if point goes behind all cameras that see it 95 | //valid = true; 96 | 97 | return MonocularMeasurement(u, v, valid); 98 | } 99 | 100 | Point3dh backproject(const Pose3d& pose, const MonocularMeasurement& measure, 101 | double distance = 5.) const { 102 | double lx = (measure.u-_pp(0)) * distance / _f; 103 | double ly = (measure.v-_pp(1)) * distance / _f; 104 | double lz = distance; 105 | double lw = 1; 106 | if (lz<0.) { 107 | std::cout << "Warning: MonocularCamera.backproject called with negative distance\n"; 108 | } 109 | Point3dh X(lz, lx, ly, lw); 110 | 111 | return pose.transform_from(X); 112 | } 113 | 114 | }; 115 | 116 | //typedef NodeT Point3dh_Node; 117 | 118 | /** 119 | * Monocular observation of a 3D homogeneous point; 120 | * projective or Euclidean geometry depending on constructor used. 121 | */ 122 | class Monocular_Factor : public FactorT { 123 | Pose3d_Node* _pose; 124 | Point3d_Node* _point; 125 | Point3dh_Node* _point_h; 126 | MonocularCamera* _camera; 127 | 128 | public: 129 | 130 | // constructor for projective geometry 131 | Monocular_Factor(Pose3d_Node* pose, Point3dh_Node* point, MonocularCamera* camera, 132 | const MonocularMeasurement& measure, const isam::Noise& noise) 133 | : FactorT("Monocular_Factor", 2, noise, measure), 134 | _pose(pose), _point(NULL), _point_h(point), _camera(camera) { 135 | // MonocularCamera could also be a node later (either with 0 variables, 136 | // or with calibration as variables) 137 | _nodes.resize(2); 138 | _nodes[0] = pose; 139 | _nodes[1] = point; 140 | } 141 | 142 | // constructor for Euclidean geometry - WARNING: only use for points at short range 143 | Monocular_Factor(Pose3d_Node* pose, Point3d_Node* point, MonocularCamera* camera, 144 | const MonocularMeasurement& measure, const isam::Noise& noise) 145 | : FactorT("Monocular_Factor", 2, noise, measure), 146 | _pose(pose), _point(point), _point_h(NULL), _camera(camera) { 147 | _nodes.resize(2); 148 | _nodes[0] = pose; 149 | _nodes[1] = point; 150 | } 151 | 152 | void initialize() { 153 | require(_pose->initialized(), "Monocular_Factor requires pose to be initialized"); 154 | bool initialized = (_point_h!=NULL) ? _point_h->initialized() : _point->initialized(); 155 | if (!initialized) { 156 | Point3dh predict = _camera->backproject(_pose->value(), _measure); 157 | // normalize homogeneous vector 158 | predict = Point3dh(predict.vector()).normalize(); 159 | if (_point_h!=NULL) { 160 | _point_h->init(predict); 161 | } else { 162 | _point->init(predict.to_point3d()); 163 | } 164 | } 165 | } 166 | 167 | Eigen::VectorXd basic_error(Selector s = ESTIMATE) const { 168 | Point3dh point = (_point_h!=NULL) ? _point_h->value(s) : _point->value(s); 169 | MonocularMeasurement predicted = _camera->project(_pose->value(s), point); 170 | if (predicted.valid == true) { 171 | return (predicted.vector() - _measure.vector()); 172 | } else { 173 | // effectively disables points behind the camera 174 | return Eigen::Vector2d::Zero(); 175 | } 176 | } 177 | 178 | }; 179 | 180 | } 181 | -------------------------------------------------------------------------------- /include/isam/util.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file util.h 3 | * @brief Basic utility functions that are independent of iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: util.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace isam { 37 | 38 | // some math constants 39 | #ifndef PI 40 | const double PI = 3.14159265358979323846; 41 | #endif 42 | const double TWOPI = 2.0*PI; 43 | const double HALFPI = PI/2.0; 44 | 45 | // values up to this constant are considered zero and removed from the matrix 46 | const double NUMERICAL_ZERO = 1e-12; 47 | 48 | /** 49 | * Return current system time in seconds. 50 | */ 51 | double tic(); 52 | 53 | /** 54 | * Remember and return system time in seconds. 55 | * @param id Name of time slot. 56 | */ 57 | double tic(std::string id); 58 | 59 | /** 60 | * Return difference between current system time and t in seconds. 61 | * @param t0 Start time as returned by tic(); 62 | */ 63 | double toc(double t0); 64 | 65 | /** 66 | * Return difference between current system time and remembered time in 67 | * seconds, and add to accumulated time. 68 | * @param id Name of time slot. 69 | */ 70 | double toc(std::string id); 71 | 72 | /** 73 | * Print a list of accumulated times and additional statistics 74 | * for each name used in tic/toc. 75 | */ 76 | void tictoc_print(); 77 | 78 | /** 79 | * Return the accumulated time. 80 | * @param id Name of time slot. 81 | */ 82 | double tictoc(std::string id); 83 | 84 | /** 85 | * Return identity matrix. 86 | */ 87 | Eigen::MatrixXd eye(int num); 88 | 89 | /** 90 | * Calculate Givens rotation so that a specific entry becomes 0. 91 | * @param a Diagonal entry from above. 92 | * @param b Entry to be zeroed out. 93 | * @param c Resulting cosine part. 94 | * @param s Resulting sine part. 95 | */ 96 | void givens(const double a, const double b, double& c, double& s); 97 | 98 | /** 99 | * Normalize angle to be within the interval [-pi,pi]. 100 | */ 101 | inline double standardRad(double t) { 102 | if (t >= 0.) { 103 | t = fmod(t+PI, TWOPI) - PI; 104 | } else { 105 | t = fmod(t-PI, -TWOPI) + PI; 106 | } 107 | return t; 108 | } 109 | 110 | inline double deg_to_rad(double d) { 111 | return (d/180.*PI); 112 | } 113 | 114 | inline double rad_to_deg(double r) { 115 | return (r/PI*180.); 116 | } 117 | 118 | /** 119 | * Calculate the pseudo inverse of an arbitrary matrix using the SVD 120 | * @input a Eigen matrix to invert 121 | * @input eps Numerical epsilon to determine zero singular values (defaults to std::numeric_limits::eps) 122 | * @return Eigen matrix of same type as a 123 | */ 124 | template 125 | T pinv(const T &a, double eps = std::numeric_limits::epsilon()) { 126 | 127 | bool m_lt_n = (a.rows() < a.cols()); 128 | 129 | Eigen::JacobiSVD svd; 130 | if (m_lt_n) { 131 | T tmp = a.transpose(); 132 | svd = tmp.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); 133 | } else { 134 | svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV); 135 | } 136 | 137 | typename T::Scalar tolerance = eps * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff(); 138 | 139 | T result = svd.matrixV() * 140 | T( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).asDiagonal() * 141 | svd.matrixU().adjoint(); 142 | 143 | if (m_lt_n) { 144 | return result.transpose(); 145 | } else { 146 | return result; 147 | } 148 | } 149 | 150 | /** 151 | * Calculate the pseudo inverse of an positive semidefinite matrix using the eigenvalue decomposition 152 | * @input a Eigen matrix to invert 153 | * @input eps Numerical epsilon to determine zero singular values (defaults to std::numeric_limits::eps) 154 | * @return Eigen matrix of same type as a 155 | */ 156 | template 157 | T posdef_pinv(const T &a, double eps = std::numeric_limits::epsilon()) { 158 | 159 | Eigen::SelfAdjointEigenSolver eig(a); 160 | if (eig.info() != Eigen::Success) { 161 | return T(); 162 | } 163 | 164 | typename T::Scalar tolerance = eps * std::max(a.cols(), a.rows()) * eig.eigenvalues().array().abs().maxCoeff(); 165 | 166 | T result = eig.eigenvectors() * 167 | T( (eig.eigenvalues().array() > tolerance).select(eig.eigenvalues().array().inverse(), 0) ).asDiagonal() * 168 | eig.eigenvectors().transpose(); 169 | 170 | return result; 171 | } 172 | 173 | 174 | #ifdef NDEBUG 175 | // Release mode 176 | // remove requirements in inner loops for speed, but keep standard require functional 177 | #define requireDebug(req,msg) 178 | #define require(req,msg) if (!(req)) {fputs(msg, stderr);fputs("\n\n", stderr); exit(1);} 179 | #else 180 | // Debug mode 181 | // cause a crash to allow backtracing 182 | #define requireDebug(req,msg) if (!(req)) {fputs(msg, stderr);fputs("\n\n", stderr); abort();} 183 | #define require(req,msg) requireDebug(req,msg) 184 | #endif 185 | 186 | } 187 | -------------------------------------------------------------------------------- /isamlib/Anchor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Anchor.cpp 3 | * @brief Implementation of the anchor node. 4 | * @author Hordur Johannsson 5 | * @author Michael Kaess 6 | * @version $Id: Anchor.cpp 6335 2012-03-22 23:13:52Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace isam 34 | { 35 | 36 | Anchor2d_Node::Anchor2d_Node(Slam* slam) : Pose2d_Node("Anchor2d"), _parent(NULL), _prior(NULL), _slam(slam) 37 | { 38 | } 39 | 40 | Anchor2d_Node::~Anchor2d_Node() { 41 | if (_prior) { 42 | _slam->remove_factor(_prior); 43 | delete _prior; 44 | } 45 | } 46 | 47 | void Anchor2d_Node::set_prior() { 48 | Noise noise = SqrtInformation(10000. * eye(3)); 49 | Pose2d prior_origin(0., 0., 0.); 50 | _prior = new Pose2d_Factor(this, prior_origin, noise); 51 | _slam->add_factor(_prior); 52 | } 53 | 54 | void Anchor2d_Node::add_anchor(Anchor2d_Node* a) { 55 | if (a->_prior) { 56 | _slam->remove_factor(a->_prior); 57 | delete a->_prior; 58 | a->_prior = NULL; 59 | } 60 | a->_parent = this; 61 | _childs.push_back(a); 62 | } 63 | 64 | void Anchor2d_Node::merge(Anchor2d_Node* a, Pose2d old_origin) { 65 | if (_parent != NULL) { 66 | _parent->merge(a, old_origin); 67 | } else if (a->_parent != NULL) { 68 | merge(a->parent(), old_origin); 69 | } else { 70 | // this and a are root anchors. 71 | 72 | // Add a to the child list 73 | Pose2d old_anchor_pose = a->value(); 74 | a->init(old_origin.oplus(old_anchor_pose)); 75 | add_anchor(a); 76 | 77 | // Move the children of a to this 78 | for (size_t i = 0; i < a->_childs.size(); i++) { 79 | Anchor2d_Node* child = a->_childs[i]; 80 | 81 | old_anchor_pose = child->value(); 82 | child->init(old_origin.oplus(old_anchor_pose)); 83 | add_anchor(child); 84 | } 85 | a->_childs.clear(); 86 | } 87 | } 88 | 89 | Anchor3d_Node::Anchor3d_Node(Slam* slam) : Pose3d_Node("Anchor3d"), _parent(NULL), _prior(NULL), _slam(slam) 90 | { 91 | } 92 | 93 | Anchor3d_Node::~Anchor3d_Node() { 94 | if (_prior) { 95 | _slam->remove_factor(_prior); 96 | delete _prior; 97 | } 98 | } 99 | 100 | void Anchor3d_Node::set_prior() { 101 | Noise noise = SqrtInformation(10000. * eye(6)); 102 | Pose3d prior_origin; 103 | _prior = new Pose3d_Factor(this, prior_origin, noise); 104 | _slam->add_factor(_prior); 105 | } 106 | 107 | void Anchor3d_Node::add_anchor(Anchor3d_Node* a) { 108 | if (a->_prior) { 109 | _slam->remove_factor(a->_prior); 110 | delete a->_prior; 111 | a->_prior = NULL; 112 | } 113 | a->_parent = this; 114 | _childs.push_back(a); 115 | } 116 | 117 | void Anchor3d_Node::merge(Anchor3d_Node* a, Pose3d old_origin) { 118 | if (_parent != NULL) { 119 | _parent->merge(a, old_origin); 120 | } else if (a->_parent != NULL) { 121 | merge(a->parent(), old_origin); 122 | } else { 123 | // this and a are root anchors. 124 | 125 | // Add a to the child list 126 | Pose3d old_anchor_pose = a->value(); 127 | a->init(old_origin.oplus(old_anchor_pose)); 128 | add_anchor(a); 129 | 130 | // Move the children of a to this 131 | for (size_t i = 0; i < a->_childs.size(); i++) { 132 | Anchor3d_Node* child = a->_childs[i]; 133 | 134 | old_anchor_pose = child->value(); 135 | child->init(old_origin.oplus(old_anchor_pose)); 136 | add_anchor(child); 137 | } 138 | a->_childs.clear(); 139 | } 140 | } 141 | 142 | } 143 | -------------------------------------------------------------------------------- /isamlib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | 3 | # find all source and header files 4 | file(GLOB SRCS RELATIVE "${PROJECT_SOURCE_DIR}/isamlib" "*.cpp") 5 | 6 | # isamlib 7 | add_library(isamlib ${SRCS}) 8 | set_property(TARGET isamlib PROPERTY OUTPUT_NAME isam) 9 | 10 | # simple way of finding CHOLMOD 11 | find_package(Cholmod REQUIRED) 12 | include_directories(${CHOLMOD_INCLUDES}) 13 | target_link_libraries(isamlib ${CHOLMOD_LIBRARIES}) 14 | 15 | # install library 16 | install(TARGETS isamlib 17 | PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE 18 | LIBRARY DESTINATION lib 19 | ARCHIVE DESTINATION lib 20 | ) 21 | # install header files 22 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/isam/ DESTINATION include/isam 23 | FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) 24 | 25 | -------------------------------------------------------------------------------- /isamlib/ChowLiuTree.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ChowLiuTree.cpp 3 | * @brief ChowLiuTree for information form Gaussian distributions 4 | * @author Michael Kaess 5 | * @author Nicholas Carlevaris-Bianco 6 | * @version $Id: covariance.cpp 4975 2011-07-13 17:49:09Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #include "isam/ChowLiuTree.h" 30 | #include "isam/util.h" 31 | 32 | using namespace std; 33 | using namespace Eigen; 34 | 35 | namespace isam { 36 | 37 | MatrixXd matslice (const MatrixXd A, vector ii, vector jj) { 38 | MatrixXd B(ii.size(), jj.size()); 39 | for (size_t i=0; i iia(0); 52 | vector iib(0); 53 | int off = 0; 54 | for (size_t i=0; i<_nodes.size(); i++) { 55 | if ((int)i == id) 56 | for (int j=0; j<_nodes[i]->dim(); j++) iia.push_back(off+j); 57 | else 58 | for (int j=0; j<_nodes[i]->dim(); j++) iib.push_back(off+j); 59 | off += _nodes[i]->dim(); 60 | } 61 | 62 | if (iib.size() > 0) { 63 | MatrixXd Laa = matslice(_L, iia, iia); 64 | MatrixXd Lbb = matslice(_L, iib, iib); 65 | MatrixXd Lab = matslice(_L, iia, iib); 66 | MatrixXd Lbbinv = posdef_pinv(Lbb); 67 | return Laa - Lab * Lbbinv * Lab.transpose(); 68 | //return Laa - Lab * Lbb.inverse() * Lab.transpose(); 69 | } else 70 | return _L; 71 | } 72 | 73 | MatrixXd ChowLiuTreeInfo::joint(int ida, int idb) { 74 | 75 | vector iia(0); 76 | vector iib(0); 77 | vector iic(0); 78 | int off = 0; 79 | for (size_t i=0; i<_nodes.size(); i++) { 80 | if ((int)i == ida) 81 | for (int j=0; j<_nodes[i]->dim(); j++) iia.push_back(off+j); 82 | else if ((int)i == idb) 83 | for (int j=0; j<_nodes[i]->dim(); j++) iib.push_back(off+j); 84 | else 85 | for (int j=0; j<_nodes[i]->dim(); j++) iic.push_back(off+j); 86 | off += _nodes[i]->dim(); 87 | } 88 | vector iiab(0); 89 | iiab.insert(iiab.end(), iia.begin(), iia.end()); 90 | iiab.insert(iiab.end(), iib.begin(), iib.end()); 91 | 92 | if (iic.size() > 0) { 93 | MatrixXd Labab = matslice(_L, iiab, iiab); 94 | MatrixXd Lcc = matslice(_L, iic, iic); 95 | MatrixXd Labc = matslice(_L, iiab, iic); 96 | MatrixXd Lccinv = posdef_pinv(Lcc); 97 | return Labab - Labc * Lccinv * Labc.transpose(); 98 | //return Labab - Labc * Lcc.inverse() * Labc.transpose(); 99 | } else 100 | return matslice(_L, iiab, iiab); 101 | } 102 | 103 | MatrixXd ChowLiuTreeInfo::conditional(int ida, int idb) { 104 | 105 | MatrixXd Lj = joint(ida, idb); 106 | return Lj.block(0, 0, _nodes[ida]->dim(), _nodes[ida]->dim()); 107 | 108 | } 109 | 110 | ChowLiuTree::ChowLiuTree (const Eigen::MatrixXd &L, const std::vector& nodes) 111 | : _clt_info(L, nodes) 112 | { 113 | // make sure we have at least two nodes otherwise return a trival tree 114 | if (nodes.size() == 1) { 115 | 116 | ChowLiuTreeNode node; 117 | node.id = 0; 118 | node.pid = -1; 119 | node.marginal = _clt_info.marginal(0); 120 | node.conditional = node.marginal; 121 | node.joint = node.marginal; 122 | tree[node.id] = node; 123 | 124 | } else { 125 | 126 | //calculate the parent nodes based on maximising mutual information 127 | _calc_edges(); 128 | _max_span_tree(); 129 | tree.clear(); 130 | _build_tree_rec(_edges.front().id1, -1); 131 | 132 | } 133 | } 134 | 135 | bool mi_sort(MI &first, MI &second) { 136 | return first.mi > second.mi; 137 | } 138 | 139 | void 140 | ChowLiuTree::_calc_edges() { 141 | 142 | int nn = _clt_info.num_nodes(); 143 | //double npairs = floor(pow((double)bb, 2.0) / 2) - floor((double)nn/2); 144 | 145 | for (int i=0; i groups; // map 177 | for (int i=0; i<_clt_info.num_nodes(); i++) { 178 | groups[i] = i; 179 | } 180 | 181 | int group1, group2; 182 | list::iterator edge = _edges.begin(); 183 | while(edge != _edges.end()) { 184 | if(groups[edge->id1] != groups[edge->id2]) { 185 | group1 = groups[edge->id1]; 186 | group2 = groups[edge->id2]; 187 | 188 | // merge group2 into group 1 189 | map::iterator groupIt; 190 | for(groupIt = groups.begin(); groupIt != groups.end(); groupIt++) 191 | if(groupIt->second == group2) groupIt->second = group1; 192 | 193 | edge++; 194 | } else { 195 | edge = _edges.erase(edge); 196 | } 197 | } 198 | 199 | } 200 | 201 | void ChowLiuTree::_build_tree_rec(int id, int pid) { 202 | 203 | ChowLiuTreeNode new_node; 204 | 205 | new_node.id = id; 206 | new_node.pid = pid; 207 | new_node.marginal = _clt_info.marginal(id); 208 | if (pid == -1) { 209 | new_node.conditional = new_node.marginal; 210 | new_node.joint = new_node.marginal; 211 | } else { 212 | new_node.conditional = _clt_info.conditional(id, pid); 213 | new_node.joint = _clt_info.joint(id, pid); 214 | } 215 | 216 | vector cids; // vector of child ids 217 | list::iterator edge = _edges.begin(); 218 | while(edge != _edges.end()) { 219 | if(edge->id1 == new_node.id) { 220 | cids.push_back(edge->id2); 221 | edge = _edges.erase(edge); 222 | continue; 223 | } 224 | if(edge->id2 == new_node.id) { 225 | cids.push_back(edge->id1); 226 | edge = _edges.erase(edge); 227 | continue; 228 | } 229 | edge++; 230 | } 231 | for(size_t i=0; i < cids.size(); i++) { 232 | new_node.cids.push_back(cids[i]); 233 | _build_tree_rec(cids[i], new_node.id); 234 | } 235 | 236 | tree[new_node.id] = new_node; 237 | } 238 | 239 | } //namespace isam -------------------------------------------------------------------------------- /isamlib/Covariances.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Covariances.cpp 3 | * @brief Providing access to entries of the covariance. 4 | * @author Michael Kaess 5 | * @version $Id: Covariances.cpp 7858 2013-01-14 03:50:24Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include "isam/covariance.h" 33 | #include "isam/Slam.h" 34 | 35 | #include "isam/Covariances.h" 36 | 37 | using namespace std; 38 | using namespace Eigen; 39 | 40 | namespace isam { 41 | 42 | Covariances::Covariances(Slam& slam) : _slam(NULL), _R(slam._R) { 43 | // update pointers into matrix before making a copy 44 | slam.update_starts(); 45 | const list& nodes = slam.get_nodes(); 46 | for (list::const_iterator it = nodes.begin(); it!=nodes.end(); it++) { 47 | Node* node = *it; 48 | int start = node->_start; 49 | int dim = node->dim(); 50 | _index[node] = make_pair(start, dim); 51 | } 52 | } 53 | 54 | int Covariances::get_start(Node* node) const { 55 | if (_slam) { 56 | return node->_start; 57 | } else { 58 | return _index.find(node)->second.first; 59 | } 60 | } 61 | 62 | int Covariances::get_dim(Node* node) const { 63 | if (_slam) { 64 | return node->_dim; 65 | } else { 66 | return _index.find(node)->second.second; 67 | } 68 | } 69 | 70 | list Covariances::marginal(const node_lists_t& node_lists) const { 71 | const SparseSystem& R = (_slam==NULL) ? _R : _slam->_R; 72 | if (_slam) { 73 | _slam->update_starts(); 74 | } 75 | 76 | vector < vector > index_lists(node_lists.size()); 77 | 78 | if (R.num_rows()>1) { // skip if _R not calculated yet (eg. before batch step) 79 | int n=0; 80 | const int* trans = R.a_to_r(); 81 | for (node_lists_t::const_iterator it_list = node_lists.begin(); 82 | it_list != node_lists.end(); 83 | it_list++, n++) { 84 | // assemble list of indices 85 | vector indices; 86 | for (list::const_iterator it = it_list->begin(); it!=it_list->end(); it++) { 87 | Node* node = *it; 88 | int start = get_start(node); 89 | int dim = get_dim(node); 90 | for (int i=0; i empty_list; 98 | return empty_list; 99 | } 100 | 101 | MatrixXd Covariances::marginal(const list& nodes) const { 102 | node_lists_t node_lists; 103 | node_lists.push_back(nodes); 104 | return marginal(node_lists).front(); 105 | } 106 | 107 | list Covariances::access(const node_pair_list_t& node_pair_list) const { 108 | const SparseSystem& R = (_slam==NULL) ? _R : _slam->_R; 109 | if (_slam) { 110 | _slam->update_starts(); 111 | } 112 | 113 | if (R.num_rows()>1) { // skip if _R not calculated yet (eg. before batch step) 114 | 115 | // count how many entries in requested blocks 116 | int num = 0; 117 | for (node_pair_list_t::const_iterator it = node_pair_list.begin(); 118 | it != node_pair_list.end(); it++) { 119 | num += get_dim(it->first) * get_dim(it->second); 120 | } 121 | 122 | // request individual covariance entries 123 | vector < pair > index_list(num); 124 | const int* trans = R.a_to_r(); 125 | int n = 0; 126 | for (node_pair_list_t::const_iterator it = node_pair_list.begin(); 127 | it != node_pair_list.end(); it++) { 128 | int start_r = get_start(it->first); 129 | int start_c = get_start(it->second); 130 | int dim_r = get_dim(it->first); 131 | int dim_c = get_dim(it->second); 132 | for (int r=0; r covs = cov_marginal(R, _cache, index_list); 140 | 141 | // assemble into block matrices 142 | list result; 143 | list::iterator it_cov = covs.begin(); 144 | for (node_pair_list_t::const_iterator it = node_pair_list.begin(); 145 | it != node_pair_list.end(); it++) { 146 | int dim_r = get_dim(it->first); 147 | int dim_c = get_dim(it->second); 148 | MatrixXd matrix(dim_r, dim_c); 149 | for (int r=0; r empty_list; 160 | return empty_list; 161 | } 162 | 163 | } 164 | -------------------------------------------------------------------------------- /isamlib/GLCReparam.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GLCReparam.cpp 3 | * @brief Generic Linear Constraint Reparametrization 4 | * @author Nicholas Carlevaris-Bianco 5 | * @author Michael Kaess 6 | * @version $Id: GLCReparam.cpp 8578 2013-07-01 00:28:49Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #include 30 | 31 | #include "isam/GLCReparam.h" 32 | 33 | #include "isam/Pose3d.h" 34 | #include "isam/Point3d.h" 35 | #include "isam/Pose2d.h" 36 | #include "isam/Point2d.h" 37 | #include "isam/slam2d.h" 38 | #include "isam/slam3d.h" 39 | 40 | using namespace std; 41 | using namespace Eigen; 42 | 43 | namespace isam { 44 | 45 | VectorXd GLC_RootShift::reparameterize (Selector s) { 46 | 47 | VectorXd x; 48 | 49 | int np = _nodes.size(); 50 | 51 | if (np == 1) { 52 | x = _nodes[0]->vector(s); 53 | return x; 54 | } 55 | 56 | x.resize(_dim); 57 | x.setZero(); 58 | 59 | // decide on the root for the root shift 60 | // for now just use the first pose2d or pose3d 61 | int ir=0; 62 | for (ir=0; irname(), "Pose3d") || 64 | 0 == strcmp(_nodes[ir]->name(), "Pose2d")) 65 | break; 66 | } 67 | assert (ir != np); // assumes a pose3d or pose2d node is avaliable 68 | Node *node_i = _nodes[ir]; 69 | 70 | int ioff = 0; 71 | for (int j=0; jdim()) = node_i->vector(s); 74 | ioff += node_i->dim(); 75 | } else { 76 | Node *node_j = _nodes[j]; 77 | VectorXd x_i_j = root_shift (node_i, node_j, s); 78 | assert(node_j->dim() == x_i_j.size()); 79 | x.segment(ioff, node_j->dim()) = x_i_j; 80 | ioff += node_j->dim(); 81 | } 82 | } 83 | 84 | return x; 85 | } 86 | 87 | VectorXd GLC_RootShift::root_shift (Node* node_i, Node* node_j, Selector s) { 88 | 89 | VectorXd x_i_j; 90 | if (0 == strcmp(node_i->name(), "Pose3d")) { 91 | Pose3d pose3d_i = dynamic_cast(node_i)->value(s); 92 | if (0 == strcmp(node_j->name(), "Pose3d")) { 93 | x_i_j = dynamic_cast(node_j)->value(s).ominus(pose3d_i).vector(); 94 | } else if (0 == strcmp(node_j->name(), "Point3d")) { 95 | x_i_j = pose3d_i.transform_to(dynamic_cast(node_j)->value(s)).vector(); 96 | } else { 97 | std::cout << "Warning: root shift undefined for:" << node_i->name() << " and " << node_j->name() << std::endl; 98 | x_i_j = node_j->vector(s); // identity 99 | } 100 | } else if (0 == strcmp(node_i->name(), "Pose2d")) { 101 | Pose2d pose2d_i = dynamic_cast(node_i)->value(s); 102 | if (0 == strcmp(node_j->name(), "Pose2d")) { 103 | x_i_j = dynamic_cast(node_j)->value(s).ominus(pose2d_i).vector(); 104 | } else if (0 == strcmp(node_j->name(), "Point2d")) { 105 | x_i_j = pose2d_i.transform_to(dynamic_cast(node_j)->value(s)).vector(); 106 | } else { 107 | std::cout << "Warning: root shift undefined for:" << node_i->name() << " and " << node_j->name() << std::endl; 108 | x_i_j = node_j->vector(s); // identity 109 | } 110 | } 111 | 112 | return x_i_j; 113 | } 114 | 115 | const double epsilon = 0.0001; 116 | 117 | // TODO replace with analytical jacobians 118 | MatrixXd GLC_RootShift::jacobian() { 119 | 120 | MatrixXd J(_dim,_dim); 121 | 122 | int col = 0; 123 | // for each node... 124 | for (vector::iterator it = _nodes.begin(); it!=_nodes.end(); it++) { 125 | Node* node = *it; 126 | int dim_n = node->dim(); 127 | // for each dimension of the node... 128 | for (int j=0; jvector0(); 133 | // evaluate positive delta 134 | delta(j) = epsilon; 135 | //node->self_exmap(delta); // taken care of in exmap_jacobian() in glc.[h/cpp] 136 | node->update0(node->vector0() + delta); 137 | VectorXd y_plus = reparameterize(LINPOINT); 138 | node->update0(original); 139 | // evaluate negative delta 140 | delta(j) = -epsilon; 141 | //node->self_exmap(delta); // taken care of in exmap_jacobian() in glc.[h/cpp] 142 | node->update0(node->vector0() + delta); 143 | VectorXd y_minus = reparameterize(LINPOINT); 144 | node->update0(original); 145 | // store column 146 | VectorXd diff = (y_plus - y_minus) / (epsilon + epsilon); 147 | // wrap angular difference 148 | for (int k=0; k<_dim; k++) { 149 | if (_is_angle(k)) 150 | diff(k) = standardRad(diff(k)); 151 | } 152 | J.col(col) = diff; 153 | } 154 | } 155 | 156 | return J; 157 | } 158 | 159 | } // namespace isam 160 | -------------------------------------------------------------------------------- /isamlib/Node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Node.cpp 3 | * @brief A single variable node 4 | * @author Michael Kaess 5 | * @author Hordur Johannsson 6 | * @version $Id: $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #include "isam/Node.h" 30 | #include "isam/Factor.h" 31 | 32 | namespace isam 33 | { 34 | void Node::mark_deleted() { 35 | _deleted = true; 36 | for (std::list::iterator factor = _factors.begin(); factor != _factors.end(); ++factor) 37 | (*factor)->mark_deleted(); 38 | } 39 | void Node::erase_marked_factors() { 40 | for (std::list::iterator factor = _factors.begin(); factor != _factors.end();) 41 | if ((*factor)->deleted()) factor = _factors.erase(factor); 42 | else ++factor; 43 | } 44 | } // namespace - isam 45 | -------------------------------------------------------------------------------- /isamlib/OrderedSparseMatrix.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OrderedSparseMatrix.cpp 3 | * @brief Adds column ordering to sparse matrix functionality for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: OrderedSparseMatrix.cpp 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | #include // memset() 30 | #include 31 | #include 32 | #include 33 | 34 | #include "isam/util.h" 35 | 36 | #include "isam/OrderedSparseMatrix.h" 37 | 38 | using namespace std; 39 | 40 | namespace isam { 41 | 42 | void OrderedSparseMatrix::_allocate_OrderedSparseMatrix(bool init_table) { 43 | _r_to_a = new int[_max_num_cols]; 44 | _a_to_r = new int[_max_num_cols]; 45 | if (init_table) { 46 | for (int col=0; col<_num_cols; col++) { 47 | // reset translation table 48 | _r_to_a[col] = col; 49 | _a_to_r[col] = col; 50 | } 51 | } 52 | } 53 | 54 | void OrderedSparseMatrix::_copy_from_OrderedSparseMatrix(const OrderedSparseMatrix& mat) { 55 | _allocate_OrderedSparseMatrix(false); 56 | memcpy(_r_to_a, mat._r_to_a, _num_cols*sizeof(int)); 57 | memcpy(_a_to_r, mat._a_to_r, _num_cols*sizeof(int)); 58 | } 59 | 60 | void OrderedSparseMatrix::_dealloc_OrderedSparseMatrix() { 61 | delete[] _r_to_a; 62 | _r_to_a = NULL; // recommended, as freeing a second time has unpredictable consequences 63 | delete[] _a_to_r; 64 | _a_to_r = NULL; 65 | } 66 | 67 | void OrderedSparseMatrix::_calc_reverse_order(int num, const int* order, int* reverse_order) const { 68 | for (int i=0; i. 25 | * 26 | */ 27 | 28 | #include 29 | #include // memset() 30 | #include 31 | #include 32 | #include 33 | 34 | #include "isam/util.h" 35 | 36 | #include "isam/SparseSystem.h" 37 | 38 | using namespace std; 39 | using namespace Eigen; 40 | 41 | namespace isam { 42 | 43 | SparseSystem::SparseSystem(int num_rows, int num_cols) : OrderedSparseMatrix(num_rows, num_cols), _rhs(VectorXd(num_rows)) { 44 | } 45 | 46 | SparseSystem::SparseSystem(const SparseSystem& mat) : OrderedSparseMatrix(mat), _rhs(mat._rhs) { 47 | } 48 | 49 | SparseSystem::SparseSystem(const SparseSystem& mat, int num_rows, int num_cols, int first_row, int first_col) : 50 | OrderedSparseMatrix(mat, num_rows, num_cols, first_row, first_col), _rhs(mat._rhs.segment(first_row, num_rows)) { 51 | } 52 | 53 | SparseSystem::SparseSystem(int num_rows, int num_cols, SparseVector_p* rows, const VectorXd& rhs) : 54 | OrderedSparseMatrix(num_rows, num_cols, rows) { 55 | _rhs = rhs; 56 | } 57 | 58 | SparseSystem::~SparseSystem() { 59 | } 60 | 61 | const SparseSystem& SparseSystem::operator= (const SparseSystem& mat) { 62 | if (this==&mat) 63 | return *this; 64 | 65 | OrderedSparseMatrix::operator=(mat); 66 | _rhs = mat._rhs; 67 | 68 | return *this; 69 | } 70 | 71 | void SparseSystem::apply_givens(int row, int col, double* c_givens, double* s_givens) { 72 | double c, s; 73 | SparseMatrix::apply_givens(row, col, &c, &s); 74 | // modify rhs 75 | double r1 = _rhs(col); 76 | double r2 = _rhs(row); 77 | _rhs(col) = c*r1 - s*r2; 78 | _rhs(row) = s*r1 + c*r2; 79 | } 80 | 81 | void SparseSystem::append_new_rows(int num) { 82 | OrderedSparseMatrix::append_new_rows(num); 83 | _rhs.conservativeResize(_rhs.size() + num); 84 | } 85 | 86 | void SparseSystem::add_row(const SparseVector& new_row, double new_r) { 87 | ensure_num_cols(new_row.last()+1); 88 | 89 | append_new_rows(1); 90 | int row = num_rows() - 1; 91 | _rhs(row) = new_r; 92 | set_row(row, new_row); 93 | } 94 | 95 | int SparseSystem::add_row_givens(const SparseVector& new_row, double new_r) { 96 | // set new row (also translates according to current variable ordering) 97 | add_row(new_row, new_r); 98 | int count = 0; 99 | 100 | int row = num_rows() - 1; // last row 101 | 102 | int col = get_row(row).first(); // first entry to be zeroed 103 | while (col>=0 && col= num_cols(), "SparseSystem::solve: cannot solve system, not enough constraints"); 121 | requireDebug(num_rows() == num_cols(), "SparseSystem::solve: system not triangular"); 122 | int n = num_cols(); 123 | VectorXd result(n); 124 | for (int row=n-1; row>=0; row--) { 125 | const SparseVector& rowvec = get_row(row); 126 | // start with rhs... 127 | double terms = _rhs(row); 128 | double diag; 129 | 130 | // ... and subtract already solved variables multiplied with respective coefficient from R 131 | // We assume that the SpareSystem is triangular 132 | SparseVectorIter iter(rowvec); 133 | iter.get(diag); // we can check return value it should be == row 134 | iter.next(); 135 | for (; iter.valid(); iter.next()) { 136 | double v; 137 | int col = iter.get(v); 138 | terms = terms - result(col)*v; 139 | } 140 | // divide result by diagonal entry of R 141 | result(row) = terms / diag; 142 | } 143 | return result; 144 | } 145 | 146 | } 147 | -------------------------------------------------------------------------------- /isamlib/covariance.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file covariance.cpp 3 | * @brief Recovery of marginal covariance matrix, for details see Kaess09ras. 4 | * @author Michael Kaess 5 | * @author Nicholas Carlevaris-Bianco 6 | * @version $Id: covariance.cpp 8578 2013-07-01 00:28:49Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #include 30 | #include // pair 31 | 32 | #include "isam/covariance.h" 33 | #include "isam/util.h" 34 | 35 | using namespace std; 36 | using namespace Eigen; 37 | 38 | namespace isam { 39 | 40 | void prepare(const SparseMatrix& R, CovarianceCache& cache) { 41 | // reset hash table 42 | cache.entries.clear(); 43 | // speedup row access 44 | int n = R.num_cols(); 45 | cache.rows.resize(n); 46 | cache.rows_valid.resize(n); 47 | cache.current_valid++; // invalidate previous entries 48 | // wrapped back to 0? then we do have to reset the table 49 | if (cache.current_valid==0) { 50 | for (int i=0; il) { 84 | lj = recover(R, cache, n, l, j); 85 | } else { 86 | lj = recover(R, cache, n, j, l); 87 | } 88 | sum += rij * lj; 89 | } 90 | } 91 | return sum; 92 | } 93 | 94 | double recover(const SparseMatrix& R, CovarianceCache& cache, int n, int i, int l) { 95 | if (i>l) {int tmp=i; i=l; l=tmp;} 96 | int id = i*n + l; // flatten index for hash table 97 | umap::iterator it = cache.entries.find(id); 98 | double res; 99 | if (it == cache.entries.end()) { 100 | //printf("calculating entry %i,%i\n", i, l); 101 | // need to calculate entry 102 | if (i==l) { 103 | res = cache.diag[l] * (cache.diag[l] - sum_j(R, cache, n, l, l)); 104 | } else { 105 | res = -sum_j(R, cache, n, l, i) * cache.diag[i]; 106 | } 107 | // insert into hash 108 | pair entry(id, res); 109 | cache.entries.insert(entry); 110 | cache.num_calc++; 111 | } else { 112 | //printf("retrieved entry %i,%i\n", i, l); 113 | // retrieve value from hash 114 | res = it->second; 115 | } 116 | return res; 117 | } 118 | 119 | list cov_marginal(const SparseMatrix& R, CovarianceCache& cache, 120 | const index_lists_t& index_lists, bool debug, int step) { 121 | prepare(R, cache); 122 | list Cs; 123 | 124 | // debugging 125 | int requested = 0; 126 | double t0 = tic(); 127 | 128 | for (unsigned int i=0; i& indices = index_lists[i]; 131 | unsigned int n_indices = indices.size(); 132 | MatrixXd C(n_indices, n_indices); 133 | // recover entries of marginal cov 134 | int n = R.num_cols(); 135 | for (int r=n_indices-1; r>=0; r--) { 136 | for (int c=n_indices-1; c>=r; c--) { 137 | C(r,c) = recover(R, cache, n, indices[r], indices[c]); 138 | } 139 | } 140 | for (unsigned int r=1; r=0) { 155 | // stats for gnuplot 156 | printf("ggg %i %i %i %i %i %i %f ", 157 | step, 158 | R.num_cols(), // side length 159 | R.num_cols()*R.num_cols(), // #entries dense 160 | R.nnz(), // #entries sparse 161 | cache.num_calc, // #entries calculated 162 | requested, // #entries requested 163 | time); // #execution time 164 | } 165 | } 166 | 167 | return Cs; 168 | } 169 | 170 | list cov_marginal(const SparseMatrix& R, CovarianceCache& cache, 171 | const entry_list_t& entry_list) { 172 | prepare(R, cache); 173 | list entries; 174 | 175 | int n = R.num_cols(); 176 | for (unsigned int i=0; i& index = entry_list[i]; 178 | double entry = recover(R, cache, n, index.first, index.second); 179 | entries.push_back(entry); 180 | } 181 | 182 | return entries; 183 | } 184 | 185 | } 186 | -------------------------------------------------------------------------------- /isamlib/numericalDiff.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file numericalDiff.cpp 3 | * @brief Numerical differentiation. 4 | * @author Michael Kaess 5 | * @version $Id: numericalDiff.cpp 4038 2011-02-26 04:31:00Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | 30 | #include "isam/numericalDiff.h" 31 | 32 | #define SYMMETRIC 33 | 34 | const double epsilon = 0.0001; 35 | 36 | using namespace std; 37 | using namespace Eigen; 38 | 39 | namespace isam { 40 | 41 | MatrixXd numericalDiff(Function& func) { 42 | #ifndef SYMMETRIC 43 | VectorXd y0 = func.evaluate(); 44 | #endif 45 | // number of measurement rows 46 | int m = func.num_measurements(); 47 | // number of variables 48 | int n = 0; 49 | vector& nodes = func.nodes(); 50 | for (vector::iterator it = nodes.begin(); it!=nodes.end(); it++) { 51 | n += (*it)->dim(); 52 | } 53 | // result has one column per variable 54 | MatrixXd Jacobian(m,n); 55 | int col = 0; 56 | // for each node... 57 | for (vector::iterator it = nodes.begin(); it!=nodes.end(); it++) { 58 | Node* node = *it; 59 | int dim_n = node->dim(); 60 | // for each dimension of the node... 61 | for (int j=0; jvector0(); 66 | // evaluate positive delta 67 | delta(j) = epsilon; 68 | node->self_exmap(delta); 69 | VectorXd y_plus = func.evaluate(); 70 | node->update0(original); 71 | #ifdef SYMMETRIC 72 | // evaluate negative delta 73 | delta(j) = -epsilon; 74 | node->self_exmap(delta); 75 | VectorXd y_minus = func.evaluate(); 76 | node->update0(original); 77 | // store column 78 | VectorXd diff = (y_plus - y_minus) / (epsilon + epsilon); 79 | #else 80 | VectorXd diff = (y_plus - y0) / epsilon; 81 | #endif 82 | Jacobian.col(col) = diff; 83 | } 84 | } 85 | 86 | return Jacobian; 87 | } 88 | 89 | } 90 | -------------------------------------------------------------------------------- /isamlib/util.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file util.cpp 3 | * @brief Basic utility functions that are independent of iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: util.cpp 6335 2012-03-22 23:13:52Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include // abs 32 | 33 | #include "isam/util.h" 34 | 35 | using namespace std; 36 | 37 | namespace isam { 38 | 39 | // simple class for accumulating execution timing information by name 40 | class Timing { 41 | class Stats { 42 | public: 43 | double t0; 44 | double t; 45 | double t_max; 46 | double t_min; 47 | int n; 48 | }; 49 | map stats; 50 | public: 51 | void add_t0(string id, double t0) { 52 | stats[id].t0 = t0; 53 | } 54 | double get_t0(string id) { 55 | return stats[id].t0; 56 | } 57 | void add_dt(string id, double dt) { 58 | Stats& s = stats[id]; 59 | s.t += dt; 60 | s.n++; 61 | if (s.n==1 || s.t_max < dt) s.t_max = dt; 62 | if (s.n==1 || s.t_min > dt) s.t_min = dt; 63 | } 64 | void print() { 65 | map::iterator it; 66 | for(it = stats.begin(); it!=stats.end(); it++) { 67 | Stats& s = it->second; 68 | printf("%s: %g (%i times, min: %g, max: %g)\n", 69 | it->first.c_str(), s.t, s.n, s.t_min, s.t_max); 70 | } 71 | } 72 | double time(string id) { 73 | Stats& s = stats[id]; 74 | return s.t; 75 | } 76 | }; 77 | Timing timing; 78 | 79 | double tic() { 80 | struct timeval t; 81 | gettimeofday(&t, NULL); 82 | return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.); 83 | } 84 | 85 | double tic(string id) { 86 | double t0 = tic(); 87 | timing.add_t0(id, t0); 88 | return t0; 89 | } 90 | 91 | double toc(double t) { 92 | double s = tic(); 93 | return (max(0., s-t)); 94 | } 95 | 96 | double toc(string id) { 97 | double dt = toc(timing.get_t0(id)); 98 | timing.add_dt(id, dt); 99 | return dt; 100 | } 101 | 102 | void tictoc_print() { 103 | timing.print(); 104 | } 105 | 106 | double tictoc(string id) { 107 | return (timing.time(id)); 108 | } 109 | 110 | Eigen::MatrixXd eye(int num) { 111 | return Eigen::MatrixXd::Identity(num, num); 112 | } 113 | 114 | void givens(const double a, const double b, double& c, double& s) { 115 | if (b==0) { 116 | c = 1.0; 117 | s = 0.0; 118 | } else if (fabs(b)>fabs(a)) { 119 | double t = -a/b; 120 | s = 1/sqrt(1+t*t); 121 | c = t*s; 122 | } else { 123 | double t = -b/a; 124 | c = 1/sqrt(1+t*t); 125 | s = t*c; 126 | } 127 | } 128 | 129 | } 130 | -------------------------------------------------------------------------------- /manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Depth Enhanced Monocular Odometry (RGBD camera version) 5 | 6 | 7 | Ji Zhang 8 | LGPL 9 | 10 | http://wiki.ros.org/demo_rgbd 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/cameraParameters.h: -------------------------------------------------------------------------------- 1 | #ifndef camera_parameters_h 2 | #define camera_parameters_h 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "opencv2/contrib/contrib.hpp" 12 | #include "opencv2/calib3d/calib3d.hpp" 13 | 14 | #include 15 | 16 | const int imageWidth = 640; 17 | const int imageHeight = 480; 18 | 19 | double kImage[9] = {525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0}; 20 | 21 | double kDepth[9] = {525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0}; 22 | 23 | #endif 24 | 25 | -------------------------------------------------------------------------------- /src/pointDefinition.h: -------------------------------------------------------------------------------- 1 | #ifndef point_definition_h 2 | #define point_definition_h 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | struct ImagePoint { 14 | float u, v; 15 | int ind; 16 | }; 17 | 18 | POINT_CLOUD_REGISTER_POINT_STRUCT (ImagePoint, 19 | (float, u, u) 20 | (float, v, v) 21 | (int, ind, ind)) 22 | 23 | struct DepthPoint { 24 | float u, v; 25 | float depth; 26 | int label; 27 | int ind; 28 | }; 29 | 30 | POINT_CLOUD_REGISTER_POINT_STRUCT (DepthPoint, 31 | (float, u, u) 32 | (float, v, v) 33 | (float, depth, depth) 34 | (int, label, label) 35 | (int, ind, ind)) 36 | 37 | #endif 38 | 39 | -------------------------------------------------------------------------------- /src/registerPointCloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "cameraParameters.h" 13 | #include "pointDefinition.h" 14 | 15 | const double PI = 3.1415926; 16 | 17 | const int imagePixelNum = imageHeight * imageWidth; 18 | 19 | const int keepSyncCloudNum = 15; 20 | double syncCloudTime[keepSyncCloudNum] = {0}; 21 | pcl::PointCloud::Ptr syncCloudArray[keepSyncCloudNum]; 22 | int syncCloudInd = -1; 23 | int cloudRegInd = 0; 24 | 25 | pcl::PointCloud::Ptr surroundCloud(new pcl::PointCloud()); 26 | pcl::PointCloud::Ptr tempCloud(new pcl::PointCloud()); 27 | pcl::PointCloud::Ptr tempCloud2(new pcl::PointCloud()); 28 | 29 | double timeRec = 0; 30 | double rxRec = 0, ryRec = 0, rzRec = 0; 31 | double txRec = 0, tyRec = 0, tzRec = 0; 32 | 33 | int cloudCount = -1; 34 | const int cloudSkipNum = 5; 35 | 36 | int showCount = -1; 37 | const int showSkipNum = 10; 38 | 39 | ros::Publisher *surroundCloudPubPointer = NULL; 40 | 41 | void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData) 42 | { 43 | double time = voData->header.stamp.toSec(); 44 | 45 | double rx, ry, rz; 46 | geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation; 47 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(rz, rx, ry); 48 | 49 | double tx = voData->pose.pose.position.x; 50 | double ty = voData->pose.pose.position.y; 51 | double tz = voData->pose.pose.position.z; 52 | 53 | if (time - timeRec < 0.5 && syncCloudInd >= 0) { 54 | pcl::PointXYZ point; 55 | while (syncCloudTime[cloudRegInd] <= time && cloudRegInd != (syncCloudInd + 1) % keepSyncCloudNum) { 56 | double scaleLast = (time - syncCloudTime[cloudRegInd]) / (time - timeRec); 57 | double scaleCur = (syncCloudTime[cloudRegInd] - timeRec) / (time - timeRec); 58 | if (scaleLast > 1) { 59 | scaleLast = 1; 60 | } else if (scaleLast < 0) { 61 | scaleLast = 0; 62 | } 63 | if (scaleCur > 1) { 64 | scaleCur = 1; 65 | } else if (scaleCur < 0) { 66 | scaleCur = 0; 67 | } 68 | 69 | double rx2 = rx * scaleCur + rxRec * scaleLast; 70 | double ry2; 71 | if (ry - ryRec > PI) { 72 | ry2 = ry * scaleCur + (ryRec + 2 * PI) * scaleLast; 73 | } else if (ry - ryRec < -PI) { 74 | ry2 = ry * scaleCur + (ryRec - 2 * PI) * scaleLast; 75 | } else { 76 | ry2 = ry * scaleCur + ryRec * scaleLast; 77 | } 78 | double rz2 = rz * scaleCur + rzRec * scaleLast; 79 | 80 | double tx2 = tx * scaleCur + txRec * scaleLast; 81 | double ty2 = ty * scaleCur + tyRec * scaleLast; 82 | double tz2 = tz * scaleCur + tzRec * scaleLast; 83 | 84 | double cosrx2 = cos(rx2); 85 | double sinrx2 = sin(rx2); 86 | double cosry2 = cos(ry2); 87 | double sinry2 = sin(ry2); 88 | double cosrz2 = cos(rz2); 89 | double sinrz2 = sin(rz2); 90 | 91 | pcl::PointCloud::Ptr syncCloudPointer = syncCloudArray[cloudRegInd]; 92 | int syncCloudNum = syncCloudPointer->points.size(); 93 | for (int i = 0; i < syncCloudNum; i++) { 94 | point = syncCloudPointer->points[i]; 95 | double pointDis = sqrt(point.x * point.x + point.y * point.y + point.z * point.z); 96 | if (pointDis > 0.3 && pointDis < 5) { 97 | double x1 = cosrz2 * point.x - sinrz2 * point.y; 98 | double y1 = sinrz2 * point.x + cosrz2 * point.y; 99 | double z1 = point.z; 100 | 101 | double x2 = x1; 102 | double y2 = cosrx2 * y1 + sinrx2 * z1; 103 | double z2 = -sinrx2 * y1 + cosrx2 * z1; 104 | 105 | point.x = cosry2 * x2 - sinry2 * z2 + tx2; 106 | point.y = y2 + ty2; 107 | point.z = sinry2 * x2 + cosry2 * z2 + tz2; 108 | 109 | tempCloud->push_back(point); 110 | } 111 | } 112 | 113 | cloudRegInd = (cloudRegInd + 1) % keepSyncCloudNum; 114 | } 115 | 116 | showCount = (showCount + 1) % (showSkipNum + 1); 117 | if (showCount != 0) { 118 | rxRec = rx; ryRec = ry; rzRec = rz; 119 | txRec = tx; tyRec = ty; tzRec = tz; 120 | timeRec = time; 121 | return; 122 | } 123 | 124 | int surroundCloudNum = surroundCloud->points.size(); 125 | for (int i = 0; i < surroundCloudNum; i++) { 126 | point = surroundCloud->points[i]; 127 | 128 | double xDiff = point.x - tx; 129 | double yDiff = point.y - ty; 130 | double zDiff = point.z - tz; 131 | 132 | double pointDis = sqrt(xDiff * xDiff + yDiff * yDiff + zDiff * zDiff); 133 | if (pointDis < 50) { 134 | tempCloud->push_back(point); 135 | } 136 | } 137 | 138 | surroundCloud->clear(); 139 | pcl::VoxelGrid downSizeFilter; 140 | downSizeFilter.setInputCloud(tempCloud); 141 | downSizeFilter.setLeafSize(0.2, 0.2, 0.2); 142 | downSizeFilter.filter(*surroundCloud); 143 | tempCloud->clear(); 144 | 145 | sensor_msgs::PointCloud2 surroundCloud2; 146 | pcl::toROSMsg(*surroundCloud, surroundCloud2); 147 | surroundCloud2.header.frame_id = "/camera_init"; 148 | surroundCloud2.header.stamp = voData->header.stamp; 149 | surroundCloudPubPointer->publish(surroundCloud2); 150 | } 151 | 152 | rxRec = rx; ryRec = ry; rzRec = rz; 153 | txRec = tx; tyRec = ty; tzRec = tz; 154 | timeRec = time; 155 | } 156 | 157 | void syncCloudHandler(const sensor_msgs::Image::ConstPtr& syncCloud2) 158 | { 159 | cloudCount = (cloudCount + 1) % (cloudSkipNum + 1); 160 | if (cloudCount != 0) { 161 | return; 162 | } 163 | 164 | double time = syncCloud2->header.stamp.toSec(); 165 | 166 | syncCloudInd = (syncCloudInd + 1) % keepSyncCloudNum; 167 | syncCloudTime[syncCloudInd] = time; 168 | 169 | tempCloud2->clear(); 170 | pcl::PointXYZ point; 171 | const float* syncCloud2Pointer = reinterpret_cast(&syncCloud2->data[0]); 172 | for (int i = 0; i < imagePixelNum; i++) { 173 | float val = syncCloud2Pointer[i]; 174 | 175 | int xd = i % imageWidth; 176 | int yd = int(i / imageWidth); 177 | 178 | double ud = (kDepth[2] - xd) / kDepth[0]; 179 | double vd = (kDepth[5] - yd) / kDepth[4]; 180 | 181 | point.z = val; 182 | point.x = ud * val; 183 | point.y = vd * val; 184 | 185 | if (point.z > 0.3 && point.z < 7) { 186 | tempCloud2->push_back(point); 187 | } 188 | } 189 | 190 | pcl::PointCloud::Ptr syncCloudPointer = syncCloudArray[syncCloudInd]; 191 | syncCloudPointer->clear(); 192 | 193 | pcl::VoxelGrid downSizeFilter; 194 | downSizeFilter.setInputCloud(tempCloud2); 195 | downSizeFilter.setLeafSize(0.1, 0.1, 0.1); 196 | downSizeFilter.filter(*syncCloudPointer); 197 | } 198 | 199 | int main(int argc, char** argv) 200 | { 201 | ros::init(argc, argv, "registerPointCloud"); 202 | ros::NodeHandle nh; 203 | 204 | for (int i = 0; i < keepSyncCloudNum; i++) { 205 | pcl::PointCloud::Ptr syncCloudTemp(new pcl::PointCloud()); 206 | syncCloudArray[i] = syncCloudTemp; 207 | } 208 | 209 | ros::Subscriber voDataSub = nh.subscribe ("/cam2_to_init", 5, voDataHandler); 210 | 211 | ros::Subscriber syncCloudSub = nh.subscribe 212 | ("/camera/depth_registered/image", 1, syncCloudHandler); 213 | 214 | ros::Publisher surroundCloudPub = nh.advertise ("/surround_cloud", 1); 215 | surroundCloudPubPointer = &surroundCloudPub; 216 | 217 | ros::spin(); 218 | 219 | return 0; 220 | } 221 | -------------------------------------------------------------------------------- /src/stackDepthPoint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "pointDefinition.h" 7 | 8 | const double PI = 3.1415926; 9 | 10 | const int keyframeNum = 5; 11 | pcl::PointCloud::Ptr depthPoints[keyframeNum]; 12 | double depthPointsTime[keyframeNum]; 13 | int keyframeCount = 0; 14 | int frameCount = 0; 15 | 16 | pcl::PointCloud::Ptr depthPointsStacked(new pcl::PointCloud()); 17 | ros::Publisher *depthPointsPubPointer = NULL; 18 | 19 | double lastPubTime = 0; 20 | 21 | void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2) 22 | { 23 | frameCount = (frameCount + 1) % 4; 24 | if (frameCount != 0) { 25 | return; 26 | } 27 | 28 | pcl::PointCloud::Ptr depthPointsCur = depthPoints[0]; 29 | depthPointsCur->clear(); 30 | pcl::fromROSMsg(*depthPoints2, *depthPointsCur); 31 | 32 | for (int i = 0; i < keyframeNum - 1; i++) { 33 | depthPoints[i] = depthPoints[i + 1]; 34 | depthPointsTime[i] = depthPointsTime[i + 1]; 35 | } 36 | depthPoints[keyframeNum - 1] = depthPointsCur; 37 | depthPointsTime[keyframeNum - 1] = depthPoints2->header.stamp.toSec(); 38 | 39 | keyframeCount++; 40 | if (keyframeCount >= keyframeNum && depthPointsTime[0] >= lastPubTime) { 41 | depthPointsStacked->clear(); 42 | for (int i = 0; i < keyframeNum; i++) { 43 | *depthPointsStacked += *depthPoints[i]; 44 | } 45 | 46 | sensor_msgs::PointCloud2 depthPoints3; 47 | pcl::toROSMsg(*depthPointsStacked, depthPoints3); 48 | depthPoints3.header.frame_id = "camera"; 49 | depthPoints3.header.stamp = depthPoints2->header.stamp; 50 | depthPointsPubPointer->publish(depthPoints3); 51 | 52 | lastPubTime = depthPointsTime[keyframeNum - 1]; 53 | } 54 | } 55 | 56 | int main(int argc, char** argv) 57 | { 58 | ros::init(argc, argv, "stackDepthPoint"); 59 | ros::NodeHandle nh; 60 | 61 | for (int i = 0; i < keyframeNum; i++) { 62 | pcl::PointCloud::Ptr depthPointsTemp(new pcl::PointCloud()); 63 | depthPoints[i] = depthPointsTemp; 64 | } 65 | 66 | ros::Subscriber depthPointsSub = nh.subscribe 67 | ("/depth_points_last", 5, depthPointsHandler); 68 | 69 | ros::Publisher depthPointsPub = nh.advertise ("/depth_points_stacked", 1); 70 | depthPointsPubPointer = &depthPointsPub; 71 | 72 | ros::spin(); 73 | 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /stack.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 0.9.1 4 | 5 | 6 | 7 | Depth Enhanced Monocular Odometry (RGBD camera version) 8 | 9 | 10 | Ji Zhang 11 | LGPL 12 | 13 | http://wiki.ros.org/demo_rgbd 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | --------------------------------------------------------------------------------